/// <summary>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
if (isParticle)
{
this.inertia = JMatrix.Zero;
this.invInertia = this.invInertiaWorld = JMatrix.Zero;
this.invOrientation = this.orientation = JMatrix.Identity;
this.boundingBox = shape.boundingBox;
JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min);
JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max);
angularVelocity.MakeZero();
}
else
{
// Given: Orientation, Inertia
JMatrix.Transpose(ref orientation, out invOrientation);
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min);
JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max);
if (!isStatic)
{
JMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
JMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
}
}
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(float timestep)
{
float dvx, dvy, dvz;
if (considerAngularVelocity)
{
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;
}
else
{
dvx = body2.linearVelocity.X;
dvy = body2.linearVelocity.Y;
dvz = body2.linearVelocity.Z;
dvx = dvx - body1.linearVelocity.X;
dvy = dvy - body1.linearVelocity.Y;
dvz = dvz - body1.linearVelocity.Z;
}
float kNormal = 0.0f;
JVector rantra = JVector.Zero;
if (!treatBody1AsStatic)
{
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 (!treatBody2AsStatic)
{
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 (!treatBody1AsStatic)
{
kNormal += rantra.X * normal.X + rantra.Y * normal.Y + rantra.Z * normal.Z;
}
if (!treatBody2AsStatic)
{
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 (treatBody1AsStatic)
{
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 (treatBody2AsStatic)
{
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 (!treatBody1AsStatic)
{
kTangent += JVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += JVector.Dot(ref rbntrb, ref tangent);
}
massTangent = 1.0f / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = 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);
// body1IsMassPoint = body2IsMassPoint = false;
}
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);
}
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = 0.0f;
}
else
{
lostSpeculativeBounce = 0.0f;
}
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 (!treatBody1AsStatic)
{
body1.linearVelocity.X -= (impulse.X * body1.inverseMass);
body1.linearVelocity.Y -= (impulse.Y * body1.inverseMass);
body1.linearVelocity.Z -= (impulse.Z * body1.inverseMass);
if (!body1IsMassPoint && considerAngularVelocity)
{
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 (!treatBody2AsStatic)
{
body2.linearVelocity.X += (impulse.X * body2.inverseMass);
body2.linearVelocity.Y += (impulse.Y * body2.inverseMass);
body2.linearVelocity.Z += (impulse.Z * body2.inverseMass);
if (!body2IsMassPoint && considerAngularVelocity)
{
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;
}
