Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 10385 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

Property svn:eol-style set to native

File size: 49.7 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
[8351]	37	int gNumSplitImpulseRecoveries = 0;
	38
[2882]	39	btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
	40	:m_btSeed2(0)
	41	{
[1963]	42
[2882]	43	}
[1963]	44
[2882]	45	btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
	46	{
	47	}
[1963]	48
[2882]	49	#ifdef USE_SIMD
	50	#include <emmintrin.h>
[8393]	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 )
[1963]	53	{
[2882]	54	__m128 result = _mm_mul_ps( vec0, vec1);
[8393]	55	return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
[2882]	56	}
	57	#endif//USE_SIMD
[1963]	58
[2882]	59	// Project Gauss Seidel or the equivalent Sequential Impulse
[8351]	60	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
[2882]	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)));
[8393]	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));
[2882]	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) );
[8351]	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);
[2882]	83	__m128 impulseMagnitude = deltaImpulse;
[8351]	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));
[2882]	88	#else
	89	resolveSingleConstraintRowGeneric(body1,body2,c);
	90	#endif
	91	}
[1963]	92
[2882]	93	// Project Gauss Seidel or the equivalent Sequential Impulse
[8351]	94	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
[2882]	95	{
	96	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
[8351]	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());
[1963]	99
[8351]	100	// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
[2882]	101	deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
	102	deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
[1963]	103
[2882]	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	}
[8351]	119	body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
	120	body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
[2882]	121	}
[1963]	122
[8351]	123	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& 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)));
[8393]	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));
[2882]	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) );
[8351]	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);
[2882]	143	__m128 impulseMagnitude = deltaImpulse;
[8351]	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));
[2882]	148	#else
	149	resolveSingleConstraintRowLowerLimit(body1,body2,c);
	150	#endif
[1963]	151	}
	152
[2882]	153	// Project Gauss Seidel or the equivalent Sequential Impulse
[8351]	154	void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
[1963]	155	{
[2882]	156	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
[8351]	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());
[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	}
[8351]	172	body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
	173	body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
[1963]	174	}
	175
	176
[8351]	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());
[1963]	188
[8351]	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)));
[8393]	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));
[8351]	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
[2882]	243	unsigned long btSequentialImpulseConstraintSolver::btRand2()
[1963]	244	{
[2882]	245	m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
	246	return m_btSeed2;
[1963]	247	}
	248
	249
	250
[2882]	251	//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
	252	int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
[1963]	253	{
[2882]	254	// seems good; xor-fold and modulus
	255	const unsigned long un = static_cast<unsigned long>(n);
	256	unsigned long r = btRand2();
[1963]	257
[2882]	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	}
[1963]	275
[2882]	276	return (int) (r % un);
[1963]	277	}
	278
[2882]	279
[8351]	280	#if 0
[2882]	281	void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
[1963]	282	{
[2882]	283	btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
[1963]	284
[8351]	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);
[1963]	289
	290	if (rb)
	291	{
[8351]	292	solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
[1963]	293	solverBody->m_originalBody = rb;
	294	solverBody->m_angularFactor = rb->getAngularFactor();
	295	} else
	296	{
[8351]	297	solverBody->internalGetInvMass().setValue(0,0,0);
[1963]	298	solverBody->m_originalBody = 0;
[8351]	299	solverBody->m_angularFactor.setValue(1,1,1);
[1963]	300	}
	301	}
[8351]	302	#endif
[1963]	303
	304
	305
[8351]	306
	307
[2882]	308	btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
[1963]	309	{
	310	btScalar rest = restitution * -rel_vel;
	311	return rest;
	312	}
	313
	314
	315
[2882]	316	void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
	317	void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
[1963]	318	{
[2882]	319	if (colObj && colObj->hasAnisotropicFriction())
[1963]	320	{
[2882]	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;
[1963]	328	}
	329	}
	330
	331
