parry3d/query/shape_cast/
shape_cast_composite_shape_shape.rs

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

parry3d/query/shape_cast/
shape_cast_composite_shape_shape.rs
