diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver')
5 files changed, 183 insertions, 816 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp index 2a5efc6495..27f76b8425 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp @@ -890,6 +890,8 @@ static void setupSpatialGridBatchesMt( btVector3 gridExtent = bboxMax - bboxMin; + gridExtent.setMax(btVector3(btScalar(1), btScalar(1), btScalar(1))); + btVector3 gridCellSize = consExtent; int gridDim[3]; gridDim[0] = int(1.0 + gridExtent.x() / gridCellSize.x()); diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 63d7c98e16..e82d1b139e 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -30,7 +30,8 @@ enum btSolverMode SOLVER_SIMD = 256, SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024, - SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048 + SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048, + SOLVER_USE_ARTICULATED_WARMSTARTING = 4096, }; struct btContactSolverInfoData @@ -45,6 +46,7 @@ struct btContactSolverInfoData btScalar m_sor; //successive over-relaxation term btScalar m_erp; //error reduction for non-contact constraints btScalar m_erp2; //error reduction for contact constraints + btScalar m_deformable_erp; //error reduction for deformable constraints btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -54,7 +56,7 @@ struct btContactSolverInfoData btScalar m_splitImpulseTurnErp; btScalar m_linearSlop; btScalar m_warmstartingFactor; - + btScalar m_articulatedWarmstartingFactor; int m_solverMode; int m_restingContactRestitutionThreshold; int m_minimumSolverBatchSize; @@ -80,6 +82,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); + m_deformable_erp = btScalar(0.); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); @@ -89,6 +92,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_splitImpulseTurnErp = 0.1f; m_linearSlop = btScalar(0.0); m_warmstartingFactor = btScalar(0.85); + m_articulatedWarmstartingFactor = btScalar(0.85); //m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER; m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD; // | SOLVER_RANDMIZE_ORDER; m_restingContactRestitutionThreshold = 2; //unused as of 2.81 @@ -120,6 +124,7 @@ struct btContactSolverInfoDoubleData double m_splitImpulseTurnErp; double m_linearSlop; double m_warmstartingFactor; + double m_articulatedWarmstartingFactor; double m_maxGyroscopicForce; ///it is only used for 'explicit' version of gyroscopic force double m_singleAxisRollingFrictionThreshold; @@ -150,16 +155,17 @@ struct btContactSolverInfoFloatData float m_linearSlop; float m_warmstartingFactor; + float m_articulatedWarmstartingFactor; float m_maxGyroscopicForce; - float m_singleAxisRollingFrictionThreshold; + float m_singleAxisRollingFrictionThreshold; int m_numIterations; int m_solverMode; int m_restingContactRestitutionThreshold; - int m_minimumSolverBatchSize; + int m_minimumSolverBatchSize; int m_splitImpulse; - char m_padding[4]; + }; #endif //BT_CONTACT_SOLVER_INFO diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index 9a3b39e6f8..93626f18ff 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -32,7 +32,7 @@ Cons: /* 2007-09-09 -btGeneric6DofConstraint Refactored by Francisco Leon +btGeneric6DofConstraint Refactored by Francisco Le?n email: projectileman@yahoo.com http://gimpact.sf.net */ @@ -311,9 +311,9 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() case RO_XYZ: { //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles) - //The two planes are non-homologous, so this is a Tait-Bryan angle formalism and not a proper Euler + //The two planes are non-homologous, so this is a Tait Bryan angle formalism and not a proper Euler //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation) - //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait-Bryan angles) + //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait Bryan angles) // x' = Nperp = N.cross(axis2) // y' = N = axis2.cross(axis0) // z' = z diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index d3b71e4583..e4da468299 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -394,18 +394,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 +421,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 +454,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 +498,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 +513,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 +605,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 +614,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 +624,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 +677,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,217 +686,6 @@ 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 @@ -1064,10 +789,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 +798,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 +906,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 +974,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 +1023,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 +1045,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 +1065,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 +1098,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 +1106,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 +1115,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 +1132,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 +1155,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 +1165,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 +1287,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 +1299,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 +1324,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 +1359,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 +1396,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 +1519,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 +1534,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 +1545,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 +1579,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 +1590,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 +1598,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 +1608,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 +1639,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 +1682,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 +1690,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 +1701,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 +1758,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 +1803,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 +1868,4 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod void btSequentialImpulseConstraintSolver::reset() { m_btSeed2 = 0; -}
\ No newline at end of file +} diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 2b88e25be7..f3ef02fccc 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -29,68 +29,6 @@ class btCollisionObject; typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); -struct btSISolverSingleIterationData -{ - btAlignedObjectArray<btSolverBody>& m_tmpSolverBodyPool; - btConstraintArray& m_tmpSolverContactConstraintPool; - btConstraintArray& m_tmpSolverNonContactConstraintPool; - btConstraintArray& m_tmpSolverContactFrictionConstraintPool; - btConstraintArray& m_tmpSolverContactRollingFrictionConstraintPool; - - btAlignedObjectArray<int>& m_orderTmpConstraintPool; - btAlignedObjectArray<int>& m_orderNonContactConstraintPool; - btAlignedObjectArray<int>& m_orderFrictionConstraintPool; - btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& m_tmpConstraintSizesPool; - unsigned long& m_seed; - - btSingleConstraintRowSolver& m_resolveSingleConstraintRowGeneric; - btSingleConstraintRowSolver& m_resolveSingleConstraintRowLowerLimit; - btSingleConstraintRowSolver& m_resolveSplitPenetrationImpulse; - btAlignedObjectArray<int>& m_kinematicBodyUniqueIdToSolverBodyTable; - int& m_fixedBodyId; - int& m_maxOverrideNumSolverIterations; - int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep); - static void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep); - int getSolverBody(btCollisionObject& body) const; - - - btSISolverSingleIterationData(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, - btConstraintArray& tmpSolverContactConstraintPool, - btConstraintArray& tmpSolverNonContactConstraintPool, - btConstraintArray& tmpSolverContactFrictionConstraintPool, - btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, - btAlignedObjectArray<int>& orderTmpConstraintPool, - btAlignedObjectArray<int>& orderNonContactConstraintPool, - btAlignedObjectArray<int>& orderFrictionConstraintPool, - btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& tmpConstraintSizesPool, - btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric, - btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit, - btSingleConstraintRowSolver& resolveSplitPenetrationImpulse, - btAlignedObjectArray<int>& kinematicBodyUniqueIdToSolverBodyTable, - unsigned long& seed, - int& fixedBodyId, - int& maxOverrideNumSolverIterations - ) - :m_tmpSolverBodyPool(tmpSolverBodyPool), - m_tmpSolverContactConstraintPool(tmpSolverContactConstraintPool), - m_tmpSolverNonContactConstraintPool(tmpSolverNonContactConstraintPool), - m_tmpSolverContactFrictionConstraintPool(tmpSolverContactFrictionConstraintPool), - m_tmpSolverContactRollingFrictionConstraintPool(tmpSolverContactRollingFrictionConstraintPool), - m_orderTmpConstraintPool(orderTmpConstraintPool), - m_orderNonContactConstraintPool(orderNonContactConstraintPool), - m_orderFrictionConstraintPool(orderFrictionConstraintPool), - m_tmpConstraintSizesPool(tmpConstraintSizesPool), - m_seed(seed), - m_resolveSingleConstraintRowGeneric(resolveSingleConstraintRowGeneric), - m_resolveSingleConstraintRowLowerLimit(resolveSingleConstraintRowLowerLimit), - m_resolveSplitPenetrationImpulse(resolveSplitPenetrationImpulse), - m_kinematicBodyUniqueIdToSolverBodyTable(kinematicBodyUniqueIdToSolverBodyTable), - m_fixedBodyId(fixedBodyId), - m_maxOverrideNumSolverIterations(maxOverrideNumSolverIterations) - { - } -}; - struct btSolverAnalyticsData { btSolverAnalyticsData() @@ -178,7 +116,6 @@ protected: virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal); void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal); - virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint) @@ -204,8 +141,7 @@ protected: return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint); } -public: - +protected: void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); @@ -213,7 +149,6 @@ public: virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); @@ -225,51 +160,12 @@ public: virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); - static btScalar solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal); - static void convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - static void convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal); - static void convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); - static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, - const btVector3& rel_pos1, const btVector3& rel_pos2); - static btScalar restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold); - static btSolverConstraint& 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 = 0, btScalar cfmSlip = 0.); - static void setupTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, 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); - static void 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); - static btSolverConstraint& 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 = 0., btScalar cfmSlip = 0.); - static void setFrictionConstraintImpulseInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, - - btSolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); - static void convertJointInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, - int& maxOverrideNumSolverIterations, - btSolverConstraint* currentConstraintRow, - btTypedConstraint* constraint, - const btTypedConstraint::btConstraintInfo1& info1, - int solverBodyIdA, - int solverBodyIdB, - const btContactSolverInfo& infoGlobal); - - static btScalar solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - - static void writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); - - static void writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); - static void writeBackBodiesInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); - static void solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); - - ///clear internal cached data and reset random seed virtual void reset(); unsigned long btRand2(); - int btRandInt2(int n); - static unsigned long btRand2a(unsigned long& seed); - static int btRandInt2a(int n, unsigned long& seed); + int btRandInt2(int n); void setRandSeed(unsigned long seed) { @@ -305,18 +201,14 @@ public: ///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4 - static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric(); - static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric(); - static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric(); + btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric(); + btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric(); + btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric(); ///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4 - static btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); - static btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit(); - static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit(); - - static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric(); - static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric(); - + btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); + btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit(); + btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit(); btSolverAnalyticsData m_analyticsData; }; |