public override void CalculateWSCollisionBody()
{
CollisionBodyWS = new FCollPoly()
{
Vertices = new Vector2[CollisionBody.Vertices.Length]
};
for (int i = 0; i < CollisionBody.Vertices.Length; i++)
{
CollisionBodyWS.Vertices[i] = CollisionBody.Vertices[i] + (Vector2)transform.position;
}
}
protected override void UpdateCollisionBody_internal()
{
FMatrix3x3 TRS = FMatrix3x3.CreateTRS(Offset, transform.rotation.eulerAngles.z, transform.localScale);
CollisionBody = new FCollPoly()
{
Vertices = new Vector2[Vertices.Length]
};
for (int i = 0; i < Vertices.Length; i++)
{
CollisionBody.Vertices[i] = TRS * Vertices[i];
}
}
private static bool SphereSAT(FCollPoly p1, FCollSphere p2, out CollisionContact manifold)
{
FSATAxis[] axisPoly = GetAxis_sat(ref p1.Vertices);
FSATAxis circleAxis = GetCircleAxis_sat(p2.Center, ref p1.Vertices);
FSATAxis collisionAxis = axisPoly[0];
float penetration = 100000f;
manifold = null;
for (int i = 0; i < axisPoly.Length; i++)
{
Vector2 proj1 = ProjOntoAxis_sat(axisPoly[i], ref p1.Vertices);
float circleCenter = Vector2.Dot(p2.Center, axisPoly[i].Axis);
Vector2 proj2 = new Vector2(circleCenter - p2.Radius, circleCenter + p2.Radius);
float overlap = 0f;
if (!GetOverlapFromProjection_sat(proj1, proj2, out axisPoly[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 = axisPoly[i];
penetration = overlap;
}
}
}
//Circle axis
{
Vector2 proj1 = ProjOntoAxis_sat(circleAxis, ref p1.Vertices);
float circleCenter = Vector2.Dot(p2.Center, circleAxis.Axis);
Vector2 proj2 = new Vector2(circleCenter - p2.Radius, circleCenter + p2.Radius);
float overlap = 0f;
if (!GetOverlapFromProjection_sat(proj1, proj2, out circleAxis.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 = circleAxis;
penetration = overlap;
}
}
}
//Debug.Log("Collision axis: " + collisionAxis.Axis);
//Debug.Log("Penetration: " + penetration);
manifold = new CollisionContact();
FEdge eRef;
if (!collisionAxis.BodyA)
{
collisionAxis.Axis *= -1.0f;
}
eRef = GetBestEdge_sat(ref p1.Vertices, collisionAxis.Axis);
eRef.Normal = MeshUtils.CalcNormal(eRef.AV, eRef.BV);
Vector2 vLeft = p1.Vertices[CollMeshUtils.LeftIndex(eRef.AI, p1.Vertices.Length)];
Vector2 vRight = p1.Vertices[CollMeshUtils.RightIndex(eRef.BI, p1.Vertices.Length)];
Vector2 normLeft = MeshUtils.CalcNormalRaw(vLeft, eRef.AV);
Vector2 normRight = MeshUtils.CalcNormalRaw(eRef.BV, vRight);
Vector2 leftRel = p2.Center - eRef.AV;
Vector2 rightRel = p2.Center - eRef.BV;
if (Vector2.Dot(normLeft, leftRel) > 0f && Vector2.Dot(eRef.Normal, leftRel) > 0f)
{
manifold.Penetration = penetration;
manifold.Normal = collisionAxis.Axis;
manifold.ContactPoints = new Vector2[] { p2.Center - leftRel.normalized * p2.Radius };
}
else if (Vector2.Dot(normRight, rightRel) > 0f && Vector2.Dot(eRef.Normal, rightRel) > 0f)
{
manifold.Penetration = penetration;
manifold.Normal = collisionAxis.Axis;
manifold.ContactPoints = new Vector2[] { p2.Center - rightRel.normalized * p2.Radius };
}
else
{
manifold.Penetration = penetration;
manifold.Normal = collisionAxis.Axis;
manifold.ContactPoints = new Vector2[] { p2.Center - eRef.Normal * p2.Radius };
}
manifold.BodyAInc = false;
manifold.EdgeNormalA = eRef.Normal;
manifold.EdgeNormalB = (p2.Center - manifold.ContactPoints[0]).normalized;
return(true);
}
public static bool IsCollidingPoly(FCollPoly a, FCollSphere b, out CollisionContact manifold)
{
return(SphereSAT(a, b, out manifold));
}
public static bool IsCollidingRect(FCollPoly a, FCollPoly b, out CollisionContact manifold)
{
return(PolySAT(a, b, out manifold));
}
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);
}