int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep, btScalar fixedSubSteps)
{
(void)fixedTimeStep;
(void)maxSubSteps;
(void)fixedSubSteps;
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection
performDiscreteCollisionDetection();
///solve contact constraints
int numManifolds = m_dispatcher1->getNumManifolds();
if (numManifolds)
{
btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep;
m_constraintSolver->prepareSolve(0, numManifolds);
m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds,0,0, infoGlobal, m_debugDrawer, m_dispatcher1);
m_constraintSolver->allSolved(infoGlobal, m_debugDrawer);
}
///integrate transforms
integrateTransforms(timeStep);
updateAabbs();
synchronizeMotionStates();
clearForces();
return 1;
}
int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
{
startProfiling(timeStep);
BT_PROFILE("stepSimulation");
int numSimulationSubSteps = 0;
if (maxSubSteps)
{
//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 (btFuzzyZero(timeStep))
{
numSimulationSubSteps = 0;
maxSubSteps = 0;
} else
{
numSimulationSubSteps = 1;
maxSubSteps = 1;
}
}
//process some debugging flags
if (getDebugDrawer())
{
btIDebugDraw* debugDrawer = getDebugDrawer ();
gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
}
if (numSimulationSubSteps)
{
//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
saveKinematicState(fixedTimeStep*clampedSimulationSteps);
applyGravity();
for (int i=0;i<clampedSimulationSteps;i++)
{
internalSingleStepSimulation(fixedTimeStep);
synchronizeMotionStates();
}
} else
{
synchronizeMotionStates();
}
clearForces();
#ifndef BT_NO_PROFILE
CProfileManager::Increment_Frame_Counter();
#endif //BT_NO_PROFILE
return numSimulationSubSteps;
}
int b3GpuDynamicsWorld::stepSimulation( btScalar timeStepUnused, int maxSubStepsUnused, btScalar fixedTimeStep)
{
///Don't use more than 1 simulation step, it destroys the performance having to copy the data between CPU and GPU multiple times per frame
///Please use the CPU version in btDiscreteDynamicsWorld if you don't like this
#ifndef BT_NO_PROFILE
CProfileManager::Reset();
#endif //BT_NO_PROFILE
BT_PROFILE("stepSimulation");
{
BT_PROFILE("sync constraints CPU");
//todo: determine what data has changed, or perform copy on GPU?
for (int i=0;i<m_constraints.size();i++)
{
btTypedConstraint* constraint = m_constraints[i];
b3TypedConstraint* c = (b3TypedConstraint*) constraint->getUserConstraintPtr();
if (c)
{
switch (constraint->getConstraintType())
{
case POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint* p2 = (btPoint2PointConstraint*) constraint;
b3Point2PointConstraint* p3 = (b3Point2PointConstraint*) c;
p3->setPivotA((const b3Vector3&)p2->getPivotInA());
p3->setPivotB((const b3Vector3&)p2->getPivotInB());
p3->m_setting.m_damping = p2->m_setting.m_damping;
p3->m_setting.m_impulseClamp = p2->m_setting.m_impulseClamp;
p3->m_setting.m_tau = p2->m_setting.m_tau;
break;
};
case D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint;
b3Generic6DofConstraint* dof3 = (b3Generic6DofConstraint*) c;
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
b3Transform frameInA = (b3Transform&) dof2->getFrameOffsetA();
b3Transform frameInB = (b3Transform&) dof2->getFrameOffsetB();
dof3->setFrames(frameInA,frameInB,bodiesCL);
break;
}
default:
{
}
};
}
}
}
// detect any change (very simple)
{
BT_PROFILE("body update revision detection (CPU)");
#ifdef BT_USE_BODY_UPDATE_REVISION
b3Assert(m_bodyUpdateRevisions.size() == m_collisionObjects.size());
b3Assert(m_np->getNumRigidBodies() == m_bodyUpdateRevisions.size());
#endif //BT_USE_BODY_UPDATE_REVISION
for (int i=0;i<this->m_collisionObjects.size();i++)
{
if (i>=m_np->getNumRigidBodies())
{
b3Error("bodiesCL out-of-range\n");
continue;
}
#ifdef BT_USE_BODY_UPDATE_REVISION
if (m_bodyUpdateRevisions[i] != m_collisionObjects[i]->getUpdateRevisionInternal())
#endif//BT_USE_BODY_UPDATE_REVISION
{
m_cpuGpuSync = true;
#ifdef BT_USE_BODY_UPDATE_REVISION
m_bodyUpdateRevisions[i] = m_collisionObjects[i]->getUpdateRevisionInternal();
#endif
btRigidBody* body = btRigidBody::upcast(m_collisionObjects[i]);
if (body)
{
b3Vector3 pos = (const b3Vector3&)m_collisionObjects[i]->getWorldTransform().getOrigin();
btQuaternion orn2 = m_collisionObjects[i]->getWorldTransform().getRotation();
b3Quaternion orn(orn2[0],orn2[1],orn2[2],orn2[3]);
body->integrateVelocities(fixedTimeStep);
m_np->setObjectTransformCpu(&pos[0],&orn[0],i);
b3Vector3 linVel = (const b3Vector3&)body->getLinearVelocity();
b3Vector3 angVel = (const b3Vector3&)body->getAngularVelocity();
m_np->setObjectVelocityCpu(&linVel[0],&angVel[0],i);
}
}
}
}
if (m_cpuGpuSync)
{
BT_PROFILE("cpuGpuSync");
m_cpuGpuSync = false;
m_np->writeAllBodiesToGpu();
m_bp->writeAabbsToGpu();
m_rigidBodyPipeline->writeAllInstancesToGpu();
}
//internalSingleStepSimulation(fixedTimeStep);
// dispatch preTick callback
if(0 != m_internalPreTickCallback)
{
BT_PROFILE("internalPreTickCallback");
(*m_internalPreTickCallback)(this, fixedTimeStep);
}
{
BT_PROFILE("m_rigidBodyPipeline->stepSimulation");
m_rigidBodyPipeline->stepSimulation(fixedTimeStep);
}
{
BT_PROFILE("readbackBodiesToCpu");
//now copy info back to rigid bodies....
m_np->readbackAllBodiesToCpu();
}
{
BT_PROFILE("scatter transforms into rigidbody (CPU)");
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
for (int i=0;i<this->m_collisionObjects.size();i++)
{
btVector3 pos;
btQuaternion orn;
if (m_np->getObjectTransformFromCpu(&pos[0],&orn[0],i))
{
btTransform newTrans;
newTrans.setOrigin(pos);
newTrans.setRotation(orn);
btCollisionObject* colObj = this->m_collisionObjects[i];
colObj->setWorldTransform(newTrans);
btRigidBody* body = btRigidBody::upcast(m_collisionObjects[i]);
if (body)
{
body->setLinearVelocity((btVector3&)bodiesCL[i].m_linVel);
body->setAngularVelocity((btVector3&)bodiesCL[i].m_angVel);
}
}
#ifdef BT_USE_BODY_UPDATE_REVISION
//ignore this revision update
m_bodyUpdateRevisions[i] = m_collisionObjects[i]->getUpdateRevisionInternal();
#endif //BT_USE_BODY_UPDATE_REVISION
}
{
BT_PROFILE("synchronizeMotionStates");
synchronizeMotionStates();
}
}
clearForces();
#ifndef B3_NO_PROFILE
CProfileManager::Increment_Frame_Counter();
#endif //B3_NO_PROFILE
return 1;
}
