diff options
Diffstat (limited to 'thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h')
| -rw-r--r-- | thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h | 149 |
1 files changed, 149 insertions, 0 deletions
diff --git a/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h new file mode 100644 index 0000000000..d2ca307fab --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h @@ -0,0 +1,149 @@ +#ifndef B3_PGS_JACOBI_SOLVER +#define B3_PGS_JACOBI_SOLVER + + +struct b3Contact4; +struct b3ContactPoint; + + +class b3Dispatcher; + +#include "b3TypedConstraint.h" +#include "b3ContactSolverInfo.h" +#include "b3SolverBody.h" +#include "b3SolverConstraint.h" + +struct b3RigidBodyData; +struct b3InertiaData; + +class b3PgsJacobiSolver +{ + +protected: + b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool; + b3ConstraintArray m_tmpSolverContactConstraintPool; + b3ConstraintArray m_tmpSolverNonContactConstraintPool; + b3ConstraintArray m_tmpSolverContactFrictionConstraintPool; + b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + + b3AlignedObjectArray<int> m_orderTmpConstraintPool; + b3AlignedObjectArray<int> m_orderNonContactConstraintPool; + b3AlignedObjectArray<int> m_orderFrictionConstraintPool; + b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool; + + b3AlignedObjectArray<int> m_bodyCount; + b3AlignedObjectArray<int> m_bodyCountCheck; + + b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities; + b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities; + + bool m_usePgs; + void averageVelocities(); + + int m_maxOverrideNumSolverIterations; + + int m_numSplitImpulseRecoveries; + + b3Scalar getContactProcessingThreshold(b3Contact4* contact) + { + return 0.02f; + } + void setupFrictionConstraint( b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, + b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, + b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); + + void setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, + b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, + b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); + + b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); + b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f); + + + void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, + b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, + const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, + b3Vector3& rel_pos1, b3Vector3& rel_pos2); + + void setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, + b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal); + + ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction + unsigned long m_btSeed2; + + + b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution); + + void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal); + + + void resolveSplitPenetrationSIMD( + b3SolverBody& bodyA,b3SolverBody& bodyB, + const b3SolverConstraint& contactConstraint); + + void resolveSplitPenetrationImpulseCacheFriendly( + b3SolverBody& bodyA,b3SolverBody& bodyB, + const b3SolverConstraint& contactConstraint); + + //internal method + int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias); + void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject); + + void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); + + void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); + + void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); + + void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); + +protected: + + virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + + + virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + + + virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal); + + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3PgsJacobiSolver(bool usePgs); + virtual ~b3PgsJacobiSolver(); + +// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts); + void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints); + + b3Scalar solveGroup(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + + ///clear internal cached data and reset random seed + virtual void reset(); + + unsigned long b3Rand2(); + + int b3RandInt2 (int n); + + void setRandSeed(unsigned long seed) + { + m_btSeed2 = seed; + } + unsigned long getRandSeed() const + { + return m_btSeed2; + } + + + + +}; + +#endif //B3_PGS_JACOBI_SOLVER + |