OBBvsOBB collision function

src/oct_tree.rs

@@ -1,5 +1,6 @@
 #![allow(dead_code)]
-use bevy::math::Vec3;
+use bevy::math::{Quat, Vec3};
+use bevy::prelude::Transform;
 
 use crate::oct_tree::constants::to_region_id;
 
@@ -10,11 +11,11 @@ enum TreeNode<T> {
     Branch(Box<[TreeNode<T>; 8]>),
     Leaf(T),
     #[default]
-    None,
+    Empty,
 }
 
 #[derive(Debug)]
-struct OctTree<T> {
+pub struct OctTree<T> {
     size: usize,
     center: Vec3,
     root: TreeNode<T>,
@@ -26,6 +27,16 @@ struct Path {
     length: usize,
 }
 
+pub fn collision_test<T: Clone>(
+    _structure1: OctTree<T>,
+    _context1: Transform,
+    _structure2: OctTree<T>,
+    _context2: Transform,
+) -> Option<Vec<Vec3>> {
+    // TODO: actual function
+    None
+}
+
 impl Path {
     pub fn pop(&mut self) -> usize {
         self.length -= 1;
@@ -112,7 +123,7 @@ impl<T: Clone> OctTree<T> {
     }
     fn trim(&mut self, removal_count: &mut usize) {
         let count;
-        let mut extracted: &mut TreeNode<T> = &mut TreeNode::None;
+        let mut extracted: &mut TreeNode<T> = &mut TreeNode::Empty;
         match &mut self.root {
             TreeNode::Branch(a) => {
                 let contents = a.as_mut();
@@ -140,18 +151,18 @@ impl<T: Clone> OctTree<T> {
                 let ar: &mut [TreeNode<T>; 8] = a.as_mut();
                 Self::prune(&mut ar[i], target, count);
 
-                if ar.iter().all(|x| matches!(x, TreeNode::None)) {
-                    *element = TreeNode::None;
+                if ar.iter().all(|x| matches!(x, TreeNode::Empty)) {
+                    *element = TreeNode::Empty;
                     *count += 1;
                 }
             }
             TreeNode::Leaf(_) => {
                 if target.length == 0 {
-                    *element = TreeNode::None;
+                    *element = TreeNode::Empty;
                     *count += 1;
                 }
             }
-            TreeNode::None => return,
+            TreeNode::Empty => return,
         }
     }
 
@@ -195,7 +206,7 @@ impl<T: Clone> OctTree<T> {
     pub fn set_voxel_at_location(&mut self, location: Vec3, value: T) -> Path {
         // if the tree is empty, we want to fall out immediately - no looping needed.
         match self.root {
-            TreeNode::None => {
+            TreeNode::Empty => {
                 self.center = location;
                 self.size = 1;
                 self.root = TreeNode::Leaf(value);
@@ -205,11 +216,11 @@ impl<T: Clone> OctTree<T> {
         }
         let mut path: Path = Default::default();
         // if we don't already have a big enough non-empty tree, we need to branch upward till our tree contains our subject matter
-        if !bounds_contains(self.center, self.size, location) {
+        if !aa_bounds_contains(self.center, self.size, location) {
             println!("building up to contain addition {location}");
             let search_direction = constants::to_region_id(self.center, location);
 
-            while !bounds_contains(self.center, self.size, location) {
+            while !aa_bounds_contains(self.center, self.size, location) {
                 println!(
                     "{0}, size {1}, region {search_direction}",
                     self.center, self.size
@@ -221,7 +232,7 @@ impl<T: Clone> OctTree<T> {
                             constants::parent_region(self.center, self.size, search_direction);
                         let slot_within_new_branch =
                             constants::to_region_id(extension_center, self.center);
-                        let mut old: TreeNode<T> = TreeNode::None;
+                        let mut old: TreeNode<T> = TreeNode::Empty;
 
                         swap(&mut self.root, &mut old);
                         branch_extension[slot_within_new_branch as usize] = old;
@@ -234,10 +245,10 @@ impl<T: Clone> OctTree<T> {
                                 println!("root is a branch");
                             }
                             TreeNode::Leaf(_) => todo!(),
-                            TreeNode::None => todo!(),
+                            TreeNode::Empty => todo!(),
                         }
                     }
-                    TreeNode::None => {
+                    TreeNode::Empty => {
                         println!("something went wrong with early fall-out. shouldn't be plausible unless you're using unsafe rust.");
                         return Default::default();
                     }
@@ -292,7 +303,7 @@ impl<T: Clone> OctTree<T> {
                                 return path;
                             }
                         }
-                        TreeNode::None => {
+                        TreeNode::Empty => {
                             let branch_extension: [TreeNode<T>; 8] = Default::default();
                             let next_branch = TreeNode::Branch(Box::new(branch_extension));
                             *next_rc_loc = next_branch;
@@ -312,7 +323,7 @@ impl<T: Clone> OctTree<T> {
                         return Default::default();
                     }
                 }
-                TreeNode::None => {
+                TreeNode::Empty => {
                     println!(
                         "should be handled automatically before arrival at target node. returning"
                     );
@@ -406,7 +417,7 @@ impl<T: Clone> OctTree<T> {
             TreeNode::Leaf(value) => {
                 result.push((path, location, value.clone()));
             }
-            TreeNode::None => {
+            TreeNode::Empty => {
                 return;
             }
         }
@@ -448,7 +459,7 @@ impl<T: Clone> OctTree<T> {
     }
 
     pub fn voxel_at_location(&self, location: Vec3) -> Option<(T, Path)> {
-        if bounds_contains(self.center, self.size, location) {
+        if aa_bounds_contains(self.center, self.size, location) {
             let mut rc_center = self.center;
             let mut rc_size = self.size;
             let mut path_result: Path = Default::default();
@@ -475,7 +486,7 @@ impl<T: Clone> OctTree<T> {
                     TreeNode::Leaf(_) => {
                         panic!("tree not constructed correctly - non-unit voxel size");
                     }
-                    TreeNode::None => {
+                    TreeNode::Empty => {
                         println!("doesn't seem to exist");
                         return None; // does not exist.
                     }
@@ -496,7 +507,40 @@ impl<T: Clone> OctTree<T> {
     }
 }
 
-pub fn bounds_contains(center: Vec3, size: usize, subject: Vec3) -> bool {
+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.);
     let max = center + Vec3::splat(fsize as f32 / 2.);
@@ -508,8 +552,74 @@ pub fn bounds_contains(center: Vec3, size: usize, subject: Vec3) -> bool {
         && 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
+}
+
+pub struct OBB {
+    position: Vec3,
+    orientation: Quat,
+    half_size: Vec3,
+}
+
+impl OBB {
+    pub fn new(position: Vec3, orientation: Quat, half_size: Vec3) -> OBB {
+        return OBB {
+            position: position,
+            orientation: orientation,
+            half_size: half_size,
+        };
+    }
+    pub fn axis_x(&self) -> Vec3 {
+        return self.orientation.mul_vec3(Vec3::X);
+    }
+    pub fn axis_y(&self) -> Vec3 {
+        return self.orientation.mul_vec3(Vec3::Y);
+    }
+    pub fn axis_z(&self) -> Vec3 {
+        return self.orientation.mul_vec3(Vec3::Z);
+    }
+}
+
+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());
+}
 mod constants {
     use bevy::math::Vec3;
+    pub const CORNER: f32 = 0.866025;
     pub fn to_region_id(center: Vec3, subject: Vec3) -> i32 {
         if subject.x == center.x && subject.y == center.y && subject.z == center.z {
             return -1;
@@ -717,7 +827,41 @@ pub fn test_removal() {
     test_tree.remove_voxel(Vec3::ZERO);
     assert_eq!(test_tree.size, 1);
 }
-
+#[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() {
+    {
+        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(45.),
+            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));
+    }
+}
 /*
 use rand::Rng;
 #[test]