public static FEdge[] GetEdgeNeigbhours(FEdge edge, ref Vector2[] vertices)
{
int lIndex = LeftIndex(edge.AI, vertices.Length);
int rIndex = RightIndex(edge.BI, vertices.Length);
return(new FEdge[2]
{
new FEdge(vertices[lIndex], edge.AV, lIndex, edge.AI),
new FEdge(edge.BV, vertices[rIndex], edge.BI, rIndex)
});
}
private static bool PolySAT(FCollPoly p1, FCollPoly p2, out CollisionContact manifold)
{
manifold = null;
FSATAxis[] axis1 = GetAxis_sat(ref p1.Vertices);
FSATAxis[] axis2 = GetAxis_sat(ref p2.Vertices);
FSATAxis collisionAxis = axis1[0];
float penetration = 100000f;
for (int i = 0; i < axis1.Length; i++)
{
Vector2 proj1 = ProjOntoAxis_sat(axis1[i], ref p1.Vertices);
Vector2 proj2 = ProjOntoAxis_sat(axis1[i], ref p2.Vertices);
float overlap = 0f;
if (!GetOverlapFromProjection_sat(proj1, proj2, out axis1[i].BodyA, out overlap))
{
return(false);
}
else
{
if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1))
{
float mins = Mathf.Abs(proj1.x - proj2.x);
float maxs = Mathf.Abs(proj1.y - proj2.y);
if (mins < maxs)
{
overlap += mins;
}
else
{
overlap += maxs;
}
}
if (overlap < penetration)
{
collisionAxis = axis1[i];
penetration = overlap;
}
}
}
for (int i = 0; i < axis2.Length; i++)
{
Vector2 proj1 = ProjOntoAxis_sat(axis2[i], ref p1.Vertices);
Vector2 proj2 = ProjOntoAxis_sat(axis2[i], ref p2.Vertices);
float overlap = 0f;
if (!GetOverlapFromProjection_sat(proj1, proj2, out axis2[i].BodyA, out overlap))
{
return(false);
}
else
{
if (ContainsOtherProjection_sat(proj1, proj2) || ContainsOtherProjection_sat(proj2, proj1))
{
float mins = Mathf.Abs(proj1.x - proj2.x);
float maxs = Mathf.Abs(proj1.y - proj2.y);
if (mins < maxs)
{
overlap += mins;
}
else
{
overlap += maxs;
}
}
if (overlap < penetration)
{
collisionAxis = axis2[i];
penetration = overlap;
}
}
}
manifold = new CollisionContact();
FEdge eRef;
FEdge eInc;
if (collisionAxis.BodyA)
{
eRef = GetBestEdge_sat(ref p1.Vertices, collisionAxis.Axis);
eInc = GetBestEdge_sat(ref p2.Vertices, -collisionAxis.Axis);
}
else
{
eInc = GetBestEdge_sat(ref p1.Vertices, -collisionAxis.Axis);
eRef = GetBestEdge_sat(ref p2.Vertices, collisionAxis.Axis);
}
bool flipped = false;
if (Mathf.Abs(Vector2.Dot(eInc.BV - eInc.AV, collisionAxis.Axis)) < Mathf.Abs(Vector2.Dot(eRef.BV - eRef.AV, collisionAxis.Axis)))
{
FEdge copy = eRef;
eRef = eInc;
eInc = copy;
flipped = true;
}
Vector2 refDir = (eRef.BV - eRef.AV).normalized;
float offset = Vector2.Dot(refDir, eRef.AV);
List <Vector2> contactPoints = Clip_sat(eInc.AV, eInc.BV, refDir, offset);
if (contactPoints.Count < 2)
{
return(true);
}
offset = Vector2.Dot(eRef.BV, refDir);
contactPoints = Clip_sat(contactPoints[0], contactPoints[1], -refDir, -offset);
if (contactPoints.Count < 2)
{
return(true);
}
eRef.Normal = MeshUtils.CalcNormal(eRef.AV, eRef.BV);
eInc.Normal = MeshUtils.CalcNormal(eInc.AV, eInc.BV);
//Debug.Log(eRef.Normal);
if (Vector2.Dot(eRef.Normal, contactPoints[1] - eRef.AV) > 0f)
{
contactPoints.RemoveAt(1);
}
if (Vector2.Dot(eRef.Normal, contactPoints[0] - eRef.AV) > 0f)
{
contactPoints.RemoveAt(0);
}
manifold.ContactPoints = contactPoints.ToArray();
manifold.Penetration = penetration;
if (flipped)
{
manifold.Normal = -collisionAxis.Axis;
manifold.BodyAInc = collisionAxis.BodyA;
manifold.EdgeNormalA = manifold.BodyAInc ? eInc.Normal : eRef.Normal;
manifold.EdgeNormalB = manifold.BodyAInc ? eRef.Normal : eInc.Normal;
}
else
{
manifold.Normal = collisionAxis.Axis;
manifold.BodyAInc = !collisionAxis.BodyA;
manifold.EdgeNormalA = manifold.BodyAInc ? eInc.Normal : eRef.Normal;
manifold.EdgeNormalB = manifold.BodyAInc ? eRef.Normal : eInc.Normal;
}
return(true);
}
