Fix triangle intersection algorithm

src/editor/ray.rs

@@ -1,8 +1,9 @@
 use nalgebra::Vector3;
 
+#[derive(Debug)]
 pub struct Ray {
-    normal: Vector3<f32>,
-    origin: Vector3<f32>,
+    pub normal: Vector3<f32>,
+    pub origin: Vector3<f32>,
 }
 
 impl Ray {
@@ -13,8 +14,8 @@ impl Ray {
 
     pub fn plane_intersect(
         &self,
-        plane_origin: Vector3<f32>,
-        plane_normal: Vector3<f32>,
+        plane_origin: &Vector3<f32>,
+        plane_normal: &Vector3<f32>,
     ) -> Option<Vector3<f32>> {
         let a = plane_normal.dot(&self.normal);
 
@@ -33,32 +34,31 @@ impl Ray {
         Some(self.origin + self.normal.scale(t))
     }
 
+    // Algorithm from https://math.stackexchange.com/questions/4322/check-whether-a-point-is-within-a-3d-triangle#28552
     pub fn triangle_intersect(
         &self,
-        triangle_a: Vector3<f32>,
-        triangle_b: Vector3<f32>,
-        triangle_c: Vector3<f32>,
-        triangle_normal: Vector3<f32>,
+        a: &Vector3<f32>,
+        b: &Vector3<f32>,
+        c: &Vector3<f32>,
+        triangle_normal: &Vector3<f32>,
     ) -> Option<Vector3<f32>> {
-        let point = self.plane_intersect(triangle_a, triangle_normal);
+        let p = self.plane_intersect(a, triangle_normal);
 
-        if point.is_none() {
+        if p.is_none() {
             return None;
         }
 
-        let point = point.unwrap();
+        let p = p.unwrap();
 
-        let ab = triangle_a - triangle_b;
-        let ac = triangle_a - triangle_c;
-        let pa = point - triangle_a;
-        let pb = point - triangle_b;
-        let pc = point - triangle_c;
+        let n = (b - a).cross(&(c - a));
+        let nl = n.magnitude_squared();
+        let n_a = (c - b).cross(&(p - b));
+        let n_b = (a - c).cross(&(p - c));
+        let n_c = (b - a).cross(&(p - a));
 
-        let two_area_abc = ab.cross(&ac).magnitude();
-
-        let alpha = pb.cross(&pc).magnitude() / two_area_abc;
-        let beta = pc.cross(&pa).magnitude() / two_area_abc;
-        let gamma = 1.0 - alpha - beta;
+        let alpha = n.dot(&n_a) / nl;
+        let beta = n.dot(&n_b) / nl;
+        let gamma = n.dot(&n_c) / nl;
 
         if alpha >= 0.0
             && alpha <= 1.0
@@ -67,7 +67,7 @@ impl Ray {
             && gamma >= 0.0
             && gamma <= 1.0
         {
-            Some(point)
+            Some(p)
         } else {
             None
         }