Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/SuperOrxoBros_HS18/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @ 12177

Last change on this file since 12177 was 12177, checked in by siramesh, 5 years ago
Super Orxo Bros Final (Sidharth Ramesh, Nisa Balta, Jeff Ren)
File size: 49.7 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 btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
52	static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
53	{
54	__m128 result = _mm_mul_ps( vec0, vec1);
55	return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( 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(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
68	__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((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(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
131	__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
132	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
133	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
134	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
135	btSimdScalar resultLowerLess,resultUpperLess;
136	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
137	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
138	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
139	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
140	c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
141	__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
142	__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
143	__m128 impulseMagnitude = deltaImpulse;
144	body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
145	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
146	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
147	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
148	#else
149	resolveSingleConstraintRowLowerLimit(body1,body2,c);
150	#endif
151	}
152
153	// Project Gauss Seidel or the equivalent Sequential Impulse
154	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(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(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
219	__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
220	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
221	deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
222	btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
223	btSimdScalar resultLowerLess,resultUpperLess;
224	resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
225	resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
226	__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
227	deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
228	c.m_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	btScalar velocityError = restitution - rel_vel;// * damping;
561
562	if (penetration>0)
563	{
564	positionalError = 0;
565	velocityError -= penetration / infoGlobal.m_timeStep;
566	} else
567	{
568	positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
569	}
570
571	btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
572	btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
573	if (!infoGlobal.m_splitImpulse \|\| (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
574	{
575	//combine position and velocity into rhs
576	solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
577	solverConstraint.m_rhsPenetration = 0.f;
578	} else
579	{
580	//split position and velocity into rhs and m_rhsPenetration
581	solverConstraint.m_rhs = velocityImpulse;
582	solverConstraint.m_rhsPenetration = penetrationImpulse;
583	}
584	solverConstraint.m_cfm = 0.f;
585	solverConstraint.m_lowerLimit = 0;
586	solverConstraint.m_upperLimit = 1e10f;
587	}
588
589
590
591
592	}
593
594
595
596	void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
597	btRigidBody* rb0, btRigidBody* rb1,
598	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
599	{
600	if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
601	{
602	{
603	btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
604	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
605	{
606	frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
607	if (rb0)
608	rb0->internalApplyImpulse(frictionConstraint1.m_contactNormalrb0->getInvMass()rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
609	if (rb1)
610	rb1->internalApplyImpulse(frictionConstraint1.m_contactNormalrb1->getInvMass()rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
611	} else
612	{
613	frictionConstraint1.m_appliedImpulse = 0.f;
614	}
615	}
616
617	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
618	{
619	btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
620	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
621	{
622	frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
623	if (rb0)
624	rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
625	if (rb1)
626	rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
627	} else
628	{
629	frictionConstraint2.m_appliedImpulse = 0.f;
630	}
631	}
632	} else
633	{
634	btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
635	frictionConstraint1.m_appliedImpulse = 0.f;
636	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
637	{
638	btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
639	frictionConstraint2.m_appliedImpulse = 0.f;
640	}
641	}
642	}
643
644
645
646
647	void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
648	{
649	btCollisionObject* colObj0=0,*colObj1=0;
650
651	colObj0 = (btCollisionObject*)manifold->getBody0();
652	colObj1 = (btCollisionObject*)manifold->getBody1();
653
654
655	btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
656	btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
657
658	///avoid collision response between two static objects
659	if ((!solverBodyA \|\| !solverBodyA->getInvMass()) && (!solverBodyB \|\| !solverBodyB->getInvMass()))
660	return;
661
662	for (int j=0;j<manifold->getNumContacts();j++)
663	{
664
665	btManifoldPoint& cp = manifold->getContactPoint(j);
666
667	if (cp.getDistance() <= manifold->getContactProcessingThreshold())
668	{
669	btVector3 rel_pos1;
670	btVector3 rel_pos2;
671	btScalar relaxation;
672	btScalar rel_vel;
673	btVector3 vel;
674
675	int frictionIndex = m_tmpSolverContactConstraintPool.size();
676	btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
677	btRigidBody* rb0 = btRigidBody::upcast(colObj0);
678	btRigidBody* rb1 = btRigidBody::upcast(colObj1);
679	solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
680	solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
681	solverConstraint.m_originalContactPoint = &cp;
682
683	setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
684
685	// const btVector3& pos1 = cp.getPositionWorldOnA();
686	// const btVector3& pos2 = cp.getPositionWorldOnB();
687
688	/////setup the friction constraints
689
690	solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
691
692	if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) \|\| !cp.m_lateralFrictionInitialized)
693	{
694	cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
695	btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
696	if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
697	{
698	cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
699	if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
700	{
701	cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
702	cp.m_lateralFrictionDir2.normalize();//??
703	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
704	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
705	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
706	}
707
708	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
709	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
710	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
711	cp.m_lateralFrictionInitialized = true;
712	} else
713	{
714	//re-calculate friction direction every frame, todo: check if this is really needed
715	btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
716	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
717	{
718	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
719	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
720	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
721	}
722
723	applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
724	applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
725	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
726
727	cp.m_lateralFrictionInitialized = true;
728	}
729
730	} else
731	{
732	addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
733	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
734	addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
735	}
736
737	setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
738
739	}
740	}
741	}
742
743
744	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject bodies, int numBodies, btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
745	{
746	BT_PROFILE("solveGroupCacheFriendlySetup");
747	(void)stackAlloc;
748	(void)debugDrawer;
749
750
751	if (!(numConstraints + numManifolds))
752	{
753	// printf("empty\n");
754	return 0.f;
755	}
756
757	if (infoGlobal.m_splitImpulse)
758	{
759	for (int i = 0; i < numBodies; i++)
760	{
761	btRigidBody* body = btRigidBody::upcast(bodies[i]);
762	if (body)
763	{
764	body->internalGetDeltaLinearVelocity().setZero();
765	body->internalGetDeltaAngularVelocity().setZero();
766	body->internalGetPushVelocity().setZero();
767	body->internalGetTurnVelocity().setZero();
768	}
769	}
770	}
771	else
772	{
773	for (int i = 0; i < numBodies; i++)
774	{
775	btRigidBody* body = btRigidBody::upcast(bodies[i]);
776	if (body)
777	{
778	body->internalGetDeltaLinearVelocity().setZero();
779	body->internalGetDeltaAngularVelocity().setZero();
780	}
781	}
782	}
783
784	if (1)
785	{
786	int j;
787	for (j=0;j<numConstraints;j++)
788	{
789	btTypedConstraint* constraint = constraints[j];
790	constraint->buildJacobian();
791	constraint->internalSetAppliedImpulse(0.0f);
792	}
793	}
794	//btRigidBody* rb0=0,*rb1=0;
795
796	//if (1)
797	{
798	{
799
800	int totalNumRows = 0;
801	int i;
802
803	m_tmpConstraintSizesPool.resize(numConstraints);
804	//calculate the total number of contraint rows
805	for (i=0;i<numConstraints;i++)
806	{
807	btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
808	if (constraints[i]->isEnabled())
809	{
810	constraints[i]->getInfo1(&info1);
811	} else
812	{
813	info1.m_numConstraintRows = 0;
814	info1.nub = 0;
815	}
816	totalNumRows += info1.m_numConstraintRows;
817	}
818	m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
819
820
821	///setup the btSolverConstraints
822	int currentRow = 0;
823
824	for (i=0;i<numConstraints;i++)
825	{
826	const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
827
828	if (info1.m_numConstraintRows)
829	{
830	btAssert(currentRow<totalNumRows);
831
832	btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
833	btTypedConstraint* constraint = constraints[i];
834
835
836	btRigidBody& rbA = constraint->getRigidBodyA();
837	btRigidBody& rbB = constraint->getRigidBodyB();
838
839
840	int j;
841	for ( j=0;j<info1.m_numConstraintRows;j++)
842	{
843	memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
844	currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
845	currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
846	currentConstraintRow[j].m_appliedImpulse = 0.f;
847	currentConstraintRow[j].m_appliedPushImpulse = 0.f;
848	currentConstraintRow[j].m_solverBodyA = &rbA;
849	currentConstraintRow[j].m_solverBodyB = &rbB;
850	}
851
852	rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
853	rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
854	rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
855	rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
856
857
858
859	btTypedConstraint::btConstraintInfo2 info2;
860	info2.fps = 1.f/infoGlobal.m_timeStep;
861	info2.erp = infoGlobal.m_erp;
862	info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
863	info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
864	info2.m_J2linearAxis = 0;
865	info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
866	info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
867	///the size of btSolverConstraint needs be a multiple of btScalar
868	btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
869	info2.m_constraintError = &currentConstraintRow->m_rhs;
870	currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
871	info2.m_damping = infoGlobal.m_damping;
872	info2.cfm = &currentConstraintRow->m_cfm;
873	info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
874	info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
875	info2.m_numIterations = infoGlobal.m_numIterations;
876	constraints[i]->getInfo2(&info2);
877
878	if (currentConstraintRow->m_upperLimit>constraints[i]->getBreakingImpulseThreshold())
879	{
880	currentConstraintRow->m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
881	}
882
883	if (currentConstraintRow->m_lowerLimit<-constraints[i]->getBreakingImpulseThreshold())
884	{
885	currentConstraintRow->m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
886	}
887
888
889
890	///finalize the constraint setup
891	for ( j=0;j<info1.m_numConstraintRows;j++)
892	{
893	btSolverConstraint& solverConstraint = currentConstraintRow[j];
894	solverConstraint.m_originalContactPoint = constraint;
895
896	{
897	const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
898	solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()ftorqueAxis1constraint->getRigidBodyA().getAngularFactor();
899	}
900	{
901	const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
902	solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()ftorqueAxis2constraint->getRigidBodyB().getAngularFactor();
903	}
904
905	{
906	btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
907	btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
908	btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
909	btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
910
911	btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
912	sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
913	sum += iMJlB.dot(solverConstraint.m_contactNormal);
914	sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
915
916	solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
917	}
918
919
920	///fix rhs
921	///todo: add force/torque accelerators
922	{
923	btScalar rel_vel;
924	btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
925	btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
926
927	rel_vel = vel1Dotn+vel2Dotn;
928
929	btScalar restitution = 0.f;
930	btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
931	btScalar velocityError = restitution - rel_vel * info2.m_damping;
932	btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
933	btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
934	solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
935	solverConstraint.m_appliedImpulse = 0.f;
936
937	}
938	}
939	}
940	currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
941	}
942	}
943
944	{
945	int i;
946	btPersistentManifold* manifold = 0;
947	// btCollisionObject* colObj0=0,*colObj1=0;
948
949
950	for (i=0;i<numManifolds;i++)
951	{
952	manifold = manifoldPtr[i];
953	convertContact(manifold,infoGlobal);
954	}
955	}
956	}
957
958	btContactSolverInfo info = infoGlobal;
959
960
961
962	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
963	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
964
965	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
966	m_orderTmpConstraintPool.resize(numConstraintPool);
967	m_orderFrictionConstraintPool.resize(numFrictionPool);
968	{
969	int i;
970	for (i=0;i<numConstraintPool;i++)
971	{
972	m_orderTmpConstraintPool[i] = i;
973	}
974	for (i=0;i<numFrictionPool;i++)
975	{
976	m_orderFrictionConstraintPool[i] = i;
977	}
978	}
979
980	return 0.f;
981
982	}
983
984	btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /bodies /,int /numBodies/,btPersistentManifold** /manifoldPtr/, int /numManifolds/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /debugDrawer/,btStackAlloc* /stackAlloc/)
985	{
986
987	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
988	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
989
990	int j;
991
992	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
993	{
994	if ((iteration & 7) == 0) {
995	for (j=0; j<numConstraintPool; ++j) {
996	int tmp = m_orderTmpConstraintPool[j];
997	int swapi = btRandInt2(j+1);
998	m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
999	m_orderTmpConstraintPool[swapi] = tmp;
1000	}
1001
1002	for (j=0; j<numFrictionPool; ++j) {
1003	int tmp = m_orderFrictionConstraintPool[j];
1004	int swapi = btRandInt2(j+1);
1005	m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
1006	m_orderFrictionConstraintPool[swapi] = tmp;
1007	}
1008	}
1009	}
1010
1011	if (infoGlobal.m_solverMode & SOLVER_SIMD)
1012	{
1013	///solve all joint constraints, using SIMD, if available
1014	for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1015	{
1016	btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
1017	resolveSingleConstraintRowGenericSIMD(constraint.m_solverBodyA,constraint.m_solverBodyB,constraint);
1018	}
1019
1020	for (j=0;j<numConstraints;j++)
1021	{
1022	constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1023	}
1024
1025	///solve all contact constraints using SIMD, if available
1026	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1027	for (j=0;j<numPoolConstraints;j++)
1028	{
1029	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1030	resolveSingleConstraintRowLowerLimitSIMD(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1031
1032	}
1033	///solve all friction constraints, using SIMD, if available
1034	int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1035	for (j=0;j<numFrictionPoolConstraints;j++)
1036	{
1037	btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1038	btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1039
1040	if (totalImpulse>btScalar(0))
1041	{
1042	solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1043	solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1044
1045	resolveSingleConstraintRowGenericSIMD(solveManifold.m_solverBodyA, solveManifold.m_solverBodyB,solveManifold);
1046	}
1047	}
1048	} else
1049	{
1050
1051	///solve all joint constraints
1052	for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1053	{
1054	btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
1055	resolveSingleConstraintRowGeneric(constraint.m_solverBodyA,constraint.m_solverBodyB,constraint);
1056	}
1057
1058	for (j=0;j<numConstraints;j++)
1059	{
1060	constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1061	}
1062	///solve all contact constraints
1063	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1064	for (j=0;j<numPoolConstraints;j++)
1065	{
1066	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1067	resolveSingleConstraintRowLowerLimit(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1068	}
1069	///solve all friction constraints
1070	int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1071	for (j=0;j<numFrictionPoolConstraints;j++)
1072	{
1073	btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1074	btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1075
1076	if (totalImpulse>btScalar(0))
1077	{
1078	solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1079	solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1080
1081	resolveSingleConstraintRowGeneric(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1082	}
1083	}
1084	}
1085	return 0.f;
1086	}
1087
1088
1089	void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject bodies,int numBodies,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1090	{
1091	int iteration;
1092	if (infoGlobal.m_splitImpulse)
1093	{
1094	if (infoGlobal.m_solverMode & SOLVER_SIMD)
1095	{
1096	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1097	{
1098	{
1099	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1100	int j;
1101	for (j=0;j<numPoolConstraints;j++)
1102	{
1103	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1104
1105	resolveSplitPenetrationSIMD(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1106	}
1107	}
1108	}
1109	}
1110	else
1111	{
1112	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1113	{
1114	{
1115	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1116	int j;
1117	for (j=0;j<numPoolConstraints;j++)
1118	{
1119	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1120
1121	resolveSplitPenetrationImpulseCacheFriendly(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
1122	}
1123	}
1124	}
1125	}
1126	}
1127	}
1128
1129	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject bodies ,int numBodies,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1130	{
1131	BT_PROFILE("solveGroupCacheFriendlyIterations");
1132
1133
1134	//should traverse the contacts random order...
1135	int iteration;
1136	{
1137	solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1138
1139	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1140	{
1141	solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1142	}
1143
1144	}
1145	return 0.f;
1146	}
1147
1148	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject bodies ,int numBodies,btPersistentManifold /manifoldPtr/, int /numManifolds/,btTypedConstraint** /constraints/,int /* numConstraints/,const btContactSolverInfo& infoGlobal,btIDebugDraw /debugDrawer/,btStackAlloc* /stackAlloc/)
1149	{
1150	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1151	int i,j;
1152
1153	for (j=0;j<numPoolConstraints;j++)
1154	{
1155
1156	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1157	btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1158	btAssert(pt);
1159	pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1160	if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
1161	{
1162	pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1163	pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1164	}
1165
1166	//do a callback here?
1167	}
1168
1169	numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1170	for (j=0;j<numPoolConstraints;j++)
1171	{
1172	const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1173	btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1174	constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1175	if (solverConstr.m_appliedImpulse>constr->getBreakingImpulseThreshold())
1176	{
1177	constr->setEnabled(false);
1178	}
1179	}
1180
1181
1182	if (infoGlobal.m_splitImpulse)
1183	{
1184	for ( i=0;i<numBodies;i++)
1185	{
1186	btRigidBody* body = btRigidBody::upcast(bodies[i]);
1187	if (body)
1188	body->internalWritebackVelocity(infoGlobal.m_timeStep);
1189	}
1190	} else
1191	{
1192	for ( i=0;i<numBodies;i++)
1193	{
1194	btRigidBody* body = btRigidBody::upcast(bodies[i]);
1195	if (body)
1196	body->internalWritebackVelocity();
1197	}
1198	}
1199
1200
1201	m_tmpSolverContactConstraintPool.resize(0);
1202	m_tmpSolverNonContactConstraintPool.resize(0);
1203	m_tmpSolverContactFrictionConstraintPool.resize(0);
1204
1205	return 0.f;
1206	}
1207
1208
1209
1210	/// btSequentialImpulseConstraintSolver Sequentially applies impulses
1211	btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject bodies,int numBodies,btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /dispatcher/)
1212	{
1213
1214	BT_PROFILE("solveGroup");
1215	//you need to provide at least some bodies
1216	btAssert(bodies);
1217	btAssert(numBodies);
1218
1219	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1220
1221	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1222
1223	solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1224
1225	return 0.f;
1226	}
1227
1228	void btSequentialImpulseConstraintSolver::reset()
1229	{
1230	m_btSeed2 = 0;
1231	}
1232
1233	btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
1234	{
1235	static btRigidBody s_fixed(0, 0,0);
1236	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
1237	return s_fixed;
1238	}
1239

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

Download in other formats: