public override bool IsColliding(PhysCollider collider, out CollisionContact manifold, out bool isBodyA)
{
if (collider is PhysBoxCollider2D)
{
PhysBoxCollider2D other = collider as PhysBoxCollider2D;
isBodyA = false;
return(CollisionDetector.IsCollidingRect(other.CollisionBodyWS, CollisionBodyWS, out manifold));
}
else if (collider is PhysSphereCollider2D)
{
PhysSphereCollider2D other = collider as PhysSphereCollider2D;
isBodyA = true;
return(CollisionDetector.IsCollidingSphere(CollisionBodyWS, other.CollisionBodyWS, out manifold));
}
else if (collider is PhysPolyCollider2D)
{
PhysPolyCollider2D other = collider as PhysPolyCollider2D;
isBodyA = false;
return(CollisionDetector.IsCollidingPoly(other.CollisionBodyWS, CollisionBodyWS, out manifold));
}
isBodyA = false;
manifold = null;
return(false);
}
public void RegisterCollision(CollisionContact collision, bool isBodyA)
{
collisions.Push(new FRawCollision()
{
CollisionContact = collision, IsBodyA = isBodyA
});
if (!componentsInCollision.ContainsKey(isBodyA ? collision.B : collision.A))
{
componentsInCollision.TryAdd(isBodyA ? collision.B : collision.A, 0);
}
}
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));
}
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);
}
private void ResolveContact(CollisionContact contact, Vector2 actualCollisionNormal, Vector2 edgeNormalColl, Vector2 edgeNormal, FCollisionBodyExtractor otherObject)
{
float bounce = (Bounciness + otherObject.Bounciness) * 0.5f;//Mathf.Min(Bounciness, otherObject.Bounciness);
float frictionCoef = Mathf.Min(Roughness * 0.1f, otherObject.Body.Roughness * 0.1f);
Vector2 fN = actualCollisionNormal * Vector2.Dot(force, actualCollisionNormal);
Vector2 startVel = Velocity;
float startAngularVel = AngularVelocity;
Vector2 startVelB = otherObject.Velocity;
float startAngularVelB = otherObject.AngularVelocity;
if (contact.ContactPoints != null)
{
for (int i = 0; i < contact.ContactPoints.Length; i++)
{
Vector2 radA = contact.ContactPoints[i] - MassPoint;
Vector2 radB = contact.ContactPoints[i] - otherObject.Body.MassPoint;
Vector2 relVel = startVel + radA.Cross(startAngularVel) - startVelB - radB.Cross(startAngularVelB);
//Vector2 relVel = startVel - startVelB;
float contactVel = Vector2.Dot(relVel, actualCollisionNormal);
if (contactVel < 0)
{
float invMassSum = InvMass + otherObject.Body.InvMass + Mathf.Pow(radA.Cross(actualCollisionNormal), 2) * InvInertia +
Mathf.Pow(radB.Cross(actualCollisionNormal), 2) * otherObject.Body.InvInertia;
float impulseScalar = -(1f + Bounciness) * contactVel;
impulseScalar /= invMassSum;
impulseScalar /= contact.ContactPoints.Length;
Vector2 impulse = actualCollisionNormal * impulseScalar;
ApplyImpulse(impulse, radA);
Vector2 velAImpulse = startVel + InvMass * impulse;
float anVelAImpulse = startAngularVel + InvInertia * radA.Cross(impulse);
Vector2 velAImpulseB = Vector2.zero;
float anVelAImpulseB = 0f;
if (otherObject.IsRigidbody)
{
((PhysRigidbody)otherObject.Body).ApplyImpulse(-impulse, radB);
velAImpulseB = startVelB + otherObject.Body.InvMass * -impulse;
anVelAImpulseB = startAngularVelB + otherObject.Body.InvInertia * radB.Cross(-impulse);
}
Vector2 movementTangent = new Vector2(-edgeNormalColl.y, edgeNormalColl.x);
Vector2 frictionVel;
Vector2 friction;
float tanDot = Vector2.Dot(movementTangent, velAImpulse);
if (tanDot > 0)
{
frictionVel = (movementTangent * tanDot) / contact.ContactPoints.Length;
friction = (movementTangent * frictionCoef) / contact.ContactPoints.Length;
if (frictionVel.sqrMagnitude > friction.sqrMagnitude)
{
ApplyImpulse(-friction, radA);
}
else
{
ApplyImpulse(-frictionVel, radA);
}
}
else if (tanDot < 0)
{
frictionVel = (-movementTangent * -tanDot) / contact.ContactPoints.Length;
friction = (-movementTangent * frictionCoef) / contact.ContactPoints.Length;
if (frictionVel.sqrMagnitude > friction.sqrMagnitude)
{
ApplyImpulse(-friction, radA);
}
else
{
ApplyImpulse(-frictionVel, radA);
}
}
}
}
}
if (Vector2.Dot(force, edgeNormalColl) < 0f)
{
force -= fN;
}
if (contact.Penetration > SLOP)
{
if (otherObject.IsRigidbody)
{
positionCorrection.Add(actualCollisionNormal * contact.Penetration * POSITION_CORRECTION_MOD * 0.5f);
}
else
{
positionCorrection.Add(actualCollisionNormal * contact.Penetration * POSITION_CORRECTION_MOD);
}
}
}
public void GatherCollisionInformation(PhysCompoundCollider other)
{
if (other is PhysRigidbody)
{
PhysRigidbody rigid = other as PhysRigidbody;
for (int c1 = 0; c1 < Collider.Length; c1++)
{
FAABB2D bounds = other.Collider[c1].CachedBoundsWS;
for (int c2 = 0; c2 < other.Collider.Length; c2++)
{
if (bounds.Intersects(other.Collider[c2].CachedBoundsWS))
{
CollisionContact manifold = null;
bool isBodyA = false;
if (Collider[c1].IsColliding(other.Collider[c2], out manifold, out isBodyA))
{
if (isBodyA)
{
manifold.A = this;
manifold.B = rigid;
}
else
{
manifold.A = rigid;
manifold.B = this;
}
RegisterCollision(manifold, isBodyA);
rigid.RegisterCollision(manifold, !isBodyA);
}
}
}
}
}
else
{
for (int c1 = 0; c1 < Collider.Length; c1++)
{
FAABB2D bounds = other.Collider[c1].CachedBoundsWS;
for (int c2 = 0; c2 < other.Collider.Length; c2++)
{
if (bounds.Intersects(other.Collider[c2].CachedBoundsWS))
{
CollisionContact manifold = null;
bool isBodyA = false;
if (Collider[c1].IsColliding(other.Collider[c2], out manifold, out isBodyA))
{
if (isBodyA)
{
manifold.A = this;
manifold.B = other;
}
else
{
manifold.A = other;
manifold.B = this;
}
RegisterCollision(manifold, isBodyA);
}
}
}
}
}
}
public virtual bool IsColliding(PhysCollider collider, out CollisionContact manifold, out bool isBodyA)
{
manifold = null;
isBodyA = true;
return(false);
}
