Search code examples
pythonc++algorithmcollision-detectiongame-engine

Can't implement GJK distance algorithm


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:

stack1.png

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)


Solution

  • 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.