2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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:
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.
16 #ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
17 #define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
20 class btPersistentManifold;
22 class btCollisionObject;
23 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
24 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
25 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
26 #include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
27 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
28 #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
30 typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
32 struct btSolverAnalyticsData
34 btSolverAnalyticsData()
37 m_numIterationsUsed = -1;
38 m_remainingLeastSquaresResidual = -1;
43 int m_numContactManifolds;
45 int m_numIterationsUsed;
46 double m_remainingLeastSquaresResidual;
49 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
50 ATTRIBUTE_ALIGNED16(class)
51 btSequentialImpulseConstraintSolver : public btConstraintSolver
56 btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
57 btConstraintArray m_tmpSolverContactConstraintPool;
58 btConstraintArray m_tmpSolverNonContactConstraintPool;
59 btConstraintArray m_tmpSolverContactFrictionConstraintPool;
60 btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
62 btAlignedObjectArray<int> m_orderTmpConstraintPool;
63 btAlignedObjectArray<int> m_orderNonContactConstraintPool;
64 btAlignedObjectArray<int> m_orderFrictionConstraintPool;
65 btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
66 int m_maxOverrideNumSolverIterations;
68 // When running solvers on multiple threads, a race condition exists for Kinematic objects that
69 // participate in more than one solver.
70 // The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body
71 // for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island
72 // (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once.
73 // To avoid this race condition, this solver does not write the companionId, instead it stores the solver body
74 // index in this solver-local table, indexed by the uniqueId of the body.
75 btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading
77 btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
78 btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
79 btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
80 int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed
81 void setupSolverFunctions(bool useSimd);
83 btScalar m_leastSquaresResidual;
85 void setupFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
86 btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2,
87 btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
88 const btContactSolverInfo& infoGlobal,
89 btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
91 void setupTorsionalFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
92 btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
93 btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
94 btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
96 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.);
97 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);
99 void setupContactConstraint(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
100 const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
102 static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode);
104 void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB,
105 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
107 ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
108 unsigned long m_btSeed2;
110 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
112 virtual void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
114 void convertContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
116 virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
117 void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
119 virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
121 btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
123 return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
126 btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
128 return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
132 int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
133 void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
135 btScalar resolveSingleConstraintRowGeneric(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
136 btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
137 btScalar resolveSingleConstraintRowLowerLimit(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
138 btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint);
139 btScalar resolveSplitPenetrationImpulse(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
141 return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
145 void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
146 void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
147 void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
148 virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
149 virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
150 virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
152 virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
153 virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
156 BT_DECLARE_ALIGNED_ALLOCATOR();
158 btSequentialImpulseConstraintSolver();
159 virtual ~btSequentialImpulseConstraintSolver();
161 virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
163 ///clear internal cached data and reset random seed
164 virtual void reset();
166 unsigned long btRand2();
168 int btRandInt2(int n);
170 void setRandSeed(unsigned long seed)
174 unsigned long getRandSeed() const
179 virtual btConstraintSolverType getSolverType() const
181 return BT_SEQUENTIAL_IMPULSE_SOLVER;
184 btSingleConstraintRowSolver getActiveConstraintRowSolverGeneric()
186 return m_resolveSingleConstraintRowGeneric;
188 void setConstraintRowSolverGeneric(btSingleConstraintRowSolver rowSolver)
190 m_resolveSingleConstraintRowGeneric = rowSolver;
192 btSingleConstraintRowSolver getActiveConstraintRowSolverLowerLimit()
194 return m_resolveSingleConstraintRowLowerLimit;
196 void setConstraintRowSolverLowerLimit(btSingleConstraintRowSolver rowSolver)
198 m_resolveSingleConstraintRowLowerLimit = rowSolver;
203 ///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
204 btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
205 btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
206 btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
208 ///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
209 btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
210 btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
211 btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
212 btSolverAnalyticsData m_analyticsData;
215 #endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H