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

revert to previous version of constraint solver, until the memory allocation issues are sorted properly.

Browse Source
This commit is contained in:
ejcoumans
2007-09-10 19:19:52 +00:00
parent cfde010808
commit 15df0a7d7a
2 changed files with 225 additions and 302 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,8 +27,9 @@ subject to the following restrictions:
#include <new> #include <new>
#include "LinearMath/btStackAlloc.h" #include "LinearMath/btStackAlloc.h"
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
#include "btSolverBody.h"
#include "btSolverConstraint.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#ifdef USE_PROFILE #ifdef USE_PROFILE
@@ -137,7 +138,6 @@ void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
solverBody->m_invMass = rigidbody->getInvMass(); solverBody->m_invMass = rigidbody->getInvMass();
solverBody->m_linearVelocity = rigidbody->getLinearVelocity(); solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
solverBody->m_originalBody = rigidbody; solverBody->m_originalBody = rigidbody;
btAssert(rigidbody);
solverBody->m_angularFactor = rigidbody->getAngularFactor(); solverBody->m_angularFactor = rigidbody->getAngularFactor();
} }
@@ -333,10 +333,12 @@ btScalar resolveSingleFrictionCacheFriendly(
#endif //NO_FRICTION_TANGENTIALS #endif //NO_FRICTION_TANGENTIALS
btAlignedObjectArray<btSolverBody> tmpSolverBodyPool;
btAlignedObjectArray<btSolverConstraint> tmpSolverConstraintPool;
btAlignedObjectArray<btSolverConstraint> tmpSolverFrictionConstraintPool;
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
{ {
(void)stackAlloc; (void)stackAlloc;
(void)debugDrawer; (void)debugDrawer;
@@ -349,35 +351,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
BEGIN_PROFILE("refreshManifolds"); BEGIN_PROFILE("refreshManifolds");
int numActiveBodies = 0;
int i; int i;
for (i=0;i<numBodies;i++)
{
btRigidBody* rb = btRigidBody::upcast(bodies[i]);
if (rb && (rb->getIslandTag() >= 0))
{
numActiveBodies++;
}
}
int numActiveManifolds = 0;
int totalContacts = 0;
for (i=0;i<numManifolds;i++) for (i=0;i<numManifolds;i++)
{ {
btPersistentManifold* manifold = manifoldPtr[i]; btPersistentManifold* manifold = manifoldPtr[i];
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0(); btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
if (((rb0) && rb0->getActivationState() != ISLAND_SLEEPING) ||
((rb1) && rb1->getActivationState() != ISLAND_SLEEPING))
{
if (dispatcher->needsResponse(rb0,rb1))
{
manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
totalContacts += manifold->getNumContacts();
numActiveManifolds++;
}
}
} }
END_PROFILE("refreshManifolds"); END_PROFILE("refreshManifolds");
@@ -385,55 +367,24 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
BEGIN_PROFILE("gatherSolverData"); BEGIN_PROFILE("gatherSolverData");
btBlock* sablock; //int sizeofSB = sizeof(btSolverBody);
sablock = stackAlloc->beginBlock(); //int sizeofSC = sizeof(btSolverConstraint);
int tmpSolverBodyPoolSize = 0;
int memNeeded = sizeof(btSolverBody) * numActiveBodies*2;
btSolverBody* tmpSolverBodyPool = 0;
if (memNeeded < stackAlloc->getAvailableMemory())
{
tmpSolverBodyPool = (btSolverBody*) stackAlloc->allocate(memNeeded);
} else
{
m_solverBodyPool.resize(numActiveBodies*2);
tmpSolverBodyPool = &m_solverBodyPool[0];
}
int tmpSolverConstraintPoolSize = 0;
int mem2Needed = sizeof(btSolverConstraint)*totalContacts;
btSolverConstraint* tmpSolverConstraintPool = 0;
if (mem2Needed < stackAlloc->getAvailableMemory())
{
tmpSolverConstraintPool = (btSolverConstraint*) stackAlloc->allocate(sizeof(btSolverConstraint)*totalContacts);
} else
{
m_solverConstraintPool.resize(totalContacts);
tmpSolverConstraintPool = &m_solverConstraintPool[0];
}
int tmpSolverFrictionConstraintPoolSize = 0;
int mem3Needed = sizeof(btSolverConstraint)*totalContacts*2;
btSolverConstraint* tmpSolverFrictionConstraintPool = 0;
if (mem3Needed < stackAlloc->getAvailableMemory())
{
tmpSolverFrictionConstraintPool = (btSolverConstraint*) stackAlloc->allocate(mem3Needed);
} else
{
m_solverFrictionConstraintPool.resize(totalContacts*2);
tmpSolverFrictionConstraintPool = &m_solverFrictionConstraintPool[0];
}
//if (1) //if (1)
{ {
//if m_stackAlloc, try to pack bodies/constraints to speed up solving
// btBlock* sablock;
// sablock = stackAlloc->beginBlock();
// int memsize = 16;
// unsigned char* stackMemory = stackAlloc->allocate(memsize);
//todo: use stack allocator for this temp memory //todo: use stack allocator for this temp memory
int minReservation = numManifolds*2; int minReservation = numManifolds*2;
tmpSolverBodyPool.reserve(minReservation);
{ {
for (int i=0;i<numBodies;i++) for (int i=0;i<numBodies;i++)
@@ -442,8 +393,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
if (rb && (rb->getIslandTag() >= 0)) if (rb && (rb->getIslandTag() >= 0))
{ {
btAssert(rb->getCompanionId() < 0); btAssert(rb->getCompanionId() < 0);
int solverBodyId = tmpSolverBodyPoolSize; int solverBodyId = tmpSolverBodyPool.size();
btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++]; btSolverBody& solverBody = tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb); initSolverBody(&solverBody,rb);
rb->setCompanionId(solverBodyId); rb->setCompanionId(solverBodyId);
} }
@@ -451,6 +402,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} }
tmpSolverConstraintPool.reserve(minReservation);
tmpSolverFrictionConstraintPool.reserve(minReservation);
{ {
int i; int i;
@@ -460,12 +413,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0(); btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
if (((rb0) && rb0->getActivationState() != ISLAND_SLEEPING) ||
((rb1) && rb1->getActivationState() != ISLAND_SLEEPING))
{
if (dispatcher->needsResponse(rb0,rb1))
{
int solverBodyIdA=-1; int solverBodyIdA=-1;
int solverBodyIdB=-1; int solverBodyIdB=-1;
@@ -481,8 +428,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} else } else
{ {
//create a static body //create a static body
solverBodyIdA = tmpSolverBodyPoolSize; solverBodyIdA = tmpSolverBodyPool.size();
btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++]; btSolverBody& solverBody = tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb0); initSolverBody(&solverBody,rb0);
} }
@@ -492,8 +439,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} else } else
{ {
//create a static body //create a static body
solverBodyIdB = tmpSolverBodyPoolSize; solverBodyIdB = tmpSolverBodyPool.size();
btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++]; btSolverBody& solverBody = tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb1); initSolverBody(&solverBody,rb1);
} }
} }
@@ -503,7 +450,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btManifoldPoint& cp = manifold->getContactPoint(j); btManifoldPoint& cp = manifold->getContactPoint(j);
int frictionIndex = tmpSolverConstraintPoolSize; int frictionIndex = tmpSolverConstraintPool.size();
if (cp.getDistance() <= btScalar(0.)) if (cp.getDistance() <= btScalar(0.))
{ {
@@ -518,7 +465,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btScalar relaxation = 1.f; btScalar relaxation = 1.f;
{ {
btSolverConstraint& solverConstraint = tmpSolverConstraintPool[tmpSolverConstraintPoolSize++]; btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -582,7 +529,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b); btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b);
{ {
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool[tmpSolverFrictionConstraintPoolSize++]; btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
solverConstraint.m_contactNormal = frictionTangential0a; solverConstraint.m_contactNormal = frictionTangential0a;
solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdA = solverBodyIdA;
@@ -616,7 +563,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
{ {
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool[tmpSolverFrictionConstraintPoolSize++]; btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
solverConstraint.m_contactNormal = frictionTangential1b; solverConstraint.m_contactNormal = frictionTangential1b;
solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdA = solverBodyIdA;
@@ -649,9 +596,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} }
} }
} }
}
}
} }
END_PROFILE("gatherSolverData"); END_PROFILE("gatherSolverData");
@@ -668,40 +612,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} }
} }
int numConstraintPool = tmpSolverConstraintPoolSize; btAlignedObjectArray<int> gOrderTmpConstraintPool;
int numFrictionPool = tmpSolverFrictionConstraintPoolSize; btAlignedObjectArray<int> gOrderFrictionConstraintPool;
///use the stack allocator for temporarily memory int numConstraintPool = tmpSolverConstraintPool.size();
int numFrictionPool = tmpSolverFrictionConstraintPool.size();
int gOrderTmpConstraintPoolSize = numConstraintPool;
int mem4Needed = sizeof(int)*numConstraintPool;
int* gOrderTmpConstraintPool = 0;
if (mem4Needed < stackAlloc->getAvailableMemory())
{
gOrderTmpConstraintPool = (int*) stackAlloc->allocate(mem4Needed);
} else
{
m_constraintOrder.resize(numConstraintPool);
gOrderTmpConstraintPool = &m_constraintOrder[0];
}
int gOrderFrictionConstraintPoolSize = numFrictionPool;
int mem5Needed = sizeof(int)*numFrictionPool;
int* gOrderFrictionConstraintPool =0;
if (mem5Needed < stackAlloc->getAvailableMemory())
{
gOrderFrictionConstraintPool = (int*) stackAlloc->allocate(mem5Needed);
}
else
{
m_frictionConstraintOrder.resize(numFrictionPool);
gOrderFrictionConstraintPool = &m_frictionConstraintOrder[0];
}
///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
gOrderTmpConstraintPool.resize(numConstraintPool);
gOrderFrictionConstraintPool.resize(numFrictionPool);
{ {
int i; int i;
for (i=0;i<numConstraintPool;i++) for (i=0;i<numConstraintPool;i++)
@@ -776,7 +695,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} }
{ {
int numPoolConstraints = tmpSolverConstraintPoolSize; int numPoolConstraints = tmpSolverConstraintPool.size();
for (j=0;j<numPoolConstraints;j++) for (j=0;j<numPoolConstraints;j++)
{ {
btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
@@ -786,7 +705,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} }
{ {
int numFrictionPoolConstraints = tmpSolverFrictionConstraintPoolSize; int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++) for (j=0;j<numFrictionPoolConstraints;j++)
{ {
btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
@@ -802,16 +721,30 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} }
} }
for ( i=0;i<tmpSolverBodyPoolSize;i++) for ( i=0;i<tmpSolverBodyPool.size();i++)
{ {
tmpSolverBodyPool[i].writebackVelocity(); tmpSolverBodyPool[i].writebackVelocity();
} }
END_PROFILE("solveConstraints"); END_PROFILE("solveConstraints");
///release stack memory // printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
/*
printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size());
printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size());
printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity());
printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity());
printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity());
*/
tmpSolverBodyPool.resize(0);
tmpSolverConstraintPool.resize(0);
tmpSolverFrictionConstraintPool.resize(0);
stackAlloc->endBlock(sablock);
return 0.f; return 0.f;
} }
@@ -826,7 +759,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
btAssert(bodies); btAssert(bodies);
btAssert(numBodies); btAssert(numBodies);
return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc,dispatcher); return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
} }

12
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -20,8 +20,6 @@ subject to the following restrictions:
class btIDebugDraw; class btIDebugDraw;
#include "btContactConstraint.h" #include "btContactConstraint.h"
#include "btSolverBody.h"
#include "btSolverConstraint.h"
/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses /// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
@@ -31,14 +29,6 @@ class btIDebugDraw;
class btSequentialImpulseConstraintSolver : public btConstraintSolver class btSequentialImpulseConstraintSolver : public btConstraintSolver
{ {
btAlignedObjectArray<btSolverBody> m_solverBodyPool;
btAlignedObjectArray<btSolverConstraint> m_solverConstraintPool;
btAlignedObjectArray<btSolverConstraint> m_solverFrictionConstraintPool;
btAlignedObjectArray<int> m_constraintOrder;
btAlignedObjectArray<int> m_frictionConstraintOrder;
protected: protected:
btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
@@ -82,7 +72,7 @@ public:
virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher); virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
///clear internal cached data and reset random seed ///clear internal cached data and reset random seed
virtual void reset(); virtual void reset();