Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @ 6757

Last change on this file since 6757 was 5781, checked in by rgrieder, 15 years ago
Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).
Property svn:eol-style set to `native`
File size: 39.8 KB

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

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

Download in other formats: