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"); } } } }