most basic oct-tree oct-tree collision test case passes

src/oct_tree.rs

@@ -1,11 +1,12 @@
 #![allow(dead_code)]
 use bevy::math::{Mat3, Quat, Vec3};
 use bevy::prelude::Transform;
+use bevy::transform::TransformPoint;
 
 use crate::oct_tree::constants::to_region_id;
 
 use self::constants::sub_region;
-use std::f32::consts::SQRT_2;
+//use std::f32::consts::SQRT_2; // <- in test
 use std::mem::swap;
 
 #[derive(Clone, Default, Debug)]
@@ -16,6 +17,39 @@ enum TreeNode<T> {
     Empty,
 }
 
+impl<T: Clone> TreeNode<T> {
+    pub fn is_branch(&self) -> bool {
+        match self {
+            TreeNode::Branch(_) => true,
+            _ => false,
+        }
+    }
+    pub fn is_leaf(&self) -> bool {
+        match self {
+            TreeNode::Leaf(_) => true,
+            _ => false,
+        }
+    }
+    pub fn is_empty(&self) -> bool {
+        match self {
+            TreeNode::Empty => true,
+            _ => false,
+        }
+    }
+    pub fn branch_contents_reference(&self) -> Option<&[TreeNode<T>; 8]> {
+        match self {
+            TreeNode::Branch(a) => Some(a.as_ref()),
+            _ => None,
+        }
+    }
+    pub fn branch_contents_mutable(&mut self) -> Option<&mut [TreeNode<T>; 8]> {
+        match self {
+            TreeNode::Branch(a) => Some(a.as_mut()),
+            _ => None,
+        }
+    }
+}
+
 #[derive(Debug)]
 pub struct OctTree<T> {
     size: usize,
@@ -509,39 +543,6 @@ impl<T: Clone> OctTree<T> {
     }
 }
 
-impl<T: Clone> TreeNode<T> {
-    pub fn is_branch(&self) -> bool {
-        match self {
-            TreeNode::Branch(_) => true,
-            _ => false,
-        }
-    }
-    pub fn is_leaf(&self) -> bool {
-        match self {
-            TreeNode::Leaf(_) => true,
-            _ => false,
-        }
-    }
-    pub fn is_empty(&self) -> bool {
-        match self {
-            TreeNode::Empty => true,
-            _ => false,
-        }
-    }
-    pub fn branch_contents_reference(&self) -> Option<&[TreeNode<T>; 8]> {
-        match self {
-            TreeNode::Branch(a) => Some(a.as_ref()),
-            _ => None,
-        }
-    }
-    pub fn branch_contents_mutable(&mut self) -> Option<&mut [TreeNode<T>; 8]> {
-        match self {
-            TreeNode::Branch(a) => Some(a.as_mut()),
-            _ => None,
-        }
-    }
-}
-
 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.);
@@ -566,6 +567,7 @@ pub fn sphere_sphere_overlap(
     None
 }
 
