private Vector3 ComputeFrictionImpulsion(SortAndSweep.GameObjectsPair pair, CollisionInfo collision, float j)
{
RigidBodyScript rb1 = pair.gameObject1.GetComponent <RigidBodyScript>();
RigidBodyScript rb2 = pair.gameObject2.GetComponent <RigidBodyScript>();
Vector3 rv = rb2.velocity - rb1.velocity;
Vector3 tangent = (rv - Vector3.Dot(rv, collision.normalVector) * collision.normalVector).normalized;
float jt = -1f * Vector3.Dot(rv, tangent);
jt = jt / (rb1.invMass + rb2.invMass);
float mu = new Vector2(rb1.staticFriction, rb2.staticFriction).magnitude;
Vector3 frictionImpulse;
if (jt < j * mu)
{
frictionImpulse = jt * tangent;
}
else
{
float dynamicFriction = new Vector2(rb1.dynamicFriction, rb2.dynamicFriction).magnitude;
frictionImpulse = -1f * tangent * dynamicFriction;
}
return(frictionImpulse);
}
private void ResolveCollisionWithImpulse(SortAndSweep.GameObjectsPair pair, CollisionInfo collision)
{
GameObject gameObject1 = pair.gameObject1;
GameObject gameObject2 = pair.gameObject2;
RigidBodyScript rb1 = gameObject1.GetComponent <RigidBodyScript>();
RigidBodyScript rb2 = gameObject2.GetComponent <RigidBodyScript>();
Vector3 v1 = rb1.velocity;
Vector3 v2 = rb2.velocity;
if ((rb1.invMass != 0) || (rb2.invMass != 0))
{
Vector3 vrel = v1 - v2;
float elasticity = Mathf.Min(rb1.restitution, rb2.restitution);
float k = (elasticity + 1) * Vector3.Dot(vrel, collision.normalVector) / ((rb1.invMass + rb2.invMass) * Vector3.Dot(collision.normalVector, collision.normalVector));
Vector3 w1 = rb1.angularSpeed;
Vector3 w2 = rb2.angularSpeed;
Vector3 u1 = Vector3.Cross(w1 * Mathf.Deg2Rad, collision.collisionPoint - gameObject1.transform.position);
Vector3 u2 = Vector3.Cross(w2 * Mathf.Deg2Rad, collision.collisionPoint - gameObject2.transform.position);
Vector3 urel = u1 - u2;
Vector3 r1 = collision.collisionPoint - rb1.transform.position;
Vector3 r2 = collision.collisionPoint - rb2.transform.position;
float krnom = (elasticity + 1) * Vector3.Dot(urel, collision.normalVector);
Vector3 factor1 = (rb1.invMass + rb2.invMass) * collision.normalVector;
Vector3 factor2 = Vector3.Cross(rb1.inertiaMatrixInv * (Vector3.Cross(r1, collision.normalVector)), r1);
Vector3 factor3 = Vector3.Cross(rb2.inertiaMatrixInv * (Vector3.Cross(r2, collision.normalVector)), r2);
float kr = krnom / Vector3.Dot(factor1 + factor2 + factor3, collision.normalVector);
rb1.velocity -= k * collision.normalVector * rb1.invMass;
rb2.velocity += k * collision.normalVector * rb2.invMass;
rb1.angularSpeed -= (rb1.inertiaMatrixInv * Vector3.Cross(r1, kr * collision.normalVector)) * Mathf.Rad2Deg;
rb2.angularSpeed += (rb2.inertiaMatrixInv * Vector3.Cross(r2, kr * collision.normalVector)) * Mathf.Rad2Deg;
if (useFriction)
{
if (!USE_COMPUTED_FRICTION)
{
rb1.AddForce(-1f * rb1.staticFriction * u1);
rb2.AddForce(-1f * rb2.staticFriction * u2);
}
else
{
Vector3 frictionImpulse = ComputeFrictionImpulsion(pair, collision, k);
rb1.velocity -= frictionImpulse * rb1.invMass;
rb2.velocity += frictionImpulse * rb2.invMass;
}
}
PositionalCorrection(pair.gameObject1, pair.gameObject2, collision);
}
}
protected virtual void Awake()
{
rb = GetComponent <RigidBodyScript>();
}
