diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp | 863 |
1 files changed, 167 insertions, 696 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index d3b71e4583..d2641c582f 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -14,7 +14,9 @@ subject to the following restrictions: */ //#define COMPUTE_IMPULSE_DENOM 1 -//#define BT_ADDITIONAL_DEBUG +#ifdef BT_DEBUG +# define BT_ADDITIONAL_DEBUG +#endif //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. @@ -394,18 +396,6 @@ btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstr return gResolveSingleConstraintRowLowerLimit_scalar_reference; } -btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric() -{ - return gResolveSplitPenetrationImpulse_scalar_reference; -} - -btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2SplitPenetrationImpulseGeneric() -{ - return gResolveSplitPenetrationImpulse_sse2; -} - - - #ifdef USE_SIMD btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric() { @@ -433,11 +423,6 @@ unsigned long btSequentialImpulseConstraintSolver::btRand2() return m_btSeed2; } -unsigned long btSequentialImpulseConstraintSolver::btRand2a(unsigned long& seed) -{ - seed = (1664525L * seed + 1013904223L) & 0xffffffff; - return seed; -} //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) int btSequentialImpulseConstraintSolver::btRandInt2(int n) { @@ -471,44 +456,42 @@ int btSequentialImpulseConstraintSolver::btRandInt2(int n) return (int)(r % un); } -int btSequentialImpulseConstraintSolver::btRandInt2a(int n, unsigned long& seed) +void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) { - // seems good; xor-fold and modulus - const unsigned long un = static_cast<unsigned long>(n); - unsigned long r = btSequentialImpulseConstraintSolver::btRand2a(seed); + btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0; - // note: probably more aggressive than it needs to be -- might be - // able to get away without one or two of the innermost branches. - if (un <= 0x00010000UL) + solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); + + if (rb) { - r ^= (r >> 16); - if (un <= 0x00000100UL) + solverBody->m_worldTransform = rb->getWorldTransform(); + solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor()); + solverBody->m_originalBody = rb; + solverBody->m_angularFactor = rb->getAngularFactor(); + solverBody->m_linearFactor = rb->getLinearFactor(); + solverBody->m_linearVelocity = rb->getLinearVelocity(); + solverBody->m_angularVelocity = rb->getAngularVelocity(); + solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep; + solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep; + } + else { - r ^= (r >> 8); - if (un <= 0x00000010UL) - { - r ^= (r >> 4); - if (un <= 0x00000004UL) - { - r ^= (r >> 2); - if (un <= 0x00000002UL) - { - r ^= (r >> 1); + solverBody->m_worldTransform.setIdentity(); + solverBody->internalSetInvMass(btVector3(0, 0, 0)); + solverBody->m_originalBody = 0; + solverBody->m_angularFactor.setValue(1, 1, 1); + solverBody->m_linearFactor.setValue(1, 1, 1); + solverBody->m_linearVelocity.setValue(0, 0, 0); + solverBody->m_angularVelocity.setValue(0, 0, 0); + solverBody->m_externalForceImpulse.setValue(0, 0, 0); + solverBody->m_externalTorqueImpulse.setValue(0, 0, 0); } } - } - } - } - - return (int)(r % un); -} -void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) -{ - btSISolverSingleIterationData::initSolverBody(solverBody, collisionObject, timeStep); -} - -btScalar btSequentialImpulseConstraintSolver::restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) +btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) { //printf("rel_vel =%f\n", rel_vel); if (btFabs(rel_vel) < velocityThreshold) @@ -517,10 +500,6 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurveInternal(btScalar btScalar rest = restitution * -rel_vel; return rest; } -btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) -{ - return btSequentialImpulseConstraintSolver::restitutionCurveInternal(rel_vel, restitution, velocityThreshold); -} void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj, btVector3& frictionDirection, int frictionMode) { @@ -536,13 +515,13 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb } } -void btSequentialImpulseConstraintSolver::setupFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { - btSolverBody& solverBodyA = tmpSolverBodyPool[solverBodyIdA]; - btSolverBody& solverBodyB = tmpSolverBodyPool[solverBodyIdB]; + btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; - btRigidBody* body0 = tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* bodyA = tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; + btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -628,22 +607,6 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraintInternal(btAlig } } -void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) -{ - btSequentialImpulseConstraintSolver::setupFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); -} - -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) -{ - btSolverConstraint& solverConstraint = tmpSolverContactFrictionConstraintPool.expandNonInitializing(); - solverConstraint.m_frictionIndex = frictionIndex; - setupFrictionConstraintInternal(tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); - return solverConstraint; -} - - - btSolverConstraint& 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); @@ -653,8 +616,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c return solverConstraint; } - -void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, +void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) @@ -664,11 +626,11 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraintIntern solverConstraint.m_contactNormal1 = normalAxis; solverConstraint.m_contactNormal2 = -normalAxis; - btSolverBody& solverBodyA = tmpSolverBodyPool[solverBodyIdA]; - btSolverBody& solverBodyB = tmpSolverBodyPool[solverBodyIdB]; + btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; - btRigidBody* body0 = tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* bodyA = tmpSolverBodyPool[solverBodyIdB].m_originalBody; + btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; + btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -717,30 +679,6 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraintIntern } } - -void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, - btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, - btScalar desiredVelocity, btScalar cfmSlip) - -{ - setupTorsionalFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis1, solverBodyIdA, solverBodyIdB, - cp, combinedTorsionalFriction, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, - desiredVelocity, cfmSlip); - -} - -btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) -{ - btSolverConstraint& solverConstraint = tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); - solverConstraint.m_frictionIndex = frictionIndex; - setupTorsionalFrictionConstraintInternal(tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); - return solverConstraint; -} - - btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); @@ -750,223 +688,14 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionCon return solverConstraint; } -int btSISolverSingleIterationData::getOrInitSolverBody(btCollisionObject & body, btScalar timeStep) -{ -#if BT_THREADSAFE - int solverBodyId = -1; - bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; - if (isRigidBodyType && !body.isStaticOrKinematicObject()) - { - // dynamic body - // Dynamic bodies can only be in one island, so it's safe to write to the companionId - solverBodyId = body.getCompanionId(); - if (solverBodyId < 0) - { - solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody, &body, timeStep); - body.setCompanionId(solverBodyId); - } - } - else if (isRigidBodyType && body.isKinematicObject()) - { - // - // NOTE: must test for kinematic before static because some kinematic objects also - // identify as "static" - // - // Kinematic bodies can be in multiple islands at once, so it is a - // race condition to write to them, so we use an alternate method - // to record the solverBodyId - int uniqueId = body.getWorldArrayIndex(); - const int INVALID_SOLVER_BODY_ID = -1; - if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size()) - { - m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID); - } - solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId]; - // if no table entry yet, - if (solverBodyId == INVALID_SOLVER_BODY_ID) - { - // create a table entry for this body - solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody, &body, timeStep); - m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId; - } - } - else - { - bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK); - // Incorrectly set collision object flags can degrade performance in various ways. - if (!isMultiBodyType) - { - btAssert(body.isStaticOrKinematicObject()); - } - //it could be a multibody link collider - // all fixed bodies (inf mass) get mapped to a single solver id - if (m_fixedBodyId < 0) - { - m_fixedBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&fixedBody, 0, timeStep); - } - solverBodyId = m_fixedBodyId; - } - btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size()); - return solverBodyId; -#else // BT_THREADSAFE - - int solverBodyIdA = -1; - - if (body.getCompanionId() >= 0) - { - //body has already been converted - solverBodyIdA = body.getCompanionId(); - btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); - } - else - { - btRigidBody* rb = btRigidBody::upcast(&body); - //convert both active and kinematic objects (for their velocity) - if (rb && (rb->getInvMass() || rb->isKinematicObject())) - { - solverBodyIdA = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody, &body, timeStep); - body.setCompanionId(solverBodyIdA); - } - else - { - if (m_fixedBodyId < 0) - { - m_fixedBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&fixedBody, 0, timeStep); - } - return m_fixedBodyId; - // return 0;//assume first one is a fixed solver body - } - } - - return solverBodyIdA; -#endif // BT_THREADSAFE -} -void btSISolverSingleIterationData::initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep) -{ - btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0; - - solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); - solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); - solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); - solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); - - if (rb) - { - solverBody->m_worldTransform = rb->getWorldTransform(); - solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor()); - solverBody->m_originalBody = rb; - solverBody->m_angularFactor = rb->getAngularFactor(); - solverBody->m_linearFactor = rb->getLinearFactor(); - solverBody->m_linearVelocity = rb->getLinearVelocity(); - solverBody->m_angularVelocity = rb->getAngularVelocity(); - solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep; - solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep; - } - else - { - solverBody->m_worldTransform.setIdentity(); - solverBody->internalSetInvMass(btVector3(0, 0, 0)); - solverBody->m_originalBody = 0; - solverBody->m_angularFactor.setValue(1, 1, 1); - solverBody->m_linearFactor.setValue(1, 1, 1); - solverBody->m_linearVelocity.setValue(0, 0, 0); - solverBody->m_angularVelocity.setValue(0, 0, 0); - solverBody->m_externalForceImpulse.setValue(0, 0, 0); - solverBody->m_externalTorqueImpulse.setValue(0, 0, 0); - } -} - -int btSISolverSingleIterationData::getSolverBody(btCollisionObject& body) const -{ -#if BT_THREADSAFE - int solverBodyId = -1; - bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; - if (isRigidBodyType && !body.isStaticOrKinematicObject()) - { - // dynamic body - // Dynamic bodies can only be in one island, so it's safe to write to the companionId - solverBodyId = body.getCompanionId(); - btAssert(solverBodyId >= 0); - } - else if (isRigidBodyType && body.isKinematicObject()) - { - // - // NOTE: must test for kinematic before static because some kinematic objects also - // identify as "static" - // - // Kinematic bodies can be in multiple islands at once, so it is a - // race condition to write to them, so we use an alternate method - // to record the solverBodyId - int uniqueId = body.getWorldArrayIndex(); - const int INVALID_SOLVER_BODY_ID = -1; - if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size()) - { - m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID); - } - solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId]; - btAssert(solverBodyId != INVALID_SOLVER_BODY_ID); - } - else - { - bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK); - // Incorrectly set collision object flags can degrade performance in various ways. - if (!isMultiBodyType) - { - btAssert(body.isStaticOrKinematicObject()); - } - btAssert(m_fixedBodyId >= 0); - solverBodyId = m_fixedBodyId; - } - btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size()); - return solverBodyId; -#else // BT_THREADSAFE - int solverBodyIdA = -1; - - if (body.getCompanionId() >= 0) - { - //body has already been converted - solverBodyIdA = body.getCompanionId(); - btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); - } - else - { - btRigidBody* rb = btRigidBody::upcast(&body); - //convert both active and kinematic objects (for their velocity) - if (rb && (rb->getInvMass() || rb->isKinematicObject())) - { - btAssert(0); - } - else - { - if (m_fixedBodyId < 0) - { - btAssert(0); - } - return m_fixedBodyId; - // return 0;//assume first one is a fixed solver body - } - } - - return solverBodyIdA; -#endif // BT_THREADSAFE -} - int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body, btScalar timeStep) { #if BT_THREADSAFE int solverBodyId = -1; - bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; - if (isRigidBodyType && !body.isStaticOrKinematicObject()) + const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; + const bool isStaticOrKinematic = body.isStaticOrKinematicObject(); + const bool isKinematic = body.isKinematicObject(); + if (isRigidBodyType && !isStaticOrKinematic) { // dynamic body // Dynamic bodies can only be in one island, so it's safe to write to the companionId @@ -979,7 +708,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body.setCompanionId(solverBodyId); } } - else if (isRigidBodyType && body.isKinematicObject()) + else if (isRigidBodyType && isKinematic) { // // NOTE: must test for kinematic before static because some kinematic objects also @@ -1064,10 +793,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& } #include <stdio.h> - - -void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISolverSingleIterationData& siData, - btSolverConstraint& solverConstraint, +void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, @@ -1076,8 +802,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISol // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); - btSolverBody* bodyA = &siData.m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyB = &siData.m_tmpSolverBodyPool[solverBodyIdB]; + btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; btRigidBody* rb0 = bodyA->m_originalBody; btRigidBody* rb1 = bodyB->m_originalBody; @@ -1184,7 +910,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISol solverConstraint.m_friction = cp.m_combinedFriction; - restitution = btSequentialImpulseConstraintSolver::restitutionCurveInternal(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); if (restitution <= btScalar(0.)) { restitution = 0.f; @@ -1252,109 +978,39 @@ void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISol } } -void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - const btVector3& rel_pos1, const btVector3& rel_pos2) -{ - btSISolverSingleIterationData siData(m_tmpSolverBodyPool, - m_tmpSolverContactConstraintPool, - m_tmpSolverNonContactConstraintPool, - m_tmpSolverContactFrictionConstraintPool, - m_tmpSolverContactRollingFrictionConstraintPool, - m_orderTmpConstraintPool, - m_orderNonContactConstraintPool, - m_orderFrictionConstraintPool, - m_tmpConstraintSizesPool, - m_resolveSingleConstraintRowGeneric, - m_resolveSingleConstraintRowLowerLimit, - m_resolveSplitPenetrationImpulse, - m_kinematicBodyUniqueIdToSolverBodyTable, - m_btSeed2, - m_fixedBodyId, - m_maxOverrideNumSolverIterations - ); - - - setupContactConstraintInternal(siData, solverConstraint, - solverBodyIdA, solverBodyIdB, - cp, infoGlobal, - relaxation, - rel_pos1, rel_pos2); -} - - -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, - btSolverConstraint& solverConstraint, +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { - btSolverBody* bodyA = &tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyB = &tmpSolverBodyPool[solverBodyIdB]; - - btRigidBody* rb0 = bodyA->m_originalBody; - btRigidBody* rb1 = bodyB->m_originalBody; - { - btSolverConstraint& frictionConstraint1 = tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; - if (rb0) - bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1 * rb0->getInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse); - if (rb1) - bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2 * rb1->getInvMass(), -frictionConstraint1.m_angularComponentB, -(btScalar)frictionConstraint1.m_appliedImpulse); - } - else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } + btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + + frictionConstraint1.m_appliedImpulse = 0.f; } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - btSolverConstraint& frictionConstraint2 = tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (rb0) - bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1 * rb0->getInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse); - if (rb1) - bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2 * rb1->getInvMass(), -frictionConstraint2.m_angularComponentB, -(btScalar)frictionConstraint2.m_appliedImpulse); - } - else - { - frictionConstraint2.m_appliedImpulse = 0.f; - } + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; + + frictionConstraint2.m_appliedImpulse = 0.f; } } -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) -{ - setFrictionConstraintImpulseInternal(m_tmpSolverBodyPool, m_tmpSolverContactFrictionConstraintPool, - solverConstraint, - solverBodyIdA, solverBodyIdB, - cp, infoGlobal); - -} -void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) { btCollisionObject *colObj0 = 0, *colObj1 = 0; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA = siData.getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); - int solverBodyIdB = siData.getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); + int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); // btRigidBody* bodyA = btRigidBody::upcast(colObj0); // btRigidBody* bodyB = btRigidBody::upcast(colObj1); - btSolverBody* solverBodyA = &siData.m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* solverBodyB = &siData.m_tmpSolverBodyPool[solverBodyIdB]; + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; ///avoid collision response between two static objects if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero()))) @@ -1371,8 +1027,8 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl btVector3 rel_pos2; btScalar relaxation; - int frictionIndex = siData.m_tmpSolverContactConstraintPool.size(); - btSolverConstraint& solverConstraint = siData.m_tmpSolverContactConstraintPool.expandNonInitializing(); + int frictionIndex = m_tmpSolverContactConstraintPool.size(); + btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -1393,20 +1049,16 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl btVector3 vel = vel1 - vel2; btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); - setupContactConstraintInternal(siData, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); + setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); /////setup the friction constraints - solverConstraint.m_frictionIndex = siData.m_tmpSolverContactFrictionConstraintPool.size(); + solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0)) { { - - btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, - siData.m_tmpSolverContactRollingFrictionConstraintPool, - cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - + addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); btVector3 axis0, axis1; btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1); axis0.normalize(); @@ -1417,19 +1069,13 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl applyAnisotropicFriction(colObj0, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); if (axis0.length() > 0.001) - { - btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, - siData.m_tmpSolverContactRollingFrictionConstraintPool, axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); - } if (axis1.length() > 0.001) - { - btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool, - siData.m_tmpSolverContactRollingFrictionConstraintPool, axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); } } - } ///Bullet has several options to set the friction directions ///By default, each contact has only a single friction direction that is recomputed automatically very frame @@ -1456,8 +1102,7 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel); applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, - cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { @@ -1465,8 +1110,7 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl cp.m_lateralFrictionDir2.normalize(); //?? applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, - cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); } } else @@ -1475,15 +1119,13 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); - btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, - cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); - btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, - cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) @@ -1494,43 +1136,15 @@ void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingl } else { - btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, - cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); + addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, - cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); + addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); } + setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); } - btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal( - siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool, - solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); } } -} - -void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) -{ - btSISolverSingleIterationData siData(m_tmpSolverBodyPool, - m_tmpSolverContactConstraintPool, - m_tmpSolverNonContactConstraintPool, - m_tmpSolverContactFrictionConstraintPool, - m_tmpSolverContactRollingFrictionConstraintPool, - m_orderTmpConstraintPool, - m_orderNonContactConstraintPool, - m_orderFrictionConstraintPool, - m_tmpConstraintSizesPool, - m_resolveSingleConstraintRowGeneric, - m_resolveSingleConstraintRowLowerLimit, - m_resolveSplitPenetrationImpulse, - m_kinematicBodyUniqueIdToSolverBodyTable, - m_btSeed2, - m_fixedBodyId, - m_maxOverrideNumSolverIterations); - - btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, infoGlobal); -} void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { @@ -1545,9 +1159,7 @@ void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** } } -void btSequentialImpulseConstraintSolver::convertJointInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, - int& maxOverrideNumSolverIterations, - btSolverConstraint* currentConstraintRow, +void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, @@ -1557,12 +1169,12 @@ void btSequentialImpulseConstraintSolver::convertJointInternal(btAlignedObjectAr const btRigidBody& rbA = constraint->getRigidBodyA(); const btRigidBody& rbB = constraint->getRigidBodyB(); - const btSolverBody* bodyAPtr = &tmpSolverBodyPool[solverBodyIdA]; - const btSolverBody* bodyBPtr = &tmpSolverBodyPool[solverBodyIdB]; + const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations > maxOverrideNumSolverIterations) - maxOverrideNumSolverIterations = overrideNumSolverIterations; + if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; for (int j = 0; j < info1.m_numConstraintRows; j++) { @@ -1679,16 +1291,7 @@ void btSequentialImpulseConstraintSolver::convertJointInternal(btAlignedObjectAr } } -void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow, - btTypedConstraint* constraint, - const btTypedConstraint::btConstraintInfo1& info1, - int solverBodyIdA, - int solverBodyIdB, - const btContactSolverInfo& infoGlobal) -{ -} - -void btSequentialImpulseConstraintSolver::convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) { BT_PROFILE("convertJoints"); for (int j = 0; j < numConstraints; j++) @@ -1700,11 +1303,11 @@ void btSequentialImpulseConstraintSolver::convertJointsInternal(btSISolverSingle int totalNumRows = 0; - siData.m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); //calculate the total number of contraint rows for (int i = 0; i < numConstraints; i++) { - btTypedConstraint::btConstraintInfo1& info1 = siData.m_tmpConstraintSizesPool[i]; + btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; btJointFeedback* fb = constraints[i]->getJointFeedback(); if (fb) { @@ -1725,58 +1328,34 @@ void btSequentialImpulseConstraintSolver::convertJointsInternal(btSISolverSingle } totalNumRows += info1.m_numConstraintRows; } - siData.m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); ///setup the btSolverConstraints int currentRow = 0; for (int i = 0; i < numConstraints; i++) { - const btTypedConstraint::btConstraintInfo1& info1 = siData.m_tmpConstraintSizesPool[i]; + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; if (info1.m_numConstraintRows) { btAssert(currentRow < totalNumRows); - btSolverConstraint* currentConstraintRow = &siData.m_tmpSolverNonContactConstraintPool[currentRow]; + btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; btTypedConstraint* constraint = constraints[i]; btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); - int solverBodyIdA = siData.getOrInitSolverBody(rbA, infoGlobal.m_timeStep); - int solverBodyIdB = siData.getOrInitSolverBody(rbB, infoGlobal.m_timeStep); + int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep); + int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep); - convertJointInternal(siData.m_tmpSolverBodyPool, siData.m_maxOverrideNumSolverIterations, - currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); + convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); } currentRow += info1.m_numConstraintRows; } } -void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) -{ - btSISolverSingleIterationData siData(m_tmpSolverBodyPool, - m_tmpSolverContactConstraintPool, - m_tmpSolverNonContactConstraintPool, - m_tmpSolverContactFrictionConstraintPool, - m_tmpSolverContactRollingFrictionConstraintPool, - m_orderTmpConstraintPool, - m_orderNonContactConstraintPool, - m_orderFrictionConstraintPool, - m_tmpConstraintSizesPool, - m_resolveSingleConstraintRowGeneric, - m_resolveSingleConstraintRowLowerLimit, - m_resolveSplitPenetrationImpulse, - m_kinematicBodyUniqueIdToSolverBodyTable, - m_btSeed2, - m_fixedBodyId, - m_maxOverrideNumSolverIterations); - - convertJointsInternal(siData, constraints, numConstraints, infoGlobal); -} - - -void btSequentialImpulseConstraintSolver::convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("convertBodies"); for (int i = 0; i < numBodies; i++) @@ -1784,23 +1363,23 @@ void btSequentialImpulseConstraintSolver::convertBodiesInternal(btSISolverSingle bodies[i]->setCompanionId(-1); } #if BT_THREADSAFE - siData.m_kinematicBodyUniqueIdToSolverBodyTable.resize(0); + m_kinematicBodyUniqueIdToSolverBodyTable.resize(0); #endif // BT_THREADSAFE - siData.m_tmpSolverBodyPool.reserve(numBodies + 1); - siData.m_tmpSolverBodyPool.resize(0); + m_tmpSolverBodyPool.reserve(numBodies + 1); + m_tmpSolverBodyPool.resize(0); //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); //initSolverBody(&fixedBody,0); for (int i = 0; i < numBodies; i++) { - int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); btRigidBody* body = btRigidBody::upcast(bodies[i]); if (body && body->getInvMass()) { - btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId]; + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; btVector3 gyroForce(0, 0, 0); if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) { @@ -1821,29 +1400,6 @@ void btSequentialImpulseConstraintSolver::convertBodiesInternal(btSISolverSingle } } - -void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) -{ - btSISolverSingleIterationData siData(m_tmpSolverBodyPool, - m_tmpSolverContactConstraintPool, - m_tmpSolverNonContactConstraintPool, - m_tmpSolverContactFrictionConstraintPool, - m_tmpSolverContactRollingFrictionConstraintPool, - m_orderTmpConstraintPool, - m_orderNonContactConstraintPool, - m_orderFrictionConstraintPool, - m_tmpConstraintSizesPool, - m_resolveSingleConstraintRowGeneric, - m_resolveSingleConstraintRowLowerLimit, - m_resolveSplitPenetrationImpulse, - m_kinematicBodyUniqueIdToSolverBodyTable, - m_btSeed2, - m_fixedBodyId, - m_maxOverrideNumSolverIterations); - - convertBodiesInternal(siData, bodies, numBodies, infoGlobal); -} - btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { m_fixedBodyId = -1; @@ -1967,14 +1523,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) +btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) { BT_PROFILE("solveSingleIteration"); btScalar leastSquaresResidual = 0.f; - int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size(); - int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size(); - int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size(); + int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); + int numConstraintPool = m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { @@ -1982,10 +1538,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS { for (int j = 0; j < numNonContactPool; ++j) { - int tmp = siData.m_orderNonContactConstraintPool[j]; - int swapi = btRandInt2a(j + 1, siData.m_seed); - siData.m_orderNonContactConstraintPool[j] = siData.m_orderNonContactConstraintPool[swapi]; - siData.m_orderNonContactConstraintPool[swapi] = tmp; + int tmp = m_orderNonContactConstraintPool[j]; + int swapi = btRandInt2(j + 1); + m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; + m_orderNonContactConstraintPool[swapi] = tmp; } //contact/friction constraints are not solved more than @@ -1993,30 +1549,30 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS { for (int j = 0; j < numConstraintPool; ++j) { - int tmp = siData.m_orderTmpConstraintPool[j]; - int swapi = btRandInt2a(j + 1, siData.m_seed); - siData.m_orderTmpConstraintPool[j] = siData.m_orderTmpConstraintPool[swapi]; - siData.m_orderTmpConstraintPool[swapi] = tmp; + int tmp = m_orderTmpConstraintPool[j]; + int swapi = btRandInt2(j + 1); + m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; + m_orderTmpConstraintPool[swapi] = tmp; } for (int j = 0; j < numFrictionPool; ++j) { - int tmp = siData.m_orderFrictionConstraintPool[j]; - int swapi = btRandInt2a(j + 1, siData.m_seed); - siData.m_orderFrictionConstraintPool[j] = siData.m_orderFrictionConstraintPool[swapi]; - siData.m_orderFrictionConstraintPool[swapi] = tmp; + int tmp = m_orderFrictionConstraintPool[j]; + int swapi = btRandInt2(j + 1); + m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; + m_orderFrictionConstraintPool[swapi] = tmp; } } } } ///solve all joint constraints - for (int j = 0; j < siData.m_tmpSolverNonContactConstraintPool.size(); j++) + for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) { - btSolverConstraint& constraint = siData.m_tmpSolverNonContactConstraintPool[siData.m_orderNonContactConstraintPool[j]]; + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; if (iteration < constraint.m_overrideNumSolverIterations) { - btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[constraint.m_solverBodyIdA], siData.m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } @@ -2027,10 +1583,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS { if (constraints[j]->isEnabled()) { - int bodyAid = siData.getSolverBody(constraints[j]->getRigidBodyA()); - int bodyBid = siData.getSolverBody(constraints[j]->getRigidBodyB()); - btSolverBody& bodyA = siData.m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = siData.m_tmpSolverBodyPool[bodyBid]; + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep); } } @@ -2038,7 +1594,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS ///solve all contact constraints if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) { - int numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size(); + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; for (int c = 0; c < numPoolConstraints; c++) @@ -2046,8 +1602,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS btScalar totalImpulse = 0; { - const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[c]]; - btScalar residual = siData.m_resolveSingleConstraintRowLowerLimit(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; + btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); totalImpulse = solveManifold.m_appliedImpulse; @@ -2056,28 +1612,28 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS if (applyFriction) { { - btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[c * multiplier]]; + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]]; if (totalImpulse > btScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) { - btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[c * multiplier + 1]]; + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]]; if (totalImpulse > btScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } @@ -2087,40 +1643,40 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS { //solve the friction constraints after all contact constraints, don't interleave them - int numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size(); + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int j; for (j = 0; j < numPoolConstraints; j++) { - const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[j]]; - btScalar residual = siData.m_resolveSingleConstraintRowLowerLimit(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } ///solve all friction constraints - int numFrictionPoolConstraints = siData.m_tmpSolverContactFrictionConstraintPool.size(); + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); for (j = 0; j < numFrictionPoolConstraints; j++) { - btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = siData.m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; if (totalImpulse > btScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } } - int numRollingFrictionPoolConstraints = siData.m_tmpSolverContactRollingFrictionConstraintPool.size(); + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); for (int j = 0; j < numRollingFrictionPoolConstraints; j++) { - btSolverConstraint& rollingFrictionConstraint = siData.m_tmpSolverContactRollingFrictionConstraintPool[j]; - btScalar totalImpulse = siData.m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; if (totalImpulse > btScalar(0)) { btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; @@ -2130,7 +1686,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], siData.m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } @@ -2138,56 +1694,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSIS return leastSquaresResidual; } - -btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) -{ - btSISolverSingleIterationData siData(m_tmpSolverBodyPool, - m_tmpSolverContactConstraintPool, - m_tmpSolverNonContactConstraintPool, - m_tmpSolverContactFrictionConstraintPool, - m_tmpSolverContactRollingFrictionConstraintPool, - m_orderTmpConstraintPool, - m_orderNonContactConstraintPool, - m_orderFrictionConstraintPool, - m_tmpConstraintSizesPool, - m_resolveSingleConstraintRowGeneric, - m_resolveSingleConstraintRowLowerLimit, - m_resolveSplitPenetrationImpulse, - m_kinematicBodyUniqueIdToSolverBodyTable, - m_btSeed2, - m_fixedBodyId, - m_maxOverrideNumSolverIterations); - - btScalar leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, - iteration, constraints, numConstraints, infoGlobal); - return leastSquaresResidual; -} - void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - btSISolverSingleIterationData siData(m_tmpSolverBodyPool, - m_tmpSolverContactConstraintPool, - m_tmpSolverNonContactConstraintPool, - m_tmpSolverContactFrictionConstraintPool, - m_tmpSolverContactRollingFrictionConstraintPool, - m_orderTmpConstraintPool, - m_orderNonContactConstraintPool, - m_orderFrictionConstraintPool, - m_tmpConstraintSizesPool, - m_resolveSingleConstraintRowGeneric, - m_resolveSingleConstraintRowLowerLimit, - m_resolveSplitPenetrationImpulse, - m_kinematicBodyUniqueIdToSolverBodyTable, - m_btSeed2, - m_fixedBodyId, - m_maxOverrideNumSolverIterations); - - solveGroupCacheFriendlySplitImpulseIterationsInternal(siData, - bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - -} -void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) -{ BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); int iteration; if (infoGlobal.m_splitImpulse) @@ -2197,13 +1705,13 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte { btScalar leastSquaresResidual = 0.f; { - int numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size(); + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int j; for (j = 0; j < numPoolConstraints; j++) { - const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[j]]; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - btScalar residual = siData.m_resolveSplitPenetrationImpulse(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } @@ -2254,42 +1762,31 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( return 0.f; } -void btSequentialImpulseConstraintSolver::writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { for (int j = iBegin; j < iEnd; j++) { - const btSolverConstraint& solveManifold = tmpSolverContactConstraintPool[j]; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint; btAssert(pt); pt->m_appliedImpulse = solveManifold.m_appliedImpulse; // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; // printf("pt->m_appliedImpulseLateral1 = %f\n", f); - pt->m_appliedImpulseLateral1 = tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - pt->m_appliedImpulseLateral2 = tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; } //do a callback here? } } -void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) -{ - writeBackContactsInternal(m_tmpSolverContactConstraintPool, m_tmpSolverContactFrictionConstraintPool, iBegin, iEnd, infoGlobal); - -} - void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - writeBackJointsInternal(m_tmpSolverNonContactConstraintPool, iBegin, iEnd, infoGlobal); -} - -void btSequentialImpulseConstraintSolver::writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) -{ for (int j = iBegin; j < iEnd; j++) { - const btSolverConstraint& solverConstr = tmpSolverNonContactConstraintPool[j]; + const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; btJointFeedback* fb = constr->getJointFeedback(); if (fb) @@ -2310,79 +1807,53 @@ void btSequentialImpulseConstraintSolver::writeBackJointsInternal(btConstraintAr void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - writeBackBodiesInternal(m_tmpSolverBodyPool, iBegin, iEnd, infoGlobal); -} -void btSequentialImpulseConstraintSolver::writeBackBodiesInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) -{ for (int i = iBegin; i < iEnd; i++) { - btRigidBody* body = tmpSolverBodyPool[i].m_originalBody; + btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; if (body) { if (infoGlobal.m_splitImpulse) - tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); + m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); else - tmpSolverBodyPool[i].writebackVelocity(); + m_tmpSolverBodyPool[i].writebackVelocity(); - tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( - tmpSolverBodyPool[i].m_linearVelocity + - tmpSolverBodyPool[i].m_externalForceImpulse); + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( + m_tmpSolverBodyPool[i].m_linearVelocity + + m_tmpSolverBodyPool[i].m_externalForceImpulse); - tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( - tmpSolverBodyPool[i].m_angularVelocity + - tmpSolverBodyPool[i].m_externalTorqueImpulse); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( + m_tmpSolverBodyPool[i].m_angularVelocity + + m_tmpSolverBodyPool[i].m_externalTorqueImpulse); if (infoGlobal.m_splitImpulse) - tmpSolverBodyPool[i].m_originalBody->setWorldTransform(tmpSolverBodyPool[i].m_worldTransform); + m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform); - tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); + m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); } } } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveGroupCacheFriendlyFinish"); if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - writeBackContactsInternal(siData.m_tmpSolverContactConstraintPool, siData.m_tmpSolverContactFrictionConstraintPool, 0, siData.m_tmpSolverContactConstraintPool.size(), infoGlobal); + writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal); } - writeBackJointsInternal(siData.m_tmpSolverNonContactConstraintPool, 0, siData.m_tmpSolverNonContactConstraintPool.size(), infoGlobal); - writeBackBodiesInternal(siData.m_tmpSolverBodyPool, 0, siData.m_tmpSolverBodyPool.size(), infoGlobal); + writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal); + writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal); - siData.m_tmpSolverContactConstraintPool.resizeNoInitialize(0); - siData.m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); - siData.m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); - siData.m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); - siData.m_tmpSolverBodyPool.resizeNoInitialize(0); + m_tmpSolverBodyPool.resizeNoInitialize(0); return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) -{ - btSISolverSingleIterationData siData(m_tmpSolverBodyPool, - m_tmpSolverContactConstraintPool, - m_tmpSolverNonContactConstraintPool, - m_tmpSolverContactFrictionConstraintPool, - m_tmpSolverContactRollingFrictionConstraintPool, - m_orderTmpConstraintPool, - m_orderNonContactConstraintPool, - m_orderFrictionConstraintPool, - m_tmpConstraintSizesPool, - m_resolveSingleConstraintRowGeneric, - m_resolveSingleConstraintRowLowerLimit, - m_resolveSplitPenetrationImpulse, - m_kinematicBodyUniqueIdToSolverBodyTable, - m_btSeed2, - m_fixedBodyId, - m_maxOverrideNumSolverIterations); - - return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, infoGlobal); -} - /// btSequentialImpulseConstraintSolver Sequentially applies impulses btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/) { @@ -2401,4 +1872,4 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod void btSequentialImpulseConstraintSolver::reset() { m_btSeed2 = 0; -}
\ No newline at end of file +} |