[8351]	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)
[1963]	333	{
	334
	335
	336	btRigidBody* body0=btRigidBody::upcast(colObj0);
	337	btRigidBody* body1=btRigidBody::upcast(colObj1);
	338
	339	solverConstraint.m_contactNormal = normalAxis;
	340
[8351]	341	solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
	342	solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
[1963]	343
	344	solverConstraint.m_friction = cp.m_combinedFriction;
	345	solverConstraint.m_originalContactPoint = 0;
	346
[2882]	347	solverConstraint.m_appliedImpulse = 0.f;
[8351]	348	solverConstraint.m_appliedPushImpulse = 0.f;
[2882]	349
[1963]	350	{
	351	btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
	352	solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
[2882]	353	solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()ftorqueAxis1body0->getAngularFactor() : btVector3(0,0,0);
[1963]	354	}
	355	{
[2882]	356	btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
[1963]	357	solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
[2882]	358	solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()ftorqueAxis1body1->getAngularFactor() : btVector3(0,0,0);
[1963]	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	{
[2882]	375	vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
[1963]	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
[2882]	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
[8351]	403	// btScalar positionalError = 0.f;
[2882]	404
[8351]	405	btSimdScalar velocityError = desiredVelocity - rel_vel;
[2882]	406	btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
	407	solverConstraint.m_rhs = velocityImpulse;
[8351]	408	solverConstraint.m_cfm = cfmSlip;
[2882]	409	solverConstraint.m_lowerLimit = 0;
	410	solverConstraint.m_upperLimit = 1e10f;
	411	}
[8351]	412	}
[2882]	413
[8351]	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);
[2430]	422	return solverConstraint;
[1963]	423	}
	424
[2882]	425	int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
[1963]	426	{
[8351]	427	#if 0
[2882]	428	int solverBodyIdA = -1;
[1963]	429
[2882]	430	if (body.getCompanionId() >= 0)
[1963]	431	{
[2882]	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	}
[1963]	447	}
[2882]	448	return solverBodyIdA;
[8351]	449	#endif
	450	return 0;
[2882]	451	}
	452	#include <stdio.h>
[1963]	453
	454
[8351]	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)
[2882]	460	{
[8351]	461	btRigidBody* rb0 = btRigidBody::upcast(colObj0);
	462	btRigidBody* rb1 = btRigidBody::upcast(colObj1);
[1963]	463
[2882]	464	const btVector3& pos1 = cp.getPositionWorldOnA();
	465	const btVector3& pos2 = cp.getPositionWorldOnB();
[1963]	466
[8351]	467	// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
	468	// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
[2882]	469	rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
	470	rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
[1963]	471
[2882]	472	relaxation = 1.f;
[1963]	473
[8351]	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);
[1963]	478
	479	{
[2882]	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)
[1963]	488	{
[2882]	489	vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
	490	denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
	491	}
	492	if (rb1)
[1963]	493	{
[2882]	494	vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
	495	denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
[1963]	496	}
[2882]	497	#endif //COMPUTE_IMPULSE_DENOM
[1963]	498
[2882]	499	btScalar denom = relaxation/(denom0+denom1);
	500	solverConstraint.m_jacDiagABInv = denom;
[1963]	501	}
	502
[2882]	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);
[1963]	506
[2882]	507
	508
	509
[8351]	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);
[2882]	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)
[1963]	523	{
[2882]	524	restitution = 0.f;
	525	} else
	526	{
	527	restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
	528	if (restitution <= btScalar(0.))
[1963]	529	{
[2882]	530	restitution = 0.f;
	531	};
	532	}
[1963]	533
	534
[2882]	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)
[8351]	540	rb0->internalApplyImpulse(solverConstraint.m_contactNormalrb0->getInvMass()rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
[2882]	541	if (rb1)
[8351]	542	rb1->internalApplyImpulse(solverConstraint.m_contactNormalrb1->getInvMass()rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
[2882]	543	} else
	544	{
	545	solverConstraint.m_appliedImpulse = 0.f;
	546	}
[1963]	547
[8351]	548	solverConstraint.m_appliedPushImpulse = 0.f;
[1963]	549
[2882]	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));
[1963]	556
[2882]	557	rel_vel = vel1Dotn+vel2Dotn;
[1963]	558
[2882]	559	btScalar positionalError = 0.f;
	560	btScalar velocityError = restitution - rel_vel;// * damping;
[8393]	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
[2882]	571	btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
	572	btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
[8351]	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	}
[2882]	584	solverConstraint.m_cfm = 0.f;
	585	solverConstraint.m_lowerLimit = 0;
	586	solverConstraint.m_upperLimit = 1e10f;
	587	}
[1963]	588
	589
	590
	591
