CODEWITHSUNDEEP
pythonphysics
Ray
Ray

Reputation: 341

GJK algorithm creates simplex with two opposite points

I am making a physics engine, and I am using the GJK algorithm. Right now I have written it based off this blog post, and here is my code:

class CollManager:
    @staticmethod
    def supportPoint(a, b, direction):
        return a.supportPoint(direction) - \
            b.supportPoint(-direction)

    @staticmethod
    def nextSimplex(args):
        length = len(args[0])
        if length == 2:
            return CollManager.lineSimplex(args)
        if length == 3:
            return CollManager.triSimplex(args)
        if length == 4:
            return CollManager.tetraSimplex(args)
        return False
    
    @staticmethod
    def lineSimplex(args):
        a, b = args[0]
        ab = b - a
        ao = -a
        if ab.dot(ao) > 0:
            args[1] = ab.cross(ao).cross(ab)
        else:
            args[0] = [a]
            args[1] = ao
    
    @staticmethod
    def triSimplex(args):
        a, b, c = args[0]
        ab = b - a
        ac = c - a
        ao = -a
        abc = ab.cross(ac)
        if abc.cross(ac).dot(ao) > 0:
            if ac.dot(ao) > 0:
                args[0] = [a, c]
                args[1] = ac.cross(ao).cross(ac)
            else:
                args[0] = [a, b]
                return CollManager.lineSimplex(args)
        elif ab.cross(abc).dot(ao) > 0:
            args[0] = [a, b]
            return CollManager.lineSimplex(args)
        else:
            if abc.dot(ao) > 0:
                args[1] = abc
            else:
                args[0] = [a, c, b]
                args[1] = -abc
        return False
    
    @staticmethod
    def tetraSimplex(args):
        a, b, c, d = args[0]
        ab = b - a
        ac = c - a
        ad = d - a
        ao = -a
        abc = ab.cross(ac)
        acd = ac.cross(ad)
        adb = ad.cross(ab)
        if abc.dot(ao) > 0:
            args[0] = [a, b, c]
            return CollManager.triSimplex(args)
        if acd.dot(ao) > 0:
            args[0] = [a, c, d]
            return CollManager.triSimplex(args)
        if adb.dot(ao) > 0:
            args[0] = [a, d, b]
            return CollManager.triSimplex(args)
        return True

    @staticmethod
    def gjk(a, b):
        ab = a.pos - b.pos
        c = Vector3(ab.z, ab.z, -ab.x - ab.y)
        if c == Vector3.zero():
            c = Vector3(-ab.y - ab.z, ab.x, ab.x)

        support = CollManager.supportPoint(a, b, ab.cross(c))
        points = [support]
        direction = -support
        while True:
            support = CollManager.supportPoint(a, b, direction)
            if support.dot(direction) <= 0:
                return None
            points.insert(0, support)
            args = [points, direction]
            if CollManager.nextSimplex(args):
                return args[0]
            points, direction = args

I have checked that the support point function works as intended, as I have worked out the maths behind it. What i did was I created two cubes, both of side length 2, with positions (0, 0, 0) and (1, 0, 0). The first direction that is calculated is Vector3(0, 1, 0), so the first point on the simplex is Vector3(1, -2, 0). The second point, looking in the other direction, is exactly the opposite, Vector3(-1, 2, 0). This raises a problem, because the next direction that is created uses the cross product of the two, which gives Vector3(0, 0, 0). Obviously no vector is in the same direction as this, so the line if support.dot(direction) <= 0: fails.

However, this happens when x is -1, 0 or 1, but not at -2 and 2.

How can I make sure the second point found on the simplex is not exactly opposite the first, thus making the check return early?

Minimum reproducible example (uses exact same code):

# main.py
from vector3 import Vector3
import math

