public Contact_c(RigidBody_c b1, RigidBody_c b2, Vector3 p1, Vector3 p2, Vector3 n)
{
this.b1 = b1;
this.b2 = b2;
this.normal = n;
this.local1 = MyMath.Rotate(MyMath.Conjugate(b1.body.q), p1 - b1.body.x);
this.local2 = MyMath.Rotate(MyMath.Conjugate(b2.body.q), p2 - b2.body.x);
this.distStart = 0.0f;
this.distStart = this.Distance();
this.constraint = new ContactConstraint_c(ref b1.body, ref b2.body, ref local1, ref local2, ref n);
this.point = 0.5f * (p1 + p2);
this.timeStamp = Contact_c.gTimeStamp;
}
Arbiter_c FindArbiter(ref RigidBody_c rb1, ref RigidBody_c rb2)
{
ArbiterKey_c key = new ArbiterKey_c(ref rb1, ref rb2);
for (int i = 0; i < arbiterArray.Count; i++)
{
if (arbiterArray[i].key.b1.id == key.b1.id &&
arbiterArray[i].key.b2.id == key.b2.id)
{
return(arbiterArray[i].arbiter);
}
}
arbiterArray.Add(new ArbiterItem_c(key, new Arbiter_c(ref rb1, ref rb2)));
return(arbiterArray[arbiterArray.Count - 1].arbiter);
}
void CheckCollisions()
{
// Check for any collisions
for (int i = 0; i < m_rigidBodies.Count; i++)
{
RigidBody_c b1 = m_rigidBodies[i];
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;
}
//if (b1.body.inv_m==0.0f && b1.body.inv_m==0.0f) continue;
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);
if (haveHit)
{
Arbiter_c arb = ArbiterContainer_c.FindArbiter(ref b1, ref b2);
// Find the support points
Vector3 s1 = Collision.TransformSupportVert(b1.collideModel, b1.body.q, b1.body.x - MyMath.Rotate(b1.body.q, b1.body.com), -n);
Vector3 s2 = Collision.TransformSupportVert(b2.collideModel, b2.body.q, b2.body.x - MyMath.Rotate(b2.body.q, b2.body.com), n);
Vector3 pp1 = (s1 - p1) * Vector3.Dot(n, n) + p1;
Vector3 pp2 = (s2 - p2) * Vector3.Dot(n, n) + p2;
arb.AddContact(pp1, pp2, n);
}
}
}
}
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
}
public Arbiter_c(ref RigidBody_c in1, ref RigidBody_c in2)
{
b1 = in1;
b2 = in2;
}