[8351]	592	}
[1963]	593
[2430]	594
[1963]	595
[8351]	596	void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
	597	btRigidBody* rb0, btRigidBody* rb1,
	598	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
	599	{
[2882]	600	if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
	601	{
	602	{
	603	btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
[1963]	604	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
	605	{
[2882]	606	frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
[1963]	607	if (rb0)
[8351]	608	rb0->internalApplyImpulse(frictionConstraint1.m_contactNormalrb0->getInvMass()rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
[1963]	609	if (rb1)
[8351]	610	rb1->internalApplyImpulse(frictionConstraint1.m_contactNormalrb1->getInvMass()rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
[1963]	611	} else
	612	{
[2882]	613	frictionConstraint1.m_appliedImpulse = 0.f;
[1963]	614	}
[2882]	615	}
[1963]	616
[2882]	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)
[1963]	621	{
[2882]	622	frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
	623	if (rb0)
[8351]	624	rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
[2882]	625	if (rb1)
[8351]	626	rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
[1963]	627	} else
	628	{
[2882]	629	frictionConstraint2.m_appliedImpulse = 0.f;
[1963]	630	}
	631	}
[2882]	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	}
[1963]	641	}
[8351]	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;
[1963]	728	}
[8351]	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);
[1963]	735	}
[8351]	736
	737	setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
[2882]	738
[1963]	739	}
	740	}
[2882]	741	}
[1963]	742
[2882]	743
[8351]	744	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject bodies, int numBodies, btPersistentManifold manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
[2882]	745	{
	746	BT_PROFILE("solveGroupCacheFriendlySetup");
	747	(void)stackAlloc;
	748	(void)debugDrawer;
	749
	750
	751	if (!(numConstraints + numManifolds))
[1963]	752	{
[2882]	753	// printf("empty\n");
	754	return 0.f;
	755	}
	756
[8351]	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
[2882]	784	if (1)
	785	{
[1963]	786	int j;
	787	for (j=0;j<numConstraints;j++)
	788	{
	789	btTypedConstraint* constraint = constraints[j];
	790	constraint->buildJacobian();
[8393]	791	constraint->internalSetAppliedImpulse(0.0f);
[1963]	792	}
	793	}
[2882]	794	//btRigidBody* rb0=0,*rb1=0;
	795
	796	//if (1)
	797	{
	798	{
	799
	800	int totalNumRows = 0;
	801	int i;
[8351]	802
	803	m_tmpConstraintSizesPool.resize(numConstraints);
[2882]	804	//calculate the total number of contraint rows
	805	for (i=0;i<numConstraints;i++)
	806	{
[8351]	807	btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
[8393]	808	if (constraints[i]->isEnabled())
	809	{
	810	constraints[i]->getInfo1(&info1);
	811	} else
	812	{
	813	info1.m_numConstraintRows = 0;
	814	info1.nub = 0;
	815	}
[2882]	816	totalNumRows += info1.m_numConstraintRows;
	817	}
	818	m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
	819
[8351]	820
[2882]	821	///setup the btSolverConstraints
	822	int currentRow = 0;
	823
[8351]	824	for (i=0;i<numConstraints;i++)
[2882]	825	{
[8351]	826	const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
	827
[2882]	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
[8351]	839
[2882]	840	int j;
	841	for ( j=0;j<info1.m_numConstraintRows;j++)
	842	{
	843	memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
[8393]	844	currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
	845	currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
[2882]	846	currentConstraintRow[j].m_appliedImpulse = 0.f;
	847	currentConstraintRow[j].m_appliedPushImpulse = 0.f;
[8351]	848	currentConstraintRow[j].m_solverBodyA = &rbA;
	849	currentConstraintRow[j].m_solverBodyB = &rbB;
[2882]	850	}
	851
[8351]	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);
[2882]	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;
[8351]	870	currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
	871	info2.m_damping = infoGlobal.m_damping;
[2882]	872	info2.cfm = &currentConstraintRow->m_cfm;
	873	info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
	874	info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
[8351]	875	info2.m_numIterations = infoGlobal.m_numIterations;
[2882]	876	constraints[i]->getInfo2(&info2);
	877
[8393]	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
[2882]	890	///finalize the constraint setup
	891	for ( j=0;j<info1.m_numConstraintRows;j++)
	892	{
	893	btSolverConstraint& solverConstraint = currentConstraintRow[j];
[8351]	894	solverConstraint.m_originalContactPoint = constraint;
[2882]	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
[8351]	931	btScalar velocityError = restitution - rel_vel * info2.m_damping;
[2882]	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	}
[8351]	940	currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
[2882]	941	}
	942	}
	943
	944	{
	945	int i;
	946	btPersistentManifold* manifold = 0;
[8351]	947	// btCollisionObject* colObj0=0,*colObj1=0;
[2882]	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
[2430]	965	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
[1963]	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
[8351]	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/)
[1963]	985	{
	986
[2882]	987	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
	988	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
	989
[8351]	990	int j;
	991
	992	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
[1963]	993	{
[8351]	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	}
[1963]	1001
[8351]	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))
[1963]	1041	{
[8351]	1042	solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
	1043	solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
[1963]	1044
[8351]	1045	resolveSingleConstraintRowGenericSIMD(solveManifold.m_solverBodyA, solveManifold.m_solverBodyB,solveManifold);
[1963]	1046	}
[8351]	1047	}
	1048	} else
	1049	{
[1963]	1050
[8351]	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))
[1963]	1077	{
[8351]	1078	solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
	1079	solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
[2882]	1080
[8351]	1081	resolveSingleConstraintRowGeneric(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
	1082	}
	1083	}
	1084	}
	1085	return 0.f;
	1086	}
