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}