+#[derive(Copy, Clone)]
 pub struct OBB {
     position: Vec3,
     orientation: Quat,
@@ -589,10 +591,13 @@ impl OBB {
     pub fn axis_z(&self) -> Vec3 {
         return self.orientation.mul_vec3(Vec3::Z);
     }
+    pub fn as_transform(&self) -> Transform {
+        Transform::from_translation(self.position)
+            .with_rotation(self.orientation)
+            .with_scale(self.half_size * 2.)
+    }
 }
 
-//***************
-
 pub fn ray_obb_intersection(
     ray_origin: Vec3,
     ray_direction_normalized: Vec3,
@@ -614,8 +619,6 @@ pub fn ray_obb_intersection(
     }
 }
 
-////.
-///
 pub fn distance_to_ray_bounds_intersection(
     ray_origin: Vec3,
     ray_direction_normalized: Vec3,
@@ -684,36 +687,6 @@ pub fn sphere_obb_intersection(
     sphere_radius: f32,
     bounds: &OBB,
 ) -> Option<Vec3> {
-    /*
-        Point closestPoint = ClosestPoint(obb, sphere.position);
-        float distSq = MagnitudeSq(sphere.position - closestPoint);
-        float radiusSq = sphere.radius * sphere.radius;
-        return distSq < radiusSq;
-    */
-
-    /*
-    Point ClosestPoint(const OBB& obb, const Point& point) {
-       Point result = obb.position;
-       vec3 dir = point - obb.position;
-
-       for (int i = 0; i < 3; ++i) {
-           const float* orientation = &obb.orientation.asArray[i * 3];
-           vec3 axis(orientation[0], orientation[1], orientation[2]);
-
-           float distance = Dot(dir, axis);
-
-           if (distance > obb.size.asArray[i]) {
-               distance = obb.size.asArray[i];
-           }
-           if (distance < -obb.size.asArray[i]) {
-               distance = -obb.size.asArray[i];
-           }
-
-           result = result + (axis * distance);
-       }
-
-       return result;
-    */
     let mut closest_point = bounds.position;
     {
         // find closest point
@@ -747,7 +720,222 @@ pub fn sphere_obb_intersection(
     None
 }
 
-//***************
+pub struct OctTreeCollisionPoint {
+    pos_in_oct: Vec3,
+    pos_in_global: Vec3,
+    path: Path,
+}
+
+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 {
+        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(first_pos);
+            let b = second_trans.half_size * second_size as f32;
+            let mut obb2 = second_trans.clone();
+            obb2.half_size = a;
+            obb2.position = b;
+            let res = obb_obb_overlap(&obb1, &obb2);
+            if res {
+                // well,
+                let half1 = OctTreeCollisionPoint {
+                    pos_in_oct: first_pos,
+                    pos_in_global: obb1.position,
+                    path: Default::default(),
+                };
+                let half2 = OctTreeCollisionPoint {
+                    pos_in_oct: second_pos,
+                    pos_in_global: obb2.position,
+                    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() {
+                    if first_iter.is_branch() && second_iter.is_branch() {
+                        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);
+                        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 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(first_pos);
+                        let b = second_trans.half_size * second_size as f32;
+                        let mut obb2 = second_trans.clone();
+                        obb2.half_size = a;
+                        obb2.position = b;
+                        let res = obb_obb_overlap(&obb1, &obb2);
+                        if res {
+                            // well,
+                            let half1 = OctTreeCollisionPoint {
+                                pos_in_oct: first_pos,
+                                pos_in_global: obb1.position,
+                                path: Default::default(),
+                            };
+                            let half2 = OctTreeCollisionPoint {
+                                pos_in_oct: second_pos,
+                                pos_in_global: obb2.position,
+                                path: Default::default(),
+                            };
+                            results.push((half1, half2));
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+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);
+}
+#[test]
+pub fn test_sphere_internal_transform() {
+    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.);
+}
 
 pub fn obb_obb_overlap(first: &OBB, second: &OBB) -> bool {
     let r_pos = second.position - first.position;
@@ -777,6 +965,7 @@ fn get_seperating_plane_obb(r_pos: Vec3, plane: Vec3, first: &OBB, second: &OBB)
             + (second.axis_y() * second.half_size.y).dot(plane).abs()
             + (second.axis_z() * second.half_size.z).dot(plane).abs());
 }
+
 mod constants {
     use bevy::math::Vec3;
     pub const CORNER: f32 = 0.866025;
@@ -830,7 +1019,7 @@ mod constants {
         let new_size = size * 2.;
         let half_size = size / 2.;
         let subvec = from_index(subject);
-        //subvec *= -1.;
+        //subvec *= -1.; // former error. tf?
         let offset = Vec3 {
             x: subvec.x * half_size,
             y: subvec.y * half_size,
@@ -1062,12 +1251,24 @@ pub fn test_sphere_x_obb_collisions() {
         Quat::from_rotation_y(f32::to_radians(45.)),
         Vec3::splat(4.),
     );
-    let sc = Vec3::new((SQRT_2 * -4.) - 1., 0., 0.);
+    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() {
+    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());
+}
 /*
 use rand::Rng;
 #[test]