ncollide3d/query/time_of_impact/time_of_impact.rs
1use na::{RealField, Unit};
2
3use crate::math::{Isometry, Point, Vector};
4use crate::query::{self, TOIDispatcher, Unsupported};
5use crate::shape::{Ball, Plane, Shape};
6
7/// The status of the time-of-impact computation algorithm.
8#[derive(Copy, Clone, Debug, PartialEq, Eq)]
9pub enum TOIStatus {
10 /// The TOI algorithm ran out of iterations before achieving convergence.
11 ///
12 /// If this happens, the content of the `NonlinearTOI` will still be a conservative approximation
13 /// of the actual result so it is often fine to interpret this case as a success.
14 OutOfIterations,
15 /// The TOI algorithm converged successfully.
16 Converged,
17 /// Something went wrong during the TOI computation, likely due to numerical instabilities.
18 ///
19 /// If this happens, the content of the `NonlinearTOI` will still be a conservative approximation
20 /// of the actual result so it is often fine to interpret this case as a success.
21 Failed,
22 /// The two shape already overlap at the time 0.
23 ///
24 /// If this happens, the witness points provided by the `NonlinearTOI` will be invalid.
25 Penetrating,
26}
27
28/// The result of a time-of-impact (TOI) computation.
29#[derive(Clone, Debug)]
30pub struct TOI<N: RealField + Copy> {
31 /// The time at which the objects touch.
32 pub toi: N,
33 /// The local-space closest point on the first shape at the time of impact.
34 pub witness1: Point<N>,
35 /// The local-space closest point on the second shape at the time of impact.
36 pub witness2: Point<N>,
37 /// The local-space outward normal on the first shape at the time of impact.
38 pub normal1: Unit<Vector<N>>,
39 /// The local-space outward normal on the second shape at the time of impact.
40 pub normal2: Unit<Vector<N>>,
41 /// The way the time-of-impact computation algorithm terminated.
42 pub status: TOIStatus,
43}
44
45impl<N: RealField + Copy> TOI<N> {
46 /// Swaps every data of this TOI result such that the role of both shapes are inverted.
47 ///
48 /// In practice, this makes it so that `self.witness1` and `self.normal1` become `self.witness2` and `self.normal2` and vice-versa.
49 pub fn swapped(self) -> Self {
50 Self {
51 toi: self.toi,
52 witness1: self.witness2,
53 witness2: self.witness1,
54 normal1: self.normal2,
55 normal2: self.normal1,
56 status: self.status,
57 }
58 }
59}
60
61/// Computes the smallest time at with two shapes under translational movement are separated by a
62/// distance smaller or equal to `distance`.
63///
64/// Returns `0.0` if the objects are touching or penetrating.
65pub fn time_of_impact<N: RealField + Copy>(
66 dispatcher: &dyn TOIDispatcher<N>,
67 m1: &Isometry<N>,
68 vel1: &Vector<N>,
69 g1: &dyn Shape<N>,
70 m2: &Isometry<N>,
71 vel2: &Vector<N>,
72 g2: &dyn Shape<N>,
73 max_toi: N,
74 target_distance: N,
75) -> Result<Option<TOI<N>>, Unsupported> {
76 if let (Some(b1), Some(b2)) = (g1.as_shape::<Ball<N>>(), g2.as_shape::<Ball<N>>()) {
77 let p1 = Point::from(m1.translation.vector);
78 let p2 = Point::from(m2.translation.vector);
79
80 Ok(
81 query::time_of_impact_ball_ball(&p1, vel1, b1, &p2, vel2, b2, max_toi, target_distance)
82 .map(|toi| {
83 // We have to transform back the points and vectors in the sphere's local space since
84 // the time_of_impact_ball_ball did not take rotation into account.
85 TOI {
86 toi: toi.toi,
87 witness1: m1.rotation.inverse_transform_point(&toi.witness1),
88 witness2: m2.rotation.inverse_transform_point(&toi.witness2),
89 normal1: m1.inverse_transform_unit_vector(&toi.normal1),
90 normal2: m2.inverse_transform_unit_vector(&toi.normal2),
91 status: toi.status,
92 }
93 }),
94 )
95 } else if let (Some(p1), Some(s2)) = (g1.as_shape::<Plane<N>>(), g2.as_support_map()) {
96 Ok(query::time_of_impact_plane_support_map(
97 m1,
98 vel1,
99 p1,
100 m2,
101 vel2,
102 s2,
103 max_toi,
104 target_distance,
105 ))
106 } else if let (Some(s1), Some(p2)) = (g1.as_support_map(), g2.as_shape::<Plane<N>>()) {
107 Ok(query::time_of_impact_support_map_plane(
108 m1,
109 vel1,
110 s1,
111 m2,
112 vel2,
113 p2,
114 max_toi,
115 target_distance,
116 ))
117 } else if let (Some(s1), Some(s2)) = (g1.as_support_map(), g2.as_support_map()) {
118 Ok(query::time_of_impact_support_map_support_map(
119 m1,
120 vel1,
121 s1,
122 m2,
123 vel2,
124 s2,
125 max_toi,
126 target_distance,
127 ))
128 } else if let Some(c1) = g1.as_composite_shape() {
129 Ok(query::time_of_impact_composite_shape_shape(
130 dispatcher,
131 m1,
132 vel1,
133 c1,
134 m2,
135 vel2,
136 g2,
137 max_toi,
138 target_distance,
139 ))
140 } else if let Some(c2) = g2.as_composite_shape() {
141 Ok(query::time_of_impact_shape_composite_shape(
142 dispatcher,
143 m1,
144 vel1,
145 g1,
146 m2,
147 vel2,
148 c2,
149 max_toi,
150 target_distance,
151 ))
152 } else {
153 Err(Unsupported)
154 }
155}