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 // Project 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)
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  } else
303  {
304  solverBody->m_worldTransform.setIdentity();
305  solverBody->internalSetInvMass(btVector3(0,0,0));
306  solverBody->m_originalBody = 0;
307  solverBody->m_angularFactor.setValue(1,1,1);
308  solverBody->m_linearFactor.setValue(1,1,1);
309  solverBody->m_linearVelocity.setValue(0,0,0);
310  solverBody->m_angularVelocity.setValue(0,0,0);
311  }
312 
313 
314 }
315 
316 
317 
318 
319 
320 
321 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
322 {
323  btScalar rest = restitution * -rel_vel;
324  return rest;
325 }
326 
327 
328 
329 static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
330 static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
331 {
332 
333 
334  if (colObj && colObj->hasAnisotropicFriction(frictionMode))
335  {
336  // transform to local coordinates
337  btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
338  const btVector3& friction_scaling = colObj->getAnisotropicFriction();
339  //apply anisotropic friction
340  loc_lateral *= friction_scaling;
341  // ... and transform it back to global coordinates
342  frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
343  }
344 
345 }
346 
347 
348 
349 
350 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)
351 {
352 
353 
354  solverConstraint.m_contactNormal1 = normalAxis;
355  solverConstraint.m_contactNormal2 = -normalAxis;
356  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
357  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
358 
359  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
360  btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
361 
362  solverConstraint.m_solverBodyIdA = solverBodyIdA;
363  solverConstraint.m_solverBodyIdB = solverBodyIdB;
364 
365  solverConstraint.m_friction = cp.m_combinedFriction;
366  solverConstraint.m_originalContactPoint = 0;
367 
368  solverConstraint.m_appliedImpulse = 0.f;
369  solverConstraint.m_appliedPushImpulse = 0.f;
370 
371  {
372  btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
373  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
374  solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
375  }
376  {
377  btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
378  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
379  solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
380  }
381 
382  {
383  btVector3 vec;
384  btScalar denom0 = 0.f;
385  btScalar denom1 = 0.f;
386  if (body0)
387  {
388  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
389  denom0 = body0->getInvMass() + normalAxis.dot(vec);
390  }
391  if (body1)
392  {
393  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
394  denom1 = body1->getInvMass() + normalAxis.dot(vec);
395  }
396  btScalar denom = relaxation/(denom0+denom1);
397  solverConstraint.m_jacDiagABInv = denom;
398  }
399 
400  {
401 
402 
403  btScalar rel_vel;
404  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
405  + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
406  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
407  + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
408 
409  rel_vel = vel1Dotn+vel2Dotn;
410 
411 // btScalar positionalError = 0.f;
412 
413  btSimdScalar velocityError = desiredVelocity - rel_vel;
414  btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
415  solverConstraint.m_rhs = velocityImpulse;
416  solverConstraint.m_cfm = cfmSlip;
417  solverConstraint.m_lowerLimit = 0;
418  solverConstraint.m_upperLimit = 1e10f;
419 
420  }
421 }
422 
423 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)
424 {
425  btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
426  solverConstraint.m_frictionIndex = frictionIndex;
427  setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
428  colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
429  return solverConstraint;
430 }
431 
432 
433 void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
434  btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
435  btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
436  btScalar desiredVelocity, btScalar cfmSlip)
437 
438 {
439  btVector3 normalAxis(0,0,0);
440 
441 
442  solverConstraint.m_contactNormal1 = normalAxis;
443  solverConstraint.m_contactNormal2 = -normalAxis;
444  btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
445  btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
446 
447  btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
448  btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
449 
450  solverConstraint.m_solverBodyIdA = solverBodyIdA;
451  solverConstraint.m_solverBodyIdB = solverBodyIdB;
452 
453  solverConstraint.m_friction = cp.m_combinedRollingFriction;
454  solverConstraint.m_originalContactPoint = 0;
455 
456  solverConstraint.m_appliedImpulse = 0.f;
457  solverConstraint.m_appliedPushImpulse = 0.f;
458 
459  {
460  btVector3 ftorqueAxis1 = -normalAxis1;
461  solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
462  solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
463  }
464  {
465  btVector3 ftorqueAxis1 = normalAxis1;
466  solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
467  solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
468  }
469 
470 
471  {
472  btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
473  btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
474  btScalar sum = 0;
475  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
476  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
477  solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
478  }
479 
480  {
481 
482 
483  btScalar rel_vel;
484  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
485  + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
486  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
487  + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
488 
489  rel_vel = vel1Dotn+vel2Dotn;
490 
491 // btScalar positionalError = 0.f;
492 
493  btSimdScalar velocityError = desiredVelocity - rel_vel;
494  btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
495  solverConstraint.m_rhs = velocityImpulse;
496  solverConstraint.m_cfm = cfmSlip;
497  solverConstraint.m_lowerLimit = 0;
498  solverConstraint.m_upperLimit = 1e10f;
499 
500  }
501 }
502 
503 
504 
505 
506 
507 
508 
509 
510 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)
511 {
512  btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
513  solverConstraint.m_frictionIndex = frictionIndex;
514  setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
515  colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
516  return solverConstraint;
517 }
518 
519 
520 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
521 {
522 
523  int solverBodyIdA = -1;
524 
525  if (body.getCompanionId() >= 0)
526  {
527  //body has already been converted
528  solverBodyIdA = body.getCompanionId();
529  btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
530  } else
531  {
532  btRigidBody* rb = btRigidBody::upcast(&body);
533  //convert both active and kinematic objects (for their velocity)
534  if (rb && (rb->getInvMass() || rb->isKinematicObject()))
535  {
536  solverBodyIdA = m_tmpSolverBodyPool.size();
537  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
538  initSolverBody(&solverBody,&body);
539  body.setCompanionId(solverBodyIdA);
540  } else
541  {
542  return 0;//assume first one is a fixed solver body
543  }
544  }
545 
546  return solverBodyIdA;
547 
548 }
549 #include <stdio.h>
550 
551 
552 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
553  int solverBodyIdA, int solverBodyIdB,
554  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
555  btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
556  btVector3& rel_pos1, btVector3& rel_pos2)
557 {
558 
559  const btVector3& pos1 = cp.getPositionWorldOnA();
560  const btVector3& pos2 = cp.getPositionWorldOnB();
561 
562  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
563  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
564 
565  btRigidBody* rb0 = bodyA->m_originalBody;
566  btRigidBody* rb1 = bodyB->m_originalBody;
567 
568 // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
569 // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
570  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
571  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
572 
573  relaxation = 1.f;
574 
575  btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
576  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
577  btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
578  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
579 
580  {
581 #ifdef COMPUTE_IMPULSE_DENOM
582  btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
583  btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
584 #else
585  btVector3 vec;
586  btScalar denom0 = 0.f;
587  btScalar denom1 = 0.f;
588  if (rb0)
589  {
590  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
591  denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
592  }
593  if (rb1)
594  {
595  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
596  denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
597  }
598 #endif //COMPUTE_IMPULSE_DENOM
599 
600  btScalar denom = relaxation/(denom0+denom1);
601  solverConstraint.m_jacDiagABInv = denom;
602  }
603 
604  solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
605  solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
606  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
607  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
608 
609  btScalar restitution = 0.f;
610  btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
611 
612  {
613  btVector3 vel1,vel2;
614 
615  vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
616  vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
617 
618  // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
619  vel = vel1 - vel2;
620  rel_vel = cp.m_normalWorldOnB.dot(vel);
621 
622 
623 
624  solverConstraint.m_friction = cp.m_combinedFriction;
625 
626 
627  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
628  if (restitution <= btScalar(0.))
629  {
630  restitution = 0.f;
631  };
632  }
633 
634 
636  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
637  {
638  solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
639  if (rb0)
640  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
641  if (rb1)
642  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
643  } else
644  {
645  solverConstraint.m_appliedImpulse = 0.f;
646  }
647 
648  solverConstraint.m_appliedPushImpulse = 0.f;
649 
650  {
651  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0))
652  + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0));
653  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0))
654  + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0));
655  btScalar rel_vel = vel1Dotn+vel2Dotn;
656 
657  btScalar positionalError = 0.f;
658  btScalar velocityError = restitution - rel_vel;// * damping;
659 
660 
661  btScalar erp = infoGlobal.m_erp2;
662  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
663  {
664  erp = infoGlobal.m_erp;
665  }
666 
667  if (penetration>0)
668  {
669  positionalError = 0;
670 
671  velocityError -= penetration / infoGlobal.m_timeStep;
672  } else
673  {
674  positionalError = -penetration * erp/infoGlobal.m_timeStep;
675  }
676 
677  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
678  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
679 
680  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
681  {
682  //combine position and velocity into rhs
683  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
684  solverConstraint.m_rhsPenetration = 0.f;
685 
686  } else
687  {
688  //split position and velocity into rhs and m_rhsPenetration
689  solverConstraint.m_rhs = velocityImpulse;
690  solverConstraint.m_rhsPenetration = penetrationImpulse;
691  }
692  solverConstraint.m_cfm = 0.f;
693  solverConstraint.m_lowerLimit = 0;
694  solverConstraint.m_upperLimit = 1e10f;
695  }
696 
697 
698 
699 
700 }
701 
702 
703 
704 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
705  int solverBodyIdA, int solverBodyIdB,
706  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
707 {
708 
709  btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
710  btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
711 
712  btRigidBody* rb0 = bodyA->m_originalBody;
713  btRigidBody* rb1 = bodyB->m_originalBody;
714 
715  {
716  btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
717  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
718  {
719  frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
720  if (rb0)
721  bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
722  if (rb1)
723  bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
724  } else
725  {
726  frictionConstraint1.m_appliedImpulse = 0.f;
727  }
728  }
729 
730  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
731  {
732  btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
733  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
734  {
735  frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
736  if (rb0)
737  bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
738  if (rb1)
739  bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
740  } else
741  {
742  frictionConstraint2.m_appliedImpulse = 0.f;
743  }
744  }
745 }
746 
747 
748 
749 
750 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
751 {
752  btCollisionObject* colObj0=0,*colObj1=0;
753 
754  colObj0 = (btCollisionObject*)manifold->getBody0();
755  colObj1 = (btCollisionObject*)manifold->getBody1();
756 
757  int solverBodyIdA = getOrInitSolverBody(*colObj0);
758  int solverBodyIdB = getOrInitSolverBody(*colObj1);
759 
760 // btRigidBody* bodyA = btRigidBody::upcast(colObj0);
761 // btRigidBody* bodyB = btRigidBody::upcast(colObj1);
762 
763  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
764  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
765 
766 
767 
769  if (!solverBodyA || (!solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody)))
770  return;
771 
772  int rollingFriction=1;
773  for (int j=0;j<manifold->getNumContacts();j++)
774  {
775 
776  btManifoldPoint& cp = manifold->getContactPoint(j);
777 
778  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
779  {
780  btVector3 rel_pos1;
781  btVector3 rel_pos2;
782  btScalar relaxation;
783  btScalar rel_vel;
784  btVector3 vel;
785 
786  int frictionIndex = m_tmpSolverContactConstraintPool.size();
787  btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
788 // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
789 // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
790  solverConstraint.m_solverBodyIdA = solverBodyIdA;
791  solverConstraint.m_solverBodyIdB = solverBodyIdB;
792 
793  solverConstraint.m_originalContactPoint = &cp;
794 
795  setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
796 
797 // const btVector3& pos1 = cp.getPositionWorldOnA();
798 // const btVector3& pos2 = cp.getPositionWorldOnB();
799 
801 
802  solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
803 
804  btVector3 angVelA,angVelB;
805  solverBodyA->getAngularVelocity(angVelA);
806  solverBodyB->getAngularVelocity(angVelB);
807  btVector3 relAngVel = angVelB-angVelA;
808 
809  if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
810  {
811  //only a single rollingFriction per manifold
812  rollingFriction--;
813  if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
814  {
815  relAngVel.normalize();
816  applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
817  applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
818  if (relAngVel.length()>0.001)
819  addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
820 
821  } else
822  {
823  addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
824  btVector3 axis0,axis1;
825  btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
826  applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
827  applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
828  applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
829  applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
830  if (axis0.length()>0.001)
831  addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
832  if (axis1.length()>0.001)
833  addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
834 
835  }
836  }
837 
842 
853  if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
854  {
855  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
856  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
857  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
858  {
859  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
860  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
861  {
862  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
863  cp.m_lateralFrictionDir2.normalize();//??
864  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
865  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
866  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
867 
868  }
869 
870  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
871  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
872  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
873 
874  } else
875  {
876  btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
877 
878  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
879  {
880  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
881  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
882  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
883  }
884 
885  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
886  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
887  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
888 
889  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
890  {
891  cp.m_lateralFrictionInitialized = true;
892  }
893  }
894 
895  } else
896  {
897  addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
898 
899  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
900  addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
901 
902  setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
903  }
904 
905 
906 
907 
908  }
909  }
910 }
911 
912 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
913 {
914  BT_PROFILE("solveGroupCacheFriendlySetup");
915  (void)stackAlloc;
916  (void)debugDrawer;
917 
918  m_maxOverrideNumSolverIterations = 0;
919 
920 #ifdef BT_ADDITIONAL_DEBUG
921  //make sure that dynamic bodies exist for all (enabled) constraints
922  for (int i=0;i<numConstraints;i++)
923  {
924  btTypedConstraint* constraint = constraints[i];
925  if (constraint->isEnabled())
926  {
927  if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
928  {
929  bool found=false;
930  for (int b=0;b<numBodies;b++)
931  {
932 
933  if (&constraint->getRigidBodyA()==bodies[b])
934  {
935  found = true;
936  break;
937  }
938  }
939  btAssert(found);
940  }
941  if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
942  {
943  bool found=false;
944  for (int b=0;b<numBodies;b++)
945  {
946  if (&constraint->getRigidBodyB()==bodies[b])
947  {
948  found = true;
949  break;
950  }
951  }
952  btAssert(found);
953  }
954  }
955  }
956  //make sure that dynamic bodies exist for all contact manifolds
957  for (int i=0;i<numManifolds;i++)
958  {
959  if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
960  {
961  bool found=false;
962  for (int b=0;b<numBodies;b++)
963  {
964 
965  if (manifoldPtr[i]->getBody0()==bodies[b])
966  {
967  found = true;
968  break;
969  }
970  }
971  btAssert(found);
972  }
973  if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
974  {
975  bool found=false;
976  for (int b=0;b<numBodies;b++)
977  {
978  if (manifoldPtr[i]->getBody1()==bodies[b])
979  {
980  found = true;
981  break;
982  }
983  }
984  btAssert(found);
985  }
986  }
987 #endif //BT_ADDITIONAL_DEBUG
988 
989 
990  for (int i = 0; i < numBodies; i++)
991  {
992  bodies[i]->setCompanionId(-1);
993  }
994 
995 
996  m_tmpSolverBodyPool.reserve(numBodies+1);
997  m_tmpSolverBodyPool.resize(0);
998 
999  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1000  initSolverBody(&fixedBody,0);
1001 
1002  //convert all bodies
1003 
1004  for (int i=0;i<numBodies;i++)
1005  {
1006  int bodyId = getOrInitSolverBody(*bodies[i]);
1007  btRigidBody* body = btRigidBody::upcast(bodies[i]);
1008  if (body && body->getInvMass())
1009  {
1010  btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1011  btVector3 gyroForce (0,0,0);
1012  if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
1013  {
1014  gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
1015  }
1016  solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep;
1017  solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1018  }
1019  }
1020 
1021  if (1)
1022  {
1023  int j;
1024  for (j=0;j<numConstraints;j++)
1025  {
1026  btTypedConstraint* constraint = constraints[j];
1027  constraint->buildJacobian();
1028  constraint->internalSetAppliedImpulse(0.0f);
1029  }
1030  }
1031 
1032  //btRigidBody* rb0=0,*rb1=0;
1033 
1034  //if (1)
1035  {
1036  {
1037 
1038  int totalNumRows = 0;
1039  int i;
1040 
1041  m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
1042  //calculate the total number of contraint rows
1043  for (i=0;i<numConstraints;i++)
1044  {
1045  btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1046  btJointFeedback* fb = constraints[i]->getJointFeedback();
1047  if (fb)
1048  {
1049  fb->m_appliedForceBodyA.setZero();
1050  fb->m_appliedTorqueBodyA.setZero();
1051  fb->m_appliedForceBodyB.setZero();
1052  fb->m_appliedTorqueBodyB.setZero();
1053  }
1054 
1055  if (constraints[i]->isEnabled())
1056  {
1057  }
1058  if (constraints[i]->isEnabled())
1059  {
1060  constraints[i]->getInfo1(&info1);
1061  } else
1062  {
1063  info1.m_numConstraintRows = 0;
1064  info1.nub = 0;
1065  }
1066  totalNumRows += info1.m_numConstraintRows;
1067  }
1068  m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
1069 
1070 
1072  int currentRow = 0;
1073 
1074  for (i=0;i<numConstraints;i++)
1075  {
1076  const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1077 
1078  if (info1.m_numConstraintRows)
1079  {
1080  btAssert(currentRow<totalNumRows);
1081 
1082  btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1083  btTypedConstraint* constraint = constraints[i];
1084  btRigidBody& rbA = constraint->getRigidBodyA();
1085  btRigidBody& rbB = constraint->getRigidBodyB();
1086 
1087  int solverBodyIdA = getOrInitSolverBody(rbA);
1088  int solverBodyIdB = getOrInitSolverBody(rbB);
1089 
1090  btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1091  btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1092 
1093 
1094 
1095 
1096  int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1097  if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
1098  m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1099 
1100 
1101  int j;
1102  for ( j=0;j<info1.m_numConstraintRows;j++)
1103  {
1104  memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1105  currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1106  currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1107  currentConstraintRow[j].m_appliedImpulse = 0.f;
1108  currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1109  currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1110  currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1111  currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1112  }
1113 
1114  bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1115  bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1116  bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1117  bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1118  bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1119  bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1120  bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1121  bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1122 
1123 
1124  btTypedConstraint::btConstraintInfo2 info2;
1125  info2.fps = 1.f/infoGlobal.m_timeStep;
1126  info2.erp = infoGlobal.m_erp;
1127  info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1128  info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1129  info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1130  info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1131  info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1133  btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1134  info2.m_constraintError = &currentConstraintRow->m_rhs;
1135  currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1136  info2.m_damping = infoGlobal.m_damping;
1137  info2.cfm = &currentConstraintRow->m_cfm;
1138  info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1139  info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1140  info2.m_numIterations = infoGlobal.m_numIterations;
1141  constraints[i]->getInfo2(&info2);
1142 
1144  for ( j=0;j<info1.m_numConstraintRows;j++)
1145  {
1146  btSolverConstraint& solverConstraint = currentConstraintRow[j];
1147 
1148  if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
1149  {
1150  solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
1151  }
1152 
1153  if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
1154  {
1155  solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
1156  }
1157 
1158  solverConstraint.m_originalContactPoint = constraint;
1159 
1160  {
1161  const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1162  solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1163  }
1164  {
1165  const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1166  solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1167  }
1168 
1169  {
1170  btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
1171  btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1172  btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
1173  btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1174 
1175  btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1176  sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1177  sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1178  sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1179  btScalar fsum = btFabs(sum);
1180  btAssert(fsum > SIMD_EPSILON);
1181  solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
1182  }
1183 
1184 
1187  {
1188  btScalar rel_vel;
1189  btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
1190  btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
1191 
1192  rel_vel = vel1Dotn+vel2Dotn;
1193 
1194  btScalar restitution = 0.f;
1195  btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1196  btScalar velocityError = restitution - rel_vel * info2.m_damping;
1197  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1198  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1199  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1200  solverConstraint.m_appliedImpulse = 0.f;
1201 
1202  }
1203  }
1204  }
1205  currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1206  }
1207  }
1208 
1209  {
1210  int i;
1211  btPersistentManifold* manifold = 0;
1212 // btCollisionObject* colObj0=0,*colObj1=0;
1213 
1214 
1215  for (i=0;i<numManifolds;i++)
1216  {
1217  manifold = manifoldPtr[i];
1218  convertContact(manifold,infoGlobal);
1219  }
1220  }
1221  }
1222 
1223 // btContactSolverInfo info = infoGlobal;
1224 
1225 
1226  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1227  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1228  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1229 
1231  m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
1232  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1233  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
1234  else
1235  m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1236 
1237  m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
1238  {
1239  int i;
1240  for (i=0;i<numNonContactPool;i++)
1241  {
1242  m_orderNonContactConstraintPool[i] = i;
1243  }
1244  for (i=0;i<numConstraintPool;i++)
1245  {
1246  m_orderTmpConstraintPool[i] = i;
1247  }
1248  for (i=0;i<numFrictionPool;i++)
1249  {
1250  m_orderFrictionConstraintPool[i] = i;
1251  }
1252  }
1253 
1254  return 0.f;
1255 
1256 }
1257 
1258 
1259 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
1260 {
1261 
1262  int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1263  int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1264  int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1265 
1266  if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1267  {
1268  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1269  {
1270 
1271  for (int j=0; j<numNonContactPool; ++j) {
1272  int tmp = m_orderNonContactConstraintPool[j];
1273  int swapi = btRandInt2(j+1);
1274  m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
1275  m_orderNonContactConstraintPool[swapi] = tmp;
1276  }
1277 
1278  //contact/friction constraints are not solved more than
1279  if (iteration< infoGlobal.m_numIterations)
1280  {
1281  for (int j=0; j<numConstraintPool; ++j) {
1282  int tmp = m_orderTmpConstraintPool[j];
1283  int swapi = btRandInt2(j+1);
1284  m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
1285  m_orderTmpConstraintPool[swapi] = tmp;
1286  }
1287 
1288  for (int j=0; j<numFrictionPool; ++j) {
1289  int tmp = m_orderFrictionConstraintPool[j];
1290  int swapi = btRandInt2(j+1);
1291  m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
1292  m_orderFrictionConstraintPool[swapi] = tmp;
1293  }
1294  }
1295  }
1296  }
1297 
1298  if (infoGlobal.m_solverMode & SOLVER_SIMD)
1299  {
1301  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1302  {
1303  btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1304  if (iteration < constraint.m_overrideNumSolverIterations)
1305  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1306  }
1307 
1308  if (iteration< infoGlobal.m_numIterations)
1309  {
1310  for (int j=0;j<numConstraints;j++)
1311  {
1312  if (constraints[j]->isEnabled())
1313  {
1314  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
1315  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
1316  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1317  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1318  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1319  }
1320  }
1321 
1323  if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
1324  {
1325  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1326  int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
1327 
1328  for (int c=0;c<numPoolConstraints;c++)
1329  {
1330  btScalar totalImpulse =0;
1331 
1332  {
1333  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
1334  resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1335  totalImpulse = solveManifold.m_appliedImpulse;
1336  }
1337  bool applyFriction = true;
1338  if (applyFriction)
1339  {
1340  {
1341 
1342  btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
1343 
1344  if (totalImpulse>btScalar(0))
1345  {
1346  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1347  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1348 
1349  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1350  }
1351  }
1352 
1353  if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
1354  {
1355 
1356  btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
1357 
1358  if (totalImpulse>btScalar(0))
1359  {
1360  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1361  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1362 
1363  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1364  }
1365  }
1366  }
1367  }
1368 
1369  }
1370  else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1371  {
1372  //solve the friction constraints after all contact constraints, don't interleave them
1373  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1374  int j;
1375 
1376  for (j=0;j<numPoolConstraints;j++)
1377  {
1378  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1379  resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1380 
1381  }
1382 
1383 
1384 
1386 
1387  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1388  for (j=0;j<numFrictionPoolConstraints;j++)
1389  {
1390  btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1391  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1392 
1393  if (totalImpulse>btScalar(0))
1394  {
1395  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1396  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1397 
1398  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1399  }
1400  }
1401 
1402 
1403  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1404  for (j=0;j<numRollingFrictionPoolConstraints;j++)
1405  {
1406 
1407  btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1408  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1409  if (totalImpulse>btScalar(0))
1410  {
1411  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1412  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1413  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1414 
1415  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1416  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1417 
1418  resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1419  }
1420  }
1421 
1422 
1423  }
1424  }
1425  } else
1426  {
1427  //non-SIMD version
1429  for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1430  {
1431  btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1432  if (iteration < constraint.m_overrideNumSolverIterations)
1433  resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1434  }
1435 
1436  if (iteration< infoGlobal.m_numIterations)
1437  {
1438  for (int j=0;j<numConstraints;j++)
1439  {
1440  if (constraints[j]->isEnabled())
1441  {
1442  int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
1443  int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
1444  btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1445  btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1446  constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1447  }
1448  }
1450  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1451  for (int j=0;j<numPoolConstraints;j++)
1452  {
1453  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1454  resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1455  }
1457  int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1458  for (int j=0;j<numFrictionPoolConstraints;j++)
1459  {
1460  btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1461  btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1462 
1463  if (totalImpulse>btScalar(0))
1464  {
1465  solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1466  solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1467 
1468  resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1469  }
1470  }
1471 
1472  int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1473  for (int j=0;j<numRollingFrictionPoolConstraints;j++)
1474  {
1475  btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1476  btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1477  if (totalImpulse>btScalar(0))
1478  {
1479  btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1480  if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1481  rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1482 
1483  rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1484  rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1485 
1486  resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1487  }
1488  }
1489  }
1490  }
1491  return 0.f;
1492 }
1493 
1494 
1495 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1496 {
1497  int iteration;
1498  if (infoGlobal.m_splitImpulse)
1499  {
1500  if (infoGlobal.m_solverMode & SOLVER_SIMD)
1501  {
1502  for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1503  {
1504  {
1505  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1506  int j;
1507  for (j=0;j<numPoolConstraints;j++)
1508  {
1509  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1510 
1511  resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1512  }
1513  }
1514  }
1515  }
1516  else
1517  {
1518  for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1519  {
1520  {
1521  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1522  int j;
1523  for (j=0;j<numPoolConstraints;j++)
1524  {
1525  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1526 
1527  resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1528  }
1529  }
1530  }
1531  }
1532  }
1533 }
1534 
1535 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1536 {
1537  BT_PROFILE("solveGroupCacheFriendlyIterations");
1538 
1539  {
1541  solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1542 
1543  int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
1544 
1545  for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1546  //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1547  {
1548  solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1549  }
1550 
1551  }
1552  return 0.f;
1553 }
1554 
1555 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
1556 {
1557  int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1558  int i,j;
1559 
1560  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1561  {
1562  for (j=0;j<numPoolConstraints;j++)
1563  {
1564  const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1565  btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1566  btAssert(pt);
1567  pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1568  // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1569  // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1570  pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1571  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1572  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1573  {
1574  pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1575  }
1576  //do a callback here?
1577  }
1578  }
1579 
1580  numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1581  for (j=0;j<numPoolConstraints;j++)
1582  {
1583  const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1584  btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1585  btJointFeedback* fb = constr->getJointFeedback();
1586  if (fb)
1587  {
1588  fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1589  fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1590  fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1591  fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1592 
1593  }
1594 
1595  constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1596  if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1597  {
1598  constr->setEnabled(false);
1599  }
1600  }
1601 
1602 
1603 
1604  for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1605  {
1606  btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1607  if (body)
1608  {
1609  if (infoGlobal.m_splitImpulse)
1610  m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1611  else
1612  m_tmpSolverBodyPool[i].writebackVelocity();
1613 
1614  m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
1615  m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
1616  if (infoGlobal.m_splitImpulse)
1617  m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1618 
1619  m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1620  }
1621  }
1622 
1623  m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1624  m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1625  m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1626  m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1627 
1628  m_tmpSolverBodyPool.resizeNoInitialize(0);
1629  return 0.f;
1630 }
1631 
1632 
1633 
1635 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
1636 {
1637 
1638  BT_PROFILE("solveGroup");
1639  //you need to provide at least some bodies
1640 
1641  solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1642 
1643  solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1644 
1645  solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1646 
1647  return 0.f;
1648 }
1649 
1650 void btSequentialImpulseConstraintSolver::reset()
1651 {
1652  m_btSeed2 = 0;
1653 }
1654 
1655 
Generated on Mon Mar 18 2013 14:12:56 for Bullet Collision Detection & Physics Library by   doxygen 1.8.3.1