public override void ReportProximities(ChProximityContainer mproximitycontainer)
{
mproximitycontainer.BeginAddProximities();
/*
* int numManifolds = bt_collision_world->getDispatcher()->getNumManifolds();
* for (int i = 0; i < numManifolds; i++) {
* btPersistentManifold* contactManifold = bt_collision_world->getDispatcher()->getManifoldByIndexInternal(i);
* btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
* btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
* contactManifold->refreshContactPoints(obA->getWorldTransform(), obB->getWorldTransform());
*
* ChCollisionModel* modelA = (ChCollisionModel*)obA->getUserPointer();
* ChCollisionModel* modelB = (ChCollisionModel*)obB->getUserPointer();
*
* // Add to proximity container
* mproximitycontainer->AddProximity(modelA, modelB);
* }
*/
int numPairs = bt_collision_world.GetBroadphase().GetOverlappingPairCache().GetNumOverlappingPairs();
for (int i = 0; i < numPairs; i++)
{
BroadphasePair mp = bt_collision_world.GetBroadphase().GetOverlappingPairCache().GetOverlappingPairArray()[i];
CollisionObject obA = (CollisionObject)(mp.m_pProxy0.GetClientObject());
CollisionObject obB = (CollisionObject)(mp.m_pProxy1.GetClientObject());
ChCollisionModel modelA = (ChCollisionModel)obA.GetUserPointer();
ChCollisionModel modelB = (ChCollisionModel)obB.GetUserPointer();
// Add to proximity container
mproximitycontainer.AddProximity(modelA, modelB);
}
mproximitycontainer.EndAddProximities();
}
public override void ReportContacts(ChContactContainer mcontactcontainer)
{
// This should remove all old contacts (or at least rewind the index)
mcontactcontainer.BeginAddContact();
// NOTE: Bullet does not provide information on radius of curvature at a contact point.
// As such, for all Bullet-identified contacts, the default value will be used (SMC only).
ChCollisionInfo icontact = new ChCollisionInfo();
int numManifolds = bt_collision_world.GetDispatcher().GetNumManifolds();
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = bt_collision_world.GetDispatcher().GetManifoldByIndexInternal(i);
CollisionObject obA = (CollisionObject)(contactManifold.GetBody0());
CollisionObject obB = (CollisionObject)(contactManifold.GetBody1());
if (obA != null && obA != null) // Alan
{
contactManifold.RefreshContactPoints(ref obA.GetWorldTransform(), ref obB.GetWorldTransform());
icontact.modelA = (ChCollisionModel)obA.GetUserPointer();
icontact.modelB = (ChCollisionModel)obB.GetUserPointer();
double envelopeA = icontact.modelA.GetEnvelope();
double envelopeB = icontact.modelB.GetEnvelope();
double marginA = icontact.modelA.GetSafeMargin();
double marginB = icontact.modelB.GetSafeMargin();
// Execute custom broadphase callback, if any
bool do_narrow_contactgeneration = true;
if (this.broad_callback != null)
{
do_narrow_contactgeneration = this.broad_callback.OnBroadphase(icontact.modelA, icontact.modelB);
}
if (do_narrow_contactgeneration)
{
int numContacts = contactManifold.GetNumContacts();
//GetLog() << "numContacts=" << numContacts << "\n";
for (int j = 0; j < numContacts; j++)
{
// Debug.Log("contacts " + numContacts);
ManifoldPoint pt = contactManifold.GetContactPoint(j);
// Discard "too far" constraints (the Bullet engine also has its threshold)
if (pt.GetDistance() < marginA + marginB)
{
IndexedVector3 ptA = pt.GetPositionWorldOnA();
IndexedVector3 ptB = pt.GetPositionWorldOnB();
icontact.vpA.Set(ptA.X, ptA.Y, ptA.Z);
icontact.vpB.Set(ptB.X, ptB.Y, ptB.Z);
icontact.vN.Set(-pt.GetNormalWorldOnB().X, -pt.GetNormalWorldOnB().Y,
-pt.GetNormalWorldOnB().Z);
icontact.vN.Normalize();
double ptdist = pt.GetDistance();
icontact.vpA = icontact.vpA - icontact.vN * envelopeA;
icontact.vpB = icontact.vpB + icontact.vN * envelopeB;
icontact.distance = ptdist + envelopeA + envelopeB;
icontact.reaction_cache = pt.reactions_cache;// reactions_cache;
// Execute some user custom callback, if any
bool add_contact = true;
if (this.narrow_callback != null)
{
add_contact = this.narrow_callback.OnNarrowphase(icontact);
}
// Add to contact container
if (add_contact)
{
mcontactcontainer.AddContact(icontact);
}
}
}
}
}
// you can un-comment out this line, and then all points are removed
// contactManifold->clearManifold();
}
mcontactcontainer.EndAddContact();
}
