ncollide3d/query/time_of_impact/
time_of_impact_plane_support_map.rs

1use na::RealField;
2
3use crate::math::{Isometry, Vector};
4use crate::query::{Ray, RayCast, TOIStatus, TOI};
5use crate::shape::{Plane, SupportMap};
6
7/// Time Of Impact of a plane with a support-mapped shape under translational movement.
8pub fn time_of_impact_plane_support_map<N, G: ?Sized>(
9    mplane: &Isometry<N>,
10    vel_plane: &Vector<N>,
11    plane: &Plane<N>,
12    mother: &Isometry<N>,
13    vel_other: &Vector<N>,
14    other: &G,
15    max_toi: N,
16    target_distance: N,
17) -> Option<TOI<N>>
18where
19    N: RealField + Copy,
20    G: SupportMap<N>,
21{
22    let vel = *vel_other - *vel_plane;
23    let plane_normal = mplane * plane.normal;
24    // FIXME: add method to get only the local support point.
25    // This would avoid the `inverse_transform_point` later.
26    let support_point = other.support_point(mother, &-plane_normal);
27    let closest_point = support_point - *plane_normal * target_distance;
28    let ray = Ray::new(closest_point, vel);
29
30    if let Some(toi) = plane.toi_with_ray(mplane, &ray, max_toi, true) {
31        if toi > max_toi {
32            return None;
33        }
34
35        let status;
36        let witness1 = mother.inverse_transform_point(&support_point);
37        let mut witness2 = mplane.inverse_transform_point(&ray.point_at(toi));
38
39        if (support_point.coords - mplane.translation.vector).dot(&plane_normal) < N::zero() {
40            status = TOIStatus::Penetrating
41        } else {
42            // Project the witness point to the plane.
43            // Note that witness2 is already in the plane's local-space.
44            witness2 = witness2 - *plane.normal * witness2.coords.dot(&plane.normal);
45            status = TOIStatus::Converged
46        }
47
48        Some(TOI {
49            toi,
50            normal1: plane.normal,
51            normal2: mother.inverse_transform_unit_vector(&-plane_normal),
52            witness1,
53            witness2,
54            status,
55        })
56    } else {
57        None
58    }
59}
60
61/// Time Of Impact of a plane with a support-mapped shape under translational movement.
62pub fn time_of_impact_support_map_plane<N, G: ?Sized>(
63    mother: &Isometry<N>,
64    vel_other: &Vector<N>,
65    other: &G,
66    mplane: &Isometry<N>,
67    vel_plane: &Vector<N>,
68    plane: &Plane<N>,
69    max_toi: N,
70    target_distance: N,
71) -> Option<TOI<N>>
72where
73    N: RealField + Copy,
74    G: SupportMap<N>,
75{
76    time_of_impact_plane_support_map(
77        mplane,
78        vel_plane,
79        plane,
80        mother,
81        vel_other,
82        other,
83        max_toi,
84        target_distance,
85    )
86    .map(|toi| toi.swapped())
87}