///<summary> /// Integrates the rigid body forward in time by the given amount. /// This function uses a Newton-Euler integration method, which is a /// linear approximation to the correct integral. For this reason it /// may be inaccurate in some cases. ///</summary> public void Integrate(double dt) { if (!m_isAwake) { return; } // Calculate linear acceleration from force inputs. LastFrameAcceleration = m_acceleration; LastFrameAcceleration += m_forceAccum * InverseMass; // Calculate angular acceleration from torque inputs. Vector3d angularAcceleration = InverseInertiaTensorWorld * m_torqueAccum; // Adjust velocities // Update linear velocity from both acceleration and impulse. Velocity += LastFrameAcceleration * dt; // Update angular velocity from both acceleration and impulse. Rotation += angularAcceleration * dt; // Impose drag. Velocity *= Math.Pow(LinearDamping, dt); Rotation *= Math.Pow(AngularDamping, dt); // Adjust positions // Update linear position. Position += Velocity * dt; // Update angular position. Orientation.AddScaledVector(Rotation, dt); // Normalise the orientation, and update the matrices with the new // position and orientation CalculateDerivedData(); // Clear accumulators. ClearAccumulators(); // Update the kinetic energy store, and possibly put the body to // sleep. if (m_canSleep) { double currentMotion = Vector3d.Dot(Velocity, Velocity) + Vector3d.Dot(Rotation, Rotation); double bias = Math.Pow(0.5, dt); m_motion = bias * m_motion + (1 - bias) * currentMotion; if (m_motion < SleepEpsilon) { SetAwake(false); } else if (m_motion > 10 * SleepEpsilon) { m_motion = 10 * SleepEpsilon; } } }
///<summary> /// Performs an inertia weighted penetration resolution of this /// contact alone. ///</summary> public void ApplyPositionChange(Vector3d[] linearChange, Vector3d[] angularChange, double penetration) { double angularLimit = 0.2; double[] angularMove = new double[2]; double[] linearMove = new double[2]; double totalInertia = 0; double[] linearInertia = new double[2]; double[] angularInertia = new double[2]; // We need to work out the inertia of each object in the direction // of the contact normal, due to angular inertia only. for (int i = 0; i < 2; i++) { if (Body[i] == null) { continue; } Matrix3 inverseInertiaTensor = Body[i].InverseInertiaTensorWorld; // Use the same procedure as for calculating frictionless // velocity change to work out the angular inertia. Vector3d angularInertiaWorld = Vector3d.Cross(RelativeContactPosition[i], ContactNormal); angularInertiaWorld = inverseInertiaTensor.Transform(angularInertiaWorld); angularInertiaWorld = Vector3d.Cross(angularInertiaWorld, RelativeContactPosition[i]); angularInertia[i] = Vector3d.Dot(angularInertiaWorld, ContactNormal); // The linear component is simply the inverse mass linearInertia[i] = Body[i].InverseMass; // Keep track of the total inertia from all components totalInertia += linearInertia[i] + angularInertia[i]; // We break the loop here so that the totalInertia value is // completely calculated (by both iterations) before // continuing. } // Loop through again calculating and applying the changes for (int i = 0; i < 2; i++) { if (Body[i] == null) { continue; } // The linear and angular movements required are in proportion to // the two inverse inertias. double sign = (i == 0) ? 1 : -1; angularMove[i] = sign * penetration * (angularInertia[i] / totalInertia); linearMove[i] = sign * penetration * (linearInertia[i] / totalInertia); // To avoid angular projections that are too great (when mass is large // but inertia tensor is small) limit the angular move. Vector3d projection = RelativeContactPosition[i]; projection += ContactNormal * Vector3d.Dot(-RelativeContactPosition[i], ContactNormal); // Use the small angle approximation for the sine of the angle (i.e. // the magnitude would be sine(angularLimit) * projection.magnitude // but we approximate sine(angularLimit) to angularLimit). double maxMagnitude = angularLimit * projection.Magnitude; if (angularMove[i] < -maxMagnitude) { double totalMove = angularMove[i] + linearMove[i]; angularMove[i] = -maxMagnitude; linearMove[i] = totalMove - angularMove[i]; } else if (angularMove[i] > maxMagnitude) { double totalMove = angularMove[i] + linearMove[i]; angularMove[i] = maxMagnitude; linearMove[i] = totalMove - angularMove[i]; } // We have the linear amount of movement required by turning // the rigid body (in angularMove[i]). We now need to // calculate the desired rotation to achieve that. if (angularMove[i] == 0) { // Easy case - no angular movement means no rotation. angularChange[i] = Vector3d.Zero; } else { // Work out the direction we'd like to rotate in. Vector3d targetAngularDirection = Vector3d.Cross(RelativeContactPosition[i], ContactNormal); Matrix3 inverseInertiaTensor = Body[i].InverseInertiaTensorWorld; // Work out the direction we'd need to rotate to achieve that angularChange[i] = inverseInertiaTensor.Transform(targetAngularDirection) * (angularMove[i] / angularInertia[i]); } // Velocity change is easier - it is just the linear movement // along the contact normal. linearChange[i] = ContactNormal * linearMove[i]; // Now we can start to apply the values we've calculated. // Apply the linear movement Vector3d pos = Body[i].Position; pos += ContactNormal * linearMove[i]; Body[i].Position = pos; // And the change in orientation Quaternion q = Body[i].Orientation; q.AddScaledVector(angularChange[i], 1.0); q.Normalise(); Body[i].Orientation = q; // We need to calculate the derived data for any body that is // asleep, so that the changes are reflected in the object's // data. Otherwise the resolution will not change the position // of the object, and the next collision detection round will // have the same penetration. if (!Body[i].GetAwake()) { Body[i].CalculateDerivedData(); } } }