public override void Update(float dt)
{
entityAIsDynamic = entityA != null && entityA.isDynamic;
entityBIsDynamic = entityB != null && entityB.isDynamic;
//Compute the three dimensional relative velocity at the point.
Vector3 velocityA = new Vector3(), velocityB = new Vector3();
Vector3 ra = penetrationConstraint.ra, rb = penetrationConstraint.rb;
if (entityA != null)
{
Vector3.Cross(ref entityA.angularVelocity, ref ra, out velocityA);
Vector3.Add(ref velocityA, ref entityA.linearVelocity, out velocityA);
}
if (entityB != null)
{
Vector3.Cross(ref entityB.angularVelocity, ref rb, out velocityB);
Vector3.Add(ref velocityB, ref entityB.linearVelocity, out velocityB);
}
Vector3 relativeVelocity;
Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity);
//Get rid of the normal velocity.
Vector3 normal = penetrationConstraint.contact.Normal;
float normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y + normal.Z * relativeVelocity.Z;
relativeVelocity.X -= normalVelocityScalar * normal.X;
relativeVelocity.Y -= normalVelocityScalar * normal.Y;
relativeVelocity.Z -= normalVelocityScalar * normal.Z;
//Create the jacobian entry and decide the friction coefficient.
float length = relativeVelocity.LengthSquared();
if (length > Toolbox.Epsilon)
{
length = (float)Math.Sqrt(length);
linearAX = relativeVelocity.X / length;
linearAY = relativeVelocity.Y / length;
linearAZ = relativeVelocity.Z / length;
friction = length > CollisionResponseSettings.StaticFrictionVelocityThreshold
? contactManifoldConstraint.materialInteraction.KineticFriction
: contactManifoldConstraint.materialInteraction.StaticFriction;
}
else
{
//If there's no velocity, there's no jacobian. Give up.
//This is 'fast' in that it will early out on essentially resting objects,
//but it may introduce instability.
//If it doesn't look good, try the next approach.
//isActive = false;
//return;
//if the above doesn't work well, try using the previous frame's jacobian.
if (linearAX != 0 || linearAY != 0 || linearAZ != 0)
{
friction = contactManifoldConstraint.materialInteraction.StaticFriction;
}
else
{
//Can't really do anything here, give up.
isActiveInSolver = false;
return;
//Could also cross the up with normal to get a random direction. Questionable value.
}
}
//angular A = Ra x N
angularAX = (ra.Y * linearAZ) - (ra.Z * linearAY);
angularAY = (ra.Z * linearAX) - (ra.X * linearAZ);
angularAZ = (ra.X * linearAY) - (ra.Y * linearAX);
//Angular B = N x Rb
angularBX = (linearAY * rb.Z) - (linearAZ * rb.Y);
angularBY = (linearAZ * rb.X) - (linearAX * rb.Z);
angularBZ = (linearAX * rb.Y) - (linearAY * rb.X);
//Compute inverse effective mass matrix
float entryA, entryB;
//these are the transformed coordinates
float tX, tY, tZ;
if (entityAIsDynamic)
{
tX = angularAX * entityA.inertiaTensorInverse.M11 + angularAY * entityA.inertiaTensorInverse.M21 + angularAZ * entityA.inertiaTensorInverse.M31;
tY = angularAX * entityA.inertiaTensorInverse.M12 + angularAY * entityA.inertiaTensorInverse.M22 + angularAZ * entityA.inertiaTensorInverse.M32;
tZ = angularAX * entityA.inertiaTensorInverse.M13 + angularAY * entityA.inertiaTensorInverse.M23 + angularAZ * entityA.inertiaTensorInverse.M33;
entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + entityA.inverseMass;
}
else
entryA = 0;
if (entityBIsDynamic)
{
tX = angularBX * entityB.inertiaTensorInverse.M11 + angularBY * entityB.inertiaTensorInverse.M21 + angularBZ * entityB.inertiaTensorInverse.M31;
tY = angularBX * entityB.inertiaTensorInverse.M12 + angularBY * entityB.inertiaTensorInverse.M22 + angularBZ * entityB.inertiaTensorInverse.M32;
tZ = angularBX * entityB.inertiaTensorInverse.M13 + angularBY * entityB.inertiaTensorInverse.M23 + angularBZ * entityB.inertiaTensorInverse.M33;
entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + entityB.inverseMass;
}
else
entryB = 0;
velocityToImpulse = -1 / (entryA + entryB); //Softness?
}
