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); }