From 5fc865bb584868ff4d04d398a65612e8c2dea975 Mon Sep 17 00:00:00 2001 From: Ugo Di Girolamo Date: Sat, 14 May 2022 19:07:57 -0400 Subject: [PATCH 1/3] Fix Triangle::contains_point fixes issue #75 --- src/shape/triangle.rs | 135 ++++++++++++++++++++++++++++++++++++------ 1 file changed, 118 insertions(+), 17 deletions(-) diff --git a/src/shape/triangle.rs b/src/shape/triangle.rs index cb4fa2f9..a87effe5 100644 --- a/src/shape/triangle.rs +++ b/src/shape/triangle.rs @@ -453,24 +453,50 @@ impl Triangle { /// Tests if a point is inside of this triangle. 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 d11 = p1p.dot(&p1p2); - let d12 = p2p.dot(&p2p3); - let d13 = p3p.dot(&p3p1); + 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. @@ -522,6 +548,7 @@ impl Triangle { } } + impl SupportMap for Triangle { #[inline] fn local_support_point(&self, dir: &Vector) -> Point { @@ -655,6 +682,7 @@ impl ConvexPolyhedron for Triangle { #[cfg(test)] mod test { use crate::shape::Triangle; + use crate::math::Real; use na::Point3; #[test] @@ -665,4 +693,77 @@ 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 + } + } + + } } From b37cedfaeb4c47814cdd2b8750095e432f5dbde5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 7 Dec 2022 10:13:47 +0100 Subject: [PATCH 2/3] Fix 2D Triangle::contains_point too --- src/shape/triangle.rs | 94 +++++++++++++++++++++++++++++++++++-------- 1 file changed, 78 insertions(+), 16 deletions(-) diff --git a/src/shape/triangle.rs b/src/shape/triangle.rs index a87effe5..53b26c79 100644 --- a/src/shape/triangle.rs +++ b/src/shape/triangle.rs @@ -452,8 +452,23 @@ impl Triangle { } /// Tests if a point is inside of this triangle. + #[cfg(feature = "dim2")] + pub fn contains_point(&self, p: &Point) -> bool { + 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 + } + + /// 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; + const EPS: Real = crate::math::DEFAULT_EPSILON; let vb = self.b - self.a; let vc = self.c - self.a; @@ -491,12 +506,11 @@ impl Triangle { 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; + 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. @@ -548,7 +562,6 @@ impl Triangle { } } - impl SupportMap for Triangle { #[inline] fn local_support_point(&self, dir: &Vector) -> Point { @@ -678,11 +691,53 @@ impl ConvexPolyhedron for Triangle { } */ -#[cfg(feature = "dim3")] +#[cfg(feature = "dim2")] #[cfg(test)] mod test { + use crate::math::Real; use crate::shape::Triangle; + use na::Point3; + + #[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; #[test] @@ -752,18 +807,25 @@ mod test { // 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 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 + 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 } } - } } From bd1cca9a42c025942ee7136715a25ca5bb23d08c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Wed, 7 Dec 2022 10:27:43 +0100 Subject: [PATCH 3/3] Fix warning --- src/shape/triangle.rs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/shape/triangle.rs b/src/shape/triangle.rs index 53b26c79..c15329c3 100644 --- a/src/shape/triangle.rs +++ b/src/shape/triangle.rs @@ -694,9 +694,8 @@ impl ConvexPolyhedron for Triangle { #[cfg(feature = "dim2")] #[cfg(test)] mod test { - use crate::math::Real; use crate::shape::Triangle; - use na::Point3; + use na::Point2; #[test] fn test_triangle_area() {