public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep)
{
//apply gravity, predict motion
PredictUnconstraintMotion(timeStep);
DispatcherInfo dispatchInfo = new DispatcherInfo();
dispatchInfo.TimeStep = timeStep;
dispatchInfo.StepCount = 0;
dispatchInfo.DebugDraw = DebugDrawer;
//perform collision detection
PerformDiscreteCollisionDetection();
//solve contact constraints
int numManifolds = Dispatcher.ManifoldCount;
if (numManifolds != 0)
{
List <PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds;
//int numManifolds = m_dispatcher1.GetNumManifolds();
ContactSolverInfo infoGlobal = new ContactSolverInfo();
infoGlobal.TimeStep = timeStep;
_constraintSolver.SolveGroup(new List <CollisionObject>(), manifolds, manifolds.Count, new List <TypedConstraint>(), infoGlobal, _debugDrawer);
}
//integrate transforms
IntegrateTransforms(timeStep);
UpdateAabbs();
SynchronizeMotionStates();
}
public void ProcessIsland(List <CollisionObject> bodies, List <PersistentManifold> manifolds, int numManifolds, int islandID)
{
//also add all non-contact constraints/joints for this island
List <TypedConstraint> startConstraint = new List <TypedConstraint>();
int numCurConstraints = 0;
int startIndex = 0;
int i;
//find the first constraint for this island
for (i = 0; i < _sortedConstraints.Count; i++)
{
if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
{
//startConstraint = &m_sortedConstraints[i];
startIndex = i;
break;
}
}
//count the number of constraints in this island
for (; i < _sortedConstraints.Count; i++)
{
if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
{
numCurConstraints++;
}
}
for (i = startIndex; i < startIndex + numCurConstraints; i++)
{
startConstraint.Add(_sortedConstraints[i]);
}
_solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer);
}
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");
}
}
}
}