void ComputeClosetPoint()
{
/*
* 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)
*/
SupportPoint A = simplex.getSupport(0);
SupportPoint B = simplex.getSupport(1);
Vector2 L = B.point - A.point;
float sqrDistanceL = L.sqrMagnitude;
// support点重合了
if (sqrDistanceL < 0.0001f)
{
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 IEnumerator queryStepByStep(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;
yield return(null);
direction = findFirstDirection();
simplex.add(support(direction));
simplex.add(support(-direction));
yield return(null);
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);
yield return(null);
// 单形体包含原点了
if (simplex.contains(Vector2.zero))
{
isCollision = true;
break;
}
direction = findNextDirection();
}
yield return(null);
if (!isCollision)
{
ComputeClosetPoint();
}
else
{
// EPA算法计算穿透向量
simplexEdge.initEdges(simplex);
for (int i = 0; i < maxIterCount; ++i)
{
Edge e = simplexEdge.findClosestEdge();
currentEpaEdge = e;
penetrationVector = e.normal * e.distance;
yield return(null);
Vector2 point = support(e.normal).point;
float distance = Vector2.Dot(point, e.normal);
if (distance - e.distance < epsilon)
{
penetrationVector = e.normal * distance;
break;
}
simplexEdge.insertEdgePoint(e, point);
yield return(null);
}
}
}
public void add(SupportPoint point)
{
points.Add(point.point);
fromA.Add(point.fromA);
fromB.Add(point.fromB);
}