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

+ make compound versus soft body work (soft body uses interpolated transform)

Browse Source 
+ fixed issue with persistent manifold, warmstarting values were not initialized properly
+ don't clear manifold in sphere-sphere collision (need warmstarting)
+ added support for 'split impulse', decouple positional error correction from velocity correction
This avoids adding momentum due to penetration correction, it can be tuned using following variables:
solverInfo.m_splitImpulse = true/false (disable/enable)
solverInfo.m_splitImpulsePenetrationThreshold (below this value, baumgarte/mixed velocity/penetration is used (cheaper, looks more plausible)
solverInfo.m_linearSlop (less jitter, when small amound of penetration is allowed)
This commit is contained in:
erwin.coumans
2008-05-23 09:05:37 +00:00
parent ea86559480
commit 561066af75
13 changed files with 1611 additions and 1394 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
Demos/GimpactTestDemo/GimpactTestDemo.cpp
View File

@@ -40,6 +40,7 @@ subject to the following restrictions:
#ifdef SHOW_NUM_DEEP_PENETRATIONS
extern int gNumDeepPenetrationChecks;
extern int gNumSplitImpulseRecoveries;
extern int gNumGjkChecks;
#endif //
@@ -236,6 +237,14 @@ void GimpactConcaveDemo::renderme()
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"gNumSplitImpulseRecoveries= %d",gNumSplitImpulseRecoveries);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"gNumGjkChecks= %d",gNumGjkChecks);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);

5
Demos/OpenGL/DemoApplication.cpp
View File

@@ -41,6 +41,7 @@ btCollisionShape* gShapePtr[maxNumObjects];//1 rigidbody has 1 shape (no re-use
#ifdef SHOW_NUM_DEEP_PENETRATIONS
extern int gNumDeepPenetrationChecks;
extern int gNumSplitImpulseRecoveries;
extern int gNumGjkChecks;
extern int gNumAlignedAllocs;
extern int gNumAlignedFree;
@@ -1073,6 +1074,10 @@ void DemoApplication::renderme()
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"gNumSplitImpulseRecoveries= %d",gNumSplitImpulseRecoveries);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"gNumAlignedAllocs = %d",gNumAlignedAllocs);

79
Demos/SoftDemo/SoftDemo.cpp
View File

@@ -43,6 +43,14 @@ extern int glutScreenHeight;
const int maxProxies = 32766;
const int maxOverlap = 65535;
static btVector3* gGroundVertices=0;
static int* gGroundIndices=0;
static btBvhTriangleMeshShape* trimeshShape =0;
static btRigidBody* staticBody = 0;
static float waveheight = 5.f;
const float TRIANGLE_SIZE=8.f;
#ifdef _DEBUG
@@ -279,9 +287,10 @@ static void Ctor_RbUpStack(SoftDemo* pdemo,int count)
cylinderCompound->addChildShape(localTransform,cylinderShape);
btCollisionShape* shape[]={ new btSphereShape(1.5),
new btBoxShape(btVector3(1,1,1)),
cylinderCompound};
btCollisionShape* shape[]={ cylinderCompound,
new btSphereShape(1.5),
new btBoxShape(btVector3(1,1,1))
};
static const int nshapes=sizeof(shape)/sizeof(shape[0]);
for(int i=0;i<count;++i)
{
@@ -1169,9 +1178,71 @@ if(button==0)
void SoftDemo::initPhysics()
{
///create concave ground mesh
m_collisionShapes.push_back(new btBoxShape (btVector3(200,CUBE_HALF_EXTENTS,200)));
btCollisionShape* groundShape = 0;
bool useConcaveMesh = false;//not ready yet true;
if (useConcaveMesh)
{
int i;
int j;
const int NUM_VERTS_X = 30;
const int NUM_VERTS_Y = 30;
const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
gGroundVertices = new btVector3[totalVerts];
gGroundIndices = new int[totalTriangles*3];
btScalar offset(-50);
for ( i=0;i<NUM_VERTS_X;i++)
{
for (j=0;j<NUM_VERTS_Y;j++)
{
gGroundVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
//0.f,
waveheight*sinf((float)i)*cosf((float)j+offset),
(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
}
}
int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int);
int index=0;
for ( i=0;i<NUM_VERTS_X-1;i++)
{
for (int j=0;j<NUM_VERTS_Y-1;j++)
{
gGroundIndices[index++] = j*NUM_VERTS_X+i;
gGroundIndices[index++] = j*NUM_VERTS_X+i+1;
gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gGroundIndices[index++] = j*NUM_VERTS_X+i;
gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i;
}
}
btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
gGroundIndices,
indexStride,
totalVerts,(btScalar*) &gGroundVertices[0].x(),vertStride);
bool useQuantizedAabbCompression = true;
groundShape = new btBvhTriangleMeshShape(indexVertexArrays,useQuantizedAabbCompression);
} else
{
groundShape = new btBoxShape (btVector3(200,CUBE_HALF_EXTENTS,200));
}
m_collisionShapes.push_back(groundShape);
btCompoundShape* cylinderCompound = new btCompoundShape;
btCollisionShape* cylinderShape = new btCylinderShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));

