diff --git a/CHANGELOG.md b/CHANGELOG.md index 28b8811c..87f1f4f4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,18 @@ -## 0.31.0 +## 0.31.1 + +### Modified + +- The `CompositeShapeRef` queries (`cast_local_ray_and_get_normal`, `project_local_point`, + `project_local_point_and_get_feature`, `project_local_point_and_get_location`, `cast_shape`, + `cast_shape_nonlinear`, `intersects_shape`, `contact_with_shape`, `distance_to_shape`) no longer + overwrite the result's sub-shape with the index of the part of the composite that answered: + they return that index alongside the result, which stays as the part reported it. The caller + stores it where it wants, so a composite of composites (a scene of trimeshes) can keep both + levels. `intersects_shape` returns `None` instead of a non-intersecting result. The `Shape` + impls of `TriMesh`, `Polyline` and `Compound`, and the `*_composite_shape_shape` free functions, + still report the part in the result's `subshape`/`subshape1`. + +## 0.31.0 (yanked) ### Modified diff --git a/Cargo.toml b/Cargo.toml index 4fb198e6..06507e69 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -8,7 +8,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.31.0" +version = "0.31.1" authors = ["Sébastien Crozet "] documentation = "https://parry.rs/docs" homepage = "https://parry.rs" diff --git a/src/query/contact/contact_composite_shape_shape.rs b/src/query/contact/contact_composite_shape_shape.rs index b978a29d..c82b6391 100644 --- a/src/query/contact/contact_composite_shape_shape.rs +++ b/src/query/contact/contact_composite_shape_shape.rs @@ -1,7 +1,7 @@ use crate::bounding_volume::BoundingVolume; use crate::math::{Pose, Real}; use crate::query::{Contact, QueryDispatcher}; -use crate::shape::{CompositeShape, CompositeShapeRef, Shape}; +use crate::shape::{CompositeShape, CompositeShapeRef, Shape, SubShapeId}; use crate::utils::PoseOpt; impl CompositeShapeRef<'_, S> { @@ -9,32 +9,31 @@ impl CompositeShapeRef<'_, S> { /// `pose12` relative to `self`. /// /// Returns `None` if `self` and `shape2` are separated by a distance larger than - /// `prediction`. Otherwise the contact's `subshape1` says which sub-shape of `self` it is on. + /// `prediction`. Otherwise returns the index of the sub-shape of `self` the contact is on + /// alongside the contact, which is left as that sub-shape reported it (its `subshape1` is the + /// sub-shape's own when it is a composite too, and `subshape2` is `shape2`'s). pub fn contact_with_shape( &self, dispatcher: &D, pose12: &Pose, shape2: &dyn Shape, prediction: Real, - ) -> Option { + ) -> Option<(SubShapeId, Contact)> { let ls_aabb2 = shape2.compute_aabb(pose12).loosened(prediction); - let mut result = None::; + let mut result = None::<(SubShapeId, Contact)>; 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); } - // `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) + result = Some((part_id, c)) } } }); @@ -56,7 +55,14 @@ where D: ?Sized + QueryDispatcher, G1: ?Sized + CompositeShape, { - CompositeShapeRef(g1).contact_with_shape(dispatcher, pose12, g2, prediction) + // `subshape2` is left as the dispatch set it: `g2` may be a composite too, and only it + // knows which of its parts answered. + CompositeShapeRef(g1) + .contact_with_shape(dispatcher, pose12, g2, prediction) + .map(|(part_id, mut c)| { + c.subshape1 = part_id; + c + }) } /// Best contact between a shape and a composite (`Mesh`, `Compound`) shape. diff --git a/src/query/distance/distance_composite_shape_shape.rs b/src/query/distance/distance_composite_shape_shape.rs index 2b485aec..ce7118da 100644 --- a/src/query/distance/distance_composite_shape_shape.rs +++ b/src/query/distance/distance_composite_shape_shape.rs @@ -2,50 +2,44 @@ use crate::bounding_volume::Aabb; use crate::math::{Pose, Real}; use crate::partitioning::BvhNode; use crate::query::{QueryDispatcher, ShapeDistance}; -use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; +use crate::shape::{CompositeShapeRef, Shape, SubShapeId, TypedCompositeShape}; use crate::utils::PoseOpt; impl CompositeShapeRef<'_, S> { /// Calculates the closest distance between `self` and the given `shape2` positioned at /// `pose12` relative to `self`. /// - /// The result's `subshape1` says which sub-shape of `self` is closest to `shape2`. + /// Returns the index of the sub-shape of `self` closest to `shape2` alongside the distance, + /// which is left as that sub-shape reported it (its `subshape1` is the sub-shape's own when + /// it is a composite too, and `subshape2` is `shape2`'s). pub fn distance_to_shape( &self, dispatcher: &D, pose12: &Pose, shape2: &dyn Shape, - ) -> Option { + ) -> Option<(SubShapeId, ShapeDistance)> { 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() - }, - ) - // `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 - }) + 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() + }, + ) } } @@ -60,8 +54,14 @@ where D: ?Sized + QueryDispatcher, G1: ?Sized + TypedCompositeShape, { + // `subshape2` is left as the dispatch set it: `g2` may be a composite too, and only it + // knows which of its parts answered. CompositeShapeRef(g1) .distance_to_shape(dispatcher, pos12, g2) + .map(|(part_id, mut result)| { + result.subshape1 = part_id; + result + }) .unwrap_or(ShapeDistance::new(Real::MAX)) } 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 a1fe9dac..86a42c68 100644 --- a/src/query/intersection_test/intersection_test_composite_shape_shape.rs +++ b/src/query/intersection_test/intersection_test_composite_shape_shape.rs @@ -2,44 +2,37 @@ use crate::bounding_volume::BoundingVolume; use crate::math::Pose; use crate::partitioning::BvhNode; use crate::query::{QueryDispatcher, ShapeIntersection}; -use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; +use crate::shape::{CompositeShapeRef, Shape, SubShapeId, TypedCompositeShape}; use crate::utils::PoseOpt; impl CompositeShapeRef<'_, S> { /// Tests whether the given other `shape`, positioned at `pose12` relative to `self`, /// intersects `self`. /// - /// The result's `subshape1` says which sub-shape of `self` it intersects. + /// Returns `None` when they do not intersect. Otherwise returns the index of a sub-shape of + /// `self` that `shape` intersects alongside that sub-shape's own result (its `subshape1` is + /// the sub-shape's own when it is a composite too, and `subshape2` is `shape`'s). pub fn intersects_shape( &self, dispatcher: &D, pose12: &Pose, shape: &dyn Shape, - ) -> ShapeIntersection { + ) -> Option<(SubShapeId, ShapeIntersection)> { let ls_aabb2 = shape.compute_aabb(pose12); - let found = self - .0 + self.0 .bvh() .leaves(|node: &BvhNode| node.aabb().intersects(&ls_aabb2)) .find_map(|leaf_id| { self.0 .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)) + .map(|result| (leaf_id, result)) }) .flatten() - }); - - match found { - Some((subshape1, subshape2)) => { - ShapeIntersection::new(true).with_subshapes(subshape1, subshape2) - } - None => ShapeIntersection::new(false), - } + }) } } @@ -54,7 +47,14 @@ where D: ?Sized + QueryDispatcher, G1: ?Sized + TypedCompositeShape, { - CompositeShapeRef(g1).intersects_shape(dispatcher, pos12, g2) + // `subshape2` is left as the dispatch set it: `g2` may be a composite too, and only it + // knows which of its parts answered. + match CompositeShapeRef(g1).intersects_shape(dispatcher, pos12, g2) { + Some((part_id, result)) => { + ShapeIntersection::new(true).with_subshapes(part_id, result.subshape2) + } + None => ShapeIntersection::new(false), + } } /// Proximity between a shape and a composite (`Mesh`, `Compound`) shape. 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 a553becb..de5a8258 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 @@ -3,14 +3,15 @@ use crate::partitioning::BvhNode; use crate::query::{ self, details::NonlinearShapeCastMode, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit, }; -use crate::shape::{Ball, CompositeShapeRef, Shape, TypedCompositeShape}; +use crate::shape::{Ball, CompositeShapeRef, Shape, SubShapeId, TypedCompositeShape}; impl CompositeShapeRef<'_, S> { /// Performs a non-linear shape-cast between `self` animated subject to the `motion1` and /// the `shape2` subject to the `motion2`. /// - /// Returns the shape-cast hit (if any) as well as the index of the sub-shape of `self` involved - /// in the hit. + /// Returns the index of the sub-shape of `self` that was involved alongside the hit, which is + /// left as that sub-shape reported it (its `subshape1` is the sub-shape's own when it is a + /// composite too, and `subshape2` is `shape2`'s). pub fn cast_shape_nonlinear( &self, dispatcher: &D, @@ -20,75 +21,67 @@ impl CompositeShapeRef<'_, S> { start_time: Real, end_time: Real, stop_at_penetration: bool, - ) -> Option { + ) -> Option<(SubShapeId, ShapeCastHit)> { 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()? - } - })? - }, - ) - // `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 - }) + 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()? + } + })? + }, + ) } } @@ -107,15 +100,20 @@ where D: ?Sized + QueryDispatcher, G1: ?Sized + TypedCompositeShape, { - CompositeShapeRef(shape1).cast_shape_nonlinear( - dispatcher, - motion1, - motion2, - shape2, - start_time, - end_time, - stop_at_penetration, - ) + CompositeShapeRef(shape1) + .cast_shape_nonlinear( + dispatcher, + motion1, + motion2, + shape2, + start_time, + end_time, + stop_at_penetration, + ) + .map(|(part_id, mut hit)| { + hit.subshape1 = part_id; + hit + }) } /// Time Of Impact of any shape with a composite shape, under a rigid motion (translation + rotation). diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index 92ec31d5..48d2463d 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -26,8 +26,8 @@ fn triangle_point_location_feature(location: TrianglePointLocation) -> FeatureId impl CompositeShapeRef<'_, S> { /// Project a point on this composite shape. /// - /// The projection's `subshape` says which sub-shape of `self` answered. The second tuple - /// element contains some shape-specific information about the projected point. + /// Returns the index of the sub-shape of `self` that answered, the projection as that + /// sub-shape reported it, and some shape-specific information about the projected point. #[inline] pub fn project_local_point_and_get_location( &self, @@ -35,6 +35,7 @@ impl CompositeShapeRef<'_, S> { max_dist: Real, solid: bool, ) -> Option<( + SubShapeId, PointProjection, ::Location, )> @@ -58,20 +59,20 @@ impl CompositeShapeRef<'_, S> { Some((cost, proj)) }, ) - .map(|(best_id, (_, (proj, location)))| (proj.with_subshape(best_id), location)) + .map(|(best_id, (_, (proj, location)))| (best_id, proj, location)) } /// Project a point on this composite shape. /// - /// 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. + /// Returns the index of the sub-shape of `self` that answered and the projection as that + /// sub-shape reported it. 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 { + ) -> Option<(SubShapeId, PointProjection)> { let (best_id, (_, proj)) = self.0.bvh().find_best( max_dist, |node: &BvhNode, _best_so_far| node.aabb().distance_to_local_point(point, true), @@ -87,19 +88,19 @@ impl CompositeShapeRef<'_, S> { Some((dist, proj)) }, )?; - Some(proj.with_subshape(best_id)) + Some((best_id, proj)) } /// Project a point on this composite shape. /// - /// 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. + /// Returns the index of the sub-shape of `self` that answered, the projection as that + /// sub-shape reported it, and 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<(PointProjection, FeatureId)> { + ) -> Option<(SubShapeId, 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), @@ -115,7 +116,7 @@ impl CompositeShapeRef<'_, S> { Some((cost, proj)) }, )?; - Some((proj.with_subshape(best_id), feature_id)) + Some((best_id, proj, feature_id)) } // TODO: implement distance_to_point too? @@ -152,15 +153,16 @@ 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((mut proj, feature)) = + let Some((segment_id, mut proj, feature)) = CompositeShapeRef(self).project_local_point_and_get_feature(point, Real::MAX) else { return (PointProjection::new(false, point), FeatureId::Unknown); }; + proj.subshape = segment_id; // A point behind the outward pseudo-normal is inside. #[cfg(feature = "dim2")] - if let Some(constraints) = self.segment_normal_constraints(proj.subshape) { + if let Some(constraints) = self.segment_normal_constraints(segment_id) { let pseudo_normal = match feature { FeatureId::Vertex(i) => constraints.edges[i as usize], _ => constraints.face, @@ -213,13 +215,16 @@ impl PointQuery for TriMesh { let solid = cfg!(feature = "dim2"); // No candidate: `point` (or `self`) isn’t finite. See // `Polyline::project_local_point_and_get_feature`. - let Some((proj, location)) = + let Some((triangle_id, proj, location)) = CompositeShapeRef(self).project_local_point_and_get_location(point, Real::MAX, solid) else { return (PointProjection::new(false, point), FeatureId::Unknown); }; // The feature is the triangle's own; `proj.subshape` says which triangle it belongs to. - (proj, triangle_point_location_feature(location)) + ( + proj.with_subshape(triangle_id), + triangle_point_location_feature(location), + ) } // TODO: implement distance_to_point too? @@ -257,6 +262,7 @@ impl PointQuery for Compound { fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection { CompositeShapeRef(self) .project_local_point(point, Real::MAX, solid) + .map(|(part_id, proj)| proj.with_subshape(part_id)) // No candidate: `point` (or `self`) isn’t finite. See // `Polyline::project_local_point_and_get_feature`. .unwrap_or(PointProjection::new(false, point)) @@ -267,6 +273,7 @@ impl PointQuery for Compound { // 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) + .map(|(part_id, proj, feature)| (proj.with_subshape(part_id), feature)) // No candidate: `point` (or `self`) isn’t finite. See // `Polyline::project_local_point_and_get_feature`. .unwrap_or((PointProjection::new(false, point), FeatureId::Unknown)) @@ -306,10 +313,10 @@ 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((mut proj, loc)) = + if let Some((seg_id, mut proj, loc)) = CompositeShapeRef(self).project_local_point_and_get_location(point, max_dist, solid) { - let seg_id = proj.subshape; + proj.subshape = seg_id; // A point behind the outward pseudo-normal is inside. #[cfg(feature = "dim2")] @@ -359,10 +366,10 @@ impl PointQueryWithLocation for TriMesh { max_dist: Real, ) -> Option<(PointProjection, Self::Location)> { #[allow(unused_mut)] // mut is needed in 3D. - if let Some((mut proj, location)) = + if let Some((part_id, mut proj, location)) = CompositeShapeRef(self).project_local_point_and_get_location(point, max_dist, solid) { - let part_id = proj.subshape; + proj.subshape = part_id; #[cfg(feature = "dim3")] if let Some(pseudo_normals) = self.pseudo_normals_if_oriented() { diff --git a/src/query/ray/ray_composite_shape.rs b/src/query/ray/ray_composite_shape.rs index 846ee15a..6938c08c 100644 --- a/src/query/ray/ray_composite_shape.rs +++ b/src/query/ray/ray_composite_shape.rs @@ -39,29 +39,30 @@ impl CompositeShapeRef<'_, S> { } /// Same as [`Self::cast_local_ray`] but also computes the normal at the hit location. + /// + /// Returns the index of the sub-shape of `self` that was hit alongside the hit, which is + /// left as that sub-shape reported it (its `subshape` is the sub-shape's own, when it is a + /// composite too). #[inline] pub fn cast_local_ray_and_get_normal( &self, ray: &Ray, max_time_of_impact: Real, solid: bool, - ) -> 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)) + ) -> Option<(SubShapeId, 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) + } + })? + }, + ) } } @@ -80,7 +81,9 @@ 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) + CompositeShapeRef(self) + .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + .map(|(segment_id, hit)| hit.with_subshape(segment_id)) } } @@ -99,6 +102,8 @@ 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) + CompositeShapeRef(self) + .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + .map(|(part_id, hit)| hit.with_subshape(part_id)) } } diff --git a/src/query/ray/ray_trimesh.rs b/src/query/ray/ray_trimesh.rs index 94703359..e4e8aea5 100644 --- a/src/query/ray/ray_trimesh.rs +++ b/src/query/ray/ray_trimesh.rs @@ -20,7 +20,9 @@ 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) + CompositeShapeRef(self) + .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + .map(|(triangle_id, hit)| hit.with_subshape(triangle_id)) } } @@ -152,11 +154,9 @@ mod ray_cast_with_culling { culling, ray, }; - CompositeShapeRef(&mesh_with_culling).cast_local_ray_and_get_normal( - ray, - max_time_of_impact, - false, - ) + CompositeShapeRef(&mesh_with_culling) + .cast_local_ray_and_get_normal(ray, max_time_of_impact, false) + .map(|(triangle_id, hit)| hit.with_subshape(triangle_id)) } } 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 09248b2f..b5cc7ada 100644 --- a/src/query/shape_cast/shape_cast_composite_shape_shape.rs +++ b/src/query/shape_cast/shape_cast_composite_shape_shape.rs @@ -3,13 +3,15 @@ use crate::math::{Pose, Real, Vector}; use crate::partitioning::BvhNode; use crate::query::shape_cast::ShapeCastOptions; use crate::query::{QueryDispatcher, Ray, RayCast, ShapeCastHit}; -use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; +use crate::shape::{CompositeShapeRef, Shape, SubShapeId, TypedCompositeShape}; 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`. /// - /// The hit's `subshape1` says which sub-shape of `self` was involved. + /// Returns the index of the sub-shape of `self` that was involved alongside the hit, which is + /// left as that sub-shape reported it (its `subshape1` is the sub-shape's own when it is a + /// composite too, and `subshape2` is `g2`'s). pub fn cast_shape( &self, dispatcher: &D, @@ -17,55 +19,47 @@ impl CompositeShapeRef<'_, S> { vel12: Vector, g2: &dyn Shape, options: ShapeCastOptions, - ) -> Option { + ) -> Option<(SubShapeId, ShapeCastHit)> { 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()? - } - })? - }, - ) - // `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 - }) + // 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()? + } + })? + }, + ) } } @@ -82,7 +76,12 @@ where D: ?Sized + QueryDispatcher, G1: ?Sized + TypedCompositeShape, { - CompositeShapeRef(g1).cast_shape(dispatcher, pos12, vel12, g2, options) + CompositeShapeRef(g1) + .cast_shape(dispatcher, pos12, vel12, g2, options) + .map(|(part_id, mut hit)| { + hit.subshape1 = part_id; + hit + }) } /// Time Of Impact of any shape with a composite shape, under translational movement.