diff --git a/src/shape/triangle.rs b/src/shape/triangle.rs index cb4fa2f9..c15329c3 100644 --- a/src/shape/triangle.rs +++ b/src/shape/triangle.rs @@ -452,25 +452,65 @@ impl Triangle { } /// Tests if a point is inside of this triangle. + #[cfg(feature = "dim2")] pub fn contains_point(&self, p: &Point) -> bool { - let p1p2 = self.b - self.a; - let p2p3 = self.c - self.b; - let p3p1 = self.a - self.c; - - let p1p = *p - self.a; - let p2p = *p - self.b; - let p3p = *p - self.c; + let ab = self.b - self.a; + let bc = self.c - self.b; + let ca = self.a - self.c; + let sgn1 = ab.perp(&(p - self.a)); + let sgn2 = bc.perp(&(p - self.b)); + let sgn3 = ca.perp(&(p - self.c)); + sgn1.signum() * sgn2.signum() >= 0.0 + && sgn1.signum() * sgn3.signum() >= 0.0 + && sgn2.signum() * sgn3.signum() >= 0.0 + } - let d11 = p1p.dot(&p1p2); - let d12 = p2p.dot(&p2p3); - let d13 = p3p.dot(&p3p1); + /// Tests if a point is inside of this triangle. + #[cfg(feature = "dim3")] + pub fn contains_point(&self, p: &Point) -> bool { + const EPS: Real = crate::math::DEFAULT_EPSILON; + + let vb = self.b - self.a; + let vc = self.c - self.a; + let vp = p - self.a; + + let n = vc.cross(&vb); + let n_norm = n.norm_squared(); + if n_norm < EPS || vp.dot(&n).abs() > EPS * n_norm { + // the triangle is degenerate or the + // point does not lie on the same plane as the triangle. + return false; + } - d11 >= 0.0 - && d11 <= p1p2.norm_squared() - && d12 >= 0.0 - && d12 <= p2p3.norm_squared() - && d13 >= 0.0 - && d13 <= p3p1.norm_squared() + // We are seeking B, C such that vp = vb * B + vc * C . + // If B and C are both in [0, 1] and B + C <= 1 then p is in the triangle. + // + // We can project this equation along a vector nb coplanar to the triangle + // and perpendicular to vb: + // vp.dot(nb) = vb.dot(nb) * B + vc.dot(nb) * C + // => C = vp.dot(nb) / vc.dot(nb) + // and similarly for B. + // + // In order to avoid divisions and sqrts we scale both B and C - so + // b = vb.dot(nc) * B and c = vc.dot(nb) * C - this results in harder-to-follow math but + // hopefully fast code. + + let nb = vb.cross(&n); + let nc = vc.cross(&n); + + let signed_blim = vb.dot(&nc); + let b = vp.dot(&nc) * signed_blim.signum(); + let blim = signed_blim.abs(); + + let signed_clim = vc.dot(&nb); + let c = vp.dot(&nb) * signed_clim.signum(); + let clim = signed_clim.abs(); + + return c >= 0.0 + && c <= clim + && b >= 0.0 + && b <= blim + && c * blim + b * clim <= blim * clim; } /// The normal of the given feature of this shape. @@ -651,9 +691,51 @@ impl ConvexPolyhedron for Triangle { } */ +#[cfg(feature = "dim2")] +#[cfg(test)] +mod test { + use crate::shape::Triangle; + use na::Point2; + + #[test] + fn test_triangle_area() { + let pa = Point2::new(5.0, 0.0); + let pb = Point2::new(0.0, 0.0); + let pc = Point2::new(0.0, 4.0); + + assert!(relative_eq!(Triangle::new(pa, pb, pc).area(), 10.0)); + } + + #[test] + fn test_triangle_contains_point() { + let tri = Triangle::new( + Point2::new(5.0, 0.0), + Point2::new(0.0, 0.0), + Point2::new(0.0, 4.0), + ); + + assert!(tri.contains_point(&Point2::new(1.0, 1.0))); + assert!(!tri.contains_point(&Point2::new(-1.0, 1.0))); + } + + #[test] + fn test_obtuse_triangle_contains_point() { + let tri = Triangle::new( + Point2::new(-10.0, 10.0), + Point2::new(0.0, 0.0), + Point2::new(20.0, 0.0), + ); + + assert!(tri.contains_point(&Point2::new(-3.0, 5.0))); + assert!(tri.contains_point(&Point2::new(5.0, 1.0))); + assert!(!tri.contains_point(&Point2::new(0.0, -1.0))); + } +} + #[cfg(feature = "dim3")] #[cfg(test)] mod test { + use crate::math::Real; use crate::shape::Triangle; use na::Point3; @@ -665,4 +747,84 @@ mod test { assert!(relative_eq!(Triangle::new(pa, pb, pc).area(), 10.0)); } + + #[test] + fn test_triangle_contains_point() { + let tri = Triangle::new( + Point3::new(0.0, 5.0, 0.0), + Point3::new(0.0, 0.0, 0.0), + Point3::new(0.0, 0.0, 4.0), + ); + + assert!(tri.contains_point(&Point3::new(0.0, 1.0, 1.0))); + assert!(!tri.contains_point(&Point3::new(0.0, -1.0, 1.0))); + } + + #[test] + fn test_obtuse_triangle_contains_point() { + let tri = Triangle::new( + Point3::new(-10.0, 10.0, 0.0), + Point3::new(0.0, 0.0, 0.0), + Point3::new(20.0, 0.0, 0.0), + ); + + assert!(tri.contains_point(&Point3::new(-3.0, 5.0, 0.0))); + assert!(tri.contains_point(&Point3::new(5.0, 1.0, 0.0))); + assert!(!tri.contains_point(&Point3::new(0.0, -1.0, 0.0))); + } + + #[test] + fn test_3dtriangle_contains_point() { + let o = Point3::new(0.0, 0.0, 0.0); + let pa = Point3::new(1.2, 1.4, 5.6); + let pb = Point3::new(1.5, 6.7, 1.9); + let pc = Point3::new(5.0, 2.1, 1.3); + + let tri = Triangle::new(pa, pb, pc); + + let va = pa - o; + let vb = pb - o; + let vc = pc - o; + + let n = (pa - pb).cross(&(pb - pc)); + + // This is a simple algorithm for generating points that are inside the + // triangle: o + (va * alpha + vb * beta + vc * gamma) is always inside the + // triangle if: + // * each of alpha, beta, gamma is in (0, 1) + // * alpha + beta + gamma = 1 + let contained_p = o + (va * 0.2 + vb * 0.3 + vc * 0.5); + let not_contained_coplanar_p = o + (va * -0.5 + vb * 0.8 + vc * 0.7); + let not_coplanar_p = o + (va * 0.2 + vb * 0.3 + vc * 0.5) + n * 0.1; + let not_coplanar_p2 = o + (va * -0.5 + vb * 0.8 + vc * 0.7) + n * 0.1; + assert!(tri.contains_point(&contained_p)); + assert!(!tri.contains_point(¬_contained_coplanar_p)); + assert!(!tri.contains_point(¬_coplanar_p)); + assert!(!tri.contains_point(¬_coplanar_p2)); + + // Test that points that are clearly within the triangle as seen as such, by testing + // a number of points along a line intersecting the triangle. + for i in -50i16..150 { + let a = 0.15; + let b = 0.01 * Real::from(i); // b ranges from -0.5 to 1.5 + let c = 1.0 - a - b; + let p = o + (va * a + vb * b + vc * c); + + match i { + ii if ii < 0 || ii > 85 => assert!( + !tri.contains_point(&p), + "Should not contain: i = {}, b = {}", + i, + b + ), + ii if ii > 0 && ii < 85 => assert!( + tri.contains_point(&p), + "Should contain: i = {}, b = {}", + i, + b + ), + _ => (), // Points at the edge may be seen as inside or outside + } + } + } }