1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155

use na::{RealField, Unit};

use crate::math::{Isometry, Point, Vector};
use crate::query::{self, TOIDispatcher, Unsupported};
use crate::shape::{Ball, Plane, Shape};

/// The status of the time-of-impact computation algorithm.
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub enum TOIStatus {
    /// The TOI algorithm ran out of iterations before achieving convergence.
    ///
    /// If this happens, the content of the `NonlinearTOI` will still be a conservative approximation
    /// of the actual result so it is often fine to interpret this case as a success.
    OutOfIterations,
    /// The TOI algorithm converged successfully.
    Converged,
    /// Something went wrong during the TOI computation, likely due to numerical instabilities.
    ///
    /// If this happens, the content of the `NonlinearTOI` will still be a conservative approximation
    /// of the actual result so it is often fine to interpret this case as a success.
    Failed,
    /// The two shape already overlap at the time 0.
    ///
    /// If this happens, the witness points provided by the `NonlinearTOI` will be invalid.
    Penetrating,
}

/// The result of a time-of-impact (TOI) computation.
#[derive(Clone, Debug)]
pub struct TOI<N: RealField> {
    /// The time at which the objects touch.
    pub toi: N,
    /// The local-space closest point on the first shape at the time of impact.
    pub witness1: Point<N>,
    /// The local-space closest point on the second shape at the time of impact.
    pub witness2: Point<N>,
    /// The local-space outward normal on the first shape at the time of impact.
    pub normal1: Unit<Vector<N>>,
    /// The local-space outward normal on the second shape at the time of impact.
    pub normal2: Unit<Vector<N>>,
    /// The way the time-of-impact computation algorithm terminated.
    pub status: TOIStatus,
}

impl<N: RealField> TOI<N> {
    /// Swaps every data of this TOI result such that the role of both shapes are inverted.
    ///
    /// In practice, this makes it so that `self.witness1` and `self.normal1` become `self.witness2` and `self.normal2` and vice-versa.
    pub fn swapped(self) -> Self {
        Self {
            toi: self.toi,
            witness1: self.witness2,
            witness2: self.witness1,
            normal1: self.normal2,
            normal2: self.normal1,
            status: self.status,
        }
    }
}

/// Computes the smallest time at with two shapes under translational movement are separated by a
/// distance smaller or equal to `distance`.
///
/// Returns `0.0` if the objects are touching or penetrating.
pub fn time_of_impact<N: RealField>(
    dispatcher: &dyn TOIDispatcher<N>,
    m1: &Isometry<N>,
    vel1: &Vector<N>,
    g1: &dyn Shape<N>,
    m2: &Isometry<N>,
    vel2: &Vector<N>,
    g2: &dyn Shape<N>,
    max_toi: N,
    target_distance: N,
) -> Result<Option<TOI<N>>, Unsupported> {
    if let (Some(b1), Some(b2)) = (g1.as_shape::<Ball<N>>(), g2.as_shape::<Ball<N>>()) {
        let p1 = Point::from(m1.translation.vector);
        let p2 = Point::from(m2.translation.vector);

        Ok(
            query::time_of_impact_ball_ball(&p1, vel1, b1, &p2, vel2, b2, max_toi, target_distance)
                .map(|toi| {
                    // We have to transform back the points and vectors in the sphere's local space since
                    // the time_of_impact_ball_ball did not take rotation into account.
                    TOI {
                        toi: toi.toi,
                        witness1: m1.rotation.inverse_transform_point(&toi.witness1),
                        witness2: m2.rotation.inverse_transform_point(&toi.witness2),
                        normal1: m1.inverse_transform_unit_vector(&toi.normal1),
                        normal2: m2.inverse_transform_unit_vector(&toi.normal2),
                        status: toi.status,
                    }
                }),
        )
    } else if let (Some(p1), Some(s2)) = (g1.as_shape::<Plane<N>>(), g2.as_support_map()) {
        Ok(query::time_of_impact_plane_support_map(
            m1,
            vel1,
            p1,
            m2,
            vel2,
            s2,
            max_toi,
            target_distance,
        ))
    } else if let (Some(s1), Some(p2)) = (g1.as_support_map(), g2.as_shape::<Plane<N>>()) {
        Ok(query::time_of_impact_support_map_plane(
            m1,
            vel1,
            s1,
            m2,
            vel2,
            p2,
            max_toi,
            target_distance,
        ))
    } else if let (Some(s1), Some(s2)) = (g1.as_support_map(), g2.as_support_map()) {
        Ok(query::time_of_impact_support_map_support_map(
            m1,
            vel1,
            s1,
            m2,
            vel2,
            s2,
            max_toi,
            target_distance,
        ))
    } else if let Some(c1) = g1.as_composite_shape() {
        Ok(query::time_of_impact_composite_shape_shape(
            dispatcher,
            m1,
            vel1,
            c1,
            m2,
            vel2,
            g2,
            max_toi,
            target_distance,
        ))
    } else if let Some(c2) = g2.as_composite_shape() {
        Ok(query::time_of_impact_shape_composite_shape(
            dispatcher,
            m1,
            vel1,
            g1,
            m2,
            vel2,
            c2,
            max_toi,
            target_distance,
        ))
    } else {
        Err(Unsupported)
    }
}