/// <summary>
/// 判断是否包围了原点
/// <para>如果没有包含，且点数超过2时，只保留连线离原点最近的两个点</para>
/// </summary>
/// <returns></returns>
public bool DoSimplex2D()
{
bool certain = false;
if (count == 1 ||
count == 2)
{
certain = false;
}
else if (count == 3)
{
LVector2 p02 = LGeometryUtil2D.GetPerpendicular2D(vertics[0].p, vertics[2].p, vertics[1].p);
if (LVector2.Dot(-vertics[0].p, p02) >= 0)
{
LVector2 p12 = LGeometryUtil2D.GetPerpendicular2D(vertics[1].p, vertics[2].p, vertics[0].p);
if (LVector2.Dot(-vertics[1].p, p12) >= 0)
{
certain = true;
}
else
{
RemoveIndex(0);
}
}
else
{
RemoveIndex(1);
}
}
return(certain);
}
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;
}
}
}
internal override void PrepareForCollision()
{
base.PrepareForCollision();
CalculateLocalVertics();
for (int i = mLocalVertics.Length - 1; i >= 0; i--)
{
mVertics[i] = transform.TransformPoint(mLocalVertics[i]);
}
mBounds = LGeometryUtil2D.CalcBounds(mVertics);
}
/// <summary>
/// 返回一个离原点最近的一个点到原点方向的向量
/// <para>注:如果vertics中只有一个点，直接计算返回</para>
/// <para>注:如果vertics中有两个点，向量为两点连线的垂线(指向原点的方向)</para>
/// </summary>
/// <returns></returns>
public LVector2 FindClosestPointToOriginDirection()
{
LVector2 dir = LVector2.up;
if (count == 1)
{
dir = -vertics[0].p;
}
else if (count == 2)
{
dir = LGeometryUtil2D.GetPerpendicular2D(vertics[0].p, vertics[1].p, LVector2.zero);
}
return(dir);
}
private LContactConstraint2D ContactConstraintGenerate(LPhysicsObject2D objA, LPhysicsObject2D objB)
{
LContactConstraint2D constraint = null;
Vector2 contactPointsA = LGeometryUtil2D.CalcBarycentric(closestFace.sp1.a, closestFace.sp2.a);
Vector2 contactPointsB = LGeometryUtil2D.CalcBarycentric(closestFace.sp1.b, closestFace.sp2.b);
if (closestFace.sqrMagnitude > LPhysicsStatic2D.DistanceSqrtThreshold)
{
float penetration = Mathf.Sqrt(closestFace.sqrMagnitude);
Vector2 normal = closestFace.direction / penetration;
if (objA.rigidBody != null ||
objB.rigidBody != null)
{
constraint = new LContactConstraint2D(objA, objB);
constraint.anchorA = contactPointsA;
constraint.anchorB = contactPointsB;
constraint.normal = normal;
constraint.penetration = penetration;
}
}
return(constraint);
}
