-
Notifications
You must be signed in to change notification settings - Fork 1
/
GJK.py
123 lines (93 loc) · 4.35 KB
/
GJK.py
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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
###############################################################################
#
# Implementation of the Gilbert–Johnson–Keerthi distance algorithm.
# See:
# * https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm
# * the original paper: A fast procedure for computing the distance between complex objects
# in three-dimensional space, E.G. Gilbert ; D.W. Johnson ; S.S. Keerthi
# * Real-Time Collision Detection, Christer Ericson
#
#
import numpy as np
import itertools
###############################################################################
# Support function for a convex shape:
def support(poly, direction):
dot_products = poly.dot(direction)
idx = np.argmax(dot_products)
return poly[idx]
###############################################################################
# Find the closest point to the origin in the affine hull of the simplex.
# The affine hull is much larger than the convex hull because it allows negative coefficients,
# so the closest point might not be in the simplex. Return whether or not the closest
# point is in the simplex, and the closest point.
#
def simplex_closest_point(simplex, tol=1e-6):
if simplex.shape[0] == 1:
return True, simplex[0]
x0 = simplex[0]
d = simplex[1:] - x0
D = d.dot(d.T)
b = -d.dot(x0)
a = np.linalg.solve(D, b)
min = np.min(a)
sum = np.sum(a)
in_face = min > -tol and sum < 1 + tol
p = x0 + a.dot(d)
return in_face, p
###############################################################################
# Given a list of points, find the point on or in the simplex closest to the origin (it will only
# be inside if the simplex contains the origin).
# If the closest point is not the origin, return closest point and the the smallest subset if the origin
# simplex so that the simplex made by the subset contains the closest point.
# The original points are assumed to be a in tuple, which is preserved (this is done so
# we can track the points in the original shapes).
#
def update_simplex(simplex_list, tol=1e-6):
nump_points = len(simplex_list)
best_p = None
# To find the smallest sub-simplex with the closest point, we iterate over all possible
# subsets, starting with the smallest, and keep the one that produces the smallest magnitude.
#
for i in range(1, nump_points + 1):
for sub_tuple in itertools.combinations(simplex_list, i):
sub_simplex = np.vstack([t[0] for t in sub_tuple])
in_face, p = simplex_closest_point(sub_simplex, tol)
if in_face:
d2 = p.dot(p)
if best_p is None or d2 < best_d2:
best_d2 = d2
best_p = p
best_sub_tuple = sub_tuple
# Since we test 0-d simplices (points), there should alway be a best point:
assert (best_p is not None)
return best_p, list(best_sub_tuple)
def support_point(direction, poly_A, poly_B):
a = support(poly_A, direction)
b = support(poly_B, -direction)
return a - b, (a, b)
###############################################################################
# Test to convex shapes for intersection. Return True if they intersect.
# If they don't intersect, return the closest distance between them, along
# with a list of tuple with points in each shape that form the closest features.
# Depending on the situation there might be duplications.
#
def GJK(poly_A, poly_B, epsilon=1e-6):
dimension = poly_A.shape[1]
initial_dir = np.ones(dimension)
p, sups = support_point(initial_dir, poly_A, poly_B)
simplex_list = [(p, sups)]
while True:
# This is the termination condition, which is not trivial to understand. See eqn 25
# (and the proof in Appendix 1) in the original GJK paper for an
# explanation:
next, next_sups = support_point(-p, poly_A, poly_B)
dp = p.dot(p) - next.dot(p)
if dp < epsilon * epsilon:
return False, np.linalg.norm(p), [t[1] for t in simplex_list]
simplex_list.append((next, next_sups))
p, new_simplex_list = update_simplex(simplex_list)
pl2 = p.dot(p)
if pl2 < epsilon * epsilon:
return True, 0., None
simplex_list = new_simplex_list