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
use i_shape::{IShape, ShapeType};
use mat::Mat3x1;
fn support(a: &dyn IShape, b: &dyn IShape, v: &Mat3x1<f64>) -> Option<Mat3x1<f64>> {
let p0 = match a.get_support(&v) {
Some(o) => o,
_ => return None,
};
let v_oppose = v.scale(-1f64).unwrap();
let p1 = match b.get_support(&v_oppose) {
Some(o) => o,
_ => return None,
};
let p10 = p0.minus(&p1).unwrap();
Some(p10)
}
fn pass_minkowski_origin(last_vert: &Mat3x1<f64>, support: &Mat3x1<f64>) -> bool {
last_vert.dot(&support).unwrap() > 0f64
}
fn contains_minkowski_origin(simplex: &mut Vec<Mat3x1<f64>>, support: &mut Mat3x1<f64>) -> bool {
let a = simplex.last().unwrap().clone();
let ao = a.scale(-1f64).unwrap();
if simplex.len() == 3 {
let b = simplex[1];
let c = simplex[0];
let ab = b.minus(&a).unwrap();
let ac = c.minus(&a).unwrap();
let ab_normal = ac.cross(&ab).unwrap().cross(&ab).unwrap();
let ac_normal = ab.cross(&ac).unwrap().cross(&ac).unwrap();
if ab_normal.dot(&ao).unwrap() > 0f64 {
let simplex_new = vec![simplex[1], simplex[2]];
*simplex = simplex_new;
*support = ab_normal.clone();
} else if ac_normal.dot(&ao).unwrap() > 0f64 {
let simplex_new = vec![simplex[0], simplex[2]];
*simplex = simplex_new.clone();
*support = ac_normal.clone();
} else {
return true;
}
} else {
let b = simplex[0];
let ab = b.minus(&a).unwrap();
let ab_normal = ab.cross(&ao).unwrap().cross(&ao).unwrap();
if ab_normal.magnitude().unwrap() == 0f64 {
return true;
} else {
*support = ab_normal.clone();
}
}
return false;
}
pub fn query_intersect(a: &dyn IShape, b: &dyn IShape) -> Option<bool> {
match (a.get_type(), b.get_type()) {
(ShapeType::Sphere, ShapeType::Sphere) => {}
_ => {
panic!("unsupported shape type");
}
}
let mut d = Mat3x1 {
_val: [-1f64, 0f64, 0f64],
};
let mut simplex = vec![];
{
let sup = support(a, b, &d).unwrap();
simplex.push(sup);
}
d = d.scale(-1f64).unwrap();
loop {
{
let sup = support(a, b, &d).unwrap();
simplex.push(sup);
}
assert!(simplex.len() <= 3, "simplex vertices count unexpected");
if !pass_minkowski_origin(simplex.last().unwrap(), &d) {
return Some(false);
} else {
if contains_minkowski_origin(&mut simplex, &mut d) {
return Some(true);
}
}
}
}