From f881e7c05081dd722bd965d37c04585608ce226d Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 18 Sep 2024 16:14:12 +0200 Subject: [PATCH 01/15] addressed a few simple todos --- src/bounding_volume/bounding_sphere_convex.rs | 4 +--- src/bounding_volume/bounding_sphere_convex_polygon.rs | 4 +--- src/bounding_volume/bounding_sphere_segment.rs | 4 +--- src/bounding_volume/bounding_sphere_triangle.rs | 4 +--- src/bounding_volume/bounding_sphere_utils.rs | 11 +++++------ src/lib.rs | 4 ++-- src/mass_properties/mass_properties_convex_polygon.rs | 2 -- src/query/closest_points/closest_points_line_line.rs | 2 +- src/query/epa/epa3.rs | 2 ++ src/utils/sdp_matrix.rs | 7 ++++--- 10 files changed, 18 insertions(+), 26 deletions(-) diff --git a/src/bounding_volume/bounding_sphere_convex.rs b/src/bounding_volume/bounding_sphere_convex.rs index 9885d0f0..793bd3ec 100644 --- a/src/bounding_volume/bounding_sphere_convex.rs +++ b/src/bounding_volume/bounding_sphere_convex.rs @@ -14,8 +14,6 @@ impl ConvexPolyhedron { /// Computes the local-space bounding sphere of this convex polyhedron. #[inline] pub fn local_bounding_sphere(&self) -> BoundingSphere { - let (center, radius) = bounding_volume::details::point_cloud_bounding_sphere(self.points()); - - BoundingSphere::new(center, radius) + bounding_volume::details::point_cloud_bounding_sphere(self.points()) } } diff --git a/src/bounding_volume/bounding_sphere_convex_polygon.rs b/src/bounding_volume/bounding_sphere_convex_polygon.rs index fafd2518..1f1479a8 100644 --- a/src/bounding_volume/bounding_sphere_convex_polygon.rs +++ b/src/bounding_volume/bounding_sphere_convex_polygon.rs @@ -14,8 +14,6 @@ impl ConvexPolygon { /// Computes the local-space bounding sphere of this convex polygon. #[inline] pub fn local_bounding_sphere(&self) -> BoundingSphere { - let (center, radius) = bounding_volume::details::point_cloud_bounding_sphere(self.points()); - - BoundingSphere::new(center, radius) + bounding_volume::details::point_cloud_bounding_sphere(self.points()) } } diff --git a/src/bounding_volume/bounding_sphere_segment.rs b/src/bounding_volume/bounding_sphere_segment.rs index 4ebc42e5..4069280a 100644 --- a/src/bounding_volume/bounding_sphere_segment.rs +++ b/src/bounding_volume/bounding_sphere_segment.rs @@ -15,8 +15,6 @@ impl Segment { #[inline] pub fn local_bounding_sphere(&self) -> BoundingSphere { let pts = [self.a, self.b]; - let (center, radius) = bounding_volume::details::point_cloud_bounding_sphere(&pts[..]); - - BoundingSphere::new(center, radius) + bounding_volume::details::point_cloud_bounding_sphere(&pts[..]) } } diff --git a/src/bounding_volume/bounding_sphere_triangle.rs b/src/bounding_volume/bounding_sphere_triangle.rs index e99fe9c7..936a3435 100644 --- a/src/bounding_volume/bounding_sphere_triangle.rs +++ b/src/bounding_volume/bounding_sphere_triangle.rs @@ -15,8 +15,6 @@ impl Triangle { #[inline] pub fn local_bounding_sphere(&self) -> BoundingSphere { let pts = [self.a, self.b, self.c]; - let (center, radius) = bounding_volume::details::point_cloud_bounding_sphere(&pts[..]); - - BoundingSphere::new(center, radius) + bounding_volume::details::point_cloud_bounding_sphere(&pts[..]) } } diff --git a/src/bounding_volume/bounding_sphere_utils.rs b/src/bounding_volume/bounding_sphere_utils.rs index 238f15c3..0a257bea 100644 --- a/src/bounding_volume/bounding_sphere_utils.rs +++ b/src/bounding_volume/bounding_sphere_utils.rs @@ -2,13 +2,14 @@ use crate::math::{Point, Real}; use crate::utils; use na::{self, ComplexField}; +use super::BoundingSphere; + /// Computes the bounding sphere of a set of point, given its center. -// TODO: return a bounding sphere? #[inline] pub fn point_cloud_bounding_sphere_with_center( pts: &[Point], center: Point, -) -> (Point, Real) { +) -> BoundingSphere { let mut sqradius = 0.0; for pt in pts.iter() { @@ -18,13 +19,11 @@ pub fn point_cloud_bounding_sphere_with_center( sqradius = distance_squared } } - - (center, ComplexField::sqrt(sqradius)) + BoundingSphere::new(center, ComplexField::sqrt(sqradius)) } /// Computes a bounding sphere of the specified set of point. -// TODO: return a bounding sphere? #[inline] -pub fn point_cloud_bounding_sphere(pts: &[Point]) -> (Point, Real) { +pub fn point_cloud_bounding_sphere(pts: &[Point]) -> BoundingSphere { point_cloud_bounding_sphere_with_center(pts, utils::center(pts)) } diff --git a/src/lib.rs b/src/lib.rs index 38351293..d4c3cb6e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -11,7 +11,7 @@ the rust programming language. #![deny(unused_parens)] #![deny(non_upper_case_globals)] #![deny(unused_results)] -#![warn(missing_docs)] // TODO: deny this +#![deny(missing_docs)] #![warn(unused_imports)] #![allow(missing_copy_implementations)] #![allow(clippy::too_many_arguments)] // Maybe revisit this one later. @@ -20,7 +20,7 @@ the rust programming language. #![allow(clippy::type_complexity)] // Complains about closures that are fairly simple. #![doc(html_root_url = "http://docs.rs/parry/0.1.1")] #![cfg_attr(not(feature = "std"), no_std)] -#![cfg_attr(not(feature = "rkyv"), deny(unused_qualifications))] // TODO: deny that everytime +#![deny(unused_qualifications)] // TODO: deny that everytime #[cfg(all( feature = "simd-is-enabled", diff --git a/src/mass_properties/mass_properties_convex_polygon.rs b/src/mass_properties/mass_properties_convex_polygon.rs index 28708caa..379366ef 100644 --- a/src/mass_properties/mass_properties_convex_polygon.rs +++ b/src/mass_properties/mass_properties_convex_polygon.rs @@ -1,5 +1,3 @@ -#![allow(dead_code)] // TODO: remove this - use crate::mass_properties::MassProperties; use crate::math::{Point, Real}; use crate::shape::Triangle; diff --git a/src/query/closest_points/closest_points_line_line.rs b/src/query/closest_points/closest_points_line_line.rs index 6e895752..9b6ef50a 100644 --- a/src/query/closest_points/closest_points_line_line.rs +++ b/src/query/closest_points/closest_points_line_line.rs @@ -71,7 +71,7 @@ pub fn closest_points_line_line_parameters_eps( } } -// TODO: can we re-used this for the segment/segment case? +// TODO: can we re-use this for the segment/segment case? /// Closest points between two segments. #[inline] pub fn closest_points_line_line( diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index 05afdd97..d153e1da 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -375,6 +375,8 @@ impl EPA { let new_face_id = self.faces.len(); let new_face; + // TODO: Thierry: We can probably remove that scope now, but I prefer to discuss it first. + // I assume NLL is for Non Lexical Lifetimes. // TODO: NLL { let face_adj = &mut self.faces[edge.face_id]; diff --git a/src/utils/sdp_matrix.rs b/src/utils/sdp_matrix.rs index 50b4130b..0385a117 100644 --- a/src/utils/sdp_matrix.rs +++ b/src/utils/sdp_matrix.rs @@ -3,7 +3,8 @@ use na::{Matrix2, Matrix3, Matrix3x2, SimdRealField, Vector2, Vector3}; use std::ops::{Add, Mul}; #[cfg(feature = "rkyv")] -use rkyv::{bytecheck, CheckBytes}; +#[cfg(feature = "rkyv")] +use rkyv::{bytecheck, Archive, CheckBytes}; /// A 2x2 symmetric-definite-positive matrix. #[derive(Copy, Clone, Debug, PartialEq)] @@ -11,7 +12,7 @@ use rkyv::{bytecheck, CheckBytes}; #[cfg_attr( feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self", bound(archive = "N: rkyv::Archive")) + archive(as = "Self", bound(archive = "N: Archive")) )] pub struct SdpMatrix2 { /// The component at the first row and first column of this matrix. @@ -122,7 +123,7 @@ impl Mul for SdpMatrix2 { #[cfg_attr( feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), - archive(as = "Self", bound(archive = "N: rkyv::Archive")) + archive(as = "Self", bound(archive = "N: Archive")) )] pub struct SdpMatrix3 { /// The component at the first row and first column of this matrix. From 8d2b591e0e141570da39fab54eb7a3262256d844 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 18 Sep 2024 17:27:37 +0200 Subject: [PATCH 02/15] compute_axes_aligned_clipping_planes now a VoxelSet function --- src/query/ray/ray.rs | 2 +- src/transformation/vhacd/vhacd.rs | 32 +------------------- src/transformation/voxelization/voxel_set.rs | 25 +++++++++++++++ 3 files changed, 27 insertions(+), 32 deletions(-) diff --git a/src/query/ray/ray.rs b/src/query/ray/ray.rs index d20bd478..4c9489c6 100644 --- a/src/query/ray/ray.rs +++ b/src/query/ray/ray.rs @@ -79,7 +79,7 @@ pub struct RayIntersection { /// Otherwise, the normal points outward. /// /// If the `time_of_impact` is exactly zero, the normal might not be reliable. - // TODO: use a Unit instead. + // TODO: use a Unit instead. // TODO: Thierry: should we use Unit for [`Ray::dir`] too ? pub normal: Vector, /// Feature at the intersection point. diff --git a/src/transformation/vhacd/vhacd.rs b/src/transformation/vhacd/vhacd.rs index c7885d3e..79271b82 100644 --- a/src/transformation/vhacd/vhacd.rs +++ b/src/transformation/vhacd/vhacd.rs @@ -164,32 +164,6 @@ impl VHACD { } } - // TODO: this should be a method of VoxelSet. - fn compute_axes_aligned_clipping_planes( - vset: &VoxelSet, - downsampling: u32, - planes: &mut Vec, - ) { - let min_v = vset.min_bb_voxels(); - let max_v = vset.max_bb_voxels(); - - for dim in 0..DIM { - let i0 = min_v[dim]; - let i1 = max_v[dim]; - - for i in (i0..=i1).step_by(downsampling as usize) { - let plane = CutPlane { - abc: Vector::ith(dim, 1.0), - axis: dim as u8, - d: -(vset.origin[dim] + (i as Real + 0.5) * vset.scale), - index: i, - }; - - planes.push(plane); - } - } - } - fn refine_axes_aligned_clipping_planes( vset: &VoxelSet, best_plane: &CutPlane, @@ -328,11 +302,7 @@ impl VHACD { Self::compute_preferred_cutting_direction(&eigenvalues); let mut planes = Vec::new(); - Self::compute_axes_aligned_clipping_planes( - &voxels, - params.plane_downsampling, - &mut planes, - ); + voxels.compute_axes_aligned_clipping_planes(params.plane_downsampling, &mut planes); let (mut best_plane, mut min_concavity) = self.compute_best_clipping_plane( &voxels, diff --git a/src/transformation/voxelization/voxel_set.rs b/src/transformation/voxelization/voxel_set.rs index c1619beb..5b37ff00 100644 --- a/src/transformation/voxelization/voxel_set.rs +++ b/src/transformation/voxelization/voxel_set.rs @@ -617,6 +617,31 @@ impl VoxelSet { cov_mat.symmetric_eigenvalues() } + + pub(crate) fn compute_axes_aligned_clipping_planes( + &self, + downsampling: u32, + planes: &mut Vec, + ) { + let min_v = self.min_bb_voxels(); + let max_v = self.max_bb_voxels(); + + for dim in 0..DIM { + let i0 = min_v[dim]; + let i1 = max_v[dim]; + + for i in (i0..=i1).step_by(downsampling as usize) { + let plane = CutPlane { + abc: Vector::ith(dim, 1.0), + axis: dim as u8, + d: -(self.origin[dim] + (i as Real + 0.5) * self.scale), + index: i, + }; + + planes.push(plane); + } + } + } } #[cfg(feature = "dim2")] From 9a5695ccb029790609950b110374a132b4869bb2 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 26 Sep 2024 12:13:17 +0200 Subject: [PATCH 03/15] remove comment/scope about nll --- src/query/epa/epa3.rs | 23 +++++++++-------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index d153e1da..c8de9b50 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -375,20 +375,15 @@ impl EPA { let new_face_id = self.faces.len(); let new_face; - // TODO: Thierry: We can probably remove that scope now, but I prefer to discuss it first. - // I assume NLL is for Non Lexical Lifetimes. - // TODO: NLL - { - let face_adj = &mut self.faces[edge.face_id]; - let pt_id1 = face_adj.pts[(edge.opp_pt_id + 2) % 3]; - let pt_id2 = face_adj.pts[(edge.opp_pt_id + 1) % 3]; - - let pts = [pt_id1, pt_id2, support_point_id]; - let adj = [edge.face_id, new_face_id + 1, new_face_id - 1]; - new_face = Face::new(&self.vertices, pts, adj); - - face_adj.adj[(edge.opp_pt_id + 1) % 3] = new_face_id; - } + let face_adj = &mut self.faces[edge.face_id]; + let pt_id1 = face_adj.pts[(edge.opp_pt_id + 2) % 3]; + let pt_id2 = face_adj.pts[(edge.opp_pt_id + 1) % 3]; + + let pts = [pt_id1, pt_id2, support_point_id]; + let adj = [edge.face_id, new_face_id + 1, new_face_id - 1]; + new_face = Face::new(&self.vertices, pts, adj); + + face_adj.adj[(edge.opp_pt_id + 1) % 3] = new_face_id; self.faces.push(new_face.0); From 82cd28fb9174e35406ad37bc27f12ca3744bcd29 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 26 Sep 2024 12:13:28 +0200 Subject: [PATCH 04/15] remove IntersectionCompositeShapeShapeBestFirstVisitor --- ...intersection_test_composite_shape_shape.rs | 99 +------------------ src/query/intersection_test/mod.rs | 4 +- 2 files changed, 4 insertions(+), 99 deletions(-) diff --git a/src/query/intersection_test/intersection_test_composite_shape_shape.rs b/src/query/intersection_test/intersection_test_composite_shape_shape.rs index 3bf4df59..c3a61375 100644 --- a/src/query/intersection_test/intersection_test_composite_shape_shape.rs +++ b/src/query/intersection_test/intersection_test_composite_shape_shape.rs @@ -1,14 +1,10 @@ -#![allow(deprecated)] // Silence warning until we actually remove IntersectionCompositeShapeShapeBestFirstVisitor - use crate::bounding_volume::SimdAabb; -use crate::math::{Isometry, Real, SimdReal, Vector, SIMD_WIDTH}; -use crate::partitioning::{ - SimdBestFirstVisitStatus, SimdBestFirstVisitor, SimdVisitStatus, SimdVisitor, -}; +use crate::math::{Isometry, Real, SIMD_WIDTH}; +use crate::partitioning::{SimdVisitStatus, SimdVisitor}; use crate::query::QueryDispatcher; use crate::shape::{Shape, TypedSimdCompositeShape}; use crate::utils::IsometryOpt; -use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; +use simba::simd::SimdBool as _; /// Intersection test between a composite shape (`Mesh`, `Compound`) and any other shape. pub fn intersection_test_composite_shape_shape( @@ -118,92 +114,3 @@ where SimdVisitStatus::MaybeContinue(mask) } } - -/// A visitor for checking if a composite-shape and a shape intersect. -#[deprecated(note = "Use IntersectionCompositeShapeShapeVisitor instead.")] -pub struct IntersectionCompositeShapeShapeBestFirstVisitor<'a, D: ?Sized, G1: ?Sized + 'a> { - msum_shift: Vector, - msum_margin: Vector, - - dispatcher: &'a D, - pos12: &'a Isometry, - g1: &'a G1, - g2: &'a dyn Shape, -} - -impl<'a, D, G1> IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> -where - D: ?Sized + QueryDispatcher, - G1: ?Sized + TypedSimdCompositeShape, -{ - /// Initialize a visitor for checking if a composite-shape and a shape intersect. - pub fn new( - dispatcher: &'a D, - pos12: &'a Isometry, - g1: &'a G1, - g2: &'a dyn Shape, - ) -> IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> { - let ls_aabb2 = g2.compute_aabb(pos12); - - IntersectionCompositeShapeShapeBestFirstVisitor { - dispatcher, - msum_shift: Vector::splat(-ls_aabb2.center().coords), - msum_margin: Vector::splat(ls_aabb2.half_extents()), - pos12, - g1, - g2, - } - } -} - -impl<'a, D, G1> SimdBestFirstVisitor - for IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> -where - D: ?Sized + QueryDispatcher, - G1: ?Sized + TypedSimdCompositeShape, -{ - type Result = (G1::PartId, bool); - - fn visit( - &mut self, - best: Real, - bv: &SimdAabb, - data: Option<[Option<&G1::PartId>; SIMD_WIDTH]>, - ) -> SimdBestFirstVisitStatus { - // Compute the minkowski sum of the two Aabbs. - let msum = SimdAabb { - mins: bv.mins + self.msum_shift + (-self.msum_margin), - maxs: bv.maxs + self.msum_shift + self.msum_margin, - }; - let dist = msum.distance_to_origin(); - let mask = dist.simd_lt(SimdReal::splat(best)); - - if let Some(data) = data { - let bitmask = mask.bitmask(); - let mut found_intersection = false; - - for (ii, data) in data.into_iter().enumerate() { - if (bitmask & (1 << ii)) != 0 && data.is_some() { - let part_id = *data.unwrap(); - self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| { - found_intersection = self.dispatcher.intersection_test( - &part_pos1.inv_mul(self.pos12), - g1, - self.g2, - ) == Ok(true); - }); - - if found_intersection { - return SimdBestFirstVisitStatus::ExitEarly(Some((part_id, true))); - } - } - } - } - - SimdBestFirstVisitStatus::MaybeContinue { - weights: dist, - mask, - results: [None; SIMD_WIDTH], - } - } -} diff --git a/src/query/intersection_test/mod.rs b/src/query/intersection_test/mod.rs index fceb5383..a3efc8de 100644 --- a/src/query/intersection_test/mod.rs +++ b/src/query/intersection_test/mod.rs @@ -6,11 +6,9 @@ pub use self::intersection_test_ball_point_query::{ intersection_test_ball_point_query, intersection_test_point_query_ball, }; #[cfg(feature = "std")] -// TODO: remove this once we get rid of IntersectionCompositeShapeShapeBestFirstVisitor -#[allow(deprecated)] pub use self::intersection_test_composite_shape_shape::{ intersection_test_composite_shape_shape, intersection_test_shape_composite_shape, - IntersectionCompositeShapeShapeBestFirstVisitor, IntersectionCompositeShapeShapeVisitor, + IntersectionCompositeShapeShapeVisitor, }; pub use self::intersection_test_cuboid_cuboid::intersection_test_cuboid_cuboid; pub use self::intersection_test_cuboid_segment::{ From a641209b69298720464ac40138207e0a7ce43d40 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 26 Sep 2024 16:45:11 +0200 Subject: [PATCH 05/15] =?UTF-8?q?epa:=20Adds=20an=20enum=20to=20detect=20w?= =?UTF-8?q?hen=20a=20normal=20couldn=C2=B4t=20be=20generated=20because=20o?= =?UTF-8?q?f=20a=20degenerate=20face.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../contact_support_map_support_map.rs | 7 +- .../contact_manifolds_pfm_pfm.rs | 2 +- src/query/epa/epa2.rs | 68 ++++++++++++----- src/query/epa/epa3.rs | 74 +++++++++++++------ src/query/epa/mod.rs | 4 +- src/query/gjk/gjk.rs | 15 ++-- 6 files changed, 119 insertions(+), 51 deletions(-) diff --git a/src/query/contact/contact_support_map_support_map.rs b/src/query/contact/contact_support_map_support_map.rs index 831ba246..240aa421 100644 --- a/src/query/contact/contact_support_map_support_map.rs +++ b/src/query/contact/contact_support_map_support_map.rs @@ -19,12 +19,16 @@ where { let simplex = &mut VoronoiSimplex::new(); match contact_support_map_support_map_with_params(pos12, g1, g2, prediction, simplex, None) { - GJKResult::ClosestPoints(point1, point2_1, normal1) => { + GJKResult::ClosestPoints(point1, point2_1, Ok(normal1)) => { let dist = (point2_1 - point1).dot(&normal1); let point2 = pos12.inverse_transform_point(&point2_1); let normal2 = pos12.inverse_transform_unit_vector(&-normal1); Some(Contact::new(point1, point2, normal1, normal2, dist)) } + GJKResult::ClosestPoints(_, _, Err(_)) => { + // TODO: propagate the error. + None + } GJKResult::NoIntersection(_) => None, GJKResult::Intersection => unreachable!(), GJKResult::Proximity(_) => unreachable!(), @@ -68,6 +72,7 @@ where // The point is inside of the CSO: use the fallback algorithm let mut epa = EPA::new(); + if let Some((p1, p2, n)) = epa.closest_points(pos12, g1, g2, simplex) { return GJKResult::ClosestPoints(p1, p2, n); } diff --git a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs index bb13a838..20058300 100644 --- a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs +++ b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs @@ -79,7 +79,7 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( manifold.clear(); match contact { - GJKResult::ClosestPoints(p1, p2_1, dir) => { + GJKResult::ClosestPoints(p1, p2_1, Ok(dir)) => { let mut local_n1 = dir; let mut local_n2 = pos12.inverse_transform_unit_vector(&-dir); let dist = (p2_1 - p1).dot(&local_n1); diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index 02357f08..a55e3b4f 100644 --- a/src/query/epa/epa2.rs +++ b/src/query/epa/epa2.rs @@ -49,10 +49,14 @@ impl Ord for FaceId { } } +/// Represents a degenerate [`Face`] normal. +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct Degenerate; + #[derive(Clone, Debug)] struct Face { pts: [usize; 2], - normal: Unit>, + normal: Result>, Degenerate>, proj: Point, bcoords: [Real; 2], deleted: bool, @@ -83,10 +87,10 @@ impl Face { if let Some(n) = utils::ccw_face_normal([&vertices[pts[0]].point, &vertices[pts[1]].point]) { - normal = n; + normal = Ok(n); deleted = false; } else { - normal = Unit::new_unchecked(na::zero()); + normal = Err(Degenerate); deleted = true; } @@ -158,7 +162,11 @@ impl EPA { g1: &G1, g2: &G2, simplex: &VoronoiSimplex, - ) -> Option<(Point, Point, Unit>)> + ) -> Option<( + Point, + Point, + Result>, Degenerate>, + )> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -213,7 +221,7 @@ impl EPA { } } - return Some((Point::origin(), Point::origin(), n)); + return Some((Point::origin(), Point::origin(), Ok(n))); } else if simplex.dimension() == 2 { let dp1 = self.vertices[1] - self.vertices[0]; let dp2 = self.vertices[2] - self.vertices[0]; @@ -235,18 +243,24 @@ impl EPA { self.faces.push(face3); if proj_is_inside1 { - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - self.heap.push(FaceId::new(0, -dist1)?); + if let Ok(normal) = self.faces[0].normal { + let dist1 = normal.dot(&self.vertices[0].point.coords); + self.heap.push(FaceId::new(0, -dist1)?); + } } if proj_is_inside2 { - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); - self.heap.push(FaceId::new(1, -dist2)?); + if let Ok(normal) = self.faces[1].normal { + let dist2 = normal.dot(&self.vertices[1].point.coords); + self.heap.push(FaceId::new(1, -dist2)?); + } } if proj_is_inside3 { - let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); - self.heap.push(FaceId::new(2, -dist3)?); + if let Ok(normal) = self.faces[2].normal { + let dist3 = normal.dot(&self.vertices[2].point.coords); + self.heap.push(FaceId::new(2, -dist3)?); + } } } else { let pts1 = [0, 1]; @@ -265,8 +279,16 @@ impl EPA { pts2, )); - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); + let dist1 = self.faces[0] + .normal + .as_ref() + .unwrap_or(&Unit::new_unchecked(Vector::zeros())) + .dot(&self.vertices[0].point.coords); + let dist2 = self.faces[1] + .normal + .as_ref() + .unwrap_or(&Unit::new_unchecked(Vector::zeros())) + .dot(&self.vertices[1].point.coords); self.heap.push(FaceId::new(0, dist1)?); self.heap.push(FaceId::new(1, dist2)?); @@ -287,12 +309,15 @@ impl EPA { if face.deleted { continue; } + let Ok(face_normal) = face.normal else { + continue; + }; - let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal); + let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face_normal); let support_point_id = self.vertices.len(); self.vertices.push(cso_point); - let candidate_max_dist = cso_point.point.coords.dot(&face.normal); + let candidate_max_dist = cso_point.point.coords.dot(&face_normal); if candidate_max_dist < max_dist { best_face_id = face_id; @@ -308,7 +333,7 @@ impl EPA { { let best_face = &self.faces[best_face_id.id]; let cpts = best_face.closest_points(&self.vertices); - return Some((cpts.0, cpts.1, best_face.normal)); + return Some((cpts.0, cpts.1, best_face.normal.clone())); } old_dist = curr_dist; @@ -323,12 +348,17 @@ impl EPA { for f in new_faces.iter() { if f.1 { - let dist = f.0.normal.dot(&f.0.proj.coords); + let new_face_normal = + f.0.normal + .clone() + .unwrap_or(Unit::new_unchecked(Vector::zeros())); + + let dist = new_face_normal.dot(&f.0.proj.coords); if dist < curr_dist { // TODO: if we reach this point, there were issues due to // numerical errors. let cpts = f.0.closest_points(&self.vertices); - return Some((cpts.0, cpts.1, f.0.normal)); + return Some((cpts.0, cpts.1, Ok(new_face_normal))); } if !f.0.deleted { @@ -349,7 +379,7 @@ impl EPA { let best_face = &self.faces[best_face_id.id]; let cpts = best_face.closest_points(&self.vertices); - Some((cpts.0, cpts.1, best_face.normal)) + Some((cpts.0, cpts.1, best_face.normal.clone())) } } diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index c8de9b50..e2568c61 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -48,11 +48,15 @@ impl Ord for FaceId { } } +/// Represents a degenerate [`Face`] normal. +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct Degenerate; + #[derive(Clone, Debug)] struct Face { pts: [usize; 3], adj: [usize; 3], - normal: Unit>, + normal: Result>, Degenerate>, bcoords: [Real; 3], deleted: bool, } @@ -71,13 +75,13 @@ impl Face { &vertices[pts[1]].point, &vertices[pts[2]].point, ]) { - normal = n; + normal = Ok(n); } else { // This is a bit of a hack for degenerate faces. // TODO: It will work OK with our current code, though // we should do this in another way to avoid any risk // of misusing the face normal in the future. - normal = Unit::new_unchecked(na::zero()); + normal = Err(Degenerate); } Face { @@ -149,8 +153,13 @@ impl Face { // have a zero normal, causing the dot product to be zero. // So return true for these case will let us skip the triangle // during silhouette computation. - (*pt - *p0).dot(&self.normal) >= -gjk::eps_tol() - || Triangle::new(*p1, *p2, *pt).is_affinely_dependent() + match &self.normal { + Ok(normal) => { + (*pt - *p0).dot(normal) >= -gjk::eps_tol() + || Triangle::new(*p1, *p2, *pt).is_affinely_dependent() + } + Err(_) => true, + } } } @@ -215,7 +224,11 @@ impl EPA { g1: &G1, g2: &G2, simplex: &VoronoiSimplex, - ) -> Option<(Point, Point, Unit>)> + ) -> Option<( + Point, + Point, + Result>, Degenerate>, + )> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -235,7 +248,7 @@ impl EPA { if simplex.dimension() == 0 { let mut n: Vector = na::zero(); n[1] = 1.0; - return Some((Point::origin(), Point::origin(), Unit::new_unchecked(n))); + return Some((Point::origin(), Point::origin(), Ok(Unit::new_unchecked(n)))); } else if simplex.dimension() == 3 { let dp1 = self.vertices[1] - self.vertices[0]; let dp2 = self.vertices[2] - self.vertices[0]; @@ -266,23 +279,31 @@ impl EPA { self.faces.push(face4); if proj_inside1 { - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - self.heap.push(FaceId::new(0, -dist1)?); + if let Ok(normal) = self.faces[0].normal { + let dist1 = normal.dot(&self.vertices[0].point.coords); + self.heap.push(FaceId::new(0, -dist1)?); + } } if proj_inside2 { - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); - self.heap.push(FaceId::new(1, -dist2)?); + if let Ok(normal) = self.faces[1].normal { + let dist2 = normal.dot(&self.vertices[1].point.coords); + self.heap.push(FaceId::new(1, -dist2)?); + } } if proj_inside3 { - let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); - self.heap.push(FaceId::new(2, -dist3)?); + if let Ok(normal) = self.faces[2].normal { + let dist3 = normal.dot(&self.vertices[2].point.coords); + self.heap.push(FaceId::new(2, -dist3)?); + } } if proj_inside4 { - let dist4 = self.faces[3].normal.dot(&self.vertices[3].point.coords); - self.heap.push(FaceId::new(3, -dist4)?); + if let Ok(normal) = self.faces[3].normal { + let dist4 = normal.dot(&self.vertices[3].point.coords); + self.heap.push(FaceId::new(3, -dist4)?); + } } } else { if simplex.dimension() == 1 { @@ -327,13 +348,15 @@ impl EPA { if face.deleted { continue; } - - let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal); + let face_normal = &face + .normal + .clone() + .unwrap_or(Unit::new_unchecked(Vector::zeros())); + let cso_point = CSOPoint::from_shapes(pos12, g1, g2, face_normal); + let candidate_max_dist = cso_point.point.coords.dot(face_normal); let support_point_id = self.vertices.len(); self.vertices.push(cso_point); - let candidate_max_dist = cso_point.point.coords.dot(&face.normal); - if candidate_max_dist < max_dist { best_face_id = face_id; max_dist = candidate_max_dist; @@ -347,8 +370,9 @@ impl EPA { ((curr_dist - old_dist).abs() < _eps && candidate_max_dist < max_dist) { let best_face = &self.faces[best_face_id.id]; + let best_face_normal = face.normal.unwrap_or(Unit::new_unchecked(Vector::zeros())); let points = best_face.closest_points(&self.vertices); - return Some((points.0, points.1, best_face.normal)); + return Some((points.0, points.1, Ok(best_face_normal))); } old_dist = curr_dist; @@ -389,12 +413,16 @@ impl EPA { if new_face.1 { let pt = self.vertices[self.faces[new_face_id].pts[0]].point.coords; - let dist = self.faces[new_face_id].normal.dot(&pt); + let new_face_normal = self.faces[new_face_id] + .normal + .clone() + .unwrap_or(Unit::new_unchecked(Vector::zeros())); + let dist = new_face_normal.dot(&pt); if dist < curr_dist { // TODO: if we reach this point, there were issues due to // numerical errors. let points = face.closest_points(&self.vertices); - return Some((points.0, points.1, face.normal)); + return Some((points.0, points.1, Ok(new_face_normal))); } self.heap.push(FaceId::new(new_face_id, -dist)?); @@ -424,7 +452,7 @@ impl EPA { let best_face = &self.faces[best_face_id.id]; let points = best_face.closest_points(&self.vertices); - Some((points.0, points.1, best_face.normal)) + Some((points.0, points.1, best_face.normal.clone())) } fn compute_silhouette(&mut self, point: usize, id: usize, opp_pt_id: usize) { diff --git a/src/query/epa/mod.rs b/src/query/epa/mod.rs index b61f3dc4..d8c3989c 100644 --- a/src/query/epa/mod.rs +++ b/src/query/epa/mod.rs @@ -1,9 +1,9 @@ //! The EPA algorithm for penetration depth computation. //! #[cfg(feature = "dim2")] -pub use self::epa2::EPA; +pub use self::epa2::{Degenerate, EPA}; #[cfg(feature = "dim3")] -pub use self::epa3::EPA; +pub use self::epa3::{Degenerate, EPA}; #[cfg(feature = "dim2")] pub mod epa2; diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index ebee30ae..3c4f73aa 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -2,6 +2,7 @@ use na::{self, ComplexField, Unit}; +use crate::query::epa::Degenerate; use crate::query::gjk::{CSOPoint, ConstantOrigin, VoronoiSimplex}; use crate::shape::SupportMap; // use query::Proximity; @@ -19,7 +20,11 @@ pub enum GJKResult { /// /// Both points and vector are expressed in the local-space of the first geometry involved /// in the GJK execution. - ClosestPoints(Point, Point, Unit>), + ClosestPoints( + Point, + Point, + Result>, Degenerate>, + ), /// Result of the GJK algorithm when the origin is too close to the polytope but not inside of it. /// /// The returned vector is expressed in the local-space of the first geometry involved in the @@ -125,7 +130,7 @@ where if max_bound >= old_max_bound { if exact_dist { let (p1, p2) = result(simplex, true); - return GJKResult::ClosestPoints(p1, p2, old_dir); // upper bounds inconsistencies + return GJKResult::ClosestPoints(p1, p2, Ok(old_dir)); // upper bounds inconsistencies } else { return GJKResult::Proximity(old_dir); } @@ -143,7 +148,7 @@ where } else if max_bound - min_bound <= _eps_rel * max_bound { if exact_dist { let (p1, p2) = result(simplex, false); - return GJKResult::ClosestPoints(p1, p2, dir); // the distance found has a good enough precision + return GJKResult::ClosestPoints(p1, p2, Ok(dir)); // the distance found has a good enough precision } else { return GJKResult::Proximity(dir); } @@ -152,7 +157,7 @@ where if !simplex.add_point(cso_point) { if exact_dist { let (p1, p2) = result(simplex, false); - return GJKResult::ClosestPoints(p1, p2, dir); + return GJKResult::ClosestPoints(p1, p2, Ok(dir)); } else { return GJKResult::Proximity(dir); } @@ -165,7 +170,7 @@ where if min_bound >= _eps_tol { if exact_dist { let (p1, p2) = result(simplex, true); - return GJKResult::ClosestPoints(p1, p2, old_dir); + return GJKResult::ClosestPoints(p1, p2, Ok(old_dir)); } else { // NOTE: previous implementation used old_proj here. return GJKResult::Proximity(old_dir); From 34bb714720a7804a166cb3cd665836d858829fbb Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 26 Sep 2024 16:49:53 +0200 Subject: [PATCH 06/15] remove the initial todo which prompted those changes --- src/query/epa/epa3.rs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index e2568c61..bc1b0b5b 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -77,10 +77,6 @@ impl Face { ]) { normal = Ok(n); } else { - // This is a bit of a hack for degenerate faces. - // TODO: It will work OK with our current code, though - // we should do this in another way to avoid any risk - // of misusing the face normal in the future. normal = Err(Degenerate); } From de82965ddc8620419acaec9895d11803cccb7ec8 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 26 Sep 2024 17:38:54 +0200 Subject: [PATCH 07/15] fix clippy --- src/query/epa/epa3.rs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index c8de9b50..9f8bdc90 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -373,7 +373,6 @@ impl EPA { for edge in &self.silhouette { if !self.faces[edge.face_id].deleted { let new_face_id = self.faces.len(); - let new_face; let face_adj = &mut self.faces[edge.face_id]; let pt_id1 = face_adj.pts[(edge.opp_pt_id + 2) % 3]; @@ -381,7 +380,7 @@ impl EPA { let pts = [pt_id1, pt_id2, support_point_id]; let adj = [edge.face_id, new_face_id + 1, new_face_id - 1]; - new_face = Face::new(&self.vertices, pts, adj); + let new_face = Face::new(&self.vertices, pts, adj); face_adj.adj[(edge.opp_pt_id + 1) % 3] = new_face_id; From e0535b19897b7568bf96840d784aa816733fa114 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 6 Nov 2024 16:45:03 +0100 Subject: [PATCH 08/15] pr feedbacks --- src/query/epa/epa2.rs | 9 +++++---- src/query/epa/epa3.rs | 8 +++----- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index a55e3b4f..b5d5045e 100644 --- a/src/query/epa/epa2.rs +++ b/src/query/epa/epa2.rs @@ -282,13 +282,14 @@ impl EPA { let dist1 = self.faces[0] .normal .as_ref() - .unwrap_or(&Unit::new_unchecked(Vector::zeros())) - .dot(&self.vertices[0].point.coords); + .map(|normal| normal.dot(&self.vertices[0].point.coords)) + .unwrap_or(0.0); + let dist2 = self.faces[1] .normal .as_ref() - .unwrap_or(&Unit::new_unchecked(Vector::zeros())) - .dot(&self.vertices[1].point.coords); + .map(|normal| normal.dot(&self.vertices[1].point.coords)) + .unwrap_or(0.0); self.heap.push(FaceId::new(0, dist1)?); self.heap.push(FaceId::new(1, dist2)?); diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index 24889ca7..b6266894 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -340,14 +340,12 @@ impl EPA { while let Some(face_id) = self.heap.pop() { // Create new faces. let face = self.faces[face_id.id].clone(); - if face.deleted { continue; } - let face_normal = &face - .normal - .clone() - .unwrap_or(Unit::new_unchecked(Vector::zeros())); + let Ok(face_normal) = &face.normal else { + continue; + }; let cso_point = CSOPoint::from_shapes(pos12, g1, g2, face_normal); let candidate_max_dist = cso_point.point.coords.dot(face_normal); let support_point_id = self.vertices.len(); From 38b4a6a6429112c9c60293a5ec967412199726bf Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 6 Nov 2024 17:09:07 +0100 Subject: [PATCH 09/15] face degenerate in upper module --- src/query/epa/epa3.rs | 12 ++++-------- src/query/epa/mod.rs | 4 ++-- src/query/gjk/gjk.rs | 4 ++-- src/query/mod.rs | 4 ++++ 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index b6266894..c195a1c2 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -2,7 +2,7 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; -use crate::query::PointQueryWithLocation; +use crate::query::{FaceDegenerate, PointQueryWithLocation}; use crate::shape::{SupportMap, Triangle, TrianglePointLocation}; use crate::utils; use na::{self, Unit}; @@ -48,15 +48,11 @@ impl Ord for FaceId { } } -/// Represents a degenerate [`Face`] normal. -#[derive(Clone, Debug, PartialEq, Eq)] -pub struct Degenerate; - #[derive(Clone, Debug)] struct Face { pts: [usize; 3], adj: [usize; 3], - normal: Result>, Degenerate>, + normal: Result>, FaceDegenerate>, bcoords: [Real; 3], deleted: bool, } @@ -77,7 +73,7 @@ impl Face { ]) { normal = Ok(n); } else { - normal = Err(Degenerate); + normal = Err(FaceDegenerate); } Face { @@ -223,7 +219,7 @@ impl EPA { ) -> Option<( Point, Point, - Result>, Degenerate>, + Result>, FaceDegenerate>, )> where G1: ?Sized + SupportMap, diff --git a/src/query/epa/mod.rs b/src/query/epa/mod.rs index d8c3989c..b61f3dc4 100644 --- a/src/query/epa/mod.rs +++ b/src/query/epa/mod.rs @@ -1,9 +1,9 @@ //! The EPA algorithm for penetration depth computation. //! #[cfg(feature = "dim2")] -pub use self::epa2::{Degenerate, EPA}; +pub use self::epa2::EPA; #[cfg(feature = "dim3")] -pub use self::epa3::{Degenerate, EPA}; +pub use self::epa3::EPA; #[cfg(feature = "dim2")] pub mod epa2; diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index 3c4f73aa..88d4d89a 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -2,8 +2,8 @@ use na::{self, ComplexField, Unit}; -use crate::query::epa::Degenerate; use crate::query::gjk::{CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::FaceDegenerate; use crate::shape::SupportMap; // use query::Proximity; use crate::math::{Isometry, Point, Real, Vector, DIM}; @@ -23,7 +23,7 @@ pub enum GJKResult { ClosestPoints( Point, Point, - Result>, Degenerate>, + Result>, FaceDegenerate>, ), /// Result of the GJK algorithm when the origin is too close to the polytope but not inside of it. /// diff --git a/src/query/mod.rs b/src/query/mod.rs index ed6e1537..ac3daf13 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -80,3 +80,7 @@ pub mod details { pub use super::ray::*; pub use super::shape_cast::*; } + +/// Represents a degenerate [`Face`] normal. +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct FaceDegenerate; From f7acdf89a54932d011782ed9aa0738b40b7ad3fb Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 6 Nov 2024 17:10:35 +0100 Subject: [PATCH 10/15] remove unneeded comment --- src/query/ray/ray.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/query/ray/ray.rs b/src/query/ray/ray.rs index 4c9489c6..d20bd478 100644 --- a/src/query/ray/ray.rs +++ b/src/query/ray/ray.rs @@ -79,7 +79,7 @@ pub struct RayIntersection { /// Otherwise, the normal points outward. /// /// If the `time_of_impact` is exactly zero, the normal might not be reliable. - // TODO: use a Unit instead. // TODO: Thierry: should we use Unit for [`Ray::dir`] too ? + // TODO: use a Unit instead. pub normal: Vector, /// Feature at the intersection point. From 6a984320c55d0660aa65a6d659b484542bf2fe06 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 7 Nov 2024 15:04:41 +0100 Subject: [PATCH 11/15] fix facedegenerate --- src/query/epa/epa2.rs | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index b5d5045e..2ceb89c3 100644 --- a/src/query/epa/epa2.rs +++ b/src/query/epa/epa2.rs @@ -8,6 +8,7 @@ use num::Bounded; use crate::math::{Isometry, Point, Real, Vector}; use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::FaceDegenerate; use crate::shape::SupportMap; use crate::utils; @@ -49,14 +50,10 @@ impl Ord for FaceId { } } -/// Represents a degenerate [`Face`] normal. -#[derive(Clone, Debug, PartialEq, Eq)] -pub struct Degenerate; - #[derive(Clone, Debug)] struct Face { pts: [usize; 2], - normal: Result>, Degenerate>, + normal: Result>, FaceDegenerate>, proj: Point, bcoords: [Real; 2], deleted: bool, @@ -90,7 +87,7 @@ impl Face { normal = Ok(n); deleted = false; } else { - normal = Err(Degenerate); + normal = Err(FaceDegenerate); deleted = true; } @@ -165,7 +162,7 @@ impl EPA { ) -> Option<( Point, Point, - Result>, Degenerate>, + Result>, FaceDegenerate>, )> where G1: ?Sized + SupportMap, From 253bf30052f2f973c362e7c19ac16da5fa639dcd Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Fri, 8 Nov 2024 17:29:26 +0100 Subject: [PATCH 12/15] fix doc --- src/query/mod.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/query/mod.rs b/src/query/mod.rs index ac3daf13..aa6cfaa4 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -81,6 +81,6 @@ pub mod details { pub use super::shape_cast::*; } -/// Represents a degenerate [`Face`] normal. +/// Represents a degenerate Face normal. #[derive(Clone, Debug, PartialEq, Eq)] pub struct FaceDegenerate; From 473185293af30c97ee5803cea3e5f5119605b258 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 12 Nov 2024 09:18:07 +0100 Subject: [PATCH 13/15] remove todo --- src/query/contact/contact_support_map_support_map.rs | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/query/contact/contact_support_map_support_map.rs b/src/query/contact/contact_support_map_support_map.rs index 240aa421..15a43424 100644 --- a/src/query/contact/contact_support_map_support_map.rs +++ b/src/query/contact/contact_support_map_support_map.rs @@ -25,10 +25,7 @@ where let normal2 = pos12.inverse_transform_unit_vector(&-normal1); Some(Contact::new(point1, point2, normal1, normal2, dist)) } - GJKResult::ClosestPoints(_, _, Err(_)) => { - // TODO: propagate the error. - None - } + GJKResult::ClosestPoints(_, _, Err(_)) => None, GJKResult::NoIntersection(_) => None, GJKResult::Intersection => unreachable!(), GJKResult::Proximity(_) => unreachable!(), From a7f8dce7c7b2f04093d6c694cb2e2894607b74cf Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Fri, 15 Nov 2024 17:30:50 +0100 Subject: [PATCH 14/15] add warn! call --- src/query/contact/contact_support_map_support_map.rs | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/query/contact/contact_support_map_support_map.rs b/src/query/contact/contact_support_map_support_map.rs index 15a43424..aae2ace5 100644 --- a/src/query/contact/contact_support_map_support_map.rs +++ b/src/query/contact/contact_support_map_support_map.rs @@ -4,6 +4,7 @@ use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex}; use crate::query::Contact; use crate::shape::SupportMap; +use log::warn; use na::Unit; /// Contact between support-mapped shapes (`Cuboid`, `ConvexHull`, etc.) @@ -25,7 +26,10 @@ where let normal2 = pos12.inverse_transform_unit_vector(&-normal1); Some(Contact::new(point1, point2, normal1, normal2, dist)) } - GJKResult::ClosestPoints(_, _, Err(_)) => None, + GJKResult::ClosestPoints(_, _, Err(_)) => { + warn!("`contact_support_map_support_map` found the closest points on a degenerate face: verify your shapes' correctness."); + None + } GJKResult::NoIntersection(_) => None, GJKResult::Intersection => unreachable!(), GJKResult::Proximity(_) => unreachable!(), From 92cd2971e1f9ac68ea53cfd36dd03a384f853fd2 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 19 Nov 2024 09:59:55 +0100 Subject: [PATCH 15/15] avoid a panic in --- src/shape/convex_polyhedron.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/shape/convex_polyhedron.rs b/src/shape/convex_polyhedron.rs index da03d7ec..a357ca8f 100644 --- a/src/shape/convex_polyhedron.rs +++ b/src/shape/convex_polyhedron.rs @@ -117,7 +117,7 @@ impl ConvexPolyhedron { /// This explicitly computes the convex hull of the given set of points. Use /// Returns `None` if the convex hull computation failed. pub fn from_convex_hull(points: &[Point]) -> Option { - let (vertices, indices) = crate::transformation::convex_hull(points); + let (vertices, indices) = crate::transformation::try_convex_hull(points).ok()?; Self::from_convex_mesh(vertices, &indices) }