diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h | 187 |
1 files changed, 89 insertions, 98 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index b834c3dac3..70db83b063 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -27,147 +27,142 @@ class btCollisionObject; #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" -typedef btScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); +typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. -ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver +ATTRIBUTE_ALIGNED16(class) +btSequentialImpulseConstraintSolver : public btConstraintSolver { protected: - 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<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; - int m_maxOverrideNumSolverIterations; + int m_maxOverrideNumSolverIterations; int m_fixedBodyId; - // When running solvers on multiple threads, a race condition exists for Kinematic objects that - // participate in more than one solver. - // The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body - // for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island - // (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once. - // To avoid this race condition, this solver does not write the companionId, instead it stores the solver body - // index in this solver-local table, indexed by the uniqueId of the body. - btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading + // When running solvers on multiple threads, a race condition exists for Kinematic objects that + // participate in more than one solver. + // The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body + // for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island + // (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once. + // To avoid this race condition, this solver does not write the companionId, instead it stores the solver body + // index in this solver-local table, indexed by the uniqueId of the body. + btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric; btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit; - btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse; - int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed - void setupSolverFunctions( bool useSimd ); + btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse; + int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed + void setupSolverFunctions(bool useSimd); - btScalar m_leastSquaresResidual; + btScalar m_leastSquaresResidual; - void 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=0., btScalar cfmSlip=0.); + void 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 = 0., btScalar cfmSlip = 0.); - void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, - btScalar desiredVelocity=0., btScalar cfmSlip=0.); + void setupTorsionalFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity = 0., btScalar cfmSlip = 0.); - btSolverConstraint& 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=0., btScalar cfmSlip=0.); - btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); + btSolverConstraint& 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 = 0., btScalar cfmSlip = 0.); + btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar torsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.f); + void setupContactConstraint(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, + const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); - void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, - const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); + static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode); - static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode); - - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); + void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction - unsigned long m_btSeed2; - + unsigned long m_btSeed2; btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold); - virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); + virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); - void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); + void convertContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal); - 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 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); + virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); - btScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) - { - return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); - } + btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint) + { + return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint); + } - btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) - { - return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); - } + btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint) + { + return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint); + } //internal method - int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep); - void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep); - - btScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - btScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - btScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) - { - return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); - } + int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep); + void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep); + + btScalar resolveSingleConstraintRowGeneric(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowLowerLimit(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint); + btScalar resolveSplitPenetrationImpulse(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint) + { + return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint); + } 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); + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + 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); - 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); - virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); - 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); - + 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); public: - BT_DECLARE_ALIGNED_ALLOCATOR(); btSequentialImpulseConstraintSolver(); virtual ~btSequentialImpulseConstraintSolver(); - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); + virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); ///clear internal cached data and reset random seed - virtual void reset(); + virtual void reset(); unsigned long btRand2(); - int btRandInt2 (int n); + int btRandInt2(int n); - void setRandSeed(unsigned long seed) + void setRandSeed(unsigned long seed) { m_btSeed2 = seed; } - unsigned long getRandSeed() const + unsigned long getRandSeed() const { return m_btSeed2; } - - virtual btConstraintSolverType getSolverType() const + virtual btConstraintSolverType getSolverType() const { return BT_SEQUENTIAL_IMPULSE_SOLVER; } - btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric() + btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric() { return m_resolveSingleConstraintRowGeneric; } @@ -175,7 +170,7 @@ public: { m_resolveSingleConstraintRowGeneric = rowSolver; } - btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit() + btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit() { return m_resolveSingleConstraintRowLowerLimit; } @@ -185,18 +180,14 @@ public: } ///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4 - btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric(); - btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric(); - 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 - btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); - btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit(); - btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit(); + btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit(); + btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit(); + btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit(); }; - - - -#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H - +#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H |