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