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