From e252f5caddf53cb56d9637308ea5bcdc8953f742 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Wed, 28 Nov 2007 03:17:09 +0000 Subject: [PATCH] Bugfix: allow btCollisionObjects (non-btRigidBody) to interact properly with btRigidBody for cache-friendly btSequentialImpulseConstraintSolver. Thanks Andy O'Neil for pointing this out. --- Demos/RagdollDemo/RagdollDemo.cpp | 10 ++ .../CollisionDispatch/btCollisionObject.cpp | 4 +- .../btSequentialImpulseConstraintSolver.cpp | 141 +++++++++++------- .../btSequentialImpulseConstraintSolver.h | 2 +- 4 files changed, 100 insertions(+), 57 deletions(-) diff --git a/Demos/RagdollDemo/RagdollDemo.cpp b/Demos/RagdollDemo/RagdollDemo.cpp index 34a954473..fe2bfd214 100644 --- a/Demos/RagdollDemo/RagdollDemo.cpp +++ b/Demos/RagdollDemo/RagdollDemo.cpp @@ -329,7 +329,17 @@ void RagdollDemo::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-10,0)); + +#define CREATE_GROUND_COLLISION_OBJECT 1 +#ifdef CREATE_GROUND_COLLISION_OBJECT + btCollisionObject* fixedGround = new btCollisionObject(); + fixedGround->setCollisionShape(groundShape); + fixedGround->setWorldTransform(groundTransform); + m_dynamicsWorld->addCollisionObject(fixedGround); +#else localCreateRigidBody(btScalar(0.),groundTransform,groundShape); +#endif //CREATE_GROUND_COLLISION_OBJECT + } // Spawn one ragdoll diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 6b72a131c..12267e2a9 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -19,11 +19,13 @@ subject to the following restrictions: btCollisionObject::btCollisionObject() : m_broadphaseHandle(0), m_collisionShape(0), - m_collisionFlags(0), + m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), m_islandTag1(-1), m_companionId(-1), m_activationState1(1), m_deactivationTime(btScalar(0.)), + m_friction(btScalar(0.5)), + m_restitution(btScalar(0.)), m_userObjectPointer(0), m_internalOwner(0), m_hitFraction(btScalar(1.)), diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 39faf7527..7619a31dc 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -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;igetBody1(); - - 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); } } + + } } } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 83a96d4dc..a4067fb55 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -41,7 +41,7 @@ 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); void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); - void 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 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); ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];