/// <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; }