From 0dcc00df8bc55f1415831e9c91294a3acf7972bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 3 Sep 2026 22:56:10 +0200 Subject: [PATCH 1/2] feat!: have most shape queries return the SubShapeId to identify the parts of a composite shape involved --- .gitignore | 3 +- CHANGELOG.md | 14 + crates/parry2d/examples/distance_query2d.rs | 9 +- crates/parry2d/examples/proximity_query2d.rs | 12 +- .../issue_431_cuboid_distance_asymmetry.rs | 8 +- .../query/closest_points_cuboid_cuboid.rs | 12 +- .../tests/query/point_composite_shape.rs | 12 +- crates/parry2d/tests/sub_shape_id.rs | 160 ++++++++++++ crates/parry3d/examples/distance_query3d.rs | 9 +- crates/parry3d/examples/proximity_query3d.rs | 8 +- .../tests/issue_157_frustum_contact.rs | 2 +- ...96_cylinder_intersection_false_negative.rs | 20 +- .../issue_431_cuboid_distance_asymmetry.rs | 8 +- ...issue_70_capsule_cuboid_false_negatives.rs | 20 +- .../query/closest_points_cuboid_cuboid.rs | 12 +- crates/parry3d/tests/sub_shape_id.rs | 239 ++++++++++++++++++ .../closest_points_composite_shape_shape.rs | 4 +- src/query/contact/contact.rs | 22 ++ .../contact/contact_ball_convex_polyhedron.rs | 2 +- .../contact/contact_composite_shape_shape.rs | 18 +- src/query/default_query_dispatcher.rs | 64 ++--- src/query/distance/distance.rs | 61 ++++- .../distance_composite_shape_shape.rs | 59 +++-- src/query/distance/mod.rs | 2 +- .../intersection_test/intersection_test.rs | 53 +++- .../intersection_test_ball_point_query.rs | 13 +- ...intersection_test_composite_shape_shape.rs | 46 ++-- .../intersection_test_voxels_shape.rs | 27 +- src/query/intersection_test/mod.rs | 2 +- src/query/mod.rs | 4 +- ...linear_shape_cast_composite_shape_shape.rs | 144 ++++++----- ...near_shape_cast_support_map_support_map.rs | 4 + src/query/point/point_composite_shape.rs | 106 ++++---- src/query/point/point_query.rs | 23 +- src/query/point/point_voxels.rs | 6 +- src/query/query_dispatcher.rs | 30 ++- src/query/ray/ray.rs | 20 +- src/query/ray/ray_composite_shape.rs | 43 ++-- src/query/ray/ray_heightfield.rs | 40 ++- src/query/ray/ray_trimesh.rs | 34 +-- src/query/ray/ray_voxels.rs | 9 +- src/query/shape_cast/shape_cast.rs | 14 +- src/query/shape_cast/shape_cast_ball_ball.rs | 2 + .../shape_cast_composite_shape_shape.rs | 85 ++++--- .../shape_cast_halfspace_support_map.rs | 2 + .../shape_cast_support_map_support_map.rs | 4 + src/shape/feature_id.rs | 12 + src/shape/heightfield3.rs | 48 +++- src/shape/mod.rs | 2 +- src/shape/polyline.rs | 28 +- src/shape/shape.rs | 65 ++++- src/shape/trimesh.rs | 22 +- 52 files changed, 1205 insertions(+), 463 deletions(-) create mode 100644 crates/parry2d/tests/sub_shape_id.rs create mode 100644 crates/parry3d/tests/sub_shape_id.rs diff --git a/.gitignore b/.gitignore index 25ca1173..65495b56 100644 --- a/.gitignore +++ b/.gitignore @@ -9,4 +9,5 @@ Cargo.lock Makefile .vscode .idea -.DS_store \ No newline at end of file +.DS_store +.claude diff --git a/CHANGELOG.md b/CHANGELOG.md index 507f40ec..bad433f0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,19 @@ ## Unreleased +### Modified + +- Query results now identify the sub-shape they came from, through the new `SubShapeId` type alias: + `RayIntersection` and `PointProjection` gained a `subshape` field, and `Contact` and `ShapeCastHit` + gained `subshape1`/`subshape2`. A shape with no sub-shapes reports `0`. +- `QueryDispatcher::distance` and `QueryDispatcher::intersection_test`, and the `distance` and + `intersection_test` free functions, now return `ShapeDistance` and `ShapeIntersection` instead + of a bare `Real` and `bool`, so they can report the sub-shapes too. Read `.distance` or + `.intersecting` to recover the previous value. +- Query results now report the feature of the sub-shape they hit rather than one encoding that + sub-shape's index: a `TriMesh` ray-cast reports the triangle's own face, and the triangle itself + is the result's `subshape`. `TriMesh::triangle_normal` and `HeightField::convert_triangle_feature_id` + take that sub-shape index directly, and `TriMesh::is_backface` tests the triangle's own face. + ### Added - `CompoundFlags::FIX_INTERNAL_EDGES` makes a `Compound` treat the edges (2D) or faces (3D) its parts share as diff --git a/crates/parry2d/examples/distance_query2d.rs b/crates/parry2d/examples/distance_query2d.rs index 65c1105e..1560ce12 100644 --- a/crates/parry2d/examples/distance_query2d.rs +++ b/crates/parry2d/examples/distance_query2d.rs @@ -13,9 +13,12 @@ fn main() { let ball_pos_intersecting = Pose::translation(0.0, 1.0); let ball_pos_disjoint = Pose::translation(0.0, 3.0); - let dist_intersecting = - query::distance(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid).unwrap(); - let dist_disjoint = query::distance(&ball_pos_disjoint, &ball, &cuboid_pos, &cuboid).unwrap(); + let dist_intersecting = query::distance(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid) + .unwrap() + .distance; + let dist_disjoint = query::distance(&ball_pos_disjoint, &ball, &cuboid_pos, &cuboid) + .unwrap() + .distance; assert_eq!(dist_intersecting, 0.0); assert!(relative_eq!(dist_disjoint, 1.0, epsilon = 1.0e-7)); diff --git a/crates/parry2d/examples/proximity_query2d.rs b/crates/parry2d/examples/proximity_query2d.rs index 401946a6..644f1962 100644 --- a/crates/parry2d/examples/proximity_query2d.rs +++ b/crates/parry2d/examples/proximity_query2d.rs @@ -10,6 +10,14 @@ fn main() { let ball_pos_intersecting = Pose::translation(1.0, 1.0); let ball_pos_disjoint = Pose::translation(3.0, 3.0); - assert!(query::intersection_test(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid).unwrap()); - assert!(!query::intersection_test(&ball_pos_disjoint, &ball, &cuboid_pos, &cuboid).unwrap()); + assert!( + query::intersection_test(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid) + .unwrap() + .intersecting + ); + assert!( + !query::intersection_test(&ball_pos_disjoint, &ball, &cuboid_pos, &cuboid) + .unwrap() + .intersecting + ); } diff --git a/crates/parry2d/tests/issue_431_cuboid_distance_asymmetry.rs b/crates/parry2d/tests/issue_431_cuboid_distance_asymmetry.rs index 53823daa..1993f88b 100644 --- a/crates/parry2d/tests/issue_431_cuboid_distance_asymmetry.rs +++ b/crates/parry2d/tests/issue_431_cuboid_distance_asymmetry.rs @@ -11,8 +11,8 @@ use parry2d::query::{self, ClosestPoints}; use parry2d::shape::Cuboid; fn check_symmetric_and_exact(p1: &Pose, c1: &Cuboid, p2: &Pose, c2: &Cuboid, expected: Real) { - let d12 = query::distance(p1, c1, p2, c2).unwrap(); - let d21 = query::distance(p2, c2, p1, c1).unwrap(); + let d12 = query::distance(p1, c1, p2, c2).unwrap().distance; + let d21 = query::distance(p2, c2, p1, c1).unwrap().distance; // Cross-check against the exact GJK closest points. let gjk_dist = match query::closest_points(p1, c1, p2, c2, Real::MAX).unwrap() { @@ -91,8 +91,8 @@ fn touching_and_overlapping() { for x in [2.0, 1.5] { let p2 = Pose::new(Vector::new(x, 0.0), 0.0); - let d12 = query::distance(&p1, &c1, &p2, &c2).unwrap(); - let d21 = query::distance(&p2, &c2, &p1, &c1).unwrap(); + let d12 = query::distance(&p1, &c1, &p2, &c2).unwrap().distance; + let d21 = query::distance(&p2, &c2, &p1, &c1).unwrap().distance; assert!(d12.abs() < 1.0e-6, "expected zero distance, got {d12}"); assert!(d21.abs() < 1.0e-6, "expected zero distance, got {d21}"); } diff --git a/crates/parry2d/tests/query/closest_points_cuboid_cuboid.rs b/crates/parry2d/tests/query/closest_points_cuboid_cuboid.rs index 48403f5f..ac4b9aab 100644 --- a/crates/parry2d/tests/query/closest_points_cuboid_cuboid.rs +++ b/crates/parry2d/tests/query/closest_points_cuboid_cuboid.rs @@ -36,7 +36,9 @@ fn closest_points_cuboid_cuboid_axis_aligned_diagonal() { // The same failure is reachable through the public API: `query::distance` // dispatches cuboid-cuboid pairs to `distance_cuboid_cuboid`, which is // implemented on top of `closest_points_cuboid_cuboid`. - let dist = query::distance(&Pose::IDENTITY, &cuboid, &pos12, &cuboid).unwrap(); + let dist = query::distance(&Pose::IDENTITY, &cuboid, &pos12, &cuboid) + .unwrap() + .distance; assert_relative_eq!(dist, true_dist, epsilon = 1e-5); } } @@ -60,7 +62,9 @@ fn closest_points_cuboid_cuboid_axis_aligned_corner_touching() { other => panic!("expected WithinMargin at distance {gap}, got {other:?}"), } - let dist = query::distance(&Pose::IDENTITY, &cuboid, &pos12, &cuboid).unwrap(); + let dist = query::distance(&Pose::IDENTITY, &cuboid, &pos12, &cuboid) + .unwrap() + .distance; assert_relative_eq!(dist, gap, epsilon = 1e-5); } @@ -89,6 +93,8 @@ fn closest_points_cuboid_cuboid_axis_aligned_subulp_gap() { ClosestPoints::Disjoint => panic!("expected WithinMargin, got Disjoint"), } - let dist = query::distance(&Pose::IDENTITY, &c1, &pos12, &c2).unwrap(); + let dist = query::distance(&Pose::IDENTITY, &c1, &pos12, &c2) + .unwrap() + .distance; assert!(dist < 1.0e-5, "distance = {dist}"); } diff --git a/crates/parry2d/tests/query/point_composite_shape.rs b/crates/parry2d/tests/query/point_composite_shape.rs index d561a206..473d1602 100644 --- a/crates/parry2d/tests/query/point_composite_shape.rs +++ b/crates/parry2d/tests/query/point_composite_shape.rs @@ -13,7 +13,7 @@ fn project_local_point_and_get_feature_gets_the_enclosing_triangle() { let mesh = TriMesh::new(vertices, vec![[0, 1, 2], [3, 0, 2]]).unwrap(); let query_pt = Vector::new(0.6, 0.6); // Inside the top-right triangle (index 1) - let (proj, feat) = mesh.project_local_point_and_get_feature(query_pt); + let (proj, _feat) = mesh.project_local_point_and_get_feature(query_pt); let correct_tri_idx = 1; let correct_tri = mesh.triangle(correct_tri_idx); @@ -22,7 +22,7 @@ fn project_local_point_and_get_feature_gets_the_enclosing_triangle() { assert!(is_inside_correct); assert_eq!(proj.is_inside, is_inside_correct); - assert_eq!(feat.unwrap_face(), correct_tri_idx); + assert_eq!(proj.subshape, correct_tri_idx); } #[test] @@ -39,7 +39,7 @@ fn project_local_point_and_get_feature_projects_correctly_from_outside() { { let query_pt = Vector::new(-1.0, 0.0); // Left from the bottom-left triangle (index 0) - let (proj, feat) = mesh.project_local_point_and_get_feature(query_pt); + let (proj, _feat) = mesh.project_local_point_and_get_feature(query_pt); let correct_tri_idx = 0; let correct_tri = mesh.triangle(correct_tri_idx); @@ -49,12 +49,12 @@ fn project_local_point_and_get_feature_projects_correctly_from_outside() { assert_eq!(is_inside_correct, false); assert_eq!(proj.is_inside, is_inside_correct); assert_eq!(proj.point, Vector::ZERO); - assert_eq!(feat.unwrap_face(), correct_tri_idx); + assert_eq!(proj.subshape, correct_tri_idx); } { let query_pt = Vector::new(0.5, 2.0); // Above the top-right triangle (index 1) - let (proj, feat) = mesh.project_local_point_and_get_feature(query_pt); + let (proj, _feat) = mesh.project_local_point_and_get_feature(query_pt); let correct_tri_idx = 1; let correct_tri = mesh.triangle(correct_tri_idx); @@ -64,6 +64,6 @@ fn project_local_point_and_get_feature_projects_correctly_from_outside() { assert_eq!(is_inside_correct, false); assert_eq!(proj.is_inside, is_inside_correct); assert_eq!(proj.point, Vector::new(0.5, 1.0)); - assert_eq!(feat.unwrap_face(), correct_tri_idx); + assert_eq!(proj.subshape, correct_tri_idx); } } diff --git a/crates/parry2d/tests/sub_shape_id.rs b/crates/parry2d/tests/sub_shape_id.rs new file mode 100644 index 00000000..417c62d9 --- /dev/null +++ b/crates/parry2d/tests/sub_shape_id.rs @@ -0,0 +1,160 @@ +use parry2d::math::{Pose, Real, Vector}; +use parry2d::query::{self, PointQuery, Ray, RayCast}; +use parry2d::shape::{Ball, Compound, Cuboid, Polyline, SharedShape}; + +/// Three unit boxes in a row along x, centered at x = 0, 4 and 8. +fn three_boxes() -> Compound { + Compound::new( + (0..3) + .map(|i| { + ( + Pose::from_translation(Vector::new(i as Real * 4.0, 0.0)), + SharedShape::new(Cuboid::new(Vector::splat(0.5))), + ) + }) + .collect(), + ) +} + +#[test] +fn queries_against_a_compound_report_the_part() { + let compound = three_boxes(); + let probe = Ball::new(0.25); + + for part in 0..3u32 { + let x = part as Real * 4.0; + + let ray = Ray::new(Vector::new(x, 5.0), Vector::new(0.0, -1.0)); + assert_eq!( + compound + .cast_local_ray_and_get_normal(&ray, Real::MAX, true) + .expect("hits a box") + .subshape, + part + ); + + assert_eq!( + compound + .project_local_point(Vector::new(x, 3.0), false) + .subshape, + part + ); + + let pose12 = Pose::from_translation(Vector::new(x, 0.7)); + let contact = query::contact(&Pose::IDENTITY, &compound, &pose12, &probe, 1.0) + .unwrap() + .expect("within prediction"); + assert_eq!((contact.subshape1, contact.subshape2), (part, 0)); + + let dist = query::distance(&Pose::IDENTITY, &compound, &pose12, &probe).unwrap(); + assert_eq!((dist.subshape1, dist.subshape2), (part, 0)); + + let overlapping = Pose::from_translation(Vector::new(x, 0.0)); + let test = + query::intersection_test(&Pose::IDENTITY, &compound, &overlapping, &probe).unwrap(); + assert!(test.intersecting); + assert_eq!((test.subshape1, test.subshape2), (part, 0)); + } +} + +#[test] +fn a_polyline_reports_the_segment() { + // A staircase: segment 0 spans x in [0,1], segment 1 [1,2], segment 2 [2,3]. + let polyline = Polyline::new( + vec![ + Vector::new(0.0, 0.0), + Vector::new(1.0, 0.0), + Vector::new(2.0, 0.0), + Vector::new(3.0, 0.0), + ], + None, + ); + + for segment in 0..3u32 { + let x = segment as Real + 0.5; + assert_eq!( + polyline + .project_local_point(Vector::new(x, 2.0), false) + .subshape, + segment + ); + + let ray = Ray::new(Vector::new(x, 2.0), Vector::new(0.0, -1.0)); + assert_eq!( + polyline + .cast_local_ray_and_get_normal(&ray, Real::MAX, true) + .expect("hits the polyline") + .subshape, + segment + ); + } +} + +/// The conversion resolves a segment endpoint to the polyline vertex it indexes, so the two +/// segments meeting at a corner name the same vertex. +#[test] +fn segment_features_convert_to_polyline_features() { + use parry2d::shape::FeatureId; + + // Segment 0 spans vertices 0-1, segment 1 spans 1-2: they share vertex 1. + let polyline = Polyline::new( + vec![ + Vector::new(0.0, 0.0), + Vector::new(1.0, 0.0), + Vector::new(2.0, 0.0), + ], + None, + ); + + // Endpoint 1 of segment 0 and endpoint 0 of segment 1 are the same polyline vertex. + assert_eq!( + polyline.segment_feature_to_polyline_feature(0, FeatureId::Vertex(1)), + FeatureId::Vertex(1) + ); + assert_eq!( + polyline.segment_feature_to_polyline_feature(1, FeatureId::Vertex(0)), + FeatureId::Vertex(1) + ); + assert_eq!( + polyline.segment_feature_to_polyline_feature(1, FeatureId::Vertex(1)), + FeatureId::Vertex(2) + ); + + // Each segment has a side facing each way, and every (segment, side) pair is distinct. + let sides: Vec<_> = (0..2u32) + .flat_map(|segment| (0..2u32).map(move |side| (segment, side))) + .map(|(segment, side)| { + polyline.segment_feature_to_polyline_feature(segment, FeatureId::Face(side)) + }) + .collect(); + for (i, face) in sides.iter().enumerate() { + assert!(!sides[..i].contains(face), "{face:?} reused"); + } +} + +#[test] +fn shapes_without_sub_shapes_report_zero() { + let ball = Ball::new(1.0); + let ray = Ray::new(Vector::new(0.0, 5.0), Vector::new(0.0, -1.0)); + + assert_eq!( + ball.cast_local_ray_and_get_normal(&ray, Real::MAX, true) + .unwrap() + .subshape, + 0 + ); + assert_eq!( + ball.project_local_point(Vector::new(3.0, 0.0), false) + .subshape, + 0 + ); + + let dist = query::distance( + &Pose::IDENTITY, + &ball, + &Pose::from_translation(Vector::new(5.0, 0.0)), + &Ball::new(1.0), + ) + .unwrap(); + assert_eq!((dist.subshape1, dist.subshape2), (0, 0)); +} diff --git a/crates/parry3d/examples/distance_query3d.rs b/crates/parry3d/examples/distance_query3d.rs index 1bf97289..f39d2ed8 100644 --- a/crates/parry3d/examples/distance_query3d.rs +++ b/crates/parry3d/examples/distance_query3d.rs @@ -13,9 +13,12 @@ fn main() { let ball_pos_intersecting = Pose::translation(0.0, 1.0, 0.0); let ball_pos_disjoint = Pose::translation(0.0, 3.0, 0.0); - let dist_intersecting = - query::distance(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid).unwrap(); - let dist_disjoint = query::distance(&ball_pos_disjoint, &ball, &cuboid_pos, &cuboid).unwrap(); + let dist_intersecting = query::distance(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid) + .unwrap() + .distance; + let dist_disjoint = query::distance(&ball_pos_disjoint, &ball, &cuboid_pos, &cuboid) + .unwrap() + .distance; assert_eq!(dist_intersecting, 0.0); assert!(relative_eq!(dist_disjoint, 1.0, epsilon = 1.0e-7)); diff --git a/crates/parry3d/examples/proximity_query3d.rs b/crates/parry3d/examples/proximity_query3d.rs index 235c96c5..3023b7db 100644 --- a/crates/parry3d/examples/proximity_query3d.rs +++ b/crates/parry3d/examples/proximity_query3d.rs @@ -10,9 +10,13 @@ fn main() { let ball_pos_disjoint = Pose::translation(3.0, 3.0, 3.0); let intersecting = - query::intersection_test(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid).unwrap(); + query::intersection_test(&ball_pos_intersecting, &ball, &cuboid_pos, &cuboid) + .unwrap() + .intersecting; let not_intersecting = - !query::intersection_test(&ball_pos_disjoint, &ball, &cuboid_pos, &cuboid).unwrap(); + !query::intersection_test(&ball_pos_disjoint, &ball, &cuboid_pos, &cuboid) + .unwrap() + .intersecting; assert!(intersecting); assert!(not_intersecting); diff --git a/crates/parry3d/tests/issue_157_frustum_contact.rs b/crates/parry3d/tests/issue_157_frustum_contact.rs index a07bbe3f..275c3908 100644 --- a/crates/parry3d/tests/issue_157_frustum_contact.rs +++ b/crates/parry3d/tests/issue_157_frustum_contact.rs @@ -111,5 +111,5 @@ fn convex_polyhedra_intersection() { .unwrap(); assert!(num_contained_points == 4); - assert!(intersects); + assert!(intersects.intersecting); } diff --git a/crates/parry3d/tests/issue_396_cylinder_intersection_false_negative.rs b/crates/parry3d/tests/issue_396_cylinder_intersection_false_negative.rs index d8643fa1..23e65f4e 100644 --- a/crates/parry3d/tests/issue_396_cylinder_intersection_false_negative.rs +++ b/crates/parry3d/tests/issue_396_cylinder_intersection_false_negative.rs @@ -15,7 +15,10 @@ fn coaxial_overlapping_cylinders_intersect() { let big = Cylinder::new(50.0, 100.0); let small = Cylinder::new(1.5, 1.0); - assert_eq!(intersection_test(&iso, &big, &iso, &small), Ok(true)); + assert_eq!( + intersection_test(&iso, &big, &iso, &small).map(|r| r.intersecting), + Ok(true) + ); } #[test] @@ -29,7 +32,10 @@ fn coaxial_cylinder_capsule_intersect() { 10.0, ); - assert_eq!(intersection_test(&iso, &big, &iso, &small), Ok(true)); + assert_eq!( + intersection_test(&iso, &big, &iso, &small).map(|r| r.intersecting), + Ok(true) + ); } #[test] @@ -46,7 +52,7 @@ fn offset_and_rotated_cylinders_intersecting() { ] { let pos2 = Pose::from_translation(offset); assert_eq!( - intersection_test(&Pose::IDENTITY, &big, &pos2, &small), + intersection_test(&Pose::IDENTITY, &big, &pos2, &small).map(|r| r.intersecting), Ok(true), "offset {offset:?} should intersect" ); @@ -56,7 +62,7 @@ fn offset_and_rotated_cylinders_intersecting() { for angle in [0.01, 0.5, core::f32::consts::FRAC_PI_2] { let pos2 = Pose::rotation(Vector::new(0.0, 0.0, angle)); assert_eq!( - intersection_test(&Pose::IDENTITY, &big, &pos2, &small), + intersection_test(&Pose::IDENTITY, &big, &pos2, &small).map(|r| r.intersecting), Ok(true), "rotation angle {angle} should intersect" ); @@ -74,7 +80,7 @@ fn coaxial_cylinders_separated() { for dy in [51.6, 60.0, 200.0] { let pos2 = Pose::translation(0.0, dy, 0.0); assert_eq!( - intersection_test(&Pose::IDENTITY, &big, &pos2, &small), + intersection_test(&Pose::IDENTITY, &big, &pos2, &small).map(|r| r.intersecting), Ok(false), "axial offset {dy} should be disjoint" ); @@ -84,7 +90,7 @@ fn coaxial_cylinders_separated() { for dx in [101.1, 110.0, 500.0] { let pos2 = Pose::translation(dx, 0.0, 0.0); assert_eq!( - intersection_test(&Pose::IDENTITY, &big, &pos2, &small), + intersection_test(&Pose::IDENTITY, &big, &pos2, &small).map(|r| r.intersecting), Ok(false), "radial offset {dx} should be disjoint" ); @@ -93,7 +99,7 @@ fn coaxial_cylinders_separated() { // Separated and rotated. let pos2 = Pose::new(Vector::new(0.0, 53.0, 0.0), Vector::new(0.0, 0.0, 0.7)); assert_eq!( - intersection_test(&Pose::IDENTITY, &big, &pos2, &small), + intersection_test(&Pose::IDENTITY, &big, &pos2, &small).map(|r| r.intersecting), Ok(false), "rotated separated pair should be disjoint" ); diff --git a/crates/parry3d/tests/issue_431_cuboid_distance_asymmetry.rs b/crates/parry3d/tests/issue_431_cuboid_distance_asymmetry.rs index 2bd8f2a0..9c3f3943 100644 --- a/crates/parry3d/tests/issue_431_cuboid_distance_asymmetry.rs +++ b/crates/parry3d/tests/issue_431_cuboid_distance_asymmetry.rs @@ -11,8 +11,8 @@ use parry3d::query::{self, ClosestPoints}; use parry3d::shape::Cuboid; fn check_symmetric_and_exact(p1: &Pose, c1: &Cuboid, p2: &Pose, c2: &Cuboid, expected: Real) { - let d12 = query::distance(p1, c1, p2, c2).unwrap(); - let d21 = query::distance(p2, c2, p1, c1).unwrap(); + let d12 = query::distance(p1, c1, p2, c2).unwrap().distance; + let d21 = query::distance(p2, c2, p1, c1).unwrap().distance; // Cross-check against the exact GJK closest points. let gjk_dist = match query::closest_points(p1, c1, p2, c2, Real::MAX).unwrap() { @@ -102,8 +102,8 @@ fn touching_and_overlapping() { for x in [2.0, 1.5] { let p2 = Pose::from_translation(Vector::new(x, 0.0, 0.0)); - let d12 = query::distance(&p1, &c1, &p2, &c2).unwrap(); - let d21 = query::distance(&p2, &c2, &p1, &c1).unwrap(); + let d12 = query::distance(&p1, &c1, &p2, &c2).unwrap().distance; + let d21 = query::distance(&p2, &c2, &p1, &c1).unwrap().distance; assert!(d12.abs() < 1.0e-6, "expected zero distance, got {d12}"); assert!(d21.abs() < 1.0e-6, "expected zero distance, got {d21}"); } diff --git a/crates/parry3d/tests/issue_70_capsule_cuboid_false_negatives.rs b/crates/parry3d/tests/issue_70_capsule_cuboid_false_negatives.rs index 89e1e101..a7bf5aa5 100644 --- a/crates/parry3d/tests/issue_70_capsule_cuboid_false_negatives.rs +++ b/crates/parry3d/tests/issue_70_capsule_cuboid_false_negatives.rs @@ -34,16 +34,28 @@ fn capsule_cuboid_sweep_has_no_false_negatives() { let y = y_min + step_size * step as f32; let test_pos = Pose::translation(0.0, y, 0.0); - if intersection_test(&test_pos, &capsule, &Pose::IDENTITY, &halfspace).unwrap() { + if intersection_test(&test_pos, &capsule, &Pose::IDENTITY, &halfspace) + .unwrap() + .intersecting + { capsule_halfspace += 1; } - if intersection_test(&test_pos, &capsule, &cuboid_pos, &cuboid).unwrap() { + if intersection_test(&test_pos, &capsule, &cuboid_pos, &cuboid) + .unwrap() + .intersecting + { capsule_cuboid += 1; } - if intersection_test(&test_pos, &ball, &Pose::IDENTITY, &halfspace).unwrap() { + if intersection_test(&test_pos, &ball, &Pose::IDENTITY, &halfspace) + .unwrap() + .intersecting + { ball_halfspace += 1; } - if intersection_test(&test_pos, &ball, &cuboid_pos, &cuboid).unwrap() { + if intersection_test(&test_pos, &ball, &cuboid_pos, &cuboid) + .unwrap() + .intersecting + { ball_cuboid += 1; } } diff --git a/crates/parry3d/tests/query/closest_points_cuboid_cuboid.rs b/crates/parry3d/tests/query/closest_points_cuboid_cuboid.rs index b73373e7..f615cdb5 100644 --- a/crates/parry3d/tests/query/closest_points_cuboid_cuboid.rs +++ b/crates/parry3d/tests/query/closest_points_cuboid_cuboid.rs @@ -30,7 +30,9 @@ fn closest_points_cuboid_cuboid_axis_aligned_vertex_vertex() { ), } - let dist = query::distance(&Pose::IDENTITY, &cuboid, &pos12, &cuboid).unwrap(); + let dist = query::distance(&Pose::IDENTITY, &cuboid, &pos12, &cuboid) + .unwrap() + .distance; assert_relative_eq!(dist, true_dist, epsilon = 1e-5); } } @@ -66,7 +68,9 @@ fn closest_points_cuboid_cuboid_axis_aligned_edge_edge() { other => panic!("expected WithinMargin at distance {true_dist}, got {other:?}"), } - let dist = query::distance(&Pose::IDENTITY, &c1, &pos12, &c2).unwrap(); + let dist = query::distance(&Pose::IDENTITY, &c1, &pos12, &c2) + .unwrap() + .distance; assert_relative_eq!(dist, true_dist, epsilon = 1e-5); } @@ -87,6 +91,8 @@ fn closest_points_cuboid_cuboid_axis_aligned_edge_touching() { other => panic!("expected WithinMargin at distance {true_dist}, got {other:?}"), } - let dist = query::distance(&Pose::IDENTITY, &cuboid, &pos12, &cuboid).unwrap(); + let dist = query::distance(&Pose::IDENTITY, &cuboid, &pos12, &cuboid) + .unwrap() + .distance; assert_relative_eq!(dist, true_dist, epsilon = 1e-4); } diff --git a/crates/parry3d/tests/sub_shape_id.rs b/crates/parry3d/tests/sub_shape_id.rs new file mode 100644 index 00000000..7470f4c2 --- /dev/null +++ b/crates/parry3d/tests/sub_shape_id.rs @@ -0,0 +1,239 @@ +use parry3d::math::{Pose, Real, Vector}; +use parry3d::query::{self, PointQuery, Ray, RayCast, ShapeCastOptions}; +use parry3d::shape::{Ball, Compound, Cuboid, SharedShape, TriMesh}; + +/// Three unit boxes in a row along x, centered at x = 0, 4 and 8, so a query aimed at one of them +/// can only be answered by that part. +fn three_boxes() -> Compound { + Compound::new( + (0..3) + .map(|i| { + ( + Pose::from_translation(Vector::new(i as Real * 4.0, 0.0, 0.0)), + SharedShape::new(Cuboid::new(Vector::splat(0.5))), + ) + }) + .collect(), + ) +} + +#[test] +fn ray_cast_reports_the_part_it_hit() { + let compound = three_boxes(); + + for part in 0..3u32 { + let ray = Ray::new( + Vector::new(part as Real * 4.0, 5.0, 0.0), + Vector::new(0.0, -1.0, 0.0), + ); + let hit = compound + .cast_local_ray_and_get_normal(&ray, Real::MAX, true) + .expect("hits a box"); + assert_eq!(hit.subshape, part, "ray aimed at part {part}"); + } +} + +#[test] +fn point_projection_reports_the_part_it_projects_onto() { + let compound = three_boxes(); + + for part in 0..3u32 { + let point = Vector::new(part as Real * 4.0, 3.0, 0.0); + assert_eq!(compound.project_local_point(point, false).subshape, part); + assert_eq!( + compound + .project_local_point_and_get_feature(point) + .0 + .subshape, + part + ); + } +} + +#[test] +fn contact_shape_cast_distance_and_intersection_report_the_part() { + let compound = three_boxes(); + let probe = Ball::new(0.25); + + for part in 0..3u32 { + // A ball resting just above the top face of one box. + let pose12 = Pose::from_translation(Vector::new(part as Real * 4.0, 0.7, 0.0)); + + let contact = query::contact(&Pose::IDENTITY, &compound, &pose12, &probe, 1.0) + .unwrap() + .expect("within prediction"); + assert_eq!(contact.subshape1, part, "contact"); + assert_eq!(contact.subshape2, 0, "the ball has no sub-shapes"); + + let dist = query::distance(&Pose::IDENTITY, &compound, &pose12, &probe).unwrap(); + assert_eq!(dist.subshape1, part, "distance"); + assert_eq!(dist.subshape2, 0); + + // Cast the ball straight down into that box. + let hit = query::cast_shapes( + &Pose::IDENTITY, + Vector::ZERO, + &compound, + &Pose::from_translation(Vector::new(part as Real * 4.0, 5.0, 0.0)), + Vector::new(0.0, -1.0, 0.0), + &probe, + ShapeCastOptions::default(), + ) + .unwrap() + .expect("the ball reaches the box"); + assert_eq!(hit.subshape1, part, "shape cast"); + + // Overlapping the box outright. + let overlapping = Pose::from_translation(Vector::new(part as Real * 4.0, 0.0, 0.0)); + let test = + query::intersection_test(&Pose::IDENTITY, &compound, &overlapping, &probe).unwrap(); + assert!(test.intersecting); + assert_eq!(test.subshape1, part, "intersection test"); + } +} + +#[test] +fn the_roles_swap_when_the_composite_is_the_second_shape() { + let compound = three_boxes(); + let probe = Ball::new(0.25); + let part = 2u32; + let pose1 = Pose::from_translation(Vector::new(part as Real * 4.0, 0.7, 0.0)); + + // Same query with the composite as shape 2: the id must land on `subshape2`. + let contact = query::contact(&pose1, &probe, &Pose::IDENTITY, &compound, 1.0) + .unwrap() + .expect("within prediction"); + assert_eq!(contact.subshape1, 0, "the ball has no sub-shapes"); + assert_eq!(contact.subshape2, part); + + let dist = query::distance(&pose1, &probe, &Pose::IDENTITY, &compound).unwrap(); + assert_eq!(dist.subshape1, 0); + assert_eq!(dist.subshape2, part); + + let overlapping = Pose::from_translation(Vector::new(part as Real * 4.0, 0.0, 0.0)); + let test = query::intersection_test(&overlapping, &probe, &Pose::IDENTITY, &compound).unwrap(); + assert!(test.intersecting); + assert_eq!(test.subshape1, 0); + assert_eq!(test.subshape2, part); +} + +#[test] +fn a_trimesh_reports_the_triangle() { + // Two triangles forming a quad in the xz plane; each query lands on a known one. + let vertices = vec![ + Vector::new(0.0, 0.0, 0.0), + Vector::new(1.0, 0.0, 0.0), + Vector::new(1.0, 0.0, 1.0), + Vector::new(0.0, 0.0, 1.0), + ]; + let mesh = TriMesh::new(vertices, vec![[0, 1, 2], [0, 2, 3]]).unwrap(); + + // A point over the first triangle's interior, and one over the second's. + let over_first = Vector::new(0.8, 1.0, 0.4); + let over_second = Vector::new(0.2, 1.0, 0.6); + assert_eq!(mesh.project_local_point(over_first, false).subshape, 0); + assert_eq!(mesh.project_local_point(over_second, false).subshape, 1); + + let ray = Ray::new(over_first, Vector::new(0.0, -1.0, 0.0)); + let hit = mesh + .cast_local_ray_and_get_normal(&ray, Real::MAX, true) + .expect("hits the mesh"); + assert_eq!(hit.subshape, 0); +} + +/// The feature a query reports is the sub-shape's own; the sub-shape itself is `subshape`. +#[test] +fn features_are_local_to_the_sub_shape() { + use parry3d::shape::FeatureId; + + // A quad of two triangles in the xz plane, wound so its front faces up. + let mesh = TriMesh::new( + vec![ + Vector::new(0.0, 0.0, 0.0), + Vector::new(1.0, 0.0, 0.0), + Vector::new(1.0, 0.0, 1.0), + Vector::new(0.0, 0.0, 1.0), + ], + vec![[0, 1, 2], [0, 2, 3]], + ) + .unwrap(); + + // Hitting triangle 1 from above reports one of the triangle's own two faces, never its index. + let from_above = Ray::new(Vector::new(0.2, 1.0, 0.6), Vector::new(0.0, -1.0, 0.0)); + let hit = mesh + .cast_local_ray_and_get_normal(&from_above, Real::MAX, true) + .expect("hits the mesh"); + assert_eq!(hit.subshape, 1); + assert!(matches!( + hit.feature, + FeatureId::Face(0) | FeatureId::Face(1) + )); + + // The two sides of a triangle are told apart by the feature alone. + let from_below = Ray::new(Vector::new(0.2, -1.0, 0.6), Vector::new(0.0, 1.0, 0.0)); + let below = mesh + .cast_local_ray_and_get_normal(&from_below, Real::MAX, true) + .expect("hits the mesh"); + assert_eq!(below.subshape, 1); + assert_ne!( + mesh.is_backface(hit.feature), + mesh.is_backface(below.feature), + "one of the two sides has to be the backface" + ); +} + +/// `feature_normal_at_point` needs the sub-shape to pick the triangle: the feature alone is the +/// triangle's own and says nothing about which one it is. +#[test] +fn a_trimesh_normal_follows_the_sub_shape() { + use parry3d::shape::{FeatureId, Shape}; + + // Two triangles with clearly different normals: one flat, one tilted. + let mesh = TriMesh::new( + vec![ + Vector::new(0.0, 0.0, 0.0), + Vector::new(1.0, 0.0, 0.0), + Vector::new(1.0, 0.0, 1.0), + Vector::new(0.0, 1.0, 1.0), + ], + vec![[0, 1, 2], [0, 2, 3]], + ) + .unwrap(); + + for triangle in 0..2u32 { + let expected = mesh.triangle(triangle).normal().unwrap(); + let normal = mesh + .feature_normal_at_point(triangle, FeatureId::Face(0), Vector::ZERO) + .expect("the sub-shape names the triangle"); + assert_eq!(mesh.triangle_normal(triangle), Some(normal)); + assert!( + (normal - expected).length() < 1.0e-5, + "triangle {triangle}: {normal:?} vs {expected:?}" + ); + } +} + +#[test] +fn shapes_without_sub_shapes_report_zero() { + let ball = Ball::new(1.0); + let cuboid = Cuboid::new(Vector::splat(1.0)); + + let ray = Ray::new(Vector::new(0.0, 5.0, 0.0), Vector::new(0.0, -1.0, 0.0)); + assert_eq!( + ball.cast_local_ray_and_get_normal(&ray, Real::MAX, true) + .unwrap() + .subshape, + 0 + ); + assert_eq!( + ball.project_local_point(Vector::new(3.0, 0.0, 0.0), false) + .subshape, + 0 + ); + + let pose12 = Pose::from_translation(Vector::new(1.5, 0.0, 0.0)); + let contact = query::contact(&Pose::IDENTITY, &ball, &pose12, &cuboid, 1.0) + .unwrap() + .expect("within prediction"); + assert_eq!((contact.subshape1, contact.subshape2), (0, 0)); +} diff --git a/src/query/closest_points/closest_points_composite_shape_shape.rs b/src/query/closest_points/closest_points_composite_shape_shape.rs index 90cd0fea..ad909d6f 100644 --- a/src/query/closest_points/closest_points_composite_shape_shape.rs +++ b/src/query/closest_points/closest_points_composite_shape_shape.rs @@ -2,7 +2,7 @@ use crate::bounding_volume::Aabb; use crate::math::{Pose, Real}; use crate::partitioning::BvhNode; use crate::query::{ClosestPoints, QueryDispatcher}; -use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; +use crate::shape::{CompositeShapeRef, Shape, SubShapeId, TypedCompositeShape}; use crate::utils::PoseOpt; impl CompositeShapeRef<'_, S> { @@ -23,7 +23,7 @@ impl CompositeShapeRef<'_, S> { pose12: &Pose, shape2: &dyn Shape, margin: Real, - ) -> Option<(u32, ClosestPoints)> { + ) -> Option<(SubShapeId, ClosestPoints)> { let ls_aabb2 = shape2.compute_aabb(pose12); let msum_shift = -ls_aabb2.center(); let msum_margin = ls_aabb2.half_extents(); diff --git a/src/query/contact/contact.rs b/src/query/contact/contact.rs index 268f6bfa..63a27446 100644 --- a/src/query/contact/contact.rs +++ b/src/query/contact/contact.rs @@ -1,4 +1,5 @@ use crate::math::{Pose, Real, Vector}; +use crate::shape::SubShapeId; use core::mem; /// Geometric description of a contact between two shapes. @@ -97,6 +98,16 @@ pub struct Contact { /// /// For collision resolution, use `-dist` as the penetration depth when `dist < 0.0`. pub dist: Real, + + /// The sub-shape of the first shape this contact is on. + /// + /// Always `0` for a shape with no sub-shapes. + pub subshape1: SubShapeId, + + /// The sub-shape of the second shape this contact is on. + /// + /// Always `0` for a shape with no sub-shapes. + pub subshape2: SubShapeId, } impl Contact { @@ -141,8 +152,18 @@ impl Contact { normal1, normal2, dist, + subshape1: 0, + subshape2: 0, } } + + /// Sets the sub-shapes this contact is between. + #[inline] + pub fn with_subshapes(mut self, subshape1: SubShapeId, subshape2: SubShapeId) -> Self { + self.subshape1 = subshape1; + self.subshape2 = subshape2; + self + } } impl Contact { @@ -151,6 +172,7 @@ impl Contact { pub fn flip(&mut self) { mem::swap(&mut self.point1, &mut self.point2); mem::swap(&mut self.normal1, &mut self.normal2); + mem::swap(&mut self.subshape1, &mut self.subshape2); } /// Returns a new contact containing the swapped points and normals of `self`. diff --git a/src/query/contact/contact_ball_convex_polyhedron.rs b/src/query/contact/contact_ball_convex_polyhedron.rs index da3fde53..fdec89ac 100644 --- a/src/query/contact/contact_ball_convex_polyhedron.rs +++ b/src/query/contact/contact_ball_convex_polyhedron.rs @@ -44,7 +44,7 @@ pub fn contact_convex_polyhedron_ball( } else { dist = -ball2.radius; normal1 = shape1 - .feature_normal_at_point(f1, proj.point) + .feature_normal_at_point(proj.subshape, f1, proj.point) .or_else(|| (proj.point).try_normalize()) .unwrap_or(Vector::Y); } diff --git a/src/query/contact/contact_composite_shape_shape.rs b/src/query/contact/contact_composite_shape_shape.rs index 6012e76e..b978a29d 100644 --- a/src/query/contact/contact_composite_shape_shape.rs +++ b/src/query/contact/contact_composite_shape_shape.rs @@ -9,30 +9,32 @@ impl CompositeShapeRef<'_, S> { /// `pose12` relative to `self`. /// /// Returns `None` if `self` and `shape2` are separated by a distance larger than - /// `prediction`. Otherwise, returns the index of the sub-shape of `self` involved in the contact - /// as well as the contact information. + /// `prediction`. Otherwise the contact's `subshape1` says which sub-shape of `self` it is on. pub fn contact_with_shape( &self, dispatcher: &D, pose12: &Pose, shape2: &dyn Shape, prediction: Real, - ) -> Option<(u32, Contact)> { + ) -> Option { let ls_aabb2 = shape2.compute_aabb(pose12).loosened(prediction); - let mut result = None::<(u32, Contact)>; + let mut result = None::; for part_id in self.0.bvh().intersect_aabb(&ls_aabb2) { self.0.map_part_at(part_id, &mut |part_pos1, part1, _| { if let Ok(Some(mut c)) = dispatcher.contact(&part_pos1.inv_mul(pose12), part1, shape2, prediction) { - let replace = result.is_none_or(|(_, cbest)| c.dist < cbest.dist); + let replace = result.is_none_or(|cbest| c.dist < cbest.dist); if replace { if let Some(part_pos1) = part_pos1 { c.transform1_by_mut(part_pos1); } - result = Some((part_id, c)) + // `subshape2` is left as the dispatch set it: `shape2` may be a composite + // too, and only it knows which of its parts answered. + c.subshape1 = part_id; + result = Some(c) } } }); @@ -54,9 +56,7 @@ where D: ?Sized + QueryDispatcher, G1: ?Sized + CompositeShape, { - CompositeShapeRef(g1) - .contact_with_shape(dispatcher, pose12, g2, prediction) - .map(|c| c.1) + CompositeShapeRef(g1).contact_with_shape(dispatcher, pose12, g2, prediction) } /// Best contact between a shape and a composite (`Mesh`, `Compound`) shape. diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 2c03f029..675c337a 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -2,7 +2,7 @@ use crate::math::{Pose, Real, Vector}; use crate::query::details::ShapeCastOptions; use crate::query::{ self, details::NonlinearShapeCastMode, ClosestPoints, Contact, NonlinearRigidMotion, - QueryDispatcher, ShapeCastHit, Unsupported, + QueryDispatcher, ShapeCastHit, ShapeDistance, ShapeIntersection, Unsupported, }; #[cfg(feature = "alloc")] use crate::query::{ @@ -104,11 +104,13 @@ use crate::shape::ShapeType; /// /// // Query intersection /// let intersects = dispatcher.intersection_test(&pos12, &ball, &cuboid) -/// .expect("This shape pair is supported"); +/// .expect("This shape pair is supported") +/// .intersecting; /// /// // Query distance /// let dist = dispatcher.distance(&pos12, &ball, &cuboid) -/// .expect("This shape pair is supported"); +/// .expect("This shape pair is supported") +/// .distance; /// /// println!("Distance: {}, Intersecting: {}", dist, intersects); /// # } @@ -179,22 +181,16 @@ impl QueryDispatcher for DefaultQueryDispatcher { pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, - ) -> Result { + ) -> Result { if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) { let p12 = pos12.translation; - Ok(query::details::intersection_test_ball_ball(p12, b1, b2)) + Ok(query::details::intersection_test_ball_ball(p12, b1, b2).into()) } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) { - Ok(query::details::intersection_test_cuboid_cuboid( - pos12, c1, c2, - )) + Ok(query::details::intersection_test_cuboid_cuboid(pos12, c1, c2).into()) } else if let (Some(t1), Some(c2)) = (shape1.as_triangle(), shape2.as_cuboid()) { - Ok(query::details::intersection_test_triangle_cuboid( - pos12, t1, c2, - )) + Ok(query::details::intersection_test_triangle_cuboid(pos12, t1, c2).into()) } else if let (Some(c1), Some(t2)) = (shape1.as_cuboid(), shape2.as_triangle()) { - Ok(query::details::intersection_test_cuboid_triangle( - pos12, c1, t2, - )) + Ok(query::details::intersection_test_cuboid_triangle(pos12, c1, t2).into()) } else if let Some(b1) = shape1.as_ball() { Ok(query::details::intersection_test_ball_point_query( pos12, b1, shape2, @@ -206,19 +202,13 @@ impl QueryDispatcher for DefaultQueryDispatcher { } else if let (Some(p1), Some(s2)) = (shape1.as_shape::(), shape2.as_support_map()) { - Ok(query::details::intersection_test_halfspace_support_map( - pos12, p1, s2, - )) + Ok(query::details::intersection_test_halfspace_support_map(pos12, p1, s2).into()) } else if let (Some(s1), Some(p2)) = (shape1.as_support_map(), shape2.as_shape::()) { - Ok(query::details::intersection_test_support_map_halfspace( - pos12, s1, p2, - )) + Ok(query::details::intersection_test_support_map_halfspace(pos12, s1, p2).into()) } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { - Ok(query::details::intersection_test_support_map_support_map( - pos12, s1, s2, - )) + Ok(query::details::intersection_test_support_map_support_map(pos12, s1, s2).into()) } else { #[cfg(feature = "alloc")] if let Some(c1) = shape1.as_composite_shape() { @@ -251,41 +241,31 @@ impl QueryDispatcher for DefaultQueryDispatcher { pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, - ) -> Result { + ) -> Result { let ball1 = shape1.as_ball(); let ball2 = shape2.as_ball(); if let (Some(b1), Some(b2)) = (ball1, ball2) { let p2 = pos12.translation; - Ok(query::details::distance_ball_ball(b1, p2, b2)) + Ok(query::details::distance_ball_ball(b1, p2, b2).into()) } else if let (Some(b1), true) = (ball1, shape2.is_convex()) { - Ok(query::details::distance_ball_convex_polyhedron( - pos12, b1, shape2, - )) + Ok(query::details::distance_ball_convex_polyhedron(pos12, b1, shape2).into()) } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) { - Ok(query::details::distance_convex_polyhedron_ball( - pos12, shape1, b2, - )) + Ok(query::details::distance_convex_polyhedron_ball(pos12, shape1, b2).into()) } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) { - Ok(query::details::distance_cuboid_cuboid(pos12, c1, c2)) + Ok(query::details::distance_cuboid_cuboid(pos12, c1, c2).into()) } else if let (Some(s1), Some(s2)) = (shape1.as_segment(), shape2.as_segment()) { - Ok(query::details::distance_segment_segment(pos12, s1, s2)) + Ok(query::details::distance_segment_segment(pos12, s1, s2).into()) } else if let (Some(p1), Some(s2)) = (shape1.as_shape::(), shape2.as_support_map()) { - Ok(query::details::distance_halfspace_support_map( - pos12, p1, s2, - )) + Ok(query::details::distance_halfspace_support_map(pos12, p1, s2).into()) } else if let (Some(s1), Some(p2)) = (shape1.as_support_map(), shape2.as_shape::()) { - Ok(query::details::distance_support_map_halfspace( - pos12, s1, p2, - )) + Ok(query::details::distance_support_map_halfspace(pos12, s1, p2).into()) } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { - Ok(query::details::distance_support_map_support_map( - pos12, s1, s2, - )) + Ok(query::details::distance_support_map_support_map(pos12, s1, s2).into()) } else { #[cfg(feature = "alloc")] if let Some(c1) = shape1.as_composite_shape() { diff --git a/src/query/distance/distance.rs b/src/query/distance/distance.rs index 49094c6a..ecf4d151 100644 --- a/src/query/distance/distance.rs +++ b/src/query/distance/distance.rs @@ -1,3 +1,58 @@ +use crate::shape::SubShapeId; + +/// The distance between two shapes, and the sub-shapes it was measured between. +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct ShapeDistance { + /// The separation between the two shapes; `0.0` when they touch or overlap. + pub distance: Real, + /// The sub-shape of the first shape the distance was measured to. + /// + /// Always `0` for a shape with no sub-shapes. + pub subshape1: SubShapeId, + /// The sub-shape of the second shape the distance was measured to. + /// + /// Always `0` for a shape with no sub-shapes. + pub subshape2: SubShapeId, +} + +impl ShapeDistance { + /// A distance measured between shapes with no sub-shapes to distinguish. + pub fn new(distance: Real) -> Self { + Self { + distance, + subshape1: 0, + subshape2: 0, + } + } + + /// Sets the sub-shapes this distance was measured between. + pub fn with_subshapes(mut self, subshape1: SubShapeId, subshape2: SubShapeId) -> Self { + self.subshape1 = subshape1; + self.subshape2 = subshape2; + self + } + + /// Swaps the roles of the two shapes. + pub fn swapped(mut self) -> Self { + core::mem::swap(&mut self.subshape1, &mut self.subshape2); + self + } +} + +#[cfg(feature = "alloc")] +impl crate::partitioning::BvhLeafCost for ShapeDistance { + #[inline] + fn cost(&self) -> Real { + self.distance + } +} + +impl From for ShapeDistance { + fn from(distance: Real) -> Self { + Self::new(distance) + } +} + use crate::math::{Pose, Real}; use crate::query::{DefaultQueryDispatcher, QueryDispatcher, Unsupported}; @@ -53,7 +108,7 @@ use crate::shape::Shape; /// let pos2 = Pose::translation(10.0, 0.0, 0.0); /// /// // Compute distance -/// let dist = distance(&pos1, &ball1, &pos2, &ball2).unwrap(); +/// let dist = distance(&pos1, &ball1, &pos2, &ball2).unwrap().distance; /// /// // Distance = 10.0 (separation) - 1.0 (radius1) - 2.0 (radius2) = 7.0 /// assert_eq!(dist, 7.0); @@ -74,7 +129,7 @@ use crate::shape::Shape; /// let pos1 = Pose::translation(0.0, 0.0, 0.0); /// let pos2 = Pose::translation(1.5, 0.0, 0.0); // Edge to edge /// -/// let dist = distance(&pos1, &box1, &pos2, &box2).unwrap(); +/// let dist = distance(&pos1, &box1, &pos2, &box2).unwrap().distance; /// /// // They're touching, so distance is 0.0 /// assert_eq!(dist, 0.0); @@ -91,7 +146,7 @@ pub fn distance( g1: &dyn Shape, pos2: &Pose, g2: &dyn Shape, -) -> Result { +) -> Result { let pos12 = pos1.inv_mul(pos2); DefaultQueryDispatcher.distance(&pos12, g1, g2) } diff --git a/src/query/distance/distance_composite_shape_shape.rs b/src/query/distance/distance_composite_shape_shape.rs index 8ed7184f..2b485aec 100644 --- a/src/query/distance/distance_composite_shape_shape.rs +++ b/src/query/distance/distance_composite_shape_shape.rs @@ -1,7 +1,7 @@ use crate::bounding_volume::Aabb; use crate::math::{Pose, Real}; use crate::partitioning::BvhNode; -use crate::query::QueryDispatcher; +use crate::query::{QueryDispatcher, ShapeDistance}; use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; use crate::utils::PoseOpt; @@ -9,35 +9,43 @@ impl CompositeShapeRef<'_, S> { /// Calculates the closest distance between `self` and the given `shape2` positioned at /// `pose12` relative to `self`. /// - /// Returns the distance and the index of the sub-shape of `self` that is closest to `shape2`. + /// The result's `subshape1` says which sub-shape of `self` is closest to `shape2`. pub fn distance_to_shape( &self, dispatcher: &D, pose12: &Pose, shape2: &dyn Shape, - ) -> Option<(u32, Real)> { + ) -> Option { let ls_aabb2 = shape2.compute_aabb(pose12); let msum_shift = -ls_aabb2.center(); let msum_margin = ls_aabb2.half_extents(); - self.0.bvh().find_best( - Real::MAX, - |node: &BvhNode, _| { - // Compute the minkowski sum of the two Aabbs. - let msum = Aabb { - mins: node.mins() + msum_shift - msum_margin, - maxs: node.maxs() + msum_shift + msum_margin, - }; - msum.distance_to_origin() - }, - |part_id, _| { - self.0 - .map_untyped_part_at(part_id, |part_pos1, part_g1, _| { - dispatcher.distance(&part_pos1.inv_mul(pose12), part_g1, shape2) - })? - .ok() - }, - ) + self.0 + .bvh() + .find_best( + Real::MAX, + |node: &BvhNode, _| { + // Compute the minkowski sum of the two Aabbs. + let msum = Aabb { + mins: node.mins() + msum_shift - msum_margin, + maxs: node.maxs() + msum_shift + msum_margin, + }; + msum.distance_to_origin() + }, + |part_id, _| { + self.0 + .map_untyped_part_at(part_id, |part_pos1, part_g1, _| { + dispatcher.distance(&part_pos1.inv_mul(pose12), part_g1, shape2) + })? + .ok() + }, + ) + // `subshape2` is left as the dispatch set it: `shape2` may be a composite too, and only it + // knows which of its parts answered. + .map(|(part_id, mut result)| { + result.subshape1 = part_id; + result + }) } } @@ -47,15 +55,14 @@ pub fn distance_composite_shape_shape( pos12: &Pose, g1: &G1, g2: &dyn Shape, -) -> Real +) -> ShapeDistance where D: ?Sized + QueryDispatcher, G1: ?Sized + TypedCompositeShape, { CompositeShapeRef(g1) .distance_to_shape(dispatcher, pos12, g2) - .unwrap_or((u32::MAX, Real::MAX)) - .1 + .unwrap_or(ShapeDistance::new(Real::MAX)) } /// Smallest distance between a shape and a composite shape. @@ -64,10 +71,10 @@ pub fn distance_shape_composite_shape( pos12: &Pose, g1: &dyn Shape, g2: &G2, -) -> Real +) -> ShapeDistance where D: ?Sized + QueryDispatcher, G2: ?Sized + TypedCompositeShape, { - distance_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1) + distance_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1).swapped() } diff --git a/src/query/distance/mod.rs b/src/query/distance/mod.rs index a3f85967..94248180 100644 --- a/src/query/distance/mod.rs +++ b/src/query/distance/mod.rs @@ -1,6 +1,6 @@ //! Implementation details of the `distance` function. -pub use self::distance::distance; +pub use self::distance::{distance, ShapeDistance}; pub use self::distance_ball_ball::distance_ball_ball; pub use self::distance_ball_convex_polyhedron::{ distance_ball_convex_polyhedron, distance_convex_polyhedron_ball, diff --git a/src/query/intersection_test/intersection_test.rs b/src/query/intersection_test/intersection_test.rs index d714044b..6443f92b 100644 --- a/src/query/intersection_test/intersection_test.rs +++ b/src/query/intersection_test/intersection_test.rs @@ -1,3 +1,50 @@ +use crate::shape::SubShapeId; + +/// Whether two shapes intersect, and the sub-shapes that do. +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub struct ShapeIntersection { + /// Whether the two shapes intersect. + pub intersecting: bool, + /// The sub-shape of the first shape that intersects. + /// + /// Always `0` for a shape with no sub-shapes, and meaningless unless `intersecting` is `true`. + pub subshape1: SubShapeId, + /// The sub-shape of the second shape that intersects. + /// + /// Always `0` for a shape with no sub-shapes, and meaningless unless `intersecting` is `true`. + pub subshape2: SubShapeId, +} + +impl ShapeIntersection { + /// A result between shapes with no sub-shapes to distinguish. + pub fn new(intersecting: bool) -> Self { + Self { + intersecting, + subshape1: 0, + subshape2: 0, + } + } + + /// Sets the sub-shapes that intersect. + pub fn with_subshapes(mut self, subshape1: SubShapeId, subshape2: SubShapeId) -> Self { + self.subshape1 = subshape1; + self.subshape2 = subshape2; + self + } + + /// Swaps the roles of the two shapes. + pub fn swapped(mut self) -> Self { + core::mem::swap(&mut self.subshape1, &mut self.subshape2); + self + } +} + +impl From for ShapeIntersection { + fn from(intersecting: bool) -> Self { + Self::new(intersecting) + } +} + use crate::math::Pose; use crate::query::{DefaultQueryDispatcher, QueryDispatcher, Unsupported}; use crate::shape::Shape; @@ -56,12 +103,12 @@ use crate::shape::Shape; /// let pos1 = Pose::translation(0.0, 0.0, 0.0); /// let pos2 = Pose::translation(1.5, 0.0, 0.0); /// -/// let intersecting = intersection_test(&pos1, &ball1, &pos2, &ball2).unwrap(); +/// let intersecting = intersection_test(&pos1, &ball1, &pos2, &ball2).unwrap().intersecting; /// assert!(intersecting); // Distance 1.5 < combined radii 2.0 /// /// // Separated balls /// let pos3 = Pose::translation(5.0, 0.0, 0.0); -/// let not_intersecting = intersection_test(&pos1, &ball1, &pos3, &ball2).unwrap(); +/// let not_intersecting = intersection_test(&pos1, &ball1, &pos3, &ball2).unwrap().intersecting; /// assert!(!not_intersecting); // Distance 5.0 > combined radii 2.0 /// # } /// ``` @@ -90,7 +137,7 @@ pub fn intersection_test( g1: &dyn Shape, pos2: &Pose, g2: &dyn Shape, -) -> Result { +) -> Result { let pos12 = pos1.inv_mul(pos2); DefaultQueryDispatcher.intersection_test(&pos12, g1, g2) } diff --git a/src/query/intersection_test/intersection_test_ball_point_query.rs b/src/query/intersection_test/intersection_test_ball_point_query.rs index 7dc35bed..4693ea71 100644 --- a/src/query/intersection_test/intersection_test_ball_point_query.rs +++ b/src/query/intersection_test/intersection_test_ball_point_query.rs @@ -1,5 +1,5 @@ use crate::math::Pose; -use crate::query::PointQuery; +use crate::query::{PointQuery, ShapeIntersection}; use crate::shape::Ball; /// Intersection test between a ball and a shape implementing the `PointQuery` trait. @@ -7,8 +7,8 @@ pub fn intersection_test_ball_point_query( pos12: &Pose, ball1: &Ball, point_query2: &P, -) -> bool { - intersection_test_point_query_ball(&pos12.inverse(), point_query2, ball1) +) -> ShapeIntersection { + intersection_test_point_query_ball(&pos12.inverse(), point_query2, ball1).swapped() } /// Intersection test between a shape implementing the `PointQuery` trait and a ball. @@ -16,8 +16,11 @@ pub fn intersection_test_point_query_ball( pos12: &Pose, point_query1: &P, ball2: &Ball, -) -> bool { +) -> ShapeIntersection { let local_p2_1 = pos12.translation; let proj = point_query1.project_local_point(local_p2_1, true); - proj.is_inside || (local_p2_1 - proj.point).length_squared() <= ball2.radius * ball2.radius + let intersecting = + proj.is_inside || (local_p2_1 - proj.point).length_squared() <= ball2.radius * ball2.radius; + // The projection knows which sub-shape of `point_query1` answered. + ShapeIntersection::new(intersecting).with_subshapes(proj.subshape, 0) } 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 266a5a4a..a1fe9dac 100644 --- a/src/query/intersection_test/intersection_test_composite_shape_shape.rs +++ b/src/query/intersection_test/intersection_test_composite_shape_shape.rs @@ -1,33 +1,45 @@ use crate::bounding_volume::BoundingVolume; use crate::math::Pose; use crate::partitioning::BvhNode; -use crate::query::QueryDispatcher; +use crate::query::{QueryDispatcher, ShapeIntersection}; use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; use crate::utils::PoseOpt; impl CompositeShapeRef<'_, S> { - /// Returns the index of the shape in `self` that intersects the given other `shape` positioned - /// at `pose12` relative to `self`. + /// Tests whether the given other `shape`, positioned at `pose12` relative to `self`, + /// intersects `self`. /// - /// Returns `None` if no intersection is found. + /// The result's `subshape1` says which sub-shape of `self` it intersects. pub fn intersects_shape( &self, dispatcher: &D, pose12: &Pose, shape: &dyn Shape, - ) -> Option { + ) -> ShapeIntersection { let ls_aabb2 = shape.compute_aabb(pose12); - self.0 + let found = self + .0 .bvh() .leaves(|node: &BvhNode| node.aabb().intersects(&ls_aabb2)) - .find(|leaf_id| { + .find_map(|leaf_id| { self.0 - .map_untyped_part_at(*leaf_id, |part_pose1, sub1, _| { - dispatcher.intersection_test(&part_pose1.inv_mul(pose12), sub1, shape) - == Ok(true) + .map_untyped_part_at(leaf_id, |part_pose1, sub1, _| { + // `shape` may be a composite too; keep the sub-shape it reported. + dispatcher + .intersection_test(&part_pose1.inv_mul(pose12), sub1, shape) + .ok() + .filter(|result| result.intersecting) + .map(|result| (leaf_id, result.subshape2)) }) - .unwrap_or(false) - }) + .flatten() + }); + + match found { + Some((subshape1, subshape2)) => { + ShapeIntersection::new(true).with_subshapes(subshape1, subshape2) + } + None => ShapeIntersection::new(false), + } } } @@ -37,14 +49,12 @@ pub fn intersection_test_composite_shape_shape( pos12: &Pose, g1: &G1, g2: &dyn Shape, -) -> bool +) -> ShapeIntersection where D: ?Sized + QueryDispatcher, G1: ?Sized + TypedCompositeShape, { - CompositeShapeRef(g1) - .intersects_shape(dispatcher, pos12, g2) - .is_some() + CompositeShapeRef(g1).intersects_shape(dispatcher, pos12, g2) } /// Proximity between a shape and a composite (`Mesh`, `Compound`) shape. @@ -53,10 +63,10 @@ pub fn intersection_test_shape_composite_shape( pos12: &Pose, g1: &dyn Shape, g2: &G2, -) -> bool +) -> ShapeIntersection where D: ?Sized + QueryDispatcher, G2: ?Sized + TypedCompositeShape, { - intersection_test_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1) + intersection_test_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1).swapped() } diff --git a/src/query/intersection_test/intersection_test_voxels_shape.rs b/src/query/intersection_test/intersection_test_voxels_shape.rs index 5e36affc..c77eb06e 100644 --- a/src/query/intersection_test/intersection_test_voxels_shape.rs +++ b/src/query/intersection_test/intersection_test_voxels_shape.rs @@ -1,5 +1,5 @@ use crate::math::Pose; -use crate::query::PersistentQueryDispatcher; +use crate::query::{PersistentQueryDispatcher, ShapeIntersection}; use crate::shape::{Cuboid, Shape, VoxelType, Voxels}; /// Checks for any intersection between voxels and an arbitrary shape, both represented as a `Shape` trait-object. @@ -8,13 +8,13 @@ pub fn intersection_test_voxels_shape_shapes( pos12: &Pose, shape1: &dyn Shape, shape2: &dyn Shape, -) -> bool { +) -> ShapeIntersection { if let Some(voxels1) = shape1.as_voxels() { intersection_test_voxels_shape(dispatcher, pos12, voxels1, shape2) } else if let Some(voxels2) = shape2.as_voxels() { - intersection_test_voxels_shape(dispatcher, &pos12.inverse(), voxels2, shape1) + intersection_test_voxels_shape(dispatcher, &pos12.inverse(), voxels2, shape1).swapped() } else { - false + ShapeIntersection::new(false) } } @@ -24,7 +24,7 @@ pub fn intersection_test_voxels_shape( pos12: &Pose, voxels1: &Voxels, shape2: &dyn Shape, -) -> bool { +) -> ShapeIntersection { let radius1 = voxels1.voxel_size() / 2.0; let aabb1 = voxels1.local_aabb(); let aabb2_1 = shape2.compute_aabb(pos12); @@ -41,16 +41,17 @@ pub fn intersection_test_voxels_shape( let cuboid1 = Cuboid::new(radius1); let cuboid_pose12 = Pose::from_translation(-center1) * pos12; - if dispatcher - .intersection_test(&cuboid_pose12, &cuboid1, shape2) - .unwrap_or(false) - { - return true; + if let Ok(result) = dispatcher.intersection_test(&cuboid_pose12, &cuboid1, shape2) { + if result.intersecting { + // `shape2` may be a composite; keep the sub-shape it reported. + return ShapeIntersection::new(true) + .with_subshapes(vox1.linear_id.flat_id() as u32, result.subshape2); + } } } } - false + ShapeIntersection::new(false) } /// Checks for any intersection between voxels and an arbitrary shape. @@ -59,6 +60,6 @@ pub fn intersection_test_shape_voxels( pos12: &Pose, shape1: &dyn Shape, voxels2: &Voxels, -) -> bool { - intersection_test_voxels_shape(dispatcher, &pos12.inverse(), voxels2, shape1) +) -> ShapeIntersection { + intersection_test_voxels_shape(dispatcher, &pos12.inverse(), voxels2, shape1).swapped() } diff --git a/src/query/intersection_test/mod.rs b/src/query/intersection_test/mod.rs index eb9d12ea..3499329a 100644 --- a/src/query/intersection_test/mod.rs +++ b/src/query/intersection_test/mod.rs @@ -1,6 +1,6 @@ //! Implementation details of the `intersection_test` function. -pub use self::intersection_test::intersection_test; +pub use self::intersection_test::{intersection_test, ShapeIntersection}; pub use self::intersection_test_ball_ball::intersection_test_ball_ball; pub use self::intersection_test_ball_point_query::{ intersection_test_ball_point_query, intersection_test_point_query_ball, diff --git a/src/query/mod.rs b/src/query/mod.rs index fed9909f..48bd33f8 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -32,9 +32,9 @@ pub use self::contact_manifolds::{ ContactManifold, ContactManifoldsWorkspace, TrackedContact, TypedWorkspaceData, WorkspaceData, }; pub use self::default_query_dispatcher::DefaultQueryDispatcher; -pub use self::distance::distance; +pub use self::distance::{distance, ShapeDistance}; pub use self::error::Unsupported; -pub use self::intersection_test::intersection_test; +pub use self::intersection_test::{intersection_test, ShapeIntersection}; pub use self::nonlinear_shape_cast::{cast_shapes_nonlinear, NonlinearRigidMotion}; pub use self::point::{PointProjection, PointQuery, PointQueryWithLocation}; #[cfg(feature = "alloc")] diff --git a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs index 402f1571..a553becb 100644 --- a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs @@ -20,67 +20,75 @@ impl CompositeShapeRef<'_, S> { start_time: Real, end_time: Real, stop_at_penetration: bool, - ) -> Option<(u32, ShapeCastHit)> { + ) -> Option { let sphere2 = shape2.compute_local_bounding_sphere(); - self.0.bvh().find_best( - end_time, - |node: &BvhNode, _| { - let aabb1 = node.aabb(); - let center1 = aabb1.center(); - let radius1 = aabb1.half_extents().length(); - let ball1 = Ball::new(radius1); - let ball2 = Ball::new(sphere2.radius()); - let ball_motion1 = motion1.prepend_translation(center1); - let ball_motion2 = motion2.prepend_translation(sphere2.center); + self.0 + .bvh() + .find_best( + end_time, + |node: &BvhNode, _| { + let aabb1 = node.aabb(); + let center1 = aabb1.center(); + let radius1 = aabb1.half_extents().length(); + let ball1 = Ball::new(radius1); + let ball2 = Ball::new(sphere2.radius()); + let ball_motion1 = motion1.prepend_translation(center1); + let ball_motion2 = motion2.prepend_translation(sphere2.center); - query::details::cast_shapes_nonlinear_support_map_support_map( - dispatcher, - &ball_motion1, - &ball1, - &ball1, - &ball_motion2, - &ball2, - &ball2, - start_time, - end_time, - NonlinearShapeCastMode::StopAtPenetration, - ) - .map(|hit| hit.time_of_impact) - .unwrap_or(Real::MAX) - }, - |part_id, _| { - self.0 - .map_untyped_part_at(part_id, |part_pos1, part_shape1, _| { - if let Some(part_pos1) = part_pos1 { - dispatcher - .cast_shapes_nonlinear( - &motion1.prepend(*part_pos1), - part_shape1, - motion2, - shape2, - start_time, - end_time, - stop_at_penetration, - ) - .ok()? - .map(|hit| hit.transform1_by(part_pos1)) - } else { - dispatcher - .cast_shapes_nonlinear( - motion1, - part_shape1, - motion2, - shape2, - start_time, - end_time, - stop_at_penetration, - ) - .ok()? - } - })? - }, - ) + query::details::cast_shapes_nonlinear_support_map_support_map( + dispatcher, + &ball_motion1, + &ball1, + &ball1, + &ball_motion2, + &ball2, + &ball2, + start_time, + end_time, + NonlinearShapeCastMode::StopAtPenetration, + ) + .map(|hit| hit.time_of_impact) + .unwrap_or(Real::MAX) + }, + |part_id, _| { + self.0 + .map_untyped_part_at(part_id, |part_pos1, part_shape1, _| { + if let Some(part_pos1) = part_pos1 { + dispatcher + .cast_shapes_nonlinear( + &motion1.prepend(*part_pos1), + part_shape1, + motion2, + shape2, + start_time, + end_time, + stop_at_penetration, + ) + .ok()? + .map(|hit| hit.transform1_by(part_pos1)) + } else { + dispatcher + .cast_shapes_nonlinear( + motion1, + part_shape1, + motion2, + shape2, + start_time, + end_time, + stop_at_penetration, + ) + .ok()? + } + })? + }, + ) + // `subshape2` is left as the dispatch set it: `shape2` may be a composite too, and only it + // knows which of its parts answered. + .map(|(part_id, mut hit)| { + hit.subshape1 = part_id; + hit + }) } } @@ -99,17 +107,15 @@ where D: ?Sized + QueryDispatcher, G1: ?Sized + TypedCompositeShape, { - CompositeShapeRef(shape1) - .cast_shape_nonlinear( - dispatcher, - motion1, - motion2, - shape2, - start_time, - end_time, - stop_at_penetration, - ) - .map(|hit| hit.1) + CompositeShapeRef(shape1).cast_shape_nonlinear( + dispatcher, + motion1, + motion2, + shape2, + start_time, + end_time, + stop_at_penetration, + ) } /// Time Of Impact of any shape with a composite shape, under a rigid motion (translation + rotation). diff --git a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs index 83b1e19e..36f40254 100644 --- a/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs @@ -115,6 +115,8 @@ where witness1: Vector::ZERO, witness2: Vector::ZERO, status: ShapeCastStatus::PenetratingOrWithinTargetDist, + subshape1: 0, + subshape2: 0, }; loop { @@ -363,6 +365,8 @@ where normal1: contact.normal1, normal2: contact.normal2, status: ShapeCastStatus::Converged, + subshape1: 0, + subshape2: 0, }; if contact.dist > 0.0 { diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index d2d0642e..cf42cd0e 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -4,17 +4,30 @@ use crate::math::{Real, Vector}; use crate::partitioning::BvhNode; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{ - CompositeShapeRef, FeatureId, SegmentPointLocation, TriMesh, TrianglePointLocation, + CompositeShapeRef, FeatureId, SegmentPointLocation, SubShapeId, TriMesh, TrianglePointLocation, TypedCompositeShape, }; use crate::shape::{Compound, Polyline}; +/// The feature of a triangle a point projected onto, as `Triangle`'s own point query reports it. +fn triangle_point_location_feature(location: TrianglePointLocation) -> FeatureId { + match location { + TrianglePointLocation::OnVertex(i) => FeatureId::Vertex(i), + #[cfg(feature = "dim3")] + TrianglePointLocation::OnEdge(i, _) => FeatureId::Edge(i), + #[cfg(feature = "dim2")] + TrianglePointLocation::OnEdge(i, _) => FeatureId::Face(i), + TrianglePointLocation::OnFace(i, _) => FeatureId::Face(i), + TrianglePointLocation::OnSolid => FeatureId::Face(0), + } +} + impl CompositeShapeRef<'_, S> { /// Project a point on this composite shape. /// - /// Returns the projected point as well as the index of the sub-shape of `self` that was hit. - /// The third tuple element contains some shape-specific information about the projected point. + /// The projection's `subshape` says which sub-shape of `self` answered. The second tuple + /// element contains some shape-specific information about the projected point. #[inline] pub fn project_local_point_and_get_location( &self, @@ -22,11 +35,8 @@ impl CompositeShapeRef<'_, S> { max_dist: Real, solid: bool, ) -> Option<( - u32, - ( - PointProjection, - ::Location, - ), + PointProjection, + ::Location, )> where S::PartShape: PointQueryWithLocation, @@ -48,20 +58,20 @@ impl CompositeShapeRef<'_, S> { Some((cost, proj)) }, ) - .map(|(best_id, (_, (proj, location)))| (best_id, (proj, location))) + .map(|(best_id, (_, (proj, location)))| (proj.with_subshape(best_id), location)) } /// Project a point on this composite shape. /// - /// Returns the projected point as well as the index of the sub-shape of `self` that was hit. - /// If `solid` is `false` then the point will be projected to the closest boundary of `self` even - /// if it is contained by one of its sub-shapes. + /// The projection's `subshape` says which sub-shape of `self` answered. If `solid` is `false` + /// then the point will be projected to the closest boundary of `self` even if it is contained + /// by one of its sub-shapes. pub fn project_local_point( &self, point: Vector, max_dist: Real, solid: bool, - ) -> Option<(u32, PointProjection)> { + ) -> Option { let (best_id, (_, proj)) = self.0.bvh().find_best( max_dist, |node: &BvhNode, _best_so_far| node.aabb().distance_to_local_point(point, true), @@ -77,20 +87,19 @@ impl CompositeShapeRef<'_, S> { Some((dist, proj)) }, )?; - Some((best_id, proj)) + Some(proj.with_subshape(best_id)) } /// Project a point on this composite shape. /// - /// Returns the projected point as well as the index of the sub-shape of `self` that was hit. - /// The third tuple element contains some shape-specific information about the shape feature - /// hit by the projection. + /// The projection's `subshape` says which sub-shape of `self` answered. The second tuple + /// element is the feature of that sub-shape the projection landed on. #[inline] pub fn project_local_point_and_get_feature( &self, point: Vector, max_dist: Real, - ) -> Option<(u32, (PointProjection, FeatureId))> { + ) -> Option<(PointProjection, FeatureId)> { let (best_id, (_, (proj, feature_id))) = self.0.bvh().find_best( max_dist, |node: &BvhNode, _best_so_far| node.aabb().distance_to_local_point(point, true), @@ -106,14 +115,14 @@ impl CompositeShapeRef<'_, S> { Some((cost, proj)) }, )?; - Some((best_id, (proj, feature_id))) + Some((proj.with_subshape(best_id), feature_id)) } // TODO: implement distance_to_point too? /// Returns the index of any sub-shape of `self` that contains the given point. #[inline] - pub fn contains_local_point(&self, point: Vector) -> Option { + pub fn contains_local_point(&self, point: Vector) -> Option { self.0 .bvh() .leaves(|node: &BvhNode| node.aabb().contains_local_point(point)) @@ -143,7 +152,7 @@ impl PointQuery for Polyline { // Every comparison involving a NaN is false, so the traversal finds no candidate // at all when `point` (or `self`) isn’t finite. Report `point` itself rather than // an arbitrary projection onto whichever part we happened to pick. - let Some((seg_id, (mut proj, feature))) = + let Some((mut proj, feature)) = CompositeShapeRef(self).project_local_point_and_get_feature(point, Real::MAX) else { return (PointProjection::new(false, point), FeatureId::Unknown); @@ -151,7 +160,7 @@ impl PointQuery for Polyline { // A point behind the outward pseudo-normal is inside. #[cfg(feature = "dim2")] - if let Some(constraints) = self.segment_normal_constraints(seg_id) { + if let Some(constraints) = self.segment_normal_constraints(proj.subshape) { let pseudo_normal = match feature { FeatureId::Vertex(i) => constraints.edges[i as usize], _ => constraints.face, @@ -159,8 +168,8 @@ impl PointQuery for Polyline { proj.is_inside = (point - proj.point).dot(pseudo_normal) <= 0.0; } - let polyline_feature = self.segment_feature_to_polyline_feature(seg_id, feature); - (proj, polyline_feature) + // The feature is the segment's own; `proj.subshape` says which segment it belongs to. + (proj, feature) } // TODO: implement distance_to_point too? @@ -185,33 +194,32 @@ impl PointQuery for Polyline { impl PointQuery for TriMesh { #[inline] fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection { - CompositeShapeRef(self) - .project_local_point(point, Real::MAX, solid) - .map(|(_, proj)| proj) - // No candidate: `point` (or `self`) isn’t finite. See - // `Polyline::project_local_point_and_get_feature`. - .unwrap_or(PointProjection::new(false, point)) + self.project_local_point_with_max_dist(point, solid, Real::MAX) + // Shouldn’t happen (trimesh must not be empty). But return something + // instead of crashing with `unwrap`. + .unwrap_or((PointProjection::new(false, point))) } #[inline] fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { #[cfg(feature = "dim3")] if self.pseudo_normals().is_some() { - // If we can, in 3D, take the pseudo-normals into account. - let (proj, (id, _feature)) = self.project_local_point_and_get_location(point, false); - let feature_id = FeatureId::Face(id); - return (proj, feature_id); + // If we can, in 3D, take the pseudo-normals into account. The location carries the + // triangle's own feature; `proj.subshape` says which triangle it belongs to. + let (proj, (_, location)) = self.project_local_point_and_get_location(point, false); + return (proj, triangle_point_location_feature(location)); } let solid = cfg!(feature = "dim2"); // No candidate: `point` (or `self`) isn’t finite. See // `Polyline::project_local_point_and_get_feature`. - let Some((tri_id, proj)) = - CompositeShapeRef(self).project_local_point(point, Real::MAX, solid) + let Some((proj, location)) = + CompositeShapeRef(self).project_local_point_and_get_location(point, Real::MAX, solid) else { return (PointProjection::new(false, point), FeatureId::Unknown); }; - (proj, FeatureId::Face(tri_id)) + // The feature is the triangle's own; `proj.subshape` says which triangle it belongs to. + (proj, triangle_point_location_feature(location)) } // TODO: implement distance_to_point too? @@ -249,7 +257,6 @@ impl PointQuery for Compound { fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection { CompositeShapeRef(self) .project_local_point(point, Real::MAX, solid) - .map(|(_, proj)| proj) // No candidate: `point` (or `self`) isn’t finite. See // `Polyline::project_local_point_and_get_feature`. .unwrap_or(PointProjection::new(false, point)) @@ -257,15 +264,12 @@ impl PointQuery for Compound { #[inline] fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { - ( - CompositeShapeRef(self) - .project_local_point_and_get_feature(point, Real::MAX) - .map(|(_, (proj, _))| proj) - // No candidate: `point` (or `self`) isn’t finite. See - // `Polyline::project_local_point_and_get_feature`. - .unwrap_or(PointProjection::new(false, point)), - FeatureId::Unknown, - ) + // The feature is the part's own; `proj.subshape` says which part it belongs to. + CompositeShapeRef(self) + .project_local_point_and_get_feature(point, Real::MAX) + // No candidate: `point` (or `self`) isn’t finite. See + // `Polyline::project_local_point_and_get_feature`. + .unwrap_or((PointProjection::new(false, point), FeatureId::Unknown)) } #[inline] @@ -302,9 +306,11 @@ impl PointQueryWithLocation for Polyline { max_dist: Real, ) -> Option<(PointProjection, Self::Location)> { #[allow(unused_mut)] // Because we need mut in 2D but not in 3D. - if let Some((seg_id, (mut proj, loc))) = + if let Some((mut proj, loc)) = CompositeShapeRef(self).project_local_point_and_get_location(point, max_dist, solid) { + let seg_id = proj.subshape; + // A point behind the outward pseudo-normal is inside. #[cfg(feature = "dim2")] if let Some(constraints) = self.segment_normal_constraints(seg_id) { @@ -353,9 +359,11 @@ impl PointQueryWithLocation for TriMesh { max_dist: Real, ) -> Option<(PointProjection, Self::Location)> { #[allow(unused_mut)] // mut is needed in 3D. - if let Some((part_id, (mut proj, location))) = + if let Some((mut proj, location)) = CompositeShapeRef(self).project_local_point_and_get_location(point, max_dist, solid) { + let part_id = proj.subshape; + #[cfg(feature = "dim3")] if let Some(pseudo_normals) = self.pseudo_normals_if_oriented() { let pseudo_normal = match location { diff --git a/src/query/point/point_query.rs b/src/query/point/point_query.rs index b2c22bad..7b66d3e7 100644 --- a/src/query/point/point_query.rs +++ b/src/query/point/point_query.rs @@ -1,5 +1,5 @@ use crate::math::{Pose, Real, Vector}; -use crate::shape::FeatureId; +use crate::shape::{FeatureId, SubShapeId}; /// The result of projecting a point onto a shape. /// @@ -69,12 +69,30 @@ pub struct PointProjection { /// - `true`: Vector is in the interior (for solid shapes) /// - `false`: Vector is outside the shape pub is_inside: bool, + + /// The sub-shape the point projects onto. + /// + /// Identifies the part of a composite shape that answered: a triangle of a + /// [`TriMesh`](crate::shape::TriMesh), a part of a [`Compound`](crate::shape::Compound), a + /// voxel of a [`Voxels`](crate::shape::Voxels), and so on. Always `0` for a shape with no + /// sub-shapes. + pub subshape: SubShapeId, } impl PointProjection { /// Initializes a new `PointProjection`. pub fn new(is_inside: bool, point: Vector) -> Self { - PointProjection { is_inside, point } + PointProjection { + is_inside, + point, + subshape: 0, + } + } + + /// Sets the sub-shape this projection came from. + pub fn with_subshape(mut self, subshape: SubShapeId) -> Self { + self.subshape = subshape; + self } /// Transforms `self.point` by `pos`. @@ -82,6 +100,7 @@ impl PointProjection { PointProjection { is_inside: self.is_inside, point: pos * self.point, + subshape: self.subshape, } } diff --git a/src/query/point/point_voxels.rs b/src/query/point/point_voxels.rs index 87a5b147..45bf6076 100644 --- a/src/query/point/point_voxels.rs +++ b/src/query/point/point_voxels.rs @@ -10,7 +10,7 @@ impl PointQuery for Voxels { let chunk = self.chunk_ref(chunk_id); chunk .project_local_point_and_get_vox_id(pt, solid) - .map(|(proj, _)| proj) + .map(|(proj, vox)| proj.with_subshape(vox)) }) .map(|res| res.1 .1) .unwrap_or(PointProjection::new(false, Vector::splat(Real::MAX))) @@ -21,10 +21,10 @@ impl PointQuery for Voxels { self.chunk_bvh() .project_point_and_get_feature(pt, Real::MAX, |chunk_id, _| { let chunk = self.chunk_ref(chunk_id); - // TODO: we need a way to return both the voxel id, and the feature on the voxel. + // TODO: report the feature on the voxel; `subshape` already identifies the voxel. chunk .project_local_point_and_get_vox_id(pt, false) - .map(|(proj, vox)| (proj, FeatureId::Face(vox))) + .map(|(proj, vox)| (proj.with_subshape(vox), FeatureId::Unknown)) }) .map(|res| res.1 .1) .unwrap_or(( diff --git a/src/query/query_dispatcher.rs b/src/query/query_dispatcher.rs index 630c2e8b..2c3515a8 100644 --- a/src/query/query_dispatcher.rs +++ b/src/query/query_dispatcher.rs @@ -133,7 +133,7 @@ //! pos12: &Pose, //! g1: &dyn Shape, //! g2: &dyn Shape, -//! ) -> Result { +//! ) -> Result { //! // Try to downcast to your custom shape types //! if let (Some(my_shape1), Some(my_shape2)) = ( //! g1.as_any().downcast_ref::(), @@ -152,7 +152,7 @@ //! pos12: &Pose, //! g1: &dyn Shape, //! g2: &dyn Shape, -//! ) -> Result { +//! ) -> Result { //! // Implement other query methods similarly //! Err(Unsupported) //! } @@ -195,7 +195,10 @@ use crate::query::{ contact_manifolds::{ContactManifoldsWorkspace, NormalConstraints}, ContactManifold, }; -use crate::query::{ClosestPoints, Contact, NonlinearRigidMotion, ShapeCastHit, Unsupported}; +use crate::query::{ + ClosestPoints, Contact, NonlinearRigidMotion, ShapeCastHit, ShapeDistance, ShapeIntersection, + Unsupported, +}; use crate::shape::Shape; #[cfg(feature = "alloc")] use alloc::vec::Vec; @@ -355,10 +358,10 @@ pub trait PersistentQueryDispatcher: QueryD /// let pos12 = pos1.inv_mul(&pos2); /// /// // Test intersection -/// let intersects = dispatcher.intersection_test(&pos12, &ball, &cuboid).unwrap(); +/// let intersects = dispatcher.intersection_test(&pos12, &ball, &cuboid).unwrap().intersecting; /// /// // Compute distance -/// let dist = dispatcher.distance(&pos12, &ball, &cuboid).unwrap(); +/// let dist = dispatcher.distance(&pos12, &ball, &cuboid).unwrap().distance; /// /// println!("Intersects: {}, Distance: {}", intersects, dist); /// # } @@ -378,7 +381,7 @@ pub trait PersistentQueryDispatcher: QueryD /// pos12: &Pose, /// g1: &dyn Shape, /// g2: &dyn Shape, -/// ) -> Result { +/// ) -> Result { /// // Handle custom shape types /// if let Some(my_shape) = g1.as_any().downcast_ref::() { /// // ... custom distance computation @@ -412,12 +415,17 @@ pub trait QueryDispatcher: Send + Sync { pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, - ) -> Result; + ) -> Result; /// Computes the minimum distance separating two shapes. /// /// Returns `0.0` if the objects are touching or penetrating. - fn distance(&self, pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape) -> Result; + fn distance( + &self, + pos12: &Pose, + g1: &dyn Shape, + g2: &dyn Shape, + ) -> Result; /// Computes one pair of contact points point between two shapes. /// @@ -537,7 +545,7 @@ pub trait QueryDispatcher: Send + Sync { /// pos12: &Pose, /// g1: &dyn Shape, /// g2: &dyn Shape, -/// ) -> Result { +/// ) -> Result { /// // Try to handle custom shapes /// match (g1.as_any().downcast_ref::(), /// g2.as_any().downcast_ref::()) { @@ -595,9 +603,9 @@ where pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape, - ) -> bool); + ) -> ShapeIntersection); - chain_method!(distance(pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape,) -> Real); + chain_method!(distance(pos12: &Pose, g1: &dyn Shape, g2: &dyn Shape,) -> ShapeDistance); chain_method!(contact( pos12: &Pose, diff --git a/src/query/ray/ray.rs b/src/query/ray/ray.rs index 59fff7ab..017f6e4c 100644 --- a/src/query/ray/ray.rs +++ b/src/query/ray/ray.rs @@ -1,7 +1,7 @@ //! Traits and structure needed to cast rays. use crate::math::{Pose, Real, Vector}; -use crate::shape::FeatureId; +use crate::shape::{FeatureId, SubShapeId}; #[cfg(feature = "alloc")] use crate::partitioning::BvhLeafCost; @@ -308,6 +308,14 @@ pub struct RayIntersection { /// This can be used for more detailed collision response or to identify /// exactly which part of the shape was struck. pub feature: FeatureId, + + /// The sub-shape that was hit. + /// + /// Identifies the part of a composite shape the ray struck: a triangle of a + /// [`TriMesh`](crate::shape::TriMesh), a part of a [`Compound`](crate::shape::Compound), a + /// voxel of a [`Voxels`](crate::shape::Voxels), and so on. Always `0` for a shape with no + /// sub-shapes. + pub subshape: SubShapeId, } impl RayIntersection { @@ -319,6 +327,7 @@ impl RayIntersection { time_of_impact, normal, feature, + subshape: 0, } } @@ -330,15 +339,24 @@ impl RayIntersection { time_of_impact, normal, feature, + subshape: 0, } } + /// Sets the sub-shape this intersection came from. + #[inline] + pub fn with_subshape(mut self, subshape: SubShapeId) -> Self { + self.subshape = subshape; + self + } + #[inline] pub fn transform_by(&self, transform: &Pose) -> Self { RayIntersection { time_of_impact: self.time_of_impact, normal: transform.rotation * self.normal, feature: self.feature, + subshape: self.subshape, } } } diff --git a/src/query/ray/ray_composite_shape.rs b/src/query/ray/ray_composite_shape.rs index 82018592..846ee15a 100644 --- a/src/query/ray/ray_composite_shape.rs +++ b/src/query/ray/ray_composite_shape.rs @@ -1,7 +1,7 @@ use crate::math::Real; use crate::partitioning::BvhNode; use crate::query::{Ray, RayCast, RayIntersection}; -use crate::shape::{CompositeShapeRef, Compound, Polyline, TypedCompositeShape}; +use crate::shape::{CompositeShapeRef, Compound, Polyline, SubShapeId, TypedCompositeShape}; impl CompositeShapeRef<'_, S> { /// Casts a ray on this composite shape. @@ -22,7 +22,7 @@ impl CompositeShapeRef<'_, S> { ray: &Ray, max_time_of_impact: Real, solid: bool, - ) -> Option<(u32, Real)> { + ) -> Option<(SubShapeId, Real)> { let hit = self .0 .bvh() @@ -45,20 +45,23 @@ impl CompositeShapeRef<'_, S> { ray: &Ray, max_time_of_impact: Real, solid: bool, - ) -> Option<(u32, RayIntersection)> { - self.0.bvh().find_best( - max_time_of_impact, - |node: &BvhNode, best_so_far| node.cast_ray(ray, best_so_far), - |primitive, best_so_far| { - self.0.map_typed_part_at(primitive, |pose, part, _| { - if let Some(pose) = pose { - part.cast_ray_and_get_normal(pose, ray, best_so_far, solid) - } else { - part.cast_local_ray_and_get_normal(ray, best_so_far, solid) - } - })? - }, - ) + ) -> Option { + self.0 + .bvh() + .find_best( + max_time_of_impact, + |node: &BvhNode, best_so_far| node.cast_ray(ray, best_so_far), + |primitive, best_so_far| { + self.0.map_typed_part_at(primitive, |pose, part, _| { + if let Some(pose) = pose { + part.cast_ray_and_get_normal(pose, ray, best_so_far, solid) + } else { + part.cast_local_ray_and_get_normal(ray, best_so_far, solid) + } + })? + }, + ) + .map(|(best_id, hit)| hit.with_subshape(best_id)) } } @@ -77,9 +80,7 @@ impl RayCast for Polyline { max_time_of_impact: Real, solid: bool, ) -> Option { - CompositeShapeRef(self) - .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) - .map(|hit| hit.1) + CompositeShapeRef(self).cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) } } @@ -98,8 +99,6 @@ impl RayCast for Compound { max_time_of_impact: Real, solid: bool, ) -> Option { - CompositeShapeRef(self) - .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) - .map(|hit| hit.1) + CompositeShapeRef(self).cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) } } diff --git a/src/query/ray/ray_heightfield.rs b/src/query/ray/ray_heightfield.rs index 24246ee2..760ed1e6 100644 --- a/src/query/ray/ray_heightfield.rs +++ b/src/query/ray/ray_heightfield.rs @@ -48,15 +48,12 @@ impl RayCast for HeightField { if s >= 0.0 && t >= 0.0 && t <= 1.0 { // Cast succeeded on the first element! let n = seg.normal().unwrap(); - let fid = if n.dot(ray.dir) > 0.0 { - // The ray hit the back face. - curr + self.num_cells() - } else { - // The ray hit the front face. - curr - }; + // The feature is the hit segment's own side; `subshape` says which segment. + let fid = if n.dot(ray.dir) > 0.0 { 1 } else { 0 }; - return Some(RayIntersection::new(s, n, FeatureId::Face(fid as u32))); + return Some( + RayIntersection::new(s, n, FeatureId::Face(fid)).with_subshape(curr as u32), + ); } } @@ -98,14 +95,11 @@ impl RayCast for HeightField { if t >= 0.0 && t <= 1.0 && s <= max_time_of_impact { let n = seg.normal().unwrap(); - let fid = if n.dot(ray.dir) > 0.0 { - // The ray hit the back face. - curr + self.num_cells() - } else { - // The ray hit the front face. - curr - }; - return Some(RayIntersection::new(s, n, FeatureId::Face(fid as u32))); + // The feature is the hit segment's own side; `subshape` says which segment. + let fid = if n.dot(ray.dir) > 0.0 { 1 } else { 0 }; + return Some( + RayIntersection::new(s, n, FeatureId::Face(fid)).with_subshape(curr as u32), + ); } } } @@ -158,26 +152,24 @@ impl RayCast for HeightField { .1 .and_then(|tri| tri.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid)); + // The feature stays the one the triangle reported; `subshape` says which triangle it + // belongs to. match (inter1, inter2) { (Some(mut inter1), Some(mut inter2)) => { if inter1.time_of_impact < inter2.time_of_impact { - inter1.feature = - self.convert_triangle_feature_id(cell.0, cell.1, true, inter1.feature); + inter1.subshape = self.triangle_id(cell.0, cell.1, true); return Some(inter1); } else { - inter2.feature = - self.convert_triangle_feature_id(cell.0, cell.1, false, inter2.feature); + inter2.subshape = self.triangle_id(cell.0, cell.1, false); return Some(inter2); } } (Some(mut inter), None) => { - inter.feature = - self.convert_triangle_feature_id(cell.0, cell.1, true, inter.feature); + inter.subshape = self.triangle_id(cell.0, cell.1, true); return Some(inter); } (None, Some(mut inter)) => { - inter.feature = - self.convert_triangle_feature_id(cell.0, cell.1, false, inter.feature); + inter.subshape = self.triangle_id(cell.0, cell.1, false); return Some(inter); } (None, None) => {} diff --git a/src/query/ray/ray_trimesh.rs b/src/query/ray/ray_trimesh.rs index 1d68bdbb..94703359 100644 --- a/src/query/ray/ray_trimesh.rs +++ b/src/query/ray/ray_trimesh.rs @@ -1,6 +1,6 @@ use crate::math::Real; use crate::query::{Ray, RayCast, RayIntersection}; -use crate::shape::{CompositeShapeRef, FeatureId, TriMesh}; +use crate::shape::{CompositeShapeRef, TriMesh}; #[cfg(feature = "dim3")] pub use ray_cast_with_culling::RayCullingMode; @@ -20,18 +20,7 @@ impl RayCast for TriMesh { max_time_of_impact: Real, solid: bool, ) -> Option { - CompositeShapeRef(self) - .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) - .map(|(best, mut res)| { - // We hit a backface. - // NOTE: we need this for `TriMesh::is_backface` to work properly. - if res.feature == FeatureId::Face(1) { - res.feature = FeatureId::Face(best + self.indices().len() as u32) - } else { - res.feature = FeatureId::Face(best); - } - res - }) + CompositeShapeRef(self).cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) } } @@ -43,7 +32,7 @@ mod ray_cast_with_culling { use crate::query::details::NormalConstraints; use crate::query::{Ray, RayIntersection}; use crate::shape::{ - CompositeShape, CompositeShapeRef, FeatureId, Shape, TriMesh, Triangle, TypedCompositeShape, + CompositeShape, CompositeShapeRef, Shape, TriMesh, Triangle, TypedCompositeShape, }; /// Controls which side of a triangle a ray-cast is allowed to hit. @@ -163,18 +152,11 @@ mod ray_cast_with_culling { culling, ray, }; - CompositeShapeRef(&mesh_with_culling) - .cast_local_ray_and_get_normal(ray, max_time_of_impact, false) - .map(|(best, mut res)| { - // We hit a backface. - // NOTE: we need this for `TriMesh::is_backface` to work properly. - if res.feature == FeatureId::Face(1) { - res.feature = FeatureId::Face(best + self.indices().len() as u32) - } else { - res.feature = FeatureId::Face(best); - } - res - }) + CompositeShapeRef(&mesh_with_culling).cast_local_ray_and_get_normal( + ray, + max_time_of_impact, + false, + ) } } diff --git a/src/query/ray/ray_voxels.rs b/src/query/ray/ray_voxels.rs index 4da55478..0ecc0722 100644 --- a/src/query/ray/ray_voxels.rs +++ b/src/query/ray/ray_voxels.rs @@ -1,7 +1,7 @@ use crate::math::{IVectorExt, Real, Vector, VectorExt}; use crate::partitioning::BvhNode; use crate::query::{Ray, RayCast, RayIntersection}; -use crate::shape::{FeatureId, Voxels, VoxelsChunkRef}; +use crate::shape::{Voxels, VoxelsChunkRef}; impl RayCast for Voxels { #[inline] @@ -63,10 +63,9 @@ impl<'a> RayCast for VoxelsChunkRef<'a> { let hit = aabb.cast_local_ray_and_get_normal(ray, max_t, solid); if let Some(mut hit) = hit { - // TODO: have the feature id be based on the voxel type? - hit.feature = FeatureId::Face( - self.flat_id(voxel_key).unwrap_or_else(|| unreachable!()), - ); + // The feature stays the one the voxel's Aabb reported; `subshape` says + // which voxel it belongs to. + hit.subshape = self.flat_id(voxel_key).unwrap_or_else(|| unreachable!()); return Some(hit); } } diff --git a/src/query/shape_cast/shape_cast.rs b/src/query/shape_cast/shape_cast.rs index 857b42a6..4d2b9d93 100644 --- a/src/query/shape_cast/shape_cast.rs +++ b/src/query/shape_cast/shape_cast.rs @@ -1,6 +1,6 @@ use crate::math::{Pose, Real, Vector}; use crate::query::{DefaultQueryDispatcher, QueryDispatcher, Unsupported}; -use crate::shape::Shape; +use crate::shape::{Shape, SubShapeId}; #[cfg(feature = "alloc")] use crate::partitioning::BvhLeafCost; @@ -73,6 +73,14 @@ pub struct ShapeCastHit { pub normal2: Vector, /// The way the shape-casting algorithm terminated. pub status: ShapeCastStatus, + /// The sub-shape of the first shape that was hit. + /// + /// Always `0` for a shape with no sub-shapes. + pub subshape1: SubShapeId, + /// The sub-shape of the second shape that was hit. + /// + /// Always `0` for a shape with no sub-shapes. + pub subshape2: SubShapeId, } impl ShapeCastHit { @@ -88,6 +96,8 @@ impl ShapeCastHit { normal1: self.normal2, normal2: self.normal1, status: self.status, + subshape1: self.subshape2, + subshape2: self.subshape1, } } @@ -100,6 +110,8 @@ impl ShapeCastHit { normal1: pos.rotation * self.normal1, normal2: self.normal2, status: self.status, + subshape1: self.subshape1, + subshape2: self.subshape2, } } } diff --git a/src/query/shape_cast/shape_cast_ball_ball.rs b/src/query/shape_cast/shape_cast_ball_ball.rs index 53212f29..47ab0ebd 100644 --- a/src/query/shape_cast/shape_cast_ball_ball.rs +++ b/src/query/shape_cast/shape_cast_ball_ball.rs @@ -60,6 +60,8 @@ pub fn cast_shapes_ball_ball( witness1, witness2, status, + subshape1: 0, + subshape2: 0, }) } else { None diff --git a/src/query/shape_cast/shape_cast_composite_shape_shape.rs b/src/query/shape_cast/shape_cast_composite_shape_shape.rs index df37392a..09248b2f 100644 --- a/src/query/shape_cast/shape_cast_composite_shape_shape.rs +++ b/src/query/shape_cast/shape_cast_composite_shape_shape.rs @@ -9,8 +9,7 @@ impl CompositeShapeRef<'_, S> { /// Performs a shape-cast between `self` and a `shape2` positioned at `pose12` and subject to /// a linear velocity `vel12`, relative to `self`. /// - /// Returns the shape-cast hit (if any) as well as the index of the sub-shape of `self` involved - /// in the hit. + /// The hit's `subshape1` says which sub-shape of `self` was involved. pub fn cast_shape( &self, dispatcher: &D, @@ -18,47 +17,55 @@ impl CompositeShapeRef<'_, S> { vel12: Vector, g2: &dyn Shape, options: ShapeCastOptions, - ) -> Option<(u32, ShapeCastHit)> { + ) -> Option { let ls_aabb2 = g2.compute_aabb(pose12); let ray = Ray::new(Vector::ZERO, vel12); let msum_shift = -ls_aabb2.center(); let msum_margin = ls_aabb2.half_extents() + Vector::splat(options.target_distance); - self.0.bvh().find_best( - options.max_time_of_impact, - |node: &BvhNode, best_so_far| { - // Compute the minkowski sum of the two Aabbs. - let msum = Aabb { - mins: node.mins() + msum_shift - msum_margin, - maxs: node.maxs() + msum_shift + msum_margin, - }; + self.0 + .bvh() + .find_best( + options.max_time_of_impact, + |node: &BvhNode, best_so_far| { + // Compute the minkowski sum of the two Aabbs. + let msum = Aabb { + mins: node.mins() + msum_shift - msum_margin, + maxs: node.maxs() + msum_shift + msum_margin, + }; - // Compute the time of impact. - msum.cast_local_ray(&ray, best_so_far, true) - .unwrap_or(Real::MAX) - }, - |part_id, _| { - self.0 - .map_untyped_part_at(part_id, |part_pose1, part_g1, _| { - if let Some(part_pose1) = part_pose1 { - dispatcher - .cast_shapes( - &part_pose1.inv_mul(pose12), - part_pose1.rotation.inverse() * vel12, - part_g1, - g2, - options, - ) - .ok()? - .map(|hit| hit.transform1_by(part_pose1)) - } else { - dispatcher - .cast_shapes(pose12, vel12, part_g1, g2, options) - .ok()? - } - })? - }, - ) + // Compute the time of impact. + msum.cast_local_ray(&ray, best_so_far, true) + .unwrap_or(Real::MAX) + }, + |part_id, _| { + self.0 + .map_untyped_part_at(part_id, |part_pose1, part_g1, _| { + if let Some(part_pose1) = part_pose1 { + dispatcher + .cast_shapes( + &part_pose1.inv_mul(pose12), + part_pose1.rotation.inverse() * vel12, + part_g1, + g2, + options, + ) + .ok()? + .map(|hit| hit.transform1_by(part_pose1)) + } else { + dispatcher + .cast_shapes(pose12, vel12, part_g1, g2, options) + .ok()? + } + })? + }, + ) + // `subshape2` is left as the dispatch set it: `g2` may be a composite too, and only it + // knows which of its parts answered. + .map(|(part_id, mut hit)| { + hit.subshape1 = part_id; + hit + }) } } @@ -75,9 +82,7 @@ where D: ?Sized + QueryDispatcher, G1: ?Sized + TypedCompositeShape, { - CompositeShapeRef(g1) - .cast_shape(dispatcher, pos12, vel12, g2, options) - .map(|hit| hit.1) + CompositeShapeRef(g1).cast_shape(dispatcher, pos12, vel12, g2, options) } /// Time Of Impact of any shape with a composite shape, under translational movement. diff --git a/src/query/shape_cast/shape_cast_halfspace_support_map.rs b/src/query/shape_cast/shape_cast_halfspace_support_map.rs index 42f94925..1317c1d6 100644 --- a/src/query/shape_cast/shape_cast_halfspace_support_map.rs +++ b/src/query/shape_cast/shape_cast_halfspace_support_map.rs @@ -53,6 +53,8 @@ pub fn cast_shapes_halfspace_support_map( witness1, witness2: pos12.inverse_transform_point(witness2), status, + subshape1: 0, + subshape2: 0, }) } else { None diff --git a/src/query/shape_cast/shape_cast_support_map_support_map.rs b/src/query/shape_cast/shape_cast_support_map_support_map.rs index b965e7ed..9b16f0e1 100644 --- a/src/query/shape_cast/shape_cast_support_map_support_map.rs +++ b/src/query/shape_cast/shape_cast_support_map_support_map.rs @@ -58,6 +58,8 @@ where } else { ShapeCastStatus::Converged }, + subshape1: 0, + subshape2: 0, }) } } else { @@ -80,6 +82,8 @@ where } else { ShapeCastStatus::Converged }, + subshape1: 0, + subshape2: 0, }) } }) diff --git a/src/shape/feature_id.rs b/src/shape/feature_id.rs index 9d2643c2..1f897436 100644 --- a/src/shape/feature_id.rs +++ b/src/shape/feature_id.rs @@ -1,3 +1,15 @@ +/// The index of a sub-shape within a shape that has several. +/// +/// Identifies a part of a [`Compound`](crate::shape::Compound), a triangle of a +/// [`TriMesh`](crate::shape::TriMesh), a segment of a [`Polyline`](crate::shape::Polyline), a cell +/// of a [`HeightField`](crate::shape::HeightField), or a voxel of a +/// [`Voxels`](crate::shape::Voxels) shape. +/// +/// Query results carry the sub-shape they came from, so a hit against a composite shape says which +/// part answered. A shape with no sub-shapes, like a [`Ball`](crate::shape::Ball) or a +/// [`Cuboid`](crate::shape::Cuboid), always reports `0`. +pub type SubShapeId = u32; + /// An identifier of a geometric feature (vertex, edge, or face) of a shape. /// /// Feature IDs are used throughout Parry to identify specific geometric features on shapes diff --git a/src/shape/heightfield3.rs b/src/shape/heightfield3.rs index 4adfc7c5..1d1e59a5 100644 --- a/src/shape/heightfield3.rs +++ b/src/shape/heightfield3.rs @@ -5,7 +5,7 @@ use crate::utils::Array2; use crate::bounding_volume::Aabb; use crate::math::{Real, Vector}; -use crate::shape::{FeatureId, Triangle, TrianglePseudoNormals}; +use crate::shape::{FeatureId, SubShapeId, Triangle, TrianglePseudoNormals}; #[cfg(not(feature = "std"))] use crate::math::ComplexField; @@ -127,7 +127,7 @@ impl HeightField { self.heights.ncols() - 1 } - fn triangle_id(&self, i: usize, j: usize, left: bool) -> u32 { + pub(crate) fn triangle_id(&self, i: usize, j: usize, left: bool) -> u32 { let tid = j * (self.heights.nrows() - 1) + i; if left { tid as u32 @@ -586,15 +586,15 @@ impl HeightField { &self.aabb } - /// Converts the FeatureID of the left or right triangle at the cell `(i, j)` into a FeatureId - /// of the whole heightfield. + /// Converts a FeatureId of the triangle `triangle_id` into a FeatureId of the whole + /// heightfield. pub fn convert_triangle_feature_id( &self, - i: usize, - j: usize, - left: bool, + triangle_id: SubShapeId, fid: FeatureId, ) -> FeatureId { + let (i, j, left) = self.split_triangle_id(triangle_id); + match fid { FeatureId::Vertex(ivertex) => { let nrows = self.heights.nrows(); @@ -903,3 +903,37 @@ impl HeightFieldRadialTriangles<'_> { } } } + +#[cfg(test)] +#[cfg(all(feature = "dim3", feature = "alloc", feature = "std"))] +mod test { + use super::HeightField; + use crate::math::{Real, Vector}; + use crate::shape::FeatureId; + use crate::utils::Array2; + use alloc::vec::Vec; + + /// The conversion recovers the cell and side from the triangle id, so every triangle has to + /// come out with a face of its own. + #[test] + fn every_triangle_converts_to_its_own_feature() { + let (nrows, ncols) = (4, 5); + let heights: Vec = (0..nrows * ncols).map(|k| k as Real * 0.13).collect(); + let heightfield = HeightField::new( + Array2::new(nrows, ncols, heights), + Vector::new(2.0, 1.0, 3.0), + ); + + let num_triangles = (nrows - 1) * (ncols - 1) * 2; + let faces: Vec<_> = (0..num_triangles as u32) + .map(|triangle| heightfield.convert_triangle_feature_id(triangle, FeatureId::Face(0))) + .collect(); + + for (triangle, face) in faces.iter().enumerate() { + assert!( + !faces[..triangle].contains(face), + "triangle {triangle} reuses {face:?}" + ); + } + } +} diff --git a/src/shape/mod.rs b/src/shape/mod.rs index e6b95f01..e3b0c2d6 100644 --- a/src/shape/mod.rs +++ b/src/shape/mod.rs @@ -3,7 +3,7 @@ pub use self::ball::Ball; pub use self::capsule::Capsule; pub use self::cuboid::Cuboid; -pub use self::feature_id::{FeatureId, PackedFeatureId}; +pub use self::feature_id::{FeatureId, PackedFeatureId, SubShapeId}; pub use self::half_space::HalfSpace; pub use self::polygonal_feature_map::PolygonalFeatureMap; pub use self::round_shape::RoundShape; diff --git a/src/shape/polyline.rs b/src/shape/polyline.rs index 37760f8e..15a23a4e 100644 --- a/src/shape/polyline.rs +++ b/src/shape/polyline.rs @@ -4,7 +4,8 @@ use crate::partitioning::{Bvh, BvhBuildStrategy}; use crate::query::{PointProjection, PointQueryWithLocation}; use crate::shape::composite_shape::CompositeShape; use crate::shape::{ - FeatureId, Segment, SegmentPointLocation, SegmentPseudoNormals, Shape, TypedCompositeShape, + FeatureId, Segment, SegmentPointLocation, SegmentPseudoNormals, Shape, SubShapeId, + TypedCompositeShape, }; #[cfg(feature = "alloc")] use alloc::vec::Vec; @@ -570,17 +571,26 @@ impl Polyline { ) } - /// Transforms the feature-id of a segment to the feature-id of this polyline. + /// Converts a FeatureId of the segment `segment` into a FeatureId of the whole polyline. + /// + /// An endpoint becomes the polyline vertex it indexes, so the two segments meeting at a corner + /// agree on it. In 2D a segment has a side facing each way, and they are numbered like the + /// segments themselves: the second side of segment `i` is `i + self.indices().len()`. pub fn segment_feature_to_polyline_feature( &self, - segment: u32, - _feature: FeatureId, + segment: SubShapeId, + feature: FeatureId, ) -> FeatureId { - // TODO: return a vertex feature when it makes sense. - #[cfg(feature = "dim2")] - return FeatureId::Face(segment); - #[cfg(feature = "dim3")] - return FeatureId::Edge(segment); + match feature { + FeatureId::Vertex(endpoint) => { + FeatureId::Vertex(self.indices[segment as usize][endpoint as usize]) + } + #[cfg(feature = "dim2")] + FeatureId::Face(side) => FeatureId::Face(segment + side * self.indices.len() as u32), + #[cfg(feature = "dim3")] + FeatureId::Edge(_) => FeatureId::Edge(segment), + _ => FeatureId::Unknown, + } } /// Returns a slice containing all vertices of this polyline. diff --git a/src/shape/shape.rs b/src/shape/shape.rs index 40d25db5..0889f61c 100644 --- a/src/shape/shape.rs +++ b/src/shape/shape.rs @@ -8,7 +8,7 @@ use crate::shape::SharedShape; use crate::shape::{composite_shape::CompositeShape, Compound, HeightField, Polyline, TriMesh}; use crate::shape::{ Ball, Capsule, Cuboid, FeatureId, HalfSpace, PolygonalFeatureMap, RoundCuboid, RoundShape, - RoundTriangle, Segment, SupportMap, Triangle, + RoundTriangle, Segment, SubShapeId, SupportMap, Triangle, }; #[cfg(feature = "dim3")] use crate::shape::{Cone, Cylinder, RoundCone, RoundCylinder}; @@ -418,7 +418,15 @@ pub trait Shape: RayCast + PointQuery + Any + Send + Sync { // } /// The shape's normal at the given point located on a specific feature. - fn feature_normal_at_point(&self, _feature: FeatureId, _point: Vector) -> Option { + /// + /// `subshape` identifies the sub-shape the feature belongs to, as a query result reports it; + /// it is `0` for a shape with no sub-shapes. `feature` is that sub-shape's own feature. + fn feature_normal_at_point( + &self, + _subshape: SubShapeId, + _feature: FeatureId, + _point: Vector, + ) -> Option { None } @@ -717,7 +725,12 @@ impl Shape for Ball { /// The shape's normal at the given point located on a specific feature. #[inline] - fn feature_normal_at_point(&self, _: FeatureId, point: Vector) -> Option { + fn feature_normal_at_point( + &self, + _: SubShapeId, + _: FeatureId, + point: Vector, + ) -> Option { (point).try_normalize() } } @@ -777,7 +790,12 @@ impl Shape for Cuboid { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option { + fn feature_normal_at_point( + &self, + _subshape: SubShapeId, + feature: FeatureId, + _point: Vector, + ) -> Option { self.feature_normal(feature) } } @@ -898,7 +916,12 @@ impl Shape for Triangle { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point(&self, _feature: FeatureId, _point: Vector) -> Option { + fn feature_normal_at_point( + &self, + _subshape: SubShapeId, + _feature: FeatureId, + _point: Vector, + ) -> Option { #[cfg(feature = "dim2")] return None; #[cfg(feature = "dim3")] @@ -961,7 +984,12 @@ impl Shape for Segment { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option { + fn feature_normal_at_point( + &self, + _subshape: SubShapeId, + feature: FeatureId, + _point: Vector, + ) -> Option { self.feature_normal(feature) } } @@ -1126,12 +1154,17 @@ impl Shape for TriMesh { Real::frac_pi_4() } - /// Gets the normal of the triangle represented by `feature`. - fn feature_normal_at_point(&self, _feature: FeatureId, _point: Vector) -> Option { + /// Gets the normal of the triangle `subshape`. + fn feature_normal_at_point( + &self, + _subshape: SubShapeId, + _feature: FeatureId, + _point: Vector, + ) -> Option { #[cfg(feature = "dim2")] return None; #[cfg(feature = "dim3")] - return self.feature_normal(_feature); + return self.triangle_normal(_subshape); } #[cfg(feature = "alloc")] @@ -1243,7 +1276,12 @@ impl Shape for ConvexPolygon { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option { + fn feature_normal_at_point( + &self, + _subshape: SubShapeId, + feature: FeatureId, + _point: Vector, + ) -> Option { self.feature_normal(feature) } } @@ -1307,7 +1345,12 @@ impl Shape for ConvexPolyhedron { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option { + fn feature_normal_at_point( + &self, + _subshape: SubShapeId, + feature: FeatureId, + _point: Vector, + ) -> Option { self.feature_normal(feature) } } diff --git a/src/shape/trimesh.rs b/src/shape/trimesh.rs index ed979623..5962cf22 100644 --- a/src/shape/trimesh.rs +++ b/src/shape/trimesh.rs @@ -3,6 +3,8 @@ use crate::bounding_volume::Aabb; use crate::math::VectorExt; use crate::math::{Pose, Vector}; use crate::partitioning::{Bvh, BvhBuildStrategy}; +#[cfg(feature = "dim3")] +use crate::shape::SubShapeId; use crate::shape::{FeatureId, Shape, Triangle, TrianglePseudoNormals, TypedCompositeShape}; use crate::utils::HashablePartialEq; use alloc::{vec, vec::Vec}; @@ -1690,14 +1692,10 @@ impl TriMesh { } #[cfg(feature = "dim3")] - /// Gets the normal of the triangle represented by `feature`. - pub fn feature_normal(&self, feature: FeatureId) -> Option { - match feature { - FeatureId::Face(i) => self - .triangle(i % self.num_triangles() as u32) - .feature_normal(FeatureId::Face(0)), - _ => None, - } + /// Gets the normal of the triangle `triangle_id`. + pub fn triangle_normal(&self, triangle_id: SubShapeId) -> Option { + self.triangle(triangle_id % self.num_triangles() as u32) + .feature_normal(FeatureId::Face(0)) } } @@ -1853,11 +1851,9 @@ impl TriMesh { /// Does the given feature ID identify a backface of this trimesh? pub fn is_backface(&self, feature: FeatureId) -> bool { - if let FeatureId::Face(i) = feature { - i >= self.indices.len() as u32 - } else { - false - } + // The feature is the hit triangle's own, which reports its back face as `Face(1)`; the + // triangle itself is identified by the result's `subshape`. + feature == FeatureId::Face(1) } /// Get the `i`-th triangle of this mesh. From 4e7555350971d9b9758b81813b7de0e345748a01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 4 Sep 2026 18:38:48 +0200 Subject: [PATCH 2/2] chore: clippy fixes --- src/query/point/point_composite_shape.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index cf42cd0e..92ec31d5 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -197,7 +197,7 @@ impl PointQuery for TriMesh { self.project_local_point_with_max_dist(point, solid, Real::MAX) // Shouldn’t happen (trimesh must not be empty). But return something // instead of crashing with `unwrap`. - .unwrap_or((PointProjection::new(false, point))) + .unwrap_or(PointProjection::new(false, point)) } #[inline]