1
Extras/COLLADA_DOM/include/dae/daeTypes.h
View File

@@ -19,6 +19,7 @@
#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
#pragma warning(disable:4786) // Disable the "debug name too long" warning
#pragma warning(disable:4244) // Disable the "possible loss of data" warning
#include <dae/daeWin32Platform.h>
#elif defined( __GCC__ )

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

@@ -77,17 +77,22 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt
//backup
btTransform orgTrans = colObj->getWorldTransform();
btTransform orgInterpolationTrans = colObj->getInterpolationWorldTransform();
btCollisionShape* orgShape = colObj->getCollisionShape();
const btTransform& childTrans = compoundShape->getChildTransform(i);
//btTransform newChildWorldTrans = orgTrans*childTrans ;
colObj->setWorldTransform( orgTrans*childTrans );
btTransform newChildWorldTrans = orgTrans*childTrans ;
colObj->setWorldTransform( newChildWorldTrans);
colObj->setInterpolationWorldTransform(newChildWorldTrans);
//the contactpoint is still projected back using the original inverted worldtrans
colObj->setCollisionShape( childShape );
m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut);
//revert back
colObj->setCollisionShape( orgShape);
colObj->setWorldTransform( orgTrans );
colObj->setInterpolationWorldTransform(orgInterpolationTrans);
}
}

2
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
View File

@@ -56,7 +56,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
btScalar radius0 = sphere0->getRadius();
btScalar radius1 = sphere1->getRadius();
m_manifoldPtr->clearManifold();
//m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1))

3
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
View File

@@ -183,7 +183,8 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
}
replaceContactPoint(newPoint,insertIndex);
btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
m_pointCache[insertIndex] = newPoint;
}
btScalar btPersistentManifold::getContactBreakingThreshold() const

2
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -138,7 +138,7 @@ public:
#define MAINTAIN_PERSISTENCY 1
#ifdef MAINTAIN_PERSISTENCY
int lifeTime = m_pointCache[insertIndex].getLifeTime();
btScalar appliedImpulse = 0.f;//m_pointCache[insertIndex].m_appliedImpulse;
btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
btAssert(lifeTime>=0);
void* cache = m_pointCache[insertIndex].m_userPersistentData;

14
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -26,7 +26,12 @@ struct btContactSolverInfoData
int m_numIterations;
btScalar m_maxErrorReduction;
btScalar m_sor;
btScalar m_erp;
btScalar m_erp;//used as Baumgarte factor
bool m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold;
btScalar m_linearSlop;
};
@@ -41,11 +46,12 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_restitution = btScalar(0.);
m_maxErrorReduction = btScalar(20.);
m_numIterations = 10;
m_erp = btScalar(0.4);
m_erp = btScalar(0.2);
m_sor = btScalar(1.3);
m_splitImpulse = true;
m_splitImpulsePenetrationThreshold = 1.f;
m_linearSlop = 0.0002f;
}
};
#endif //CONTACT_SOLVER_INFO

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

