public bool RecoverFromPenetration(CollisionWorld collisionWorld)
{
bool penetration = false;
collisionWorld.GetDispatcher().DispatchAllCollisionPairs(m_ghostObject.GetOverlappingPairCache(), collisionWorld.GetDispatchInfo(), collisionWorld.GetDispatcher());
m_currentPosition = m_ghostObject.GetWorldTransform()._origin;
float maxPen = 0f;
for (int i = 0; i < m_ghostObject.GetOverlappingPairCache().GetNumOverlappingPairs(); i++)
{
m_manifoldArray.Clear();
BroadphasePair collisionPair = m_ghostObject.GetOverlappingPairCache().GetOverlappingPairArray()[i];
if (collisionPair.m_algorithm != null)
{
collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray);
}
for (int j = 0; j < m_manifoldArray.Count; j++)
{
PersistentManifold manifold = m_manifoldArray[j];
float directionSign = manifold.GetBody0() == m_ghostObject ? -1f : 1f;
for (int p = 0; p < manifold.GetNumContacts(); p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
float dist = pt.GetDistance();
if (dist < 0.0)
{
if (dist < maxPen)
{
maxPen = dist;
m_touchingNormal = pt.m_normalWorldOnB * directionSign; //??
}
m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * 0.2f;
penetration = true;
}
else
{
//printf("touching %f\n", dist);
}
}
//manifold->clearManifold();
}
}
IndexedMatrix newTrans = m_ghostObject.GetWorldTransform();
newTrans._origin = m_currentPosition;
m_ghostObject.SetWorldTransform(ref newTrans);
// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
return(penetration);
}
public static int GetIslandId(PersistentManifold lhs)
{
int islandId;
CollisionObject rcolObj0 = (CollisionObject)(lhs.GetBody0());
CollisionObject rcolObj1 = (CollisionObject)(lhs.GetBody1());
islandId = rcolObj0.GetIslandTag()>=0?rcolObj0.GetIslandTag():rcolObj1.GetIslandTag();
return islandId;
}
protected void ConvertContact(PersistentManifold manifold, ContactSolverInfo infoGlobal)
{
CollisionObject colObj0 = null, colObj1 = null;
colObj0 = manifold.GetBody0() as CollisionObject;
colObj1 = manifold.GetBody1() as CollisionObject;
RigidBody solverBodyA = RigidBody.Upcast(colObj0);
RigidBody solverBodyB = RigidBody.Upcast(colObj1);
///avoid collision response between two static objects
if ((solverBodyA == null || solverBodyA.GetInvMass() <= 0f) && (solverBodyB == null || solverBodyB.GetInvMass() <= 0f))
{
return;
}
for (int j = 0; j < manifold.GetNumContacts(); j++)
{
ManifoldPoint cp = manifold.GetContactPoint(j);
if (cp.GetDistance() <= manifold.GetContactProcessingThreshold())
{
// Vector3 pos1 = cp.getPositionWorldOnA();
// Vector3 pos2 = cp.getPositionWorldOnB();
Vector3 rel_pos1 = Vector3.Zero;
Vector3 rel_pos2 = Vector3.Zero;
//;
float relaxation = 1f;
float rel_vel = 0f;
Vector3 vel = Vector3.Zero;
int frictionIndex = m_tmpSolverContactConstraintPool.Count;
SolverConstraint solverConstraint = new SolverConstraint();
//m_tmpSolverContactConstraintPool.Add(solverConstraint);
RigidBody rb0 = RigidBody.Upcast(colObj0);
RigidBody rb1 = RigidBody.Upcast(colObj1);
if (BulletGlobals.g_streamWriter != null && rb0 != null && debugSolver)
{
MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
}
solverConstraint.m_solverBodyA = rb0 != null ? rb0 : GetFixedBody();
solverConstraint.m_solverBodyB = rb1 != null ? rb1 : GetFixedBody();
solverConstraint.m_originalContactPoint = cp;
SetupContactConstraint(ref solverConstraint, colObj0, colObj1, cp, infoGlobal, ref vel, ref rel_vel, ref relaxation, ref rel_pos1, ref rel_pos2);
if (BulletGlobals.g_streamWriter != null && debugSolver)
{
TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, 99);
}
/////setup the friction constraints
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.Count;
if (!(TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING)) || !cp.GetLateralFrictionInitialized())
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
float lat_rel_vel = cp.m_lateralFrictionDir1.LengthSquared();
if (!TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > MathUtil.SIMD_EPSILON)
{
cp.m_lateralFrictionDir1 /= (float)System.Math.Sqrt(lat_rel_vel);
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = Vector3.Cross(cp.m_lateralFrictionDir1, cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.Normalize();//??
ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir2);
ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir2);
AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
}
ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir1);
ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir1);
AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
cp.m_lateralFrictionInitialized = true;
}
else
{
//re-calculate friction direction every frame, todo: check if this is really needed
TransformUtil.PlaneSpace1(ref cp.m_normalWorldOnB, ref cp.m_lateralFrictionDir1, ref cp.m_lateralFrictionDir2);
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir2);
ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir2);
AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
}
ApplyAnisotropicFriction(colObj0, ref cp.m_lateralFrictionDir1);
ApplyAnisotropicFriction(colObj1, ref cp.m_lateralFrictionDir1);
AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
cp.m_lateralFrictionInitialized = true;
}
}
else
{
AddFrictionConstraint(ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1);
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
AddFrictionConstraint(ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
}
}
SetFrictionConstraintImpulse(ref solverConstraint, rb0, rb1, cp, infoGlobal);
m_tmpSolverContactConstraintPool.Add(solverConstraint);
}
}
}
// Portable static method: prerequisite call: m_dynamicsWorld.getBroadphase().getOverlappingPairCache().setInternalGhostPairCallback(new btGhostPairCallback());
public static void GetCollidingObjectsInsidePairCachingGhostObject(DiscreteDynamicsWorld m_dynamicsWorld, PairCachingGhostObject m_pairCachingGhostObject, ObjectArray <CollisionObject> collisionArrayOut)
{
bool addOnlyObjectsWithNegativeDistance = true; // With "false" things don't change much, and the code is a bit faster and cleaner...
collisionArrayOut.Resize(0);
if (m_pairCachingGhostObject == null || m_dynamicsWorld == null)
{
return;
}
//#define USE_PLAIN_COLLISION_WORLD // We dispatch all collision pairs of the ghost object every step (slow)
#if USE_PLAIN_COLLISION_WORLD
//======================================================================================================
// I thought this line was no longer needed, but it seems to be necessary (and I believe it's an expensive call):
m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(m_pairCachingGhostObject.getOverlappingPairCache(), m_dynamicsWorld.getDispatchInfo(), m_dynamicsWorld.getDispatcher());
// Maybe the call can be automatically triggered by some other Bullet call (I'm almost sure I could comment it out in another demo I made long ago...)
// So by now the general rule is: in real projects, simply comment it out and see if it works!
//======================================================================================================
// UPDATE: in dynamic worlds, the line above can be commented out and the broadphase pair can be retrieved through the call to findPair(...) below.
// In collision worlds probably the above line is needed only if collision detection for all the bodies hasn't been made... This is something
// I'm still not sure of... the general rule is to try to comment out the line above and try to use findPair(...) and see if it works whenever possible....
//======================================================================================================
#endif //USE_PLAIN_COLLISION_WORLD
ObjectArray <BroadphasePair> collisionPairs = m_pairCachingGhostObject.GetOverlappingPairCache().GetOverlappingPairArray();
int numObjects = collisionPairs.Count;
PersistentManifoldArray m_manifoldArray = new PersistentManifoldArray();
bool added;
for (int i = 0; i < numObjects; i++)
{
m_manifoldArray.Resize(0);
#if USE_PLAIN_COLLISION_WORLD
const btBroadphasePair& collisionPair = collisionPairs[i];
if (collisionPair.m_algorithm)
{
collisionPair.m_algorithm.getAllContactManifolds(m_manifoldArray);
}
else // THIS SHOULD NEVER HAPPEN, AND IF IT DOES, PLEASE RE-ENABLE the "call" a few lines above...
{
printf("No collisionPair.m_algorithm - probably m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(...) must be missing.\n");
}
#else // USE_PLAIN_COLLISION_WORLD
BroadphasePair cPair = collisionPairs[i];
//unless we manually perform collision detection on this pair, the contacts are in the dynamics world paircache:
BroadphasePair collisionPair = m_dynamicsWorld.GetPairCache().FindPair(cPair.m_pProxy0, cPair.m_pProxy1);
if (collisionPair == null)
{
continue;
}
if (collisionPair.m_algorithm != null)
{
collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray);
}
else
{ // THIS SHOULD NEVER HAPPEN, AND IF IT DOES, PLEASE RE-ENABLE the "call" a few lines above...
//printf("No collisionPair.m_algorithm - probably m_dynamicsWorld.getDispatcher().dispatchAllCollisionPairs(...) must be missing.\n");
}
#endif //USE_PLAIN_COLLISION_WORLD
added = false;
for (int j = 0; j < m_manifoldArray.Count; j++)
{
PersistentManifold manifold = m_manifoldArray[j];
// Here we are in the narrowphase, but can happen that manifold.getNumContacts()==0:
if (addOnlyObjectsWithNegativeDistance)
{
for (int p = 0, numContacts = manifold.GetNumContacts(); p < numContacts; p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
if (pt.GetDistance() < 0.0)
{
// How can I be sure that the colObjs are all distinct ? I use the "added" flag.
collisionArrayOut.Add((CollisionObject)(manifold.GetBody0() == m_pairCachingGhostObject ? manifold.GetBody1() : manifold.GetBody0()));
added = true;
break;
}
}
if (added)
{
break;
}
}
else if (manifold.GetNumContacts() > 0)
{
collisionArrayOut.Add((CollisionObject)(manifold.GetBody0() == m_pairCachingGhostObject ? manifold.GetBody1() : manifold.GetBody0()));
break;
}
}
}
}
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();
}
