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 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 = shapes[selectedIndex].rigidbody;
Vector2 position = t.position;
selectedOffset = mousePos - position;
float mouseAngle = Mathf.Atan2(selectedOffset.y, selectedOffset.x) * Mathf.Rad2Deg;
selectedAngle = mouseAngle - t.rotation;
}
}
else if (Input.GetMouseButtonUp(0))
{
selectedIndex = -1;
}
else if (Input.GetMouseButton(0))
{
if (selectedIndex < 0)
{
return;
}
Rigidbody t = shapes[selectedIndex].rigidbody;
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.rotation = angle;
}
}
}
public bool contains(Vector2 point)
{
return(GJKTool.contains(points, point));
}
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 applyTorqueImpulse(Vector2 point, Vector2 torque)
{
Vector2 radius = point - position;
angleVelocity += GJKTool.cross(radius, torque) * Mathf.Rad2Deg * invInertial;
}