Bullet Collision Detection & Physics Library
btSequentialImpulseConstraintSolver.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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 "LinearMath/btCpuFeatureUtility.h"
26
27//#include "btJacobianEntry.h"
28#include "LinearMath/btMinMax.h"
29#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
30#include <new>
31#include "LinearMath/btStackAlloc.h"
32#include "LinearMath/btQuickprof.h"
33//#include "btSolverBody.h"
34//#include "btSolverConstraint.h"
35#include "LinearMath/btAlignedObjectArray.h"
36#include <string.h> //for memset
37
38int gNumSplitImpulseRecoveries = 0;
39
40#include "BulletDynamics/Dynamics/btRigidBody.h"
41
42//#define VERBOSE_RESIDUAL_PRINTF 1
45static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
46{
47 btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
48 const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
49 const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
50
51 // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
52 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
53 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
54
55 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
56 if (sum < c.m_lowerLimit)
57 {
58 deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
59 c.m_appliedImpulse = c.m_lowerLimit;
60 }
61 else if (sum > c.m_upperLimit)
62 {
63 deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
64 c.m_appliedImpulse = c.m_upperLimit;
65 }
66 else
67 {
68 c.m_appliedImpulse = sum;
69 }
70
71 body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
72 body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
73
74 return deltaImpulse;
75}
76
77
78static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
79{
80 btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
81 const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
82 const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
83
84 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
85 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
86 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
87 if (sum < c.m_lowerLimit)
88 {
89 deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
90 c.m_appliedImpulse = c.m_lowerLimit;
91 }
92 else
93 {
94 c.m_appliedImpulse = sum;
95 }
96 body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
97 body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
98
99 return deltaImpulse;
100}
101
102
103
104#ifdef USE_SIMD
105#include <emmintrin.h>
106
107
108#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
109static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
110{
111 __m128 result = _mm_mul_ps( vec0, vec1);
112 return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
113}
114
115#if defined (BT_ALLOW_SSE4)
116#include <intrin.h>
117
118#define USE_FMA 1
119#define USE_FMA3_INSTEAD_FMA4 1
120#define USE_SSE4_DOT 1
121
122#define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f)
123#define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
124
125#if USE_SSE4_DOT
126#define DOT_PRODUCT(a, b) SSE4_DP(a, b)
127#else
128#define DOT_PRODUCT(a, b) btSimdDot3(a, b)
129#endif
130
131#if USE_FMA
132#if USE_FMA3_INSTEAD_FMA4
133// a*b + c
134#define FMADD(a, b, c) _mm_fmadd_ps(a, b, c)
135// -(a*b) + c
136#define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c)
137#else // USE_FMA3
138// a*b + c
139#define FMADD(a, b, c) _mm_macc_ps(a, b, c)
140// -(a*b) + c
141#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
142#endif
143#else // USE_FMA
144// c + a*b
145#define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b))
146// c - a*b
147#define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b))
148#endif
149#endif
150
151// Project Gauss Seidel or the equivalent Sequential Impulse
152static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
153{
154 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
155 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
156 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
157 btSimdScalar 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)));
158 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
159 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
160 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
161 deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
162 btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
163 btSimdScalar resultLowerLess, resultUpperLess;
164 resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
165 resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
166 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
167 deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse));
168 c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
169 __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
170 deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied));
171 c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1));
172 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
173 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128);
174 __m128 impulseMagnitude = deltaImpulse;
175 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
176 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
177 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
178 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
179 return deltaImpulse;
180}
181
182
183// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
184static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
185{
186#if defined (BT_ALLOW_SSE4)
187 __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
188 __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
189 const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
190 const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit);
191 const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
192 const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
193 deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
194 deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
195 tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum
196 const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit);
197 const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp);
198 deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower);
199 c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower);
200 body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
201 body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
202 body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
203 body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
204 return deltaImpulse;
205#else
206 return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c);
207#endif
208}
209
210
211
212static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
213{
214 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
215 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
216 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
217 btSimdScalar 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)));
218 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
219 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().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_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
229 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
230 __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128);
231 __m128 impulseMagnitude = deltaImpulse;
232 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
233 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
234 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
235 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
236 return deltaImpulse;
237}
238
239
240// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
241static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
242{
243#ifdef BT_ALLOW_SSE4
244 __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv);
245 __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm);
246 const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit);
247 const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
248 const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
249 deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse);
250 deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse);
251 tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse);
252 const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit);
253 deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask);
254 c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask);
255 body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
256 body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
257 body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
258 body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
259 return deltaImpulse;
260#else
261 return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c);
262#endif //BT_ALLOW_SSE4
263}
264
265
266#endif //USE_SIMD
267
268
269
270btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
271{
272 return m_resolveSingleConstraintRowGeneric(body1, body2, c);
273}
274
275// Project Gauss Seidel or the equivalent Sequential Impulse
276btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
277{
278 return m_resolveSingleConstraintRowGeneric(body1, body2, c);
279}
280
281btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
282{
283 return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
284}
285
286
287btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
288{
289 return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
290}
291
292
293static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference(
294 btSolverBody& body1,
295 btSolverBody& body2,
296 const btSolverConstraint& c)
297{
298 btScalar deltaImpulse = 0.f;
299
300 if (c.m_rhsPenetration)
301 {
302 gNumSplitImpulseRecoveries++;
303 deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
304 const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
305 const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
306
307 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
308 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
309 const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
310 if (sum < c.m_lowerLimit)
311 {
312 deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
313 c.m_appliedPushImpulse = c.m_lowerLimit;
314 }
315 else
316 {
317 c.m_appliedPushImpulse = sum;
318 }
319 body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
320 body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
321 }
322 return deltaImpulse;
323}
324
325static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
326{
327#ifdef USE_SIMD
328 if (!c.m_rhsPenetration)
329 return 0.f;
330
331 gNumSplitImpulseRecoveries++;
332
333 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
334 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
335 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
336 __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)));
337 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
338 __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128));
339 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
340 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
341 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
342 btSimdScalar resultLowerLess,resultUpperLess;
343 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
344 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
345 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
346 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
347 c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
348 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128);
349 __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128);
350 __m128 impulseMagnitude = deltaImpulse;
351 body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
352 body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
353 body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
354 body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
355 return deltaImpulse;
356#else
357 return gResolveSplitPenetrationImpulse_scalar_reference(body1,body2,c);
358#endif
359}
360
361
362btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
363{
364 m_btSeed2 = 0;
365 m_cachedSolverMode = 0;
366 setupSolverFunctions( false );
367}
368
369void btSequentialImpulseConstraintSolver::setupSolverFunctions( bool useSimd )
370{
371 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference;
372 m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference;
373 m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference;
374
375 if ( useSimd )
376 {
377#ifdef USE_SIMD
378 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
379 m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2;
380 m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2;
381
382#ifdef BT_ALLOW_SSE4
383 int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
384 if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1))
385 {
386 m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3;
387 m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
388 }
389#endif//BT_ALLOW_SSE4
390#endif //USE_SIMD
391 }
392}
393
394 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
395 {
396 }
397
398 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()
399 {
400 return gResolveSingleConstraintRowGeneric_scalar_reference;
401 }
402
403 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()
404 {
405 return gResolveSingleConstraintRowLowerLimit_scalar_reference;
406 }
407
408
409#ifdef USE_SIMD
410 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
411 {
412 return gResolveSingleConstraintRowGeneric_sse2;
413 }
414 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit()
415 {
416 return gResolveSingleConstraintRowLowerLimit_sse2;
417 }
418#ifdef BT_ALLOW_SSE4
419 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric()
420 {
421 return gResolveSingleConstraintRowGeneric_sse4_1_fma3;
422 }
423 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit()
424 {
425 return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
426 }
427#endif //BT_ALLOW_SSE4
428#endif //USE_SIMD
429
430unsigned long btSequentialImpulseConstraintSolver::btRand2()
431{
432 m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
433 return m_btSeed2;
434}
435
436
437
438//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
439int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
440{
441 // seems good; xor-fold and modulus
442 const unsigned long un = static_cast<unsigned long>(n);
443 unsigned long r = btRand2();
444
445 // note: probably more aggressive than it needs to be -- might be
446 // able to get away without one or two of the innermost branches.
447 if (un <= 0x00010000UL) {
448 r ^= (r >> 16);
449 if (un <= 0x00000100UL) {
450 r ^= (r >> 8);
451 if (un <= 0x00000010UL) {
452 r ^= (r >> 4);
453 if (un <= 0x00000004UL) {
454 r ^= (r >> 2);
455 if (un <= 0x00000002UL) {
456 r ^= (r >> 1);
457 }
458 }
459 }
460 }
461 }
462
463 return (int) (r % un);
464}
465
466
467
468void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
469{
470
471 btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
472
473 solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
474 solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
475 solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
476 solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
477
478 if (rb)
479 {
480 solverBody->m_worldTransform = rb->getWorldTransform();
481 solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
482 solverBody->m_originalBody = rb;
483 solverBody->m_angularFactor = rb->getAngularFactor();
484 solverBody->m_linearFactor = rb->getLinearFactor();
485 solverBody->m_linearVelocity = rb->getLinearVelocity();
486 solverBody->m_angularVelocity = rb->getAngularVelocity();
487 solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
488 solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
489
490 } else
491 {
492 solverBody->m_worldTransform.setIdentity();
493 solverBody->internalSetInvMass(btVector3(0,0,0));
494 solverBody->m_originalBody = 0;
495 solverBody->m_angularFactor.setValue(1,1,1);
496 solverBody->m_linearFactor.setValue(1,1,1);
497 solverBody->m_linearVelocity.setValue(0,0,0);
498 solverBody->m_angularVelocity.setValue(0,0,0);
499 solverBody->m_externalForceImpulse.setValue(0,0,0);
500 solverBody->m_externalTorqueImpulse.setValue(0,0,0);
501 }
502
503
504}
505
506
507
508
509
510
511btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
512{
513 //printf("rel_vel =%f\n", rel_vel);
514 if (btFabs(rel_vel)<velocityThreshold)
515 return 0.;
516
517 btScalar rest = restitution * -rel_vel;
518 return rest;
519}
520
521
522
523void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
524{
525
526
527 if (colObj && colObj->hasAnisotropicFriction(frictionMode))
528 {
529 // transform to local coordinates
530 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
531 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
532 //apply anisotropic friction
533 loc_lateral *= friction_scaling;
534 // ... and transform it back to global coordinates
535 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
536 }
537
538}
539
540
541
542
543void 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
544{
545
546
547 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
548 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
549
550 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
551 btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
552
553 solverConstraint.m_solverBodyIdA = solverBodyIdA;
554 solverConstraint.m_solverBodyIdB = solverBodyIdB;
555
556 solverConstraint.m_friction = cp.m_combinedFriction;
557 solverConstraint.m_originalContactPoint = 0;
558
559 solverConstraint.m_appliedImpulse = 0.f;
560 solverConstraint.m_appliedPushImpulse = 0.f;
561
562 if (body0)
563 {
564 solverConstraint.m_contactNormal1 = normalAxis;
565 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
566 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
567 solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
568 }else
569 {
570 solverConstraint.m_contactNormal1.setZero();
571 solverConstraint.m_relpos1CrossNormal.setZero();
572 solverConstraint.m_angularComponentA .setZero();
573 }
574
575 if (body1)
576 {
577 solverConstraint.m_contactNormal2 = -normalAxis;
578 btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
579 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
580 solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
581 } else
582 {
583 solverConstraint.m_contactNormal2.setZero();
584 solverConstraint.m_relpos2CrossNormal.setZero();
585 solverConstraint.m_angularComponentB.setZero();
586 }
587
588 {
589 btVector3 vec;
590 btScalar denom0 = 0.f;
591 btScalar denom1 = 0.f;
592 if (body0)
593 {
594 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
595 denom0 = body0->getInvMass() + normalAxis.dot(vec);
596 }
597 if (body1)
598 {
599 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
600 denom1 = body1->getInvMass() + normalAxis.dot(vec);
601 }
602 btScalar denom = relaxation/(denom0+denom1);
603 solverConstraint.m_jacDiagABInv = denom;
604 }
605
606 {
607
608
609 btScalar rel_vel;
610 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
611 + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
612 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
613 + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
614
615 rel_vel = vel1Dotn+vel2Dotn;
616
617// btScalar positionalError = 0.f;
618
619 btScalar velocityError = desiredVelocity - rel_vel;
620 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
621
622 btScalar penetrationImpulse = btScalar(0);
623
624 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
625 {
626 btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
627 btScalar positionalError = -distance * infoGlobal.m_frictionERP/infoGlobal.m_timeStep;
628 penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
629 }
630
631 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
632 solverConstraint.m_rhsPenetration = 0.f;
633 solverConstraint.m_cfm = cfmSlip;
634 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
635 solverConstraint.m_upperLimit = solverConstraint.m_friction;
636
637 }
638}
639
640btSolverConstraint& 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
641{
642 btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
643 solverConstraint.m_frictionIndex = frictionIndex;
644 setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
645 colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
646 return solverConstraint;
647}
648
649
650void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
651 btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
652 btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
653 btScalar desiredVelocity, btScalar cfmSlip)
654
655{
656 btVector3 normalAxis(0,0,0);
657
658
659 solverConstraint.m_contactNormal1 = normalAxis;
660 solverConstraint.m_contactNormal2 = -normalAxis;
661 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
662 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
663
664 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
665 btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
666
667 solverConstraint.m_solverBodyIdA = solverBodyIdA;
668 solverConstraint.m_solverBodyIdB = solverBodyIdB;
669
670 solverConstraint.m_friction = combinedTorsionalFriction;
671 solverConstraint.m_originalContactPoint = 0;
672
673 solverConstraint.m_appliedImpulse = 0.f;
674 solverConstraint.m_appliedPushImpulse = 0.f;
675
676 {
677 btVector3 ftorqueAxis1 = -normalAxis1;
678 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
679 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
680 }
681 {
682 btVector3 ftorqueAxis1 = normalAxis1;
683 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
684 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
685 }
686
687
688 {
689 btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
690 btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
691 btScalar sum = 0;
692 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
693 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
694 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
695 }
696
697 {
698
699
700 btScalar rel_vel;
701 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0))
702 + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
703 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
704 + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
705
706 rel_vel = vel1Dotn+vel2Dotn;
707
708// btScalar positionalError = 0.f;
709
710 btSimdScalar velocityError = desiredVelocity - rel_vel;
711 btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
712 solverConstraint.m_rhs = velocityImpulse;
713 solverConstraint.m_cfm = cfmSlip;
714 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
715 solverConstraint.m_upperLimit = solverConstraint.m_friction;
716
717 }
718}
719
720
721
722
723
724
725
726
727btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
728{
729 btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
730 solverConstraint.m_frictionIndex = frictionIndex;
731 setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2,
732 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
733 return solverConstraint;
734}
735
736
737int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
738{
739#if BT_THREADSAFE
740 int solverBodyId = -1;
741 if ( !body.isStaticOrKinematicObject() )
742 {
743 // dynamic body
744 // Dynamic bodies can only be in one island, so it's safe to write to the companionId
745 solverBodyId = body.getCompanionId();
746 if ( solverBodyId < 0 )
747 {
748 if ( btRigidBody* rb = btRigidBody::upcast( &body ) )
749 {
750 solverBodyId = m_tmpSolverBodyPool.size();
751 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
752 initSolverBody( &solverBody, &body, timeStep );
753 body.setCompanionId( solverBodyId );
754 }
755 }
756 }
757 else if (body.isKinematicObject())
758 {
759 //
760 // NOTE: must test for kinematic before static because some kinematic objects also
761 // identify as "static"
762 //
763 // Kinematic bodies can be in multiple islands at once, so it is a
764 // race condition to write to them, so we use an alternate method
765 // to record the solverBodyId
766 int uniqueId = body.getWorldArrayIndex();
767 const int INVALID_SOLVER_BODY_ID = -1;
768 if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
769 {
770 m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
771 }
772 solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ];
773 // if no table entry yet,
774 if ( solverBodyId == INVALID_SOLVER_BODY_ID )
775 {
776 // create a table entry for this body
777 btRigidBody* rb = btRigidBody::upcast( &body );
778 solverBodyId = m_tmpSolverBodyPool.size();
779 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
780 initSolverBody( &solverBody, &body, timeStep );
781 m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId;
782 }
783 }
784 else
785 {
786 // all fixed bodies (inf mass) get mapped to a single solver id
787 if ( m_fixedBodyId < 0 )
788 {
789 m_fixedBodyId = m_tmpSolverBodyPool.size();
790 btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
791 initSolverBody( &fixedBody, 0, timeStep );
792 }
793 solverBodyId = m_fixedBodyId;
794 }
795 btAssert( solverBodyId < m_tmpSolverBodyPool.size() );
796 return solverBodyId;
797#else // BT_THREADSAFE
798
799 int solverBodyIdA = -1;
800
801 if (body.getCompanionId() >= 0)
802 {
803 //body has already been converted
804 solverBodyIdA = body.getCompanionId();
805 btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
806 } else
807 {
808 btRigidBody* rb = btRigidBody::upcast(&body);
809 //convert both active and kinematic objects (for their velocity)
810 if (rb && (rb->getInvMass() || rb->isKinematicObject()))
811 {
812 solverBodyIdA = m_tmpSolverBodyPool.size();
813 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
814 initSolverBody(&solverBody,&body,timeStep);
815 body.setCompanionId(solverBodyIdA);
816 } else
817 {
818
819 if (m_fixedBodyId<0)
820 {
821 m_fixedBodyId = m_tmpSolverBodyPool.size();
822 btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
823 initSolverBody(&fixedBody,0,timeStep);
824 }
825 return m_fixedBodyId;
826// return 0;//assume first one is a fixed solver body
827 }
828 }
829
830 return solverBodyIdA;
831#endif // BT_THREADSAFE
832
833}
834#include <stdio.h>
835
836
837void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
838 int solverBodyIdA, int solverBodyIdB,
839 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
840 btScalar& relaxation,
841 const btVector3& rel_pos1, const btVector3& rel_pos2)
842{
843
844 // const btVector3& pos1 = cp.getPositionWorldOnA();
845 // const btVector3& pos2 = cp.getPositionWorldOnB();
846
847 btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
848 btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
849
850 btRigidBody* rb0 = bodyA->m_originalBody;
851 btRigidBody* rb1 = bodyB->m_originalBody;
852
853// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
854// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
855 //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
856 //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
857
858 relaxation = infoGlobal.m_sor;
859 btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
860
861 //cfm = 1 / ( dt * kp + kd )
862 //erp = dt * kp / ( dt * kp + kd )
863
864 btScalar cfm = infoGlobal.m_globalCfm;
865 btScalar erp = infoGlobal.m_erp2;
866
867 if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
868 {
869 if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
870 cfm = cp.m_contactCFM;
871 if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
872 erp = cp.m_contactERP;
873 } else
874 {
875 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
876 {
877 btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
878 if (denom < SIMD_EPSILON)
879 {
880 denom = SIMD_EPSILON;
881 }
882 cfm = btScalar(1) / denom;
883 erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
884 }
885 }
886
887 cfm *= invTimeStep;
888
889
890 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
891 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
892 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
893 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
894
895 {
896#ifdef COMPUTE_IMPULSE_DENOM
897 btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
898 btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
899#else
900 btVector3 vec;
901 btScalar denom0 = 0.f;
902 btScalar denom1 = 0.f;
903 if (rb0)
904 {
905 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
906 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
907 }
908 if (rb1)
909 {
910 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
911 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
912 }
913#endif //COMPUTE_IMPULSE_DENOM
914
915 btScalar denom = relaxation/(denom0+denom1+cfm);
916 solverConstraint.m_jacDiagABInv = denom;
917 }
918
919 if (rb0)
920 {
921 solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
922 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
923 } else
924 {
925 solverConstraint.m_contactNormal1.setZero();
926 solverConstraint.m_relpos1CrossNormal.setZero();
927 }
928 if (rb1)
929 {
930 solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
931 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
932 }else
933 {
934 solverConstraint.m_contactNormal2.setZero();
935 solverConstraint.m_relpos2CrossNormal.setZero();
936 }
937
938 btScalar restitution = 0.f;
939 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
940
941 {
942 btVector3 vel1,vel2;
943
944 vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
945 vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
946
947 // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
948 btVector3 vel = vel1 - vel2;
949 btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
950
951
952
953 solverConstraint.m_friction = cp.m_combinedFriction;
954
955
956 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
957 if (restitution <= btScalar(0.))
958 {
959 restitution = 0.f;
960 };
961 }
962
963
965 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
966 {
967 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
968 if (rb0)
969 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
970 if (rb1)
971 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
972 } else
973 {
974 solverConstraint.m_appliedImpulse = 0.f;
975 }
976
977 solverConstraint.m_appliedPushImpulse = 0.f;
978
979 {
980
981 btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0);
982 btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
983 btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0);
984 btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
985
986
987 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
988 + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
989 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
990 + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
991 btScalar rel_vel = vel1Dotn+vel2Dotn;
992
993 btScalar positionalError = 0.f;
994 btScalar velocityError = restitution - rel_vel;// * damping;
995
996
997
998 if (penetration>0)
999 {
1000 positionalError = 0;
1001
1002 velocityError -= penetration *invTimeStep;
1003 } else
1004 {
1005 positionalError = -penetration * erp*invTimeStep;
1006
1007 }
1008
1009 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1010 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1011
1012 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
1013 {
1014 //combine position and velocity into rhs
1015 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
1016 solverConstraint.m_rhsPenetration = 0.f;
1017
1018 } else
1019 {
1020 //split position and velocity into rhs and m_rhsPenetration
1021 solverConstraint.m_rhs = velocityImpulse;
1022 solverConstraint.m_rhsPenetration = penetrationImpulse;
1023 }
1024 solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
1025 solverConstraint.m_lowerLimit = 0;
1026 solverConstraint.m_upperLimit = 1e10f;
1027 }
1028
1029
1030
1031
1032}
1033
1034
1035
1036void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
1037 int solverBodyIdA, int solverBodyIdB,
1038 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
1039{
1040
1041 btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1042 btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1043
1044 btRigidBody* rb0 = bodyA->m_originalBody;
1045 btRigidBody* rb1 = bodyB->m_originalBody;
1046
1047 {
1048 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
1049 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1050 {
1051 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
1052 if (rb0)
1053 bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
1054 if (rb1)
1055 bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
1056 } else
1057 {
1058 frictionConstraint1.m_appliedImpulse = 0.f;
1059 }
1060 }
1061
1062 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1063 {
1064 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
1065 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1066 {
1067 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
1068 if (rb0)
1069 bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
1070 if (rb1)
1071 bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
1072 } else
1073 {
1074 frictionConstraint2.m_appliedImpulse = 0.f;
1075 }
1076 }
1077}
1078
1079
1080
1081
1082void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
1083{
1084 btCollisionObject* colObj0=0,*colObj1=0;
1085
1086 colObj0 = (btCollisionObject*)manifold->getBody0();
1087 colObj1 = (btCollisionObject*)manifold->getBody1();
1088
1089 int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
1090 int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
1091
1092// btRigidBody* bodyA = btRigidBody::upcast(colObj0);
1093// btRigidBody* bodyB = btRigidBody::upcast(colObj1);
1094
1095 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
1096 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
1097
1098
1099
1101 if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
1102 return;
1103
1104 int rollingFriction=1;
1105 for (int j=0;j<manifold->getNumContacts();j++)
1106 {
1107
1108 btManifoldPoint& cp = manifold->getContactPoint(j);
1109
1110 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1111 {
1112 btVector3 rel_pos1;
1113 btVector3 rel_pos2;
1114 btScalar relaxation;
1115
1116
1117 int frictionIndex = m_tmpSolverContactConstraintPool.size();
1118 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
1119 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1120 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1121
1122 solverConstraint.m_originalContactPoint = &cp;
1123
1124 const btVector3& pos1 = cp.getPositionWorldOnA();
1125 const btVector3& pos2 = cp.getPositionWorldOnB();
1126
1127 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
1128 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
1129
1130 btVector3 vel1;
1131 btVector3 vel2;
1132
1133 solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
1134 solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
1135
1136 btVector3 vel = vel1 - vel2;
1137 btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
1138
1139 setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
1140
1141
1142
1143
1145
1146 solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
1147
1148 if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
1149 {
1150
1151 {
1152 addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1153 btVector3 axis0,axis1;
1154 btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
1155 axis0.normalize();
1156 axis1.normalize();
1157
1158 applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1159 applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1160 applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1161 applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1162 if (axis0.length()>0.001)
1163 addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1164 cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1165 if (axis1.length()>0.001)
1166 addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
1167 cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1168
1169 }
1170 }
1171
1176
1187
1188 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
1189 {
1190 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1191 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1192 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
1193 {
1194 cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1195 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1196 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1197 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,infoGlobal);
1198
1199 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1200 {
1201 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
1202 cp.m_lateralFrictionDir2.normalize();//??
1203 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1204 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1205 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
1206 }
1207
1208 } else
1209 {
1210 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
1211
1212 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1213 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1214 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
1215
1216 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1217 {
1218 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1219 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1220 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
1221 }
1222
1223
1224 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
1225 {
1226 cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
1227 }
1228 }
1229
1230 } else
1231 {
1232 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1233
1234 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1235 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1236
1237 }
1238 setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1239
1240
1241
1242
1243 }
1244 }
1245}
1246
1247void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
1248{
1249 int i;
1250 btPersistentManifold* manifold = 0;
1251// btCollisionObject* colObj0=0,*colObj1=0;
1252
1253
1254 for (i=0;i<numManifolds;i++)
1255 {
1256 manifold = manifoldPtr[i];
1257 convertContact(manifold,infoGlobal);
1258 }
1259}
1260
1261btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1262{
1263 m_fixedBodyId = -1;
1264 BT_PROFILE("solveGroupCacheFriendlySetup");
1265 (void)debugDrawer;
1266
1267 // if solver mode has changed,
1268 if ( infoGlobal.m_solverMode != m_cachedSolverMode )
1269 {
1270 // update solver functions to use SIMD or non-SIMD
1271 bool useSimd = !!( infoGlobal.m_solverMode & SOLVER_SIMD );
1272 setupSolverFunctions( useSimd );
1273 m_cachedSolverMode = infoGlobal.m_solverMode;
1274 }
1275 m_maxOverrideNumSolverIterations = 0;
1276
1277#ifdef BT_ADDITIONAL_DEBUG
1278 //make sure that dynamic bodies exist for all (enabled) constraints
1279 for (int i=0;i<numConstraints;i++)
1280 {
1281 btTypedConstraint* constraint = constraints[i];
1282 if (constraint->isEnabled())
1283 {
1284 if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
1285 {
1286 bool found=false;
1287 for (int b=0;b<numBodies;b++)
1288 {
1289
1290 if (&constraint->getRigidBodyA()==bodies[b])
1291 {
1292 found = true;
1293 break;
1294 }
1295 }
1296 btAssert(found);
1297 }
1298 if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
1299 {
1300 bool found=false;
1301 for (int b=0;b<numBodies;b++)
1302 {
1303 if (&constraint->getRigidBodyB()==bodies[b])
1304 {
1305 found = true;
1306 break;
1307 }
1308 }
1309 btAssert(found);
1310 }
1311 }
1312 }
1313 //make sure that dynamic bodies exist for all contact manifolds
1314 for (int i=0;i<numManifolds;i++)
1315 {
1316 if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
1317 {
1318 bool found=false;
1319 for (int b=0;b<numBodies;b++)
1320 {
1321
1322 if (manifoldPtr[i]->getBody0()==bodies[b])
1323 {
1324 found = true;
1325 break;
1326 }
1327 }
1328 btAssert(found);
1329 }
1330 if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
1331 {
1332 bool found=false;
1333 for (int b=0;b<numBodies;b++)
1334 {
1335 if (manifoldPtr[i]->getBody1()==bodies[b])
1336 {
1337 found = true;
1338 break;
1339 }
1340 }
1341 btAssert(found);
1342 }
1343 }
1344#endif //BT_ADDITIONAL_DEBUG
1345
1346
1347 for (int i = 0; i < numBodies; i++)
1348 {
1349 bodies[i]->setCompanionId(-1);
1350 }
1351#if BT_THREADSAFE
1352 m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 );
1353#endif // BT_THREADSAFE
1354
1355 m_tmpSolverBodyPool.reserve(numBodies+1);
1356 m_tmpSolverBodyPool.resize(0);
1357
1358 //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
1359 //initSolverBody(&fixedBody,0);
1360
1361 //convert all bodies
1362
1363
1364 for (int i=0;i<numBodies;i++)
1365 {
1366 int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
1367
1368 btRigidBody* body = btRigidBody::upcast(bodies[i]);
1369 if (body && body->getInvMass())
1370 {
1371 btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1372 btVector3 gyroForce (0,0,0);
1373 if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
1374 {
1375 gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
1376 solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1377 }
1378 if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
1379 {
1380 gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
1381 solverBody.m_externalTorqueImpulse += gyroForce;
1382 }
1383 if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
1384 {
1385 gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
1386 solverBody.m_externalTorqueImpulse += gyroForce;
1387
1388 }
1389
1390
1391 }
1392 }
1393
1394 if (1)
1395 {
1396 int j;
1397 for (j=0;j<numConstraints;j++)
1398 {
1399 btTypedConstraint* constraint = constraints[j];
1400 constraint->buildJacobian();
1401 constraint->internalSetAppliedImpulse(0.0f);
1402 }
1403 }
1404
1405 //btRigidBody* rb0=0,*rb1=0;
1406
1407 //if (1)
1408 {
1409 {
1410
1411 int totalNumRows = 0;
1412 int i;
1413
1414 m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
1415 //calculate the total number of contraint rows
1416 for (i=0;i<numConstraints;i++)
1417 {
1418 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1419 btJointFeedback* fb = constraints[i]->getJointFeedback();
1420 if (fb)
1421 {
1422 fb->m_appliedForceBodyA.setZero();
1423 fb->m_appliedTorqueBodyA.setZero();
1424 fb->m_appliedForceBodyB.setZero();
1425 fb->m_appliedTorqueBodyB.setZero();
1426 }
1427
1428 if (constraints[i]->isEnabled())
1429 {
1430 }
1431 if (constraints[i]->isEnabled())
1432 {
1433 constraints[i]->getInfo1(&info1);
1434 } else
1435 {
1436 info1.m_numConstraintRows = 0;
1437 info1.nub = 0;
1438 }
1439 totalNumRows += info1.m_numConstraintRows;
1440 }
1441 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
1442
1443
1445 int currentRow = 0;
1446
1447 for (i=0;i<numConstraints;i++)
1448 {
1449 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1450
1451 if (info1.m_numConstraintRows)
1452 {
1453 btAssert(currentRow<totalNumRows);
1454
1455 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1456 btTypedConstraint* constraint = constraints[i];
1457 btRigidBody& rbA = constraint->getRigidBodyA();
1458 btRigidBody& rbB = constraint->getRigidBodyB();
1459
1460 int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep);
1461 int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep);
1462
1463 btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1464 btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1465
1466
1467
1468
1469 int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1470 if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
1471 m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1472
1473
1474 int j;
1475 for ( j=0;j<info1.m_numConstraintRows;j++)
1476 {
1477 memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1478 currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1479 currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1480 currentConstraintRow[j].m_appliedImpulse = 0.f;
1481 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1482 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1483 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1484 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1485 }
1486
1487 bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1488 bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1489 bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1490 bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1491 bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1492 bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1493 bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1494 bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1495
1496
1497 btTypedConstraint::btConstraintInfo2 info2;
1498 info2.fps = 1.f/infoGlobal.m_timeStep;
1499 info2.erp = infoGlobal.m_erp;
1500 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1;
1501 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1502 info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
1503 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1504 info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1506 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1507 info2.m_constraintError = &currentConstraintRow->m_rhs;
1508 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1509 info2.m_damping = infoGlobal.m_damping;
1510 info2.cfm = &currentConstraintRow->m_cfm;
1511 info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1512 info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1513 info2.m_numIterations = infoGlobal.m_numIterations;
1514 constraints[i]->getInfo2(&info2);
1515
1517 for ( j=0;j<info1.m_numConstraintRows;j++)
1518 {
1519 btSolverConstraint& solverConstraint = currentConstraintRow[j];
1520
1521 if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
1522 {
1523 solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
1524 }
1525
1526 if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
1527 {
1528 solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
1529 }
1530
1531 solverConstraint.m_originalContactPoint = constraint;
1532
1533 {
1534 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1535 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1536 }
1537 {
1538 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1539 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1540 }
1541
1542 {
1543 btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass();
1544 btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1545 btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal?
1546 btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1547
1548 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1);
1549 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1550 sum += iMJlB.dot(solverConstraint.m_contactNormal2);
1551 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1552 btScalar fsum = btFabs(sum);
1553 btAssert(fsum > SIMD_EPSILON);
1554 btScalar sorRelaxation = 1.f;//todo: get from globalInfo?
1555 solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f;
1556 }
1557
1558
1559
1560 {
1561 btScalar rel_vel;
1562 btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0);
1563 btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1564
1565 btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0);
1566 btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
1567
1568 btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
1569 + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
1570
1571 btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
1572 + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
1573
1574 rel_vel = vel1Dotn+vel2Dotn;
1575 btScalar restitution = 0.f;
1576 btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1577 btScalar velocityError = restitution - rel_vel * info2.m_damping;
1578 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1579 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1580 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1581 solverConstraint.m_appliedImpulse = 0.f;
1582
1583
1584 }
1585 }
1586 }
1587 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1588 }
1589 }
1590
1591 convertContacts(manifoldPtr,numManifolds,infoGlobal);
1592
1593 }
1594
1595// btContactSolverInfo info = infoGlobal;
1596
1597
1598 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1599 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1600 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1601
1603 m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
1604 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1605 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
1606 else
1607 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1608
1609 m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
1610 {
1611 int i;
1612 for (i=0;i<numNonContactPool;i++)
1613 {
1614 m_orderNonContactConstraintPool[i] = i;
1615 }
1616 for (i=0;i<numConstraintPool;i++)
1617 {
1618 m_orderTmpConstraintPool[i] = i;
1619 }
1620 for (i=0;i<numFrictionPool;i++)
1621 {
1622 m_orderFrictionConstraintPool[i] = i;
1623 }
1624 }
1625
1626 return 0.f;
1627
1628}
1629
1630
1631btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
1632{
1633 btScalar leastSquaresResidual = 0.f;
1634
1635 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1636 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1637 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1638
1639 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1640 {
1641 if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
1642 {
1643
1644 for (int j=0; j<numNonContactPool; ++j) {
1645 int tmp = m_orderNonContactConstraintPool[j];
1646 int swapi = btRandInt2(j+1);
1647 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
1648 m_orderNonContactConstraintPool[swapi] = tmp;
1649 }
1650
1651 //contact/friction constraints are not solved more than
1652 if (iteration< infoGlobal.m_numIterations)
1653 {
1654 for (int j=0; j<numConstraintPool; ++j) {
1655 int tmp = m_orderTmpConstraintPool[j];
1656 int swapi = btRandInt2(j+1);
1657 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
1658 m_orderTmpConstraintPool[swapi] = tmp;
1659 }
1660
1661 for (int j=0; j<numFrictionPool; ++j) {
1662 int tmp = m_orderFrictionConstraintPool[j];
1663 int swapi = btRandInt2(j+1);
1664 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
1665 m_orderFrictionConstraintPool[swapi] = tmp;
1666 }
1667 }
1668 }
1669 }
1670
1672 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1673 {
1674 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1675 if (iteration < constraint.m_overrideNumSolverIterations)
1676 {
1677 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1678 leastSquaresResidual += residual*residual;
1679 }
1680 }
1681
1682 if (iteration< infoGlobal.m_numIterations)
1683 {
1684 for (int j=0;j<numConstraints;j++)
1685 {
1686 if (constraints[j]->isEnabled())
1687 {
1688 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
1689 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1690 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1691 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1692 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1693 }
1694 }
1695
1697 if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
1698 {
1699 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1700 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
1701
1702 for (int c=0;c<numPoolConstraints;c++)
1703 {
1704 btScalar totalImpulse =0;
1705
1706 {
1707 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
1708 btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1709 leastSquaresResidual += residual*residual;
1710
1711 totalImpulse = solveManifold.m_appliedImpulse;
1712 }
1713 bool applyFriction = true;
1714 if (applyFriction)
1715 {
1716 {
1717
1718 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
1719
1720 if (totalImpulse>btScalar(0))
1721 {
1722 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1723 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1724
1725 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1726 leastSquaresResidual += residual*residual;
1727 }
1728 }
1729
1730 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
1731 {
1732
1733 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
1734
1735 if (totalImpulse>btScalar(0))
1736 {
1737 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1738 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1739
1740 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1741 leastSquaresResidual += residual*residual;
1742 }
1743 }
1744 }
1745 }
1746
1747 }
1748 else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1749 {
1750 //solve the friction constraints after all contact constraints, don't interleave them
1751 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1752 int j;
1753
1754 for (j=0;j<numPoolConstraints;j++)
1755 {
1756 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1757 btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1758 leastSquaresResidual += residual*residual;
1759 }
1760
1761
1762
1764
1765 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1766 for (j=0;j<numFrictionPoolConstraints;j++)
1767 {
1768 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1769 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1770
1771 if (totalImpulse>btScalar(0))
1772 {
1773 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1774 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1775
1776 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1777 leastSquaresResidual += residual*residual;
1778 }
1779 }
1780 }
1781
1782
1783 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1784 for (int j=0;j<numRollingFrictionPoolConstraints;j++)
1785 {
1786
1787 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1788 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1789 if (totalImpulse>btScalar(0))
1790 {
1791 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1792 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1793 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1794
1795 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1796 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1797
1798 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1799 leastSquaresResidual += residual*residual;
1800 }
1801 }
1802
1803
1804 }
1805 return leastSquaresResidual;
1806}
1807
1808
1809void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1810{
1811 int iteration;
1812 if (infoGlobal.m_splitImpulse)
1813 {
1814 {
1815 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1816 {
1817 btScalar leastSquaresResidual =0.f;
1818 {
1819 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1820 int j;
1821 for (j=0;j<numPoolConstraints;j++)
1822 {
1823 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1824
1825 btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1826 leastSquaresResidual += residual*residual;
1827 }
1828 }
1829 if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1))
1830 {
1831#ifdef VERBOSE_RESIDUAL_PRINTF
1832 printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration);
1833#endif
1834 break;
1835 }
1836 }
1837 }
1838 }
1839}
1840
1841btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
1842{
1843 BT_PROFILE("solveGroupCacheFriendlyIterations");
1844
1845 {
1847 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1848
1849 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
1850
1851 for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1852 //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
1853 {
1854 m_leastSquaresResidual = solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
1855
1856 if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration>= (maxIterations-1)))
1857 {
1858#ifdef VERBOSE_RESIDUAL_PRINTF
1859 printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration);
1860#endif
1861 break;
1862 }
1863 }
1864
1865 }
1866 return 0.f;
1867}
1868
1869btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
1870{
1871 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1872 int i,j;
1873
1874 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1875 {
1876 for (j=0;j<numPoolConstraints;j++)
1877 {
1878 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1879 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1880 btAssert(pt);
1881 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1882 // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1883 // printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1884 pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1885 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1886 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1887 {
1888 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1889 }
1890 //do a callback here?
1891 }
1892 }
1893
1894 numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1895 for (j=0;j<numPoolConstraints;j++)
1896 {
1897 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1898 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1899 btJointFeedback* fb = constr->getJointFeedback();
1900 if (fb)
1901 {
1902 fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1903 fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1904 fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1905 fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1906
1907 }
1908
1909 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1910 if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1911 {
1912 constr->setEnabled(false);
1913 }
1914 }
1915
1916
1917
1918 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1919 {
1920 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1921 if (body)
1922 {
1923 if (infoGlobal.m_splitImpulse)
1924 m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1925 else
1926 m_tmpSolverBodyPool[i].writebackVelocity();
1927
1928 m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
1929 m_tmpSolverBodyPool[i].m_linearVelocity+
1930 m_tmpSolverBodyPool[i].m_externalForceImpulse);
1931
1932 m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
1933 m_tmpSolverBodyPool[i].m_angularVelocity+
1934 m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
1935
1936 if (infoGlobal.m_splitImpulse)
1937 m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1938
1939 m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1940 }
1941 }
1942
1943 m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1944 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1945 m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1946 m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1947
1948 m_tmpSolverBodyPool.resizeNoInitialize(0);
1949 return 0.f;
1950}
1951
1952
1953
1955btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/)
1956{
1957
1958 BT_PROFILE("solveGroup");
1959 //you need to provide at least some bodies
1960
1961 solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1962
1963 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer);
1964
1965 solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1966
1967 return 0.f;
1968}
1969
1970void btSequentialImpulseConstraintSolver::reset()
1971{
1972 m_btSeed2 = 0;
1973}
SOLVER_SIMD
@ SOLVER_SIMD
Definition btContactSolverInfo.h:30
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
@ SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
Definition btContactSolverInfo.h:27
SOLVER_USE_WARMSTARTING
@ SOLVER_USE_WARMSTARTING
Definition btContactSolverInfo.h:25
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
@ SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
Definition btContactSolverInfo.h:31
SOLVER_RANDMIZE_ORDER
@ SOLVER_RANDMIZE_ORDER
Definition btContactSolverInfo.h:23
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
@ SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
Definition btContactSolverInfo.h:28
SOLVER_USE_2_FRICTION_DIRECTIONS
@ SOLVER_USE_2_FRICTION_DIRECTIONS
Definition btContactSolverInfo.h:26
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
Definition btManifoldPoint.h:40
BT_CONTACT_FLAG_HAS_CONTACT_ERP
@ BT_CONTACT_FLAG_HAS_CONTACT_ERP
Definition btManifoldPoint.h:42
BT_CONTACT_FLAG_HAS_CONTACT_CFM
@ BT_CONTACT_FLAG_HAS_CONTACT_CFM
Definition btManifoldPoint.h:41
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
@ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
Definition btManifoldPoint.h:43
BT_CONTACT_FLAG_FRICTION_ANCHOR
@ BT_CONTACT_FLAG_FRICTION_ANCHOR
Definition btManifoldPoint.h:44
btMax
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:29
maxIterations
int maxIterations
Definition btQuantizedBvh.cpp:370
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition btQuaternion.h:878
BT_PROFILE
#define BT_PROFILE(name)
Definition btQuickprof.h:215
uniqueId
static int uniqueId
Definition btRigidBody.cpp:27
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
Definition btRigidBody.h:49
BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT
@ BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT
BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
Definition btRigidBody.h:47
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD
Definition btRigidBody.h:48
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:292
btSqrt
btScalar btSqrt(btScalar y)
Definition btScalar.h:444
btFabs
btScalar btFabs(btScalar x)
Definition btScalar.h:475
SIMD_INFINITY
#define SIMD_INFINITY
Definition btScalar.h:522
SIMD_EPSILON
#define SIMD_EPSILON
Definition btScalar.h:521
btAssert
#define btAssert(x)
Definition btScalar.h:131
gResolveSplitPenetrationImpulse_sse2
static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody &body1, btSolverBody &body2, const btSolverConstraint &c)
Definition btSequentialImpulseConstraintSolver.cpp:325
gResolveSingleConstraintRowGeneric_scalar_reference
static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody &body1, btSolverBody &body2, const btSolverConstraint &c)
This is the scalar reference implementation of solving a single constraint row, the innerloop of the ...
Definition btSequentialImpulseConstraintSolver.cpp:45
gResolveSplitPenetrationImpulse_scalar_reference
static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference(btSolverBody &body1, btSolverBody &body2, const btSolverConstraint &c)
Definition btSequentialImpulseConstraintSolver.cpp:293
gResolveSingleConstraintRowLowerLimit_scalar_reference
static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody &body1, btSolverBody &body2, const btSolverConstraint &c)
Definition btSequentialImpulseConstraintSolver.cpp:78
btSingleConstraintRowSolver
btSimdScalar(* btSingleConstraintRowSolver)(btSolverBody &, btSolverBody &, const btSolverConstraint &)
Definition btSequentialImpulseConstraintSolver.h:30
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition btSoftBodyHelpers.cpp:84
btSimdScalar
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
Definition btSolverBody.h:104
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition btVector3.h:1283
btAlignedObjectArray::resizeNoInitialize
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
Definition btAlignedObjectArray.h:209
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition btAlignedObjectArray.h:155
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition btAlignedObjectArray.h:218
btAlignedObjectArray::expand
T & expand(const T &fillValue=T())
Definition btAlignedObjectArray.h:258
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition btCollisionObject.h:50
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition btCollisionObject.h:207
btCollisionObject::getWorldArrayIndex
int getWorldArrayIndex() const
Definition btCollisionObject.h:463
btCollisionObject::setCompanionId
void setCompanionId(int id)
Definition btCollisionObject.h:458
btCollisionObject::isKinematicObject
bool isKinematicObject() const
Definition btCollisionObject.h:202
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition btDispatcher.h:76
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition btIDebugDraw.h:30
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition btManifoldPoint.h:50
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition btPersistentManifold.h:64
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition btRigidBody.h:63
btRigidBody::getFlags
int getFlags() const
Definition btRigidBody.h:533
btRigidBody::getInvMass
btScalar getInvMass() const
Definition btRigidBody.h:273
btRigidBody::computeGyroscopicImpulseImplicit_World
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const
perform implicit force computation in world space
Definition btRigidBody.cpp:350
btRigidBody::computeGyroscopicImpulseImplicit_Body
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const
perform implicit force computation in body space (inertial frame)
Definition btRigidBody.cpp:309
btRigidBody::upcast
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
Definition btRigidBody.h:203
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition btRigidBody.h:274
btRigidBody::computeGyroscopicForceExplicit
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
explicit version is best avoided, it gains energy
Definition btRigidBody.cpp:294
btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulse
btSimdScalar resolveSplitPenetrationImpulse(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition btSequentialImpulseConstraintSolver.h:117
btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit()
Various implementations of solving a single constraint row using an inequality (lower limit) constrai...
Definition btSequentialImpulseConstraintSolver.cpp:403
btSequentialImpulseConstraintSolver::initSolverBody
void initSolverBody(btSolverBody *solverBody, btCollisionObject *collisionObject, btScalar timeStep)
Definition btSequentialImpulseConstraintSolver.cpp:468
btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric()
btSequentialImpulseConstraintSolver::addFrictionConstraint
btSolverConstraint & addFrictionConstraint(const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint &cp, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
Definition btSequentialImpulseConstraintSolver.cpp:640
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit
btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition btSequentialImpulseConstraintSolver.cpp:287
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition btSequentialImpulseConstraintSolver.cpp:1869
btSequentialImpulseConstraintSolver::solveGroup
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
btSequentialImpulseConstraintSolver Sequentially applies impulses
Definition btSequentialImpulseConstraintSolver.cpp:1955
btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint
btSolverConstraint & addTorsionalFrictionConstraint(const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint &cp, btScalar torsionalFriction, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f)
Definition btSequentialImpulseConstraintSolver.cpp:727
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btSequentialImpulseConstraintSolver.cpp:1841
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btSequentialImpulseConstraintSolver.cpp:1809
btSequentialImpulseConstraintSolver::m_btSeed2
unsigned long m_btSeed2
m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
Definition btSequentialImpulseConstraintSolver.h:89
btSequentialImpulseConstraintSolver::reset
virtual void reset()
clear internal cached data and reset random seed
Definition btSequentialImpulseConstraintSolver.cpp:1970
btSequentialImpulseConstraintSolver::convertContacts
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
Definition btSequentialImpulseConstraintSolver.cpp:1247
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric
btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition btSequentialImpulseConstraintSolver.cpp:276
btSequentialImpulseConstraintSolver::applyAnisotropicFriction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
Definition btSequentialImpulseConstraintSolver.cpp:523
btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit()
btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint
void setupTorsionalFrictionConstraint(btSolverConstraint &solverConstraint, const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
Definition btSequentialImpulseConstraintSolver.cpp:650
btSequentialImpulseConstraintSolver::convertContact
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition btSequentialImpulseConstraintSolver.cpp:1082
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition btSequentialImpulseConstraintSolver.h:36
btSequentialImpulseConstraintSolver::m_resolveSingleConstraintRowLowerLimit
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit
Definition btSequentialImpulseConstraintSolver.h:58
btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric()
Various implementations of solving a single constraint row using a generic equality constraint,...
Definition btSequentialImpulseConstraintSolver.cpp:398
btSequentialImpulseConstraintSolver::m_tmpConstraintSizesPool
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
Definition btSequentialImpulseConstraintSolver.h:45
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD
btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition btSequentialImpulseConstraintSolver.cpp:281
btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric()
btSequentialImpulseConstraintSolver::m_kinematicBodyUniqueIdToSolverBodyTable
btAlignedObjectArray< int > m_kinematicBodyUniqueIdToSolverBodyTable
Definition btSequentialImpulseConstraintSolver.h:55
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btSequentialImpulseConstraintSolver.cpp:1261
btSequentialImpulseConstraintSolver::setupFrictionConstraint
void 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, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
Definition btSequentialImpulseConstraintSolver.cpp:543
btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD
btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
Definition btSequentialImpulseConstraintSolver.cpp:270
btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse
void setFrictionConstraintImpulse(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal)
Definition btSequentialImpulseConstraintSolver.cpp:1036
btSequentialImpulseConstraintSolver::getOrInitSolverBody
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
Definition btSequentialImpulseConstraintSolver.cpp:737
btSequentialImpulseConstraintSolver::restitutionCurve
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
Definition btSequentialImpulseConstraintSolver.cpp:511
btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit()
btSequentialImpulseConstraintSolver::solveSingleIteration
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btSequentialImpulseConstraintSolver.cpp:1631
btSequentialImpulseConstraintSolver::setupContactConstraint
void setupContactConstraint(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, const btVector3 &rel_pos1, const btVector3 &rel_pos2)
Definition btSequentialImpulseConstraintSolver.cpp:837
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition btTypedConstraint.h:79
btTypedConstraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
Definition btTypedConstraint.h:168
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:84
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition btVector3.h:389
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:235
btVector3::setZero
void setZero()
Definition btVector3.h:683
btJointFeedback::m_appliedForceBodyA
btVector3 m_appliedForceBodyA
Definition btTypedConstraint.h:70
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition btSolverBody.h:109
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition btSolverConstraint.h:31
btSolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition btSolverConstraint.h:44
btSolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition btSolverConstraint.h:43
Generated by doxygen 1.10.0