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