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 188