From e2bdd7dbb11b292b5f9fa1d8967993d13d2976fb Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 16 Jul 2016 17:40:44 -0700 Subject: [PATCH] fix more memory leaks, ImportURDFExample is now leak-free eliminate all run-time memory allocation (except for mouse-pick/ray-intersection) in ImportURDFExample --- .../CommonInterfaces/CommonMultiBodyBase.h | 16 +++- .../ImportURDFDemo/BulletUrdfImporter.cpp | 2 +- .../ImportURDFDemo/ImportURDFSetup.cpp | 1 + .../BroadphaseCollision/btDbvt.h | 93 ++++++++++++++----- .../btCompoundCollisionAlgorithm.cpp | 6 +- .../btCompoundCollisionAlgorithm.h | 4 + .../btConvexConvexAlgorithm.cpp | 7 +- .../btConvexConvexAlgorithm.h | 3 + .../btPolyhedralContactClipping.cpp | 10 +- .../btPolyhedralContactClipping.h | 7 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 55 +++++------ .../Featherstone/btMultiBodyDynamicsWorld.h | 10 ++ src/LinearMath/btAlignedAllocator.h | 2 +- 13 files changed, 146 insertions(+), 70 deletions(-) diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h index 3219f8294..48130b2d1 100644 --- a/examples/CommonInterfaces/CommonMultiBodyBase.h +++ b/examples/CommonInterfaces/CommonMultiBodyBase.h @@ -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) m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); - m_broadphase = new btDbvtBroadphase(); + m_broadphase = new btDbvtBroadphase();//btSimpleBroadphase(); m_solver = new btMultiBodyConstraintSolver; @@ -97,6 +97,20 @@ struct CommonMultiBodyBase : public CommonExampleInterface { 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--) { btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index c1ae57c4d..628d99bc2 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1058,7 +1058,7 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index) { const UrdfCollision& col = link->m_collisionArray[v]; btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix); - //m_data->m_allocatedCollisionShapes.push_back(childShape); + m_data->m_allocatedCollisionShapes.push_back(childShape); if (childShape) { diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index ebe8334a5..46b69cc08 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -352,6 +352,7 @@ void ImportUrdfSetup::initPhysics() btVector3 groundHalfExtents(20,20,20); groundHalfExtents[upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); + m_collisionShapes.push_back(box); box->initializePolyhedralFeatures(); m_guiHelper->createCollisionShapeGraphicsObject(box); diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.h b/src/BulletCollision/BroadphaseCollision/btDbvt.h index db4e482f2..bee17e5c8 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -122,6 +122,7 @@ subject to the following restrictions: #error "DBVT_INT0_IMPL undefined" #endif + // // Defaults volumes // @@ -188,6 +189,9 @@ struct btDbvtNode }; }; +typedef btAlignedObjectArray btNodeStack; + + ///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. ///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, const btDbvtVolume& volume, 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 slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time DBVT_PREFIX @@ -917,39 +931,72 @@ inline void btDbvt::collideTT( const btDbvtNode* root0, } #endif -// DBVT_PREFIX inline void btDbvt::collideTV( const btDbvtNode* root, const btDbvtVolume& vol, DBVT_IPOLICY) const { DBVT_CHECKTYPE - if(root) - { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray stack; - 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(root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + btAlignedObjectArray stack; + 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()) { - if(n->isinternal()) - { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); - } - else - { - policy.Process(n); - } + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); } - } while(stack.size()>0); - } + else + { + policy.Process(n); + } + } + } while(stack.size()>0); + } } +// +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 inline void btDbvt::rayTestInternal( const btDbvtNode* root, const btVector3& rayFrom, diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index a80c438d1..13cddc11a 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -244,7 +244,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; - btManifoldArray manifoldArray; + manifoldArray.resize(0); for (i=0;icollideTV(tree->m_root,bounds,callback); + tree->collideTVNoStackAlloc(tree->m_root,bounds,stack2,callback); } else { @@ -291,7 +291,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.size(); int i; - btManifoldArray manifoldArray; + manifoldArray.resize(0); const btCollisionShape* childShape = 0; btTransform orgTrans; diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 7d792c18d..ea469e7ca 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -26,6 +26,7 @@ class btDispatcher; #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" #include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/BroadPhaseCollision/btDbvt.h" class btDispatcher; class btCollisionObject; @@ -36,6 +37,9 @@ extern btShapePairCallback gCompoundChildShapePairCallback; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm { + btNodeStack stack2; + btManifoldArray manifoldArray; + protected: btAlignedObjectArray m_childCollisionAlgorithms; bool m_isSwapped; diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 7f2722aa4..b9cc04186 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -503,9 +503,11 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + worldVertsB1.resize(0); btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut); + body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2, + *resultOut); } if (m_ownManifold) @@ -568,8 +570,9 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* if (foundSepAxis) { + worldVertsB2.resize(0); btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); + body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut); } diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index 51db0c654..d0ff3b3c1 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -24,6 +24,7 @@ subject to the following restrictions: #include "btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" #include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" class btConvexPenetrationDepthSolver; @@ -45,6 +46,8 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm btSimplexSolverInterface* m_simplexSolver; btConvexPenetrationDepthSolver* m_pdSolver; + btVertexArray worldVertsB1; + btVertexArray worldVertsB2; bool m_ownManifold; btPersistentManifold* m_manifoldPtr; diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index d5f4a964b..ea380bc5f 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -411,9 +411,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& 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* pVtxOut = &worldVertsB2; 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(); @@ -552,7 +552,7 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin } } } - btVertexArray worldVertsB1; + worldVertsB1.resize(0); { const btFace& polyB = hullB.m_faces[closestFaceB]; const int numVertices = polyB.m_indices.size(); @@ -565,6 +565,6 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin if (closestFaceB>=0) - clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut); + clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, worldVertsB2,minDist, maxDist,resultOut); } diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h index b87bd4f32..30e3db687 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h @@ -32,8 +32,11 @@ typedef btAlignedObjectArray btVertexArray; // Clips a face to the back of a plane 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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index a9c0b33b3..d94d1d4ea 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -396,22 +396,17 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () void btMultiBodyDynamicsWorld::forwardKinematics() { - btAlignedObjectArray world_to_local; - btAlignedObjectArray local_origin; for (int b=0;bforwardKinematics(world_to_local,local_origin); + bod->forwardKinematics(m_scratch_world_to_local,m_scratch_local_origin); } } void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { forwardKinematics(); - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray 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 world_to_local; - btAlignedObjectArray local_origin; - + for (int b=0;bupdateCollisionObjectWorldTransforms(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 world_to_local1; - btAlignedObjectArray local_origin1; for (int c=0;cforwardKinematics(world_to_local1,local_origin1); + bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1); getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 03ef3335c..2c912da5c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -36,6 +36,16 @@ protected: btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + //cached data to avoid memory allocations + btAlignedObjectArray m_scratch_world_to_local; + btAlignedObjectArray m_scratch_local_origin; + btAlignedObjectArray m_scratch_world_to_local1; + btAlignedObjectArray m_scratch_local_origin1; + btAlignedObjectArray m_scratch_r; + btAlignedObjectArray m_scratch_v; + btAlignedObjectArray m_scratch_m; + + virtual void calculateSimulationIslands(); virtual void updateActivationState(btScalar timeStep); virtual void solveConstraints(btContactSolverInfo& solverInfo); diff --git a/src/LinearMath/btAlignedAllocator.h b/src/LinearMath/btAlignedAllocator.h index f168f3c66..3cc6140de 100644 --- a/src/LinearMath/btAlignedAllocator.h +++ b/src/LinearMath/btAlignedAllocator.h @@ -21,7 +21,7 @@ subject to the following restrictions: ///that is better portable and more predictable #include "btScalar.h" -//#define BT_DEBUG_MEMORY_ALLOCATIONS 1 +///#define BT_DEBUG_MEMORY_ALLOCATIONS 1 #ifdef BT_DEBUG_MEMORY_ALLOCATIONS #define btAlignedAlloc(a,b) \