// EPA算法计算穿透向量
void queryEPA()
{
simplexEdge.initEdges(simplex);
currentEpaEdge = null;
for (int i = 0; i < maxIterCount; ++i)
{
Edge e = simplexEdge.findClosestEdge();
currentEpaEdge = e;
if (e == null)
{
break;
}
penetrationVector = e.normal * e.distance;
SupportPoint point = support(e.normal);
float distance = Vector2.Dot(point.point, e.normal);
if (distance - e.distance < epsilon)
{
penetrationVector = e.normal * distance;
break;
}
simplexEdge.insertEdgePoint(e, point);
}
}
public Edge createEdge(SupportPoint a, SupportPoint b)
{
Edge e = new Edge();
e.a = a;
e.b = b;
e.normal = GJKTool.getPerpendicularToOrigin(a.point, b.point);
float lengthSq = e.normal.sqrMagnitude;
// 单位化边
if (lengthSq > 0.0001f)
{
e.distance = Mathf.Sqrt(lengthSq);
e.normal *= 1.0f / e.distance;
}
else
{
// 如果距离原点太近，用数学的方法来得到直线的垂线
// 方向可以随便取，刚好另外一边是反着来的
Vector2 v = a.point - b.point;
v.Normalize();
e.normal = new Vector2(-v.y, v.x);
}
return(e);
}
public void insertEdgePoint(Edge e, SupportPoint point)
{
Edge e1 = createEdge(e.a, point);
edges[e.index] = e1;
Edge e2 = createEdge(point, e.b);
edges.Insert(e.index + 1, e2);
updateEdgeIndex();
}
void computeClosetPoint(SupportPoint A, SupportPoint B)
{
/*
* L = AB，是Minkowski差集上的一个边，同时构成A、B两点的顶点也来自各自shape的边。
* E1 = Aa - Ba，E2 = Ab - Bb
* 则求两个凸包的最近距离，就演变成了求E1和E2两个边的最近距离。
*
* 设Q点是原点到L的垂点，则有:
* L = B - A
* Q · L = 0
* 因为Q是L上的点，可以用r1, r2来表示Q (r1 + r2 = 1)，则有: Q = A * r1 + B * r2
* (A * r1 + B * r2) · L = 0
* 用r2代替r1: r1 = 1 - r2
* (A - A * r2 + B * r2) · L = 0
* (A + (B - A) * r2) · L = 0
* L · A + L · L * r2 = 0
* r2 = -(L · A) / (L · L)
*/
Vector2 L = B.point - A.point;
float sqrDistanceL = L.sqrMagnitude;
// support点重合了
if (sqrDistanceL < epsilon)
{
closestOnA = closestOnB = A.point;
}
else
{
float r2 = -Vector2.Dot(L, A.point) / sqrDistanceL;
r2 = Mathf.Clamp01(r2);
float r1 = 1.0f - r2;
closestOnA = A.fromA * r1 + B.fromA * r2;
closestOnB = A.fromB * r1 + B.fromB * r2;
}
}
public bool queryCollision(Shape shapeA, Shape shapeB)
{
this.shapeA = shapeA;
this.shapeB = shapeB;
simplex.clear();
isCollision = false;
direction = Vector2.zero;
closestOnA = Vector2.zero;
closestOnB = Vector2.zero;
simplexEdge.clear();
currentEpaEdge = null;
penetrationVector = Vector2.zero;
direction = findFirstDirection();
simplex.add(support(direction));
simplex.add(support(-direction));
direction = -GJKTool.getClosestPointToOrigin(simplex.get(0), simplex.get(1));
for (int i = 0; i < maxIterCount; ++i)
{
// 方向接近于0，说明原点就在边上
if (direction.sqrMagnitude < epsilon)
{
isCollision = true;
break;
}
SupportPoint p = support(direction);
// 新点与之前的点重合了。也就是沿着dir的方向，已经找不到更近的点了。
if (GJKTool.sqrDistance(p.point, simplex.get(0)) < epsilon ||
GJKTool.sqrDistance(p.point, simplex.get(1)) < epsilon)
{
isCollision = false;
break;
}
simplex.add(p);
// 单形体包含原点了
if (simplex.contains(Vector2.zero))
{
isCollision = true;
break;
}
direction = findNextDirection();
}
if (!isCollision)
{
computeClosetPoint(simplex.getSupport(0), simplex.getSupport(1));
}
else
{
queryEPA();
computeClosetPoint(currentEpaEdge.a, currentEpaEdge.b);
}
return(isCollision);
}
public void add(SupportPoint point)
{
points.Add(point.point);
fromA.Add(point.fromA);
fromB.Add(point.fromB);
}
