GJK algorithm creates simplex with two opposite points

Question

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

Answer 1

解决方法是将ab.cross(ao).cross(ab)更改为ab / 2 ，因为这给出了从 AB 的中点到原点的向量，并且比两个叉积便宜得多。