845 lines
28 KiB
Rust
845 lines
28 KiB
Rust
#![allow(dead_code)]
|
|
|
|
use bevy::{
|
|
math::{Mat3, Vec3, Vec3Swizzles},
|
|
transform::{components::Transform, TransformPoint as _},
|
|
};
|
|
|
|
use crate::vvlib::octtree::Path;
|
|
|
|
use super::{
|
|
constants,
|
|
obb::OBB,
|
|
octtree::{OctTree, TreeNode},
|
|
};
|
|
|
|
#[derive(Debug, Copy, Clone, PartialEq)]
|
|
pub struct VectorPair {
|
|
pub global: Vec3,
|
|
pub local: Vec3,
|
|
}
|
|
impl VectorPair {
|
|
pub fn new(global: Vec3, local: Vec3) -> VectorPair {
|
|
VectorPair {
|
|
global: global,
|
|
local: local,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, PartialEq, Clone)]
|
|
pub struct RayAABBIntersection {
|
|
hitpoint: Vec3,
|
|
normal: Vec3,
|
|
}
|
|
impl RayAABBIntersection {
|
|
pub fn new(hitpoint: Vec3, normal: Vec3) -> RayAABBIntersection {
|
|
RayAABBIntersection {
|
|
hitpoint: hitpoint,
|
|
normal: normal,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, PartialEq)]
|
|
pub struct OctTreeCollisionPoint {
|
|
pos: VectorPair,
|
|
pub(crate) path: Path,
|
|
}
|
|
|
|
#[derive(Clone, Debug)]
|
|
pub struct RayOctTreeIntersection {
|
|
pub hit_data: RayOBBIntersection,
|
|
pub path_to_hit: Path,
|
|
pub voxel_index_location: Vec3,
|
|
}
|
|
|
|
pub fn octtree_overlap<T: Clone>(
|
|
first_oct: &OctTree<T>,
|
|
first_trans: &OBB,
|
|
second_oct: &OctTree<T>,
|
|
second_trans: &OBB,
|
|
) -> Option<Vec<(OctTreeCollisionPoint, OctTreeCollisionPoint)>> {
|
|
let first = obb_to_global_sphere(first_oct.center, first_oct.size, first_trans);
|
|
let second = obb_to_global_sphere(second_oct.center, second_oct.size, second_trans);
|
|
let early_out = sphere_sphere_overlap(first.0, first.1, second.0, second.1);
|
|
if early_out.is_none() {
|
|
return None;
|
|
}
|
|
let mut x = Vec::new();
|
|
visitor_octtree_x_octtree(
|
|
&first_oct.root,
|
|
first_oct.center,
|
|
first_oct.size,
|
|
first_trans,
|
|
&second_oct.root,
|
|
second_oct.center,
|
|
second_oct.size,
|
|
second_trans,
|
|
&mut x,
|
|
);
|
|
if x.is_empty() {
|
|
return None;
|
|
} else {
|
|
return Some(x);
|
|
}
|
|
}
|
|
|
|
fn visitor_octtree_x_octtree<T: Clone>(
|
|
first_node: &TreeNode<T>,
|
|
first_pos: Vec3,
|
|
first_size: usize,
|
|
first_trans: &OBB,
|
|
second_node: &TreeNode<T>,
|
|
second_pos: Vec3,
|
|
second_size: usize,
|
|
second_trans: &OBB,
|
|
results: &mut Vec<(OctTreeCollisionPoint, OctTreeCollisionPoint)>,
|
|
) {
|
|
if second_size > first_size {
|
|
// make second smaller till they match, one recursion at a time
|
|
let second_el = second_node.branch_contents_reference();
|
|
if second_el.is_none() {
|
|
return; // we're done in this subsector of the tree
|
|
}
|
|
for (second_count, second_iter) in second_el.unwrap().iter().enumerate() {
|
|
let (second_center, second_bound) =
|
|
constants::sub_region(second_pos, second_size, second_count as i32);
|
|
visitor_octtree_x_octtree(
|
|
first_node,
|
|
first_pos,
|
|
first_size,
|
|
first_trans,
|
|
second_iter,
|
|
second_center,
|
|
second_bound,
|
|
second_trans,
|
|
results,
|
|
);
|
|
}
|
|
} else if first_size > second_size {
|
|
// make first smaller till they match, one recursion at a time
|
|
let first_el = first_node.branch_contents_reference();
|
|
if first_el.is_none() {
|
|
return; // we're done in this subsector of the tree
|
|
}
|
|
for (first_count, first_iter) in first_el.unwrap().iter().enumerate() {
|
|
let (first_center, first_bound) =
|
|
constants::sub_region(first_pos, first_size, first_count as i32);
|
|
visitor_octtree_x_octtree(
|
|
first_iter,
|
|
first_center,
|
|
first_bound,
|
|
first_trans,
|
|
second_node,
|
|
second_pos,
|
|
second_size,
|
|
second_trans,
|
|
results,
|
|
);
|
|
}
|
|
} else {
|
|
// special case for roots being leaves
|
|
if first_node.is_leaf() && second_node.is_leaf() {
|
|
// do an intersection on them with OBBxOBB
|
|
let x = first_trans.as_transform().transform_point(first_pos);
|
|
let y = first_trans.half_size * first_size as f32;
|
|
let mut obb1 = first_trans.clone();
|
|
obb1.half_size = y;
|
|
obb1.position = x;
|
|
|
|
let a = second_trans.as_transform().transform_point(second_pos);
|
|
let b = second_trans.half_size * second_size as f32;
|
|
let mut obb2 = second_trans.clone();
|
|
obb2.half_size = b;
|
|
obb2.position = a;
|
|
let res = obb_obb_overlap(&obb1, &obb2);
|
|
if res {
|
|
// well,
|
|
let half1 = OctTreeCollisionPoint {
|
|
pos: VectorPair::new(obb1.position, first_pos),
|
|
path: Default::default(),
|
|
};
|
|
let half2 = OctTreeCollisionPoint {
|
|
pos: VectorPair::new(obb2.position, second_pos),
|
|
path: Default::default(),
|
|
};
|
|
results.push((half1, half2));
|
|
}
|
|
}
|
|
// check each element in each side of it and collide, then recurse
|
|
let first_el = first_node.branch_contents_reference();
|
|
let second_el = second_node.branch_contents_reference();
|
|
if first_el.is_some() && second_el.is_some() {
|
|
for (first_count, first_iter) in first_el.unwrap().iter().enumerate() {
|
|
for (second_count, second_iter) in second_el.unwrap().iter().enumerate() {
|
|
let (first_center, first_bound) =
|
|
constants::sub_region(first_pos, first_size, first_count as i32);
|
|
let (second_center, second_bound) =
|
|
constants::sub_region(second_pos, second_size, second_count as i32);
|
|
if first_iter.is_branch() && second_iter.is_branch() {
|
|
let (first_sa_center, first_sa_radius) =
|
|
obb_to_global_sphere(first_center, first_bound, first_trans);
|
|
let (second_sa_center, second_sa_radius) =
|
|
obb_to_global_sphere(second_center, second_bound, second_trans);
|
|
let res = sphere_sphere_overlap(
|
|
first_sa_center,
|
|
first_sa_radius,
|
|
second_sa_center,
|
|
second_sa_radius,
|
|
);
|
|
if res.is_some() {
|
|
visitor_octtree_x_octtree(
|
|
first_iter,
|
|
first_center,
|
|
first_bound,
|
|
first_trans,
|
|
second_iter,
|
|
second_center,
|
|
second_bound,
|
|
second_trans,
|
|
results,
|
|
);
|
|
}
|
|
} else if first_iter.is_leaf() && second_iter.is_leaf() {
|
|
// do an intersection on them with OBBxOBB
|
|
let mut obb1 = first_trans.clone();
|
|
obb1.position = first_trans.as_transform().transform_point(first_center);
|
|
|
|
let mut obb2 = second_trans.clone();
|
|
obb2.position = second_trans.as_transform().transform_point(second_center);
|
|
|
|
let res = obb_obb_overlap(&obb1, &obb2);
|
|
if res {
|
|
println!("adding collision");
|
|
// well,
|
|
let half1 = OctTreeCollisionPoint {
|
|
pos: VectorPair::new(obb1.position, first_center),
|
|
path: Default::default(),
|
|
};
|
|
let half2 = OctTreeCollisionPoint {
|
|
pos: VectorPair::new(obb2.position, second_center),
|
|
path: Default::default(),
|
|
};
|
|
results.push((half1, half2));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn aa_bounds_contains(center: Vec3, size: usize, subject: Vec3) -> bool {
|
|
let fsize = size as f32;
|
|
let min = center - Vec3::splat(fsize as f32 / 2.);
|
|
let max = center + Vec3::splat(fsize as f32 / 2.);
|
|
return min.x <= subject.x
|
|
&& subject.x <= max.x
|
|
&& min.y <= subject.y
|
|
&& subject.y <= max.y
|
|
&& min.z <= subject.z
|
|
&& subject.z <= max.z;
|
|
}
|
|
|
|
pub fn sphere_sphere_overlap(
|
|
center1: Vec3,
|
|
radius1: f32,
|
|
center2: Vec3,
|
|
radius2: f32,
|
|
) -> Option<Vec3> {
|
|
if center1.distance(center2) < radius1 + radius2 {
|
|
return Some((center1 + center2) / 2.);
|
|
}
|
|
None
|
|
}
|
|
|
|
#[derive(Debug, Clone)]
|
|
pub struct RayOBBIntersection {
|
|
pub hitpoint: VectorPair,
|
|
pub normal: VectorPair,
|
|
}
|
|
impl RayOBBIntersection {
|
|
pub fn new(hitpoint: VectorPair, normal: VectorPair) -> RayOBBIntersection {
|
|
RayOBBIntersection { hitpoint, normal }
|
|
}
|
|
}
|
|
|
|
pub fn ray_obb_intersection(
|
|
ray_origin: Vec3,
|
|
ray_direction_normalized: Vec3,
|
|
bounds: &OBB,
|
|
) -> Option<RayOBBIntersection> {
|
|
let transform: Transform = Transform::from_translation(bounds.position)
|
|
.with_rotation(bounds.orientation)
|
|
.with_scale(bounds.half_size * 2.);
|
|
let matrix = transform.compute_matrix().inverse();
|
|
let ro_trans: Vec3 = matrix.transform_point3(ray_origin);
|
|
let rd_trans: Vec3 = matrix
|
|
.transform_vector3(ray_direction_normalized)
|
|
.normalize();
|
|
let passthru =
|
|
distance_to_ray_bounds_intersection(ro_trans, rd_trans, Vec3::ZERO, Vec3::splat(0.5));
|
|
if passthru.is_some() {
|
|
let pt = passthru.unwrap();
|
|
let hitpoint = ray_origin + (ray_direction_normalized * pt.0 * (bounds.half_size * 2.)); // bounds.half_size scales back out of normalized unit-space
|
|
let hitpoint_ro = ro_trans + (rd_trans * pt.0);
|
|
let hit_vp = VectorPair::new(hitpoint, hitpoint_ro);
|
|
/*
|
|
let mut dist_array: [(Vec3, f32); 6] = Default::default();
|
|
let mut min_dist: usize = 8;
|
|
for (index, norm) in crate::vvlib::constants::cardinal_directions::ORDERED
|
|
.iter()
|
|
.enumerate()
|
|
{
|
|
let pos = norm.clone() * bounds.half_size * 2.;
|
|
let pos = pos + bounds.position;
|
|
let pos = matrix.transform_vector3(pos);
|
|
let dist = pos.distance(hitpoint);
|
|
dist_array[index] = (norm.clone(), dist);
|
|
if index == 0 {
|
|
min_dist = index;
|
|
} else if dist_array[min_dist].1 < dist {
|
|
min_dist = index;
|
|
}
|
|
}
|
|
let normal = dist_array[min_dist].0;
|
|
*/
|
|
let normal_vp = VectorPair::new(matrix.transpose().transform_vector3(pt.1), pt.1);
|
|
return Some(RayOBBIntersection::new(hit_vp, normal_vp));
|
|
} else {
|
|
return None;
|
|
}
|
|
}
|
|
|
|
pub fn distance_to_ray_bounds_intersection(
|
|
ray_origin: Vec3,
|
|
ray_direction_normalized: Vec3,
|
|
bound_center: Vec3,
|
|
bound_half_size: Vec3,
|
|
) -> Option<(f32, Vec3)> {
|
|
let bound_min: Vec3 = bound_center - bound_half_size;
|
|
let bound_max: Vec3 = bound_center + bound_half_size;
|
|
let tmin: Vec3 = (bound_min - ray_origin) / ray_direction_normalized;
|
|
let tmax: Vec3 = (bound_max - ray_origin) / ray_direction_normalized;
|
|
let sc = tmin.min(tmax);
|
|
let sf = tmin.max(tmax);
|
|
let t0: f32 = sc.x.max(sc.y).max(sc.z);
|
|
let t1: f32 = sf.x.min(sf.y).min(sf.z);
|
|
|
|
if !(t0 <= t1 && t1 > 0.0) {
|
|
return None;
|
|
}
|
|
|
|
let n =
|
|
-ray_direction_normalized.signum() * step(sc.yzx(), sc.xyz()) * step(sc.zxy(), sc.xyz());
|
|
let n = n.normalize();
|
|
return Some((t0, n));
|
|
}
|
|
pub fn step(l: Vec3, r: Vec3) -> Vec3 {
|
|
let v = l.cmple(r);
|
|
Vec3 {
|
|
x: v.x as i32 as f32,
|
|
y: v.y as i32 as f32,
|
|
z: v.z as i32 as f32,
|
|
}
|
|
}
|
|
|
|
pub fn ray_aabb_intersection(
|
|
ray_origin: Vec3,
|
|
ray_direction_normalized: Vec3,
|
|
bound_center: Vec3,
|
|
bound_half_size: Vec3,
|
|
) -> Option<RayAABBIntersection> {
|
|
let passthru = distance_to_ray_bounds_intersection(
|
|
ray_origin,
|
|
ray_direction_normalized,
|
|
bound_center,
|
|
bound_half_size,
|
|
);
|
|
if passthru.is_some() {
|
|
let hitpoint = ray_origin + (ray_direction_normalized * passthru.unwrap().0);
|
|
/*
|
|
let mut dist_array: [(Vec3, f32); 6] = Default::default();
|
|
let mut min_dist: usize = 8;
|
|
for (index, norm) in crate::vvlib::constants::cardinal_directions::ORDERED
|
|
.iter()
|
|
.enumerate()
|
|
{
|
|
let pos = norm.clone() * bound_half_size * 2.;
|
|
let pos = pos + bound_center;
|
|
let dist = pos.distance(hitpoint);
|
|
dist_array[index] = (norm.clone(), dist);
|
|
if index == 0 {
|
|
min_dist = index;
|
|
} else if dist_array[min_dist].1 > dist {
|
|
min_dist = index;
|
|
}
|
|
}
|
|
let normal = dist_array[min_dist].0;
|
|
*/
|
|
return Some(RayAABBIntersection::new(hitpoint, passthru.unwrap().1));
|
|
} else {
|
|
return None;
|
|
}
|
|
}
|
|
|
|
pub fn sphere_obb_intersection(
|
|
sphere_center: Vec3,
|
|
sphere_radius: f32,
|
|
bounds: &OBB,
|
|
) -> Option<Vec3> {
|
|
let mut closest_point = bounds.position;
|
|
{
|
|
// find closest point
|
|
let matrix = Mat3::from_quat(bounds.orientation);
|
|
let dir = sphere_center - bounds.position;
|
|
let cols = matrix.to_cols_array();
|
|
let obb_size = bounds.half_size.to_array();
|
|
|
|
for i in 0..3 {
|
|
let axis = Vec3::new(cols[i * 3], cols[i * 3 + 1], cols[i * 3 + 2]);
|
|
let mut distance = dir.dot(axis);
|
|
|
|
if distance > obb_size[i] {
|
|
distance = obb_size[i];
|
|
}
|
|
|
|
if distance < -obb_size[i] {
|
|
distance = -obb_size[i];
|
|
}
|
|
closest_point += axis * distance;
|
|
}
|
|
}
|
|
|
|
let mut distsq = (sphere_center - closest_point).length();
|
|
distsq *= distsq;
|
|
let radiussq = sphere_radius * sphere_radius;
|
|
if distsq < radiussq {
|
|
return Some(closest_point);
|
|
}
|
|
|
|
None
|
|
}
|
|
|
|
pub fn ray_sphere_intersection(
|
|
ray_origin: Vec3,
|
|
ray_direction_normalized: Vec3,
|
|
sphere_center: Vec3,
|
|
sphere_radius: f32,
|
|
) -> Option<Vec3> {
|
|
let e = sphere_center - ray_origin;
|
|
let rsq = sphere_radius * sphere_radius;
|
|
let esq = e.length() * e.length();
|
|
let a = e.dot(ray_direction_normalized);
|
|
let bsq = esq - (a * a);
|
|
let f = (((rsq) - bsq).abs()).sqrt();
|
|
let mut t = a - f;
|
|
|
|
if rsq - (esq - a * a) < 0.0 {
|
|
return None;
|
|
} else if esq < rsq {
|
|
t = a + f;
|
|
}
|
|
|
|
return Some(ray_origin + (ray_direction_normalized * t));
|
|
}
|
|
|
|
pub fn obb_to_global_sphere(local_pos: Vec3, size: usize, trans: &OBB) -> (Vec3, f32) {
|
|
//let local_pos = trans.orientation.mul_vec3(local_pos);
|
|
let bound_radius = Vec3::splat(size as f32 / 2.).length() * trans.half_size.length();
|
|
let position = trans
|
|
.as_transform()
|
|
.compute_matrix()
|
|
.transform_point(local_pos);
|
|
return (position, bound_radius);
|
|
}
|
|
|
|
pub fn obb_obb_overlap(first: &OBB, second: &OBB) -> bool {
|
|
let r_pos = second.position - first.position;
|
|
return !(get_seperating_plane_obb(r_pos, first.axis_x(), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_y(), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_z(), first, second)
|
|
|| get_seperating_plane_obb(r_pos, second.axis_x(), first, second)
|
|
|| get_seperating_plane_obb(r_pos, second.axis_y(), first, second)
|
|
|| get_seperating_plane_obb(r_pos, second.axis_z(), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_x().cross(second.axis_x()), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_x().cross(second.axis_y()), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_x().cross(second.axis_z()), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_y().cross(second.axis_x()), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_y().cross(second.axis_y()), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_y().cross(second.axis_z()), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_z().cross(second.axis_x()), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_z().cross(second.axis_y()), first, second)
|
|
|| get_seperating_plane_obb(r_pos, first.axis_z().cross(second.axis_z()), first, second));
|
|
}
|
|
|
|
fn get_seperating_plane_obb(r_pos: Vec3, plane: Vec3, first: &OBB, second: &OBB) -> bool {
|
|
return r_pos.dot(plane).abs()
|
|
> ((first.axis_x() * first.half_size.x).dot(plane).abs()
|
|
+ (first.axis_y() * first.half_size.y).dot(plane).abs()
|
|
+ (first.axis_z() * first.half_size.z).dot(plane).abs()
|
|
+ (second.axis_x() * second.half_size.x).dot(plane).abs()
|
|
+ (second.axis_y() * second.half_size.y).dot(plane).abs()
|
|
+ (second.axis_z() * second.half_size.z).dot(plane).abs());
|
|
}
|
|
|
|
pub fn octtree_ray_overlap<T: Clone>(
|
|
oct: &OctTree<T>,
|
|
transform: &OBB,
|
|
ray_origin: Vec3,
|
|
ray_normal: Vec3,
|
|
) -> Option<RayOctTreeIntersection> {
|
|
let x = ray_octtree_visitor(
|
|
&oct.root,
|
|
transform,
|
|
ray_origin,
|
|
ray_normal,
|
|
oct.center,
|
|
oct.size,
|
|
Default::default(),
|
|
);
|
|
if x.is_some() {
|
|
let x = x.unwrap();
|
|
let path = x.1;
|
|
//let matrix = transform.as_transform().compute_matrix();
|
|
let result = RayOctTreeIntersection {
|
|
hit_data: x.0,
|
|
path_to_hit: path.clone(),
|
|
voxel_index_location: path.to_vec3(oct.center, oct.size),
|
|
};
|
|
return Some(result);
|
|
} else {
|
|
None
|
|
}
|
|
}
|
|
fn ray_octtree_visitor<T: Clone>(
|
|
node: &TreeNode<T>,
|
|
transform: &OBB,
|
|
ray_origin: Vec3,
|
|
ray_normal: Vec3,
|
|
position: Vec3,
|
|
size: usize,
|
|
parent_path: Path,
|
|
) -> Option<(RayOBBIntersection, Path)> {
|
|
if node.is_branch() {
|
|
let children = node.branch_contents_reference();
|
|
if children.is_none() {
|
|
return None;
|
|
}
|
|
let children = children.unwrap();
|
|
let mut elements: Vec<(Vec3, usize, &TreeNode<T>, usize)> = Vec::new();
|
|
for (index, element) in children.iter().enumerate() {
|
|
if !element.is_empty() {
|
|
let (sub_position, sub_size) = constants::sub_region(position, size, index as i32);
|
|
elements.push((sub_position, sub_size, element, index));
|
|
}
|
|
}
|
|
elements.sort_by(|f, s| {
|
|
let first_transformed = transform
|
|
.as_transform()
|
|
.compute_matrix()
|
|
.transform_point3(f.0);
|
|
let second_transformed = transform
|
|
.as_transform()
|
|
.compute_matrix()
|
|
.transform_point3(s.0);
|
|
let fdist = first_transformed.distance(ray_origin);
|
|
let sdist = second_transformed.distance(ray_origin);
|
|
fdist.total_cmp(&sdist)
|
|
});
|
|
for element in elements {
|
|
let mut child_path = parent_path.clone();
|
|
child_path.append(element.3);
|
|
let x = ray_octtree_visitor(
|
|
element.2, transform, ray_origin, ray_normal, element.0, element.1, child_path,
|
|
);
|
|
if x.is_some() {
|
|
return x;
|
|
}
|
|
}
|
|
} else if node.is_leaf() {
|
|
let mut node_bounds = transform.clone();
|
|
let tposition = transform
|
|
.as_transform()
|
|
.compute_matrix()
|
|
.transform_point3(position);
|
|
node_bounds.position = tposition;
|
|
node_bounds.half_size = node_bounds.half_size; // * size as f32; size is 1 for leaves
|
|
|
|
let x = ray_obb_intersection(ray_origin, ray_normal, &node_bounds);
|
|
if x.is_some() {
|
|
let mut x = x.unwrap();
|
|
x.hitpoint.local += position;
|
|
return Some((x, parent_path));
|
|
}
|
|
}
|
|
//v nothing could be found, failing out so other searches can continue
|
|
None
|
|
}
|
|
|
|
#[test]
|
|
pub fn test_ray_x_octtree() {
|
|
use bevy::math::Quat;
|
|
let mut oct = OctTree::new(Vec3::new(0., 1., 0.), 0);
|
|
oct.set_voxel_at_location(Vec3::new(1., 0., 0.), 1);
|
|
let obby = OBB::new(
|
|
Vec3::splat(5.),
|
|
Quat::from_rotation_y(f32::to_radians(25.))
|
|
* Quat::from_rotation_z(f32::to_radians(25.))
|
|
* Quat::from_rotation_x(f32::to_radians(25.)),
|
|
Vec3::splat(1.),
|
|
);
|
|
let ray_origin = Vec3::ZERO;
|
|
let ray_normal = Vec3::splat(5.).normalize();
|
|
let collision = octtree_ray_overlap(&oct, &obby, ray_origin, ray_normal);
|
|
let path_local = collision
|
|
.clone()
|
|
.unwrap()
|
|
.path_to_hit
|
|
.to_vec3(oct.center, oct.size);
|
|
dbg!(path_local);
|
|
if collision.is_some() {
|
|
dbg!(collision.clone().unwrap());
|
|
let col = collision.unwrap();
|
|
let hitloc = col.hit_data.hitpoint.local.clone();
|
|
let hitnorm = col.hit_data.normal.local.clone();
|
|
let hitvox = hitloc - (hitnorm * 0.5);
|
|
dbg!(hitvox.round());
|
|
assert_eq!(hitvox.round(), path_local);
|
|
}
|
|
|
|
{
|
|
let mut oct = OctTree::new(Vec3::new(0., 10., 0.), 10);
|
|
oct.set_voxel_at_location(Vec3::ZERO, 0);
|
|
let obby = OBB::new(Vec3::new(2., -10., 0.), Quat::IDENTITY, Vec3::splat(0.5));
|
|
let ray_origin = Vec3::new(0., 0., 0.);
|
|
let ray_normal = Vec3::X;
|
|
let collision = octtree_ray_overlap(&oct, &obby, ray_origin, ray_normal);
|
|
|
|
if collision.is_some() {
|
|
let path_local = collision
|
|
.clone()
|
|
.unwrap()
|
|
.path_to_hit
|
|
.to_vec3(oct.center, oct.size);
|
|
dbg!(path_local);
|
|
dbg!(collision.clone().unwrap());
|
|
let col = collision.unwrap();
|
|
let hitloc = col.hit_data.hitpoint.local.clone();
|
|
let hitnorm = col.hit_data.normal.local.clone();
|
|
let hitvox = hitloc - (hitnorm * 0.5);
|
|
dbg!(hitvox.round());
|
|
assert_eq!(hitvox.round(), path_local);
|
|
}
|
|
}
|
|
}
|
|
#[test]
|
|
pub fn test_ray_x_bounding_collisions() {
|
|
use bevy::math::Quat;
|
|
let abounds: OBB = OBB::new(
|
|
Vec3::new(10., 0., 0.),
|
|
Quat::from_rotation_y(f32::to_radians(90.)),
|
|
Vec3::splat(5.),
|
|
);
|
|
let aabb_test = ray_aabb_intersection(Vec3::ZERO, Vec3::X, abounds.position, abounds.half_size);
|
|
let obb_test = ray_obb_intersection(Vec3::ZERO, Vec3::X, &abounds);
|
|
|
|
assert_eq!(aabb_test.clone().unwrap().hitpoint, Vec3::new(5., 0., 0.));
|
|
assert_eq!(aabb_test.unwrap().normal, Vec3::new(-1., 0., 0.));
|
|
|
|
assert_eq!(
|
|
obb_test.unwrap().hitpoint.global.round(),
|
|
Vec3::new(5., 0., 0.)
|
|
);
|
|
|
|
let rotated_bounds: OBB = OBB::new(
|
|
Vec3::new(20., 0., 0.),
|
|
Quat::from_rotation_z(f32::to_radians(45.0)),
|
|
Vec3::splat(5.),
|
|
);
|
|
let rotated_obb_test = ray_obb_intersection(Vec3::ZERO, Vec3::X, &rotated_bounds).unwrap();
|
|
dbg!(rotated_obb_test.clone());
|
|
assert_eq!(rotated_obb_test.hitpoint.global.round().x, 13.);
|
|
assert_eq!(
|
|
rotated_obb_test.normal.local.normalize(),
|
|
Vec3::new(-0.7071068, 0.7071068, -0.0)
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
pub fn test_sphere_collisions() {
|
|
let x = sphere_sphere_overlap(Vec3::new(0., 6., 0.), 4., Vec3::new(0., 12., 0.), 9.5);
|
|
println!("{}", x.unwrap());
|
|
assert_eq!(x, Some(Vec3::new(0., 9., 0.)));
|
|
}
|
|
#[test]
|
|
pub fn test_obb_collisions() {
|
|
use bevy::math::Quat;
|
|
{
|
|
let first = OBB::new(
|
|
Vec3::new(0., 0., 0.),
|
|
Quat::from_rotation_y(0.),
|
|
Vec3::splat(5.),
|
|
);
|
|
let second = OBB::new(
|
|
Vec3::new(11., 0., 0.),
|
|
Quat::from_rotation_y(0.785398),
|
|
Vec3::splat(5.),
|
|
);
|
|
assert!(obb_obb_overlap(&first, &second));
|
|
}
|
|
{
|
|
let first = OBB::new(
|
|
Vec3::new(0., 0., 0.),
|
|
Quat::from_rotation_y(0.),
|
|
Vec3::splat(5.),
|
|
);
|
|
let second = OBB::new(
|
|
Vec3::new(11., 0., 0.),
|
|
Quat::from_rotation_y(0.),
|
|
Vec3::splat(5.),
|
|
);
|
|
assert!(!obb_obb_overlap(&first, &second));
|
|
}
|
|
}
|
|
#[test]
|
|
pub fn test_ray_x_sphere_collisions() {
|
|
let ro = Vec3::ZERO;
|
|
let rd = Vec3::X;
|
|
let sc = Vec3::new(10., 0., 0.);
|
|
let sr: f32 = 4.;
|
|
let result = ray_sphere_intersection(ro, rd, sc, sr);
|
|
assert_eq!(result.unwrap(), Vec3::new(6., 0., 0.));
|
|
}
|
|
#[test]
|
|
pub fn test_sphere_x_obb_collisions() {
|
|
use bevy::math::Quat;
|
|
let obb = OBB::new(
|
|
Vec3::ZERO,
|
|
Quat::from_rotation_y(f32::to_radians(45.)),
|
|
Vec3::splat(4.),
|
|
);
|
|
let sc = Vec3::new((std::f32::consts::SQRT_2 * -4.) - 1., 0., 0.);
|
|
let sr = 1.1;
|
|
let result = sphere_obb_intersection(sc, sr, &obb);
|
|
let correct = Vec3::new(-5.656854, 0., 0.);
|
|
assert!(correct.abs_diff_eq(result.unwrap(), 0.001));
|
|
}
|
|
|
|
#[test]
|
|
pub fn test_oct_x_oct_collisions() {
|
|
use bevy::math::Quat;
|
|
let oct1 = OctTree::new(Vec3::splat(10.), 10);
|
|
let oct2 = OctTree::new(Vec3::splat(10.), 1);
|
|
let x = octtree_overlap(
|
|
&oct1,
|
|
&OBB::new(Vec3::ZERO, Quat::IDENTITY, Vec3::ONE / 2.),
|
|
&oct2,
|
|
&OBB::new(Vec3::ZERO, Quat::IDENTITY, Vec3::ONE / 2.),
|
|
);
|
|
assert!(x.is_some());
|
|
|
|
let mut oct1 = OctTree::new(Vec3::splat(10.), 10);
|
|
oct1.set_voxel_at_location(Vec3::new(1., 0., 1.), 1);
|
|
let mut oct2 = OctTree::new(Vec3::splat(0.), 10);
|
|
oct2.set_voxel_at_location(Vec3::new(0., 10., 0.), 1);
|
|
|
|
let x = octtree_overlap(
|
|
&oct1,
|
|
&OBB::new(Vec3::ZERO, Quat::IDENTITY, Vec3::ONE / 2.),
|
|
&oct2,
|
|
&OBB::new(
|
|
Vec3::ZERO,
|
|
Quat::from_rotation_x(f32::to_radians(45.)),
|
|
Vec3::ONE / 2.,
|
|
),
|
|
);
|
|
assert!(x.is_some());
|
|
let x = x.unwrap();
|
|
//dbg!(&x);
|
|
assert_eq!(
|
|
x[0],
|
|
(
|
|
OctTreeCollisionPoint {
|
|
pos: VectorPair::new(Vec3::new(1.0, 0.0, 1.0,), Vec3::new(1.0, 0.0, 1.0,)),
|
|
path: Path {
|
|
packed: 0,
|
|
length: 0,
|
|
},
|
|
},
|
|
OctTreeCollisionPoint {
|
|
pos: VectorPair::new(Vec3::new(0.0, 0.0, 0.0,), Vec3::new(0.0, 0.0, 0.0,)),
|
|
path: Path {
|
|
packed: 0,
|
|
length: 0,
|
|
},
|
|
},
|
|
),
|
|
);
|
|
|
|
{
|
|
let mut oct1 = OctTree::new(Vec3::splat(10.), 10);
|
|
let mut oct2 = OctTree::new(Vec3::splat(-10.), 10);
|
|
oct1.set_voxel_at_location(Vec3::splat(1.), 1);
|
|
oct2.set_voxel_at_location(Vec3::splat(1.), 1);
|
|
let orient1 = OBB::new(
|
|
Vec3::splat(5.),
|
|
Quat::from_rotation_x(f32::to_radians(10.)),
|
|
Vec3::splat(1.),
|
|
);
|
|
let orient2 = OBB::new(
|
|
Vec3::splat(5.),
|
|
Quat::from_rotation_x(f32::to_radians(-10.)),
|
|
Vec3::splat(1.),
|
|
);
|
|
let x = octtree_overlap(&oct1, &orient1, &oct2, &orient2);
|
|
assert!(x.is_some());
|
|
assert_eq!(
|
|
x.unwrap()[0],
|
|
(
|
|
OctTreeCollisionPoint {
|
|
pos: VectorPair::new(
|
|
Vec3::new(7.0, 6.622319, 7.3169117,),
|
|
Vec3::new(1.0, 1.0, 1.0,)
|
|
),
|
|
path: Path {
|
|
packed: 0,
|
|
length: 0,
|
|
},
|
|
},
|
|
OctTreeCollisionPoint {
|
|
pos: VectorPair::new(
|
|
Vec3::new(7.0, 7.3169117, 6.622319,),
|
|
Vec3::new(1.0, 1.0, 1.0,),
|
|
),
|
|
path: Path {
|
|
packed: 0,
|
|
length: 0,
|
|
},
|
|
},
|
|
)
|
|
);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
pub fn test_sphere_internal_transform() {
|
|
use bevy::math::Quat;
|
|
let pos = Vec3::new(0., 10., 0.);
|
|
let size = 10;
|
|
let obby = OBB::new(
|
|
Vec3::new(0., 10., 0.),
|
|
Quat::from_rotation_x(f32::to_radians(90.)),
|
|
Vec3::new(1., 1., 1.),
|
|
);
|
|
let (x, y) = obb_to_global_sphere(pos, size, &obby);
|
|
println!("{x}, {y:.3}");
|
|
let z = x.y;
|
|
println!("{z:.3}");
|
|
let x = x.round();
|
|
let y = y.round();
|
|
assert_eq!(x, Vec3::new(0., 10., 20.));
|
|
assert_eq!(y, 15.);
|
|
}
|