public Contact(Collidable firstBody, Collidable secondBody) { this.body[0] = firstBody; this.body[1] = secondBody; ContactToWorld = new Matrix3(); restitution = ContactGenerator.restitution; friction = ContactGenerator.friction; }
public Contact(Collidable firstBody, Plane plane) { this.body[0] = firstBody; this.body[1] = null; this.WithPlane = true; this.plane = new HalfSpace(plane); ContactToWorld = new Matrix3(); restitution = 0.7f; friction = 0.3f; // TODO add a dynamic mechanism }
private Vector3 CalculateFrictionlessImpulse(Contact contactData, Matrix3[] inverseInertiaTensor) { Body one = contactData.body[0]; Body two = contactData.body[1]; Vector3 impulseContact; // Build a vector that shows the change in velocity in // world space for a unit impulse in the direction of the contact // normal. Vector3 torquePerUnitImpulse1 = Vector3.Cross(contactData.relativeContactPosition[0], contactData.ContactNormal); Vector3 rotationPerUnitImpluse1 = inverseInertiaTensor[0].transform(torquePerUnitImpulse1); Vector3 VelocityPerUnitImpulse1 = Vector3.Cross(rotationPerUnitImpluse1, contactData.relativeContactPosition[0]); // Work out the change in velocity in contact coordiantes. float deltaVelocity = Vector3.Dot(VelocityPerUnitImpulse1, contactData.ContactNormal); // Add the linear component of velocity change deltaVelocity += one.InverseMass; // Check if we need to the second body's data if (two != null) { // Go through the same transformation sequence again Vector3 torquePerUnitImpulse2 = Vector3.Cross(contactData.relativeContactPosition[1], contactData.ContactNormal); Vector3 rotationPerUnitImpluse2 = inverseInertiaTensor[1].transform(torquePerUnitImpulse2); Vector3 VelocityPerUnitImpulse2 = Vector3.Cross(rotationPerUnitImpluse2, contactData.relativeContactPosition[1]); // Add the change in velocity due to rotation deltaVelocity += Vector3.Dot(VelocityPerUnitImpulse2, contactData.ContactNormal); // Add the change in velocity due to linear motion deltaVelocity += two.InverseMass; } // Calculate the required size of the impulse impulseContact.X = contactData.desiredDeltaVelocity / deltaVelocity; impulseContact.Y = 0; impulseContact.Z = 0; return impulseContact; }
private Vector3 CalculateFrictionImpulse(Contact contactData, Matrix3[] inverseInertiaTensor) { Body one = contactData.body[0]; Body two = contactData.body[1]; Vector3 impulseContact; float inverseMass = one.InverseMass; // The equivalent of a cross product in matrices is multiplication // by a skew symmetric matrix - we build the matrix for converting // between linear and angular quantities. Matrix3 impulseToTorque = new Matrix3(); impulseToTorque.setSkewSymmetric(contactData.relativeContactPosition[0]); // Build the matrix to convert contact impulse to change in velocity // in world coordinates. Matrix3 deltaVelWorld = impulseToTorque; deltaVelWorld *= inverseInertiaTensor[0]; deltaVelWorld *= impulseToTorque; deltaVelWorld *= -1; // Check if we need to add body two's data if (two != null) { // Set the cross product matrix impulseToTorque.setSkewSymmetric(contactData.relativeContactPosition[1]); // Calculate the velocity change matrix Matrix3 deltaVelWorldTwo = impulseToTorque; deltaVelWorldTwo *= inverseInertiaTensor[1]; deltaVelWorldTwo *= impulseToTorque; deltaVelWorldTwo *= -1; // Add to the total delta velocity. deltaVelWorld += deltaVelWorldTwo; // Add to the inverse mass inverseMass += two.InverseMass; } // Do a change of basis to convert into contact coordinates. Matrix3 deltaVelocity = contactData.ContactToWorld.transpose(); deltaVelocity *= deltaVelWorld; deltaVelocity *= contactData.ContactToWorld; // Add in the linear velocity change deltaVelocity.data[0] += inverseMass; deltaVelocity.data[4] += inverseMass; deltaVelocity.data[8] += inverseMass; // Invert to get the impulse needed per unit velocity Matrix3 impulseMatrix = deltaVelocity.inverse(); // Find the velocities that will be removed Vector3 velKill = new Vector3(contactData.desiredDeltaVelocity, -contactData.contactVelocity.Y, -contactData.contactVelocity.Z); // Find the impulse to kill target velocities impulseContact = impulseMatrix.transform(velKill); // Check for exceeding friction float planarImpulse = (float)Math.Sqrt(Convert.ToDouble(impulseContact.Y * impulseContact.Y + impulseContact.Z * impulseContact.Z)); if ((planarImpulse > impulseContact.X * contactData.friction) && (planarImpulse != 0)) { // We need to use dynamic friction impulseContact.Y /= planarImpulse; impulseContact.Z /= planarImpulse; impulseContact.X = deltaVelocity.data[0] + deltaVelocity.data[1] * contactData.friction * impulseContact.Y + deltaVelocity.data[2] * contactData.friction * impulseContact.Z; impulseContact.X = contactData.desiredDeltaVelocity / impulseContact.X; impulseContact.Y *= contactData.friction * impulseContact.X; impulseContact.Z *= contactData.friction * impulseContact.X; } return impulseContact; }
public void ApplyVelocityChange(Contact contactData,out Vector3[] velocityChange,out Vector3[] rotationChange) { Body one = contactData.body[0]; Body two = contactData.body[1]; velocityChange = new Vector3[2]; rotationChange = new Vector3[2]; // Get hold of the inverse mass and inverse inertia tensor, both in // world coordinates. Matrix3[] inverseInertiaTensor = new Matrix3[2]; inverseInertiaTensor[0] = one.InverseInertiaTensorWorld; if (two != null) inverseInertiaTensor[1] = two.InverseInertiaTensorWorld; // We will calculate the impulse for each contact axis Vector3 impulseContact; if (contactData.friction == 0.0f) { //ther is no friction impulseContact = CalculateFrictionlessImpulse(contactData, inverseInertiaTensor); } else { // Otherwise we may have impulses that aren't in the direction of the // contact, so we need the more complex version. impulseContact = CalculateFrictionImpulse(contactData, inverseInertiaTensor); } // Convert impulse to world coordinates Vector3 impulse = contactData.ContactToWorld.transform(impulseContact); // Split in the impulse into linear and rotational components Vector3 impulsiveTorqueOne = Vector3.Cross(contactData.relativeContactPosition[0],impulse); rotationChange[0] = inverseInertiaTensor[0].transform(impulsiveTorqueOne); velocityChange[0] = impulse * one.InverseMass; // Apply the changes one.AddVelocity(velocityChange[0]); one.Rotation += rotationChange[0]; if (two != null) { // Work out body one's linear and angular changes Vector3 impulsiveTorqueTwo = Vector3.Cross(impulse,contactData.relativeContactPosition[1]); rotationChange[1] = inverseInertiaTensor[1].transform(impulsiveTorqueTwo); velocityChange[1] = -impulse * two.InverseMass; // And apply them. two.AddVelocity(velocityChange[1]); two.Rotation += rotationChange[1]; } }
///<summary> Returns a new matrix containing the transpose of this matrix. ///</summary> public Matrix3 transpose() { Matrix3 result = new Matrix3(); result.setTranspose(this); return result; }
///<summary> ///Interpolates a couple of matrices. ///</summary> static Matrix3 linearInterpolate(Matrix3 a, Matrix3 b, float prop) { Matrix3 result = new Matrix3(); for (uint i = 0; i < 9; i++) { result.data[i] = a.data[i] * (1 - prop) + b.data[i] * prop; } return result; }
///<summary> ///Sets the matrix to be the transpose of the given matrix. /// ///@param m The matrix to transpose and use to set this. ///</summary> public void setTranspose(Matrix3 m) { data[0] = m.data[0]; data[1] = m.data[3]; data[2] = m.data[6]; data[3] = m.data[1]; data[4] = m.data[4]; data[5] = m.data[7]; data[6] = m.data[2]; data[7] = m.data[5]; data[8] = m.data[8]; }
///<summary> ///Sets the matrix to be the inverse of the given matrix. /// ///@param m The matrix to invert and use to set this. ///</summary> public void setInverse(Matrix3 m) { float t4 = m.data[0] * m.data[4]; float t6 = m.data[0] * m.data[5]; float t8 = m.data[1] * m.data[3]; float t10 = m.data[2] * m.data[3]; float t12 = m.data[1] * m.data[6]; float t14 = m.data[2] * m.data[6]; /// Calculate the determinant float t16 = (t4 * m.data[8] - t6 * m.data[7] - t8 * m.data[8] + t10 * m.data[7] + t12 * m.data[5] - t14 * m.data[4]); /// Make sure the determinant is non-zero. if (t16 == (float)0.0f) return; float t17 = 1 / t16; data[0] = (m.data[4] * m.data[8] - m.data[5] * m.data[7]) * t17; data[1] = -(m.data[1] * m.data[8] - m.data[2] * m.data[7]) * t17; data[2] = (m.data[1] * m.data[5] - m.data[2] * m.data[4]) * t17; data[3] = -(m.data[3] * m.data[8] - m.data[5] * m.data[6]) * t17; data[4] = (m.data[0] * m.data[8] - t14) * t17; data[5] = -(t6 - t10) * t17; data[6] = (m.data[3] * m.data[7] - m.data[4] * m.data[6]) * t17; data[7] = -(m.data[0] * m.data[7] - t12) * t17; data[8] = (t4 - t8) * t17; }
///<summary> Returns a new matrix containing the inverse of this matrix. ///</summary> public Matrix3 inverse() { Matrix3 result = new Matrix3(); result.setInverse(this); return result; }
///<summary> ///Does a component-wise addition of this matrix and the given ///matrix. ///</summary> public static Matrix3 operator +(Matrix3 m, Matrix3 o) { Matrix3 result = new Matrix3( m.data[0] + o.data[0], m.data[1] + o.data[1], m.data[2] + o.data[2], m.data[3] + o.data[3], m.data[4] + o.data[4], m.data[5] + o.data[5], m.data[6] + o.data[6], m.data[7] + o.data[7], m.data[8] + o.data[8] ); return result; }
///<summary> ///Multiplies this matrix in place by the given scalar. ///</summary> public static Matrix3 operator *(Matrix3 m, float scalar) { Matrix3 result = new Matrix3( m.data[0] * scalar, m.data[1] * scalar, m.data[2] * scalar, m.data[3] * scalar, m.data[4] * scalar, m.data[5] * scalar, m.data[6] * scalar, m.data[7] * scalar, m.data[8] * scalar ); return result; }