45 for (
int j = 0; j < numNonContactPool; ++j)
56 for (
int j = 0; j < numConstraintPool; ++j)
64 for (
int j = 0; j < numFrictionPool; ++j)
80 if (iteration < constraint.m_overrideNumSolverIterations)
84 deltaflengthsqr += deltaf * deltaf;
98 btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
108 if (iteration < constraint.m_overrideNumSolverIterations)
111 constraint.m_appliedImpulse =
btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
113 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
114 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
116 body1.internalApplyImpulse(
c.m_contactNormal1 * body1.internalGetInvMass(),
c.m_angularComponentA, additionaldeltaimpulse);
117 body2.internalApplyImpulse(
c.m_contactNormal2 * body2.internalGetInvMass(),
c.m_angularComponentB, additionaldeltaimpulse);
122 m_deltafLengthSqrPrev = deltaflengthsqr;
146 for (
int c = 0;
c < numPoolConstraints;
c++)
154 deltaflengthsqr += deltaf * deltaf;
155 totalImpulse = solveManifold.m_appliedImpulse;
157 bool applyFriction =
true;
165 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
166 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
169 deltaflengthsqr += deltaf * deltaf;
183 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
184 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
187 deltaflengthsqr += deltaf * deltaf;
203 for (j = 0; j < numPoolConstraints; j++)
208 deltaflengthsqr += deltaf * deltaf;
214 for (j = 0; j < numFrictionPoolConstraints; j++)
221 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
222 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
226 deltaflengthsqr += deltaf * deltaf;
237 for (
int j = 0; j < numRollingFrictionPoolConstraints; j++)
243 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
244 if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
245 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
247 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
248 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
250 btScalar deltaf =
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
252 deltaflengthsqr += deltaf * deltaf;
275 btScalar beta = m_deltafLengthSqrPrev > 0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
288 if (iteration < constraint.m_overrideNumSolverIterations)
291 constraint.m_appliedImpulse =
btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
293 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
294 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
296 body1.internalApplyImpulse(
c.m_contactNormal1 * body1.internalGetInvMass(),
c.m_angularComponentA, additionaldeltaimpulse);
297 body2.internalApplyImpulse(
c.m_contactNormal2 * body2.internalGetInvMass(),
c.m_angularComponentB, additionaldeltaimpulse);
306 constraint.m_appliedImpulse =
btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
308 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
309 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
311 body1.internalApplyImpulse(
c.m_contactNormal1 * body1.internalGetInvMass(),
c.m_angularComponentA, additionaldeltaimpulse);
312 body2.internalApplyImpulse(
c.m_contactNormal2 * body2.internalGetInvMass(),
c.m_angularComponentB, additionaldeltaimpulse);
321 constraint.m_appliedImpulse =
btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
323 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
324 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
326 body1.internalApplyImpulse(
c.m_contactNormal1 * body1.internalGetInvMass(),
c.m_angularComponentA, additionaldeltaimpulse);
327 body2.internalApplyImpulse(
c.m_contactNormal2 * body2.internalGetInvMass(),
c.m_angularComponentB, additionaldeltaimpulse);
337 constraint.m_appliedImpulse =
btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
339 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
340 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
342 body1.internalApplyImpulse(
c.m_contactNormal1 * body1.internalGetInvMass(),
c.m_angularComponentA, additionaldeltaimpulse);
343 body2.internalApplyImpulse(
c.m_contactNormal2 * body2.internalGetInvMass(),
c.m_angularComponentB, additionaldeltaimpulse);
349 m_deltafLengthSqrPrev = deltaflengthsqr;
352 return deltaflengthsqr;
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
bool m_onlyForNoneContact
btAlignedObjectArray< btScalar > m_deltafNC
btAlignedObjectArray< btScalar > m_pCRF
btAlignedObjectArray< btScalar > m_deltafC
btAlignedObjectArray< btScalar > m_deltafCRF
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< btScalar > m_deltafCF
btAlignedObjectArray< btScalar > m_pC
btAlignedObjectArray< btScalar > m_pNC
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btAlignedObjectArray< btScalar > m_pCF
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint ** constraints
btSequentialImpulseConstraintSolverMt int btPersistentManifold ** manifoldPtr
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
btSequentialImpulseConstraintSolverMt int numBodies
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool
btAlignedObjectArray< int > m_orderTmpConstraintPool
btAlignedObjectArray< int > m_orderNonContactConstraintPool
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btAlignedObjectArray< int > m_orderFrictionConstraintPool
btConstraintArray m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btConstraintArray m_tmpSolverNonContactConstraintPool
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
ccl_device_inline float beta(float x, float y)