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

Bugfix: allow btCollisionObjects (non-btRigidBody) to interact properly with btRigidBody for cache-friendly btSequentialImpulseConstraintSolver.

Browse Source 
Thanks Andy O'Neil for pointing this out.
This commit is contained in:
ejcoumans
2007-11-28 03:17:09 +00:00
parent 5c1609fb23
commit e252f5cadd
4 changed files with 100 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -130,23 +130,28 @@ btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
}
void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
{
/* int size = sizeof(btSolverBody);
int sizeofrb = sizeof(btRigidBody);
int sizemanifold = sizeof(btPersistentManifold);
int sizeofmp = sizeof(btManifoldPoint);
int sizeofPersistData = sizeof (btConstraintPersistentData);
*/
solverBody->m_angularVelocity = rigidbody->getAngularVelocity();
solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition();
solverBody->m_friction = rigidbody->getFriction();
// solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld();
solverBody->m_invMass = rigidbody->getInvMass();
solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
solverBody->m_originalBody = rigidbody;
solverBody->m_angularFactor = rigidbody->getAngularFactor();
btRigidBody* rb = btRigidBody::upcast(collisionObject);
if (rb)
{
solverBody->m_angularVelocity = rb->getAngularVelocity() ;
solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
solverBody->m_friction = collisionObject->getFriction();
solverBody->m_invMass = rb->getInvMass();
solverBody->m_linearVelocity = rb->getLinearVelocity();
solverBody->m_originalBody = rb;
solverBody->m_angularFactor = rb->getAngularFactor();
} else
{
solverBody->m_angularVelocity.setValue(0,0,0);
solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
solverBody->m_friction = collisionObject->getFriction();
solverBody->m_invMass = 0.f;
solverBody->m_linearVelocity.setValue(0,0,0);
solverBody->m_originalBody = 0;
solverBody->m_angularFactor = 1.f;
}
}
btScalar penetrationResolveFactor = btScalar(0.9);
@@ -369,9 +374,12 @@ btScalar resolveSingleFrictionCacheFriendly(
void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btRigidBody* rb0,btRigidBody* rb1, btScalar relaxation)
void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
{
btRigidBody* body0=btRigidBody::upcast(colObj0);
btRigidBody* body1=btRigidBody::upcast(colObj1);
btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
solverConstraint.m_contactNormal = normalAxis;
@@ -388,12 +396,12 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3&
{
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1;
solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
}
{
btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1;
solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
}
#ifdef COMPUTE_IMPULSE_DENOM
@@ -401,19 +409,29 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3&
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
btVector3 vec;
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
btScalar denom0 = rb0->getInvMass() + normalAxis.dot(vec);
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
btScalar denom1 = rb1->getInvMass() + normalAxis.dot(vec);
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
if (body0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = body0->getInvMass() + normalAxis.dot(vec);
}
if (body1)
{
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = body1->getInvMass() + normalAxis.dot(vec);
}
#endif //COMPUTE_IMPULSE_DENOM
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
}
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
PROFILE("solveGroupCacheFriendlySetup");
@@ -426,7 +444,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
return 0.f;
}
btPersistentManifold* manifold = 0;
btRigidBody* rb0=0,*rb1=0;
btCollisionObject* colObj0=0,*colObj1=0;
//btRigidBody* rb0=0,*rb1=0;
#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
@@ -500,10 +521,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
for (i=0;i<numManifolds;i++)
{
manifold = manifoldPtr[i];
rb1 = (btRigidBody*)manifold->getBody1();
rb0 = (btRigidBody*)manifold->getBody0();
colObj0 = (btCollisionObject*)manifold->getBody0();
colObj1 = (btCollisionObject*)manifold->getBody1();
int solverBodyIdA=-1;
int solverBodyIdB=-1;
@@ -513,45 +532,45 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (rb0->getIslandTag() >= 0)
if (colObj0->getIslandTag() >= 0)
{
if (rb0->getCompanionId() >= 0)
if (colObj0->getCompanionId() >= 0)
{
//body has already been converted
solverBodyIdA = rb0->getCompanionId();
solverBodyIdA = colObj0->getCompanionId();
} else
{
solverBodyIdA = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb0);
rb0->setCompanionId(solverBodyIdA);
initSolverBody(&solverBody,colObj0);
colObj0->setCompanionId(solverBodyIdA);
}
} else
{
//create a static body
solverBodyIdA = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb0);
initSolverBody(&solverBody,colObj0);
}
if (rb1->getIslandTag() >= 0)
if (colObj1->getIslandTag() >= 0)
{
if (rb1->getCompanionId() >= 0)
if (colObj1->getCompanionId() >= 0)
{
solverBodyIdB = rb1->getCompanionId();
solverBodyIdB = colObj1->getCompanionId();
} else
{
solverBodyIdB = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb1);
rb1->setCompanionId(solverBodyIdB);
initSolverBody(&solverBody,colObj1);
colObj1->setCompanionId(solverBodyIdB);
}
} else
{
//create a static body
solverBodyIdB = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,rb1);
initSolverBody(&solverBody,colObj1);
}
}
@@ -574,8 +593,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB();
rel_pos1 = pos1 - rb0->getCenterOfMassPosition();
rel_pos2 = pos2 - rb1->getCenterOfMassPosition();
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
relaxation = 1.f;
@@ -586,25 +605,35 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
{
btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0;
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1;
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
{
#ifdef COMPUTE_IMPULSE_DENOM
btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
btVector3 vec;
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
btScalar denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
btScalar denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
if (rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
}
if (rb1)
{
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
}
#endif //COMPUTE_IMPULSE_DENOM
btScalar denom = relaxation/(denom0+denom1);
@@ -616,8 +645,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2);
btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
vel = vel1 - vel2;
@@ -649,27 +678,29 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}
{
btVector3 frictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = frictionDir1.length2();
if (lat_rel_vel > SIMD_EPSILON)//0.0f)
{
frictionDir1 /= btSqrt(lat_rel_vel);
addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
btVector3 frictionDir2 = frictionDir1.cross(cp.m_normalWorldOnB);
frictionDir2.normalize();//??
addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
} else
{
//re-calculate friction direction every frame, todo: check if this is really needed
btVector3 frictionDir1,frictionDir2;
btPlaneSpace1(cp.m_normalWorldOnB,frictionDir1,frictionDir2);
addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,rb0,rb1, relaxation);
addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
}
}
}
}