Bullet Collision Detection & Physics Library
btSequentialImpulseConstraintSolver.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 //#define COMPUTE_IMPULSE_DENOM 1
17 //#define BT_ADDITIONAL_DEBUG
18 
19 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
20 
21 #include "btSequentialImpulseConstraintSolver.h"
22 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
23 
24 #include "LinearMath/btIDebugDraw.h"
25 //#include "btJacobianEntry.h"
26 #include "LinearMath/btMinMax.h"
27 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
28 #include <new>
29 #include "LinearMath/btStackAlloc.h"
30 #include "LinearMath/btQuickprof.h"
31 //#include "btSolverBody.h"
32 //#include "btSolverConstraint.h"
33 #include "LinearMath/btAlignedObjectArray.h"
34 #include <string.h> //for memset
35 
36 int gNumSplitImpulseRecoveries = 0;
37 
38 #include "BulletDynamics/Dynamics/btRigidBody.h"
39 
40 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
41 :m_btSeed2(0)
42 {
43 
44 }
45 
46 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
47 {
48 }
49 
50 #ifdef USE_SIMD
51 #include <emmintrin.h>
52 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
53 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
54 {
55  __m128 result = _mm_mul_ps( vec0, vec1);
56  return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
57 }
58 #endif//USE_SIMD
59 
60 // Project Gauss Seidel or the equivalent Sequential Impulse
61 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
62 {
63 #ifdef USE_SIMD
64  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
65  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
66  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
67  __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
68  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
69  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
70  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
71  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
72  btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
73  btSimdScalar resultLowerLess,resultUpperLess;
74  resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
75  resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
76  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
77  deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
78  c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
79  __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
80  deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
81  c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
82  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
83  __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128);
84  __m128 impulseMagnitude = deltaImpulse;
85  body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
86  body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
87  body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
88  body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
89 #else
90  resolveSingleConstraintRowGeneric(body1,body2,c);
91 #endif
92 }
93 
94 // Project Gauss Seidel or the equivalent Sequential Impulse
95  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
96 {
97  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
98  const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
99  const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
100 
101 // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
102  deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
103  deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
104 
105  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
106  if (sum < c.m_lowerLimit)
107  {
108  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
109  c.m_appliedImpulse = c.m_lowerLimit;
110  }
111  else if (sum > c.m_upperLimit)
112  {
113  deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
114  c.m_appliedImpulse = c.m_upperLimit;
115  }
116  else
117  {
118  c.m_appliedImpulse = sum;
119  }
120 
121  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
122  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
123 }
124 
125  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
126 {
127 #ifdef USE_SIMD
128  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
129  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
130  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
131  __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
132  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
133  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
134  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
135  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
136  btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
137  btSimdScalar resultLowerLess,resultUpperLess;
138  resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
139  resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
140  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
141  deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
142  c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
143  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
144  __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
145  __m128 impulseMagnitude = deltaImpulse;
146  body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
147  body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
148  body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
149  body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
150 #else
151  resolveSingleConstraintRowLowerLimit(body1,body2,c);
152 #endif
153 }
154 
155 // Projected Gauss Seidel or the equivalent Sequential Impulse
156  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
157 {
158  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
159  const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
160  const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
161 
162  deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
163  deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
164  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
165  if (sum < c.m_lowerLimit)
166  {
167  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
168  c.m_appliedImpulse = c.m_lowerLimit;
169  }
170  else
171  {
172  c.m_appliedImpulse = sum;
173  }
174  body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
175  body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
176 }
177 
178 
179 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
180  btSolverBody& body1,
181  btSolverBody& body2,
182  const btSolverConstraint& c)
183 {
184  if (c.m_rhsPenetration)
185  {
186  gNumSplitImpulseRecoveries++;
187  btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
188  const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
189  const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
190 
191  deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
192  deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
193  const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
194  if (sum < c.m_lowerLimit)
195  {
196  deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
197  c.m_appliedPushImpulse = c.m_lowerLimit;
198  }
199  else
200  {
201  c.m_appliedPushImpulse = sum;
202  }
203  body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
204  body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
205  }
206 }
207 
208  void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
209 {
210 #ifdef USE_SIMD
211  if (!c.m_rhsPenetration)
212  return;
213 
214  gNumSplitImpulseRecoveries++;
215 
216  __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
217  __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
218  __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
219  __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
220  __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
221  __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
222  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
223  deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
224  btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
225  btSimdScalar resultLowerLess,resultUpperLess;
226  resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
227  resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
228  __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
229  deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
230  c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
231  __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
232  __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
233  __m128 impulseMagnitude = deltaImpulse;
234  body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
235  body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
236  body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
237  body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
238 #else
239  resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
240 #endif
241 }
242 
243 
244 
245 unsigned long btSequentialImpulseConstraintSolver::btRand2()
246 {
247  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
248  return m_btSeed2;
249 }
250 
251 
252 
253 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
254 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
255 {
256  // seems good; xor-fold and modulus
257  const unsigned long un = static_cast<unsigned long>(n);
258  unsigned long r = btRand2();
259 
260  // note: probably more aggressive than it needs to be -- might be
261  // able to get away without one or two of the innermost branches.
262  if (un <= 0x00010000UL) {
263  r ^= (r >> 16);
264  if (un <= 0x00000100UL) {
265  r ^= (r >> 8);
266  if (un <= 0x00000010UL) {
267  r ^= (r >> 4);
268  if (un <= 0x00000004UL) {
269  r ^= (r >> 2);
270  if (un <= 0x00000002UL) {
271  r ^= (r >> 1);
272  }
273  }
274  }
275  }
276  }
277 
278  return (int) (r % un);
279 }
280 
281 
282 
283 void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
284 {
285 
286  btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
287 
288  solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
289  solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
290  solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
291  solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
292 
293  if (rb)
294  {
295  solverBody->m_worldTransform = rb->getWorldTransform();
296  solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
297  solverBody->m_originalBody = rb;
298  solverBody->m_angularFactor = rb->getAngularFactor();
299  solverBody->m_linearFactor = rb->getLinearFactor();
300  solverBody->m_linearVelocity = rb->getLinearVelocity();
301  solverBody->m_angularVelocity = rb->getAngularVelocity();
302  solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
303  solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
304 
305  } else
306  {
307  solverBody->m_worldTransform.setIdentity();
308  solverBody->internalSetInvMass(btVector3(0,0,0));
309  solverBody->m_originalBody = 0;
310  solverBody->m_angularFactor.setValue(1,1,1);
311  solverBody->m_linearFactor.setValue(1,1,1);
312  solverBody->m_linearVelocity.setValue(0,0,0);
313  solverBody->m_angularVelocity.setValue(0,0,0);
314  solverBody->m_externalForceImpulse.setValue(0,0,0);
315  solverBody->m_externalTorqueImpulse.setValue(0,0,0);
316  }
317 
318 
319 }
320 
321 
322 
323 
324 
325 
326 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
327 {
328  btScalar rest = restitution * -rel_vel;
329  return rest;
330 }
331 
332 
333 
334 void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
335 {
336 
337 
338  if (colObj && colObj->hasAnisotropicFriction(frictionMode))
339  {
340  // transform to local coordinates
341  btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
342  const btVector3& friction_scaling = colObj->getAnisotropicFriction();
343  //apply anisotropic friction
344  loc_lateral *= friction_scaling;
345  // ... and transform it back to global coordinates
346  frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
347  }
348 
349 }
350 
351 
352 
353 
354 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
355 {
356 
357 
358  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
359  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
360 
361  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
362  btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
363 
364  solverConstraint.m_solverBodyIdA = solverBodyIdA;
365  solverConstraint.m_solverBodyIdB = solverBodyIdB;
366 
367  solverConstraint.m_friction = cp.m_combinedFriction;
368  solverConstraint.m_originalContactPoint = 0;
369 
370  solverConstraint.m_appliedImpulse = 0.f;
371  solverConstraint.m_appliedPushImpulse = 0.f;
372 
373  if (body0)
374  {
375  solverConstraint.m_contactNormal1 = normalAxis;
376  btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
377  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
378  solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
379  }else
380  {
381  solverConstraint.m_contactNormal1.setZero();
382  solverConstraint.m_relpos1CrossNormal.setZero();
383  solverConstraint.m_angularComponentA .setZero();
384  }
385 
386  if (body1)
387  {
388  solverConstraint.m_contactNormal2 = -normalAxis;
389  btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
390  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
391  solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
392  } else
393  {
394  solverConstraint.m_contactNormal2.setZero();
395  solverConstraint.m_relpos2CrossNormal.setZero();
396  solverConstraint.m_angularComponentB.setZero();
397  }
398 
399  {
400  btVector3 vec;
401  btScalar denom0 = 0.f;
402  btScalar denom1 = 0.f;
403  if (body0)
404  {
405  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
406  denom0 = body0->getInvMass() + normalAxis.dot(vec);
407  }
408  if (body1)
409  {
410  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
411  denom1 = body1->getInvMass() + normalAxis.dot(vec);
412  }
413  btScalar denom = relaxation/(denom0+denom1);
414  solverConstraint.m_jacDiagABInv = denom;
415  }
416 
417  {
418 
419 
420  btScalar rel_vel;
421  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
422  + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
423  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
424  + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
425 
426  rel_vel = vel1Dotn+vel2Dotn;
427 
428 // btScalar positionalError = 0.f;
429 
430  btSimdScalar velocityError = desiredVelocity - rel_vel;
431  btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
432  solverConstraint.m_rhs = velocityImpulse;
433  solverConstraint.m_cfm = cfmSlip;
434  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
435  solverConstraint.m_upperLimit = solverConstraint.m_friction;
436 
437  }
438 }
439 
440 btSolverConstraint& btSequentialImpulseConstraintSolver::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, btScalar desiredVelocity, btScalar cfmSlip)
441 {
442  btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
443  solverConstraint.m_frictionIndex = frictionIndex;
444  setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
445  colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
446  return solverConstraint;
447 }
448 
449 
450 void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
451  btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
452  btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
453  btScalar desiredVelocity, btScalar cfmSlip)
454 
455 {
456  btVector3 normalAxis(0,0,0);
457 
458 
459  solverConstraint.m_contactNormal1 = normalAxis;
460  solverConstraint.m_contactNormal2 = -normalAxis;
461  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
462  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
463 
464  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
465  btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
466 
467  solverConstraint.m_solverBodyIdA = solverBodyIdA;
468  solverConstraint.m_solverBodyIdB = solverBodyIdB;
469 
470  solverConstraint.m_friction = cp.m_combinedRollingFriction;
471  solverConstraint.m_originalContactPoint = 0;
472 
473  solverConstraint.m_appliedImpulse = 0.f;
474  solverConstraint.m_appliedPushImpulse = 0.f;
475 
476  {
477  btVector3 ftorqueAxis1 = -normalAxis1;
478  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
479  solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
480  }
481  {
482  btVector3 ftorqueAxis1 = normalAxis1;
483  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
484  solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
485  }
486 
487 
488  {
489  btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
490  btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
491  btScalar sum = 0;
492  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
493  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
494  solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
495  }
496 
497  {
498 
499 
500  btScalar rel_vel;
501  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
502  + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
503  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
504  + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
505 
506  rel_vel = vel1Dotn+vel2Dotn;
507 
508 // btScalar positionalError = 0.f;
509 
510  btSimdScalar velocityError = desiredVelocity - rel_vel;
511  btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
512  solverConstraint.m_rhs = velocityImpulse;
513  solverConstraint.m_cfm = cfmSlip;
514  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
515  solverConstraint.m_upperLimit = solverConstraint.m_friction;
516 
517  }
518 }
519 
520 
521 
522 
523 
524 
525 
526 
527 btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(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, btScalar desiredVelocity, btScalar cfmSlip)
528 {
529  btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
530  solverConstraint.m_frictionIndex = frictionIndex;
531  setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
532  colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
533  return solverConstraint;
534 }
535 
536 
537 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
538 {
539 
540  int solverBodyIdA = -1;
541 
542  if (body.getCompanionId() >= 0)
543  {
544  //body has already been converted
545  solverBodyIdA = body.getCompanionId();
546  btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
547  } else
548  {
549  btRigidBody* rb = btRigidBody::upcast(&body);
550  //convert both active and kinematic objects (for their velocity)
551  if (rb && (rb->getInvMass() || rb->isKinematicObject()))
552  {
553  solverBodyIdA = m_tmpSolverBodyPool.size();
554  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
555  initSolverBody(&solverBody,&body,timeStep);
556  body.setCompanionId(solverBodyIdA);
557  } else
558  {
559 
560  if (m_fixedBodyId<0)
561  {
562  m_fixedBodyId = m_tmpSolverBodyPool.size();
563  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
564  initSolverBody(&fixedBody,0,timeStep);
565  }
566  return m_fixedBodyId;
567 // return 0;//assume first one is a fixed solver body
568  }
569  }
570 
571  return solverBodyIdA;
572 
573 }
574 #include <stdio.h>
575 
576 
577 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
578  int solverBodyIdA, int solverBodyIdB,
579  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
580  btScalar& relaxation,
581  const btVector3& rel_pos1, const btVector3& rel_pos2)
582 {
583 
584  const btVector3& pos1 = cp.getPositionWorldOnA();
585  const btVector3& pos2 = cp.getPositionWorldOnB();
586 
587  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
588  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
589 
590  btRigidBody* rb0 = bodyA->m_originalBody;
591  btRigidBody* rb1 = bodyB->m_originalBody;
592 
593 // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
594 // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
595  //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
596  //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
597 
598  relaxation = 1.f;
599 
600  btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
601  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
602  btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
603  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
604 
605  {
606 #ifdef COMPUTE_IMPULSE_DENOM
607  btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
608  btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
609 #else
610  btVector3 vec;
611  btScalar denom0 = 0.f;
612  btScalar denom1 = 0.f;
613  if (rb0)
614  {
615  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
616  denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
617  }
618  if (rb1)
619  {
620  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
621  denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
622  }
623 #endif //COMPUTE_IMPULSE_DENOM
624 
625  btScalar denom = relaxation/(denom0+denom1);
626  solverConstraint.m_jacDiagABInv = denom;
627  }
628 
629  if (rb0)
630  {
631  solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
632  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
633  } else
634  {
635  solverConstraint.m_contactNormal1.setZero();
636  solverConstraint.m_relpos1CrossNormal.setZero();
637  }
638  if (rb1)
639  {
640  solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
641  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
642  }else
643  {
644  solverConstraint.m_contactNormal2.setZero();
645  solverConstraint.m_relpos2CrossNormal.setZero();
646  }
647 
648  btScalar restitution = 0.f;
649  btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
650 
651  {
652  btVector3 vel1,vel2;
653 
654  vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
655  vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
656 
657  // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
658  btVector3 vel = vel1 - vel2;
659  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
660 
661 
662 
663  solverConstraint.m_friction = cp.m_combinedFriction;
664 
665 
666  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
667  if (restitution <= btScalar(0.))
668  {
669  restitution = 0.f;
670  };
671  }
672 
673 
675  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
676  {
677  solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
678  if (rb0)
679  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
680  if (rb1)
681  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
682  } else
683  {
684  solverConstraint.m_appliedImpulse = 0.f;
685  }
686 
687  solverConstraint.m_appliedPushImpulse = 0.f;
688 
689  {
690 
691  btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
692  btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
693  btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
694  btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
695 
696 
697  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
698  + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
699  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
700  + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
701  btScalar rel_vel = vel1Dotn+vel2Dotn;
702 
703  btScalar positionalError = 0.f;
704  btScalar velocityError = restitution - rel_vel;// * damping;
705 
706 
707  btScalar erp = infoGlobal.m_erp2;
708  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
709  {
710  erp = infoGlobal.m_erp;
711  }
712 
713  if (penetration>0)
714  {
715  positionalError = 0;
716 
717  velocityError -= penetration / infoGlobal.m_timeStep;
718  } else
719  {
720  positionalError = -penetration * erp/infoGlobal.m_timeStep;
721  }
722 
723  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
724  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
725 
726  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
727  {
728  //combine position and velocity into rhs
729  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
730  solverConstraint.m_rhsPenetration = 0.f;
731 
732  } else
733  {
734  //split position and velocity into rhs and m_rhsPenetration
735  solverConstraint.m_rhs = velocityImpulse;
736  solverConstraint.m_rhsPenetration = penetrationImpulse;
737  }
738  solverConstraint.m_cfm = 0.f;
739  solverConstraint.m_lowerLimit = 0;
740  solverConstraint.m_upperLimit = 1e10f;
741  }
742 
743 
744 
745 
746 }
747 
748 
749 
750 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
751  int solverBodyIdA, int solverBodyIdB,
752  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
753 {
754 
755  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
756  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
757 
758  btRigidBody* rb0 = bodyA->m_originalBody;
759  btRigidBody* rb1 = bodyB->m_originalBody;
760 
761  {
762  btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
763  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
764  {
765  frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
766  if (rb0)
767  bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
768  if (rb1)
769  bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
770  } else
771  {
772  frictionConstraint1.m_appliedImpulse = 0.f;
773  }
774  }
775 
776  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
777  {
778  btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
779  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
780  {
781  frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
782  if (rb0)
783  bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
784  if (rb1)
785  bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
786  } else
787  {
788  frictionConstraint2.m_appliedImpulse = 0.f;
789  }
790  }
791 }
792 
793 
794 
795 
796 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
797 {
798  btCollisionObject* colObj0=0,*colObj1=0;
799 
800  colObj0 = (btCollisionObject*)manifold->getBody0();
801  colObj1 = (btCollisionObject*)manifold->getBody1();
802 
803  int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
804  int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
805 
806 // btRigidBody* bodyA = btRigidBody::upcast(colObj0);
807 // btRigidBody* bodyB = btRigidBody::upcast(colObj1);
808 
809  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
810  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
811 
812 
813 
815  if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
816  return;
817 
818  int rollingFriction=1;
819  for (int j=0;j<manifold->getNumContacts();j++)
820  {
821 
822  btManifoldPoint& cp = manifold->getContactPoint(j);
823 
824  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
825  {
826  btVector3 rel_pos1;
827  btVector3 rel_pos2;
828  btScalar relaxation;
829 
830 
831  int frictionIndex = m_tmpSolverContactConstraintPool.size();
832  btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
833  btRigidBody* rb0 = btRigidBody::upcast(colObj0);
834  btRigidBody* rb1 = btRigidBody::upcast(colObj1);
835  solverConstraint.m_solverBodyIdA = solverBodyIdA;
836  solverConstraint.m_solverBodyIdB = solverBodyIdB;
837 
838  solverConstraint.m_originalContactPoint = &cp;
839 
840  const btVector3& pos1 = cp.getPositionWorldOnA();
841  const btVector3& pos2 = cp.getPositionWorldOnB();
842 
843  rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
844  rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
845 
846  btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
847  btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
848 
849  solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
850  solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
851 
852  btVector3 vel = vel1 - vel2;
853  btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
854 
855  setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
856 
857 
858 
859 // const btVector3& pos1 = cp.getPositionWorldOnA();
860 // const btVector3& pos2 = cp.getPositionWorldOnB();
861 
863 
864  solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
865 
866  btVector3 angVelA(0,0,0),angVelB(0,0,0);
867  if (rb0)
868  angVelA = rb0->getAngularVelocity();
869  if (rb1)
870  angVelB = rb1->getAngularVelocity();
871  btVector3 relAngVel = angVelB-angVelA;
872 
873  if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
874  {
875  //only a single rollingFriction per manifold
876  rollingFriction--;
877  if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
878  {
879  relAngVel.normalize();
880  applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
881  applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
882  if (relAngVel.length()>0.001)
883  addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
884 
885  } else
886  {
887  addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
888  btVector3 axis0,axis1;
889  btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
890  applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
891  applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
892  applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
893  applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
894  if (axis0.length()>0.001)
895  addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
896  if (axis1.length()>0.001)
897  addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
898 
899  }
900  }
901 
906 
917  if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
918  {
919  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
920  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
921  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
922  {
923  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
924  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
925  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
926  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
927 
928  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
929  {
930  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
931  cp.m_lateralFrictionDir2.normalize();//??
932  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
933  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
934  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
935  }
936 
937  } else
938  {
939  btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
940 
941  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
942  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
943  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
944 
945  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
946  {
947  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
948  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
949  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
950  }
951 
952 
953  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
954  {
955  cp.m_lateralFrictionInitialized = true;
956  }
957  }
958 
959  } else
960  {
961  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
962 
963  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
964  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
965 
966  }
967  setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
968 
969 
970 
971 
972  }
973  }
974 }
975 
976 void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
977 {
978  int i;
979  btPersistentManifold* manifold = 0;
980 // btCollisionObject* colObj0=0,*colObj1=0;
981 
982 
983  for (i=0;i<numManifolds;i++)
984  {
985  manifold = manifoldPtr[i];
986  convertContact(manifold,infoGlobal);
987  }
988 }
989 
990 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
991 {
992  m_fixedBodyId = -1;
993  BT_PROFILE("solveGroupCacheFriendlySetup");
994  (void)debugDrawer;
995 
996  m_maxOverrideNumSolverIterations = 0;
997 
998 #ifdef BT_ADDITIONAL_DEBUG
999  //make sure that dynamic bodies exist for all (enabled) constraints
1000  for (int i=0;i<numConstraints;i++)
1001  {
1002  btTypedConstraint* constraint = constraints[i];
1003  if (constraint->isEnabled())
1004  {
1005  if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1006  {
1007  bool found=false;
1008  for (int b=0;b<numBodies;b++)
1009  {
1010 
1011  if (&constraint->getRigidBodyA()==bodies[b])
1012  {
1013  found = true;
1014  break;
1015  }
1016  }
1017  btAssert(found);
1018  }
1019  if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1020  {
1021  bool found=false;
1022  for (int b=0;b<numBodies;b++)
1023  {
1024  if (&constraint->getRigidBodyB()==bodies[b])
1025  {
1026  found = true;
1027  break;
1028  }
1029  }
1030  btAssert(found);
1031  }
1032  }
1033  }
1034  //make sure that dynamic bodies exist for all contact manifolds
1035  for (int i=0;i<numManifolds;i++)
1036  {
1037  if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
1038  {
1039  bool found=false;
1040  for (int b=0;b<numBodies;b++)
1041  {
1042 
1043  if (manifoldPtr[i]->getBody0()==bodies[b])
1044  {
1045  found = true;
1046  break;
1047  }
1048  }
1049  btAssert(found);
1050  }
1051  if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
1052  {
1053  bool found=false;
1054  for (int b=0;b<numBodies;b++)
1055  {
1056  if (manifoldPtr[i]->getBody1()==bodies[b])
1057  {
1058  found = true;
1059  break;
1060  }
1061  }
1062  btAssert(found);
1063  }
1064  }
1065 #endif //BT_ADDITIONAL_DEBUG
1066 
1067 
1068  for (int i = 0; i < numBodies; i++)
1069  {
1070  bodies[i]->setCompanionId(-1);
1071  }
1072 
1073 
1074  m_tmpSolverBodyPool.reserve(numBodies+1);
1075  m_tmpSolverBodyPool.resize(0);
1076 
1077  //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1078  //initSolverBody(&fixedBody,0);
1079 
1080  //convert all bodies
1081 
1082  for (int i=0;i<numBodies;i++)
1083  {
1084  int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
1085 
1086  btRigidBody* body = btRigidBody::upcast(bodies[i]);
1087  if (body && body->getInvMass())
1088  {
1089  btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1090  btVector3 gyroForce (0,0,0);
1091  if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
1092  {
1093  gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
1094  solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1095  }
1096  }
1097  }
1098 
1099  if (1)
1100  {
1101  int j;
1102  for (j=0;j<numConstraints;j++)
1103  {
1104  btTypedConstraint* constraint = constraints[j];
1105  constraint->buildJacobian();
1106  constraint->internalSetAppliedImpulse(0.0f);
1107  }
1108  }
1109 
1110  //btRigidBody* rb0=0,*rb1=0;
1111 
1112  //if (1)
1113  {
1114  {
1115 
1116  int totalNumRows = 0;
1117  int i;
1118 
1119  m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
1120  //calculate the total number of contraint rows
1121  for (i=0;i<numConstraints;i++)
1122  {
1123  btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1124  btJointFeedback* fb = constraints[i]->getJointFeedback();
1125  if (fb)
1126  {
1127  fb->m_appliedForceBodyA.setZero();
1128  fb->m_appliedTorqueBodyA.setZero();
1129  fb->m_appliedForceBodyB.setZero();
1130  fb->m_appliedTorqueBodyB.setZero();
1131  }
1132 
1133  if (constraints[i]->isEnabled())
1134  {
1135  }
1136  if (constraints[i]->isEnabled())
1137  {
1138  constraints[i]->getInfo1(&info1);
1139  } else
1140  {
1141  info1.m_numConstraintRows = 0;
1142  info1.nub = 0;
1143  }
1144  totalNumRows += info1.m_numConstraintRows;
1145  }
1146  m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
1147 
1148 
1150  int currentRow = 0;
1151 
1152  for (i=0;i<numConstraints;i++)
1153  {
1154  const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1155 
1156  if (info1.m_numConstraintRows)
1157  {
1158  btAssert(currentRow<totalNumRows);
1159 
1160  btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1161  btTypedConstraint* constraint = constraints[i];
1162  btRigidBody& rbA = constraint->getRigidBodyA();
1163  btRigidBody& rbB = constraint->getRigidBodyB();
1164 
1165  int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
1166  int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
1167 
1168  btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1169  btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1170 
1171 
1172 
1173 
1174  int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1175  if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
1176  m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1177 
1178 
1179  int j;
1180  for ( j=0;j<info1.m_numConstraintRows;j++)
1181  {
1182  memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1183  currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1184  currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1185  currentConstraintRow[j].m_appliedImpulse = 0.f;
1186  currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1187  currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1188  currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1189  currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1190  }
1191 
1192  bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1193  bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1194  bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1195  bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1196  bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1197  bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1198  bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1199  bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1200 
1201 
1202  btTypedConstraint::btConstraintInfo2 info2;
1203  info2.fps = 1.f/infoGlobal.m_timeStep;
1204  info2.erp = infoGlobal.m_erp;
1205  info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1206  info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1207  info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1208  info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1209  info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1211  btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1212  info2.m_constraintError = &currentConstraintRow->m_rhs;
1213  currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1214  info2.m_damping = infoGlobal.m_damping;
1215  info2.cfm = &currentConstraintRow->m_cfm;
1216  info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1217  info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1218  info2.m_numIterations = infoGlobal.m_numIterations;
1219  constraints[i]->getInfo2(&info2);
1220 
1222  for ( j=0;j<info1.m_numConstraintRows;j++)
1223  {
1224  btSolverConstraint& solverConstraint = currentConstraintRow[j];
1225 
1226  if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
1227  {
1228  solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
1229  }
1230 
1231  if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
1232  {
1233  solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
1234  }
1235 
1236  solverConstraint.m_originalContactPoint = constraint;
1237 
1238  {
1239  const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1240  solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1241  }
1242  {
1243  const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1244  solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1245  }
1246 
1247  {
1248  btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
1249  btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1250  btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
1251  btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1252 
1253  btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1254  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1255  sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1256  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1257  btScalar fsum = btFabs(sum);
1258  btAssert(fsum > SIMD_EPSILON);
1259  solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
1260  }
1261 
1262 
1263 
1264  {
1265  btScalar rel_vel;
1266  btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
1267  btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1268 
1269  btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
1270  btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1271 
1272  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
1273  + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
1274 
1275  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
1276  + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
1277 
1278  rel_vel = vel1Dotn+vel2Dotn;
1279  btScalar restitution = 0.f;
1280  btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1281  btScalar velocityError = restitution - rel_vel * info2.m_damping;
1282  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1283  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1284  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1285  solverConstraint.m_appliedImpulse = 0.f;
1286 
1287 
1288  }
1289  }
1290  }
1291  currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1292  }
1293  }
1294 
1295  convertContacts(manifoldPtr,numManifolds,infoGlobal);
1296 
1297  }
1298 
1299 // btContactSolverInfo info = infoGlobal;
1300 
1301 
1302  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1303  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1304  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1305 
1307  m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
1308  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1309  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
1310  else
1311  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1312 
1313  m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
1314  {
1315  int i;
1316  for (i=0;i<numNonContactPool;i++)
1317  {
1318  m_orderNonContactConstraintPool[i] = i;
1319  }
1320  for (i=0;i<numConstraintPool;i++)
1321  {
1322  m_orderTmpConstraintPool[i] = i;
1323  }
1324  for (i=0;i<numFrictionPool;i++)
1325  {
1326  m_orderFrictionConstraintPool[i] = i;
1327  }
1328  }
1329 
1330  return 0.f;
1331 
1332 }
1333 
1334 
1335 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
1336 {
1337 
1338  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1339  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1340  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1341 
1342  if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1343  {
1344  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1345  {
1346 
1347  for (int j=0; j<numNonContactPool; ++j) {
1348  int tmp = m_orderNonContactConstraintPool[j];
1349  int swapi = btRandInt2(j+1);
1350  m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
1351  m_orderNonContactConstraintPool[swapi] = tmp;
1352  }
1353 
1354  //contact/friction constraints are not solved more than
1355  if (iteration< infoGlobal.m_numIterations)
1356  {
1357  for (int j=0; j<numConstraintPool; ++j) {
1358  int tmp = m_orderTmpConstraintPool[j];
1359  int swapi = btRandInt2(j+1);
1360  m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
1361  m_orderTmpConstraintPool[swapi] = tmp;
1362  }
1363 
1364  for (int j=0; j<numFrictionPool; ++j) {
1365  int tmp = m_orderFrictionConstraintPool[j];
1366  int swapi = btRandInt2(j+1);
1367  m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
1368  m_orderFrictionConstraintPool[swapi] = tmp;
1369  }
1370  }
1371  }
1372  }
1373 
1374  if (infoGlobal.m_solverMode & SOLVER_SIMD)
1375  {
1377  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1378  {
1379  btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1380  if (iteration < constraint.m_overrideNumSolverIterations)
1381  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1382  }
1383 
1384  if (iteration< infoGlobal.m_numIterations)
1385  {
1386  for (int j=0;j<numConstraints;j++)
1387  {
1388  if (constraints[j]->isEnabled())
1389  {
1390  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1391  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1392  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1393  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1394  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1395  }
1396  }
1397 
1399  if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
1400  {
1401  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1402  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
1403 
1404  for (int c=0;c<numPoolConstraints;c++)
1405  {
1406  btScalar totalImpulse =0;
1407 
1408  {
1409  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
1410  resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1411  totalImpulse = solveManifold.m_appliedImpulse;
1412  }
1413  bool applyFriction = true;
1414  if (applyFriction)
1415  {
1416  {
1417 
1418  btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
1419 
1420  if (totalImpulse>btScalar(0))
1421  {
1422  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1423  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1424 
1425  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1426  }
1427  }
1428 
1429  if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
1430  {
1431 
1432  btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
1433 
1434  if (totalImpulse>btScalar(0))
1435  {
1436  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1437  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1438 
1439  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1440  }
1441  }
1442  }
1443  }
1444 
1445  }
1446  else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1447  {
1448  //solve the friction constraints after all contact constraints, don't interleave them
1449  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1450  int j;
1451 
1452  for (j=0;j<numPoolConstraints;j++)
1453  {
1454  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1455  //resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1456  resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1457 
1458  }
1459 
1460 
1461 
1463 
1464  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1465  for (j=0;j<numFrictionPoolConstraints;j++)
1466  {
1467  btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1468  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1469 
1470  if (totalImpulse>btScalar(0))
1471  {
1472  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1473  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1474 
1475  //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1476  resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1477  }
1478  }
1479 
1480 
1481  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1482  for (j=0;j<numRollingFrictionPoolConstraints;j++)
1483  {
1484 
1485  btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1486  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1487  if (totalImpulse>btScalar(0))
1488  {
1489  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1490  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1491  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1492 
1493  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1494  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1495 
1496  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1497  }
1498  }
1499 
1500 
1501  }
1502  }
1503  } else
1504  {
1505  //non-SIMD version
1507  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1508  {
1509  btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1510  if (iteration < constraint.m_overrideNumSolverIterations)
1511  resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1512  }
1513 
1514  if (iteration< infoGlobal.m_numIterations)
1515  {
1516  for (int j=0;j<numConstraints;j++)
1517  {
1518  if (constraints[j]->isEnabled())
1519  {
1520  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1521  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1522  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1523  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1524  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1525  }
1526  }
1528  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1529  for (int j=0;j<numPoolConstraints;j++)
1530  {
1531  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1532  resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1533  }
1535  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1536  for (int j=0;j<numFrictionPoolConstraints;j++)
1537  {
1538  btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1539  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1540 
1541  if (totalImpulse>btScalar(0))
1542  {
1543  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1544  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1545 
1546  resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1547  }
1548  }
1549 
1550  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1551  for (int j=0;j<numRollingFrictionPoolConstraints;j++)
1552  {
1553  btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1554  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1555  if (totalImpulse>btScalar(0))
1556  {
1557  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1558  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1559  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1560 
1561  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1562  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1563 
1564  resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1565  }
1566  }
1567  }
1568  }
1569  return 0.f;
1570 }
1571 
1572 
1573 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1574 {
1575  int iteration;
1576  if (infoGlobal.m_splitImpulse)
1577  {
1578  if (infoGlobal.m_solverMode & SOLVER_SIMD)
1579  {
1580  for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1581  {
1582  {
1583  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1584  int j;
1585  for (j=0;j<numPoolConstraints;j++)
1586  {
1587  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1588 
1589  resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1590  }
1591  }
1592  }
1593  }
1594  else
1595  {
1596  for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1597  {
1598  {
1599  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1600  int j;
1601  for (j=0;j<numPoolConstraints;j++)
1602  {
1603  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1604 
1605  resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1606  }
1607  }
1608  }
1609  }
1610  }
1611 }
1612 
1613 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1614 {
1615  BT_PROFILE("solveGroupCacheFriendlyIterations");
1616 
1617  {
1619  solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1620 
1621  int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
1622 
1623  for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1624  //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1625  {
1626  solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1627  }
1628 
1629  }
1630  return 0.f;
1631 }
1632 
1633 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
1634 {
1635  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1636  int i,j;
1637 
1638  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1639  {
1640  for (j=0;j<numPoolConstraints;j++)
1641  {
1642  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1643  btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1644  btAssert(pt);
1645  pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1646  // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1647  // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1648  pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1649  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1650  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1651  {
1652  pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1653  }
1654  //do a callback here?
1655  }
1656  }
1657 
1658  numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1659  for (j=0;j<numPoolConstraints;j++)
1660  {
1661  const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1662  btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1663  btJointFeedback* fb = constr->getJointFeedback();
1664  if (fb)
1665  {
1666  fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1667  fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1668  fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1669  fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1670 
1671  }
1672 
1673  constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1674  if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1675  {
1676  constr->setEnabled(false);
1677  }
1678  }
1679 
1680 
1681 
1682  for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1683  {
1684  btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1685  if (body)
1686  {
1687  if (infoGlobal.m_splitImpulse)
1688  m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1689  else
1690  m_tmpSolverBodyPool[i].writebackVelocity();
1691 
1692  m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
1693  m_tmpSolverBodyPool[i].m_linearVelocity+
1694  m_tmpSolverBodyPool[i].m_externalForceImpulse);
1695 
1696  m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
1697  m_tmpSolverBodyPool[i].m_angularVelocity+
1698  m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
1699 
1700  if (infoGlobal.m_splitImpulse)
1701  m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1702 
1703  m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1704  }
1705  }
1706 
1707  m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1708  m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1709  m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1710  m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1711 
1712  m_tmpSolverBodyPool.resizeNoInitialize(0);
1713  return 0.f;
1714 }
1715 
1716 
1717 
1719 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
1720 {
1721 
1722  BT_PROFILE("solveGroup");
1723  //you need to provide at least some bodies
1724 
1725  solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1726 
1727  solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1728 
1729  solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1730 
1731  return 0.f;
1732 }
1733 
1734 void btSequentialImpulseConstraintSolver::reset()
1735 {
1736  m_btSeed2 = 0;
1737 }
1738 
1739 
Generated on Wed Apr 9 2014 02:38:23 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2