bgjk/
lib.rs

1#![cfg_attr(feature = "dev", allow(unstable_features))]
2#![cfg_attr(feature = "dev", feature(plugin))]
3#![cfg_attr(feature = "dev", plugin(clippy))]
4#![deny(missing_docs)]
5//! Defines 3-space and implements the boolean GJK (BGJK) algorithm
6//! for intersection testing.
7use std::ops::{Neg, Sub};
8
9/// Vector for use in the `bgjk` function
10///
11/// Uses cartesian spatial dimensions in the order
12/// x, y, z.
13#[derive(Clone, Copy, Debug, Default)]
14pub struct Vec3(pub f32, pub f32, pub f32);
15
16impl Eq for Vec3 {}
17
18impl PartialEq for Vec3 {
19	fn eq(&self, other: &Vec3) -> bool {
20		self.0 == other.0 && self.1 == other.1 && self.2 == other.2
21	}
22}
23
24impl Sub for Vec3 {
25	type Output = Vec3;
26	fn sub(self, right: Vec3) -> Self::Output {
27		Vec3(self.0 - right.0, self.1 - right.1, self.2 - right.2)
28	}
29}
30
31impl Vec3 {
32	fn dot(&self, right: Vec3) -> f32 {
33		self.0 * right.0 + self.1 * right.1 + self.2 * right.2
34	}
35
36	fn ones() -> Vec3 {
37		Vec3(1.0, 1.0, 1.0)
38	}
39}
40
41impl Neg for Vec3 {
42	type Output = Vec3;
43	fn neg(self) -> Self::Output {
44		Vec3(-self.0, -self.1, -self.2)
45	}
46}
47
48/// The BGJK algorithm
49///
50/// The Boolean-GJK algorithm gives us the answer to the question:
51/// "do these convex hulls intersect?"
52/// This algorithm takes two hulls. The ordering of the points is not
53/// important. All points are assumed to be on the surface of the hull.
54/// Having interior points should not affect the qualitative result of
55/// the algorithm, but may cause slight (very minor) degradation in
56/// performance. The algorithm is O(n+m), where n and m are the amount
57/// of points in hull1 and hull2 respectively.
58pub fn bgjk(hull1: &[Vec3], hull2: &[Vec3]) -> bool {
59	let mut sp = Vec3::ones();
60	let mut dp = Vec3::default();
61	let (mut ap, mut bp, mut cp);
62
63	cp = support(hull1, hull2, sp);
64	sp = -cp;
65	bp = support(hull1, hull2, sp);
66	if bp.dot(sp) < 0.0 {
67		return false;
68	}
69	sp = dcross3(cp - bp, -bp);
70	let mut w = 2;
71
72	loop {
73		ap = support(hull1, hull2, sp);
74		if ap.dot(sp) < 0.0 {
75			return false;
76		} else if simplex(&mut ap, &mut bp, &mut cp, &mut dp, &mut sp, &mut w) {
77			return true;
78		}
79	}
80}
81
82// Todo clean up signature, this has to be fixed, sending 6 ptrs...
83fn simplex(ap: &mut Vec3,
84           bp: &mut Vec3,
85           cp: &mut Vec3,
86           dp: &mut Vec3,
87           sp: &mut Vec3,
88           w: &mut i32)
89           -> bool {
90	let ao = -*ap;
91	let mut ab = *bp - *ap;
92	let mut ac = *cp - *ap;
93	let mut abc = cross(ab, ac);
94	match *w {
95		2 => {
96			let ab_abc = cross(ab, abc);
97			if ab_abc.dot(ao) > 0.0 {
98				*cp = *bp;
99				*bp = *ap;
100				*sp = dcross3(ab, ao);
101			} else {
102				let abc_ac = cross(abc, ac);
103				if abc_ac.dot(ao) > 0.0 {
104					*bp = *ap;
105					*sp = dcross3(ac, ao);
106				} else {
107					if abc.dot(ao) > 0.0 {
108						*dp = *cp;
109						*cp = *bp;
110						*bp = *ap;
111						*sp = abc;
112					} else {
113						*dp = *bp;
114						*bp = *ap;
115						*sp = -abc;
116					}
117					*w = 3;
118				}
119			}
120			false
121		}
122		3 => {
123			macro_rules! check_tetrahedron {
124				() => { check_tetra(Tetra(ap, bp, cp, dp), sp, w, ao, ab, ac, abc); };
125			};
126			if abc.dot(ao) > 0.0 {
127				check_tetrahedron![];;
128				false
129			} else {
130				let ad = *dp - *ap;
131				let acd = cross(ac, ad);
132				if acd.dot(ao) > 0.0 {
133					*bp = *cp;
134					*cp = *dp;
135					ab = ac;
136					ac = ad;
137					abc = acd;
138					check_tetrahedron![];;
139					false
140				} else {
141					let adb = cross(ad, ab);
142					if adb.dot(ao) > 0.0 {
143						*cp = *bp;
144						*bp = *dp;
145						ac = ab;
146						ab = ad;
147						abc = adb;
148						check_tetrahedron![];;
149						false
150					} else {
151						true
152					}
153				}
154			}
155		}
156		_ => false,
157	}
158}
159
160struct Tetra<'a>(&'a mut Vec3, &'a mut Vec3, &'a mut Vec3, &'a mut Vec3);
161
162fn check_tetra(te: Tetra, sp: &mut Vec3, w: &mut i32, ao: Vec3, ab: Vec3, ac: Vec3, abc: Vec3) {
163	let ab_abc = cross(ab, abc);
164	if ab_abc.dot(ao) > 0.0 {
165		*te.2 = *te.1;
166		*te.1 = *te.0;
167		*sp = dcross3(ab, ao);
168		*w = 2;
169	} else {
170		let acp = cross(abc, ac);
171		if acp.dot(ao) > 0.0 {
172			*te.1 = *te.0;
173			*sp = dcross3(ac, ao);
174			*w = 2;
175		} else {
176			*te.3 = *te.2;
177			*te.2 = *te.1;
178			*te.1 = *te.0;
179			*sp = abc;
180			*w = 3;
181		}
182	}
183}
184
185fn cross(a: Vec3, b: Vec3) -> Vec3 {
186	Vec3(a.1 * b.2 - a.2 * b.1,
187	     a.2 * b.0 - a.0 * b.2,
188	     a.0 * b.1 - a.1 * b.0)
189}
190
191fn cross3(a: Vec3, b: Vec3, c: Vec3) -> Vec3 {
192	cross(cross(a, b), c)
193}
194
195fn dcross3(a: Vec3, b: Vec3) -> Vec3 {
196	cross3(a, b, a)
197}
198
199fn farthest(vertices: &[Vec3], direction: Vec3) -> Vec3 {
200	let mut max: Option<f32> = None;
201	let mut max_vertex = Vec3::default();
202	for vertex in vertices {
203		let current = vertex.dot(direction);
204		if let Some(value) = max {
205			if current > value {
206				max = Some(current);
207				max_vertex = *vertex;
208			}
209		} else {
210			max = Some(current);
211			max_vertex = *vertex;
212		}
213	}
214	max_vertex
215}
216
217fn support(vertices_a: &[Vec3], vertices_b: &[Vec3], direction: Vec3) -> Vec3 {
218	farthest(vertices_a, direction) - farthest(vertices_b, -direction)
219}
220
221
222#[cfg(test)]
223mod tests {
224
225	use std::f32;
226	use std::f32::consts::PI;
227	use super::{Vec3, bgjk};
228	static EPS: f32 = f32::EPSILON;
229
230	macro_rules! pts {
231		($($e:expr),*) => {
232			[$(
233				Vec3($e.0, $e.1, $e.2)
234			),*]
235		};
236	}
237
238	#[test]
239	fn square1() {
240		let shape1 = pts![(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (1.0, 1.0, 0.0)];
241		let shape2 = pts![(-2.0, 0.0, 0.0), (-3.0, 0.0, 0.0), (-2.0, 1.0, 0.0), (-3.0, 1.0, 0.0)];
242		assert_eq![bgjk(&shape1, &shape2), false];
243	}
244
245	#[test]
246	fn exact_overlap() {
247		let shape1 = pts![(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (1.0, 1.0, 0.0)];
248		let shape2 = pts![(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (1.0, 1.0, 0.0)];
249		assert_eq![bgjk(&shape1, &shape2), true];
250	}
251
252	#[test]
253	fn line_overlap() {
254		let shape1 = pts![(0.0, 0.0, 0.0), (1.0, 0.0, 0.0)];
255		let shape2 = pts![(0.5, 1.0, 0.0), (0.5, -1.0, 0.0)];
256		assert_eq![bgjk(&shape1, &shape2), true];
257	}
258
259	#[test]
260	fn line_non_overlap() {
261		let shape1 = pts![(0.0, 0.0, 0.0), (1.0, 0.0, 0.0)];
262		let shape2 = pts![(1.5, 1.0, 0.0), (1.5, -1.0, 0.0)];
263		assert_eq![bgjk(&shape1, &shape2), false];
264	}
265
266	#[test]
267	fn small_line_point_overlap() {
268		let shape1 = pts![(0.0, 0.0, 0.0), (0.01, 0.0, 0.0)];
269		let shape2 = pts![(0.005, 0.0, 0.1)];
270		assert_eq![bgjk(&shape1, &shape2), false];
271	}
272
273	#[test]
274	fn line_point_non_overlap() {
275		let shape1 = pts![(0.0, 0.0, 0.0), (1.0, 0.0, 0.0)];
276		let shape2 = pts![(0.5, 0.0, 0.1)];
277		assert_eq![bgjk(&shape1, &shape2), false];
278	}
279
280	#[test]
281	fn point_overlap() {
282		let shape1 = pts![(0.5, 1.0, 0.0)];
283		let shape2 = pts![(0.5, 1.0, 0.0)];
284		assert_eq![bgjk(&shape1, &shape2), true];
285	}
286
287	#[test]
288	fn point_no_overlap() {
289		let shape1 = pts![(0.5, 1.0, 0.0)];
290		let shape2 = pts![(1.0, 1.0, 0.0)];
291		assert_eq![bgjk(&shape1, &shape2), false];
292	}
293
294	#[test]
295	fn empty_no_overlap() {
296		// An empty set defaults to a single point in origo in the set
297		let shape1: [Vec3; 0] = pts![];
298		let shape2 = pts![(1.0, 1.0, 1.0)];
299		assert_eq![bgjk(&shape1, &shape2), false];
300	}
301
302	#[test]
303	fn side_by_side_squares() {
304		let shape1 = pts![(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (1.0, 1.0, 0.0)];
305		let shape2 = pts![(1.0, 0.0, 0.0), (2.0, 0.0, 0.0), (1.0, 1.0, 0.0), (2.0, 1.0, 0.0)];
306		assert_eq![bgjk(&shape1, &shape2), true];
307	}
308
309	#[test]
310	fn side_by_side_squares_offset() {
311		let shape1 = pts![(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (1.0, 1.0, 0.0)];
312		let shape2 =
313			pts![(1.0 + EPS, 0.0, 0.0), (2.0, 0.0, 0.0), (1.0 + EPS, 1.0, 0.0), (2.0, 1.0, 0.0)];
314		assert_eq![bgjk(&shape1, &shape2), false];
315	}
316
317	#[test]
318	fn single_point_square_overlap() {
319		let shape1 = pts![(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (1.0, 1.0, 0.0)];
320		let shape2 = pts![(1.0, 1.0, 0.0), (2.0, 1.0, 0.0), (1.0, 2.0, 0.0), (2.0, 2.0, 0.0)];
321		assert_eq![bgjk(&shape1, &shape2), true];
322	}
323
324	#[test]
325	fn single_point_shape_overlap() {
326		let shape1 = pts![(0.0, 0.0, 0.0),
327		                 (1.0, 0.0, 0.0),
328		                 (0.0, 1.0, 0.0),
329		                 (1.0, 1.0, 0.0),
330		                 (0.0, 0.0, 1.0),
331		                 (1.0, 0.0, 1.0),
332		                 (0.0, 1.0, 1.0),
333		                 (1.0, 1.0, 1.0)];
334		let shape2 = pts![(1.0, 1.0, 1.0),
335		                 (2.0, 1.0, 1.0),
336		                 (1.0, 2.0, 1.0),
337		                 (2.0, 2.0, 1.0),
338		                 (1.0, 1.0, 2.0),
339		                 (2.0, 1.0, 2.0),
340		                 (1.0, 2.0, 2.0),
341		                 (2.0, 2.0, 2.0)];
342		assert_eq![bgjk(&shape1, &shape2), true];
343	}
344
345	#[test]
346	fn single_point_shape_non_overlap() {
347		let shape1 = pts![(0.0, 0.0, 0.0),
348		                 (1.0, 0.0, 0.0),
349		                 (0.0, 1.0, 0.0),
350		                 (1.0, 1.0, 0.0),
351		                 (0.0, 0.0, 1.0),
352		                 (1.0, 0.0, 1.0),
353		                 (0.0, 1.0, 1.0),
354		                 (1.0, 1.0, 1.0)];
355		let shape2 = pts![(1.0, 1.0, 1.0 + EPS),
356		                 (2.0, 1.0, 1.0 + EPS),
357		                 (1.0, 2.0, 1.0 + EPS),
358		                 (2.0, 2.0, 1.0 + EPS),
359		                 (1.0, 1.0, 2.0),
360		                 (2.0, 1.0, 2.0),
361		                 (1.0, 2.0, 2.0),
362		                 (2.0, 2.0, 2.0)];
363		assert_eq![bgjk(&shape1, &shape2), false];
364	}
365
366	#[test]
367	fn single_line_shape_overlap() {
368		let shape1 = pts![(0.0, 0.0, 0.0),
369		                 (1.0, 0.0, 0.0),
370		                 (0.0, 1.0, 0.0),
371		                 (1.0, 1.0, 0.0),
372		                 (0.0, 0.0, 1.0),
373		                 (1.0, 0.0, 1.0),
374		                 (0.0, 1.0, 1.0),
375		                 (1.0, 1.0, 1.0)];
376		let shape2 = pts![(1.0, 1.0, 0.0),
377		                 (2.0, 1.0, 0.0),
378		                 (1.0, 2.0, 0.0),
379		                 (2.0, 2.0, 0.0),
380		                 (1.0, 1.0, 1.0),
381		                 (2.0, 1.0, 1.0),
382		                 (1.0, 2.0, 1.0),
383		                 (2.0, 2.0, 1.0)];
384		assert_eq![bgjk(&shape1, &shape2), true];
385	}
386
387	#[test]
388	fn shape_projective_non_overlap() {
389		let shape1 = pts![(0.0, 0.0, 0.0),
390		                 (1.0, 0.0, 0.0),
391		                 (0.0, 1.0, 0.0),
392		                 (1.0, 1.0, 0.0),
393		                 (1.0, 0.0, 1.0),
394		                 (2.0, 0.0, 1.0),
395		                 (1.0, 1.0, 1.0),
396		                 (2.0, 1.0, 1.0)];
397		let shape2 = pts![(1.1, 1.0, 0.0),
398		                 (2.1, 1.0, 0.0),
399		                 (1.1, 2.0, 0.0),
400		                 (2.1, 2.0, 0.0),
401		                 (2.1, 1.0, 1.0),
402		                 (3.1, 1.0, 1.0),
403		                 (2.1, 2.0, 1.0),
404		                 (3.1, 2.0, 1.0)];
405		assert_eq![bgjk(&shape1, &shape2), false];
406	}
407
408	#[test]
409	fn shape_projective_overlap() {
410		let shape1 = pts![(0.0, 0.0, 0.0),
411		                 (1.0, 0.0, 0.0),
412		                 (0.0, 1.0, 0.0),
413		                 (1.0, 1.0, 0.0),
414		                 (1.0, 0.0, 1.0),
415		                 (2.0, 0.0, 1.0),
416		                 (1.0, 1.0, 1.0),
417		                 (2.0, 1.0, 1.0)];
418		let shape2 = pts![(1.1, 1.0, 0.0),
419		                 (2.1, 1.0, 0.0),
420		                 (1.1, 2.0, 0.0),
421		                 (2.1, 2.0, 0.0),
422		                 (2.0, 1.0, 1.0),
423		                 (3.1, 1.0, 1.0),
424		                 (2.0, 2.0, 1.0),
425		                 (3.1, 2.0, 1.0)];
426		assert_eq![bgjk(&shape1, &shape2), true];
427	}
428
429	#[test]
430	fn shape_non_overlap() {
431		let (mut shape1, mut shape2) = (vec![], vec![]);
432		let units = 100;
433		shape1.reserve(units);
434		shape2.reserve(units);
435		for i in 0..units {
436			let radian = i as f32 / units as f32 * 2.0 * PI;
437			shape1.push(Vec3(radian.cos(), radian.sin(), 0.0));
438			shape2.push(Vec3(radian.cos(), radian.sin(), EPS));
439		}
440		assert_eq![bgjk(&shape1, &shape2), false];
441	}
442
443	#[test]
444	fn shape_overlap() {
445		let (mut shape1, mut shape2) = (vec![], vec![]);
446		let units = 100;
447		shape1.reserve(units);
448		shape2.reserve(units);
449		for i in 0..units {
450			let radian = i as f32 / units as f32 * 2.0 * PI;
451			shape1.push(Vec3(radian.cos(), radian.sin(), 0.0));
452			shape2.push(Vec3(radian.cos(), radian.sin(), 0.0));
453		}
454		assert_eq![bgjk(&shape1, &shape2), true];
455	}
456
457	#[test]
458	fn shape_section() {
459		let (mut shape1, mut shape2) = (vec![], vec![]);
460		let units = 100;
461		shape1.reserve(units);
462		shape2.reserve(units);
463		for i in 0..units {
464			let radian = i as f32 / units as f32 * 2.0 * PI;
465			shape1.push(Vec3(radian.cos(), radian.sin(), 0.0));
466			shape2.push(Vec3(radian.cos() + 0.5, radian.sin(), 0.0));
467		}
468		assert_eq![bgjk(&shape1, &shape2), true];
469	}
470
471	#[test]
472	fn shape_away() {
473		let (mut shape1, mut shape2) = (vec![], vec![]);
474		let units = 100;
475		shape1.reserve(units);
476		shape2.reserve(units);
477		for i in 0..units {
478			let radian = i as f32 / units as f32 * 2.0 * PI;
479			shape1.push(Vec3(radian.cos(), radian.sin(), 0.0));
480			shape2.push(Vec3(radian.cos() + 2.0 + 2.0 * EPS, radian.sin(), 0.0));
481		}
482		assert_eq![bgjk(&shape1, &shape2), false];
483	}
484
485}
bgjk/lib.rs

bgjk/
lib.rs
