ncollide3d/query/time_of_impact/
time_of_impact_composite_shape_shape.rs

Help
1use crate::bounding_volume::bounding_volume::BoundingVolume;
2use crate::bounding_volume::AABB;
3use crate::math::{Isometry, Point, Vector};
4use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor};
5use crate::query::{Ray, RayCast, TOIDispatcher, TOI};
6use crate::shape::{CompositeShape, Shape};
7use na::{self, RealField};
8
9/// Time Of Impact of a composite shape with any other shape, under translational movement.
10pub fn time_of_impact_composite_shape_shape<N, G1: ?Sized>(
11    dispatcher: &dyn TOIDispatcher<N>,
12    m1: &Isometry<N>,
13    vel1: &Vector<N>,
14    g1: &G1,
15    m2: &Isometry<N>,
16    vel2: &Vector<N>,
17    g2: &dyn Shape<N>,
18    max_toi: N,
19    target_distance: N,
20) -> Option<TOI<N>>
21where
22    N: RealField + Copy,
23    G1: CompositeShape<N>,
24{
25    let mut visitor = CompositeShapeAgainstAnyTOIVisitor::new(
26        dispatcher,
27        m1,
28        vel1,
29        g1,
30        m2,
31        vel2,
32        g2,
33        max_toi,
34        target_distance,
35    );
36    g1.bvh().best_first_search(&mut visitor).map(|res| res.1)
37}
38
39/// Time Of Impact of any shape with a composite shape, under translational movement.
40pub fn time_of_impact_shape_composite_shape<N, G2: ?Sized>(
41    dispatcher: &dyn TOIDispatcher<N>,
42    m1: &Isometry<N>,
43    vel1: &Vector<N>,
44    g1: &dyn Shape<N>,
45    m2: &Isometry<N>,
46    vel2: &Vector<N>,
47    g2: &G2,
48    max_toi: N,
49    target_distance: N,
50) -> Option<TOI<N>>
51where
52    N: RealField + Copy,
53    G2: CompositeShape<N>,
54{
55    time_of_impact_composite_shape_shape(
56        dispatcher,
57        m2,
58        vel2,
59        g2,
60        m1,
61        vel1,
62        g1,
63        max_toi,
64        target_distance,
65    )
66    .map(|toi| toi.swapped())
67}
68
69struct CompositeShapeAgainstAnyTOIVisitor<'a, N: 'a + RealField + Copy, G1: ?Sized + 'a> {
70    dispatcher: &'a dyn TOIDispatcher<N>,
71    msum_shift: Vector<N>,
72    msum_margin: Vector<N>,
73    ray: Ray<N>,
74
75    m1: &'a Isometry<N>,
76    vel1: &'a Vector<N>,
77    g1: &'a G1,
78    m2: &'a Isometry<N>,
79    vel2: &'a Vector<N>,
80    g2: &'a dyn Shape<N>,
81    max_toi: N,
82    target_distance: N,
83}
84
85impl<'a, N, G1: ?Sized> CompositeShapeAgainstAnyTOIVisitor<'a, N, G1>
86where
87    N: RealField + Copy,
88    G1: CompositeShape<N>,
89{
90    pub fn new(
91        dispatcher: &'a dyn TOIDispatcher<N>,
92        m1: &'a Isometry<N>,
93        vel1: &'a Vector<N>,
94        g1: &'a G1,
95        m2: &'a Isometry<N>,
96        vel2: &'a Vector<N>,
97        g2: &'a dyn Shape<N>,
98        max_toi: N,
99        target_distance: N,
100    ) -> CompositeShapeAgainstAnyTOIVisitor<'a, N, G1> {
101        let ls_m2 = m1.inverse() * m2.clone();
102        let ls_aabb2 = g2.aabb(&ls_m2).loosened(target_distance);
103
104        CompositeShapeAgainstAnyTOIVisitor {
105            dispatcher,
106            msum_shift: -ls_aabb2.center().coords,
107            msum_margin: ls_aabb2.half_extents(),
108            ray: Ray::new(
109                Point::origin(),
110                m1.inverse_transform_vector(&(*vel2 - *vel1)),
111            ),
112            m1,
113            vel1,
114            g1,
115            m2,
116            vel2,
117            g2,
118            max_toi,
119            target_distance,
120        }
121    }
122}
123
124impl<'a, N, G1: ?Sized> BestFirstVisitor<N, usize, AABB<N>>
125    for CompositeShapeAgainstAnyTOIVisitor<'a, N, G1>
126where
127    N: RealField + Copy,
128    G1: CompositeShape<N>,
129{
130    type Result = TOI<N>;
131
132    #[inline]
133    fn visit(
134        &mut self,
135        best: N,
136        bv: &AABB<N>,
137        data: Option<&usize>,
138    ) -> BestFirstVisitStatus<N, Self::Result> {
139        // Compute the minkowski sum of the two AABBs.
140        let msum = AABB::new(
141            bv.mins + self.msum_shift + (-self.msum_margin),
142            bv.maxs + self.msum_shift + self.msum_margin,
143        );
144
145        // Compute the TOI.
146        if let Some(toi) = msum.toi_with_ray(&Isometry::identity(), &self.ray, self.max_toi, true) {
147            if toi > self.max_toi {
148                return BestFirstVisitStatus::Stop;
149            }
150
151            let mut res = BestFirstVisitStatus::Continue {
152                cost: toi,
153                result: None,
154            };
155
156            if let Some(b) = data {
157                if toi < best {
158                    self.g1.map_part_at(*b, self.m1, &mut |m1, g1| {
159                        if let Some(toi) = self
160                            .dispatcher
161                            .time_of_impact(
162                                self.dispatcher,
163                                m1,
164                                self.vel1,
165                                g1,
166                                self.m2,
167                                self.vel2,
168                                self.g2,
169                                self.max_toi,
170                                self.target_distance,
171                            )
172                            .unwrap_or(None)
173                        {
174                            if toi.toi > self.max_toi {
175                                res = BestFirstVisitStatus::Stop;
176                            } else {
177                                res = BestFirstVisitStatus::Continue {
178                                    cost: toi.toi,
179                                    result: Some(toi),
180                                }
181                            }
182                        }
183                    });
184                }
185            }
186
187            res
188        } else {
189            BestFirstVisitStatus::Stop
190        }
191    }
192}
ncollide3d/query/time_of_impact/time_of_impact_composite_shape_shape.rs

ncollide3d/query/time_of_impact/
time_of_impact_composite_shape_shape.rs
