public override void CalculateWSCollisionBody()
{
CollisionBodyWS = new FCollSphere()
{
Radius = CollisionBody.Radius, Center = CollisionBody.Center + (Vector2)transform.position
};
}
protected override void UpdateCollisionBody_internal()
{
float radMod = Mathf.Max(transform.localScale.x, transform.localScale.y);
CollisionBody = new FCollSphere()
{
Center = Offset, Radius = Radius * radMod
};
}
public static bool IsCollidingSphere(FCollSphere a, FCollSphere b, out CollisionContact manifold)
{
bool flipRef = b.Center.x < a.Center.x || b.Center.y < a.Center.y;
Vector2 rel = flipRef ? a.Center - b.Center : b.Center - a.Center;
float dist = rel.sqrMagnitude;
float rSum = a.Radius + b.Radius;
if (dist > (rSum * rSum))
{
manifold = null;
return(false);
}
float delta = rel.magnitude;
manifold = new CollisionContact();
if (delta == 0.0f)
{
manifold.Penetration = a.Radius;
manifold.Normal = Vector2.up;
manifold.ContactPoints = new Vector2[] { a.Center };
}
else
{
manifold.Penetration = rSum - delta;
manifold.Normal = rel / delta;
manifold.ContactPoints = new Vector2[] { flipRef?manifold.Normal *b.Radius + b.Center : manifold.Normal * a.Radius + a.Center };
}
manifold.BodyAInc = flipRef;
if (flipRef)
{
manifold.EdgeNormalA = -manifold.Normal;
manifold.EdgeNormalB = manifold.Normal;
}
else
{
manifold.EdgeNormalA = manifold.Normal;
manifold.EdgeNormalB = -manifold.Normal;
}
return(true);
}
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));
}