[1963]	1087
	1088
[8351]	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	{
[1963]	1098	{
[8351]	1099	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
	1100	int j;
	1101	for (j=0;j<numPoolConstraints;j++)
[2882]	1102	{
[8351]	1103	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
[1963]	1104
[8351]	1105	resolveSplitPenetrationSIMD(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
[2882]	1106	}
	1107	}
[8351]	1108	}
	1109	}
	1110	else
	1111	{
	1112	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
[1963]	1113	{
	1114	{
[8351]	1115	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
	1116	int j;
	1117	for (j=0;j<numPoolConstraints;j++)
[1963]	1118	{
[8351]	1119	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
[1963]	1120
[8351]	1121	resolveSplitPenetrationImpulseCacheFriendly(solveManifold.m_solverBodyA,solveManifold.m_solverBodyB,solveManifold);
[1963]	1122	}
	1123	}
	1124	}
[8351]	1125	}
	1126	}
	1127	}
[1963]	1128
[8351]	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");
[2882]	1132
[8351]	1133
	1134	//should traverse the contacts random order...
	1135	int iteration;
	1136	{
[8393]	1137	solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
	1138
[8351]	1139	for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
	1140	{
	1141	solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
[1963]	1142	}
[8351]	1143
[1963]	1144	}
	1145	return 0.f;
	1146	}
	1147
[8351]	1148	btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject bodies ,int numBodies,btPersistentManifold /manifoldPtr/, int /numManifolds/,btTypedConstraint** /constraints/,int /* numConstraints/,const btContactSolverInfo& infoGlobal,btIDebugDraw /debugDrawer/,btStackAlloc* /stackAlloc/)
[1963]	1149	{
[2882]	1150	int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
[8351]	1151	int i,j;
[2882]	1152
[1963]	1153	for (j=0;j<numPoolConstraints;j++)
	1154	{
[2882]	1155
	1156	const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
[1963]	1157	btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
	1158	btAssert(pt);
	1159	pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
[2430]	1160	if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
	1161	{
[2882]	1162	pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
	1163	pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
[2430]	1164	}
[1963]	1165
	1166	//do a callback here?
	1167	}
	1168
[8351]	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;
[8393]	1174	constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
	1175	if (solverConstr.m_appliedImpulse>constr->getBreakingImpulseThreshold())
	1176	{
	1177	constr->setEnabled(false);
	1178	}
[8351]	1179	}
	1180
	1181
[1963]	1182	if (infoGlobal.m_splitImpulse)
	1183	{
[8351]	1184	for ( i=0;i<numBodies;i++)
[1963]	1185	{
[8351]	1186	btRigidBody* body = btRigidBody::upcast(bodies[i]);
	1187	if (body)
	1188	body->internalWritebackVelocity(infoGlobal.m_timeStep);
[1963]	1189	}
	1190	} else
	1191	{
[8351]	1192	for ( i=0;i<numBodies;i++)
[2882]	1193	{
[8351]	1194	btRigidBody* body = btRigidBody::upcast(bodies[i]);
	1195	if (body)
	1196	body->internalWritebackVelocity();
[2882]	1197	}
[1963]	1198	}
	1199
	1200
[2882]	1201	m_tmpSolverContactConstraintPool.resize(0);
	1202	m_tmpSolverNonContactConstraintPool.resize(0);
	1203	m_tmpSolverContactFrictionConstraintPool.resize(0);
[1963]	1204
	1205	return 0.f;
	1206	}
	1207
	1208
	1209
[8351]	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	{
[1963]	1213
[8351]	1214	BT_PROFILE("solveGroup");
	1215	//you need to provide at least some bodies
	1216	btAssert(bodies);
	1217	btAssert(numBodies);
[1963]	1218
[8351]	1219	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
[1963]	1220
[8351]	1221	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
[1963]	1222
[8351]	1223	solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
	1224
	1225	return 0.f;
	1226	}
[1963]	1227
	1228	void btSequentialImpulseConstraintSolver::reset()
	1229	{
	1230	m_btSeed2 = 0;
	1231	}
	1232
[8351]	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	}
[1963]	1239

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

Download in other formats: