half-working ray x octtree, pre-refactor-pass

src/oct_tree.rs

@@ -2,6 +2,7 @@
 use bevy::math::{Mat3, Quat, Vec3};
 use bevy::prelude::Transform;
 use bevy::transform::TransformPoint;
+use bevy::utils::petgraph::graph::Node;
 
 use crate::oct_tree::constants::to_region_id;
 
@@ -629,8 +630,8 @@ pub fn ray_obb_intersection(
         }
         let normal = dist_array[min_dist].0;
         return Some(RayBoundsIntersection {
-            hitpoint: hitpoint,
-            untransformed_normal: normal,
+            global_hitpoint: hitpoint,
+            local_normal: normal,
         });
     } else {
         return None;
@@ -680,10 +681,18 @@ pub fn ray_sphere_intersection(
 
     return Some(ray_origin + (ray_direction_normalized * t));
 }
-#[derive(Clone)]
+#[derive(Clone, Debug)]
 pub struct RayBoundsIntersection {
-    hitpoint: Vec3,
-    untransformed_normal: Vec3,
+    global_hitpoint: Vec3,
+    local_normal: Vec3,
+}
+
+#[derive(Clone, Debug)]
+pub struct RayOctTreeIntersection {
+    hitpoint_local: Vec3,
+    hitpoint_global: Vec3,
+    local_normal: Vec3,
+    global_normal: Vec3,
 }
 
 pub fn ray_aabb_intersection(
@@ -715,8 +724,8 @@ pub fn ray_aabb_intersection(
         }
         let normal = dist_array[min_dist].0;
         return Some(RayBoundsIntersection {
-            hitpoint: hitpoint,
-            untransformed_normal: normal,
+            global_hitpoint: hitpoint,
+            local_normal: normal,
         });
     } else {
         return None;
@@ -768,6 +777,91 @@ pub struct OctTreeCollisionPoint {
     path: Path,
 }
 
+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,
+    );
+    if x.is_some() {
+        let x = x.unwrap();
+        let matrix = transform.as_transform().compute_matrix();
+        let local_hitpoint = matrix.transform_point3(x.global_hitpoint);
+        let global_normal = matrix.inverse().transform_vector3(x.local_normal);
+        let result = RayOctTreeIntersection {
+            hitpoint_local: local_hitpoint,
+            hitpoint_global: x.global_hitpoint,
+            local_normal: x.local_normal,
+            global_normal,
+        };
+        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,
+) -> Option<RayBoundsIntersection> {
+    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>)> = 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));
+            }
+        }
+        elements.sort_by(|f, s| {
+            let first_transformed = transform
+                .as_transform()
+                .compute_matrix()
+                .transform_point(f.0);
+            let second_transformed = transform
+                .as_transform()
+                .compute_matrix()
+                .transform_point(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 x = ray_octtree_visitor(
+                element.2, transform, ray_origin, ray_normal, element.0, element.1,
+            );
+            if x.is_some() {
+                return x;
+            }
+        }
+    } else if node.is_leaf() {
+        let mut node_bounds = transform.clone();
+        let position = transform
+            .as_transform()
+            .compute_matrix()
+            .transform_point(position);
+        node_bounds.position = position;
+        node_bounds.half_size = node_bounds.half_size * size as f32;
+
+        let x = ray_obb_intersection(ray_origin, ray_normal, &node_bounds);
+        if x.is_some() {
+            return x;
+        }
+    }
+    //v nothing could be found, failing out so other searches can continue
+    None
+}
+
 pub fn octtree_overlap<T: Clone>(
     first_oct: &OctTree<T>,
     first_trans: &OBB,
@@ -1263,13 +1357,16 @@ pub fn test_ray_x_bounding_collisions() {
     let obb_test: Option<RayBoundsIntersection> =
         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().untransformed_normal,
-        Vec3::new(-1., 0., 0.)
+        aabb_test.clone().unwrap().global_hitpoint,
+        Vec3::new(5., 0., 0.)
     );
+    assert_eq!(aabb_test.unwrap().local_normal, Vec3::new(-1., 0., 0.));
 
-    assert_eq!(obb_test.unwrap().hitpoint.round(), Vec3::new(5., 0., 0.));
+    assert_eq!(
+        obb_test.unwrap().global_hitpoint.round(),
+        Vec3::new(5., 0., 0.)
+    );
 
     let rotated_bounds: OBB = OBB::new(
         Vec3::new(20., 0., 0.),
@@ -1277,11 +1374,8 @@ pub fn test_ray_x_bounding_collisions() {
         Vec3::splat(5.),
     );
     let rotated_obb_test = ray_obb_intersection(Vec3::ZERO, Vec3::X, &rotated_bounds).unwrap();
-    assert_eq!(rotated_obb_test.hitpoint.round().x, 13.);
-    assert_eq!(
-        rotated_obb_test.untransformed_normal,
-        Vec3::new(0., -1., 0.)
-    );
+    assert_eq!(rotated_obb_test.global_hitpoint.round().x, 13.);
+    assert_eq!(rotated_obb_test.local_normal, Vec3::new(0., -1., 0.));
 }
 
 #[test]
@@ -1398,6 +1492,24 @@ pub fn test_oct_x_oct_collisions() {
         );
     }
 }
+#[test]
+pub fn test_ray_x_octtree() {
+    //A
+    let mut oct = OctTree::new(Vec3::new(0., 0., 0.), 0);
+    oct.set_voxel_at_location(Vec3::new(1., 1., 1.), 1);
+    let obby = OBB::new(
+        Vec3::splat(5.),
+        Quat::from_rotation_x(f32::to_radians(30.)),
+        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);
+    if collision.is_some() {
+        dbg!(collision.unwrap());
+    }
+    panic!();
+}
 /*
 use rand::Rng;
 #[test]