public void PrepareForIteration(float timestep)
{
float dvx, dvy, dvz;
dvx = (body2.angularVelocity.Y * relativePos2.Z) - (body2.angularVelocity.Z * relativePos2.Y) + body2.linearVelocity.X;
dvy = (body2.angularVelocity.Z * relativePos2.X) - (body2.angularVelocity.X * relativePos2.Z) + body2.linearVelocity.Y;
dvz = (body2.angularVelocity.X * relativePos2.Y) - (body2.angularVelocity.Y * relativePos2.X) + body2.linearVelocity.Z;
dvx = dvx - (body1.angularVelocity.Y * relativePos1.Z) + (body1.angularVelocity.Z * relativePos1.Y) - body1.linearVelocity.X;
dvy = dvy - (body1.angularVelocity.Z * relativePos1.X) + (body1.angularVelocity.X * relativePos1.Z) - body1.linearVelocity.Y;
dvz = dvz - (body1.angularVelocity.X * relativePos1.Y) + (body1.angularVelocity.Y * relativePos1.X) - body1.linearVelocity.Z;
float kNormal = 0.0f;
JVector rantra = JVector.Zero;
if (!body1.isStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.X = (relativePos1.Y * normal.Z) - (relativePos1.Z * normal.Y);
rantra.Y = (relativePos1.Z * normal.X) - (relativePos1.X * normal.Z);
rantra.Z = (relativePos1.X * normal.Y) - (relativePos1.Y * normal.X);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
float num0 = ((rantra.X * body1.invInertiaWorld.M11) + (rantra.Y * body1.invInertiaWorld.M21)) + (rantra.Z * body1.invInertiaWorld.M31);
float num1 = ((rantra.X * body1.invInertiaWorld.M12) + (rantra.Y * body1.invInertiaWorld.M22)) + (rantra.Z * body1.invInertiaWorld.M32);
float num2 = ((rantra.X * body1.invInertiaWorld.M13) + (rantra.Y * body1.invInertiaWorld.M23)) + (rantra.Z * body1.invInertiaWorld.M33);
rantra.X = num0; rantra.Y = num1; rantra.Z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.Y * relativePos1.Z) - (rantra.Z * relativePos1.Y);
num1 = (rantra.Z * relativePos1.X) - (rantra.X * relativePos1.Z);
num2 = (rantra.X * relativePos1.Y) - (rantra.Y * relativePos1.X);
rantra.X = num0; rantra.Y = num1; rantra.Z = num2;
}
}
JVector rbntrb = JVector.Zero;
if (!body2.isStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.X = (relativePos2.Y * normal.Z) - (relativePos2.Z * normal.Y);
rbntrb.Y = (relativePos2.Z * normal.X) - (relativePos2.X * normal.Z);
rbntrb.Z = (relativePos2.X * normal.Y) - (relativePos2.Y * normal.X);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
float num0 = ((rbntrb.X * body2.invInertiaWorld.M11) + (rbntrb.Y * body2.invInertiaWorld.M21)) + (rbntrb.Z * body2.invInertiaWorld.M31);
float num1 = ((rbntrb.X * body2.invInertiaWorld.M12) + (rbntrb.Y * body2.invInertiaWorld.M22)) + (rbntrb.Z * body2.invInertiaWorld.M32);
float num2 = ((rbntrb.X * body2.invInertiaWorld.M13) + (rbntrb.Y * body2.invInertiaWorld.M23)) + (rbntrb.Z * body2.invInertiaWorld.M33);
rbntrb.X = num0; rbntrb.Y = num1; rbntrb.Z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.Y * relativePos2.Z) - (rbntrb.Z * relativePos2.Y);
num1 = (rbntrb.Z * relativePos2.X) - (rbntrb.X * relativePos2.Z);
num2 = (rbntrb.X * relativePos2.Y) - (rbntrb.Y * relativePos2.X);
rbntrb.X = num0; rbntrb.Y = num1; rbntrb.Z = num2;
}
}
if (!body1.isStatic) kNormal += rantra.X * normal.X + rantra.Y * normal.Y + rantra.Z * normal.Z;
if (!body2.isStatic) kNormal += rbntrb.X * normal.X + rbntrb.Y * normal.Y + rbntrb.Z * normal.Z;
massNormal = 1.0f / kNormal;
float num = dvx * normal.X + dvy * normal.Y + dvz * normal.Z;
tangent.X = dvx - normal.X * num;
tangent.Y = dvy - normal.Y * num;
tangent.Z = dvz - normal.Z * num;
num = tangent.X * tangent.X + tangent.Y * tangent.Y + tangent.Z * tangent.Z;
if (num != 0.0f)
{
num = (float)Math.Sqrt(num);
tangent.X /= num;
tangent.Y /= num;
tangent.Z /= num;
}
float kTangent = 0.0f;
if (body1.isStatic) rantra.MakeZero();
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.X = (relativePos1.Y * tangent.Z) - (relativePos1.Z * tangent.Y);
rantra.Y = (relativePos1.Z * tangent.X) - (relativePos1.X * tangent.Z);
rantra.Z = (relativePos1.X * tangent.Y) - (relativePos1.Y * tangent.X);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
float num0 = ((rantra.X * body1.invInertiaWorld.M11) + (rantra.Y * body1.invInertiaWorld.M21)) + (rantra.Z * body1.invInertiaWorld.M31);
float num1 = ((rantra.X * body1.invInertiaWorld.M12) + (rantra.Y * body1.invInertiaWorld.M22)) + (rantra.Z * body1.invInertiaWorld.M32);
float num2 = ((rantra.X * body1.invInertiaWorld.M13) + (rantra.Y * body1.invInertiaWorld.M23)) + (rantra.Z * body1.invInertiaWorld.M33);
rantra.X = num0; rantra.Y = num1; rantra.Z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.Y * relativePos1.Z) - (rantra.Z * relativePos1.Y);
num1 = (rantra.Z * relativePos1.X) - (rantra.X * relativePos1.Z);
num2 = (rantra.X * relativePos1.Y) - (rantra.Y * relativePos1.X);
rantra.X = num0; rantra.Y = num1; rantra.Z = num2;
}
}
if (body2.isStatic) rbntrb.MakeZero();
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.X = (relativePos2.Y * tangent.Z) - (relativePos2.Z * tangent.Y);
rbntrb.Y = (relativePos2.Z * tangent.X) - (relativePos2.X * tangent.Z);
rbntrb.Z = (relativePos2.X * tangent.Y) - (relativePos2.Y * tangent.X);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
float num0 = ((rbntrb.X * body2.invInertiaWorld.M11) + (rbntrb.Y * body2.invInertiaWorld.M21)) + (rbntrb.Z * body2.invInertiaWorld.M31);
float num1 = ((rbntrb.X * body2.invInertiaWorld.M12) + (rbntrb.Y * body2.invInertiaWorld.M22)) + (rbntrb.Z * body2.invInertiaWorld.M32);
float num2 = ((rbntrb.X * body2.invInertiaWorld.M13) + (rbntrb.Y * body2.invInertiaWorld.M23)) + (rbntrb.Z * body2.invInertiaWorld.M33);
rbntrb.X = num0; rbntrb.Y = num1; rbntrb.Z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.Y * relativePos2.Z) - (rbntrb.Z * relativePos2.Y);
num1 = (rbntrb.Z * relativePos2.X) - (rbntrb.X * relativePos2.Z);
num2 = (rbntrb.X * relativePos2.Y) - (rbntrb.Y * relativePos2.X);
rbntrb.X = num0; rbntrb.Y = num1; rbntrb.Z = num2;
}
}
if (!body1.isStatic) kTangent += JVector.Dot(ref rantra, ref tangent);
if (!body2.isStatic) kTangent += JVector.Dot(ref rbntrb, ref tangent);
massTangent = 1.0f / kTangent;
restitutionBias = 0.0f;
float relNormalVel = normal.X * dvx + normal.Y * dvy + normal.Z * dvz; //JVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (1.0f / timestep) * JMath.Max(0.0f, Penetration - settings.allowedPenetration);
restitutionBias = JMath.Clamp(restitutionBias, 0.0f, settings.maximumBias);
}
float timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
float relTangentVel = -(tangent.X * dvx + tangent.Y * dvy + tangent.Z * dvz);
float tangentImpulse = massTangent * relTangentVel;
float maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse) friction = dynamicFriction;
else friction = staticFriction;
}
JVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
if (relNormalVel < -1.0f && newContact)
{
restitutionBias = Math.Max(-restitution * relNormalVel, restitutionBias);
}
impulse.X = normal.X * accumulatedNormalImpulse + tangent.X * accumulatedTangentImpulse;
impulse.Y = normal.Y * accumulatedNormalImpulse + tangent.Y * accumulatedTangentImpulse;
impulse.Z = normal.Z * accumulatedNormalImpulse + tangent.Z * accumulatedTangentImpulse;
if (!body1.isStatic)
{
body1.linearVelocity.X -= (impulse.X * body1.inverseMass);
body1.linearVelocity.Y -= (impulse.Y * body1.inverseMass);
body1.linearVelocity.Z -= (impulse.Z * body1.inverseMass);
if (!body1IsMassPoint)
{
float num0, num1, num2;
num0 = relativePos1.Y * impulse.Z - relativePos1.Z * impulse.Y;
num1 = relativePos1.Z * impulse.X - relativePos1.X * impulse.Z;
num2 = relativePos1.X * impulse.Y - relativePos1.Y * impulse.X;
float num3 =
(((num0 * body1.invInertiaWorld.M11) +
(num1 * body1.invInertiaWorld.M21)) +
(num2 * body1.invInertiaWorld.M31));
float num4 =
(((num0 * body1.invInertiaWorld.M12) +
(num1 * body1.invInertiaWorld.M22)) +
(num2 * body1.invInertiaWorld.M32));
float num5 =
(((num0 * body1.invInertiaWorld.M13) +
(num1 * body1.invInertiaWorld.M23)) +
(num2 * body1.invInertiaWorld.M33));
body1.angularVelocity.X -= num3;
body1.angularVelocity.Y -= num4;
body1.angularVelocity.Z -= num5;
}
}
if (!body2.isStatic)
{
body2.linearVelocity.X += (impulse.X * body2.inverseMass);
body2.linearVelocity.Y += (impulse.Y * body2.inverseMass);
body2.linearVelocity.Z += (impulse.Z * body2.inverseMass);
if (!body2IsMassPoint)
{
float num0, num1, num2;
num0 = relativePos2.Y * impulse.Z - relativePos2.Z * impulse.Y;
num1 = relativePos2.Z * impulse.X - relativePos2.X * impulse.Z;
num2 = relativePos2.X * impulse.Y - relativePos2.Y * impulse.X;
float num3 =
(((num0 * body2.invInertiaWorld.M11) +
(num1 * body2.invInertiaWorld.M21)) +
(num2 * body2.invInertiaWorld.M31));
float num4 =
(((num0 * body2.invInertiaWorld.M12) +
(num1 * body2.invInertiaWorld.M22)) +
(num2 * body2.invInertiaWorld.M32));
float num5 =
(((num0 * body2.invInertiaWorld.M13) +
(num1 * body2.invInertiaWorld.M23)) +
(num2 * body2.invInertiaWorld.M33));
body2.angularVelocity.X += num3;
body2.angularVelocity.Y += num4;
body2.angularVelocity.Z += num5;
}
}
lastTimeStep = timestep;
newContact = false;
}
