Quaternion Mult(Quaternion a, Quaternion b)
{
#if true
Quaternion c;
float aW = a.W, aX = a.X, aY = a.Y, aZ = a.Z;
float bW = b.W, bX = b.X, bY = b.Y, bZ = b.Z;
c.X = (aW * bX) + (aX * bW) + (aY * bZ) - (aZ * bY);
c.Y = (aW * bY) + (aY * bW) + (aZ * bX) - (aX * bZ);
c.Z = (aW * bZ) + (aZ * bW) + (aX * bY) - (aY * bX);
c.W = (aW * bW) - (aX * bX) - (aY * bY) - (aZ * bZ);
// Debug Speedup Checks - slight error difference between our
// custom Quaternion calculation and the API one - possibly due
// to internal xna speedups?
Quaternion check = Quaternion.Multiply(a, b);
float err = 0.01f;
Debug_c.Assert((Math.Abs(check.X - c.X) < err) &&
(Math.Abs(check.Y - c.Y) < err) &&
(Math.Abs(check.Z - c.Z) < err) &&
(Math.Abs(check.W - c.W) < err));
c = check;
return(c);
#else
return(Quaternion.Multiply(a, b));
#endif
}
public Matrix inv_I; // inverse intertia tensor
public void UpdateVel(float dt)
{
if (m > 0.0f)
{
v += new Vector3(0, -109.8f, 0) * dt;
Debug_c.Valid(v);
}
}
void Valid(Matrix m)
{
MyMatrix temp = new MyMatrix(m);
for (int i = 0; i < 4; i++)
{
for (int k = 0; k < 4; k++)
{
Debug_c.Assert(float.IsNaN(temp[i, k]) == false);
}
}
}
public void UpdatePos(float dt)
{
if (m > 0.0f)
{
x += v * dt;
Debug_c.Valid(x);
Quaternion temp = MyMath.Mult(new Quaternion(omega.X, omega.Y, omega.Z, 0), q) * 0.5f;
q = q + temp * dt;
q.Normalize();
}
}
void Update(float dt)
{
Contact_c.gTimeStamp++;
//float linDrag = 0.99f;
//float angDrag = 0.98f;
/*
* //*****Integrate******
* for (int i=0; i < m_rigidBodies.Count; i++)
* {
* Body_c b = m_rigidBodies[i].body;
*
* b.x += b.v * dt;
* Debug_c.Valid(b.x);
*
* b.v += new Vector3(0, -400.8f, 0) * dt * b.m;
* Debug_c.Valid(b.v);
*
* Quaternion temp = MyMath.Mult(new Quaternion(b.omega.X, b.omega.Y, b.omega.Z, 0), b.q) * 0.5f;
* b.q = b.q + temp * dt;
* b.q.Normalize();
*
* b.v *= linDrag;
* b.omega *= angDrag;
* Debug_c.Valid(b.omega);
* }
*/
#if USE_IMPULSES
m_prevSteps[m_curStep].Clear();
for (int i = 0; i < m_rigidBodies.Count; i++)
{
m_prevSteps[m_curStep].Add(new Body_c(m_rigidBodies[i].body));
}
m_curStep = (m_curStep + 1) % m_prevSteps.Count();
// Process all collisions
//if (false)
{
for (int i = 0; i < m_rigidBodies.Count; i++)
{
Body_c b = m_rigidBodies[i].body;
b.StoreState();
}
for (int i = 0; i < m_rigidBodies.Count; i++)
{
Body_c b = m_rigidBodies[i].body;
b.UpdateVel(dt);
b.UpdatePos(dt);
}
CheckCollisions();
ArbiterContainer_c.SortInYDirection();
for (int i = 0; i < m_rigidBodies.Count; i++)
{
Body_c b = m_rigidBodies[i].body;
b.RestoreState();
}
for (int iteration = 0; iteration < 4; iteration++)
{
for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++)
{
for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++)
{
Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a];
bool zapIt = false;
if (contact.Distance() > 0.01f && Contact_c.gTimeStamp != contact.timeStamp)
{
zapIt = true;
}
if (zapIt)
{
ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a);
a--;
continue;
}
contact.constraint.GenerateImpulse(0.9f);
}
}
}
}
// Update Velocity
for (int i = 0; i < m_rigidBodies.Count; i++)
{
Body_c b = m_rigidBodies[i].body;
b.UpdateVel(dt);
}
// Process Contacts
//if (false)
{
for (int i = 0; i < m_rigidBodies.Count; i++)
{
Body_c b = m_rigidBodies[i].body;
b.StoreState();
}
for (int i = 0; i < m_rigidBodies.Count; i++)
{
Body_c b = m_rigidBodies[i].body;
b.UpdatePos(dt);
}
CheckCollisions();
ArbiterContainer_c.SortInYDirection();
for (int i = 0; i < m_rigidBodies.Count; i++)
{
Body_c b = m_rigidBodies[i].body;
b.RestoreState();
}
// For the shock propogation - should do a sort on order
// but since this is our test code, I know I've added the rigidbodies
// to the list in the order from bottom to top
//if (false)
for (int iteration = 0; iteration < 90; iteration++)
{
for (int i = 0; i < m_rigidBodies.Count; i++)
{
RigidBody_c b1 = m_rigidBodies[i];
b1.body.inv_m = 0.0f;
//int j = i+1;
//if (j>m_rigidBodies.Count-1) continue;
for (int j = i + 1; j < m_rigidBodies.Count; j++)
{
RigidBody_c b2 = m_rigidBodies[j];
float cullingRadius = b1.maxRadius + b2.maxRadius;
if ((b1.body.x - b2.body.x).LengthSquared() > cullingRadius * cullingRadius)
{
continue;
}
b1.body.StoreState();
b2.body.StoreState();
b1.body.UpdatePos(dt);
b2.body.UpdatePos(dt);
/*
* for (int mm=0; mm < m_rigidBodies.Count; mm++)
* {
* Body_c b = m_rigidBodies[mm].body;
* b.StoreState();
* }
* for (int mm=0; mm < m_rigidBodies.Count; mm++)
* {
* Body_c b = m_rigidBodies[mm].body;
* b.UpdatePos(dt);
* }
*/
Vector3 n = Vector3.Zero;
Vector3 p1 = Vector3.Zero;
Vector3 p2 = Vector3.Zero;
bool haveHit =
Intersection_c.HasIntersection(b1.collideModel, b1.body.q, b1.body.x - MyMath.Rotate(b1.body.q, b1.body.com),
b2.collideModel, b2.body.q, b2.body.x - MyMath.Rotate(b2.body.q, b2.body.com),
out n,
out p1,
out p2);
/*
* for (int mm=0; mm < m_rigidBodies.Count; mm++)
* {
* Body_c b = m_rigidBodies[mm].body;
* b.RestoreState();
* }
*/
b1.body.RestoreState();
b2.body.RestoreState();
if (haveHit)
{
Contact_c c = new Contact_c(b1, b2, p1, p2, n);
c.constraint.GenerateImpulse(0.0f);
}
}
}
for (int i = 0; i < m_rigidBodies.Count; i++)
{
m_rigidBodies[i].body.inv_m = m_rigidBodies[i].body.inv_m_back;
}
}
int numContactSteps = 2;
if (false)
{
for (int step = 0; step < numContactSteps; step++)
{
for (int iteration = 0; iteration < 5; iteration++)
{
for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++)
{
for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++)
{
/*
* for (int i=0; i < m_rigidBodies.Count; i++)
* {
* Body_c b = m_rigidBodies[i].body;
* b.StoreState();
* }
* for (int i=0; i < m_rigidBodies.Count; i++)
* {
* Body_c b = m_rigidBodies[i].body;
* b.UpdatePos(dt);
* }
* CheckCollisions();
* for (int i=0; i < m_rigidBodies.Count; i++)
* {
* Body_c b = m_rigidBodies[i].body;
* b.RestoreState();
* }
*/
Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a];
bool zapIt = false;
if (contact.Distance() > 0.01f && Contact_c.gTimeStamp != contact.timeStamp)
{
zapIt = true;
}
if (zapIt)
{
ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a);
a--;
continue;
}
float ee = (numContactSteps - step - 1) * -1.0f / (float)numContactSteps;
//if ( Math.Abs(contact.timeStamp - Contact_c.gTimeStamp) > 2 ) ee = -0.8f;
//ee = 0.0f;
contact.constraint.GenerateImpulse(ee);
}
}
}
}
}
// Shock propogation
if (false)
{
for (int iteration = 0; iteration < 5; iteration++)
{
for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++)
{
for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++)
{
Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a];
if (ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count > 0)
{
ArbiterContainer_c.arbiterArray[k].arbiter.contacts[0].b1.body.inv_m = 0.0f;
}
bool zapIt = false;
if (contact.Distance() > 0.1f && Contact_c.gTimeStamp != contact.timeStamp)
{
zapIt = true;
}
//if ( Math.Abs(contact.timeStamp - Contact_c.gTimeStamp) > 30 ) zapIt = true;
if (zapIt)
{
ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a);
a--;
continue;
}
contact.constraint.GenerateImpulse(0.0f);
}
}
for (int i = 0; i < m_rigidBodies.Count; i++)
{
m_rigidBodies[i].body.inv_m = m_rigidBodies[i].body.inv_m_back;
}
}
}
}
// Update Positions
for (int i = 0; i < m_rigidBodies.Count; i++)
{
Body_c b = m_rigidBodies[i].body;
b.UpdatePos(dt);
//b.v *= linDrag;
b.omega *= 0.95f; //angDrag;
Debug_c.Valid(b.omega);
}
#endif //USE_IMPULSES
// Resolve momentum exchanges
#if !USE_IMPULSES
for (int i = 0; i < m_rigidBodies.Count; i++)
{
Body_c b = m_rigidBodies[i].body;
b.UpdateVel(dt);
b.UpdatePos(dt);
}
CheckCollisions();
ArbiterContainer_c.SortInYDirection();
for (int iteration = 0; iteration < 10; iteration++)
{
for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++)
{
for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++)
{
Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a];
bool zapIt = false;
if (contact.Distance() > 0.1f && Contact_c.gTimeStamp != contact.timeStamp)
{
zapIt = true;
}
if (zapIt)
{
ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a);
a--;
continue;
}
if (iteration == 0)
{
contact.constraint.PrepareForIteration();
}
else
{
contact.constraint.Iterate();
}
}
}
}
#endif
}
void Valid(float v)
{
Debug_c.Assert(float.IsNaN(v) == false);
}
void Valid(Vector3 v)
{
Debug_c.Assert(float.IsNaN(v.X) == false);
Debug_c.Assert(float.IsNaN(v.Y) == false);
Debug_c.Assert(float.IsNaN(v.Z) == false);
}
void ComputeMassProperties(Body_c body, HullMaker model, float density)
{
MyVector3 diag = new MyVector3(Vector3.Zero);
MyVector3 offDiag = new MyVector3(Vector3.Zero);
Vector3 weightedCenterOfMass = Vector3.Zero;
float volume = 0;
float mass = 0;
// Iterate through the faces
for (int faceIndex = 0; faceIndex < model.surfaceTriList.Count; faceIndex++)
{
HullMaker.ClipTri face = model.surfaceTriList[faceIndex];
// Iterate through the tris in the face
for (int triIndex = 0; triIndex < 3; triIndex++)
{
MyVector3 v0 = new MyVector3(face.n1);
MyVector3 v1 = new MyVector3(face.n2);
MyVector3 v2 = new MyVector3(face.n3);
float det = Det(v0.V3(), v1.V3(), v2.V3());
// Volume
float tetVolume = det / 6.0f;
volume += tetVolume;
// Mass
float tetMass = tetVolume * density;
mass += tetMass;
// Center of Mass
Vector3 tetCenterOfMass = ((v0 + v1 + v2) / 4.0f).V3(); // Note: includes origin (0, 0, 0) as fourth vertex
weightedCenterOfMass += tetMass * tetCenterOfMass;
// Inertia Tensor
for (int i = 0; i < 3; i++)
{
int j = (i + 1) % 3;
int k = (i + 2) % 3;
diag[i] += det * (v0[i] * v1[i] + v1[i] * v2[i] + v2[i] * v0[i] + v0[i] * v0[i] + v1[i] * v1[i] + v2[i] * v2[i]) / 60.0f;
offDiag[i] += det * (
v0[j] * v1[k] + v1[j] * v2[k] + v2[j] * v0[k] +
v0[j] * v2[k] + v1[j] * v0[k] + v2[j] * v1[k] +
2 * v0[j] * v0[k] + 2 * v1[j] * v1[k] + 2 * v2[j] * v2[k]) / 120.0f;
}
}
}
Debug_c.Assert(mass > 0);
if (mass == 0.0f)
{
mass = 5.0f;
}
Vector3 centerOfMass = weightedCenterOfMass / mass;
diag *= density;
offDiag *= density;
MyMatrix I = new MyMatrix(Matrix.Identity);
I[0, 0] = diag[1] + diag[2];
I[1, 1] = diag[2] + diag[0];
I[2, 2] = diag[0] + diag[1];
I[1, 2] = I[2, 1] = -offDiag[0];
I[0, 2] = I[2, 0] = -offDiag[1];
I[0, 1] = I[1, 0] = -offDiag[2];
///
// Move inertia tensor to be relative to center of mass (rather than origin)
// Translate intertia to center of mass
float x = centerOfMass.X;
float y = centerOfMass.Y;
float z = centerOfMass.Z;
//Debug_c.Assert(Math.Abs(x)>0);
//Debug_c.Assert(Math.Abs(y)>0);
//Debug_c.Assert(Math.Abs(z)>0);
//if (x==0.0f) x = 1.0f;
//if (y==0.0f) y = 1.0f;
//if (z==0.0f) z = 1.0f;
I[0, 0] -= mass * (y * y + z * z);
I[0, 1] -= mass * (-x * y);
I[0, 2] -= mass * (-x * z);
I[1, 1] -= mass * (x * x + z * z);
I[1, 2] -= mass * (-y * z);
I[2, 2] -= mass * (x * x + y * y);
// Symmetry
I[1, 0] = I[0, 1];
I[2, 0] = I[0, 2];
I[2, 1] = I[1, 2];
float check = 0.0f;
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
check += I[r, c];
}
}
Debug_c.Assert(Math.Abs(check) > 0.0f);
if (check == 0.0f)
{
I = new MyMatrix(Matrix.Identity);
}
body.com = centerOfMass;
body.inv_m = 1.0f / mass;
body.inv_m_back = body.inv_m;
body.I = I.Get();
GeneralInverse4x4(out body.inv_I, ref I);
//body.inv_I = Matrix.Invert( I.Get() );
Debug_c.Valid(body.com);
Debug_c.Valid(body.inv_m);
Debug_c.Valid(body.inv_I);
Debug_c.Valid(body.I);
Matrix test = Matrix.Identity;
test = Matrix.Invert(I.Get());
}
public void GenerateImpulse(float e /*Bouncyness (Coefficient of restitution)*/)
{
if (m_body1.inv_m == 0.0f && m_body2.inv_m == 0.0f)
{
return;
}
Vector3 v1 = m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1));
Vector3 v2 = m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2));
// Compute the relative velocity between the bodies
Vector3 relativeVelocity = v2 - v1;
// If the objects are moving away from each other we dont need to apply an impulse
float relativeMovement = Vector3.Dot(relativeVelocity, m_velocityConstraintDirection);
if (relativeMovement < -0.01f)
{
return;
}
Vector3 r1 = MyMath.Rotate(m_body1.q, m_r1);
Vector3 r2 = MyMath.Rotate(m_body2.q, m_r2);
//Vector3 r1 = m_body1.x + MyMath.Rotate(m_body1.q, m_r1);
//Vector3 r2 = m_body2.x + MyMath.Rotate(m_body2.q, m_r2);
//Vector3 r1 = m_r1;
//Vector3 r2 = m_r2;
Matrix orientationMatrix1 = Matrix.CreateFromQuaternion(m_body1.q);
Matrix inverseOrientationMatrix1 = Matrix.Transpose(orientationMatrix1);
Matrix inverseWorldInertiaMatrix1 = orientationMatrix1 * m_body1.inv_I * inverseOrientationMatrix1;
//Matrix inverseWorldInertiaMatrix1 = inverseOrientationMatrix1 * m_body1.inv_I * orientationMatrix1;
//Matrix inverseWorldInertiaMatrix1 = m_body1.inv_I;
Matrix orientationMatrix2 = Matrix.CreateFromQuaternion(m_body2.q);
Matrix inverseOrientationMatrix2 = Matrix.Transpose(orientationMatrix2);
Matrix inverseWorldInertiaMatrix2 = orientationMatrix2 * m_body2.inv_I * inverseOrientationMatrix2;
//Matrix inverseWorldInertiaMatrix2 = inverseOrientationMatrix2 * m_body2.inv_I * orientationMatrix2;
//Matrix inverseWorldInertiaMatrix2 = m_body2.inv_I;
Vector3 a1 = Vector3.Transform(Vector3.Cross(r1, m_velocityConstraintDirection), inverseWorldInertiaMatrix1);
Vector3 a2 = Vector3.Transform(Vector3.Cross(r2, m_velocityConstraintDirection), inverseWorldInertiaMatrix2);
//Vector3 a1 = Vector3.Transform(Vector3.Cross(r1, m_velocityConstraintDirection), m_body1.inv_I);
//Vector3 a2 = Vector3.Transform(Vector3.Cross(r2, m_velocityConstraintDirection), m_body2.inv_I);
float kn = m_body1.inv_m + m_body2.inv_m +
Vector3.Dot(m_velocityConstraintDirection, Vector3.Cross(a1, r1)) +
Vector3.Dot(m_velocityConstraintDirection, Vector3.Cross(a2, r2));
float pn = (1 + e) / kn;
Vector3 J = -m_velocityConstraintDirection * relativeMovement * (1 + e) / kn;
//m_velocityConstraintDirection = Vector3.Normalize( m_velocityConstraintDirection );
//J = Vector3.Dot( J, m_velocityConstraintDirection ) * m_velocityConstraintDirection;
//J = Vector3.Normalize( J ) * len;
//float len2 = J.Length();
m_body1.v -= J * m_body1.inv_m;
m_body2.v += J * m_body2.inv_m;
Vector3 oldOmega1 = m_body1.omega;
Vector3 oldOmega2 = m_body2.omega;
m_body1.omega = oldOmega1 - Vector3.Transform(Vector3.Cross(r1, J), inverseWorldInertiaMatrix1);
m_body2.omega = oldOmega2 + Vector3.Transform(Vector3.Cross(r2, J), inverseWorldInertiaMatrix2);
//m_body1.omega = oldOmega1 - Vector3.Transform(Vector3.Cross(r1, J), m_body1.inv_I);
//m_body2.omega = oldOmega2 + Vector3.Transform(Vector3.Cross(r2, J), m_body2.inv_I);
Debug_c.Valid(m_body1.v);
Debug_c.Valid(m_body2.v);
Debug_c.Valid(m_body1.omega);
Debug_c.Valid(m_body2.omega);
// Tangent Friction
//if (false)
{
// Work out our tangent vector, with is perpendicular
// to our collision normal
Vector3 tangent = relativeVelocity - Vector3.Dot(relativeVelocity, m_velocityConstraintDirection) * m_velocityConstraintDirection;
if (tangent.LengthSquared() < 0.00001f)
{
return;
}
tangent.Normalize();
Vector3 at1 = Vector3.Transform(Vector3.Cross(r1, tangent), inverseWorldInertiaMatrix1);
Vector3 at2 = Vector3.Transform(Vector3.Cross(r2, tangent), inverseWorldInertiaMatrix2);
float ktn = m_body1.inv_m + m_body2.inv_m +
Vector3.Dot(tangent, Vector3.Cross(at1, r1)) +
Vector3.Dot(tangent, Vector3.Cross(at2, r2));
//float mu = 0.5f;
float pt = (1 + e) / ktn;
pt = MathHelper.Clamp(pt, -0.3f * pn, 0.3f * pn);
Vector3 Jt = -tangent * pt;
m_body1.v -= Jt * m_body1.inv_m;
m_body2.v += Jt * m_body2.inv_m;
Vector3 oldOmegat1 = m_body1.omega;
Vector3 oldOmegat2 = m_body2.omega;
m_body1.omega = oldOmegat1 - Vector3.Transform(Vector3.Cross(r1, Jt), inverseWorldInertiaMatrix1);
m_body2.omega = oldOmegat2 + Vector3.Transform(Vector3.Cross(r2, Jt), inverseWorldInertiaMatrix2);
Debug_c.Valid(m_body1.v);
Debug_c.Valid(m_body2.v);
Debug_c.Valid(m_body1.omega);
Debug_c.Valid(m_body2.omega);
}
}
public override void Iterate()
{
if (m_body1.inv_m == 0.0f && m_body2.inv_m == 0.0f)
{
return;
}
// Compute the relative velocity between the bodies
Vector3 relativeVelocity = (m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2))) -
(m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1)));
// Project the relative velocity onto the constraint direction
float velocityError = Vector3.Dot(relativeVelocity, m_velocityConstraintDirection);
// Compute the velocity delta needed to satisfy the constraint
float deltaVelocity = -velocityError - m_positionError;
// Compute the momentum to be exchanged to correct velocities
float momentumPacket = deltaVelocity * m_effectiveMass;
// Clamp the momentum packet to reflect the fact that the contact can only push the objects apart
momentumPacket = Math.Min(momentumPacket, -m_cachedMomentum);
Vector3 momentumPacketWithDir = momentumPacket * m_velocityConstraintDirection;
// Exchange the correctional momentum between the bodies
m_body1.v -= momentumPacketWithDir * m_body1.inv_m;
m_body2.v += momentumPacketWithDir * m_body2.inv_m;
m_body1.omega -= momentumPacket * m_invMoment1;
m_body2.omega += momentumPacket * m_invMoment2;
Debug_c.Valid(m_body1.omega);
Debug_c.Valid(m_body2.omega);
// Test code
//Vector3 newRelativeVelocity = (m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2))) -
// (m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1)));
//float newVelocityError = Vector3.Dot(newRelativeVelocity, m_velocityConstraintDirection);
// Accumulate the momentum for next frame
m_cachedMomentum += momentumPacket;
///
// FRICTION
//if (gFriction)
{
relativeVelocity = (m_body2.v + Vector3.Cross(m_body2.omega, MyMath.Rotate(m_body2.q, m_r2))) -
(m_body1.v + Vector3.Cross(m_body1.omega, MyMath.Rotate(m_body1.q, m_r1)));
Vector3 tangentVelocityDirection = relativeVelocity - Vector3.Dot(relativeVelocity, m_velocityConstraintDirection) * m_velocityConstraintDirection;
if (tangentVelocityDirection.LengthSquared() < 0.0001f)
{
return;
}
Debug_c.Valid(tangentVelocityDirection);
tangentVelocityDirection.Normalize();
float tangentVelocityError = Vector3.Dot(relativeVelocity, tangentVelocityDirection);
Vector3 r1 = MyMath.Rotate(m_body1.q, m_r1);
Vector3 r2 = MyMath.Rotate(m_body2.q, m_r2);
Vector3 invMoment1 = Vector3.Transform(Vector3.Cross(r1, tangentVelocityDirection), m_body1.inv_I);
Vector3 invMoment2 = Vector3.Transform(Vector3.Cross(r2, tangentVelocityDirection), m_body2.inv_I);
Debug_c.Valid(invMoment1);
Debug_c.Valid(invMoment2);
// Compute effective mass of the constraint system -- this is a measure of how easy it
// is to accelerate the contact points apart along the constraint direction -- it's analogous
// to effective resistance in an electric circuit [i.e., 1 / (1/R1 + 1/R2)]
float effectiveMass =
1.0f /
(
m_body1.inv_m +
m_body2.inv_m +
Vector3.Dot(tangentVelocityDirection,
(
Vector3.Cross(invMoment1, r1) +
Vector3.Cross(invMoment2, r2)
))
);
float tangentDeltaVelocity = -tangentVelocityError;
float tangentMomentumPacket = tangentDeltaVelocity * effectiveMass;
tangentMomentumPacket = Math.Max(tangentMomentumPacket, m_cachedMomentum * 0.5f);
Vector3 tangentMomentumPacketWithDir = tangentMomentumPacket * tangentVelocityDirection;
// Exchange the correctional momentum between the bodies
m_body1.v -= tangentMomentumPacketWithDir * m_body1.inv_m;
m_body2.v += tangentMomentumPacketWithDir * m_body2.inv_m;
m_body1.omega -= tangentMomentumPacket * invMoment1;
m_body2.omega += tangentMomentumPacket * invMoment2;
Debug_c.Valid(m_body1.v);
Debug_c.Valid(m_body2.v);
Debug_c.Valid(m_body1.omega);
Debug_c.Valid(m_body2.omega);
m_cachedTangentMomentum = tangentMomentumPacketWithDir;
}
}
public override void PrepareForIteration()
{
if (m_body1.inv_m == 0.0f && m_body2.inv_m == 0.0f)
{
return;
}
Vector3 r1 = MyMath.Rotate(m_body1.q, m_r1);
Vector3 r2 = MyMath.Rotate(m_body2.q, m_r2);
Vector3 x1 = m_body1.x + r1;
Vector3 x2 = m_body2.x + r2;
// Compute the positional constraint error (scaled by the Baumgarte coefficient 'm_beta')
// m_beta = 0.98f / gTimeStep;
//m_beta = 0.5f / Contact_c.gTimeStep;
m_beta = 0.5f / Contact_c.gTimeStep;
m_positionError = m_beta * Vector3.Dot((x2 - x1), m_velocityConstraintDirection);
// Add a boundary layer to the position error -- this will ensure the objects remain in contact
//m_positionError -= 1.0f;
m_positionError -= 1.0f;
// The velocity constraint direction is aligned with the contact normal
// (This represents how much angular velocity we get for every unit of momentum transferred along the constraint direction)
m_invMoment1 = Vector3.Transform(Vector3.Cross(r1, m_velocityConstraintDirection), m_body1.inv_I);
m_invMoment2 = Vector3.Transform(Vector3.Cross(r2, m_velocityConstraintDirection), m_body2.inv_I);
Debug_c.Valid(m_invMoment1);
Debug_c.Valid(m_invMoment2);
// Compute effective mass of the constraint system -- this is a measure of how easy it
// is to accelerate the contact points apart along the constraint direction -- it's analogous
// to effective resistance in an electric circuit [i.e., 1 / (1/R1 + 1/R2)]
m_effectiveMass =
1.0f /
(
m_body1.inv_m +
m_body2.inv_m +
Vector3.Dot(m_velocityConstraintDirection,
(
Vector3.Cross(m_invMoment1, r1) +
Vector3.Cross(m_invMoment2, r2)
))
);
// Convert last frame's momentum to momentum for the new time step
float timeRatio = Contact_c.gTimeRatio;
m_cachedMomentum *= Contact_c.gTimeRatio;
m_cachedTangentMomentum *= Contact_c.gTimeRatio;
// Apply last frame's momentum
m_body1.v -= m_cachedMomentum * m_velocityConstraintDirection * m_body1.inv_m;
m_body2.v += m_cachedMomentum * m_velocityConstraintDirection * m_body2.inv_m;
m_body1.omega -= m_cachedMomentum * m_invMoment1;
m_body2.omega += m_cachedMomentum * m_invMoment2;
Debug_c.Valid(m_body1.v);
Debug_c.Valid(m_body2.v);
Debug_c.Valid(m_body1.omega);
Debug_c.Valid(m_body2.omega);
m_cachedTangentMomentum = Vector3.Zero;
}
bool HasIntersection(Shape p1,
Quaternion q1,
Vector3 t1,
Shape p2,
Quaternion q2,
Vector3 t2,
out Vector3 returnNormal,
out Vector3 point1,
out Vector3 point2)
{
returnNormal = Vector3.Zero;
point1 = Vector3.Zero;
point2 = Vector3.Zero;
const float kCollideEpsilon = 1e-3f;
// v0 = center of Minkowski sum
Vector3 v01 = MyMath.Rotate(q1, p1.GetCenter()) + t1;
Vector3 v02 = MyMath.Rotate(q2, p2.GetCenter()) + t2;
Vector3 v0 = v02 - v01;
Debug_c.Valid(v02);
Debug_c.Valid(v01);
Debug_c.Valid(v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.LengthSquared() < 0.0001f)
{
v0 = new Vector3(0.00001f, 0, 0);
}
// v1 = support in direction of origin
Vector3 n = -v0;
Vector3 v11 = Collision.TransformSupportVert(p1, q1, t1, -n);
Vector3 v12 = Collision.TransformSupportVert(p2, q2, t2, n);
Debug_c.Valid(v11);
Debug_c.Valid(v12);
Vector3 v1 = v12 - v11;
if (Vector3.Dot(v1, n) <= 0.0f)
{
return(false);
}
Debug_c.Valid(v0);
Debug_c.Valid(v1);
// v2 - support perpendicular to v1,v0
n = Vector3.Cross(v1, v0);
Debug_c.Valid(n);
if (n.LengthSquared() < 0.0001f)
{
n = v1 - v0;
n.Normalize();
returnNormal = n;
point1 = v11;
point2 = v12;
return(true);
}
Vector3 v21 = Collision.TransformSupportVert(p1, q1, t1, -n);
Vector3 v22 = Collision.TransformSupportVert(p2, q2, t2, n);
Vector3 v2 = v22 - v21;
if (Vector3.Dot(v2, n) <= 0.0f)
{
return(false);
}
Debug_c.Valid(v21);
Debug_c.Valid(v22);
// Determine whether origin is on + or - side of plane (v1,v0,v2)
n = Vector3.Cross(v1 - v0, v2 - v0);
Debug_c.Valid(n);
float dist = Vector3.Dot(n, v0);
Debug_c.Assert(n.LengthSquared() > 0.0001f);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > 0)
{
Swap(ref v1, ref v2);
Swap(ref v11, ref v21);
Swap(ref v12, ref v22);
n = -n;
}
bool hit = false;
///
// Phase One: Identify a portal
//
while (true)
{
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
Vector3 v31 = Collision.TransformSupportVert(p1, q1, t1, -n);
Vector3 v32 = Collision.TransformSupportVert(p2, q2, t2, n);
Vector3 v3 = v32 - v31;
if (Vector3.Dot(v3, n) <= 0)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
if (Vector3.Dot(Vector3.Cross(v1, v3), v0) < 0)
{
v2 = v3;
v21 = v31;
v22 = v32;
n = Vector3.Cross(v1 - v0, v3 - v0);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
if (Vector3.Dot(Vector3.Cross(v3, v2), v0) < 0)
{
v1 = v3;
v11 = v31;
v12 = v32;
n = Vector3.Cross(v3 - v0, v2 - v0);
continue;
}
///
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
// Compute normal of the wedge face
n = Vector3.Cross(v2 - v1, v3 - v1);
// Can this happen??? Can it be handled more cleanly?
if (n.LengthSquared() < 0.00001f)
{
Debug_c.Assert(false);
return(false);
}
n.Normalize();
// Compute distance from origin to wedge face
float d = Vector3.Dot(n, v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
returnNormal = n;
// Compute the barycentric coordinates of the origin
float b0 = Vector3.Dot(Vector3.Cross(v1, v2), v3);
float b1 = Vector3.Dot(Vector3.Cross(v3, v2), v0);
float b2 = Vector3.Dot(Vector3.Cross(v0, v1), v3);
float b3 = Vector3.Dot(Vector3.Cross(v2, v1), v0);
float sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
b1 = Vector3.Dot(Vector3.Cross(v2, v3), n);
b2 = Vector3.Dot(Vector3.Cross(v3, v1), n);
b3 = Vector3.Dot(Vector3.Cross(v1, v2), n);
sum = b1 + b2 + b3;
}
float inv = 1.0f / sum;
point1 = (b0 * v01 + b1 * v11 + b2 * v21 + b3 * v31) * inv;
point2 = (b0 * v02 + b1 * v12 + b2 * v22 + b3 * v32) * inv;
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
Vector3 v41 = Collision.TransformSupportVert(p1, q1, t1, -n);
Vector3 v42 = Collision.TransformSupportVert(p2, q2, t2, n);
Vector3 v4 = v42 - v41;
float delta = Vector3.Dot((v4 - v3), n);
float separation = -Vector3.Dot(v4, n);
// If the boundary is thin enough or the origin is outside the support plane for the
// newly discovered vertex, then we can terminate
if (delta <= kCollideEpsilon || separation >= 0)
{
returnNormal = n;
return(hit);
}
// Compute the tetrahedron dividing face (v4,v0,v1)
float d1 = Vector3.Dot(Vector3.Cross(v4, v1), v0);
// Compute the tetrahedron dividing face (v4,v0,v2)
float d2 = Vector3.Dot(Vector3.Cross(v4, v2), v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
float d3 = Vector3.Dot(Vector3.Cross(v4, v3), v0);
if (d1 < 0)
{
if (d2 < 0)
{
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
if (d3 < 0)
{
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
