Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @ 8474

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

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

Download in other formats: