/// <summary>
/// Initializes the constraint for this frame.
/// </summary>
/// <param name="dt">Time since the last frame.</param>
/// <param name="manifoldCenter">Computed center of manifold.</param>
internal void PreStep(float dt, out Vector3 manifoldCenter)
{
numIterationsAtZeroImpulse = 0;
parentA = pair.ParentA;
parentB = pair.ParentB;
contactCount = pair.Contacts.Count;
switch (contactCount)
{
case 1:
manifoldCenter = pair.Contacts[0].Position;
break;
case 2:
Vector3.Add(ref pair.Contacts[0].Position, ref pair.Contacts[1].Position, out manifoldCenter);
manifoldCenter.X *= .5f;
manifoldCenter.Y *= .5f;
manifoldCenter.Z *= .5f;
break;
case 3:
Vector3.Add(ref pair.Contacts[0].Position, ref pair.Contacts[1].Position, out manifoldCenter);
Vector3.Add(ref pair.Contacts[2].Position, ref manifoldCenter, out manifoldCenter);
manifoldCenter.X *= .333333333f;
manifoldCenter.Y *= .333333333f;
manifoldCenter.Z *= .333333333f;
break;
case 4:
//This isn't actually the center of the manifold. Is it good enough?
Vector3.Add(ref pair.Contacts[0].Position, ref pair.Contacts[1].Position, out manifoldCenter);
Vector3.Add(ref pair.Contacts[2].Position, ref manifoldCenter, out manifoldCenter);
Vector3.Add(ref pair.Contacts[3].Position, ref manifoldCenter, out manifoldCenter);
manifoldCenter.X *= .25f;
manifoldCenter.Y *= .25f;
manifoldCenter.Z *= .25f;
break;
default:
manifoldCenter = Toolbox.NoVector;
break;
}
//Compute the three dimensional relative velocity at the point.
Vector3 ra;
Vector3 rb;
Vector3.Subtract(ref manifoldCenter, ref parentA.position, out ra);
Vector3.Subtract(ref manifoldCenter, ref parentB.position, out rb);
Vector3 velocityA, velocityB;
Vector3.Cross(ref parentA.angularVelocity, ref ra, out velocityA);
Vector3.Add(ref velocityA, ref parentA.linearVelocity, out velocityA);
Vector3.Cross(ref parentB.angularVelocity, ref rb, out velocityB);
Vector3.Add(ref velocityB, ref parentB.linearVelocity, out velocityB);
Vector3 relativeVelocity;
Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity);
//Get rid of the normal velocity.
Vector3 normal = pair.Contacts[0].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 > pair.space.simulationSettings.CollisionResponse.StaticFrictionVelocityThreshold ? pair.DynamicFriction : pair.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 = pair.StaticFriction;
}
else
{
//Can't really do anything here, give up.
isActive = false;
return;
}
}
//maximumFrictionForce = 0;
//for (int i = 0; i < count; i++)
//{
// maximumFrictionForce += pair.contacts[i].penetrationConstraint.accumulatedImpulse;
//}
//maximumFrictionForce *= friction;
//linear axis 2 = normal x N
linearAX2 = (normal.Y * linearAZ) - (normal.Z * linearAY);
linearAY2 = (normal.Z * linearAX) - (normal.X * linearAZ);
linearAZ2 = (normal.X * linearAY) - (normal.Y * linearAX);
//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 A 2 = Ra x linear axis 2
angularAX2 = (ra.Y * linearAZ2) - (ra.Z * linearAY2);
angularAY2 = (ra.Z * linearAX2) - (ra.X * linearAZ2);
angularAZ2 = (ra.X * linearAY2) - (ra.Y * linearAX2);
//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);
//Angular B 2 = linear axis 2 x Rb
angularBX2 = (linearAY2 * rb.Z) - (linearAZ2 * rb.Y);
angularBY2 = (linearAZ2 * rb.X) - (linearAX2 * rb.Z);
angularBZ2 = (linearAX2 * rb.Y) - (linearAY2 * rb.X);
//Compute inverse effective mass matrix
float entryA, entryB;
float entryA2, entryB2;
//these are the transformed coordinates
float tX, tY, tZ;
float tX2, tY2, tZ2;
if (parentA.isDynamic)
{
tX = angularAX * parentA.inertiaTensorInverse.M11 + angularAY * parentA.inertiaTensorInverse.M21 + angularAZ * parentA.inertiaTensorInverse.M31;
tY = angularAX * parentA.inertiaTensorInverse.M12 + angularAY * parentA.inertiaTensorInverse.M22 + angularAZ * parentA.inertiaTensorInverse.M32;
tZ = angularAX * parentA.inertiaTensorInverse.M13 + angularAY * parentA.inertiaTensorInverse.M23 + angularAZ * parentA.inertiaTensorInverse.M33;
entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + 1 / parentA.mass;
tX2 = angularAX2 * parentA.inertiaTensorInverse.M11 + angularAY2 * parentA.inertiaTensorInverse.M21 + angularAZ2 * parentA.inertiaTensorInverse.M31;
tY2 = angularAX2 * parentA.inertiaTensorInverse.M12 + angularAY2 * parentA.inertiaTensorInverse.M22 + angularAZ2 * parentA.inertiaTensorInverse.M32;
tZ2 = angularAX2 * parentA.inertiaTensorInverse.M13 + angularAY2 * parentA.inertiaTensorInverse.M23 + angularAZ2 * parentA.inertiaTensorInverse.M33;
entryA2 = tX2 * angularAX2 + tY2 * angularAY2 * tZ2 * angularAZ2 + 1 / parentA.mass;
}
else
{
entryA = 0;
entryA2 = 0;
}
if (parentB.isDynamic)
{
tX = angularBX * parentB.inertiaTensorInverse.M11 + angularBY * parentB.inertiaTensorInverse.M21 + angularBZ * parentB.inertiaTensorInverse.M31;
tY = angularBX * parentB.inertiaTensorInverse.M12 + angularBY * parentB.inertiaTensorInverse.M22 + angularBZ * parentB.inertiaTensorInverse.M32;
tZ = angularBX * parentB.inertiaTensorInverse.M13 + angularBY * parentB.inertiaTensorInverse.M23 + angularBZ * parentB.inertiaTensorInverse.M33;
entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + 1 / parentB.mass;
tX2 = angularBX2 * parentB.inertiaTensorInverse.M11 + angularBY2 * parentB.inertiaTensorInverse.M21 + angularBZ2 * parentB.inertiaTensorInverse.M31;
tY2 = angularBX2 * parentB.inertiaTensorInverse.M12 + angularBY2 * parentB.inertiaTensorInverse.M22 + angularBZ2 * parentB.inertiaTensorInverse.M32;
tZ2 = angularBX2 * parentB.inertiaTensorInverse.M13 + angularBY2 * parentB.inertiaTensorInverse.M23 + angularBZ2 * parentB.inertiaTensorInverse.M33;
entryB2 = tX2 * angularBX2 + tY2 * angularBY2 + tZ2 * angularBZ2 + 1 / parentB.mass;
}
else
{
entryB = 0;
entryB2 = 0;
}
velocityToImpulse = -1 / (entryA + entryB); //Softness?
velocityToImpulse2 = -1 / (entryA2 + entryB2);
//Warm starting
#if !WINDOWS
Vector3 linear = new Vector3();
Vector3 angular = new Vector3();
#else
Vector3 linear, angular;
#endif
linear.X = accumulatedImpulse * linearAX;
linear.Y = accumulatedImpulse * linearAY;
linear.Z = accumulatedImpulse * linearAZ;
if (parentA.isDynamic)
{
angular.X = accumulatedImpulse * angularAX;
angular.Y = accumulatedImpulse * angularAY;
angular.Z = accumulatedImpulse * angularAZ;
parentA.ApplyLinearImpulse(ref linear);
parentA.ApplyAngularImpulse(ref angular);
}
if (parentB.isDynamic)
{
linear.X = -linear.X;
linear.Y = -linear.Y;
linear.Z = -linear.Z;
angular.X = accumulatedImpulse * angularBX;
angular.Y = accumulatedImpulse * angularBY;
angular.Z = accumulatedImpulse * angularBZ;
parentB.ApplyLinearImpulse(ref linear);
parentB.ApplyAngularImpulse(ref angular);
}
//Warm starting 2
linear.X = accumulatedImpulse2 * linearAX2;
linear.Y = accumulatedImpulse2 * linearAY2;
linear.Z = accumulatedImpulse2 * linearAZ2;
if (parentA.isDynamic)
{
angular.X = accumulatedImpulse2 * angularAX2;
angular.Y = accumulatedImpulse2 * angularAY2;
angular.Z = accumulatedImpulse2 * angularAZ2;
parentA.ApplyLinearImpulse(ref linear);
parentA.ApplyAngularImpulse(ref angular);
}
if (parentB.isDynamic)
{
linear.X = -linear.X;
linear.Y = -linear.Y;
linear.Z = -linear.Z;
angular.X = accumulatedImpulse2 * angularBX2;
angular.Y = accumulatedImpulse2 * angularBY2;
angular.Z = accumulatedImpulse2 * angularBZ2;
parentB.ApplyLinearImpulse(ref linear);
parentB.ApplyAngularImpulse(ref angular);
}
}
