ncollide3d/query/time_of_impact/
time_of_impact_composite_shape_shape.rs
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
9pub 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
39pub 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 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 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}