Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/resource/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @ 3328

Last change on this file since 3328 was 2882, checked in by rgrieder, 16 years ago
Update from Bullet 2.73 to 2.74.
Property svn:eol-style set to `native`
File size: 39.8 KB

Line
1	/*
2	Bullet Continuous Collision Detection and Physics Library
3	Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5	This software is provided 'as-is', without any express or implied warranty.
6	In no event will the authors be held liable for any damages arising from the use of this software.
7	Permission is granted to anyone to use this software for any purpose,
8	including commercial applications, and to alter it and redistribute it freely,
9	subject to the following restrictions:
10
11	1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12	2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13	3. This notice may not be removed or altered from any source distribution.
14	*/
15
16	//#define COMPUTE_IMPULSE_DENOM 1
17	//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
18
19	#include "btSequentialImpulseConstraintSolver.h"
20	#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
21	#include "BulletDynamics/Dynamics/btRigidBody.h"
22	#include "btContactConstraint.h"
23	#include "btSolve2LinearConstraint.h"
24	#include "btContactSolverInfo.h"
25	#include "LinearMath/btIDebugDraw.h"
26	#include "btJacobianEntry.h"
27	#include "LinearMath/btMinMax.h"
28	#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
29	#include <new>
30	#include "LinearMath/btStackAlloc.h"
31	#include "LinearMath/btQuickprof.h"
32	#include "btSolverBody.h"
33	#include "btSolverConstraint.h"
34	#include "LinearMath/btAlignedObjectArray.h"
35	#include <string.h> //for memset
36
37	btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
38	:m_btSeed2(0)
39	{
40
41	}
42
43	btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
44	{
45	}
46
47	#ifdef USE_SIMD
48	#include <emmintrin.h>
49	#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
50	static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
51	{
52	__m128 result = _mm_mul_ps( vec0, vec1);
53	return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
54	}
55	#endif//USE_SIMD
56
57	// Project Gauss Seidel or the equivalent Sequential Impulse
58	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
59	{
60	#ifdef USE_SIMD
61	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
62	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
63	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
64	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
65	__m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
66	__m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
67	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
68	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
69	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
70	btSimdScalar resultLowerLess,resultUpperLess;
71	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
72	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
73	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
74	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
75	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
76	__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
77	deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
78	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
79	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
80	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
81	__m128 impulseMagnitude = deltaImpulse;
82	body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
83	body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
84	body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
85	body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
86	#else
87	resolveSingleConstraintRowGeneric(body1,body2,c);
88	#endif
89	}
90
91	// Project Gauss Seidel or the equivalent Sequential Impulse
92	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
93	{
94	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
95	const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
96	const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
97
98	const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
99	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
100	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
101
102	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
103	if (sum < c.m_lowerLimit)
104	{
105	deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
106	c.m_appliedImpulse = c.m_lowerLimit;
107	}
108	else if (sum > c.m_upperLimit)
109	{
110	deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
111	c.m_appliedImpulse = c.m_upperLimit;
112	}
113	else
114	{
115	c.m_appliedImpulse = sum;
116	}
117	if (body1.m_invMass)
118	body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
119	if (body2.m_invMass)
120	body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
121	}
122
123	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
124	{
125	#ifdef USE_SIMD
126	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
127	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
128	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
129	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
130	__m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
131	__m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
132	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
133	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
134	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
135	btSimdScalar resultLowerLess,resultUpperLess;
136	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
137	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
138	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
139	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
140	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
141	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
142	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
143	__m128 impulseMagnitude = deltaImpulse;
144	body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
145	body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
146	body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
147	body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
148	#else
149	resolveSingleConstraintRowLowerLimit(body1,body2,c);
150	#endif
151	}
152
153	// Project Gauss Seidel or the equivalent Sequential Impulse
154	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
155	{
156	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
157	const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
158	const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
159
160	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
161	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
162	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
163	if (sum < c.m_lowerLimit)
164	{
165	deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
166	c.m_appliedImpulse = c.m_lowerLimit;
167	}
168	else
169	{
170	c.m_appliedImpulse = sum;
171	}
172	if (body1.m_invMass)
173	body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
174	if (body2.m_invMass)
175	body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
176	}
177
178
179
180	unsigned long btSequentialImpulseConstraintSolver::btRand2()
181	{
182	m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
183	return m_btSeed2;
184	}
185
186
187
188	//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
189	int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
190	{
191	// seems good; xor-fold and modulus
192	const unsigned long un = static_cast<unsigned long>(n);
193	unsigned long r = btRand2();
194
195	// note: probably more aggressive than it needs to be -- might be
196	// able to get away without one or two of the innermost branches.
197	if (un <= 0x00010000UL) {
198	r ^= (r >> 16);
199	if (un <= 0x00000100UL) {
200	r ^= (r >> 8);
201	if (un <= 0x00000010UL) {
202	r ^= (r >> 4);
203	if (un <= 0x00000004UL) {
204	r ^= (r >> 2);
205	if (un <= 0x00000002UL) {
206	r ^= (r >> 1);
207	}
208	}
209	}
210	}
211	}
212
213	return (int) (r % un);
214	}
215
216
217
218	void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
219	{
220	btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
221
222	solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
223	solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
224
225	if (rb)
226	{
227	solverBody->m_invMass = rb->getInvMass();
228	solverBody->m_originalBody = rb;
229	solverBody->m_angularFactor = rb->getAngularFactor();
230	} else
231	{
232	solverBody->m_invMass = 0.f;
233	solverBody->m_originalBody = 0;
234	solverBody->m_angularFactor = 1.f;
235	}
236	}
237
238
239	int gNumSplitImpulseRecoveries = 0;
240
241	btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
242	{
243	btScalar rest = restitution * -rel_vel;
244	return rest;
245	}
246
247
248
249	void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
250	void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
251	{
252	if (colObj && colObj->hasAnisotropicFriction())
253	{
254	// transform to local coordinates
255	btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
256	const btVector3& friction_scaling = colObj->getAnisotropicFriction();
257	//apply anisotropic friction
258	loc_lateral *= friction_scaling;
259	// ... and transform it back to global coordinates
260	frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
261	}
262	}
263
264
265
266	btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
267	{
268
269
270	btRigidBody* body0=btRigidBody::upcast(colObj0);
271	btRigidBody* body1=btRigidBody::upcast(colObj1);
272
273	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
274	memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
275	solverConstraint.m_contactNormal = normalAxis;
276
277	solverConstraint.m_solverBodyIdA = solverBodyIdA;
278	solverConstraint.m_solverBodyIdB = solverBodyIdB;
279	solverConstraint.m_frictionIndex = frictionIndex;
280
281	solverConstraint.m_friction = cp.m_combinedFriction;
282	solverConstraint.m_originalContactPoint = 0;
283
284	solverConstraint.m_appliedImpulse = 0.f;
285	// solverConstraint.m_appliedPushImpulse = 0.f;
286
287	{
288	btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
289	solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
290	solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()ftorqueAxis1body0->getAngularFactor() : btVector3(0,0,0);
291	}
292	{
293	btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
294	solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
295	solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()ftorqueAxis1body1->getAngularFactor() : btVector3(0,0,0);
296	}
297
298	#ifdef COMPUTE_IMPULSE_DENOM
299	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
300	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
301	#else
302	btVector3 vec;
303	btScalar denom0 = 0.f;
304	btScalar denom1 = 0.f;
305	if (body0)
306	{
307	vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
308	denom0 = body0->getInvMass() + normalAxis.dot(vec);
309	}
310	if (body1)
311	{
312	vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
313	denom1 = body1->getInvMass() + normalAxis.dot(vec);
314	}
315
316
317	#endif //COMPUTE_IMPULSE_DENOM
318	btScalar denom = relaxation/(denom0+denom1);
319	solverConstraint.m_jacDiagABInv = denom;
320
321	#ifdef _USE_JACOBIAN
322	solverConstraint.m_jac = btJacobianEntry (
323	rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
324	body0->getInvInertiaDiagLocal(),
325	body0->getInvMass(),
326	body1->getInvInertiaDiagLocal(),
327	body1->getInvMass());
328	#endif //_USE_JACOBIAN
329
330
331	{
332	btScalar rel_vel;
333	btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
334	+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
335	btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
336	+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
337
338	rel_vel = vel1Dotn+vel2Dotn;
339
340	btScalar positionalError = 0.f;
341
342	btSimdScalar velocityError = - rel_vel;
343	btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
344	solverConstraint.m_rhs = velocityImpulse;
345	solverConstraint.m_cfm = 0.f;
346	solverConstraint.m_lowerLimit = 0;
347	solverConstraint.m_upperLimit = 1e10f;
348	}
349
350	return solverConstraint;
351	}
352
353	int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
354	{
355	int solverBodyIdA = -1;
356
357	if (body.getCompanionId() >= 0)
358	{
359	//body has already been converted
360	solverBodyIdA = body.getCompanionId();
361	} else
362	{
363	btRigidBody* rb = btRigidBody::upcast(&body);
364	if (rb && rb->getInvMass())
365	{
366	solverBodyIdA = m_tmpSolverBodyPool.size();
367	btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
368	initSolverBody(&solverBody,&body);
369	body.setCompanionId(solverBodyIdA);
370	} else
371	{
372	return 0;//assume first one is a fixed solver body
373	}
374	}
375	return solverBodyIdA;
376	}
377	#include <stdio.h>
378
379
380
381	void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
382	{
383	btCollisionObject* colObj0=0,*colObj1=0;
384
385	colObj0 = (btCollisionObject*)manifold->getBody0();
386	colObj1 = (btCollisionObject*)manifold->getBody1();
387
388	int solverBodyIdA=-1;
389	int solverBodyIdB=-1;
390
391	if (manifold->getNumContacts())
392	{
393	solverBodyIdA = getOrInitSolverBody(*colObj0);
394	solverBodyIdB = getOrInitSolverBody(*colObj1);
395	}
396
397	///avoid collision response between two static objects
398	if (!solverBodyIdA && !solverBodyIdB)
399	return;
400
401	btVector3 rel_pos1;
402	btVector3 rel_pos2;
403	btScalar relaxation;
404
405	for (int j=0;j<manifold->getNumContacts();j++)
406	{
407
408	btManifoldPoint& cp = manifold->getContactPoint(j);
409
410	if (cp.getDistance() <= manifold->getContactProcessingThreshold())
411	{
412
413	const btVector3& pos1 = cp.getPositionWorldOnA();
414	const btVector3& pos2 = cp.getPositionWorldOnB();
415
416	rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
417	rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
418
419
420	relaxation = 1.f;
421	btScalar rel_vel;
422	btVector3 vel;
423
424	int frictionIndex = m_tmpSolverContactConstraintPool.size();
425
426	{
427	btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
428	btRigidBody* rb0 = btRigidBody::upcast(colObj0);
429	btRigidBody* rb1 = btRigidBody::upcast(colObj1);
430
431	solverConstraint.m_solverBodyIdA = solverBodyIdA;
432	solverConstraint.m_solverBodyIdB = solverBodyIdB;
433
434	solverConstraint.m_originalContactPoint = &cp;
435
436	btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
437	solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()torqueAxis0rb0->getAngularFactor() : btVector3(0,0,0);
438	btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
439	solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()-torqueAxis1rb1->getAngularFactor() : btVector3(0,0,0);
440	{
441	#ifdef COMPUTE_IMPULSE_DENOM
442	btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
443	btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
444	#else
445	btVector3 vec;
446	btScalar denom0 = 0.f;
447	btScalar denom1 = 0.f;
448	if (rb0)
449	{
450	vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
451	denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
452	}
453	if (rb1)
454	{
455	vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
456	denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
457	}
458	#endif //COMPUTE_IMPULSE_DENOM
459
460	btScalar denom = relaxation/(denom0+denom1);
461	solverConstraint.m_jacDiagABInv = denom;
462	}
463
464	solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
465	solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
466	solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
467
468
469	btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
470	btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
471
472	vel = vel1 - vel2;
473
474	rel_vel = cp.m_normalWorldOnB.dot(vel);
475
476	btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
477
478
479	solverConstraint.m_friction = cp.m_combinedFriction;
480
481	btScalar restitution = 0.f;
482
483	if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
484	{
485	restitution = 0.f;
486	} else
487	{
488	restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
489	if (restitution <= btScalar(0.))
490	{
491	restitution = 0.f;
492	};
493	}
494
495
496	///warm starting (or zero if disabled)
497	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
498	{
499	solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
500	if (rb0)
501	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
502	if (rb1)
503	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
504	} else
505	{
506	solverConstraint.m_appliedImpulse = 0.f;
507	}
508
509	// solverConstraint.m_appliedPushImpulse = 0.f;
510
511	{
512	btScalar rel_vel;
513	btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
514	+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
515	btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
516	+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
517
518	rel_vel = vel1Dotn+vel2Dotn;
519
520	btScalar positionalError = 0.f;
521	positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
522	btScalar velocityError = restitution - rel_vel;// * damping;
523	btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
524	btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
525	solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
526	solverConstraint.m_cfm = 0.f;
527	solverConstraint.m_lowerLimit = 0;
528	solverConstraint.m_upperLimit = 1e10f;
529	}
530
531
532	/////setup the friction constraints
533
534
535
536	if (1)
537	{
538	solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
539	if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) \|\| !cp.m_lateralFrictionInitialized)
540	{
541	cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
542	btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
543	if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
544	{
545	cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
546	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
547	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
548	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
549	if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
550	{
551	cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
552	cp.m_lateralFrictionDir2.normalize();//??
553	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
554	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
555	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
556	}
557	cp.m_lateralFrictionInitialized = true;
558	} else
559	{
560	//re-calculate friction direction every frame, todo: check if this is really needed
561	btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
562	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
563	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
564
565	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
566	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
567	{
568	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
569	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
570	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
571	}
572	cp.m_lateralFrictionInitialized = true;
573	}
574
575	} else
576	{
577	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
578	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
579	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
580	}
581
582	if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
583	{
584	{
585	btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
586	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
587	{
588	frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
589	if (rb0)
590	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
591	if (rb1)
592	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
593	} else
594	{
595	frictionConstraint1.m_appliedImpulse = 0.f;
596	}
597	}
598
599	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
600	{
601	btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
602	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
603	{
604	frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
605	if (rb0)
606	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
607	if (rb1)
608	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
609	} else
610	{
611	frictionConstraint2.m_appliedImpulse = 0.f;
612	}
613	}
614	} else
615	{
616	btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
617	frictionConstraint1.m_appliedImpulse = 0.f;
618	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
619	{
620	btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
621	frictionConstraint2.m_appliedImpulse = 0.f;
622	}
623	}
624	}
625	}
626
627
628	}
629	}
630	}
631
632
633	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /bodies /,int /numBodies /,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
634	{
635	BT_PROFILE("solveGroupCacheFriendlySetup");
636	(void)stackAlloc;
637	(void)debugDrawer;
638
639
640	if (!(numConstraints + numManifolds))
641	{
642	// printf("empty\n");
643	return 0.f;
644	}
645
646	if (1)
647	{
648	int j;
649	for (j=0;j<numConstraints;j++)
650	{
651	btTypedConstraint* constraint = constraints[j];
652	constraint->buildJacobian();
653	}
654	}
655
656	btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
657	initSolverBody(&fixedBody,0);
658
659	//btRigidBody* rb0=0,*rb1=0;
660
661	//if (1)
662	{
663	{
664
665	int totalNumRows = 0;
666	int i;
667	//calculate the total number of contraint rows
668	for (i=0;i<numConstraints;i++)
669	{
670
671	btTypedConstraint::btConstraintInfo1 info1;
672	constraints[i]->getInfo1(&info1);
673	totalNumRows += info1.m_numConstraintRows;
674	}
675	m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
676
677	btTypedConstraint::btConstraintInfo1 info1;
678	info1.m_numConstraintRows = 0;
679
680
681	///setup the btSolverConstraints
682	int currentRow = 0;
683
684	for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
685	{
686	constraints[i]->getInfo1(&info1);
687	if (info1.m_numConstraintRows)
688	{
689	btAssert(currentRow<totalNumRows);
690
691	btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
692	btTypedConstraint* constraint = constraints[i];
693
694
695
696	btRigidBody& rbA = constraint->getRigidBodyA();
697	btRigidBody& rbB = constraint->getRigidBodyB();
698
699	int solverBodyIdA = getOrInitSolverBody(rbA);
700	int solverBodyIdB = getOrInitSolverBody(rbB);
701
702	btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
703	btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
704
705	int j;
706	for ( j=0;j<info1.m_numConstraintRows;j++)
707	{
708	memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
709	currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
710	currentConstraintRow[j].m_upperLimit = FLT_MAX;
711	currentConstraintRow[j].m_appliedImpulse = 0.f;
712	currentConstraintRow[j].m_appliedPushImpulse = 0.f;
713	currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
714	currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
715	}
716
717	bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
718	bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
719	bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
720	bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
721
722
723
724	btTypedConstraint::btConstraintInfo2 info2;
725	info2.fps = 1.f/infoGlobal.m_timeStep;
726	info2.erp = infoGlobal.m_erp;
727	info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
728	info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
729	info2.m_J2linearAxis = 0;
730	info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
731	info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
732	///the size of btSolverConstraint needs be a multiple of btScalar
733	btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
734	info2.m_constraintError = &currentConstraintRow->m_rhs;
735	info2.cfm = &currentConstraintRow->m_cfm;
736	info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
737	info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
738	constraints[i]->getInfo2(&info2);
739
740	///finalize the constraint setup
741	for ( j=0;j<info1.m_numConstraintRows;j++)
742	{
743	btSolverConstraint& solverConstraint = currentConstraintRow[j];
744
745	{
746	const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
747	solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()ftorqueAxis1constraint->getRigidBodyA().getAngularFactor();
748	}
749	{
750	const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
751	solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()ftorqueAxis2constraint->getRigidBodyB().getAngularFactor();
752	}
753
754	{
755	btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
756	btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
757	btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
758	btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
759
760	btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
761	sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
762	sum += iMJlB.dot(solverConstraint.m_contactNormal);
763	sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
764
765	solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
766	}
767
768
769	///fix rhs
770	///todo: add force/torque accelerators
771	{
772	btScalar rel_vel;
773	btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
774	btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
775
776	rel_vel = vel1Dotn+vel2Dotn;
777
778	btScalar restitution = 0.f;
779	btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
780	btScalar velocityError = restitution - rel_vel;// * damping;
781	btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
782	btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
783	solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
784	solverConstraint.m_appliedImpulse = 0.f;
785
786	}
787	}
788	}
789	}
790	}
791
792	{
793	int i;
794	btPersistentManifold* manifold = 0;
795	btCollisionObject* colObj0=0,*colObj1=0;
796
797
798	for (i=0;i<numManifolds;i++)
799	{
800	manifold = manifoldPtr[i];
801	convertContact(manifold,infoGlobal);
802	}
803	}
804	}
805
806	btContactSolverInfo info = infoGlobal;
807
808
809
810	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
811	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
812
813	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
814	m_orderTmpConstraintPool.resize(numConstraintPool);
815	m_orderFrictionConstraintPool.resize(numFrictionPool);
816	{
817	int i;
818	for (i=0;i<numConstraintPool;i++)
819	{
820	m_orderTmpConstraintPool[i] = i;
821	}
822	for (i=0;i<numFrictionPool;i++)
823	{
824	m_orderFrictionConstraintPool[i] = i;
825	}
826	}
827
828	return 0.f;
829
830	}
831
832	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /bodies /,int /numBodies/,btPersistentManifold** /manifoldPtr/, int /numManifolds/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /debugDrawer/,btStackAlloc* /stackAlloc/)
833	{
834	BT_PROFILE("solveGroupCacheFriendlyIterations");
835
836	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
837	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
838
839	//should traverse the contacts random order...
840	int iteration;
841	{
842	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
843	{
844
845	int j;
846	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
847	{
848	if ((iteration & 7) == 0) {
849	for (j=0; j<numConstraintPool; ++j) {
850	int tmp = m_orderTmpConstraintPool[j];
851	int swapi = btRandInt2(j+1);
852	m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
853	m_orderTmpConstraintPool[swapi] = tmp;
854	}
855
856	for (j=0; j<numFrictionPool; ++j) {
857	int tmp = m_orderFrictionConstraintPool[j];
858	int swapi = btRandInt2(j+1);
859	m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
860	m_orderFrictionConstraintPool[swapi] = tmp;
861	}
862	}
863	}
864
865	if (infoGlobal.m_solverMode & SOLVER_SIMD)
866	{
867	///solve all joint constraints, using SIMD, if available
868	for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
869	{
870	btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
871	resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
872	}
873
874	for (j=0;j<numConstraints;j++)
875	{
876	int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
877	int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
878	btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
879	btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
880	constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
881	}
882
883	///solve all contact constraints using SIMD, if available
884	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
885	for (j=0;j<numPoolConstraints;j++)
886	{
887	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
888	resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
889
890	}
891	///solve all friction constraints, using SIMD, if available
892	int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
893	for (j=0;j<numFrictionPoolConstraints;j++)
894	{
895	btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
896	btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
897
898	if (totalImpulse>btScalar(0))
899	{
900	solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
901	solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
902
903	resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
904	}
905	}
906	} else
907	{
908
909	///solve all joint constraints
910	for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
911	{
912	btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
913	resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
914	}
915
916	for (j=0;j<numConstraints;j++)
917	{
918	int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
919	int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
920	btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
921	btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
922
923	constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
924	}
925
926	///solve all contact constraints
927	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
928	for (j=0;j<numPoolConstraints;j++)
929	{
930	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
931	resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
932	}
933	///solve all friction constraints
934	int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
935	for (j=0;j<numFrictionPoolConstraints;j++)
936	{
937	btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
938	btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
939
940	if (totalImpulse>btScalar(0))
941	{
942	solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
943	solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
944
945	resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
946	}
947	}
948	}
949
950
951
952	}
953	}
954	return 0.f;
955	}
956
957
958
959	/// btSequentialImpulseConstraintSolver Sequentially applies impulses
960	btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject bodies,int numBodies,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /dispatcher/)
961	{
962
963
964
965	BT_PROFILE("solveGroup");
966	//we only implement SOLVER_CACHE_FRIENDLY now
967	//you need to provide at least some bodies
968	btAssert(bodies);
969	btAssert(numBodies);
970
971	int i;
972
973	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
974	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
975
976	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
977	int j;
978
979	for (j=0;j<numPoolConstraints;j++)
980	{
981
982	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
983	btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
984	btAssert(pt);
985	pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
986	if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
987	{
988	pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
989	pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
990	}
991
992	//do a callback here?
993	}
994
995	if (infoGlobal.m_splitImpulse)
996	{
997	for ( i=0;i<m_tmpSolverBodyPool.size();i++)
998	{
999	m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
1000	}
1001	} else
1002	{
1003	for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1004	{
1005	m_tmpSolverBodyPool[i].writebackVelocity();
1006	}
1007	}
1008
1009
1010	m_tmpSolverBodyPool.resize(0);
1011	m_tmpSolverContactConstraintPool.resize(0);
1012	m_tmpSolverNonContactConstraintPool.resize(0);
1013	m_tmpSolverContactFrictionConstraintPool.resize(0);
1014
1015	return 0.f;
1016	}
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026	void btSequentialImpulseConstraintSolver::reset()
1027	{
1028	m_btSeed2 = 0;
1029	}
1030
1031

Note: See TracBrowser for help on using the repository browser.

Download in other formats: