dali-physics/third-party/bullet3/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h

   1 #ifndef B3_PGS_JACOBI_SOLVER
   2 #define B3_PGS_JACOBI_SOLVER
   3
   4 struct b3Contact4;
   5 struct b3ContactPoint;
   6
   7 class b3Dispatcher;
   8
   9 #include "b3TypedConstraint.h"
  10 #include "b3ContactSolverInfo.h"
  11 #include "b3SolverBody.h"
  12 #include "b3SolverConstraint.h"
  13
  14 struct b3RigidBodyData;
  15 struct b3InertiaData;
  16
  17 class b3PgsJacobiSolver
  18 {
  19 protected:
  20         b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
  21         b3ConstraintArray m_tmpSolverContactConstraintPool;
  22         b3ConstraintArray m_tmpSolverNonContactConstraintPool;
  23         b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
  24         b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
  25
  26         b3AlignedObjectArray<int> m_orderTmpConstraintPool;
  27         b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
  28         b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
  29         b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
  30
  31         b3AlignedObjectArray<int> m_bodyCount;
  32         b3AlignedObjectArray<int> m_bodyCountCheck;
  33
  34         b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocities;
  35         b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocities;
  36
  37         bool m_usePgs;
  38         void averageVelocities();
  39
  40         int m_maxOverrideNumSolverIterations;
  41
  42         int m_numSplitImpulseRecoveries;
  43
  44         b3Scalar getContactProcessingThreshold(b3Contact4* contact)
  45         {
  46                 return 0.02f;
  47         }
  48         void setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
  49                                                                  b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
  50                                                                  b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
  51                                                                  b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
  52
  53         void setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
  54                                                                                 b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
  55                                                                                 b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
  56                                                                                 b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
  57
  58         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.);
  59         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);
  60
  61         void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
  62                                                                 b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
  63                                                                 const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
  64                                                                 b3Vector3& rel_pos1, b3Vector3& rel_pos2);
  65
  66         void setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB,
  67                                                                           b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
  68
  69         ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
  70         unsigned long m_btSeed2;
  71
  72         b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
  73
  74         void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);
  75
  76         void resolveSplitPenetrationSIMD(
  77                 b3SolverBody& bodyA, b3SolverBody& bodyB,
  78                 const b3SolverConstraint& contactConstraint);
  79
  80         void resolveSplitPenetrationImpulseCacheFriendly(
  81                 b3SolverBody& bodyA, b3SolverBody& bodyB,
  82                 const b3SolverConstraint& contactConstraint);
  83
  84         //internal method
  85         int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias);
  86         void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject);
  87
  88         void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
  89
  90         void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
  91
  92         void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
  93
  94         void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
  95
  96 protected:
  97         virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
  98
  99         virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 100         virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 101         b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 102
 103         virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal);
 104
 105 public:
 106         B3_DECLARE_ALIGNED_ALLOCATOR();
 107
 108         b3PgsJacobiSolver(bool usePgs);
 109         virtual ~b3PgsJacobiSolver();
 110
 111         //      void    solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts);
 112         void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
 113
 114         b3Scalar solveGroup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
 115
 116         ///clear internal cached data and reset random seed
 117         virtual void reset();
 118
 119         unsigned long b3Rand2();
 120
 121         int b3RandInt2(int n);
 122
 123         void setRandSeed(unsigned long seed)
 124         {
 125                 m_btSeed2 = seed;
 126         }
 127         unsigned long getRandSeed() const
 128         {
 129                 return m_btSeed2;
 130         }
 131 };
 132
 133 #endif  //B3_PGS_JACOBI_SOLVER