parry3d/query/shape_cast/
shape_cast_composite_shape_shape.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Pose, Real, Vector};
3use crate::partitioning::BvhNode;
4use crate::query::shape_cast::ShapeCastOptions;
5use crate::query::{QueryDispatcher, Ray, RayCast, ShapeCastHit};
6use crate::shape::{CompositeShapeRef, Shape, SubShapeId, TypedCompositeShape};
7
8impl<S: ?Sized + TypedCompositeShape> CompositeShapeRef<'_, S> {
9    /// Performs a shape-cast between `self` and a `shape2` positioned at `pose12` and subject to
10    /// a linear velocity `vel12`, relative to `self`.
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 `g2`'s).
15    pub fn cast_shape<D: ?Sized + QueryDispatcher>(
16        &self,
17        dispatcher: &D,
18        pose12: &Pose,
19        vel12: Vector,
20        g2: &dyn Shape,
21        options: ShapeCastOptions,
22    ) -> Option<(SubShapeId, ShapeCastHit)> {
23        let ls_aabb2 = g2.compute_aabb(pose12);
24        let ray = Ray::new(Vector::ZERO, vel12);
25        let msum_shift = -ls_aabb2.center();
26        let msum_margin = ls_aabb2.half_extents() + Vector::splat(options.target_distance);
27
28        self.0.bvh().find_best(
29            options.max_time_of_impact,
30            |node: &BvhNode, best_so_far| {
31                // Compute the minkowski sum of the two Aabbs.
32                let msum = Aabb {
33                    mins: node.mins() + msum_shift - msum_margin,
34                    maxs: node.maxs() + msum_shift + msum_margin,
35                };
36
37                // Compute the time of impact.
38                msum.cast_local_ray(&ray, best_so_far, true)
39                    .unwrap_or(Real::MAX)
40            },
41            |part_id, _| {
42                self.0
43                    .map_untyped_part_at(part_id, |part_pose1, part_g1, _| {
44                        if let Some(part_pose1) = part_pose1 {
45                            dispatcher
46                                .cast_shapes(
47                                    &part_pose1.inv_mul(pose12),
48                                    part_pose1.rotation.inverse() * vel12,
49                                    part_g1,
50                                    g2,
51                                    options,
52                                )
53                                .ok()?
54                                .map(|hit| hit.transform1_by(part_pose1))
55                        } else {
56                            dispatcher
57                                .cast_shapes(pose12, vel12, part_g1, g2, options)
58                                .ok()?
59                        }
60                    })?
61            },
62        )
63    }
64}
65
66/// Time Of Impact of a composite shape with any other shape, under translational movement.
67pub fn cast_shapes_composite_shape_shape<D, G1>(
68    dispatcher: &D,
69    pos12: &Pose,
70    vel12: Vector,
71    g1: &G1,
72    g2: &dyn Shape,
73    options: ShapeCastOptions,
74) -> Option<ShapeCastHit>
75where
76    D: ?Sized + QueryDispatcher,
77    G1: ?Sized + TypedCompositeShape,
78{
79    CompositeShapeRef(g1)
80        .cast_shape(dispatcher, pos12, vel12, g2, options)
81        .map(|(part_id, mut hit)| {
82            hit.subshape1 = part_id;
83            hit
84        })
85}
86
87/// Time Of Impact of any shape with a composite shape, under translational movement.
88pub fn cast_shapes_shape_composite_shape<D, G2>(
89    dispatcher: &D,
90    pos12: &Pose,
91    vel12: Vector,
92    g1: &dyn Shape,
93    g2: &G2,
94    options: ShapeCastOptions,
95) -> Option<ShapeCastHit>
96where
97    D: ?Sized + QueryDispatcher,
98    G2: ?Sized + TypedCompositeShape,
99{
100    cast_shapes_composite_shape_shape(
101        dispatcher,
102        &pos12.inverse(),
103        -(pos12.rotation.inverse() * vel12),
104        g2,
105        g1,
106        options,
107    )
108    .map(|time_of_impact| time_of_impact.swapped())
109}
parry3d/query/shape_cast/shape_cast_composite_shape_shape.rs

parry3d/query/shape_cast/
shape_cast_composite_shape_shape.rs
