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

fix more memory leaks, ImportURDFExample is now leak-free

Browse Source 
eliminate all run-time memory allocation (except for mouse-pick/ray-intersection) in ImportURDFExample
This commit is contained in:
Erwin Coumans
2016-07-16 17:40:44 -07:00
parent 2caa2b7ff4
commit e2bdd7dbb1
13 changed files with 146 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -396,22 +396,17 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
void btMultiBodyDynamicsWorld::forwardKinematics()
{
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(world_to_local,local_origin);
bod->forwardKinematics(m_scratch_world_to_local,m_scratch_local_origin);
}
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
forwardKinematics();
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
BT_PROFILE("solveConstraints");
@@ -463,9 +458,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
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());
@@ -500,15 +495,15 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
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, scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m);
}
else
{
@@ -594,9 +589,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
btScalar h = solverInfo.m_timeStep;
#define output &scratch_r[bod->getNumDofs()]
#define output &m_scratch_r[bod->getNumDofs()]
//calc qdd0 from: q0 & qd0
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
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();
@@ -606,7 +601,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd1 from: q1 & qd1
pCopyToVelocityVector(bod, scratch_qd1);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
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();
@@ -616,7 +611,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd2 from: q2 & qd2
pCopyToVelocityVector(bod, scratch_qd2);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd2, 0, numDofs);
//calc q3 = q0 + h * qd2
pResetQx();
@@ -626,7 +621,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
//
//calc qdd3 from: q3 & qd3
pCopyToVelocityVector(bod, scratch_qd3);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m);
pCopy(output, scratch_qdd3, 0, numDofs);
//
@@ -661,7 +656,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
for(int link = 0; link < bod->getNumLinks(); ++link)
bod->getLink(link).updateCacheMultiDof();
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m);
}
}
@@ -701,16 +696,16 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
scratch_v.resize(bod->getNumLinks()+1);
scratch_m.resize(bod->getNumLinks()+1);
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, scratch_r, scratch_v, scratch_m, isConstraintPass);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass);
}
}
}
@@ -732,9 +727,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
@@ -770,10 +763,10 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
world_to_local.resize(nLinks+1);
local_origin.resize(nLinks+1);
m_scratch_world_to_local.resize(nLinks+1);
m_scratch_local_origin.resize(nLinks+1);
bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local,m_scratch_local_origin);
} else
{
@@ -818,8 +811,6 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBody debugDrawWorld");
btAlignedObjectArray<btQuaternion> world_to_local1;
btAlignedObjectArray<btVector3> local_origin1;
for (int c=0;c<m_multiBodyConstraints.size();c++)
{
@@ -830,7 +821,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(world_to_local1,local_origin1);
bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);