public Vector2 findNextDirection()
{
if (simplex.count() == 2)
{
Vector2 crossPoint = GJKTool.getClosestPointToOrigin(simplex.get(0), simplex.get(1));
// 取靠近原点方向的向量
return(Vector2.zero - crossPoint);
}
else if (simplex.count() == 3)
{
Vector2 crossOnCA = GJKTool.getClosestPointToOrigin(simplex.get(2), simplex.get(0));
Vector2 crossOnCB = GJKTool.getClosestPointToOrigin(simplex.get(2), simplex.get(1));
// 保留距离原点近的，移除较远的那个点
if (crossOnCA.sqrMagnitude < crossOnCB.sqrMagnitude)
{
simplex.remove(1);
return(Vector2.zero - crossOnCA);
}
else
{
simplex.remove(0);
return(Vector2.zero - crossOnCB);
}
}
else
{
// 不应该执行到这里
return(new Vector2(0, 0));
}
}
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);
}