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
use na::RealField;
use crate::math::{Isometry, Vector};
use crate::query::{Ray, RayCast, TOIStatus, TOI};
use crate::shape::{Plane, SupportMap};
pub fn time_of_impact_plane_support_map<N, G: ?Sized>(
mplane: &Isometry<N>,
vel_plane: &Vector<N>,
plane: &Plane<N>,
mother: &Isometry<N>,
vel_other: &Vector<N>,
other: &G,
max_toi: N,
target_distance: N,
) -> Option<TOI<N>>
where
N: RealField,
G: SupportMap<N>,
{
let vel = *vel_other - *vel_plane;
let plane_normal = mplane * plane.normal;
let support_point = other.support_point(mother, &-plane_normal);
let closest_point = support_point - *plane_normal * target_distance;
let ray = Ray::new(closest_point, vel);
if let Some(toi) = plane.toi_with_ray(mplane, &ray, max_toi, true) {
if toi > max_toi {
return None;
}
let status;
let witness1 = mother.inverse_transform_point(&support_point);
let mut witness2 = mplane.inverse_transform_point(&ray.point_at(toi));
if (support_point.coords - mplane.translation.vector).dot(&plane_normal) < N::zero() {
status = TOIStatus::Penetrating
} else {
witness2 = witness2 - *plane.normal * witness2.coords.dot(&plane.normal);
status = TOIStatus::Converged
}
Some(TOI {
toi,
normal1: plane.normal,
normal2: mother.inverse_transform_unit_vector(&-plane_normal),
witness1,
witness2,
status,
})
} else {
None
}
}
pub fn time_of_impact_support_map_plane<N, G: ?Sized>(
mother: &Isometry<N>,
vel_other: &Vector<N>,
other: &G,
mplane: &Isometry<N>,
vel_plane: &Vector<N>,
plane: &Plane<N>,
max_toi: N,
target_distance: N,
) -> Option<TOI<N>>
where
N: RealField,
G: SupportMap<N>,
{
time_of_impact_plane_support_map(
mplane,
vel_plane,
plane,
mother,
vel_other,
other,
max_toi,
target_distance,
)
.map(|toi| toi.swapped())
}