public RigidBody_c(Body_c b, Shape cg, HullMaker hull, float radius, Microsoft.Xna.Framework.Graphics.Color c)
{
maxRadius = radius;
body = b;
collideModel = cg;
renderModel = hull;
id = idcounter;
colour = c;
idcounter++;
}
public ContactConstraint_c(ref Body_c b1, ref Body_c b2, ref Vector3 r1, ref Vector3 r2, ref Vector3 normal)
{
m_body1 = b1;
m_body2 = b2;
m_r1 = r1;
m_r2 = r2;
m_velocityConstraintDirection = normal;
m_beta = 1.0f;
m_cachedMomentum = 0;
m_cachedTangentMomentum = Vector3.Zero;
}
public Body_c(Body_c copy)
{
com = copy.com;
x = copy.x;
v = copy.v;
m = copy.m;
inv_m = copy.inv_m;
inv_m_back = copy.inv_m_back;
q = copy.q;
omega = copy.omega;
I = copy.I;
inv_I = copy.inv_I;
}
public void StoreState()
{
store = new Body_c();
store.com = com;
store.x = x;
store.v = v;
store.m = m;
store.inv_m = inv_m;
store.inv_m_back = inv_m_back;
store.q = q;
store.omega = omega;
store.I = I;
store.inv_I = inv_I;
}
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 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());
}
RigidBody_c CreateRigidBody(Shape collideModel, Quaternion q, Vector3 x, float inv_m)
{
//RenderPolytope renderModel = rb.renderModel;
HullMaker renderModel = new HullMaker(collideModel, -1);
//if (!renderModel)
//{
// renderModel = CreateRenderModel(collideModel);
//}
Body_c body = new Body_c();
body.q = q;
body.x = x;
body.inv_I = Matrix.Identity;
body.inv_I *= inv_m / 25.0f;
//body.inv_I.M33 = 1;
body.inv_I.M44 = 1;
body.inv_m = inv_m;
if (inv_m == 0.0f)
{
body.m = 0.0f;
}
if (inv_m > 0.0f)
{
HelperRigidBody_c.ComputeMassProperties(body, renderModel, 1.0f);
}
float radiusNegX = Math.Abs(collideModel.GetSupportPoint(new Vector3(-1, 0, 0)).X);
float radiusPosX = Math.Abs(collideModel.GetSupportPoint(new Vector3(1, 0, 0)).X);
float radiusNegY = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, -1, 0)).Y);
float radiusPosY = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, 1, 0)).Y);
float radiusNegZ = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, 0, -1)).Z);
float radiusPosZ = Math.Abs(collideModel.GetSupportPoint(new Vector3(0, 0, 1)).Z);
Vector3 maxRadiusVector = new Vector3
(
Math.Max(radiusNegX, radiusPosX),
Math.Max(radiusNegY, radiusPosY),
Math.Max(radiusNegZ, radiusPosZ)
);
float maxRadius = maxRadiusVector.Length();
Microsoft.Xna.Framework.Graphics.Color[] randColour = { Microsoft.Xna.Framework.Graphics.Color.Red,
Microsoft.Xna.Framework.Graphics.Color.Green,
Microsoft.Xna.Framework.Graphics.Color.PaleGoldenrod,
Microsoft.Xna.Framework.Graphics.Color.OliveDrab,
Microsoft.Xna.Framework.Graphics.Color.OrangeRed,
Microsoft.Xna.Framework.Graphics.Color.Wheat,
Microsoft.Xna.Framework.Graphics.Color.YellowGreen };
randColourIndx++;
if (randColourIndx > (randColour.Count() - 1))
{
randColourIndx = 0;
}
body.inv_m_back = body.inv_m;
return(new RigidBody_c(body, collideModel, renderModel, maxRadius, randColour[randColourIndx]));
}
