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

use na::{RealField, Unit};

use crate::math::{Isometry, Point, Vector};
use crate::query;
use crate::shape::{Ball, Plane, Shape};
use crate::utils::IsometryOps;


/// 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>(
    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,
) -> Option<TOI<N>>
{
    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);

        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()) {
        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>>()) {
        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()) {
        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() {
        query::time_of_impact_composite_shape_shape(m1, vel1, c1, m2, vel2, g2, max_toi, target_distance)
    } else if let Some(c2) = g2.as_composite_shape() {
        query::time_of_impact_shape_composite_shape(m1, vel1, g1, m2, vel2, c2, max_toi, target_distance)
    } else {
        panic!("No algorithm known to compute a contact point between the given pair of shapes.")
    }
}