void btSoftRigidDynamicsWorld::debugDrawWorld()
{
btDiscreteDynamicsWorld::debugDrawWorld();
if (getDebugDrawer())
{
int i;
for (i = 0; i < this->m_softBodies.size(); i++)
{
btSoftBody* psb = (btSoftBody*)this->m_softBodies[i];
if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
btSoftBodyHelpers::DrawFrame(psb, m_debugDrawer);
btSoftBodyHelpers::Draw(psb, m_debugDrawer, m_drawFlags);
}
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
if (m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb, m_debugDrawer);
if (m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb, m_debugDrawer);
if (m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb, m_debugDrawer);
}
}
}
}
void ForkLiftDemo::physicsDebugDraw(int debugFlags)
{
if (m_dynamicsWorld && m_dynamicsWorld->getDebugDrawer())
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugFlags);
m_dynamicsWorld->debugDrawWorld();
}
}
void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
// btAssert(0);
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback);
m_solverIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
{
startProfiling(timeStep);
if(0 != m_internalPreTickCallback) {
(*m_internalPreTickCallback)(this, timeStep);
}
///update aabbs information
updateAabbs();
//static int frame=0;
// printf("frame %d\n",frame++);
///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();
calculateSimulationIslands();
getSolverInfo().m_timeStep = timeStep;
///solve contact and other joint constraints
solveConstraints(getSolverInfo());
///CallbackTriggers();
calculateTimeOfImpacts(timeStep);
btScalar toi = dispatchInfo.m_timeOfImpact;
// if (toi < 1.f)
// printf("toi = %f\n",toi);
if (toi < 0.f)
kdPrintf("toi = %f\n",toi);
///integrate transforms
integrateTransforms(timeStep * toi);
///update vehicle simulation
updateActions(timeStep);
updateActivationState( timeStep );
if(0 != m_internalTickCallback) {
(*m_internalTickCallback)(this, timeStep);
}
}
void btDiscreteDynamicsWorld::synchronizeMotionStates()
{
{
//todo: iterate over awake simulation islands!
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
{
//we need to call the update at least once, even for sleeping objects
//otherwise the 'graphics' transform never updates properly
//so todo: add 'dirty' flag
//if (body->getActivationState() != ISLAND_SLEEPING)
{
btTransform interpolatedTransform;
btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
body->getMotionState()->setWorldTransform(interpolatedTransform);
}
}
}
}
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
{
for ( int i=0;i<this->m_vehicles.size();i++)
{
for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
{
//synchronize the wheels with the (interpolated) chassis worldtransform
// updateWheelTransform resets m_isInContact to false. Since
// this field is needed in STK, we save it here and restore
// its value after the call to updateWheelTransform.
bool contact = m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact;
m_vehicles[i]->updateWheelTransform(v,true);
m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact = contact;
}
}
}
}
void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
m_solverIslandCallbackMt->setup(&solverInfo, getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, m_solverIslandCallbackMt );
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
BT_PROFILE("internalSingleStepSimulation");
if(0 != m_internalPreTickCallback) {
(*m_internalPreTickCallback)(this, timeStep);
}
///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();
if (getDispatchInfo().m_useContinuous)
addSpeculativeContacts(timeStep);
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(0 != m_internalTickCallback) {
(*m_internalTickCallback)(this, timeStep);
}
}
void btFluidRigidDynamicsWorld::debugDrawWorld()
{
btDiscreteDynamicsWorld::debugDrawWorld();
btIDebugDraw* debugDrawer = getDebugDrawer();
if(debugDrawer)
{
const btVector3 FLUID_VELOCITY_COLOR(1, 1, 1);
const btVector3 FLUID_CONTACT_COLOR(1, 1, 0);
for(int i = 0; i < m_fluids.size(); i++)
{
btFluidSph* fluid = m_fluids[i];
const btFluidSphParametersLocal& FL = fluid->getLocalParameters();
if(debugDrawer->getDebugMode() & btIDebugDraw::DBG_MAX_DEBUG_DRAW_MODE)
{
for(int n = 0; n < fluid->numParticles(); ++n)
{
const btVector3& position = fluid->getPosition(n);
const btVector3& velocity = fluid->getVelocity(n);
btVector3 lineStart = position;
btVector3 lineEnd = position + ( velocity*btScalar(3.0) );
debugDrawer->drawLine(lineStart, lineEnd, FLUID_VELOCITY_COLOR);
}
}
if(debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
{
const btAlignedObjectArray<btFluidSphRigidContactGroup>& contactGroups = fluid->getRigidContacts();
for(int n = 0; n < contactGroups.size(); ++n)
{
const btFluidSphRigidContactGroup& contactGroup = contactGroups[n];
for(int j = 0; j < contactGroup.m_contacts.size(); ++j)
{
const int CONTACT_LIFETIME = 0; //btFluidSph contacts are regenerated every frame
const btFluidSphRigidContact& contact = contactGroup.m_contacts[j];
debugDrawer->drawContactPoint(contact.m_hitPointWorldOnObject, contact.m_normalOnObject, contact.m_distance,
CONTACT_LIFETIME, FLUID_CONTACT_COLOR);
}
}
}
}
}
}
void btDiscreteDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("debugDrawWorld");
btCollisionWorld::debugDrawWorld();
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
}
if(drawConstraints)
{
for(int i = getNumConstraints()-1; i>=0 ;i--)
{
btTypedConstraint* constraint = getConstraint(i);
debugDrawConstraint(constraint);
}
}
if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals)))
{
int i;
if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
{
for (i=0;i<m_actions.size();i++)
{
m_actions[i]->debugDraw(m_debugDrawer);
}
}
}
if (getDebugDrawer())
getDebugDrawer()->flushLines();
}
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;
}
void btHfFluidRigidDynamicsWorld::debugDrawWorld()
{
if (getDebugDrawer())
{
int i;
for ( i=0;i<this->m_hfFluids.size();i++)
{
btHfFluid* phh=(btHfFluid*)this->m_hfFluids[i];
switch (m_drawMode)
{
case DRAWMODE_NORMAL:
drawHfFluidGround (m_debugDrawer, phh);
//drawHfFluidNormal (m_debugDrawer, phh);
break;
case DRAWMODE_VELOCITY:
drawHfFluidGround (m_debugDrawer, phh);
//drawHfFluidNormal (m_debugDrawer, phh);
drawHfFluidVelocity (m_debugDrawer, phh);
break;
default:
btAssert (0);
break;
}
}
for (i = 0; i < this->m_collisionObjects.size(); i++)
{
btCollisionShape* shape = m_collisionObjects[i]->getCollisionShape();
if (shape->getShapeType() == HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE)
{
btHfFluidBuoyantConvexShape* buoyantShape = (btHfFluidBuoyantConvexShape*)shape;
drawHfFluidBuoyantConvexShape (m_debugDrawer, m_collisionObjects[i], buoyantShape, m_bodyDrawMode);
}
}
}
btDiscreteDynamicsWorld::debugDrawWorld();
}
void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
btDiscreteDynamicsWorld::debugDrawWorld();
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
if (drawConstraints)
{
BT_PROFILE("btMultiBody debugDrawWorld");
for (int c=0;c<m_multiBodyConstraints.size();c++)
{
btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
debugDrawMultiBodyConstraint(constraint);
}
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
for (int m = 0; m<bod->getNumLinks(); m++)
{
const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
getDebugDrawer()->drawTransform(tr, 0.1);
//draw the joint axis
if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
}
}
}
}
}
void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
{
constraint->debugDraw(getDebugDrawer());
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
forwardKinematics();
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
for (i=0;i<m_multiBodyConstraints.size();i++)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
}
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody addForce");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
}//if (!isSleeping)
}
}
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
bool doNotUpdatePos = false;
{
if(!bod->isUsingRK4Integration())
{
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
}
else
{
//
int numDofs = bod->getNumDofs() + 6;
int numPosVars = bod->getNumPosVars() + 7;
btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs);
//convenience
btScalar *pMem = &scratch_r2[0];
btScalar *scratch_q0 = pMem; pMem += numPosVars;
btScalar *scratch_qx = pMem; pMem += numPosVars;
btScalar *scratch_qd0 = pMem; pMem += numDofs;
btScalar *scratch_qd1 = pMem; pMem += numDofs;
btScalar *scratch_qd2 = pMem; pMem += numDofs;
btScalar *scratch_qd3 = pMem; pMem += numDofs;
btScalar *scratch_qdd0 = pMem; pMem += numDofs;
btScalar *scratch_qdd1 = pMem; pMem += numDofs;
btScalar *scratch_qdd2 = pMem; pMem += numDofs;
btScalar *scratch_qdd3 = pMem; pMem += numDofs;
btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]);
/////
//copy q0 to scratch_q0 and qd0 to scratch_qd0
scratch_q0[0] = bod->getWorldToBaseRot().x();
scratch_q0[1] = bod->getWorldToBaseRot().y();
scratch_q0[2] = bod->getWorldToBaseRot().z();
scratch_q0[3] = bod->getWorldToBaseRot().w();
scratch_q0[4] = bod->getBasePos().x();
scratch_q0[5] = bod->getBasePos().y();
scratch_q0[6] = bod->getBasePos().z();
//
for(int link = 0; link < bod->getNumLinks(); ++link)
{
for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
}
//
for(int dof = 0; dof < numDofs; ++dof)
scratch_qd0[dof] = bod->getVelocityVector()[dof];
////
struct
{
btMultiBody *bod;
btScalar *scratch_qx, *scratch_q0;
void operator()()
{
for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
scratch_qx[dof] = scratch_q0[dof];
}
} pResetQx = {bod, scratch_qx, scratch_q0};
//
struct
{
void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size)
{
for(int i = 0; i < size; ++i)
pVal[i] = pCurVal[i] + dt * pDer[i];
}
} pEulerIntegrate;
//
struct
{
void operator()(btMultiBody *pBody, const btScalar *pData)
{
btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector());
for(int i = 0; i < pBody->getNumDofs() + 6; ++i)
pVel[i] = pData[i];
}
} pCopyToVelocityVector;
//
struct
{
void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size)
{
for(int i = 0; i < size; ++i)
pDst[i] = pSrc[start + i];
}
} pCopy;
//
btScalar h = solverInfo.m_timeStep;
#define output &m_scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd0, 0, numDofs);
//calc q1 = q0 + h/2 * qd0
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0);
//calc qd1 = qd0 + h/2 * qdd0
pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
//
//calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_qd1);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd1, 0, numDofs);
//calc q2 = q0 + h/2 * qd1
pResetQx();
bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1);
//calc qd2 = qd0 + h/2 * qdd1
pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
//
//calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_qd2);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2
pResetQx();
bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
//calc qd3 = qd0 + h * qdd2
pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
//
//calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_qd3);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd3, 0, numDofs);
//
//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs);
btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs);
for(int i = 0; i < numDofs; ++i)
{
delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]);
delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]);
//delta_q[i] = h*scratch_qd0[i];
//delta_qd[i] = h*scratch_qdd0[i];
}
//
pCopyToVelocityVector(bod, scratch_qd0);
bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
//
if(!doNotUpdatePos)
{
btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector());
pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs();
for(int i = 0; i < numDofs; ++i)
pRealBuf[i] = delta_q[i];
//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
bod->setPosUpdated(true);
}
//ugly hack which resets the cached data to t0 (needed for constraint solver)
{
for(int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof();
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m);
}
}
}
#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
bod->clearForcesAndTorques();
#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
}//if (!isSleeping)
}
}
clearMultiBodyConstraintForces();
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks()+1);
m_scratch_m.resize(bod->getNumLinks()+1);
{
if(!bod->isUsingRK4Integration())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
}
}
}
}
}
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
{
btVector3 start = transform.getOrigin();
const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
// XY
getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
// XZ
getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
// YZ
getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
}
void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
{
// Draw a small simplex at the center of the object
{
btVector3 start = worldTransform.getOrigin();
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
}
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* colShape = compoundShape->getChildShape(i);
debugDrawObject(worldTransform*childTrans,colShape,color);
}
} else
{
switch (shape->getShapeType())
{
case SPHERE_SHAPE_PROXYTYPE:
{
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
debugDrawSphere(radius, worldTransform, color);
break;
}
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
{
btTransform childTransform = worldTransform;
childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
}
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
btScalar radius = capsuleShape->getRadius();
btScalar halfHeight = capsuleShape->getHalfHeight();
// Draw the ends
{
btTransform childTransform = worldTransform;
childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
debugDrawSphere(radius, childTransform, color);
}
{
btTransform childTransform = worldTransform;
childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
debugDrawSphere(radius, childTransform, color);
}
// Draw some additional lines
btVector3 start = worldTransform.getOrigin();
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color);
break;
}
case CONE_SHAPE_PROXYTYPE:
{
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
btVector3 start = worldTransform.getOrigin();
int upAxis= coneShape->getConeUpIndex();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = height * btScalar(0.5);
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
btVector3 offset2Radius(0,0,0);
offset2Radius[(upAxis+2)%3] = radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
break;
}
case CYLINDER_SHAPE_PROXYTYPE:
{
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
int upAxis = cylinder->getUpAxis();
btScalar radius = cylinder->getRadius();
btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
btVector3 start = worldTransform.getOrigin();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = halfHeight;
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
break;
}
case STATIC_PLANE_PROXYTYPE:
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
btScalar planeConst = staticPlaneShape->getPlaneConstant();
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
btVector3 planeOrigin = planeNormal * planeConst;
btVector3 vec0,vec1;
btPlaneSpace1(planeNormal,vec0,vec1);
btScalar vecLen = 100.f;
btVector3 pt0 = planeOrigin + vec0*vecLen;
btVector3 pt1 = planeOrigin - vec0*vecLen;
btVector3 pt2 = planeOrigin + vec1*vecLen;
btVector3 pt3 = planeOrigin - vec1*vecLen;
getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
break;
}
default:
{
if (shape->isConcave())
{
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
//todo pass camera, for some culling
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
}
if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
//todo: pass camera for some culling
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
//DebugDrawcallback drawCallback;
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
}
/// for polyhedral shapes
if (shape->isPolyhedral())
{
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
int i;
for (i=0;i<polyshape->getNumEdges();i++)
{
btPoint3 a,b;
polyshape->getEdge(i,a,b);
btVector3 wa = worldTransform * a;
btVector3 wb = worldTransform * b;
getDebugDrawer()->drawLine(wa,wb,color);
}
}
}
}
}
}
void btDiscreteDynamicsWorld::debugDrawWorld()
{
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
{
int i;
//todo: iterate over awake simulation islands!
for ( i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
{
btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
switch(colObj->getActivationState())
{
case ACTIVE_TAG:
color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
case ISLAND_SLEEPING:
color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
case WANTS_DEACTIVATION:
color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
case DISABLE_DEACTIVATION:
color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
case DISABLE_SIMULATION:
color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
default:
{
color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
}
};
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
}
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btPoint3 minAabb,maxAabb;
btVector3 colorvec(1,0,0);
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
}
}
for ( i=0;i<this->m_vehicles.size();i++)
{
for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
{
btVector3 wheelColor(0,255,255);
if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
{
wheelColor.setValue(0,0,255);
} else
{
wheelColor.setValue(255,0,255);
}
btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
btVector3 axle = btVector3(
m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
//debug wheels (cylinders)
m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
}
}
}
}
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;
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
BT_PROFILE("solveConstraints");
m_sortedConstraints.resize( m_constraints.size());
int i;
for (i=0;i<getNumConstraints();i++)
{
m_sortedConstraints[i] = m_constraints[i];
}
m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
for (i=0;i<m_multiBodyConstraints.size();i++)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
}
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
{
BT_PROFILE("btMultiBody addForce and stepVelocities");
for (int i=0;i<this->m_multiBodies.size();i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b=0;b<bod->getNumLinks();b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
scratch_r.resize(bod->getNumLinks()+1);
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
bod->clearForcesAndTorques();
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
}
}
}
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
void btCollisionWorld::debugDrawWorld()
{
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
{
int numManifolds = getDispatcher()->getNumManifolds();
btVector3 color(0,0,0);
for (int i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++)
{
btManifoldPoint& cp = contactManifold->getContactPoint(j);
getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
}
}
}
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
{
int i;
for ( i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0)
{
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
{
btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.));
switch(colObj->getActivationState())
{
case ACTIVE_TAG:
color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break;
case ISLAND_SLEEPING:
color = btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break;
case WANTS_DEACTIVATION:
color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break;
case DISABLE_DEACTIVATION:
color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break;
case DISABLE_SIMULATION:
color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break;
default:
{
color = btVector3(btScalar(1),btScalar(0.),btScalar(0.));
}
};
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
}
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btVector3 minAabb,maxAabb;
btVector3 colorvec(1,0,0);
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
minAabb -= contactThreshold;
maxAabb += contactThreshold;
btVector3 minAabb2,maxAabb2;
colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
minAabb2 -= contactThreshold;
maxAabb2 += contactThreshold;
minAabb.setMin(minAabb2);
maxAabb.setMax(maxAabb2);
m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
}
}
}
}
}
void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
{
// Draw a small simplex at the center of the object
getDebugDrawer()->drawTransform(worldTransform,1);
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* colShape = compoundShape->getChildShape(i);
debugDrawObject(worldTransform*childTrans,colShape,color);
}
} else
{
switch (shape->getShapeType())
{
case BOX_SHAPE_PROXYTYPE:
{
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtents = boxShape->getHalfExtentsWithMargin();
getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color);
break;
}
case SPHERE_SHAPE_PROXYTYPE:
{
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
getDebugDrawer()->drawSphere(radius, worldTransform, color);
break;
}
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
btTransform childTransform;
childTransform.setIdentity();
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
{
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
}
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
btScalar radius = capsuleShape->getRadius();
btScalar halfHeight = capsuleShape->getHalfHeight();
int upAxis = capsuleShape->getUpAxis();
getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color);
break;
}
case CONE_SHAPE_PROXYTYPE:
{
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
int upAxis= coneShape->getConeUpIndex();
getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color);
break;
}
case CYLINDER_SHAPE_PROXYTYPE:
{
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
int upAxis = cylinder->getUpAxis();
btScalar radius = cylinder->getRadius();
btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color);
break;
}
case STATIC_PLANE_PROXYTYPE:
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
btScalar planeConst = staticPlaneShape->getPlaneConstant();
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color);
break;
}
default:
{
if (shape->isConcave())
{
btConcaveShape* concaveMesh = (btConcaveShape*) shape;
///@todo pass camera, for some culling? no -> we are not a graphics lib
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
}
if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
//todo: pass camera for some culling
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
//DebugDrawcallback drawCallback;
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
}
/// for polyhedral shapes
if (shape->isPolyhedral())
{
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
int i;
for (i=0;i<polyshape->getNumEdges();i++)
{
btVector3 a,b;
polyshape->getEdge(i,a,b);
btVector3 wa = worldTransform * a;
btVector3 wb = worldTransform * b;
getDebugDrawer()->drawLine(wa,wb,color);
}
}
}
}
}
}
void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
btScalar dbgDrawSize = constraint->getDbgDrawSize();
if(dbgDrawSize <= btScalar(0.f))
{
return;
}
switch(constraint->getConstraintType())
{
case POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
btTransform tr;
tr.setIdentity();
btVector3 pivot = p2pC->getPivotInA();
pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
getDebugDrawer()->drawTransform(tr, dbgDrawSize);
// that ideally should draw the same frame
pivot = p2pC->getPivotInB();
pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
}
break;
case HINGE_CONSTRAINT_TYPE:
{
btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
btScalar minAng = pHinge->getLowerLimit();
btScalar maxAng = pHinge->getUpperLimit();
if(minAng == maxAng)
{
break;
}
bool drawSect = true;
if(minAng > maxAng)
{
minAng = btScalar(0.f);
maxAng = SIMD_2_PI;
drawSect = false;
}
if(drawLimits)
{
btVector3& center = tr.getOrigin();
btVector3 normal = tr.getBasis().getColumn(2);
btVector3 axis = tr.getBasis().getColumn(0);
getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
}
}
break;
case CONETWIST_CONSTRAINT_TYPE:
{
btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
{
//const btScalar length = btScalar(5);
const btScalar length = dbgDrawSize;
static int nSegments = 8*4;
btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
pPrev = tr * pPrev;
for (int i=0; i<nSegments; i++)
{
fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
pCur = tr * pCur;
getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
if (i%(nSegments/8) == 0)
getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
pPrev = pCur;
}
btScalar tws = pCT->getTwistSpan();
btScalar twa = pCT->getTwistAngle();
bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
if(useFrameB)
{
tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
}
else
{
tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
}
btVector3 pivot = tr.getOrigin();
btVector3 normal = tr.getBasis().getColumn(0);
btVector3 axis1 = tr.getBasis().getColumn(1);
getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
}
}
break;
case D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
btTransform tr = p6DOF->getCalculatedTransformA();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = p6DOF->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
{
tr = p6DOF->getCalculatedTransformA();
const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
btVector3 up = tr.getBasis().getColumn(2);
btVector3 axis = tr.getBasis().getColumn(0);
btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
axis = tr.getBasis().getColumn(1);
btScalar ay = p6DOF->getAngle(1);
btScalar az = p6DOF->getAngle(2);
btScalar cy = btCos(ay);
btScalar sy = btSin(ay);
btScalar cz = btCos(az);
btScalar sz = btSin(az);
btVector3 ref;
ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
ref[1] = -sz*axis[0] + cz*axis[1];
ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
tr = p6DOF->getCalculatedTransformB();
btVector3 normal = -tr.getBasis().getColumn(0);
btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
if(minFi > maxFi)
{
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
}
else if(minFi < maxFi)
{
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
}
tr = p6DOF->getCalculatedTransformA();
btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
}
}
break;
case SLIDER_CONSTRAINT_TYPE:
{
btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
btTransform tr = pSlider->getCalculatedTransformA();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = pSlider->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
{
btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();
btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
btVector3 normal = tr.getBasis().getColumn(0);
btVector3 axis = tr.getBasis().getColumn(1);
btScalar a_min = pSlider->getLowerAngLimit();
btScalar a_max = pSlider->getUpperAngLimit();
const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
}
}
break;
default :
break;
}
return;
}
void btDiscreteDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("debugDrawWorld");
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
{
int numManifolds = getDispatcher()->getNumManifolds();
btVector3 color(0,0,0);
for (int i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++)
{
btManifoldPoint& cp = contactManifold->getContactPoint(j);
getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
}
}
}
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
}
if(drawConstraints)
{
for(int i = getNumConstraints()-1; i>=0 ;i--)
{
btTypedConstraint* constraint = getConstraint(i);
debugDrawConstraint(constraint);
}
}
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
{
int i;
for ( i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
{
btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
switch(colObj->getActivationState())
{
case ACTIVE_TAG:
color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
case ISLAND_SLEEPING:
color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
case WANTS_DEACTIVATION:
color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
case DISABLE_DEACTIVATION:
color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
case DISABLE_SIMULATION:
color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
default:
{
color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
}
};
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
}
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btVector3 minAabb,maxAabb;
btVector3 colorvec(1,0,0);
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
}
}
if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
{
for (i=0;i<m_actions.size();i++)
{
m_actions[i]->debugDraw(m_debugDrawer);
}
}
}
}
