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

parry3d/query/nonlinear_shape_cast/
nonlinear_shape_cast_composite_shape_shape.rs
