parry2d/query/nonlinear_shape_cast/
nonlinear_shape_cast_composite_shape_shape.rs

1use crate::math::Real;
2use crate::partitioning::BvhNode;
3use crate::query::{
4    self, details::NonlinearShapeCastMode, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit,
5};
6use crate::shape::{Ball, CompositeShapeRef, Shape, SubShapeId, TypedCompositeShape};
7
8impl<S: ?Sized + TypedCompositeShape> CompositeShapeRef<'_, S> {
9    /// Performs a non-linear shape-cast between `self` animated subject to the `motion1` and
10    /// the `shape2` subject to the `motion2`.
11    ///
12    /// Returns the index of the sub-shape of `self` that was involved alongside the hit, which is
13    /// left as that sub-shape reported it (its `subshape1` is the sub-shape's own when it is a
14    /// composite too, and `subshape2` is `shape2`'s).
15    pub fn cast_shape_nonlinear<D: ?Sized + QueryDispatcher>(
16        &self,
17        dispatcher: &D,
18        motion1: &NonlinearRigidMotion,
19        motion2: &NonlinearRigidMotion,
20        shape2: &dyn Shape,
21        start_time: Real,
22        end_time: Real,
23        stop_at_penetration: bool,
24    ) -> Option<(SubShapeId, ShapeCastHit)> {
25        let sphere2 = shape2.compute_local_bounding_sphere();
26
27        self.0.bvh().find_best(
28            end_time,
29            |node: &BvhNode, _| {
30                let aabb1 = node.aabb();
31                let center1 = aabb1.center();
32                let radius1 = aabb1.half_extents().length();
33                let ball1 = Ball::new(radius1);
34                let ball2 = Ball::new(sphere2.radius());
35                let ball_motion1 = motion1.prepend_translation(center1);
36                let ball_motion2 = motion2.prepend_translation(sphere2.center);
37
38                query::details::cast_shapes_nonlinear_support_map_support_map(
39                    dispatcher,
40                    &ball_motion1,
41                    &ball1,
42                    &ball1,
43                    &ball_motion2,
44                    &ball2,
45                    &ball2,
46                    start_time,
47                    end_time,
48                    NonlinearShapeCastMode::StopAtPenetration,
49                )
50                .map(|hit| hit.time_of_impact)
51                .unwrap_or(Real::MAX)
52            },
53            |part_id, _| {
54                self.0
55                    .map_untyped_part_at(part_id, |part_pos1, part_shape1, _| {
56                        if let Some(part_pos1) = part_pos1 {
57                            dispatcher
58                                .cast_shapes_nonlinear(
59                                    &motion1.prepend(*part_pos1),
60                                    part_shape1,
61                                    motion2,
62                                    shape2,
63                                    start_time,
64                                    end_time,
65                                    stop_at_penetration,
66                                )
67                                .ok()?
68                                .map(|hit| hit.transform1_by(part_pos1))
69                        } else {
70                            dispatcher
71                                .cast_shapes_nonlinear(
72                                    motion1,
73                                    part_shape1,
74                                    motion2,
75                                    shape2,
76                                    start_time,
77                                    end_time,
78                                    stop_at_penetration,
79                                )
80                                .ok()?
81                        }
82                    })?
83            },
84        )
85    }
86}
87
88/// Time Of Impact of a composite shape with any other shape, under a rigid motion (translation + rotation).
89pub fn cast_shapes_nonlinear_composite_shape_shape<D, G1>(
90    dispatcher: &D,
91    motion1: &NonlinearRigidMotion,
92    shape1: &G1,
93    motion2: &NonlinearRigidMotion,
94    shape2: &dyn Shape,
95    start_time: Real,
96    end_time: Real,
97    stop_at_penetration: bool,
98) -> Option<ShapeCastHit>
99where
100    D: ?Sized + QueryDispatcher,
101    G1: ?Sized + TypedCompositeShape,
102{
103    CompositeShapeRef(shape1)
104        .cast_shape_nonlinear(
105            dispatcher,
106            motion1,
107            motion2,
108            shape2,
109            start_time,
110            end_time,
111            stop_at_penetration,
112        )
113        .map(|(part_id, mut hit)| {
114            hit.subshape1 = part_id;
115            hit
116        })
117}
118
119/// Time Of Impact of any shape with a composite shape, under a rigid motion (translation + rotation).
120pub fn cast_shapes_nonlinear_shape_composite_shape<D, G2>(
121    dispatcher: &D,
122    motion1: &NonlinearRigidMotion,
123    shape1: &dyn Shape,
124    motion2: &NonlinearRigidMotion,
125    shape2: &G2,
126    start_time: Real,
127    end_time: Real,
128    stop_at_penetration: bool,
129) -> Option<ShapeCastHit>
130where
131    D: ?Sized + QueryDispatcher,
132    G2: ?Sized + TypedCompositeShape,
133{
134    cast_shapes_nonlinear_composite_shape_shape(
135        dispatcher,
136        motion2,
137        shape2,
138        motion1,
139        shape1,
140        start_time,
141        end_time,
142        stop_at_penetration,
143    )
144    .map(|hit| hit.swapped())
145}
parry2d/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs

parry2d/query/nonlinear_shape_cast/
nonlinear_shape_cast_composite_shape_shape.rs
