Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added hierarchical profiling (needs more cleanup)

Browse Source 
avoid dynamic allocations in btRaycastVehicle
This commit is contained in:
ejcoumans
2007-11-21 03:00:40 +00:00
parent 89382c0dc4
commit cab75b53ec
19 changed files with 695 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -33,6 +33,7 @@ subject to the following restrictions:
#include "btSolverBody.h"
#include "btSolverConstraint.h"
#include "LinearMath/btAlignedObjectArray.h"
#ifdef USE_PROFILE
@@ -415,6 +416,7 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3&
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
PROFILE("solveGroupCacheFriendlySetup");
(void)stackAlloc;
(void)debugDrawer;
@@ -718,6 +720,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
PROFILE("solveGroupCacheFriendlyIterations");
BEGIN_PROFILE("solveConstraintsIterations");
int numConstraintPool = m_tmpSolverConstraintPool.size();
int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
@@ -750,6 +753,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
for (j=0;j<numConstraints;j++)
{
PROFILE("solveConstraint");
btTypedConstraint* constraint = constraints[j];
///todo: use solver bodies, so we don't need to copy from/to btRigidBody
@@ -776,9 +780,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
}
{
PROFILE("resolveSingleCollisionCombinedCacheFriendly");
int numPoolConstraints = m_tmpSolverConstraintPool.size();
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
@@ -786,6 +792,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
}
{
PROFILE("resolveSingleFrictionCacheFriendly");
int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
@@ -850,7 +857,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
{
PROFILE("solveGroup");
if (getSolverMode() & SOLVER_CACHE_FRIENDLY)
{
//you need to provide at least some bodies

1
src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
View File

@@ -47,6 +47,7 @@ void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
{
startProfiling(timeStep);
///update aabbs information
updateAabbs();

99
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -22,6 +22,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btQuickProf.h"
//rigidbody & constraints
#include "BulletDynamics/Dynamics/btRigidBody.h"
@@ -122,14 +123,14 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
}
}
void btDiscreteDynamicsWorld::synchronizeMotionStates()
void btDiscreteDynamicsWorld::debugDrawWorld()
{
//debug vehicle wheels
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
{
int i;
//todo: iterate over awake simulation islands!
for ( int i=0;i<m_collisionObjects.size();i++)
for ( i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
@@ -155,6 +156,47 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates()
debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
}
}
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);
}
}
}
}
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())
{
@@ -178,31 +220,8 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates()
{
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);
}
//synchronize the wheels with the (interpolated) chassis worldtransform
m_vehicles[i]->updateWheelTransform(v,true);
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);
}
}
}
@@ -212,6 +231,10 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates()
int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
{
startProfiling(timeStep);
PROFILE("stepSimulation");
int numSimulationSubSteps = 0;
if (maxSubSteps)
@@ -262,13 +285,16 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
synchronizeMotionStates();
CProfileManager::Increment_Frame_Counter();
return numSimulationSubSteps;
}
void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
startProfiling(timeStep);
PROFILE("internalSingleStepSimulation");
///update aabbs information
updateAabbs();
@@ -306,8 +332,6 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
updateActivationState( timeStep );
}
void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
@@ -363,6 +387,7 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
{
PROFILE("updateVehicles");
BEGIN_PROFILE("updateVehicles");
for ( int i=0;i<m_vehicles.size();i++)
@@ -375,6 +400,7 @@ void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
{
PROFILE("updateActivationState");
BEGIN_PROFILE("updateActivationState");
for ( int i=0;i<m_collisionObjects.size();i++)
@@ -462,6 +488,7 @@ class btSortConstraintOnIslandPredicate
void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
PROFILE("solveConstraints");
struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
{
@@ -569,6 +596,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
void btDiscreteDynamicsWorld::calculateSimulationIslands()
{
PROFILE("calculateSimulationIslands");
BEGIN_PROFILE("calculateSimulationIslands");
getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
@@ -606,7 +634,8 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
void btDiscreteDynamicsWorld::updateAabbs()
{
BEGIN_PROFILE("updateAabbs");
PROFILE("updateAabbs");
btVector3 colorvec(1,0,0);
btTransform predictedTrans;
@@ -658,6 +687,7 @@ void btDiscreteDynamicsWorld::updateAabbs()
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
PROFILE("integrateTransforms");
BEGIN_PROFILE("integrateTransforms");
btTransform predictedTrans;
for ( int i=0;i<m_collisionObjects.size();i++)
@@ -680,6 +710,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
PROFILE("predictUnconstraintMotion");
BEGIN_PROFILE("predictUnconstraintMotion");
for ( int i=0;i<m_collisionObjects.size();i++)
{
@@ -705,6 +736,10 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
{
(void)timeStep;
CProfileManager::Reset();
#ifdef USE_QUICKPROF

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -138,6 +138,8 @@ public:
void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
void debugDrawWorld();
virtual void setConstraintSolver(btConstraintSolver* solver);
virtual btConstraintSolver* getConstraintSolver();

62
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
View File

@@ -526,15 +526,10 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
if (!numWheel)
return;
void* mem = btAlignedAlloc(numWheel*sizeof(btVector3),16);
btVector3* forwardWS = new (mem)btVector3[numWheel];
mem = btAlignedAlloc(numWheel*sizeof(btVector3),16);
btVector3* axle = new (mem)btVector3[numWheel];
mem = btAlignedAlloc(numWheel*sizeof(btScalar),16);
btScalar* forwardImpulse = new (mem)btScalar[numWheel];
mem = btAlignedAlloc(numWheel*sizeof(btScalar),16);
btScalar* sideImpulse = new(mem) btScalar[numWheel];
m_forwardWS.resize(numWheel);
m_axle.resize(numWheel);
m_forwardImpulse.resize(numWheel);
m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0;
@@ -546,8 +541,8 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
numWheelsOnGround++;
sideImpulse[i] = btScalar(0.);
forwardImpulse[i] = btScalar(0.);
m_sideImpulse[i] = btScalar(0.);
m_forwardImpulse[i] = btScalar(0.);
}
@@ -566,25 +561,25 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
const btTransform& wheelTrans = getWheelTransformWS( i );
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
axle[i] = btVector3(
m_axle[i] = btVector3(
wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]);
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
btScalar proj = axle[i].dot(surfNormalWS);
axle[i] -= surfNormalWS * proj;
axle[i] = axle[i].normalize();
btScalar proj = m_axle[i].dot(surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i] = m_axle[i].normalize();
forwardWS[i] = surfNormalWS.cross(axle[i]);
forwardWS[i].normalize();
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
m_forwardWS[i].normalize();
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
btScalar(0.), axle[i],sideImpulse[i],timeStep);
btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
sideImpulse[i] *= sideFrictionStiffness2;
m_sideImpulse[i] *= sideFrictionStiffness2;
}
@@ -613,7 +608,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
{
btScalar defaultRollingFrictionImpulse = 0.f;
btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,forwardWS[wheel],maxImpulse);
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
rollingFriction = calcRollingFriction(contactPt);
}
}
@@ -623,7 +618,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
forwardImpulse[wheel] = btScalar(0.);
m_forwardImpulse[wheel] = btScalar(0.);
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
if (groundObject)
@@ -636,10 +631,10 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
btScalar maximpSquared = maximp * maximpSide;
forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
btScalar x = (forwardImpulse[wheel] ) * fwdFactor;
btScalar y = (sideImpulse[wheel] ) * sideFactor;
btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
btScalar impulseSquared = (x*x + y*y);
@@ -663,12 +658,12 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
{
for (int wheel = 0;wheel < getNumWheels(); wheel++)
{
if (sideImpulse[wheel] != btScalar(0.))
if (m_sideImpulse[wheel] != btScalar(0.))
{
if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
{
forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
@@ -683,11 +678,11 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody->getCenterOfMassPosition();
if (forwardImpulse[wheel] != btScalar(0.))
if (m_forwardImpulse[wheel] != btScalar(0.))
{
m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos);
m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
}
if (sideImpulse[wheel] != btScalar(0.))
if (m_sideImpulse[wheel] != btScalar(0.))
{
class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
@@ -695,7 +690,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
groundObject->getCenterOfMassPosition();
btVector3 sideImp = axle[wheel] * sideImpulse[wheel];
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
rel_pos[2] *= wheelInfo.m_rollInfluence;
m_chassisBody->applyImpulse(sideImp,rel_pos);
@@ -706,10 +701,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
}
}
btAlignedFree(forwardWS);
btAlignedFree(axle);
btAlignedFree(forwardImpulse);
btAlignedFree(sideImpulse);
}

6
src/BulletDynamics/Vehicle/btRaycastVehicle.h
View File

@@ -23,6 +23,12 @@ class btVehicleTuning;
///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
class btRaycastVehicle : public btTypedConstraint
{
btAlignedObjectArray<btVector3> m_forwardWS;
btAlignedObjectArray<btVector3> m_axle;
btAlignedObjectArray<btScalar> m_forwardImpulse;
btAlignedObjectArray<btScalar> m_sideImpulse;
public:
class btVehicleTuning
{