Add alternative plane intersection in_halfspace function

csg/src/plane.rs

@@ -126,6 +126,42 @@ impl PlaneIntersection {
         dist < math::EPSILON
     }
 
+    pub fn in_halfspace_gauss(&self, plane: &Plane) -> bool {
+        let mut planes = [
+            self.planes[0].into_vec4(),
+            self.planes[1].into_vec4(),
+            self.planes[2].into_vec4(),
+            plane.into_vec4(),
+        ];
+
+        // Range of 3 for x/y/z
+        for i in 0..3 {
+            let pi = planes[i];
+            // Find something to swap with :)
+            if pi[i] == 0.0 {
+                for j in i..3 {
+                    // We found something to swap with!
+                    if planes[j][i] != 0.0 {
+                        planes.swap(i, j);
+                        break;
+                    }
+                }
+            }
+
+            // Do our elimination. Include r4 in this!
+            let a = pi[i];
+            for j in (i + 1)..4 {
+                let pj = planes[j];
+                if pj[i] != 0.0 {
+                    let b = pj[i];
+                    planes[j] = a * pj - b * pi;
+                }
+            }
+        }
+
+        planes[3][3] < math::EPSILON
+    }
+
     fn get_matrix(&self) -> Mat3 {
         glam::mat3(
             self.planes[0].normal,
@@ -217,6 +253,8 @@ mod plane_intersection_tests {
         let p1 = Plane::from_point_normal(Vec3::NEG_ONE, Vec3::NEG_ONE);
         let p2 = Plane::from_point_normal(Vec3::ZERO, Vec3::ONE);
         assert!(intersection.in_halfspace_mat(&p1));
+        assert!(intersection.in_halfspace_gauss(&p1));
         assert!(!intersection.in_halfspace_mat(&p2));
+        assert!(!intersection.in_halfspace_gauss(&p2));
     }
 }