GJK算法创建具有两个相对点的单纯形
GJK algorithm creates simplex with two opposite points
我在做物理引擎，我用的是GJK算法。现在我已经根据 this blog post 编写了它，这是我的代码：
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
我已经检查了支持点函数是否按预期工作，因为我已经计算出它背后的数学原理。我所做的是我创建了两个立方体，边长均为 2，位置为 (0, 0, 0) 和 (1, 0, 0)。计算的第一个方向是 Vector3(0, 1, 0)，因此单纯形上的第一个点是 Vector3(1, -2, 0)。第二点，换个方向看，刚好相反，Vector3(-1, 2, 0)。这就产生了一个问题，因为创建的下一个方向使用了两者的叉积，得到 Vector3(0, 0, 0)。显然没有矢量与此方向相同，因此行 if support.dot(direction) <= 0: 失败。
但是，当 x 为 -1、0 或 1 时会发生这种情况，但不会发生在 -2 和 2 时。
如何确保在单纯形上找到的第二个点与第一个点不完全相反，从而提前进行检查 return？
最小可重现示例（使用完全相同的代码）：
# 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))
我的 vector3.py 文件太长了，下面是一个同样有效的等效文件：https://github.com/pyunity/pyunity/blob/07fed7871ace0c1b1b3cf8051d08d6677fe18209/pyunity/vector3.py
解决方法是将 ab.cross(ao).cross(ab) 更改为 ab / 2，因为这给出了从 AB 的中点到原点的向量，并且比两个叉积要便宜得多。
我在做物理引擎，我用的是GJK算法。现在我已经根据 this blog post 编写了它，这是我的代码：
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
我已经检查了支持点函数是否按预期工作，因为我已经计算出它背后的数学原理。我所做的是我创建了两个立方体，边长均为 2，位置为 (0, 0, 0) 和 (1, 0, 0)。计算的第一个方向是 Vector3(0, 1, 0)，因此单纯形上的第一个点是 Vector3(1, -2, 0)。第二点，换个方向看，刚好相反，Vector3(-1, 2, 0)。这就产生了一个问题，因为创建的下一个方向使用了两者的叉积，得到 Vector3(0, 0, 0)。显然没有矢量与此方向相同，因此行 if support.dot(direction) <= 0: 失败。
但是，当 x 为 -1、0 或 1 时会发生这种情况，但不会发生在 -2 和 2 时。
如何确保在单纯形上找到的第二个点与第一个点不完全相反，从而提前进行检查 return？
最小可重现示例（使用完全相同的代码）：
# 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))
我的 vector3.py 文件太长了，下面是一个同样有效的等效文件：https://github.com/pyunity/pyunity/blob/07fed7871ace0c1b1b3cf8051d08d6677fe18209/pyunity/vector3.py
解决方法是将 ab.cross(ao).cross(ab) 更改为 ab / 2，因为这给出了从 AB 的中点到原点的向量，并且比两个叉积要便宜得多。