Context Navigation

← Previous Revision
Latest Revision
Next Revision →
Blame
Revision Log

btSequentialImpulseConstraintSolver.cpp @ 2119

Last change on this file since 2119 was 2119, checked in by rgrieder, 15 years ago
Merged physics branch into physics_new branch.
Property svn:eol-style set to `native`
File size: 45.0 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	//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
19
20	#include "btSequentialImpulseConstraintSolver.h"
21	#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
22	#include "BulletDynamics/Dynamics/btRigidBody.h"
23	#include "btContactConstraint.h"
24	#include "btSolve2LinearConstraint.h"
25	#include "btContactSolverInfo.h"
26	#include "LinearMath/btIDebugDraw.h"
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
36
37	#include "LinearMath/btAlignedObjectArray.h"
38
39
40	int totalCpd = 0;
41
42	int gTotalContactPoints = 0;
43
44	struct btOrderIndex
45	{
46	int m_manifoldIndex;
47	int m_pointIndex;
48	};
49
50
51
52	#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
53	static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
54
55
56	unsigned long btSequentialImpulseConstraintSolver::btRand2()
57	{
58	m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
59	return m_btSeed2;
60	}
61
62
63
64	//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
65	int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
66	{
67	// seems good; xor-fold and modulus
68	const unsigned long un = static_cast<unsigned long>(n);
69	unsigned long r = btRand2();
70
71	// note: probably more aggressive than it needs to be -- might be
72	// able to get away without one or two of the innermost branches.
73	if (un <= 0x00010000UL) {
74	r ^= (r >> 16);
75	if (un <= 0x00000100UL) {
76	r ^= (r >> 8);
77	if (un <= 0x00000010UL) {
78	r ^= (r >> 4);
79	if (un <= 0x00000004UL) {
80	r ^= (r >> 2);
81	if (un <= 0x00000002UL) {
82	r ^= (r >> 1);
83	}
84	}
85	}
86	}
87	}
88
89	return (int) (r % un);
90	}
91
92
93
94
95	bool MyContactDestroyedCallback(void* userPersistentData);
96	bool MyContactDestroyedCallback(void* userPersistentData)
97	{
98	assert (userPersistentData);
99	btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
100	btAlignedFree(cpd);
101	totalCpd--;
102	//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
103	return true;
104	}
105
106
107
108	btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
109	:m_btSeed2(0)
110	{
111	gContactDestroyedCallback = &MyContactDestroyedCallback;
112
113	//initialize default friction/contact funcs
114	int i,j;
115	for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
116	for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
117	{
118
119	m_contactDispatch[i][j] = resolveSingleCollision;
120	m_frictionDispatch[i][j] = resolveSingleFriction;
121	}
122	}
123
124	btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
125	{
126
127	}
128
129	void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
130	void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
131	{
132	btRigidBody* rb = btRigidBody::upcast(collisionObject);
133	if (rb)
134	{
135	solverBody->m_angularVelocity = rb->getAngularVelocity() ;
136	solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
137	solverBody->m_friction = collisionObject->getFriction();
138	solverBody->m_invMass = rb->getInvMass();
139	solverBody->m_linearVelocity = rb->getLinearVelocity();
140	solverBody->m_originalBody = rb;
141	solverBody->m_angularFactor = rb->getAngularFactor();
142	} else
143	{
144	solverBody->m_angularVelocity.setValue(0,0,0);
145	solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
146	solverBody->m_friction = collisionObject->getFriction();
147	solverBody->m_invMass = 0.f;
148	solverBody->m_linearVelocity.setValue(0,0,0);
149	solverBody->m_originalBody = 0;
150	solverBody->m_angularFactor = 1.f;
151	}
152	solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
153	solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
154	}
155
156
157	int gNumSplitImpulseRecoveries = 0;
158
159	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
160	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
161	{
162	btScalar rest = restitution * -rel_vel;
163	return rest;
164	}
165
166
167	void resolveSplitPenetrationImpulseCacheFriendly(
168	btSolverBody& body1,
169	btSolverBody& body2,
170	const btSolverConstraint& contactConstraint,
171	const btContactSolverInfo& solverInfo);
172
173	//SIMD_FORCE_INLINE
174	void resolveSplitPenetrationImpulseCacheFriendly(
175	btSolverBody& body1,
176	btSolverBody& body2,
177	const btSolverConstraint& contactConstraint,
178	const btContactSolverInfo& solverInfo)
179	{
180	(void)solverInfo;
181
182	if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
183	{
184
185	gNumSplitImpulseRecoveries++;
186	btScalar normalImpulse;
187
188	// Optimized version of projected relative velocity, use precomputed cross products with normal
189	// body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
190	// body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
191	// btVector3 vel = vel1 - vel2;
192	// btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
193
194	btScalar rel_vel;
195	btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
196	+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
197	btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
198	+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
199
200	rel_vel = vel1Dotn-vel2Dotn;
201
202
203	btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
204	// btScalar positionalError = contactConstraint.m_penetration;
205
206	btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
207
208	btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
209	btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
210	normalImpulse = penetrationImpulse+velocityImpulse;
211
212	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
213	btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
214	btScalar sum = oldNormalImpulse + normalImpulse;
215	contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
216
217	normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
218
219	body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
220
221	body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
222
223	}
224
225	}
226
227
228	//velocity + friction
229	//response between two dynamic objects with friction
230
231	btScalar resolveSingleCollisionCombinedCacheFriendly(
232	btSolverBody& body1,
233	btSolverBody& body2,
234	const btSolverConstraint& contactConstraint,
235	const btContactSolverInfo& solverInfo);
236
237	//SIMD_FORCE_INLINE
238	btScalar resolveSingleCollisionCombinedCacheFriendly(
239	btSolverBody& body1,
240	btSolverBody& body2,
241	const btSolverConstraint& contactConstraint,
242	const btContactSolverInfo& solverInfo)
243	{
244	(void)solverInfo;
245
246	btScalar normalImpulse;
247
248	{
249
250
251	// Optimized version of projected relative velocity, use precomputed cross products with normal
252	// body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
253	// body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
254	// btVector3 vel = vel1 - vel2;
255	// btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
256
257	btScalar rel_vel;
258	btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
259	+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
260	btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
261	+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
262
263	rel_vel = vel1Dotn-vel2Dotn;
264
265	btScalar positionalError = 0.f;
266	if (!solverInfo.m_splitImpulse \|\| (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
267	{
268	positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
269	}
270
271	btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
272
273	btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
274	btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
275	normalImpulse = penetrationImpulse+velocityImpulse;
276
277
278	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
279	btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
280	btScalar sum = oldNormalImpulse + normalImpulse;
281	contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
282
283	normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
284
285	body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
286	contactConstraint.m_angularComponentA,normalImpulse);
287
288	body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
289	contactConstraint.m_angularComponentB,-normalImpulse);
290	}
291
292	return normalImpulse;
293	}
294
295
296	#ifndef NO_FRICTION_TANGENTIALS
297
298	btScalar resolveSingleFrictionCacheFriendly(
299	btSolverBody& body1,
300	btSolverBody& body2,
301	const btSolverConstraint& contactConstraint,
302	const btContactSolverInfo& solverInfo,
303	btScalar appliedNormalImpulse);
304
305	//SIMD_FORCE_INLINE
306	btScalar resolveSingleFrictionCacheFriendly(
307	btSolverBody& body1,
308	btSolverBody& body2,
309	const btSolverConstraint& contactConstraint,
310	const btContactSolverInfo& solverInfo,
311	btScalar appliedNormalImpulse)
312	{
313	(void)solverInfo;
314
315
316	const btScalar combinedFriction = contactConstraint.m_friction;
317
318	const btScalar limit = appliedNormalImpulse * combinedFriction;
319
320	if (appliedNormalImpulse>btScalar(0.))
321	//friction
322	{
323
324	btScalar j1;
325	{
326
327	btScalar rel_vel;
328	const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
329	+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
330	const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
331	+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
332	rel_vel = vel1Dotn-vel2Dotn;
333
334	// calculate j that moves us to zero relative velocity
335	j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
336	#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
337	#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
338	btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
339	contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
340
341	if (limit < contactConstraint.m_appliedImpulse)
342	{
343	contactConstraint.m_appliedImpulse = limit;
344	} else
345	{
346	if (contactConstraint.m_appliedImpulse < -limit)
347	contactConstraint.m_appliedImpulse = -limit;
348	}
349	j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
350	#else
351	if (limit < j1)
352	{
353	j1 = limit;
354	} else
355	{
356	if (j1 < -limit)
357	j1 = -limit;
358	}
359
360	#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
361
362	//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
363	//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
364
365
366
367	}
368
369	body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
370
371	body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
372
373	}
374	return 0.f;
375	}
376
377
378	#else
379
380	//velocity + friction
381	//response between two dynamic objects with friction
382	btScalar resolveSingleFrictionCacheFriendly(
383	btSolverBody& body1,
384	btSolverBody& body2,
385	btSolverConstraint& contactConstraint,
386	const btContactSolverInfo& solverInfo)
387	{
388
389	btVector3 vel1;
390	btVector3 vel2;
391	btScalar normalImpulse(0.f);
392
393	{
394	const btVector3& normal = contactConstraint.m_contactNormal;
395	if (contactConstraint.m_penetration < 0.f)
396	return 0.f;
397
398
399	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
400	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
401	btVector3 vel = vel1 - vel2;
402	btScalar rel_vel;
403	rel_vel = normal.dot(vel);
404
405	btVector3 lat_vel = vel - normal * rel_vel;
406	btScalar lat_rel_vel = lat_vel.length2();
407
408	btScalar combinedFriction = contactConstraint.m_friction;
409	const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
410	const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
411
412
413	if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
414	{
415	lat_rel_vel = btSqrt(lat_rel_vel);
416
417	lat_vel /= lat_rel_vel;
418	btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
419	btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
420	btScalar friction_impulse = lat_rel_vel /
421	(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
422	btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
423
424	GEN_set_min(friction_impulse, normal_impulse);
425	GEN_set_max(friction_impulse, -normal_impulse);
426	body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
427	body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
428	}
429	}
430
431	return normalImpulse;
432	}
433
434	#endif //NO_FRICTION_TANGENTIALS
435
436
437
438
439
440	void 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)
441	{
442
443	btRigidBody* body0=btRigidBody::upcast(colObj0);
444	btRigidBody* body1=btRigidBody::upcast(colObj1);
445
446	btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
447	solverConstraint.m_contactNormal = normalAxis;
448
449	solverConstraint.m_solverBodyIdA = solverBodyIdA;
450	solverConstraint.m_solverBodyIdB = solverBodyIdB;
451	solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
452	solverConstraint.m_frictionIndex = frictionIndex;
453
454	solverConstraint.m_friction = cp.m_combinedFriction;
455	solverConstraint.m_originalContactPoint = 0;
456
457	solverConstraint.m_appliedImpulse = btScalar(0.);
458	solverConstraint.m_appliedPushImpulse = 0.f;
459	solverConstraint.m_penetration = 0.f;
460	{
461	btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
462	solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
463	solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
464	}
465	{
466	btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
467	solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
468	solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
469	}
470
471	#ifdef COMPUTE_IMPULSE_DENOM
472	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
473	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
474	#else
475	btVector3 vec;
476	btScalar denom0 = 0.f;
477	btScalar denom1 = 0.f;
478	if (body0)
479	{
480	vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
481	denom0 = body0->getInvMass() + normalAxis.dot(vec);
482	}
483	if (body1)
484	{
485	vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
486	denom1 = body1->getInvMass() + normalAxis.dot(vec);
487	}
488
489
490	#endif //COMPUTE_IMPULSE_DENOM
491	btScalar denom = relaxation/(denom0+denom1);
492	solverConstraint.m_jacDiagABInv = denom;
493
494
495	}
496
497
498
499	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /bodies /,int /numBodies /,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
500	{
501	BT_PROFILE("solveGroupCacheFriendlySetup");
502	(void)stackAlloc;
503	(void)debugDrawer;
504
505
506	if (!(numConstraints + numManifolds))
507	{
508	// printf("empty\n");
509	return 0.f;
510	}
511	btPersistentManifold* manifold = 0;
512	btCollisionObject* colObj0=0,*colObj1=0;
513
514	//btRigidBody* rb0=0,*rb1=0;
515
516
517	#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
518
519	BEGIN_PROFILE("refreshManifolds");
520
521	int i;
522
523
524
525	for (i=0;i<numManifolds;i++)
526	{
527	manifold = manifoldPtr[i];
528	rb1 = (btRigidBody*)manifold->getBody1();
529	rb0 = (btRigidBody*)manifold->getBody0();
530
531	manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
532
533	}
534
535	END_PROFILE("refreshManifolds");
536	#endif //FORCE_REFESH_CONTACT_MANIFOLDS
537
538
539
540
541
542	//int sizeofSB = sizeof(btSolverBody);
543	//int sizeofSC = sizeof(btSolverConstraint);
544
545
546	//if (1)
547	{
548	//if m_stackAlloc, try to pack bodies/constraints to speed up solving
549	// btBlock* sablock;
550	// sablock = stackAlloc->beginBlock();
551
552	// int memsize = 16;
553	// unsigned char* stackMemory = stackAlloc->allocate(memsize);
554
555
556	//todo: use stack allocator for this temp memory
557	// int minReservation = numManifolds*2;
558
559	//m_tmpSolverBodyPool.reserve(minReservation);
560
561	//don't convert all bodies, only the one we need so solver the constraints
562	/*
563	{
564	for (int i=0;i<numBodies;i++)
565	{
566	btRigidBody* rb = btRigidBody::upcast(bodies[i]);
567	if (rb && (rb->getIslandTag() >= 0))
568	{
569	btAssert(rb->getCompanionId() < 0);
570	int solverBodyId = m_tmpSolverBodyPool.size();
571	btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
572	initSolverBody(&solverBody,rb);
573	rb->setCompanionId(solverBodyId);
574	}
575	}
576	}
577	*/
578
579	//m_tmpSolverConstraintPool.reserve(minReservation);
580	//m_tmpSolverFrictionConstraintPool.reserve(minReservation);
581
582	{
583	int i;
584
585	for (i=0;i<numManifolds;i++)
586	{
587	manifold = manifoldPtr[i];
588	colObj0 = (btCollisionObject*)manifold->getBody0();
589	colObj1 = (btCollisionObject*)manifold->getBody1();
590
591	int solverBodyIdA=-1;
592	int solverBodyIdB=-1;
593
594	if (manifold->getNumContacts())
595	{
596
597
598
599	if (colObj0->getIslandTag() >= 0)
600	{
601	if (colObj0->getCompanionId() >= 0)
602	{
603	//body has already been converted
604	solverBodyIdA = colObj0->getCompanionId();
605	} else
606	{
607	solverBodyIdA = m_tmpSolverBodyPool.size();
608	btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
609	initSolverBody(&solverBody,colObj0);
610	colObj0->setCompanionId(solverBodyIdA);
611	}
612	} else
613	{
614	//create a static body
615	solverBodyIdA = m_tmpSolverBodyPool.size();
616	btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
617	initSolverBody(&solverBody,colObj0);
618	}
619
620	if (colObj1->getIslandTag() >= 0)
621	{
622	if (colObj1->getCompanionId() >= 0)
623	{
624	solverBodyIdB = colObj1->getCompanionId();
625	} else
626	{
627	solverBodyIdB = m_tmpSolverBodyPool.size();
628	btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
629	initSolverBody(&solverBody,colObj1);
630	colObj1->setCompanionId(solverBodyIdB);
631	}
632	} else
633	{
634	//create a static body
635	solverBodyIdB = m_tmpSolverBodyPool.size();
636	btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
637	initSolverBody(&solverBody,colObj1);
638	}
639	}
640
641	btVector3 rel_pos1;
642	btVector3 rel_pos2;
643	btScalar relaxation;
644
645	for (int j=0;j<manifold->getNumContacts();j++)
646	{
647
648	btManifoldPoint& cp = manifold->getContactPoint(j);
649
650	if (cp.getDistance() <= btScalar(0.))
651	{
652
653	const btVector3& pos1 = cp.getPositionWorldOnA();
654	const btVector3& pos2 = cp.getPositionWorldOnB();
655
656	rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
657	rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
658
659
660	relaxation = 1.f;
661	btScalar rel_vel;
662	btVector3 vel;
663
664	int frictionIndex = m_tmpSolverConstraintPool.size();
665
666	{
667	btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
668	btRigidBody* rb0 = btRigidBody::upcast(colObj0);
669	btRigidBody* rb1 = btRigidBody::upcast(colObj1);
670
671	solverConstraint.m_solverBodyIdA = solverBodyIdA;
672	solverConstraint.m_solverBodyIdB = solverBodyIdB;
673	solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
674
675	solverConstraint.m_originalContactPoint = &cp;
676
677	btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
678	solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
679	btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
680	solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
681	{
682	#ifdef COMPUTE_IMPULSE_DENOM
683	btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
684	btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
685	#else
686	btVector3 vec;
687	btScalar denom0 = 0.f;
688	btScalar denom1 = 0.f;
689	if (rb0)
690	{
691	vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
692	denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
693	}
694	if (rb1)
695	{
696	vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
697	denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
698	}
699	#endif //COMPUTE_IMPULSE_DENOM
700
701	btScalar denom = relaxation/(denom0+denom1);
702	solverConstraint.m_jacDiagABInv = denom;
703	}
704
705	solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
706	solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
707	solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
708
709
710	btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
711	btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
712
713	vel = vel1 - vel2;
714
715	rel_vel = cp.m_normalWorldOnB.dot(vel);
716
717	solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
718	//solverConstraint.m_penetration = cp.getDistance();
719
720	solverConstraint.m_friction = cp.m_combinedFriction;
721	solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
722	if (solverConstraint.m_restitution <= btScalar(0.))
723	{
724	solverConstraint.m_restitution = 0.f;
725	};
726
727
728	btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
729
730
731
732	if (solverConstraint.m_restitution > penVel)
733	{
734	solverConstraint.m_penetration = btScalar(0.);
735	}
736
737
738
739	///warm starting (or zero if disabled)
740	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
741	{
742	solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
743	if (rb0)
744	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
745	if (rb1)
746	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
747	} else
748	{
749	solverConstraint.m_appliedImpulse = 0.f;
750	}
751
752	solverConstraint.m_appliedPushImpulse = 0.f;
753
754	solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
755	if (!cp.m_lateralFrictionInitialized)
756	{
757	cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
758	btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
759	if (lat_rel_vel > SIMD_EPSILON)//0.0f)
760	{
761	cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
762	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
763	cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
764	cp.m_lateralFrictionDir2.normalize();//??
765	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
766	} else
767	{
768	//re-calculate friction direction every frame, todo: check if this is really needed
769
770	btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
771	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
772	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
773	}
774	cp.m_lateralFrictionInitialized = true;
775
776	} else
777	{
778	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
779	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
780	}
781
782	{
783	btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
784	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
785	{
786	frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
787	if (rb0)
788	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
789	if (rb1)
790	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
791	} else
792	{
793	frictionConstraint1.m_appliedImpulse = 0.f;
794	}
795	}
796	{
797	btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
798	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
799	{
800	frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
801	if (rb0)
802	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
803	if (rb1)
804	m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
805	} else
806	{
807	frictionConstraint2.m_appliedImpulse = 0.f;
808	}
809	}
810	}
811
812
813	}
814	}
815	}
816	}
817	}
818
819	btContactSolverInfo info = infoGlobal;
820
821	{
822	int j;
823	for (j=0;j<numConstraints;j++)
824	{
825	btTypedConstraint* constraint = constraints[j];
826	constraint->buildJacobian();
827	}
828	}
829
830
831
832	int numConstraintPool = m_tmpSolverConstraintPool.size();
833	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
834
835	///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
836	m_orderTmpConstraintPool.resize(numConstraintPool);
837	m_orderFrictionConstraintPool.resize(numFrictionPool);
838	{
839	int i;
840	for (i=0;i<numConstraintPool;i++)
841	{
842	m_orderTmpConstraintPool[i] = i;
843	}
844	for (i=0;i<numFrictionPool;i++)
845	{
846	m_orderFrictionConstraintPool[i] = i;
847	}
848	}
849
850
851
852	return 0.f;
853
854	}
855
856	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /bodies /,int /numBodies/,btPersistentManifold** /manifoldPtr/, int /numManifolds/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /debugDrawer/,btStackAlloc* /stackAlloc/)
857	{
858	BT_PROFILE("solveGroupCacheFriendlyIterations");
859	int numConstraintPool = m_tmpSolverConstraintPool.size();
860	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
861
862	//should traverse the contacts random order...
863	int iteration;
864	{
865	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
866	{
867
868	int j;
869	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
870	{
871	if ((iteration & 7) == 0) {
872	for (j=0; j<numConstraintPool; ++j) {
873	int tmp = m_orderTmpConstraintPool[j];
874	int swapi = btRandInt2(j+1);
875	m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
876	m_orderTmpConstraintPool[swapi] = tmp;
877	}
878
879	for (j=0; j<numFrictionPool; ++j) {
880	int tmp = m_orderFrictionConstraintPool[j];
881	int swapi = btRandInt2(j+1);
882	m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
883	m_orderFrictionConstraintPool[swapi] = tmp;
884	}
885	}
886	}
887
888	for (j=0;j<numConstraints;j++)
889	{
890	btTypedConstraint* constraint = constraints[j];
891	///todo: use solver bodies, so we don't need to copy from/to btRigidBody
892
893	if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
894	{
895	m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
896	}
897	if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
898	{
899	m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
900	}
901
902	constraint->solveConstraint(infoGlobal.m_timeStep);
903
904	if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
905	{
906	m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
907	}
908	if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
909	{
910	m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
911	}
912
913	}
914
915	{
916	int numPoolConstraints = m_tmpSolverConstraintPool.size();
917	for (j=0;j<numPoolConstraints;j++)
918	{
919
920	const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
921	resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
922	m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
923	}
924	}
925
926	{
927	int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
928
929	for (j=0;j<numFrictionPoolConstraints;j++)
930	{
931	const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
932	btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
933	m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;
934
935	resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
936	m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
937	totalImpulse);
938	}
939	}
940
941
942
943	}
944
945	if (infoGlobal.m_splitImpulse)
946	{
947
948	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
949	{
950	{
951	int numPoolConstraints = m_tmpSolverConstraintPool.size();
952	int j;
953	for (j=0;j<numPoolConstraints;j++)
954	{
955	const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
956
957	resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
958	m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
959	}
960	}
961	}
962
963	}
964
965	}
966
967	return 0.f;
968	}
969
970
971	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject bodies,int numBodies,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
972	{
973	int i;
974
975	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
976	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
977
978	int numPoolConstraints = m_tmpSolverConstraintPool.size();
979	int j;
980	for (j=0;j<numPoolConstraints;j++)
981	{
982
983	const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
984	btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
985	btAssert(pt);
986	pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
987	pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
988	pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
989
990	//do a callback here?
991
992	}
993
994	if (infoGlobal.m_splitImpulse)
995	{
996	for ( i=0;i<m_tmpSolverBodyPool.size();i++)
997	{
998	m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
999	}
1000	} else
1001	{
1002	for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1003	{
1004	m_tmpSolverBodyPool[i].writebackVelocity();
1005	}
1006	}
1007
1008	// printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
1009
1010	/*
1011	printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
1012	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
1013	printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
1014
1015
1016	printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
1017	printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
1018	printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
1019	*/
1020
1021	m_tmpSolverBodyPool.resize(0);
1022	m_tmpSolverConstraintPool.resize(0);
1023	m_tmpSolverFrictionConstraintPool.resize(0);
1024
1025
1026	return 0.f;
1027	}
1028
1029	/// btSequentialImpulseConstraintSolver Sequentially applies impulses
1030	btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject bodies,int numBodies,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /dispatcher/)
1031	{
1032	BT_PROFILE("solveGroup");
1033	if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
1034	{
1035	//you need to provide at least some bodies
1036	//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
1037	btAssert(bodies);
1038	btAssert(numBodies);
1039	return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1040	}
1041
1042
1043
1044	btContactSolverInfo info = infoGlobal;
1045
1046	int numiter = infoGlobal.m_numIterations;
1047
1048	int totalPoints = 0;
1049
1050
1051	{
1052	short j;
1053	for (j=0;j<numManifolds;j++)
1054	{
1055	btPersistentManifold* manifold = manifoldPtr[j];
1056	prepareConstraints(manifold,info,debugDrawer);
1057
1058	for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
1059	{
1060	gOrder[totalPoints].m_manifoldIndex = j;
1061	gOrder[totalPoints].m_pointIndex = p;
1062	totalPoints++;
1063	}
1064	}
1065	}
1066
1067	{
1068	int j;
1069	for (j=0;j<numConstraints;j++)
1070	{
1071	btTypedConstraint* constraint = constraints[j];
1072	constraint->buildJacobian();
1073	}
1074	}
1075
1076
1077	//should traverse the contacts random order...
1078	int iteration;
1079
1080	{
1081	for ( iteration = 0;iteration<numiter;iteration++)
1082	{
1083	int j;
1084	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1085	{
1086	if ((iteration & 7) == 0) {
1087	for (j=0; j<totalPoints; ++j) {
1088	btOrderIndex tmp = gOrder[j];
1089	int swapi = btRandInt2(j+1);
1090	gOrder[j] = gOrder[swapi];
1091	gOrder[swapi] = tmp;
1092	}
1093	}
1094	}
1095
1096	for (j=0;j<numConstraints;j++)
1097	{
1098	btTypedConstraint* constraint = constraints[j];
1099	constraint->solveConstraint(info.m_timeStep);
1100	}
1101
1102	for (j=0;j<totalPoints;j++)
1103	{
1104	btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
1105	solve( (btRigidBody*)manifold->getBody0(),
1106	(btRigidBody*)manifold->getBody1()
1107	,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
1108	}
1109
1110	for (j=0;j<totalPoints;j++)
1111	{
1112	btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
1113	solveFriction((btRigidBody*)manifold->getBody0(),
1114	(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
1115	}
1116
1117	}
1118	}
1119
1120
1121
1122
1123	return btScalar(0.);
1124	}
1125
1126
1127
1128
1129
1130
1131
1132	void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
1133	{
1134
1135	(void)debugDrawer;
1136
1137	btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
1138	btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
1139
1140
1141	//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
1142	{
1143	#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
1144	manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
1145	#endif //FORCE_REFESH_CONTACT_MANIFOLDS
1146	int numpoints = manifoldPtr->getNumContacts();
1147
1148	gTotalContactPoints += numpoints;
1149
1150
1151	for (int i=0;i<numpoints ;i++)
1152	{
1153	btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
1154	if (cp.getDistance() <= btScalar(0.))
1155	{
1156	const btVector3& pos1 = cp.getPositionWorldOnA();
1157	const btVector3& pos2 = cp.getPositionWorldOnB();
1158
1159	btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
1160	btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
1161
1162
1163	//this jacobian entry is re-used for all iterations
1164	btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
1165	body1->getCenterOfMassTransform().getBasis().transpose(),
1166	rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
1167	body1->getInvInertiaDiagLocal(),body1->getInvMass());
1168
1169
1170	btScalar jacDiagAB = jac.getDiagonal();
1171
1172	btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1173	if (cpd)
1174	{
1175	//might be invalid
1176	cpd->m_persistentLifeTime++;
1177	if (cpd->m_persistentLifeTime != cp.getLifeTime())
1178	{
1179	//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
1180	new (cpd) btConstraintPersistentData;
1181	cpd->m_persistentLifeTime = cp.getLifeTime();
1182
1183	} else
1184	{
1185	//printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
1186
1187	}
1188	} else
1189	{
1190
1191	//todo: should this be in a pool?
1192	void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
1193	cpd = new (mem)btConstraintPersistentData;
1194	assert(cpd);
1195
1196	totalCpd ++;
1197	//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
1198	cp.m_userPersistentData = cpd;
1199	cpd->m_persistentLifeTime = cp.getLifeTime();
1200	//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
1201
1202	}
1203	assert(cpd);
1204
1205	cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
1206
1207	//Dependent on Rigidbody A and B types, fetch the contact/friction response func
1208	//perhaps do a similar thing for friction/restutution combiner funcs...
1209
1210	cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
1211	cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
1212
1213	btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
1214	btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
1215	btVector3 vel = vel1 - vel2;
1216	btScalar rel_vel;
1217	rel_vel = cp.m_normalWorldOnB.dot(vel);
1218
1219	btScalar combinedRestitution = cp.m_combinedRestitution;
1220
1221	cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
1222	cpd->m_friction = cp.m_combinedFriction;
1223	cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
1224	if (cpd->m_restitution <= btScalar(0.))
1225	{
1226	cpd->m_restitution = btScalar(0.0);
1227
1228	};
1229
1230	//restitution and penetration work in same direction so
1231	//rel_vel
1232
1233	btScalar penVel = -cpd->m_penetration/info.m_timeStep;
1234
1235	if (cpd->m_restitution > penVel)
1236	{
1237	cpd->m_penetration = btScalar(0.);
1238	}
1239
1240
1241	btScalar relaxation = info.m_damping;
1242	if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
1243	{
1244	cpd->m_appliedImpulse *= relaxation;
1245	} else
1246	{
1247	cpd->m_appliedImpulse =btScalar(0.);
1248	}
1249
1250	//for friction
1251	cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
1252
1253	//re-calculate friction direction every frame, todo: check if this is really needed
1254	btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
1255
1256
1257	#define NO_FRICTION_WARMSTART 1
1258
1259	#ifdef NO_FRICTION_WARMSTART
1260	cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
1261	cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
1262	#endif //NO_FRICTION_WARMSTART
1263	btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
1264	btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
1265	btScalar denom = relaxation/(denom0+denom1);
1266	cpd->m_jacDiagABInvTangent0 = denom;
1267
1268
1269	denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
1270	denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
1271	denom = relaxation/(denom0+denom1);
1272	cpd->m_jacDiagABInvTangent1 = denom;
1273
1274	btVector3 totalImpulse =
1275	#ifndef NO_FRICTION_WARMSTART
1276	cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
1277	cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
1278	#endif //NO_FRICTION_WARMSTART
1279	cp.m_normalWorldOnB*cpd->m_appliedImpulse;
1280
1281
1282
1283	///
1284	{
1285	btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
1286	cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
1287	btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
1288	cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
1289	}
1290	{
1291	btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
1292	cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
1293	}
1294	{
1295	btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
1296	cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
1297	}
1298	{
1299	btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
1300	cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
1301	}
1302	{
1303	btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
1304	cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
1305	}
1306
1307	///
1308
1309
1310
1311	//apply previous frames impulse on both bodies
1312	body0->applyImpulse(totalImpulse, rel_pos1);
1313	body1->applyImpulse(-totalImpulse, rel_pos2);
1314	}
1315
1316	}
1317	}
1318	}
1319
1320
1321	btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1322	{
1323	btScalar maxImpulse = btScalar(0.);
1324
1325	{
1326
1327
1328	{
1329	if (cp.getDistance() <= btScalar(0.))
1330	{
1331
1332
1333
1334	{
1335
1336	//btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1337	btScalar impulse = resolveSingleCollisionCombined(
1338	body0,body1,
1339	cp,
1340	info);
1341
1342	if (maxImpulse < impulse)
1343	maxImpulse = impulse;
1344
1345	}
1346	}
1347	}
1348	}
1349	return maxImpulse;
1350	}
1351
1352
1353
1354	btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1355	{
1356
1357	btScalar maxImpulse = btScalar(0.);
1358
1359	{
1360
1361
1362	{
1363	if (cp.getDistance() <= btScalar(0.))
1364	{
1365
1366
1367
1368	{
1369
1370	btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1371	btScalar impulse = cpd->m_contactSolverFunc(
1372	body0,body1,
1373	cp,
1374	info);
1375
1376	if (maxImpulse < impulse)
1377	maxImpulse = impulse;
1378
1379	}
1380	}
1381	}
1382	}
1383	return maxImpulse;
1384	}
1385
1386	btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1387	{
1388
1389	(void)debugDrawer;
1390	(void)iter;
1391
1392
1393	{
1394
1395
1396	{
1397
1398	if (cp.getDistance() <= btScalar(0.))
1399	{
1400
1401	btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1402	cpd->m_frictionSolverFunc(
1403	body0,body1,
1404	cp,
1405	info);
1406
1407
1408	}
1409	}
1410
1411
1412	}
1413	return btScalar(0.);
1414	}
1415
1416
1417	void btSequentialImpulseConstraintSolver::reset()
1418	{
1419	m_btSeed2 = 0;
1420	}
1421
1422

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

Context Navigation

source: code/branches/physics_new/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @ 2119

Download in other formats: