all collision primitives - only oct-tree recursive collisions to go

src/oct_tree.rs

@@ -1,11 +1,13 @@
 #![allow(dead_code)]
-use bevy::math::{Quat, Vec3};
+use bevy::math::{Mat3, Quat, Vec3};
 use bevy::prelude::Transform;
 
 use crate::oct_tree::constants::to_region_id;
 
 use self::constants::sub_region;
+use std::f32::consts::SQRT_2;
 use std::mem::swap;
+
 #[derive(Clone, Default, Debug)]
 enum TreeNode<T> {
     Branch(Box<[TreeNode<T>; 8]>),
@@ -635,6 +637,29 @@ pub fn distance_to_ray_bounds_intersection(
     return Some(t0);
 }
 
+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 ray_aabb_intersection(
     ray_origin: Vec3,
     ray_direction_normalized: Vec3,
@@ -654,6 +679,74 @@ pub fn ray_aabb_intersection(
     }
 }
 
+pub fn sphere_obb_intersection(
+    sphere_center: Vec3,
+    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
+        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 obb_obb_overlap(first: &OBB, second: &OBB) -> bool {
@@ -952,6 +1045,29 @@ pub fn test_ray_x_bounding_collisions() {
     let rotated_obb_test = ray_obb_intersection(Vec3::ZERO, Vec3::X, &rotated_bounds).unwrap();
     assert_eq!(rotated_obb_test.round().x, 13.);
 }
+
+#[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() {
+    let obb = OBB::new(
+        Vec3::ZERO,
+        Quat::from_rotation_y(f32::to_radians(45.)),
+        Vec3::splat(4.),
+    );
+    let sc = Vec3::new((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));
+}
 /*
 use rand::Rng;
 #[test]