public Edge createEdge(Vector2 a, Vector2 b)
{
Edge e = new Edge();
e.a = a;
e.b = b;
e.normal = GJKTool.getPerpendicularToOrigin(a, b);
float lengthSq = e.normal.sqrMagnitude;
// 单位化边
if (lengthSq > 0.00001f)
{
e.distance = Mathf.Sqrt(lengthSq);
e.normal *= 1.0f / e.distance;
}
else
{
// 如果距离原点太近，用数学的方法来得到直线的垂线
// 方向可以随便取，刚好另外一边是反着来的
Vector2 v = a - b;
v.Normalize();
e.normal = new Vector2(-v.y, v.x);
}
return(e);
}
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));
}
}
void UpdateSelection()
{
if (Input.GetMouseButtonDown(0))
{
Vector2 mousePos = Camera.main.ScreenToWorldPoint(Input.mousePosition);
//Debug.Log("mousePos: " + mousePos);
selectedIndex = -1;
for (int i = 0; i < shapes.Count; ++i)
{
if (GJKTool.contains(shapes[i].vertices, mousePos))
{
selectedIndex = i;
break;
}
}
if (selectedIndex >= 0)
{
var t = colliders[selectedIndex].transform;
Vector2 position = t.position;
selectedOffset = mousePos - position;
float mouseAngle = Mathf.Atan2(selectedOffset.y, selectedOffset.x) * Mathf.Rad2Deg;
selectedAngle = mouseAngle - t.eulerAngles.z;
}
}
else if (Input.GetMouseButtonUp(0))
{
selectedIndex = -1;
}
else if (Input.GetMouseButton(0))
{
if (selectedIndex < 0)
{
return;
}
Transform t = colliders[selectedIndex].transform;
Vector2 mousePos = Camera.main.ScreenToWorldPoint(Input.mousePosition);
if (selectedOffset.magnitude < 0.4f) // 移动
{
Vector3 position = t.position;
position.x = mousePos.x - selectedOffset.x;
position.y = mousePos.y - selectedOffset.y;
t.position = position;
}
else // 旋转
{
Vector2 position = t.position;
Vector2 mouseDir = mousePos - position;
float mouseAngle = Mathf.Atan2(mouseDir.y, mouseDir.x) * Mathf.Rad2Deg;
float angle = mouseAngle - selectedAngle;
t.eulerAngles = new Vector3(0, 0, angle);
}
}
}
/// 按步骤分解，碰撞检测
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 bool contains(Vector2 point)
{
return(GJKTool.contains(points, point));
}