Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @ 8012

Last change on this file since 8012 was 7983, checked in by rgrieder, 14 years ago
Updated Bullet Physics Engine from v2.74 to v2.77
Property svn:eol-style set to `native`
File size: 49.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
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	int gNumSplitImpulseRecoveries = 0;
38
39	btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
40	:m_btSeed2(0)
41	{
42
43	}
44
45	btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
46	{
47	}
48
49	#ifdef USE_SIMD
50	#include <emmintrin.h>
51	#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
52	static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
53	{
54	__m128 result = _mm_mul_ps( vec0, vec1);
55	return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
56	}
57	#endif//USE_SIMD
58
59	// Project Gauss Seidel or the equivalent Sequential Impulse
60	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
61	{
62	#ifdef USE_SIMD
63	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
64	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
65	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
66	__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)));
67	__m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
68	__m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
69	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
70	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
71	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
72	btSimdScalar resultLowerLess,resultUpperLess;
73	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
74	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
75	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
76	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
77	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
78	__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
79	deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
80	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
81	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
82	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
83	__m128 impulseMagnitude = deltaImpulse;
84	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
85	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
86	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
87	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
88	#else
89	resolveSingleConstraintRowGeneric(body1,body2,c);
90	#endif
91	}
92
93	// Project Gauss Seidel or the equivalent Sequential Impulse
94	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
95	{
96	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
97	const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
98	const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
99
100	// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
101	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
102	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
103
104	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
105	if (sum < c.m_lowerLimit)
106	{
107	deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
108	c.m_appliedImpulse = c.m_lowerLimit;
109	}
110	else if (sum > c.m_upperLimit)
111	{
112	deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
113	c.m_appliedImpulse = c.m_upperLimit;
114	}
115	else
116	{
117	c.m_appliedImpulse = sum;
118	}
119	body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
120	body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
121	}
122
123	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& 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.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
131	__m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
132	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
133	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
134	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
135	btSimdScalar resultLowerLess,resultUpperLess;
136	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
137	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
138	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
139	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
140	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
141	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
142	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
143	__m128 impulseMagnitude = deltaImpulse;
144	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
145	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
146	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
147	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
148	#else
149	resolveSingleConstraintRowLowerLimit(body1,body2,c);
150	#endif
151	}
152
153	// Project Gauss Seidel or the equivalent Sequential Impulse
154	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
155	{
156	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
157	const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
158	const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
159
160	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
161	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
162	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
163	if (sum < c.m_lowerLimit)
164	{
165	deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
166	c.m_appliedImpulse = c.m_lowerLimit;
167	}
168	else
169	{
170	c.m_appliedImpulse = sum;
171	}
172	body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
173	body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
174	}
175
176
177	void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
178	btRigidBody& body1,
179	btRigidBody& body2,
180	const btSolverConstraint& c)
181	{
182	if (c.m_rhsPenetration)
183	{
184	gNumSplitImpulseRecoveries++;
185	btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
186	const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
187	const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
188
189	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
190	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
191	const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
192	if (sum < c.m_lowerLimit)
193	{
194	deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
195	c.m_appliedPushImpulse = c.m_lowerLimit;
196	}
197	else
198	{
199	c.m_appliedPushImpulse = sum;
200	}
201	body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
202	body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
203	}
204	}
205
206	void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
207	{
208	#ifdef USE_SIMD
209	if (!c.m_rhsPenetration)
210	return;
211
212	gNumSplitImpulseRecoveries++;
213
214	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
215	__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
216	__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
217	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
218	__m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
219	__m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
220	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
221	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
222	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
223	btSimdScalar resultLowerLess,resultUpperLess;
224	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
225	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
226	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
227	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
228	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
229	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
230	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
231	__m128 impulseMagnitude = deltaImpulse;
232	body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
233	body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
234	body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
235	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
236	#else
237	resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
238	#endif
239	}
240
241
242
243	unsigned long btSequentialImpulseConstraintSolver::btRand2()
244	{
245	m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
246	return m_btSeed2;
247	}
248
249
250
251	//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
252	int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
253	{
254	// seems good; xor-fold and modulus
255	const unsigned long un = static_cast<unsigned long>(n);
256	unsigned long r = btRand2();
257
258	// note: probably more aggressive than it needs to be -- might be
259	// able to get away without one or two of the innermost branches.
260	if (un <= 0x00010000UL) {
261	r ^= (r >> 16);
262	if (un <= 0x00000100UL) {
263	r ^= (r >> 8);
264	if (un <= 0x00000010UL) {
265	r ^= (r >> 4);
266	if (un <= 0x00000004UL) {
267	r ^= (r >> 2);
268	if (un <= 0x00000002UL) {
269	r ^= (r >> 1);
270	}
271	}
272	}
273	}
274	}
275
276	return (int) (r % un);
277	}
278
279
280	#if 0
281	void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
282	{
283	btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
284
285	solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
286	solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
287	solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
288	solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
289
290	if (rb)
291	{
292	solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
293	solverBody->m_originalBody = rb;
294	solverBody->m_angularFactor = rb->getAngularFactor();
295	} else
296	{
297	solverBody->internalGetInvMass().setValue(0,0,0);
298	solverBody->m_originalBody = 0;
299	solverBody->m_angularFactor.setValue(1,1,1);
300	}
301	}
302	#endif
303
304
305
306
307
308	btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
309	{
310	btScalar rest = restitution * -rel_vel;
311	return rest;
312	}
313
314
315
316	void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
317	void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
318	{
319	if (colObj && colObj->hasAnisotropicFriction())
320	{
321	// transform to local coordinates
322	btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
323	const btVector3& friction_scaling = colObj->getAnisotropicFriction();
324	//apply anisotropic friction
325	loc_lateral *= friction_scaling;
326	// ... and transform it back to global coordinates
327	frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
328	}
329	}
330
331
332	void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
333	{
334
335
336	btRigidBody* body0=btRigidBody::upcast(colObj0);
337	btRigidBody* body1=btRigidBody::upcast(colObj1);
338
339	solverConstraint.m_contactNormal = normalAxis;
340
341	solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
342	solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
343
344	solverConstraint.m_friction = cp.m_combinedFriction;
345	solverConstraint.m_originalContactPoint = 0;
346
347	solverConstraint.m_appliedImpulse = 0.f;
348	solverConstraint.m_appliedPushImpulse = 0.f;
349
350	{
351	btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
352	solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
353	solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()ftorqueAxis1body0->getAngularFactor() : btVector3(0,0,0);
354	}
355	{
356	btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
357	solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
358	solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()ftorqueAxis1body1->getAngularFactor() : btVector3(0,0,0);
359	}
360
361	#ifdef COMPUTE_IMPULSE_DENOM
362	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
363	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
364	#else
365	btVector3 vec;
366	btScalar denom0 = 0.f;
367	btScalar denom1 = 0.f;
368	if (body0)
369	{
370	vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
371	denom0 = body0->getInvMass() + normalAxis.dot(vec);
372	}
373	if (body1)
374	{
375	vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
376	denom1 = body1->getInvMass() + normalAxis.dot(vec);
377	}
378
379
380	#endif //COMPUTE_IMPULSE_DENOM
381	btScalar denom = relaxation/(denom0+denom1);
382	solverConstraint.m_jacDiagABInv = denom;
383
384	#ifdef _USE_JACOBIAN
385	solverConstraint.m_jac = btJacobianEntry (
386	rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
387	body0->getInvInertiaDiagLocal(),
388	body0->getInvMass(),
389	body1->getInvInertiaDiagLocal(),
390	body1->getInvMass());
391	#endif //_USE_JACOBIAN
392
393
394	{
395	btScalar rel_vel;
396	btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
397	+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
398	btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
399	+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
400
401	rel_vel = vel1Dotn+vel2Dotn;
402
403	// btScalar positionalError = 0.f;
404
405	btSimdScalar velocityError = desiredVelocity - rel_vel;
406	btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
407	solverConstraint.m_rhs = velocityImpulse;
408	solverConstraint.m_cfm = cfmSlip;
409	solverConstraint.m_lowerLimit = 0;
410	solverConstraint.m_upperLimit = 1e10f;
411	}
412	}
413
414
415
416	btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
417	{
418	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
419	solverConstraint.m_frictionIndex = frictionIndex;
420	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2,
421	colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
422	return solverConstraint;
423	}
424
425	int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
426	{
427	#if 0
428	int solverBodyIdA = -1;
429
430	if (body.getCompanionId() >= 0)
431	{
432	//body has already been converted
433	solverBodyIdA = body.getCompanionId();
434	} else
435	{
436	btRigidBody* rb = btRigidBody::upcast(&body);
437	if (rb && rb->getInvMass())
438	{
439	solverBodyIdA = m_tmpSolverBodyPool.size();
440	btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
441	initSolverBody(&solverBody,&body);
442	body.setCompanionId(solverBodyIdA);
443	} else
444	{
445	return 0;//assume first one is a fixed solver body
446	}
447	}
448	return solverBodyIdA;
449	#endif
450	return 0;
451	}
452	#include <stdio.h>
453
454
455	void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
456	btCollisionObject* colObj0, btCollisionObject* colObj1,
457	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
458	btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
459	btVector3& rel_pos1, btVector3& rel_pos2)
460	{
461	btRigidBody* rb0 = btRigidBody::upcast(colObj0);
462	btRigidBody* rb1 = btRigidBody::upcast(colObj1);
463
464	const btVector3& pos1 = cp.getPositionWorldOnA();
465	const btVector3& pos2 = cp.getPositionWorldOnB();
466
467	// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
468	// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
469	rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
470	rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
471
472	relaxation = 1.f;
473
474	btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
475	solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()torqueAxis0rb0->getAngularFactor() : btVector3(0,0,0);
476	btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
477	solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()-torqueAxis1rb1->getAngularFactor() : btVector3(0,0,0);
478
479	{
480	#ifdef COMPUTE_IMPULSE_DENOM
481	btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
482	btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
483	#else
484	btVector3 vec;
485	btScalar denom0 = 0.f;
486	btScalar denom1 = 0.f;
487	if (rb0)
488	{
489	vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
490	denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
491	}
492	if (rb1)
493	{
494	vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
495	denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
496	}
497	#endif //COMPUTE_IMPULSE_DENOM
498
499	btScalar denom = relaxation/(denom0+denom1);
500	solverConstraint.m_jacDiagABInv = denom;
501	}
502
503	solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
504	solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
505	solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
506
507
508
509
510	btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
511	btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
512	vel = vel1 - vel2;
513	rel_vel = cp.m_normalWorldOnB.dot(vel);
514
515	btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
516
517
518	solverConstraint.m_friction = cp.m_combinedFriction;
519
520	btScalar restitution = 0.f;
521
522	if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
523	{
524	restitution = 0.f;
525	} else
526	{
527	restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
528	if (restitution <= btScalar(0.))
529	{
530	restitution = 0.f;
531	};
532	}
533
534
535	///warm starting (or zero if disabled)
536	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
537	{
538	solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
539	if (rb0)
540	rb0->internalApplyImpulse(solverConstraint.m_contactNormalrb0->getInvMass()rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
541	if (rb1)
542	rb1->internalApplyImpulse(solverConstraint.m_contactNormalrb1->getInvMass()rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
543	} else
544	{
545	solverConstraint.m_appliedImpulse = 0.f;
546	}
547
548	solverConstraint.m_appliedPushImpulse = 0.f;
549
550	{
551	btScalar rel_vel;
552	btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
553	+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
554	btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
555	+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
556
557	rel_vel = vel1Dotn+vel2Dotn;
558
559	btScalar positionalError = 0.f;
560	positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
561	btScalar velocityError = restitution - rel_vel;// * damping;
562	btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
563	btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
564	if (!infoGlobal.m_splitImpulse \|\| (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
565	{
566	//combine position and velocity into rhs
567	solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
568	solverConstraint.m_rhsPenetration = 0.f;
569	} else
570	{
571	//split position and velocity into rhs and m_rhsPenetration
572	solverConstraint.m_rhs = velocityImpulse;
573	solverConstraint.m_rhsPenetration = penetrationImpulse;
574	}
575	solverConstraint.m_cfm = 0.f;
576	solverConstraint.m_lowerLimit = 0;
577	solverConstraint.m_upperLimit = 1e10f;
578	}
579
580
581
582
583	}
584
585
586
587	void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
588	btRigidBody* rb0, btRigidBody* rb1,
589	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
590	{
591	if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
592	{
593	{
594	btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
595	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
596	{
597	frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
598	if (rb0)
599	rb0->internalApplyImpulse(frictionConstraint1.m_contactNormalrb0->getInvMass()rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
600	if (rb1)
601	rb1->internalApplyImpulse(frictionConstraint1.m_contactNormalrb1->getInvMass()rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
602	} else
603	{
604	frictionConstraint1.m_appliedImpulse = 0.f;
605	}
606	}
607
608	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
609	{
610	btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
611	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
612	{
613	frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
614	if (rb0)
615	rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
616	if (rb1)
617	rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
618	} else
619	{
620	frictionConstraint2.m_appliedImpulse = 0.f;
621	}
622	}
623	} else
624	{
625	btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
626	frictionConstraint1.m_appliedImpulse = 0.f;
627	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
628	{
629	btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
630	frictionConstraint2.m_appliedImpulse = 0.f;
631	}
632	}
633	}
634
635
636
637
638	void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
639	{
640	btCollisionObject* colObj0=0,*colObj1=0;
641
642	colObj0 = (btCollisionObject*)manifold->getBody0();
643	colObj1 = (btCollisionObject*)manifold->getBody1();
644
645
646	btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
647	btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
648
649	///avoid collision response between two static objects
650	if ((!solverBodyA \|\| !solverBodyA->getInvMass()) && (!solverBodyB \|\| !solverBodyB->getInvMass()))
651	return;
652
653	for (int j=0;j<manifold->getNumContacts();j++)
654	{
655
656	btManifoldPoint& cp = manifold->getContactPoint(j);
657
658	if (cp.getDistance() <= manifold->getContactProcessingThreshold())
659	{
660	btVector3 rel_pos1;
661	btVector3 rel_pos2;
662	btScalar relaxation;
663	btScalar rel_vel;
664	btVector3 vel;
665
666	int frictionIndex = m_tmpSolverContactConstraintPool.size();
667	btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
668	btRigidBody* rb0 = btRigidBody::upcast(colObj0);
669	btRigidBody* rb1 = btRigidBody::upcast(colObj1);
670	solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
671	solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
672	solverConstraint.m_originalContactPoint = &cp;
673
674	setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
675
676	// const btVector3& pos1 = cp.getPositionWorldOnA();
677	// const btVector3& pos2 = cp.getPositionWorldOnB();
678
679	/////setup the friction constraints
680
681	solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
682
683	if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) \|\| !cp.m_lateralFrictionInitialized)
684	{
685	cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
686	btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
687	if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
688	{
689	cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
690	if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
691	{
692	cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
693	cp.m_lateralFrictionDir2.normalize();//??
694	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
695	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
696	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
697	}
698
699	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
700	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
701	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
702	cp.m_lateralFrictionInitialized = true;
703	} else
704	{
705	//re-calculate friction direction every frame, todo: check if this is really needed
706	btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
707	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
708	{
709	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
710	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
711	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
712	}
713
714	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
715	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
716	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
717
718	cp.m_lateralFrictionInitialized = true;
719	}
720
721	} else
722	{
723	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
724	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
725	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
726	}
727
728	setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
729
730	}
731	}
732	}
733
734
735	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject bodies, int numBodies, btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
736	{
737	BT_PROFILE("solveGroupCacheFriendlySetup");
738	(void)stackAlloc;
739	(void)debugDrawer;
740
741
742	if (!(numConstraints + numManifolds))
743	{
744	// printf("empty\n");
745	return 0.f;
746	}
747
748	if (infoGlobal.m_splitImpulse)
749	{
750	for (int i = 0; i < numBodies; i++)
751	{
752	btRigidBody* body = btRigidBody::upcast(bodies[i]);
753	if (body)
754	{
755	body->internalGetDeltaLinearVelocity().setZero();
756	body->internalGetDeltaAngularVelocity().setZero();
757	body->internalGetPushVelocity().setZero();
758	body->internalGetTurnVelocity().setZero();
759	}
760	}
761	}
762	else
763	{
764	for (int i = 0; i < numBodies; i++)
765	{
766	btRigidBody* body = btRigidBody::upcast(bodies[i]);
767	if (body)
768	{
769	body->internalGetDeltaLinearVelocity().setZero();
770	body->internalGetDeltaAngularVelocity().setZero();
771	}
772	}
773	}
774
775	if (1)
776	{
777	int j;
778	for (j=0;j<numConstraints;j++)
779	{
780	btTypedConstraint* constraint = constraints[j];
781	constraint->buildJacobian();
782	}
783	}
784	//btRigidBody* rb0=0,*rb1=0;
785
786	//if (1)
787	{
788	{
789
790	int totalNumRows = 0;
791	int i;
792
793	m_tmpConstraintSizesPool.resize(numConstraints);
794	//calculate the total number of contraint rows
795	for (i=0;i<numConstraints;i++)
796	{
797	btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
798	constraints[i]->getInfo1(&info1);
799	totalNumRows += info1.m_numConstraintRows;
800	}
801	m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
802
803
804	///setup the btSolverConstraints
805	int currentRow = 0;
806
807	for (i=0;i<numConstraints;i++)
808	{
809	const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
810
811	if (info1.m_numConstraintRows)
812	{
813	btAssert(currentRow<totalNumRows);
814
815	btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
816	btTypedConstraint* constraint = constraints[i];
817
818
819
820	btRigidBody& rbA = constraint->getRigidBodyA();
821	btRigidBody& rbB = constraint->getRigidBodyB();
822
823
824	int j;
825	for ( j=0;j<info1.m_numConstraintRows;j++)
826	{
827	memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
828	currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
829	currentConstraintRow[j].m_upperLimit = FLT_MAX;
830	currentConstraintRow[j].m_appliedImpulse = 0.f;
831	currentConstraintRow[j].m_appliedPushImpulse = 0.f;
832	currentConstraintRow[j].m_solverBodyA = &rbA;
833	currentConstraintRow[j].m_solverBodyB = &rbB;
834	}
835
836	rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
837	rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
838	rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
839	rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
840
841
842
843	btTypedConstraint::btConstraintInfo2 info2;
844	info2.fps = 1.f/infoGlobal.m_timeStep;
845	info2.erp = infoGlobal.m_erp;
846	info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
847	info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
848	info2.m_J2linearAxis = 0;
849	info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
850	info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
851	///the size of btSolverConstraint needs be a multiple of btScalar
852	btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
853	info2.m_constraintError = &currentConstraintRow->m_rhs;
854	currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
855	info2.m_damping = infoGlobal.m_damping;
856	info2.cfm = &currentConstraintRow->m_cfm;
857	info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
858	info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
859	info2.m_numIterations = infoGlobal.m_numIterations;
860	constraints[i]->getInfo2(&info2);
861
862	///finalize the constraint setup
863	for ( j=0;j<info1.m_numConstraintRows;j++)
864	{
865	btSolverConstraint& solverConstraint = currentConstraintRow[j];
866	solverConstraint.m_originalContactPoint = constraint;
867
868	{
869	const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
870	solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()ftorqueAxis1constraint->getRigidBodyA().getAngularFactor();
871	}
872	{
873	const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
874	solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()ftorqueAxis2constraint->getRigidBodyB().getAngularFactor();
875	}
876
877	{
878	btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
879	btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
880	btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
881	btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
882
883	btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
884	sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
885	sum += iMJlB.dot(solverConstraint.m_contactNormal);
886	sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
887
888	solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
889	}
890
891
892	///fix rhs
893	///todo: add force/torque accelerators
894	{
895	btScalar rel_vel;
896	btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
897	btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
898
899	rel_vel = vel1Dotn+vel2Dotn;
900
901	btScalar restitution = 0.f;
902	btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
903	btScalar velocityError = restitution - rel_vel * info2.m_damping;
904	btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
905	btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
906	solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
907	solverConstraint.m_appliedImpulse = 0.f;
908
909	}
910	}
911	}
912	currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
913	}
914	}
915
916	{
917	int i;
918	btPersistentManifold* manifold = 0;
919	// btCollisionObject* colObj0=0,*colObj1=0;
920
921
922	for (i=0;i<numManifolds;i++)
923	{
924	manifold = manifoldPtr[i];
925	convertContact(manifold,infoGlobal);
926	}
927	}
928	}
929
930	btContactSolverInfo info = infoGlobal;
931
932
933
934	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
935	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
936
937	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
938	m_orderTmpConstraintPool.resize(numConstraintPool);
939	m_orderFrictionConstraintPool.resize(numFrictionPool);
940	{
941	int i;
942	for (i=0;i<numConstraintPool;i++)
943	{
944	m_orderTmpConstraintPool[i] = i;
945	}
946	for (i=0;i<numFrictionPool;i++)
947	{
948	m_orderFrictionConstraintPool[i] = i;
949	}
950	}
951
952	return 0.f;
953
954	}
955
956	btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /bodies /,int /numBodies/,btPersistentManifold** /manifoldPtr/, int /numManifolds/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /debugDrawer/,btStackAlloc* /stackAlloc/)
957	{
958
959	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
960	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
961
962	int j;
963
964	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
965	{
966	if ((iteration & 7) == 0) {
967	for (j=0; j<numConstraintPool; ++j) {
968	int tmp = m_orderTmpConstraintPool[j];
969	int swapi = btRandInt2(j+1);
970	m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
971	m_orderTmpConstraintPool[swapi] = tmp;
972	}
973
974	for (j=0; j<numFrictionPool; ++j) {
975	int tmp = m_orderFrictionConstraintPool[j];
976	int swapi = btRandInt2(j+1);
977	m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
978	m_orderFrictionConstraintPool[swapi] = tmp;
979	}
980	}
981	}
982
983	if (infoGlobal.m_solverMode & SOLVER_SIMD)
984	{
985	///solve all joint constraints, using SIMD, if available
986	for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
987	{
988	btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
989	resolveSingleConstraintRowGenericSIMD(constraint.m_solverBodyA,constraint.m_solverBodyB,constraint);
990	}
991
992	for (j=0;j<numConstraints;j++)
993	{
994	constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
995	}
996
997	///solve all contact constraints using SIMD, if available
998	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
999	for (j=0;j<numPoolConstraints;j++)
1000	{
1001	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1002	resolveSingleConstraintRowLowerLimitSIMD(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1003
1004	}
1005	///solve all friction constraints, using SIMD, if available
1006	int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1007	for (j=0;j<numFrictionPoolConstraints;j++)
1008	{
1009	btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1010	btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1011
1012	if (totalImpulse>btScalar(0))
1013	{
1014	solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1015	solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1016
1017	resolveSingleConstraintRowGenericSIMD(solveManifold.m_solverBodyA, solveManifold.m_solverBodyB,solveManifold);
1018	}
1019	}
1020	} else
1021	{
1022
1023	///solve all joint constraints
1024	for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1025	{
1026	btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
1027	resolveSingleConstraintRowGeneric(constraint.m_solverBodyA,constraint.m_solverBodyB,constraint);
1028	}
1029
1030	for (j=0;j<numConstraints;j++)
1031	{
1032	constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1033	}
1034	///solve all contact constraints
1035	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1036	for (j=0;j<numPoolConstraints;j++)
1037	{
1038	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1039	resolveSingleConstraintRowLowerLimit(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1040	}
1041	///solve all friction constraints
1042	int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1043	for (j=0;j<numFrictionPoolConstraints;j++)
1044	{
1045	btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1046	btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1047
1048	if (totalImpulse>btScalar(0))
1049	{
1050	solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1051	solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1052
1053	resolveSingleConstraintRowGeneric(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1054	}
1055	}
1056	}
1057	return 0.f;
1058	}
1059
1060
1061	void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject bodies,int numBodies,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1062	{
1063	int iteration;
1064	if (infoGlobal.m_splitImpulse)
1065	{
1066	if (infoGlobal.m_solverMode & SOLVER_SIMD)
1067	{
1068	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1069	{
1070	{
1071	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1072	int j;
1073	for (j=0;j<numPoolConstraints;j++)
1074	{
1075	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1076
1077	resolveSplitPenetrationSIMD(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1078	}
1079	}
1080	}
1081	}
1082	else
1083	{
1084	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1085	{
1086	{
1087	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1088	int j;
1089	for (j=0;j<numPoolConstraints;j++)
1090	{
1091	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1092
1093	resolveSplitPenetrationImpulseCacheFriendly(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1094	}
1095	}
1096	}
1097	}
1098	}
1099	}
1100
1101	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject bodies ,int numBodies,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1102	{
1103	BT_PROFILE("solveGroupCacheFriendlyIterations");
1104
1105
1106	//should traverse the contacts random order...
1107	int iteration;
1108	{
1109	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1110	{
1111	solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1112	}
1113
1114	solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1115	}
1116	return 0.f;
1117	}
1118
1119	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject bodies ,int numBodies,btPersistentManifold /manifoldPtr/, int /numManifolds/,btTypedConstraint** /constraints/,int /* numConstraints/,const btContactSolverInfo& infoGlobal,btIDebugDraw /debugDrawer/,btStackAlloc* /stackAlloc/)
1120	{
1121	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1122	int i,j;
1123
1124	for (j=0;j<numPoolConstraints;j++)
1125	{
1126
1127	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1128	btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1129	btAssert(pt);
1130	pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1131	if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
1132	{
1133	pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1134	pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1135	}
1136
1137	//do a callback here?
1138	}
1139
1140	numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1141	for (j=0;j<numPoolConstraints;j++)
1142	{
1143	const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1144	btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1145	btScalar sum = constr->internalGetAppliedImpulse();
1146	sum += solverConstr.m_appliedImpulse;
1147	constr->internalSetAppliedImpulse(sum);
1148	}
1149
1150
1151	if (infoGlobal.m_splitImpulse)
1152	{
1153	for ( i=0;i<numBodies;i++)
1154	{
1155	btRigidBody* body = btRigidBody::upcast(bodies[i]);
1156	if (body)
1157	body->internalWritebackVelocity(infoGlobal.m_timeStep);
1158	}
1159	} else
1160	{
1161	for ( i=0;i<numBodies;i++)
1162	{
1163	btRigidBody* body = btRigidBody::upcast(bodies[i]);
1164	if (body)
1165	body->internalWritebackVelocity();
1166	}
1167	}
1168
1169
1170	m_tmpSolverContactConstraintPool.resize(0);
1171	m_tmpSolverNonContactConstraintPool.resize(0);
1172	m_tmpSolverContactFrictionConstraintPool.resize(0);
1173
1174	return 0.f;
1175	}
1176
1177
1178
1179	/// btSequentialImpulseConstraintSolver Sequentially applies impulses
1180	btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject bodies,int numBodies,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /dispatcher/)
1181	{
1182
1183	BT_PROFILE("solveGroup");
1184	//you need to provide at least some bodies
1185	btAssert(bodies);
1186	btAssert(numBodies);
1187
1188	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1189
1190	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1191
1192	solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1193
1194	return 0.f;
1195	}
1196
1197	void btSequentialImpulseConstraintSolver::reset()
1198	{
1199	m_btSeed2 = 0;
1200	}
1201
1202	btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
1203	{
1204	static btRigidBody s_fixed(0, 0,0);
1205	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
1206	return s_fixed;
1207	}
1208

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

Download in other formats: