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 "LinearMath/btStackAlloc.h"
#include "LinearMath/btQuickprof.h"
#include "btSolverBody.h"
#include "btSolverConstraint.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "LinearMath/btAlignedObjectArray.h"
#ifdef USE_PROFILE
@@ -137,7 +138,6 @@ void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
solverBody->m_invMass = rigidbody->getInvMass();
solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
solverBody->m_originalBody = rigidbody;
btAssert(rigidbody);
solverBody->m_angularFactor = rigidbody->getAngularFactor();
}
@@ -333,10 +333,12 @@ btScalar resolveSingleFrictionCacheFriendly(
#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,btDispatcher* dispatcher)
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
(void)stackAlloc;
(void)debugDrawer;
@@ -349,35 +351,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
BEGIN_PROFILE("refreshManifolds");
int numActiveBodies = 0;
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++)
{
btPersistentManifold* manifold = manifoldPtr[i];
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
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());
totalContacts += manifold->getNumContacts();
numActiveManifolds++;
}
}
}
END_PROFILE("refreshManifolds");
@@ -385,55 +367,24 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
BEGIN_PROFILE("gatherSolverData");
btBlock* sablock;
sablock = stackAlloc->beginBlock();
//int sizeofSB = sizeof(btSolverBody);
//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 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
int minReservation = numManifolds*2;
tmpSolverBodyPool.reserve(minReservation);
{
for (int i=0;i<numBodies;i++)
@@ -442,8 +393,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
if (rb && (rb->getIslandTag() >= 0))
{
btAssert(rb->getCompanionId() < 0);
int solverBodyId = tmpSolverBodyPoolSize;
btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++];
int solverBodyId = tmpSolverBodyPool.size();
btSolverBody& solverBody = tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb);
rb->setCompanionId(solverBodyId);
}
@@ -451,6 +402,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
}
tmpSolverConstraintPool.reserve(minReservation);
tmpSolverFrictionConstraintPool.reserve(minReservation);
{
int i;
@@ -460,12 +413,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
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 solverBodyIdB=-1;
@@ -481,8 +428,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} else
{
//create a static body
solverBodyIdA = tmpSolverBodyPoolSize;
btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++];
solverBodyIdA = tmpSolverBodyPool.size();
btSolverBody& solverBody = tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb0);
}
@@ -492,8 +439,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
} else
{
//create a static body
solverBodyIdB = tmpSolverBodyPoolSize;
btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++];
solverBodyIdB = tmpSolverBodyPool.size();
btSolverBody& solverBody = tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb1);
}
}
@@ -503,7 +450,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btManifoldPoint& cp = manifold->getContactPoint(j);
int frictionIndex = tmpSolverConstraintPoolSize;
int frictionIndex = tmpSolverConstraintPool.size();
if (cp.getDistance() <= btScalar(0.))
{
@@ -518,7 +465,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btScalar relaxation = 1.f;
{
btSolverConstraint& solverConstraint = tmpSolverConstraintPool[tmpSolverConstraintPoolSize++];
btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -582,7 +529,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b);
{
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool[tmpSolverFrictionConstraintPoolSize++];
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
solverConstraint.m_contactNormal = frictionTangential0a;
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_solverBodyIdA = solverBodyIdA;
@@ -649,9 +596,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
}
}
}
}
}
}
END_PROFILE("gatherSolverData");
@@ -668,40 +612,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
}
}
int numConstraintPool = tmpSolverConstraintPoolSize;
int numFrictionPool = tmpSolverFrictionConstraintPoolSize;
btAlignedObjectArray<int> gOrderTmpConstraintPool;
btAlignedObjectArray<int> gOrderFrictionConstraintPool;
///use the stack allocator for temporarily memory
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];
}
int numConstraintPool = tmpSolverConstraintPool.size();
int numFrictionPool = tmpSolverFrictionConstraintPool.size();
///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
gOrderTmpConstraintPool.resize(numConstraintPool);
gOrderFrictionConstraintPool.resize(numFrictionPool);
{
int 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++)
{
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++)
{
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();
}
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;
}
@@ -826,7 +759,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
btAssert(bodies);
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;
#include "btContactConstraint.h"
#include "btSolverBody.h"
#include "btSolverConstraint.h"
/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
@@ -31,14 +29,6 @@ class btIDebugDraw;
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:
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);
@@ -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 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
virtual void reset();