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
use crate::math::{Isometry, Real, Vector};
use crate::query::{Ray, RayCast, TOIStatus, TOI};
use crate::shape::{HalfSpace, SupportMap};
pub fn time_of_impact_halfspace_support_map<G: ?Sized>(
pos12: &Isometry<Real>,
vel12: &Vector<Real>,
halfspace: &HalfSpace,
other: &G,
max_toi: Real,
) -> Option<TOI>
where
G: SupportMap,
{
let support_point = other.support_point(pos12, &-halfspace.normal);
let closest_point = support_point;
let ray = Ray::new(closest_point, *vel12);
if let Some(toi) = halfspace.cast_local_ray(&ray, max_toi, true) {
if toi > max_toi {
return None;
}
let status;
let witness2 = support_point;
let mut witness1 = ray.point_at(toi);
if support_point.coords.dot(&halfspace.normal) < 0.0 {
status = TOIStatus::Penetrating
} else {
witness1 = witness1 - *halfspace.normal * witness1.coords.dot(&halfspace.normal);
status = TOIStatus::Converged
}
Some(TOI {
toi,
normal1: halfspace.normal,
normal2: pos12.inverse_transform_unit_vector(&-halfspace.normal),
witness1,
witness2: pos12.inverse_transform_point(&witness2),
status,
})
} else {
None
}
}
pub fn time_of_impact_support_map_halfspace<G: ?Sized>(
pos12: &Isometry<Real>,
vel12: &Vector<Real>,
other: &G,
halfspace: &HalfSpace,
max_toi: Real,
) -> Option<TOI>
where
G: SupportMap,
{
time_of_impact_halfspace_support_map(
&pos12.inverse(),
&-pos12.inverse_transform_vector(&vel12),
halfspace,
other,
max_toi,
)
.map(|toi| toi.swapped())
}