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

16
examples/CommonInterfaces/CommonMultiBodyBase.h
View File

@@ -63,7 +63,7 @@ struct CommonMultiBodyBase : public CommonExampleInterface
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase(); m_broadphase = new btDbvtBroadphase();//btSimpleBroadphase();
m_solver = new btMultiBodyConstraintSolver; m_solver = new btMultiBodyConstraintSolver;
@@ -97,6 +97,20 @@ struct CommonMultiBodyBase : public CommonExampleInterface
{ {
m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i)); m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i));
} }
for (i = m_dynamicsWorld->getNumMultiBodyConstraints() - 1; i >= 0; i--)
{
btMultiBodyConstraint* mbc = m_dynamicsWorld->getMultiBodyConstraint(i);
m_dynamicsWorld->removeMultiBodyConstraint(mbc);
delete mbc;
}
for (i = m_dynamicsWorld->getNumMultibodies() - 1; i >= 0; i--)
{
btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
m_dynamicsWorld->removeMultiBody(mb);
delete mb;
}
for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{ {
btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -1058,7 +1058,7 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
{ {
const UrdfCollision& col = link->m_collisionArray[v]; const UrdfCollision& col = link->m_collisionArray[v];
btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix); btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix);
//m_data->m_allocatedCollisionShapes.push_back(childShape); m_data->m_allocatedCollisionShapes.push_back(childShape);
if (childShape) if (childShape)
{ {

1
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -352,6 +352,7 @@ void ImportUrdfSetup::initPhysics()
btVector3 groundHalfExtents(20,20,20); btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f; groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents); btBoxShape* box = new btBoxShape(groundHalfExtents);
m_collisionShapes.push_back(box);
box->initializePolyhedralFeatures(); box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box); m_guiHelper->createCollisionShapeGraphicsObject(box);

49
src/BulletCollision/BroadphaseCollision/btDbvt.h
View File

@@ -122,6 +122,7 @@ subject to the following restrictions:
#error "DBVT_INT0_IMPL undefined" #error "DBVT_INT0_IMPL undefined"
#endif #endif
// //
// Defaults volumes // Defaults volumes
// //
@@ -188,6 +189,9 @@ struct btDbvtNode
}; };
}; };
typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack;
///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. ///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
@@ -325,6 +329,16 @@ struct btDbvt
void collideTV( const btDbvtNode* root, void collideTV( const btDbvtNode* root,
const btDbvtVolume& volume, const btDbvtVolume& volume,
DBVT_IPOLICY) const; DBVT_IPOLICY) const;
DBVT_PREFIX
void collideTVNoStackAlloc( const btDbvtNode* root,
const btDbvtVolume& volume,
btNodeStack& stack,
DBVT_IPOLICY) const;
///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc) ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
DBVT_PREFIX DBVT_PREFIX
@@ -917,7 +931,6 @@ inline void btDbvt::collideTT( const btDbvtNode* root0,
} }
#endif #endif
//
DBVT_PREFIX DBVT_PREFIX
inline void btDbvt::collideTV( const btDbvtNode* root, inline void btDbvt::collideTV( const btDbvtNode* root,
const btDbvtVolume& vol, const btDbvtVolume& vol,
@@ -950,6 +963,40 @@ inline void btDbvt::collideTV( const btDbvtNode* root,
} }
} }
//
DBVT_PREFIX
inline void btDbvt::collideTVNoStackAlloc( const btDbvtNode* root,
const btDbvtVolume& vol,
btNodeStack& stack,
DBVT_IPOLICY) const
{
DBVT_CHECKTYPE
if(root)
{
ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
stack.resize(0);
stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(root);
do {
const btDbvtNode* n=stack[stack.size()-1];
stack.pop_back();
if(Intersect(n->volume,volume))
{
if(n->isinternal())
{
stack.push_back(n->childs[0]);
stack.push_back(n->childs[1]);
}
else
{
policy.Process(n);
}
}
} while(stack.size()>0);
}
}
DBVT_PREFIX DBVT_PREFIX
inline void btDbvt::rayTestInternal( const btDbvtNode* root, inline void btDbvt::rayTestInternal( const btDbvtNode* root,
const btVector3& rayFrom, const btVector3& rayFrom,

6
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
View File

@@ -244,7 +244,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{ {
int i; int i;
btManifoldArray manifoldArray; manifoldArray.resize(0);
for (i=0;i<m_childCollisionAlgorithms.size();i++) for (i=0;i<m_childCollisionAlgorithms.size();i++)
{ {
if (m_childCollisionAlgorithms[i]) if (m_childCollisionAlgorithms[i])
@@ -274,7 +274,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
//process all children, that overlap with the given AABB bounds //process all children, that overlap with the given AABB bounds
tree->collideTV(tree->m_root,bounds,callback); tree->collideTVNoStackAlloc(tree->m_root,bounds,stack2,callback);
} else } else
{ {
@@ -291,7 +291,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
//iterate over all children, perform an AABB check inside ProcessChildShape //iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.size(); int numChildren = m_childCollisionAlgorithms.size();
int i; int i;
btManifoldArray manifoldArray; manifoldArray.resize(0);
const btCollisionShape* childShape = 0; const btCollisionShape* childShape = 0;
btTransform orgTrans; btTransform orgTrans;

4
src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
View File

@@ -26,6 +26,7 @@ class btDispatcher;
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "btCollisionCreateFunc.h" #include "btCollisionCreateFunc.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "BulletCollision/BroadPhaseCollision/btDbvt.h"
class btDispatcher; class btDispatcher;
class btCollisionObject; class btCollisionObject;
@@ -36,6 +37,9 @@ extern btShapePairCallback gCompoundChildShapePairCallback;
/// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes
class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm
{ {
btNodeStack stack2;
btManifoldArray manifoldArray;
protected: protected:
btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
bool m_isSwapped; bool m_isSwapped;

7
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -503,9 +503,11 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
worldVertsB1.resize(0);
btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
body0Wrap->getWorldTransform(), body0Wrap->getWorldTransform(),
body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut); body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2,
*resultOut);
} }
if (m_ownManifold) if (m_ownManifold)
@@ -568,8 +570,9 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
if (foundSepAxis) if (foundSepAxis)
{ {
worldVertsB2.resize(0);
btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut);
} }

3
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
View File

@@ -24,6 +24,7 @@ subject to the following restrictions:
#include "btCollisionCreateFunc.h" #include "btCollisionCreateFunc.h"
#include "btCollisionDispatcher.h" #include "btCollisionDispatcher.h"
#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil #include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil
#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
class btConvexPenetrationDepthSolver; class btConvexPenetrationDepthSolver;
@@ -45,6 +46,8 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
btSimplexSolverInterface* m_simplexSolver; btSimplexSolverInterface* m_simplexSolver;
btConvexPenetrationDepthSolver* m_pdSolver; btConvexPenetrationDepthSolver* m_pdSolver;
btVertexArray worldVertsB1;
btVertexArray worldVertsB2;
bool m_ownManifold; bool m_ownManifold;
btPersistentManifold* m_manifoldPtr; btPersistentManifold* m_manifoldPtr;

10
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp
View File

@@ -411,9 +411,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron&
return true; return true;
} }
void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{ {
btVertexArray worldVertsB2; worldVertsB2.resize(0);
btVertexArray* pVtxIn = &worldVertsB1; btVertexArray* pVtxIn = &worldVertsB1;
btVertexArray* pVtxOut = &worldVertsB2; btVertexArray* pVtxOut = &worldVertsB2;
pVtxOut->reserve(pVtxIn->size()); pVtxOut->reserve(pVtxIn->size());
@@ -527,7 +527,7 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin
void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut)
{ {
btVector3 separatingNormal = separatingNormal1.normalized(); btVector3 separatingNormal = separatingNormal1.normalized();
@@ -552,7 +552,7 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
} }
} }
} }
btVertexArray worldVertsB1; worldVertsB1.resize(0);
{ {
const btFace& polyB = hullB.m_faces[closestFaceB]; const btFace& polyB = hullB.m_faces[closestFaceB];
const int numVertices = polyB.m_indices.size(); const int numVertices = polyB.m_indices.size();
@@ -565,6 +565,6 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin
if (closestFaceB>=0) if (closestFaceB>=0)
clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut); clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, worldVertsB2,minDist, maxDist,resultOut);
} }

