private void CheckCollisionsInGroup(GameTime gameTime, List <ICutlassCollidable> collidableObjects)
{
if (collidableObjects == null || collidableObjects.Count <= 1)
{
return;
}
for (int i = 0; i < collidableObjects.Count; i++)
{
ICutlassCollidable first = collidableObjects[i];
for (int j = i + 1; j < collidableObjects.Count; j++)
{
ICutlassCollidable second = collidableObjects[j];
bool collisionCategoriesOverlap = CheckCollisionCategories(first, second);
//If both are stationary, or neither is stationary, detect collision but don't do anything about it.
if (first.Stationary == second.Stationary)
{
if (collisionCategoriesOverlap && first.NextFrameBoundingRect.Intersects(second.NextFrameBoundingRect))
{
first.CollisionDetected(second);
second.CollisionDetected(first);
}
}
//If only one is stationary, detect collision and correct non-stationary object
else
{
//contact.A is non-stationary object.
CollisionContact collisionContact;
if (first.Stationary)
{
collisionContact = new CollisionContact()
{
A = second, B = first
}
}
;
else
{
collisionContact = new CollisionContact()
{
A = first, B = second
}
};
if (collisionCategoriesOverlap && CalculateCollisionCorrection(gameTime, ref collisionContact) && !IsInternalEdge(collisionContact))
{
collisionContact.A.CollisionDetectedWithCorrection(collisionContact.B, collisionContact.Normal, collisionContact.Distance);
collisionContact.B.CollisionDetected(collisionContact.A);
}
}
}
}
}
public override List <CollisionContact> CalculateCollision(List <CollisionPair> pairs)
{
List <CollisionContact> contacts = new List <CollisionContact>();
foreach (CollisionPair pair in pairs)
{
if (pair.a._rigidbody == null || pair.b._rigidbody == null ||
pair.a._rigidbody == pair.b._rigidbody)
{
continue;
}
currentContact = new CollisionContact();
currentContact.a = pair.a;
currentContact.b = pair.b;
if (GJK(pair.a, pair.b))
{
EPA(pair.a, pair.b);
contacts.Add(currentContact);
}
}
return(contacts);
}
private void SolveContact(CollisionContact contact)
{
//contact.a.gameObject.transform.position += contact.contactNormal * contact.penetrationDepth * 0.5f;
//contact.b.gameObject.transform.position -= contact.contactNormal * contact.penetrationDepth * 0.5f;
Debug.DrawLine(contact.a.gameObject.transform.position - Vector3.down * 0.1f, contact.a.gameObject.transform.position + Vector3.down * 0.1f, Color.yellow);
Debug.DrawLine(contact.a.gameObject.transform.position - Vector3.left * 0.1f, contact.a.gameObject.transform.position + Vector3.left * 0.1f, Color.yellow);
Debug.DrawLine(contact.a.gameObject.transform.position - Vector3.back * 0.1f, contact.a.gameObject.transform.position + Vector3.back * 0.1f, Color.yellow);
Debug.DrawLine(contact.globalContactA - Vector3.down * 0.2f, contact.globalContactA + Vector3.down * 0.2f, Color.yellow);
Debug.DrawLine(contact.globalContactA - Vector3.left * 0.2f, contact.globalContactA + Vector3.left * 0.2f, Color.yellow);
Debug.DrawLine(contact.globalContactA - Vector3.back * 0.2f, contact.globalContactA + Vector3.back * 0.2f, Color.yellow);
Debug.DrawLine(contact.b.gameObject.transform.position - Vector3.down * 0.1f, contact.b.gameObject.transform.position + Vector3.down * 0.1f, Color.cyan);
Debug.DrawLine(contact.b.gameObject.transform.position - Vector3.left * 0.1f, contact.b.gameObject.transform.position + Vector3.left * 0.1f, Color.cyan);
Debug.DrawLine(contact.b.gameObject.transform.position - Vector3.back * 0.1f, contact.b.gameObject.transform.position + Vector3.back * 0.1f, Color.cyan);
Debug.DrawLine(contact.globalContactB - Vector3.down * 0.2f, contact.globalContactB + Vector3.down * 0.2f, Color.cyan);
Debug.DrawLine(contact.globalContactB - Vector3.left * 0.2f, contact.globalContactB + Vector3.left * 0.2f, Color.cyan);
Debug.DrawLine(contact.globalContactB - Vector3.back * 0.2f, contact.globalContactB + Vector3.back * 0.2f, Color.cyan);
//法向量碰撞處理
//碰撞限制初始化
Vector3 rA = contact.globalContactA - contact.a.gameObject.transform.position;
Vector3 rB = contact.globalContactB - contact.b.gameObject.transform.position;
/* J = [j_va, j_wa, j_vb, j_wb]
* = [normal, rA × normal, -normal, - (rb × normal)]
*
* ┌ Ma 0 0 0 ┐
* M = │ 0 Ia 0 0 │ Ma, Mb are mass. Ia, Ib are inertia tensor.
* │ 0 0 Mb 0 │
* └ 0 0 0 Ib ┘
*
* Effective mass = 1 / (J × M^(-1) × J^(T))
*/
Vector3 j_va = contact.contactNormal;
Vector3 j_wa = Vector3.Cross(rA, contact.contactNormal);
Vector3 j_vb = -contact.contactNormal;
Vector3 j_wb = -Vector3.Cross(rB, contact.contactNormal);
float k = (contact.a._rigidbody.inverseMass * Vector3.Dot(j_va, j_va)) +
Vector3.Dot(j_wa, contact.a._rigidbody.localInverseInertiaTensor.Transform(j_wa)) +
(contact.b._rigidbody.inverseMass * Vector3.Dot(j_vb, j_vb)) +
Vector3.Dot(j_wb, contact.b._rigidbody.localInverseInertiaTensor.Transform(j_wb));
float effectiveMass = 1.0f / k;
float totalImpulse = 0;
//碰撞限制解析
float jv = Vector3.Dot(j_va, contact.a._rigidbody.linearVelocity) +
Vector3.Dot(j_wa, contact.a._rigidbody.angularVelocity * Mathf.Deg2Rad) +
Vector3.Dot(j_vb, contact.b._rigidbody.linearVelocity) +
Vector3.Dot(j_wb, contact.b._rigidbody.angularVelocity * Mathf.Deg2Rad);
/*float resistution = (contact.a._rigidbody.resistution + contact.b._rigidbody.resistution) * 0.5f;
* Vector3 relativeVelocity = -contact.a._rigidbody.linearVelocity
* -Vector3.Cross(contact.a._rigidbody.angularVelocity * Mathf.Deg2Rad, rA)
+contact.b._rigidbody.linearVelocity
+Vector3.Cross(contact.b._rigidbody.angularVelocity * Mathf.Deg2Rad, rB);
* float closeVelocity = Vector3.Dot(relativeVelocity, contact.contactNormal);
*
* float b = -(beta / Time.fixedDeltaTime) * contact.penetrationDepth + resistution * closeVelocity;*/
float tempLambda = effectiveMass * (-(jv /*+ b*/));
float oldTotalImpulse = totalImpulse;
totalImpulse = Mathf.Clamp(totalImpulse + tempLambda, 0, float.PositiveInfinity);
float lambda = totalImpulse - oldTotalImpulse;
if (!contact.a._rigidbody.isStatic)
{
contact.a._rigidbody.linearVelocity += contact.a._rigidbody.inverseMass * j_va * lambda;
contact.a._rigidbody.angularVelocity += contact.a._rigidbody.localInverseInertiaTensor.Transform(j_wa) * lambda;
}
if (!contact.b._rigidbody.isStatic)
{
contact.b._rigidbody.linearVelocity += contact.b._rigidbody.inverseMass * j_vb * lambda;
contact.b._rigidbody.angularVelocity += contact.b._rigidbody.localInverseInertiaTensor.Transform(j_wb) * lambda;
}
//========================
/*
* //切向量碰撞處理1
* j_va = contact.contactTangent1;
* j_wa = Vector3.Cross(rA, contact.contactTangent1);
* j_vb = -contact.contactTangent1;
* j_wb = -Vector3.Cross(rB, contact.contactTangent1);
*
* jv = Vector3.Dot(j_va, contact.a._rigidbody.linearVelocity) +
* Vector3.Dot(j_wa, contact.a._rigidbody.angularVelocity * Mathf.Deg2Rad) +
* Vector3.Dot(j_vb, contact.b._rigidbody.linearVelocity) +
* Vector3.Dot(j_wb, contact.b._rigidbody.angularVelocity * Mathf.Deg2Rad);
*
* float friction = 1 - ((contact.a._rigidbody.friction + contact.b._rigidbody.friction) * 0.5f);
* float maxFrictionForce = friction * lambda;
* relativeVelocity = -contact.a._rigidbody.linearVelocity
* - Vector3.Cross(contact.a._rigidbody.angularVelocity * Mathf.Deg2Rad, rA)
+ contact.b._rigidbody.linearVelocity
+ Vector3.Cross(contact.b._rigidbody.angularVelocity * Mathf.Deg2Rad, rB);
+ closeVelocity = Vector3.Dot(relativeVelocity, contact.contactTangent1);
+ b = friction * closeVelocity;
+
+ float lambdaT = Mathf.Clamp(effectiveMass * (-(jv + b)), -maxFrictionForce, maxFrictionForce);
+
+ if (!contact.a._rigidbody.isStatic)
+ {
+ contact.a._rigidbody.linearVelocity += contact.a._rigidbody.inverseMass * j_va * lambdaT;
+ contact.a._rigidbody.angularVelocity += contact.a._rigidbody.localInverseInertiaTensor.Transform(j_wa) * lambdaT;
+ }
+ if (!contact.b._rigidbody.isStatic)
+ {
+ contact.b._rigidbody.linearVelocity += contact.b._rigidbody.inverseMass * j_vb * lambdaT;
+ contact.b._rigidbody.angularVelocity += contact.b._rigidbody.localInverseInertiaTensor.Transform(j_wb) * lambdaT;
+ }
+ //========================
+
+ //切向量碰撞處理2
+ j_va = contact.contactTangent2;
+ j_wa = Vector3.Cross(rA, contact.contactTangent2);
+ j_vb = -contact.contactTangent2;
+ j_wb = -Vector3.Cross(rB, contact.contactTangent2);
+
+ jv = Vector3.Dot(j_va, contact.a._rigidbody.linearVelocity) +
+ Vector3.Dot(j_wa, contact.a._rigidbody.angularVelocity * Mathf.Deg2Rad) +
+ Vector3.Dot(j_vb, contact.b._rigidbody.linearVelocity) +
+ Vector3.Dot(j_wb, contact.b._rigidbody.angularVelocity * Mathf.Deg2Rad);
+
+ relativeVelocity = -contact.a._rigidbody.linearVelocity
+ - Vector3.Cross(contact.a._rigidbody.angularVelocity * Mathf.Deg2Rad, rA)
+ contact.b._rigidbody.linearVelocity
+ Vector3.Cross(contact.b._rigidbody.angularVelocity * Mathf.Deg2Rad, rB);
+ closeVelocity = Vector3.Dot(relativeVelocity, contact.contactTangent2);
+ b = friction * closeVelocity;
+ lambdaT = Mathf.Clamp(effectiveMass * (-(jv + b)), -maxFrictionForce, maxFrictionForce);
+
+ if (!contact.a._rigidbody.isStatic)
+ {
+ contact.a._rigidbody.linearVelocity += contact.a._rigidbody.inverseMass * j_va * lambdaT;
+ contact.a._rigidbody.angularVelocity += contact.a._rigidbody.localInverseInertiaTensor.Transform(j_wa) * lambdaT;
+ }
+ if (!contact.b._rigidbody.isStatic)
+ {
+ contact.b._rigidbody.linearVelocity += contact.b._rigidbody.inverseMass * j_vb * lambdaT;
+ contact.b._rigidbody.angularVelocity += contact.b._rigidbody.localInverseInertiaTensor.Transform(j_wb) * lambdaT;
+ }
+ //========================
*/
}
public CollisionData(Collider collider, CollisionContact contact)
{
Collider = collider;
Contact = contact;
}
public CollisionData(Collider collider, CollisionContact contact)
{
Collider = collider;
Contact = contact;
}
private bool IsInternalEdge(CollisionContact contact)
{
return(false);
}
private bool CalculateCollisionCorrection(GameTime gameTime, ref CollisionContact contact)
{
//Non-stationary object
ICutlassCollidable first = contact.A;
//Stationary object
ICutlassCollidable second = contact.B;
Vector2 halfExtents = new Vector2(second.CurrentFrameBoundingRect.Width / 2, second.CurrentFrameBoundingRect.Height / 2);
BoundingRectangle firstPlusHalfExtents = new BoundingRectangle(first.CurrentFrameBoundingRect.Left - halfExtents.X,
first.CurrentFrameBoundingRect.Top - halfExtents.Y,
first.CurrentFrameBoundingRect.Width + 2 * halfExtents.X,
first.CurrentFrameBoundingRect.Height + 2 * halfExtents.Y);
//Get closest point
Vector2 closestPoint = second.CurrentFrameBoundingRect.Center;
//X Axis
float x = second.CurrentFrameBoundingRect.Center.X;
if (x < firstPlusHalfExtents.Left)
{
x = firstPlusHalfExtents.Left;
}
if (x > firstPlusHalfExtents.Right)
{
x = firstPlusHalfExtents.Right;
}
closestPoint.X = x;
//Y Axis
float y = second.CurrentFrameBoundingRect.Center.Y;
if (y < firstPlusHalfExtents.Top)
{
y = firstPlusHalfExtents.Top;
}
if (y > firstPlusHalfExtents.Bottom)
{
y = firstPlusHalfExtents.Bottom;
}
closestPoint.Y = y;
//Check if second's center is inside fisrtPlusHalfExtents. If so, find closest edge for negative distance.
if (closestPoint == second.CurrentFrameBoundingRect.Center)
{
Vector2 distanceToClosestEdge;
//X Axis
if (Math.Abs(closestPoint.X - firstPlusHalfExtents.Right) > Math.Abs(closestPoint.X - firstPlusHalfExtents.Left))
{
distanceToClosestEdge.X = closestPoint.X - firstPlusHalfExtents.Left;
}
else
{
distanceToClosestEdge.X = closestPoint.X - firstPlusHalfExtents.Right;
}
//Y Axis
if (Math.Abs(closestPoint.Y - firstPlusHalfExtents.Bottom) > Math.Abs(closestPoint.Y - firstPlusHalfExtents.Top))
{
distanceToClosestEdge.Y = closestPoint.Y - firstPlusHalfExtents.Top;
}
else
{
distanceToClosestEdge.Y = closestPoint.Y - firstPlusHalfExtents.Bottom;
}
//Minimum
Vector2 axisMinor = VectorUtilities.MinorAxis(distanceToClosestEdge);
closestPoint = closestPoint + axisMinor;
}
//Calculate distance and Normal
Vector2 distance = closestPoint - second.CurrentFrameBoundingRect.Center;
contact.Distance = VectorUtilities.MaximumComponent(distance);
if (contact.Distance == 0.0f)//right up against each other, will get invalid normal.
{
//On Top
if (first.CurrentFrameBoundingRect.Bottom == second.CurrentFrameBoundingRect.Top)
{
contact.Normal = new Vector2(0.0f, -1.0f);
if ((first.Side & CollisionSide.Bottom) == 0 ||
(second.Side & CollisionSide.Top) == 0)
{
return(false);
}
}
//To the Right
else if (first.CurrentFrameBoundingRect.Left == second.CurrentFrameBoundingRect.Right)
{
contact.Normal = new Vector2(1.0f, 0.0f);
if ((first.Side & CollisionSide.Left) == 0 ||
(second.Side & CollisionSide.Right) == 0)
{
return(false);
}
}
//On Bottom
else if (first.CurrentFrameBoundingRect.Top == second.CurrentFrameBoundingRect.Bottom)
{
contact.Normal = new Vector2(0.0f, 1.0f);
if ((first.Side & CollisionSide.Top) == 0 ||
(second.Side & CollisionSide.Bottom) == 0)
{
return(false);
}
}
//To the Left
else if (first.CurrentFrameBoundingRect.Right == second.CurrentFrameBoundingRect.Left)
{
contact.Normal = new Vector2(-1.0f, 0.0f);
if ((first.Side & CollisionSide.Right) == 0 ||
(second.Side & CollisionSide.Left) == 0)
{
return(false);
}
}
}
else
{
contact.Normal = VectorUtilities.NormalizedMajorAxis(distance);
}
//On top
if (contact.Normal.Y == -1.0f &&
((first.Side & CollisionSide.Bottom) == 0 ||
(second.Side & CollisionSide.Top) == 0))
{
return(false);
}
//To the Right
else if (contact.Normal.X == 1.0f &&
((first.Side & CollisionSide.Left) == 0 ||
(second.Side & CollisionSide.Right) == 0))
{
return(false);
}
//On Bottom
else if (contact.Normal.Y == 1.0f &&
((first.Side & CollisionSide.Top) == 0 ||
(second.Side & CollisionSide.Bottom) == 0))
{
return(false);
}
//To the Left
else if (contact.Normal.X == -1.0f &&
((first.Side & CollisionSide.Right) == 0 ||
(second.Side & CollisionSide.Left) == 0))
{
return(false);
}
Boolean returnValue = false;
//Collision Direction is X
if (Math.Abs(contact.Normal.X) >= Math.Abs(contact.Normal.Y) &&
(((first.Velocity.X * gameTime.ElapsedGameTime.TotalMilliseconds) + contact.Distance) * contact.Normal.X < 0))
{
returnValue = true;
}
//Collision Direction is Y
if (Math.Abs(contact.Normal.X) <= Math.Abs(contact.Normal.Y) &&
(((first.Velocity.Y * gameTime.ElapsedGameTime.TotalMilliseconds) + contact.Distance) * contact.Normal.Y < 0))
{
returnValue = true;
}
return(returnValue);
}
