public LFloat sqrMagnitude; // 原点到直线的距离的平方
public EPAFace2D(SimplexPoint2D sp1, SimplexPoint2D sp2)
{
this.sp1 = sp1;
this.sp2 = sp2;
// 这里使用计算直线交点的方向求得原点到直线的最短向量
LVector2 D = -LGeometryUtil2D.GetPerpendicular2D(sp1.p, sp2.p, LVector2.zero);
if (sp2.p.x == sp1.p.x)
{
LFloat x = sp1.p.x;
this.direction = new LVector2(x, 0);
this.sqrMagnitude = x * x;
}
else
{
if (D.x == 0)
{
LFloat y = sp1.p.y;
this.direction = new LVector2(0, y);
this.sqrMagnitude = y * y;
}
else
{
LFloat k1 = (sp2.p.y - sp1.p.y) / (sp2.p.x - sp1.p.x);
LFloat b1 = (sp2.p.y * sp1.p.x - sp2.p.x * sp1.p.y) / (sp1.p.x - sp2.p.x);
LFloat k2 = D.y / D.x;
LFloat b2 = 0;
LFloat y = (b1 * k2 - b2 * k1) / (k2 - k1);
LFloat x = y / k2;
this.direction = new LVector2(x, y);
this.sqrMagnitude = x * x + y * y;
}
}
}
/// <summary>
/// <para>GJK判断是否碰撞</para>
/// <para>1.找direction方向的最远点</para>
/// <para>2.判断最远点是否与原点在同一方向(是否比原点还远)，如果不是则未碰撞</para>
/// <para>3.将最远点置入simplex，判断simplex是否包围了原点，如果是则碰撞，否则以离原点最近的simplex上的点到原点作为direction回到步骤1</para>
/// </summary>
/// <param name="objA"></param>
/// <param name="objB"></param>
private LContactConstraint2D CollisionDetect(LPhysicsObject2D objA, LPhysicsObject2D objB)
{
Vector2 a = objA.collider.GetFurtherPointInDirection(direction);
Vector2 b = objB.collider.GetFurtherPointInDirection(-direction);
SimplexPoint2D sp = new SimplexPoint2D(a, b);
if (simplex.IsSameDirectionWithOrigin(sp, direction))
{
simplex.Add(sp);
if (simplex.DoSimplex2D())
{
epa.Add(new EPAFace2D(simplex[0], simplex[1]));
epa.Add(new EPAFace2D(simplex[1], simplex[2]));
epa.Add(new EPAFace2D(simplex[2], simplex[0]));
PenetrationDetect(objA, objB);
return(ContactConstraintGenerate(objA, objB));
}
else
{
direction = simplex.FindClosestPointToOriginDirection();
return(CollisionDetect(objA, objB));
}
}
else
{
return(null);
}
}
public void AddExpiredEdgePoint(SimplexPoint2D sp)
{
for (int i = points.Count - 1; i >= 0; i--)
{
if (points[i].Equals(sp))
{
points.RemoveAt(i);
return;
}
}
points.Add(sp);
}
/// <summary>
/// 判断找到Support点是是否是沿找寻的方向
/// <para>如果是相反的方向，说明不会包含原点，退出GJK</para>
/// </summary>
/// <returns></returns>
public bool IsSameDirectionWithOrigin(SimplexPoint2D sp, LVector2 direction)
{
bool certain = false;
if (count == 0)
{
certain = true;
}
else if (count == 1 ||
count == 2)
{
certain = LVector2.Dot(sp.p, direction) >= 0;
}
return(certain);
}
/// <summary>
/// <para>EPA判断渗透方向和深度</para>
/// <para>1.找到list中离原点最近的face</para>
/// <para>2.如果与上一次找到的相同，直接返回</para>
/// <para>3.否则找face.normal的最远点A</para>
/// <para>4.移除使A看不到的原点的face，将(A, face.A)和(A, face.B)构建为新的face添加到list中，返回第一步</para>
/// </summary>
/// <returns></returns>
private void PenetrationDetect(LPhysicsObject2D objA, LPhysicsObject2D objB)
{
EPAFace2D face = epa.FindClosestFace();
if (closestFace == null ||
Mathf.Abs(closestFace.sqrMagnitude - face.sqrMagnitude) > EPAFace2D.Threshold)
{
closestFace = face;
safety.Add(closestFace);
Vector2 a = objA.collider.GetFurtherPointInDirection(closestFace.direction);
Vector2 b = objB.collider.GetFurtherPointInDirection(-closestFace.direction);
SimplexPoint2D sp = new SimplexPoint2D(a, b);
epa.ImportNewSupportPoint(sp);
PenetrationDetect(objA, objB);
}
}
/// <summary>
/// 引入一个新的support point，删除使得此点看不见原点的edge，将删除的edge的点保存在list中
/// 剔除list中的重复点，用list中的每个点与sp构建新的edge
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <param name="sp"></param>
public void ImportNewSupportPoint(SimplexPoint2D sp)
{
for (int i = faces.Count - 1; i >= 0; i--)
{
if (LVector2.Dot(sp.p - faces[i].sp1.p, faces[i].direction) > 0)
{
AddExpiredEdgePoint(faces[i].sp1);
AddExpiredEdgePoint(faces[i].sp2);
faces.RemoveAt(i);
}
}
for (int i = points.Count - 1; i >= 0; i--)
{
faces.Add(new EPAFace2D(sp, points[i]));
}
points.Clear();
}
public bool Equals(SimplexPoint2D sp)
{
return(ma == sp.a && mb == sp.b);
}
/// <summary>
/// 添加一个Support点
/// </summary>
/// <param name="sp"></param>
public void Add(SimplexPoint2D sp)
{
vertics[count] = sp;
count += 1;
}
