protected virtual void CalculateSimulationIslands()
{
BulletGlobals.StartProfile("calculateSimulationIslands");
GetSimulationIslandManager().UpdateActivationState(GetCollisionWorld(), GetCollisionWorld().GetDispatcher());
{
int length = m_constraints.Count;
for (int i = 0; i < length; ++i)
{
TypedConstraint constraint = m_constraints[i];
RigidBody colObj0 = constraint.GetRigidBodyA();
RigidBody colObj1 = constraint.GetRigidBodyB();
if (((colObj0 != null) && (!colObj0.IsStaticOrKinematicObject())) &&
((colObj1 != null) && (!colObj1.IsStaticOrKinematicObject())))
{
if (colObj0.IsActive() || colObj1.IsActive())
{
GetSimulationIslandManager().GetUnionFind().Unite((colObj0).GetIslandTag(),
(colObj1).GetIslandTag());
}
}
}
}
//Store the island id in each body
GetSimulationIslandManager().StoreIslandActivationState(GetCollisionWorld());
BulletGlobals.StopProfile();
}
protected virtual void PredictUnconstraintMotion(float timeStep)
{
BulletGlobals.StartProfile("predictUnconstraintMotion");
int length = m_nonStaticRigidBodies.Count;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("PredictUnconstraintMotion [{0}][{1}]", length, timeStep);
}
#endif
//for (int i = 0; i < length;i++)
for (int i = 0; i < m_nonStaticRigidBodies.Count; i++)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (!body.IsStaticOrKinematicObject())
{
body.IntegrateVelocities(timeStep);
//dampingF
body.ApplyDamping(timeStep);
IndexedMatrix temp;
body.PredictIntegratedTransform(timeStep, out temp);
body.SetInterpolationWorldTransform(ref temp);
}
}
BulletGlobals.StopProfile();
}
public override void DebugDrawWorld()
{
BulletGlobals.StartProfile("debugDrawWorld");
base.DebugDrawWorld();
if (GetDebugDrawer() != null)
{
DebugDrawModes mode = GetDebugDrawer().GetDebugMode();
if ((mode & (DebugDrawModes.DBG_DrawConstraints | DebugDrawModes.DBG_DrawConstraintLimits)) != 0)
{
for (int i = GetNumConstraints() - 1; i >= 0; i--)
{
TypedConstraint constraint = GetConstraint(i);
DrawHelper.DebugDrawConstraint(constraint, GetDebugDrawer());
}
}
if (mode != 0)
{
int actionsCount = m_actions.Count;
for (int i = 0; i < actionsCount; ++i)
{
m_actions[i].DebugDraw(m_debugDrawer);
}
}
}
BulletGlobals.StopProfile();
}
protected void UpdateActions(float timeStep)
{
BulletGlobals.StartProfile("updateActions");
int length = m_actions.Count;
for (int i = 0; i < length; ++i)
{
m_actions[i].UpdateAction(this, timeStep);
}
BulletGlobals.StopProfile();
}
protected virtual void InternalSingleStepSimulation(float timeStep)
{
BulletGlobals.StartProfile("internalSingleStepSimulation");
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("internalSingleStepSimulation");
}
#endif
if (null != m_internalPreTickCallback)
{
m_internalPreTickCallback.InternalTickCallback(this, timeStep);
}
///apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = GetDispatchInfo();
dispatchInfo.SetTimeStep(timeStep);
dispatchInfo.SetStepCount(0);
dispatchInfo.SetDebugDraw(GetDebugDrawer());
///perform collision detection
PerformDiscreteCollisionDetection();
CalculateSimulationIslands();
GetSolverInfo().m_timeStep = timeStep;
///solve contact and other joint constraints
SolveConstraints(GetSolverInfo());
///CallbackTriggers();
///integrate transforms
IntegrateTransforms(timeStep);
///update vehicle simulation
UpdateActions(timeStep);
UpdateActivationState(timeStep);
if (m_internalTickCallback != null)
{
m_internalTickCallback.InternalTickCallback(this, timeStep);
}
BulletGlobals.StopProfile();
}
protected void UpdateActivationState(float timeStep)
{
BulletGlobals.StartProfile("updateActivationState");
int length = m_nonStaticRigidBodies.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body != null)
{
body.UpdateDeactivation(timeStep);
if (body.WantsSleeping())
{
if (body.IsStaticOrKinematicObject())
{
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
else
{
if (body.GetActivationState() == ActivationState.ACTIVE_TAG)
{
body.SetActivationState(ActivationState.WANTS_DEACTIVATION);
}
if (body.GetActivationState() == ActivationState.ISLAND_SLEEPING)
{
IndexedVector3 zero = IndexedVector3.Zero;
body.SetAngularVelocity(ref zero);
body.SetLinearVelocity(ref zero);
if (body.GetMotionState() != null)
{
body.GetMotionState().SetWorldTransform(body.GetWorldTransform());
}
}
}
}
else
{
if (body.GetActivationState() != ActivationState.DISABLE_DEACTIVATION)
{
body.SetActivationState(ActivationState.ACTIVE_TAG);
}
}
}
}
BulletGlobals.StopProfile();
}
protected virtual void PredictUnconstraintMotion(float timeStep)
{
BulletGlobals.StartProfile("predictUnconstraintMotion");
int length = m_nonStaticRigidBodies.Count;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("PredictUnconstraintMotion [{0}][{1}]", length, timeStep);
}
/* the used parallel lib was never pushed it would also be totally obsolete
* Parallel.For(3,0, length, delegate(int i)
* {
* RigidBody body = m_nonStaticRigidBodies[i];
* if (!body.IsStaticOrKinematicObject())
* {
* body.IntegrateVelocities(timeStep);
* //dampingF
* body.ApplyDamping(timeStep);
* IndexedMatrix temp;
* body.PredictIntegratedTransform(timeStep, out temp);
* body.SetInterpolationWorldTransform(ref temp);
* }
* });
*/
//for (int i = 0; i < length;i++)
for (int i = 0; i < m_nonStaticRigidBodies.Count; i++)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (!body.IsStaticOrKinematicObject())
{
body.IntegrateVelocities(timeStep);
//dampingF
body.ApplyDamping(timeStep);
IndexedMatrix temp;
body.PredictIntegratedTransform(timeStep, out temp);
body.SetInterpolationWorldTransform(ref temp);
}
}
BulletGlobals.StopProfile();
}
public static void FindCollision(GImpactQuantizedBvh boxset0, ref IndexedMatrix trans0,
GImpactQuantizedBvh boxset1, ref IndexedMatrix trans1,
PairSet collision_pairs)
{
if (boxset0.GetNodeCount() == 0 || boxset1.GetNodeCount() == 0)
{
return;
}
BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0 = new BT_BOX_BOX_TRANSFORM_CACHE();
trans_cache_1to0.CalcFromHomogenic(ref trans0, ref trans1);
#if TRI_COLLISION_PROFILING
BulletGlobals.StartProfile("GIMPACT-TRIMESH");
#endif //TRI_COLLISION_PROFILING
FindQuantizedCollisionPairsRecursive(boxset0, boxset1, collision_pairs, ref trans_cache_1to0, 0, 0, true);
#if TRI_COLLISION_PROFILING
BulletGlobals.StopProfile();
#endif //TRI_COLLISION_PROFILING
}
public void BuildIslands(IDispatcher dispatcher, CollisionWorld collisionWorld)
{
BulletGlobals.StartProfile("islandUnionFindAndQuickSort");
ObjectArray <CollisionObject> collisionObjects = collisionWorld.GetCollisionObjectArray();
m_islandmanifold.Clear();
//we are going to sort the unionfind array, and store the element id in the size
//afterwards, we clean unionfind, to make sure no-one uses it anymore
GetUnionFind().sortIslands();
int numElem = GetUnionFind().GetNumElements();
int endIslandIndex = 1;
//update the sleeping state for bodies, if all are sleeping
for (int startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = GetUnionFind().GetElement(startIslandIndex).m_id;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("buildIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
}
#endif
for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (GetUnionFind().GetElement(endIslandIndex).m_id == islandId); endIslandIndex++)
{
}
//int numSleeping = 0;
bool allSleeping = true;
for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = GetUnionFind().GetElement(idx).m_sz;
CollisionObject colObj0 = collisionObjects[i];
if ((colObj0.GetIslandTag() != islandId) && (colObj0.GetIslandTag() != -1))
{
// printf("error in island management\n");
}
Debug.Assert((colObj0.GetIslandTag() == islandId) || (colObj0.GetIslandTag() == -1));
if (colObj0.GetIslandTag() == islandId)
{
if (colObj0.GetActivationState() == ActivationState.ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0.GetActivationState() == ActivationState.DISABLE_DEACTIVATION)
{
allSleeping = false;
}
}
}
if (allSleeping)
{
for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = GetUnionFind().GetElement(idx).m_sz;
CollisionObject colObj0 = collisionObjects[i];
int islandTag = colObj0.GetIslandTag();
if (islandTag != islandId && islandTag != -1)
{
// printf("error in island management\n");
}
Debug.Assert((islandTag == islandId) || (islandTag == -1));
if (islandTag == islandId)
{
colObj0.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
}
}
else
{
for (int idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = GetUnionFind().GetElement(idx).m_sz;
CollisionObject colObj0 = collisionObjects[i];
int islandTag = colObj0.GetIslandTag();
if (islandTag != islandId && islandTag != -1)
{
// printf("error in island management\n");
}
Debug.Assert((islandTag == islandId) || (islandTag == -1));
if (islandTag == islandId)
{
if (colObj0.GetActivationState() == ActivationState.ISLAND_SLEEPING)
{
colObj0.SetActivationState(ActivationState.WANTS_DEACTIVATION);
colObj0.SetDeactivationTime(0f);
}
}
}
}
}
int maxNumManifolds = dispatcher.GetNumManifolds();
//#definef SPLIT_ISLANDS 1
//#ifdef SPLIT_ISLANDS
//#endif //SPLIT_ISLANDS
for (int i = 0; i < maxNumManifolds; i++)
{
PersistentManifold manifold = dispatcher.GetManifoldByIndexInternal(i);
CollisionObject colObj0 = manifold.GetBody0() as CollisionObject;
CollisionObject colObj1 = manifold.GetBody1() as CollisionObject;
///@todo: check sleeping conditions!
if (((colObj0 != null) && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING) ||
((colObj1 != null) && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING))
{
//kinematic objects don't merge islands, but wake up all connected objects
if (colObj0.IsKinematicObject() && colObj0.GetActivationState() != ActivationState.ISLAND_SLEEPING)
{
if (colObj0.HasContactResponse())
{
colObj1.Activate();
}
}
if (colObj1.IsKinematicObject() && colObj1.GetActivationState() != ActivationState.ISLAND_SLEEPING)
{
if (colObj1.HasContactResponse())
{
colObj0.Activate();
}
}
if (m_splitIslands)
{
//filtering for response
if (dispatcher.NeedsResponse(colObj0, colObj1))
{
m_islandmanifold.Add(manifold);
}
}
}
}
BulletGlobals.StopProfile();
}
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 DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("buildAndProcessIslands start[{0}] end[{1}] id[{2}]", startIslandIndex, endIslandIndex, islandId));
}
#endif
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 DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine("curIsland[{0}] startManifold[{1}].", curIslandId, startManifoldIndex);
}
#endif
if (curIslandId == islandId)
{
startManifold = m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++)
{
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine("endManifoldIndex[{0}] islandId[{1}] getIsland[{2}].", endManifoldIndex, startManifoldIndex, GetIslandId(m_islandmanifold[endManifoldIndex]));
}
#endif
}
/// 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 DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugIslands)
{
BulletGlobals.g_streamWriter.WriteLine("islandSleeping.");
}
#endif
}
if (numIslandManifolds != 0)
{
startManifoldIndex = endManifoldIndex;
}
m_islandBodies.Clear();
}
} // else if(!splitIslands)
BulletGlobals.StopProfile();
}
public override int StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)
{
StartProfiling(timeStep);
BulletGlobals.StartProfile("stepSimulation");
int numSimulationSubSteps = 0;
if (maxSubSteps != 0)
{
//fixed timestep with interpolation
m_localTime += timeStep;
if (m_localTime >= fixedTimeStep)
{
numSimulationSubSteps = (int)(m_localTime / fixedTimeStep);
m_localTime -= numSimulationSubSteps * fixedTimeStep;
}
}
else
{
//variable timestep
fixedTimeStep = timeStep;
m_localTime = timeStep;
if (MathUtil.FuzzyZero(timeStep))
{
numSimulationSubSteps = 0;
maxSubSteps = 0;
}
else
{
numSimulationSubSteps = 1;
maxSubSteps = 1;
}
}
//process some debugging flags
if (GetDebugDrawer() != null)
{
IDebugDraw debugDrawer = GetDebugDrawer();
BulletGlobals.gDisableDeactivation = ((debugDrawer.GetDebugMode() & DebugDrawModes.DBG_NoDeactivation) != 0);
}
if (numSimulationSubSteps != 0)
{
//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("Stepsimulation numClamped[{0}] timestep[{1:0.00000}]", clampedSimulationSteps, fixedTimeStep));
}
#endif
SaveKinematicState(fixedTimeStep * clampedSimulationSteps);
ApplyGravity();
for (int i = 0; i < clampedSimulationSteps; i++)
{
InternalSingleStepSimulation(fixedTimeStep);
SynchronizeMotionStates();
}
}
else
{
SynchronizeMotionStates();
}
ClearForces();
if (m_profileManager != null)
{
m_profileManager.Increment_Frame_Counter();
}
return(numSimulationSubSteps);
}
protected virtual void IntegrateTransforms(float timeStep)
{
BulletGlobals.StartProfile("integrateTransforms");
IndexedMatrix predictedTrans;
int length = m_nonStaticRigidBodies.Count;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("IntegrateTransforms [{0}]", length);
}
#endif
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body != null)
{
body.SetHitFraction(1f);
if (body.IsActive() && (!body.IsStaticOrKinematicObject()))
{
body.PredictIntegratedTransform(timeStep, out predictedTrans);
float squareMotion = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
//if (body.GetCcdSquareMotionThreshold() != 0 && body.GetCcdSquareMotionThreshold() < squareMotion)
if (GetDispatchInfo().m_useContinuous&& body.GetCcdSquareMotionThreshold() != 0.0f && body.GetCcdSquareMotionThreshold() < squareMotion)
{
BulletGlobals.StartProfile("CCD motion clamping");
if (body.GetCollisionShape().IsConvex())
{
gNumClampedCcdMotions++;
using (ClosestNotMeConvexResultCallback sweepResults = BulletGlobals.ClosestNotMeConvexResultCallbackPool.Get())
{
sweepResults.Initialize(body, body.GetWorldTransform()._origin, predictedTrans._origin, GetBroadphase().GetOverlappingPairCache(), GetDispatcher());
//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
SphereShape tmpSphere = BulletGlobals.SphereShapePool.Get();
tmpSphere.Initialize(body.GetCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body.GetCollisionShape());
sweepResults.m_allowedPenetration = GetDispatchInfo().GetAllowedCcdPenetration();
sweepResults.m_collisionFilterGroup = body.GetBroadphaseProxy().m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body.GetBroadphaseProxy().m_collisionFilterMask;
IndexedMatrix modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans._basis = body.GetWorldTransform()._basis;
modifiedPredictedTrans._origin = predictedTrans._origin;
ConvexSweepTest(tmpSphere, body.GetWorldTransform(), modifiedPredictedTrans, sweepResults, 0f);
if (sweepResults.HasHit() && (sweepResults.m_closestHitFraction < 1.0f))
{
//printf("clamped integration to hit fraction = %f\n",fraction);
body.SetHitFraction(sweepResults.m_closestHitFraction);
body.PredictIntegratedTransform(timeStep * body.GetHitFraction(), out predictedTrans);
body.SetHitFraction(0.0f);
body.ProceedToTransform(ref predictedTrans);
#if false
btVector3 linVel = body.getLinearVelocity();
float maxSpeed = body.getCcdMotionThreshold() / getSolverInfo().m_timeStep;
float maxSpeedSqr = maxSpeed * maxSpeed;
if (linVel.LengthSquared() > maxSpeedSqr)
{
linVel.normalize();
linVel *= maxSpeed;
body.setLinearVelocity(linVel);
float ms2 = body.getLinearVelocity().LengthSquared();
body.predictIntegratedTransform(timeStep, predictedTrans);
float sm2 = (predictedTrans._origin - body.GetWorldTransform()._origin).LengthSquared();
float smt = body.getCcdSquareMotionThreshold();
printf("sm2=%f\n", sm2);
}
#else
//response between two dynamic objects without friction, assuming 0 penetration depth
float appliedImpulse = 0.0f;
float depth = 0.0f;
appliedImpulse = ContactConstraint.ResolveSingleCollision(body, sweepResults.m_hitCollisionObject, ref sweepResults.m_hitPointWorld, ref sweepResults.m_hitNormalWorld, GetSolverInfo(), depth);
#endif
continue;
}
BulletGlobals.SphereShapePool.Free(tmpSphere);
}
}
BulletGlobals.StopProfile();
}
body.ProceedToTransform(ref predictedTrans);
}
}
}
}
public void Collide(IDispatcher dispatcher)
{
BulletGlobals.StartProfile("BroadphaseCollide");
//SPC(m_profiling.m_total);
/* optimize */
m_sets[0].OptimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
if (m_fixedleft > 0)
{
int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100;
m_sets[1].OptimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100);
m_fixedleft = Math.Max(0, m_fixedleft - count);
}
/* dynamic . fixed set */
m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
DbvtProxy current = m_stageRoots[m_stageCurrent];
if (current != null)
{
DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get();
collider.Initialize(this);
do
{
DbvtProxy next = current.links[1];
ListRemove(current, ref m_stageRoots[current.stage]);
ListAppend(current, ref m_stageRoots[STAGECOUNT]);
#if DBVT_BP_ACCURATESLEEPING
m_paircache.removeOverlappingPairsContainingProxy(current, dispatcher);
collider.proxy = current;
btDbvt::collideTV(m_sets[0].m_root, current.aabb, collider);
btDbvt::collideTV(m_sets[1].m_root, current.aabb, collider);
#endif
m_sets[0].Remove(current.leaf);
DbvtAabbMm curAabb = DbvtAabbMm.FromMM(ref current.m_aabbMin, ref current.m_aabbMax);
current.leaf = m_sets[1].Insert(ref curAabb, current);
current.stage = STAGECOUNT;
current = next;
} while (current != null);
m_fixedleft = m_sets[1].m_leaves;
BulletGlobals.DbvtTreeColliderPool.Free(collider);
m_needcleanup = true;
}
/* collide dynamics */
{
DbvtTreeCollider collider = BulletGlobals.DbvtTreeColliderPool.Get();
collider.Initialize(this);
if (m_deferedcollide)
{
//Stopwatch fdCollideStopwatch = new Stopwatch();
//fdCollideStopwatch.Start();
//SPC(m_profiling.m_fdcollide);
Dbvt.CollideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
//fdCollideStopwatch.Stop();
//m_profiling.m_fdcollide += (ulong)fdCollideStopwatch.ElapsedMilliseconds;
}
if (m_deferedcollide)
{
//Stopwatch ddCollideStopwatch = new Stopwatch();
//ddCollideStopwatch.Start();
//SPC(m_profiling.m_ddcollide);
Dbvt.CollideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
//ddCollideStopwatch.Stop();
//m_profiling.m_ddcollide += (ulong)ddCollideStopwatch.ElapsedMilliseconds;
}
BulletGlobals.DbvtTreeColliderPool.Free(collider);
}
/* clean up */
if (m_needcleanup)
{
Stopwatch cleanupStopwatch = new Stopwatch();
cleanupStopwatch.Start();
//SPC(m_profiling.m_cleanup);
IList <BroadphasePair> pairs = m_paircache.GetOverlappingPairArray();
if (pairs.Count > 0)
{
int ni = Math.Min(pairs.Count, Math.Max(m_newpairs, (pairs.Count * m_cupdates) / 100));
for (int i = 0; i < ni; ++i)
{
BroadphasePair p = pairs[(m_cid + i) % pairs.Count];
DbvtProxy pa = p.m_pProxy0 as DbvtProxy;
DbvtProxy pb = p.m_pProxy1 as DbvtProxy;
if (!DbvtAabbMm.Intersect(ref pa.leaf.volume, ref pb.leaf.volume))
{
#if DBVT_BP_SORTPAIRS
if (pa.m_uniqueId > pb.m_uniqueId)
{
btSwap(pa, pb);
}
#endif
m_paircache.RemoveOverlappingPair(pa, pb, dispatcher);
--ni; --i;
}
}
if (pairs.Count > 0)
{
m_cid = (m_cid + ni) % pairs.Count;
}
else
{
m_cid = 0;
}
}
cleanupStopwatch.Stop();
//m_profiling.m_cleanup += (ulong)cleanupStopwatch.ElapsedMilliseconds;
}
++m_pid;
m_newpairs = 1;
m_needcleanup = false;
if (m_updates_call > 0)
{
m_updates_ratio = m_updates_done / (float)m_updates_call;
}
else
{
m_updates_ratio = 0;
}
m_updates_done /= 2;
m_updates_call /= 2;
BulletGlobals.StopProfile();
}