class BoxCollider:
    def __init__(self, position, side_length):
        self.pos = position
        self.side_length = side_length
    
    def supportPoint(self, direction):
        maxDistance = -math.inf
        min, max = self.pos - self.side_length // 2, self.pos + self.side_length // 2
        for x in (min.x, max.x):
            for y in (min.y, max.y):
                for z in (min.z, max.z):
                    distance = Vector3(x, y, z).dot(direction)
                    if distance > maxDistance:
                        maxDistance = distance
                        maxVertex = Vector3(x, y, z)
        return maxVertex

def supportPoint(a, b, direction):
    return a.supportPoint(direction) - \
        b.supportPoint(-direction)

def nextSimplex(args):
    length = len(args[0])
    if length == 2:
        return lineSimplex(args)
    if length == 3:
        return triSimplex(args)
    if length == 4:
        return tetraSimplex(args)
    return False

def lineSimplex(args):
    a, b = args[0]
    ab = b - a
    ao = -a
    if ab.dot(ao) > 0:
        args[1] = ab.cross(ao).cross(ab)
    else:
        args[0] = [a]
        args[1] = ao

def triSimplex(args):
    a, b, c = args[0]
    ab = b - a
    ac = c - a
    ao = -a
    abc = ab.cross(ac)
    if abc.cross(ac).dot(ao) > 0:
        if ac.dot(ao) > 0:
            args[0] = [a, c]
            args[1] = ac.cross(ao).cross(ac)
        else:
            args[0] = [a, b]
            return lineSimplex(args)
    elif ab.cross(abc).dot(ao) > 0:
        args[0] = [a, b]
        return lineSimplex(args)
    else:
        if abc.dot(ao) > 0:
            args[1] = abc
        else:
            args[0] = [a, c, b]
            args[1] = -abc
    return False

def tetraSimplex(args):
    a, b, c, d = args[0]
    ab = b - a
    ac = c - a
    ad = d - a
    ao = -a
    abc = ab.cross(ac)
    acd = ac.cross(ad)
    adb = ad.cross(ab)
    if abc.dot(ao) > 0:
        args[0] = [a, b, c]
        return triSimplex(args)
    if acd.dot(ao) > 0:
        args[0] = [a, c, d]
        return triSimplex(args)
    if adb.dot(ao) > 0:
        args[0] = [a, d, b]
        return triSimplex(args)
    return True

def gjk(a, b):
    ab = a.pos - b.pos
    c = Vector3(ab.z, ab.z, -ab.x - ab.y)
    if c == Vector3.zero():
        c = Vector3(-ab.y - ab.z, ab.x, ab.x)

    support = supportPoint(a, b, ab.cross(c))
    points = [support]
    direction = -support
    while True:
        support = supportPoint(a, b, direction)
        if support.dot(direction) <= 0:
            return None
        points.insert(0, support)
        args = [points, direction]
        if nextSimplex(args):
            return args[0]
        points, direction = args

a = BoxCollider(Vector3(0, 0, 0), 2)
b = BoxCollider(Vector3(1, 0, 0), 2)
print(gjk(a, b))

My vector3.py file is much too long, here is an equivalent that also works: https://github.com/pyunity/pyunity/blob/07fed7871ace0c1b1b3cf8051d08d6677fe18209/pyunity/vector3.py

Upvotes: 0

Views: 233

Answers (2)

Sir Nate
Sir Nate

Reputation: 399

How can I make sure the second point found on the simplex is not exactly opposite the first, thus making the check return early?

Don't. The 0 search direction is actually a sign that you are done at that simplex (1 simplex line segment) because the origin is collinear with it. Return that you have a collision if the origin was (projected onto the simplex) within it, or that you have no collision if it was outside it. (Phrased in a way that should make it clear how to handle the 2D case with triangles with a coplanar origin as well).

Upvotes: 0

Ray
Ray

Reputation: 341

The fix was to change ab.cross(ao).cross(ab) into just ab / 2, because that gives a vector from the midpoint of AB to the origin, and is much less expensive than two cross products.

Upvotes: 0

Related Questions