7
src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h
View File

@@ -32,8 +32,11 @@ typedef btAlignedObjectArray<btVector3> btVertexArray;
// Clips a face to the back of a plane // Clips a face to the back of a plane
struct btPolyhedralContactClipping struct btPolyhedralContactClipping
{ {
static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut);
static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); static void clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut);
static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut);
static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut); static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut);

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

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

10
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -36,6 +36,16 @@ protected:
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
//cached data to avoid memory allocations
btAlignedObjectArray<btQuaternion> m_scratch_world_to_local;
btAlignedObjectArray<btVector3> m_scratch_local_origin;
btAlignedObjectArray<btQuaternion> m_scratch_world_to_local1;
btAlignedObjectArray<btVector3> m_scratch_local_origin1;
btAlignedObjectArray<btScalar> m_scratch_r;
btAlignedObjectArray<btVector3> m_scratch_v;
btAlignedObjectArray<btMatrix3x3> m_scratch_m;
virtual void calculateSimulationIslands(); virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep); virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo); virtual void solveConstraints(btContactSolverInfo& solverInfo);

2
src/LinearMath/btAlignedAllocator.h
View File

@@ -21,7 +21,7 @@ subject to the following restrictions:
///that is better portable and more predictable ///that is better portable and more predictable
#include "btScalar.h" #include "btScalar.h"
//#define BT_DEBUG_MEMORY_ALLOCATIONS 1 ///#define BT_DEBUG_MEMORY_ALLOCATIONS 1
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS #ifdef BT_DEBUG_MEMORY_ALLOCATIONS
#define btAlignedAlloc(a,b) \ #define btAlignedAlloc(a,b) \