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

use na::Unit;

use crate::math::{Isometry, Real, Vector};
use crate::query::details;
use crate::query::details::ShapeCastOptions;
use crate::query::gjk::{self, VoronoiSimplex};
use crate::query::{ShapeCastHit, ShapeCastStatus};
use crate::shape::{RoundShapeRef, SupportMap};
use num::Zero;

/// Time of impacts between two support-mapped shapes under translational movement.
pub fn cast_shapes_support_map_support_map<G1, G2>(
    pos12: &Isometry<Real>,
    vel12: &Vector<Real>,
    g1: &G1,
    g2: &G2,
    options: ShapeCastOptions,
) -> Option<ShapeCastHit>
where
    G1: ?Sized + SupportMap,
    G2: ?Sized + SupportMap,
{
    let gjk_result = if options.target_distance > 0.0 {
        let round_g1 = RoundShapeRef {
            inner_shape: g1,
            border_radius: options.target_distance,
        };
        gjk::directional_distance(pos12, &round_g1, g2, vel12, &mut VoronoiSimplex::new())
    } else {
        gjk::directional_distance(pos12, g1, g2, vel12, &mut VoronoiSimplex::new())
    };

    gjk_result.and_then(|(time_of_impact, normal1, witness1, witness2)| {
        if time_of_impact > options.max_time_of_impact {
            None
        } else if (options.compute_impact_geometry_on_penetration || !options.stop_at_penetration)
            && time_of_impact < 1.0e-5
        {
            let contact = details::contact_support_map_support_map(pos12, g1, g2, Real::MAX)?;
            let normal_vel = contact.normal1.dot(vel12);

            if !options.stop_at_penetration && normal_vel >= 0.0 {
                None
            } else {
                Some(ShapeCastHit {
                    time_of_impact,
                    normal1: contact.normal1,
                    normal2: contact.normal2,
                    witness1: contact.point1,
                    witness2: contact.point2,
                    status: ShapeCastStatus::PenetratingOrWithinTargetDist,
                })
            }
        } else {
            Some(ShapeCastHit {
                time_of_impact,
                normal1: Unit::new_unchecked(normal1),
                normal2: Unit::new_unchecked(pos12.inverse_transform_vector(&-normal1)),
                witness1: witness1 - normal1 * options.target_distance,
                witness2: pos12.inverse_transform_point(&witness2),
                status: if time_of_impact.is_zero() {
                    ShapeCastStatus::PenetratingOrWithinTargetDist
                } else {
                    ShapeCastStatus::Converged
                },
            })
        }
    })
}