public override void GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
if (m_manifoldPtr != null && m_ownManifold)
{
manifoldArray.Add(m_manifoldPtr);
}
}
public SimulationIslandManager()
{
m_splitIslands = true;
m_unionFind = new UnionFind();
m_islandmanifold = new PersistentManifoldArray();
m_islandBodies = new ObjectArray <CollisionObject>();
}
public override void GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
if (m_convexTriangleCallback.m_manifoldPtr != null)
{
manifoldArray.Add(m_convexTriangleCallback.m_manifoldPtr);
}
}
public override void GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
///should we use m_ownManifold to avoid adding duplicates?
if (m_manifoldPtr != null && m_ownManifold)
{
manifoldArray.Add(m_manifoldPtr);
}
}
public override void GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
if (m_childCollisionAlgorithms[i] == this)
{
int ibreak = 0;
}
m_childCollisionAlgorithms[i].GetAllContactManifolds(manifoldArray);
}
}
}
public InplaceSolverIslandCallback(
ContactSolverInfo solverInfo,
IConstraintSolver solver,
ObjectArray <TypedConstraint> sortedConstraints,
int numConstraints,
IDebugDraw debugDrawer,
IDispatcher dispatcher)
{
m_solverInfo = solverInfo;
m_solver = solver;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_dispatcher = dispatcher;
m_bodies = new ObjectArray <CollisionObject>();
m_manifolds = new PersistentManifoldArray();
m_constraints = new ObjectArray <TypedConstraint>();
}
public override bool NeedsCollision(BroadphaseProxy proxy0)
{
//don't collide with itself
if (proxy0.m_clientObject == m_me)
{
return(false);
}
///don't do CCD when the collision filters are not matching
if (!base.NeedsCollision(proxy0))
{
return(false);
}
CollisionObject otherObj = proxy0.m_clientObject as CollisionObject;
//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
if (m_dispatcher.NeedsResponse(m_me, otherObj))
{
#if false
///don't do CCD when there are already contact points (touching contact/penetration)
PersistentManifoldArray manifoldArray = new PersistentManifoldArray();
BroadphasePair collisionPair = m_pairCache.FindPair(m_me.GetBroadphaseHandle(), proxy0);
if (collisionPair != null)
{
if (collisionPair.m_algorithm != null)
{
collisionPair.m_algorithm.GetAllContactManifolds(manifoldArray);
int length = manifoldArray.Count;
for (int i = 0; i < length; ++i)
{
if (manifoldArray[i].GetNumContacts() > 0)
{
return(false);
}
}
}
}
#endif
return(true);
}
return(false);
}
///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
public override int StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)
{
///apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = GetDispatchInfo();
dispatchInfo.SetTimeStep(timeStep);
dispatchInfo.SetStepCount(0);
dispatchInfo.SetDebugDraw(GetDebugDrawer());
///perform collision detection
PerformDiscreteCollisionDetection();
///solve contact constraints
int numManifolds = m_dispatcher1.GetNumManifolds();
if (numManifolds != 0)
{
PersistentManifoldArray manifoldPtr = (m_dispatcher1 as CollisionDispatcher).GetInternalManifoldPointer();
ContactSolverInfo infoGlobal = new ContactSolverInfo();
infoGlobal.m_timeStep = timeStep;
m_constraintSolver.PrepareSolve(0, numManifolds);
m_constraintSolver.SolveGroup(null, 0, manifoldPtr, 0, numManifolds, null, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1);
m_constraintSolver.AllSolved(infoGlobal, m_debugDrawer);
}
///integrate transforms
IntegrateTransforms(timeStep);
UpdateAabbs();
SynchronizeMotionStates();
ClearForces();
return(1);
}
public abstract void GetAllContactManifolds(PersistentManifoldArray manifoldArray);
public void BuildAndProcessIslands(IDispatcher dispatcher, CollisionWorld collisionWorld, IIslandCallback callback)
{
ObjectArray <CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();
BuildIslands(dispatcher, collisionWorld);
int endIslandIndex = 1;
int startIslandIndex;
int numElem = GetUnionFind().GetNumElements();
BulletGlobals.StartProfile("processIslands");
if (!m_splitIslands)
{
PersistentManifoldArray manifolds = dispatcher.GetInternalManifoldPointer();
int maxNumManifolds = dispatcher.GetNumManifolds();
callback.ProcessIsland(collisionObjects, collisionObjects.Count, manifolds, 0, maxNumManifolds, -1);
}
else
{
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
int numManifolds = m_islandmanifold.Count;
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
m_islandmanifold.QuickSort(m_sortPredicate);
//now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("buildAndProcessIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
}
bool islandSleeping = true;
for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId); endIslandIndex++)
{
int i = GetUnionFind().GetElement(endIslandIndex).m_sz;
CollisionObject colObj0 = collisionObjects[i];
m_islandBodies.Add(colObj0);
if (colObj0.IsActive())
{
islandSleeping = false;
}
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
PersistentManifold startManifold = null;
if (startManifoldIndex < numManifolds)
{
int curIslandId = GetIslandId(m_islandmanifold[startManifoldIndex]);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine("curIsland[{0}] startManifold[{1}].", curIslandId, startManifoldIndex);
}
if (curIslandId == islandId)
{
startManifold = m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++)
{
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine("endManifoldIndex[{0}] islandId[{1}] getIsland[{2}].", endManifoldIndex, startManifoldIndex, GetIslandId(m_islandmanifold[endManifoldIndex]));
}
}
/// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex - startManifoldIndex;
}
}
if (!islandSleeping)
{
callback.ProcessIsland(m_islandBodies, m_islandBodies.Count, m_islandmanifold, startManifoldIndex, numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
}
else
{
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine("islandSleeping.");
}
}
if (numIslandManifolds != 0)
{
startManifoldIndex = endManifoldIndex;
}
m_islandBodies.Clear();
}
} // else if(!splitIslands)
BulletGlobals.StopProfile();
}
protected virtual float SolveGroupCacheFriendlySetup(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifold, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher)
{
m_setupCount++;
BulletGlobals.StartProfile("solveGroupCacheFriendlySetup");
m_maxOverrideNumSolverIterations = 0;
m_counter++;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlySetup start [{0}].", m_counter);
}
#endif
if ((numConstraints + numManifolds) == 0)
{
// printf("empty\n");
BulletGlobals.StopProfile();
return 0f;
}
IndexedVector3 zero = IndexedVector3.Zero;
if (infoGlobal.m_splitImpulse)
{
for (int i = 0; i < numBodies; i++)
{
//RigidBody body = RigidBody.Upcast(bodies[i]);
RigidBody body = bodies[i] as RigidBody;
if (body != null)
{
body.InternalSetDeltaLinearVelocity(ref zero);
body.InternalSetDeltaAngularVelocity(ref zero);
body.InternalSetPushVelocity(ref zero);
body.InternalSetTurnVelocity(ref zero);
}
}
}
else
{
for (int i = 0; i < numBodies; i++)
{
RigidBody body = bodies[i] as RigidBody;
if (body != null)
{
body.InternalSetDeltaLinearVelocity(ref zero);
body.InternalSetDeltaAngularVelocity(ref zero);
}
}
}
if (true)
{
int j;
int lastConstraint = startConstraint + numConstraints;
for (j = startConstraint; j < lastConstraint; j++)
{
TypedConstraint constraint = constraints[j];
//constraint.BuildJacobian();
constraint.InternalSetAppliedImpulse(0.0f);
}
}
//if (1)
{
{
int totalNumRows = 0;
//calculate the total number of contraint rows
m_tmpConstraintSizesPool.Resize(numConstraints);
for (int i = 0; i < numConstraints; i++)
{
ConstraintInfo1 info1 = m_tmpConstraintSizesPool[i];
if (constraints[startConstraint + i].IsEnabled())
{
constraints[startConstraint + i].GetInfo1(info1);
}
else
{
info1.m_numConstraintRows = 0;
info1.nub = 0;
}
totalNumRows += info1.m_numConstraintRows;
}
m_tmpSolverNonContactConstraintPool.Resize(totalNumRows);
///setup the btSolverConstraints
int currentRow = 0;
for (int i = 0; i < numConstraints; i++)
{
ConstraintInfo1 info1a = m_tmpConstraintSizesPool[i];
if (info1a.m_numConstraintRows != 0)
{
Debug.Assert(currentRow < totalNumRows);
TypedConstraint constraint = constraints[startConstraint + i];
RigidBody rbA = constraint.GetRigidBodyA();
RigidBody rbB = constraint.GetRigidBodyB();
int overrideNumSolverIterations = constraint.GetOverrideNumSolverIterations() > 0 ? constraint.GetOverrideNumSolverIterations() : infoGlobal.m_numIterations;
{
if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
{
m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
}
}
for (int j = 0; j < info1a.m_numConstraintRows; j++)
{
int index = currentRow + j;
SolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[index];
solverConstraint.Reset();
solverConstraint.m_lowerLimit = -MathUtil.SIMD_INFINITY;
solverConstraint.m_upperLimit = MathUtil.SIMD_INFINITY;
solverConstraint.m_appliedImpulse = 0f;
solverConstraint.m_appliedPushImpulse = 0f;
solverConstraint.m_solverBodyA = rbA;
solverConstraint.m_solverBodyB = rbB;
solverConstraint.m_overrideNumSolverIterations = overrideNumSolverIterations;
}
rbA.InternalSetDeltaLinearVelocity(ref zero);
rbA.InternalSetDeltaAngularVelocity(ref zero);
rbB.InternalSetDeltaLinearVelocity(ref zero);
rbB.InternalSetDeltaAngularVelocity(ref zero);
ConstraintInfo2 info2 = m_tmpConstraintInfo2Pool[m_tmpConstraintInfo2Pool.Count];
Debug.Assert(info1a.m_numConstraintRows <= ConstraintInfo2.maxConstraints);
info2.m_numRows = info1a.m_numConstraintRows;
// MAN - copy the data into the info block for passing to the constraints
for (int j = 0; j < info1a.m_numConstraintRows; ++j)
{
info2.m_solverConstraints[j] = m_tmpSolverNonContactConstraintPool[currentRow + j];
}
info2.fps = 1f / infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
info2.m_numIterations = infoGlobal.m_numIterations;
info2.m_damping = infoGlobal.m_damping;
constraint.GetInfo2(info2);
//FIXME - log the output of the solverconstraints for comparison.
for (int j = 0; j < (info1a.m_numConstraintRows); j++)
{
SolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j];
if (solverConstraint.m_upperLimit >= constraint.GetBreakingImpulseThreshold())
{
solverConstraint.m_upperLimit = constraint.GetBreakingImpulseThreshold();
}
if (solverConstraint.m_lowerLimit <= -constraint.GetBreakingImpulseThreshold())
{
solverConstraint.m_lowerLimit = -constraint.GetBreakingImpulseThreshold();
}
solverConstraint.m_originalContactPoint = constraint;
{
IndexedVector3 ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
solverConstraint.m_angularComponentA = constraint.GetRigidBodyA().GetInvInertiaTensorWorld() * ftorqueAxis1 * constraint.GetRigidBodyA().GetAngularFactor();
}
{
IndexedVector3 ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
solverConstraint.m_angularComponentB = constraint.GetRigidBodyB().GetInvInertiaTensorWorld() * ftorqueAxis2 * constraint.GetRigidBodyB().GetAngularFactor();
}
{
IndexedVector3 iMJlA = solverConstraint.m_contactNormal * rbA.GetInvMass();
IndexedVector3 iMJaA = rbA.GetInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal;
IndexedVector3 iMJlB = solverConstraint.m_contactNormal * rbB.GetInvMass();//sign of normal?
IndexedVector3 iMJaB = rbB.GetInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal;
float sum = IndexedVector3.Dot(ref iMJlA, ref solverConstraint.m_contactNormal);
float a = IndexedVector3.Dot(ref iMJaA, ref solverConstraint.m_relpos1CrossNormal);
float b = IndexedVector3.Dot(ref iMJlB, ref solverConstraint.m_contactNormal);
float c = IndexedVector3.Dot(ref iMJaB, ref solverConstraint.m_relpos2CrossNormal);
sum += a;
sum += b;
sum += c;
solverConstraint.m_jacDiagABInv = 1f / sum;
MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv);
}
///fix rhs
///todo: add force/torque accelerators
{
float rel_vel;
float vel1Dotn = IndexedVector3.Dot(solverConstraint.m_contactNormal, rbA.GetLinearVelocity()) + IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, rbA.GetAngularVelocity());
float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, rbB.GetLinearVelocity()) + IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, rbB.GetAngularVelocity());
rel_vel = vel1Dotn + vel2Dotn;
float restitution = 0f;
float positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
float velocityError = restitution - rel_vel * info2.m_damping;
float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
solverConstraint.m_appliedImpulse = 0f;
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
{
TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, j);
}
#endif
//m_tmpSolverNonContactConstraintPool[currentRow + j] = solverConstraint;
}
//if(BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
//{
// TypedConstraint.PrintInfo2(BulletGlobals.g_streamWriter,constraint,info2);
//}
}
currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows;
}
}
{
int lastManifold = startManifold + numManifolds;
for (int i = startManifold; i < lastManifold; i++)
{
ConvertContact(manifold[i], infoGlobal);
}
}
}
ContactSolverInfo info = infoGlobal;
int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count;
int numConstraintPool = m_tmpSolverContactConstraintPool.Count;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count;
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
m_orderNonContactConstraintPool.EnsureCapacity(numNonContactPool);///
m_orderTmpConstraintPool.EnsureCapacity(numConstraintPool);
m_orderFrictionConstraintPool.EnsureCapacity(numFrictionPool);
{
for (int i = 0; i < numNonContactPool; i++)
{
m_orderNonContactConstraintPool[i] = i;
}
for (int i = 0; i < numConstraintPool; i++)
{
m_orderTmpConstraintPool[i] = i;
}
for (int i = 0; i < numFrictionPool; i++)
{
m_orderFrictionConstraintPool[i] = i;
}
}
BulletGlobals.StopProfile();
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlySetup stop.");
}
#endif
return 0f;
}
protected virtual float SolveGroupCacheFriendlyFinish(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifold, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
{
m_finishCount++;
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlyFinish start [{0}].",numPoolConstraints);
}
#endif
for (int j = 0; j < numPoolConstraints; j++)
{
SolverConstraint solveManifold = m_tmpSolverContactConstraintPool[j];
((ManifoldPoint)solveManifold.m_originalContactPoint).SetAppliedImpulse(solveManifold.m_appliedImpulse);
if ((infoGlobal.m_solverMode & SolverMode.SOLVER_USE_FRICTION_WARMSTARTING) != 0)
{
((ManifoldPoint)solveManifold.m_originalContactPoint).SetAppliedImpulseLateral1(m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse);
((ManifoldPoint)solveManifold.m_originalContactPoint).SetAppliedImpulseLateral2(m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse);
}
//do a callback here?
}
numPoolConstraints = m_tmpSolverNonContactConstraintPool.Count;
for (int j = 0; j < numPoolConstraints; j++)
{
SolverConstraint solverConstr = m_tmpSolverNonContactConstraintPool[j];
TypedConstraint constr = solverConstr.m_originalContactPoint as TypedConstraint;
constr.InternalSetAppliedImpulse(solverConstr.m_appliedImpulse);
if (Math.Abs(solverConstr.m_appliedImpulse) >= constr.GetBreakingImpulseThreshold())
{
constr.SetEnabled(false);
}
m_tmpSolverNonContactConstraintPool[j] = solverConstr;
}
if (infoGlobal.m_splitImpulse)
{
for (int i = 0; i < numBodies; i++)
{
RigidBody rb = RigidBody.Upcast(bodies[i]);
if (rb != null)
{
rb.InternalWritebackVelocity(infoGlobal.m_timeStep);
}
}
}
else
{
for (int i = 0; i < numBodies; i++)
{
RigidBody rb = RigidBody.Upcast(bodies[i]);
if (rb != null)
{
rb.InternalWritebackVelocity();
}
}
}
m_tmpSolverContactConstraintPool.Resize(0);
m_tmpSolverNonContactConstraintPool.Resize(0);
m_tmpSolverContactFrictionConstraintPool.Resize(0);
m_tmpConstraintInfo2Pool.Resize(0);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlyFinish stop.");
}
#endif
return 0f;
}
protected virtual void SolveGroupCacheFriendlySplitImpulseIterations(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifold, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
{
if (infoGlobal.m_splitImpulse)
{
for (int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
{
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
for (int j = 0; j < numPoolConstraints; j++)
{
SolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
ResolveSplitPenetrationImpulseCacheFriendly(solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, ref solveManifold);
m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]] = solveManifold;
}
}
}
}
}
public virtual float SolveGroup(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifoldPtr, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher)
{
BulletGlobals.StartProfile("solveGroup");
//you need to provide at least some bodies
Debug.Assert(bodies.Count > 0);
SolveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, startManifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer, dispatcher);
SolveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, startManifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer, dispatcher);
SolveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, startManifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer);
BulletGlobals.StopProfile();
return 0.0f;
}
public override void GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
}
// 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 void RecordGhostCollisions(PairCachingGhostObject obj)
{
IOverlappingPairCache cache = obj.GetOverlappingPairCache();
ObjectArray<BroadphasePair> pairs = cache.GetOverlappingPairArray();
DiscreteDynamicsWorld world = (PhysicsScene.World as BulletWorldXNA).world;
PersistentManifoldArray manifoldArray = new PersistentManifoldArray();
BroadphasePair collisionPair;
PersistentManifold contactManifold;
CollisionObject objA;
CollisionObject objB;
ManifoldPoint pt;
int numPairs = pairs.Count;
for (int i = 0; i < numPairs; i++)
{
manifoldArray.Clear();
if (LastCollisionDesc < UpdatedCollisions.Length)
break;
collisionPair = world.GetPairCache().FindPair(pairs[i].m_pProxy0, pairs[i].m_pProxy1);
if (collisionPair == null)
continue;
collisionPair.m_algorithm.GetAllContactManifolds(manifoldArray);
for (int j = 0; j < manifoldArray.Count; j++)
{
contactManifold = manifoldArray[j];
int numContacts = contactManifold.GetNumContacts();
objA = contactManifold.GetBody0() as CollisionObject;
objB = contactManifold.GetBody1() as CollisionObject;
for (int p = 0; p < numContacts; p++)
{
pt = contactManifold.GetContactPoint(p);
if (pt.GetDistance() < 0.0f)
{
RecordCollision(this, objA, objB, pt.GetPositionWorldOnA(), -pt.m_normalWorldOnB,pt.GetDistance());
break;
}
}
}
}
}
// 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 GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
if (m_convexTriangleCallback.m_manifoldPtr != null)
{
manifoldArray.Add(m_convexTriangleCallback.m_manifoldPtr);
}
}
public override void GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
if (m_childCollisionAlgorithms[i] == this)
{
int ibreak = 0;
}
m_childCollisionAlgorithms[i].GetAllContactManifolds(manifoldArray);
}
}
}
public abstract void GetAllContactManifolds(PersistentManifoldArray manifoldArray);
public override void GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
if (m_manifoldPtr != null)
{
manifoldArray.Add(m_manifoldPtr);
}
}
public virtual void ProcessIsland(ObjectArray <CollisionObject> bodies, int numBodies, PersistentManifoldArray manifolds, int startManifold, int numManifolds, int islandId)
{
if (islandId < 0)
{
if (numManifolds + m_numConstraints > 0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver.SolveGroup(bodies, numBodies, manifolds, startManifold, numManifolds, m_sortedConstraints, 0, m_numConstraints, m_solverInfo, m_debugDrawer, m_dispatcher);
}
}
else
{
//also add all non-contact constraints/joints for this island
int startConstraint = 0;
int numCurConstraints = 0;
int i = 0;
//find the first constraint for this island
for (i = 0; i < m_numConstraints; i++)
{
if (DiscreteDynamicsWorld.GetConstraintIslandId(m_sortedConstraints[i]) == islandId)
{
startConstraint = i;
break;
}
}
//count the number of constraints in this island
for (; i < m_numConstraints; i++)
{
if (DiscreteDynamicsWorld.GetConstraintIslandId(m_sortedConstraints[i]) == islandId)
{
numCurConstraints++;
}
}
if (m_solverInfo.m_minimumSolverBatchSize <= 1)
{
///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
if (numManifolds + numCurConstraints > 0)
{
m_solver.SolveGroup(bodies, numBodies, manifolds, startManifold, numManifolds, m_sortedConstraints, startConstraint, numCurConstraints, m_solverInfo, m_debugDrawer, m_dispatcher);
}
}
else
{
for (i = 0; i < numBodies; i++)
{
m_bodies.Add(bodies[i]);
}
int lastManifold = startManifold + numManifolds;
for (i = startManifold; i < lastManifold; i++)
{
m_manifolds.Add(manifolds[i]);
}
int lastConstraint = startConstraint + numCurConstraints;
for (i = startConstraint; i < lastConstraint; i++)
{
m_constraints.Add(m_sortedConstraints[i]);
}
if ((m_constraints.Count + m_manifolds.Count) > m_solverInfo.m_minimumSolverBatchSize)
{
ProcessConstraints();
}
else
{
//printf("deferred\n");
}
}
}
}
public override void GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
///should we use m_ownManifold to avoid adding duplicates?
if (m_manifoldPtr != null && m_ownManifold)
{
manifoldArray.Add(m_manifoldPtr);
}
}
//protected virtual float solveGroupCacheFriendlyIterations()
protected float SolveGroupCacheFriendlyIterations(ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifoldPtr, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher)
{
m_iterCount++;
BulletGlobals.StartProfile("solveGroupCacheFriendlyIterations");
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlyIterations start.");
}
#endif
{
///this is a special step to resolve penetrations (just for contacts)
SolveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr,startManifold,numManifolds,constraints,startConstraint,numConstraints,infoGlobal,debugDrawer);
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
//for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
for (int iteration = 0; iteration < maxIterations; iteration++)
{
SolveSingleIteration(iteration, bodies, numBodies, manifoldPtr,startManifold,numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer);
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine("SolveGroupCacheFriendlyIterations stop.");
}
#endif
BulletGlobals.StopProfile();
return 0.0f;
}
protected float SolveSingleIteration(int iteration, ObjectArray<CollisionObject> bodies, int numBodies, PersistentManifoldArray manifold, int startManifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int startConstraint, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count;
int numConstraintPool = m_tmpSolverContactConstraintPool.Count;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine( "solveSingleIter [{0}][{1}][{2}].", numNonContactPool, numConstraintPool, numFrictionPool);
}
#endif
//should traverse the contacts random order...
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_RANDMIZE_ORDER))
{
if ((iteration & 7) == 0)
{
for (int j = 0; j < numNonContactPool; ++j)
{
int tmp = m_orderNonContactConstraintPool[j];
int swapi = RandInt2(j + 1);
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp;
}
//contact/friction constraints are not solved more than
if (iteration < infoGlobal.m_numIterations)
{
for (int j = 0; j < numConstraintPool; ++j)
{
int tmp = m_orderTmpConstraintPool[j];
int swapi = RandInt2(j + 1);
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for (int j = 0; j < numFrictionPool; ++j)
{
int tmp = m_orderFrictionConstraintPool[j];
int swapi = RandInt2(j + 1);
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
m_orderFrictionConstraintPool[swapi] = tmp;
}
}
}
}
SolverConstraint[] rawTmpSolverNonContactConstraintPool = m_tmpSolverNonContactConstraintPool.GetRawArray();
int[] rawOrderNonContactConstraintPool = m_orderNonContactConstraintPool.GetRawArray();
///solve all joint constraints
for (int j=0;j<m_tmpSolverNonContactConstraintPool.Count;j++)
{
SolverConstraint constraint = rawTmpSolverNonContactConstraintPool[rawOrderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
ResolveSingleConstraintRowGeneric(constraint.m_solverBodyA,constraint.m_solverBodyB,ref constraint);
}
}
if (iteration< infoGlobal.m_numIterations)
{
SolverConstraint[] rawTmpSolverContactConstraintPool = m_tmpSolverContactConstraintPool.GetRawArray();
int[] rawOrderTmpConstraintPool = m_orderTmpConstraintPool.GetRawArray();
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
for (int j=0;j<numPoolConstraints;j++)
{
SolverConstraint solveManifold = rawTmpSolverContactConstraintPool[rawOrderTmpConstraintPool[j]];
ResolveSingleConstraintRowLowerLimit(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,ref solveManifold);
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count;
SolverConstraint[] rawTmpSolverContactFrictionConstraintPool = m_tmpSolverContactFrictionConstraintPool.GetRawArray();
int[] rawOrderFrictionConstraintPool = m_orderFrictionConstraintPool.GetRawArray();
for (int j = 0; j < numFrictionPoolConstraints; j++)
{
SolverConstraint solveManifold = rawTmpSolverContactFrictionConstraintPool[rawOrderFrictionConstraintPool[j]];
float totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>0f)
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
ResolveSingleConstraintRowGeneric(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,ref solveManifold);
}
}
}
return 0f;
}
public override void GetAllContactManifolds(PersistentManifoldArray manifoldArray)
{
}
