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

use crate::bounding_volume::{AABB, BoundingSphere};
use crate::math::Isometry;
use na::{self, RealField};
use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor};
use crate::query::{self, TOI};
use crate::shape::{CompositeShape, Shape, Ball};
use crate::interpolation::{RigidMotion, RigidMotionComposition};

/// Time Of Impact of a composite shape with any other shape, under a rigid motion (translation + rotation).
pub fn nonlinear_time_of_impact_composite_shape_shape<N, G1>(
    motion1: &(impl RigidMotion<N> + ?Sized),
    g1: &G1,
    motion2: &(impl RigidMotion<N> + ?Sized),
    g2: &dyn Shape<N>,
    max_toi: N,
    target_distance: N,
) -> Option<TOI<N>>
where
    N: RealField,
    G1: ?Sized + CompositeShape<N>,
{
    let mut visitor = CompositeShapeAgainstAnyNonlinearTOIVisitor::new(motion1, g1, motion2, g2, max_toi, target_distance);

    g1.bvh().best_first_search(&mut visitor).map(|res| res.1)
}

/// Time Of Impact of any shape with a composite shape, under a rigid motion (translation + rotation).
pub fn nonlinear_time_of_impact_shape_composite_shape<N, G2>(
    motion1: &(impl RigidMotion<N> + ?Sized),
    g1: &dyn Shape<N>,
    motion2: &(impl RigidMotion<N> + ?Sized),
    g2: &G2,
    max_toi: N,
    target_distance: N,
) -> Option<TOI<N>>
where
    N: RealField,
    G2: ?Sized + CompositeShape<N>,
{
    nonlinear_time_of_impact_composite_shape_shape(motion2, g2, motion1, g1, max_toi, target_distance)
}

struct CompositeShapeAgainstAnyNonlinearTOIVisitor<'a, N: 'a + RealField, G1: ?Sized + 'a, M1: ?Sized, M2: ?Sized> {
    sphere2: BoundingSphere<N>,
    max_toi: N,
    target_distance: N,

    motion1: &'a M1,
    g1: &'a G1,
    motion2: &'a M2,
    g2: &'a dyn Shape<N>,
}

impl<'a, N, G1, M1, M2> CompositeShapeAgainstAnyNonlinearTOIVisitor<'a, N, G1, M1, M2>
where
    N: RealField,
    G1: ?Sized + CompositeShape<N>,
    M1: ?Sized + RigidMotion<N>,
    M2: ?Sized + RigidMotion<N>,
{
    pub fn new(
        motion1: &'a M1,
        g1: &'a G1,
        motion2: &'a M2,
        g2: &'a dyn Shape<N>,
        max_toi: N,
        target_distance: N,
    ) -> CompositeShapeAgainstAnyNonlinearTOIVisitor<'a, N, G1, M1, M2>
    {
        CompositeShapeAgainstAnyNonlinearTOIVisitor {
            sphere2: g2.bounding_sphere(&Isometry::identity()),
            max_toi,
            target_distance,
            motion1,
            g1,
            motion2,
            g2,
        }
    }
}

impl<'a, N, G1, M1, M2> BestFirstVisitor<N, usize, AABB<N>>
    for CompositeShapeAgainstAnyNonlinearTOIVisitor<'a, N, G1, M1, M2>
where
    N: RealField,
    G1: ?Sized + CompositeShape<N>,
    M1: ?Sized + RigidMotion<N>,
    M2: ?Sized + RigidMotion<N>,
{
    type Result = TOI<N>;

    #[inline]
    fn visit(&mut self, best: N, bv: &AABB<N>, data: Option<&usize>) -> BestFirstVisitStatus<N, Self::Result> {
        let sphere1 = bv.bounding_sphere();
        let ball1 = Ball::new(sphere1.radius());
        let ball2 = Ball::new(self.sphere2.radius());
        let motion1 = self.motion1.prepend_translation(sphere1.center().coords);
        let motion2 = self.motion2.prepend_translation(self.sphere2.center().coords);

        if let Some(toi) = query::nonlinear_time_of_impact_ball_ball(
            &motion1, &ball1, &motion2, &ball2, self.max_toi, self.target_distance) {
            let mut res = BestFirstVisitStatus::Continue { cost: toi.toi, result: None };


            if let Some(b) = data {
                if toi.toi < best {
                    self.g1.map_part_at(*b, &Isometry::identity(), &mut |m1, g1| {
                        let motion1 = self.motion1.prepend_transformation(*m1);

                        // NOTE: we have to use a trait-object for `&motion1 as &RigidMotion<N>` to avoid infinite
                        // compiler recursion when it monomorphizes query::nonlinear_time_of_impact.
                        if let Some(toi) =
                        query::nonlinear_time_of_impact(&motion1 as &dyn RigidMotion<N>, g1, self.motion2, self.g2, self.max_toi, self.target_distance) {
                            res = BestFirstVisitStatus::Continue { cost: toi.toi, result: Some(toi) }
                        }
                    });
                }
            }

            res
        } else {
            BestFirstVisitStatus::Stop
        }
    }
}