@@ -150,9 +150,11 @@ void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject
solverBody->m_originalBody = 0;
solverBody->m_angularFactor = 1.f;
}
solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
}
btScalar penetrationResolveFactor = btScalar(0.9);
int gNumSplitImpulseRecoveries = 0;
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
@@ -162,8 +164,65 @@ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
}
void resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo);
//SIMD_FORCE_INLINE
void resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo)
{
(void)solverInfo;
if (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold)
{
gNumSplitImpulseRecoveries++;
btScalar normalImpulse;
// Optimized version of projected relative velocity, use precomputed cross products with normal
// body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
// body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
// btVector3 vel = vel1 - vel2;
// btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
btScalar rel_vel;
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
rel_vel = vel1Dotn-vel2Dotn;
btScalar positionalError = contactConstraint.m_penetration;
// btScalar positionalError = contactConstraint.m_penetration;
btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
normalImpulse = penetrationImpulse+velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
btScalar sum = oldNormalImpulse + normalImpulse;
contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
}
}
//velocity + friction
@@ -185,55 +244,52 @@ btScalar resolveSingleCollisionCombinedCacheFriendly(
(void)solverInfo;
btScalar normalImpulse;
bool useSplitImpulse = false;
// Optimized version of projected relative velocity, use precomputed cross products with normal
// body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
// body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
// btVector3 vel = vel1 - vel2;
// btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
btScalar rel_vel;
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
rel_vel = vel1Dotn-vel2Dotn;
btScalar positionalError = contactConstraint.m_penetration;
btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
normalImpulse = penetrationImpulse+velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
btScalar sum = oldNormalImpulse + normalImpulse;
contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
if (body1.m_invMass)
{
// Optimized version of projected relative velocity, use precomputed cross products with normal
// body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
// body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
// btVector3 vel = vel1 - vel2;
// btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
btScalar rel_vel;
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
rel_vel = vel1Dotn-vel2Dotn;
btScalar positionalError = 0.f;
if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration<solverInfo.m_splitImpulsePenetrationThreshold))
{
positionalError = contactConstraint.m_penetration;
}
btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
normalImpulse = penetrationImpulse+velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
btScalar sum = oldNormalImpulse + normalImpulse;
contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
contactConstraint.m_angularComponentA,normalImpulse);
}
if (body2.m_invMass)
{
contactConstraint.m_angularComponentA,normalImpulse);
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
contactConstraint.m_angularComponentB,-normalImpulse);
contactConstraint.m_angularComponentB,-normalImpulse);
}
return normalImpulse;
}
@@ -311,14 +367,9 @@ btScalar resolveSingleFrictionCacheFriendly(
}
if (body1.m_invMass)
{
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
}
if (body2.m_invMass)
{
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
}
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
}
return 0.f;
@@ -360,7 +411,6 @@ btScalar resolveSingleFrictionCacheFriendly(
const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
//if (contactConstraint.m_appliedVelocityImpulse > 0.f)
if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
{
lat_rel_vel = btSqrt(lat_rel_vel);
@@ -370,7 +420,7 @@ btScalar resolveSingleFrictionCacheFriendly(
btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
btScalar friction_impulse = lat_rel_vel /
(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction;
btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
GEN_set_min(friction_impulse, normal_impulse);
GEN_set_max(friction_impulse, -normal_impulse);
@@ -406,7 +456,7 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3&
solverConstraint.m_originalContactPoint = 0;
solverConstraint.m_appliedImpulse = btScalar(0.);
solverConstraint.m_appliedVelocityImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
solverConstraint.m_penetration = 0.f;
{
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
@@ -669,8 +719,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
rel_vel = cp.m_normalWorldOnB.dot(vel);
solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,0.f);
//solverConstraint.m_penetration = cp.getDistance();
solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (solverConstraint.m_restitution <= btScalar(0.))
@@ -692,17 +743,18 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse;
if (rb0)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,cp.m_appliedImpulse);
if (rb1)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-cp.m_appliedImpulse);
} else
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedVelocityImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
}
{
btVector3 frictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = frictionDir1.length2();
@@ -844,35 +896,41 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
for (j=0;j<numFrictionPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;
resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse);
totalImpulse);
}
}
}
}
{
int numPoolConstraints = m_tmpSolverConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
if (infoGlobal.m_splitImpulse)
{
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
{
int numPoolConstraints = m_tmpSolverConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
//do a callback here?
resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
}
}
}
}
}
return 0.f;
return 0.f;
}
@@ -883,12 +941,32 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
int numPoolConstraints = m_tmpSolverConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
m_tmpSolverBodyPool[i].writebackVelocity();
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
//do a callback here?
}
if (infoGlobal.m_splitImpulse)
{
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
{
m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
}
} else
{
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
{
m_tmpSolverBodyPool[i].writebackVelocity();
}
}
// printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
@@ -1312,3 +1390,4 @@ void btSequentialImpulseConstraintSolver::reset()
m_btSeed2 = 0;
}

41
src/BulletDynamics/ConstraintSolver/btSolverBody.h
View File

@@ -21,6 +21,7 @@ class btRigidBody;
#include "LinearMath/btMatrix3x3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.h"
///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
@@ -36,6 +37,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
btVector3 m_linearVelocity;
btVector3 m_centerOfMassPosition;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
{
velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
@@ -44,16 +49,47 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
m_linearVelocity += linearComponent*impulseMagnitude;
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
if (m_invMass)
{
m_linearVelocity += linearComponent*impulseMagnitude;
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
if (m_invMass)
{
m_pushVelocity += linearComponent*impulseMagnitude;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
void writebackVelocity()
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
void writebackVelocity(btScalar timeStep)
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity);
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
m_originalBody->setWorldTransform(newTransform);
//m_originalBody->setCompanionId(-1);
}
}
@@ -74,3 +110,4 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
#endif //BT_SOLVER_BODY_H

5
src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
View File

@@ -36,7 +36,9 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
btVector3 m_angularComponentA;
btVector3 m_angularComponentB;
mutable btScalar m_appliedVelocityImpulse;
mutable btScalar m_appliedPushImpulse;
mutable btScalar m_appliedImpulse;
int m_solverBodyIdA;
int m_solverBodyIdB;
@@ -69,3 +71,4 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
#endif //BT_SOLVER_CONSTRAINT_H

2
src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp
View File

@@ -40,7 +40,7 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura
m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc;
m_swappedSoftRigidConcaveCreateFunc->m_swapped=true;
#endif