dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2018 Google Inc. http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #ifndef BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
  17 #define BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H
  18
  19 #include "LinearMath/btMatrixX.h"
  20 #include "LinearMath/btThreads.h"
  21 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
  22
  23 class btMLCPSolverInterface;
  24 class btMultiBody;
  25
  26 class btMultiBodyMLCPConstraintSolver : public btMultiBodyConstraintSolver
  27 {
  28 protected:
  29         /// \name MLCP Formulation for Rigid Bodies
  30         /// \{
  31
  32         /// A matrix in the MLCP formulation
  33         btMatrixXu m_A;
  34
  35         /// b vector in the MLCP formulation.
  36         btVectorXu m_b;
  37
  38         /// Constraint impulse, which is an output of MLCP solving.
  39         btVectorXu m_x;
  40
  41         /// Lower bound of constraint impulse, \c m_x.
  42         btVectorXu m_lo;
  43
  44         /// Upper bound of constraint impulse, \c m_x.
  45         btVectorXu m_hi;
  46
  47         /// \}
  48
  49         /// \name Cache Variables for Split Impulse for Rigid Bodies
  50         /// When using 'split impulse' we solve two separate (M)LCPs
  51         /// \{
  52
  53         /// Split impulse Cache vector corresponding to \c m_b.
  54         btVectorXu m_bSplit;
  55
  56         /// Split impulse cache vector corresponding to \c m_x.
  57         btVectorXu m_xSplit;
  58
  59         /// \}
  60
  61         /// \name MLCP Formulation for Multibodies
  62         /// \{
  63
  64         /// A matrix in the MLCP formulation
  65         btMatrixXu m_multiBodyA;
  66
  67         /// b vector in the MLCP formulation.
  68         btVectorXu m_multiBodyB;
  69
  70         /// Constraint impulse, which is an output of MLCP solving.
  71         btVectorXu m_multiBodyX;
  72
  73         /// Lower bound of constraint impulse, \c m_x.
  74         btVectorXu m_multiBodyLo;
  75
  76         /// Upper bound of constraint impulse, \c m_x.
  77         btVectorXu m_multiBodyHi;
  78
  79         /// \}
  80
  81         /// Indices of normal contact constraint associated with frictional contact constraint for rigid bodies.
  82         ///
  83         /// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
  84         /// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
  85         /// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
  86         /// Otherwise, -1.
  87         btAlignedObjectArray<int> m_limitDependencies;
  88
  89         /// Indices of normal contact constraint associated with frictional contact constraint for multibodies.
  90         ///
  91         /// This is used by the MLCP solver to update the upper bounds of frictional contact impulse given intermediate
  92         /// normal contact impulse. For example, i-th element represents the index of a normal constraint that is
  93         /// accosiated with i-th frictional contact constraint if i-th constraint is a frictional contact constraint.
  94         /// Otherwise, -1.
  95         btAlignedObjectArray<int> m_multiBodyLimitDependencies;
  96
  97         /// Array of all the rigid body constraints
  98         btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
  99
 100         /// Array of all the multibody constraints
 101         btAlignedObjectArray<btMultiBodySolverConstraint*> m_multiBodyAllConstraintPtrArray;
 102
 103         /// MLCP solver
 104         btMLCPSolverInterface* m_solver;
 105
 106         /// Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver fails.
 107         int m_fallback;
 108
 109         /// \name MLCP Scratch Variables
 110         /// The following scratch variables are not stateful -- contents are cleared prior to each use.
 111         /// They are only cached here to avoid extra memory allocations and deallocations and to ensure
 112         /// that multiple instances of the solver can be run in parallel.
 113         ///
 114         /// \{
 115
 116         /// Cache variable for constraint Jacobian matrix.
 117         btMatrixXu m_scratchJ3;
 118
 119         /// Cache variable for constraint Jacobian times inverse mass matrix.
 120         btMatrixXu m_scratchJInvM3;
 121
 122         /// Cache variable for offsets.
 123         btAlignedObjectArray<int> m_scratchOfs;
 124
 125         /// \}
 126
 127         /// Constructs MLCP terms, which are \c m_A, \c m_b, \c m_lo, and \c m_hi.
 128         virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
 129
 130         /// Constructs MLCP terms for constraints of two rigid bodies
 131         void createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal);
 132
 133         /// Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody
 134         void createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal);
 135
 136         /// Solves MLCP and returns the success
 137         virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
 138
 139         // Documentation inherited
 140         btScalar solveGroupCacheFriendlySetup(
 141                 btCollisionObject** bodies,
 142                 int numBodies,
 143                 btPersistentManifold** manifoldPtr,
 144                 int numManifolds,
 145                 btTypedConstraint** constraints,
 146                 int numConstraints,
 147                 const btContactSolverInfo& infoGlobal,
 148                 btIDebugDraw* debugDrawer) BT_OVERRIDE;
 149
 150         // Documentation inherited
 151         btScalar solveGroupCacheFriendlyIterations(
 152                 btCollisionObject** bodies,
 153                 int numBodies,
 154                 btPersistentManifold** manifoldPtr,
 155                 int numManifolds,
 156                 btTypedConstraint** constraints,
 157                 int numConstraints,
 158                 const btContactSolverInfo& infoGlobal,
 159                 btIDebugDraw* debugDrawer) ;
 160
 161 public:
 162         BT_DECLARE_ALIGNED_ALLOCATOR()
 163
 164         /// Constructor
 165         ///
 166         /// \param[in] solver MLCP solver. Assumed it's not null.
 167         /// \param[in] maxLCPSize Maximum size of LCP to solve using MLCP solver. If the MLCP size exceeds this number, sequaltial impulse method will be used.
 168         explicit btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver);
 169
 170         /// Destructor
 171         virtual ~btMultiBodyMLCPConstraintSolver();
 172
 173         /// Sets MLCP solver. Assumed it's not null.
 174         void setMLCPSolver(btMLCPSolverInterface* solver);
 175
 176         /// Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP
 177         /// solver fails.
 178         int getNumFallbacks() const;
 179
 180         /// Sets the number of fallbacks. This function may be used to reset the number to zero.
 181         void setNumFallbacks(int num);
 182
 183         /// Returns the constraint solver type.
 184         virtual btConstraintSolverType getSolverType() const;
 185 };
 186
 187 #endif  // BT_MULTIBODY_MLCP_CONSTRAINT_SOLVER_H