I'm trying to design my own physics engine from scratch, as well as the vector/matrix libraries. Everything worked beautifully so far, until I tried to implement collision detection in my library. First with SAT, worked great for detecting, but I wanted to find the distance between the objects as well. Then I tried to implement the GJK distance algorithm, just to see if I can find the distance between the origin and a polygon. But it just doesn't work, the smallest distance perceived by the algorithm that I implemented was one of the vertex of the polygon:
I know I made the other libraries from scratch, but I'm positive that they are working. Anyways, here's the code where I've implemented the GJK:
#objectL[0] is a hexagon
v = objectL[0].nodes[0]
W = []
u = 0
close_enough = False
while not close_enough and v != Vector(0,0):
w = objectL[0].support(-v)
d = v*w/abs(v) #*:dot product abs:magnitude
u = max(u,d)
close_enough = abs(v) - u <= 0.0001
if not close_enough:
W.append(w)
while len(W)>2:
del W[0]
v = Vector(0,0).vectorToLine(*W) #distance from the origin to the simplex
#formed by W
And now the support method:
def support(self,axis):
maxP = self.nodes[0]*axis #dot product of first vertex with the axis
n = self.nodes[0]
for node in self.nodes[1:]:
p = node*axis
if p>maxP:
maxP = p
n = node
return node
Those are the code snippets, that I think is where the error is, but I can't find it. The GJK algorithm I've copied from here. Thanks!
Edit: Here is my project(implemented in pygame)
Ok, found the error. Which wasn't on the implementation, but rather on the functions that I've previously made: the support, which returned node
instead of n
and the vectorToLine
function which returned an incorrect vector(negative value).
Also for those that are reading this post years from now, and trying to implement this algorithm, please note that I only changed the while len(W)>2
part to:
while len(W)>2:
maxD = 0
for w in W:
if abs(w)>maxD:
maxD = w
W.remove(maxD)
Which removes the farthest point of the simplex/triangle, so it gets the two closest point(to the origin) to continue the algorithm.