diff --git a/csg/src/plane.rs b/csg/src/plane.rs index e78103b..21e9033 100644 --- a/csg/src/plane.rs +++ b/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)); } }