2017-08-01 12:30:58 +00:00
/*
Bullet Continuous Collision Detection and Physics Library
2022-01-06 22:37:49 +00:00
Copyright ( c ) 2003 - 2006 Erwin Coumans https : //bulletphysics.org
2017-08-01 12:30:58 +00:00
This software is provided ' as - is ' , without any express or implied warranty .
In no event will the authors be held liable for any damages arising from the use of this software .
Permission is granted to anyone to use this software for any purpose ,
including commercial applications , and to alter it and redistribute it freely ,
subject to the following restrictions :
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 .
2. Altered source versions must be plainly marked as such , and must not be misrepresented as being the original software .
3. This notice may not be removed or altered from any source distribution .
*/
//#define COMPUTE_IMPULSE_DENOM 1
2021-09-29 13:47:08 +00:00
# ifdef BT_DEBUG
# define BT_ADDITIONAL_DEBUG
# endif
2017-08-01 12:30:58 +00:00
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
# include "btSequentialImpulseConstraintSolver.h"
# include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
# include "LinearMath/btIDebugDraw.h"
# include "LinearMath/btCpuFeatureUtility.h"
//#include "btJacobianEntry.h"
# include "LinearMath/btMinMax.h"
# include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
# include <new>
# include "LinearMath/btStackAlloc.h"
# include "LinearMath/btQuickprof.h"
//#include "btSolverBody.h"
//#include "btSolverConstraint.h"
# include "LinearMath/btAlignedObjectArray.h"
2019-01-03 13:26:51 +00:00
# include <string.h> //for memset
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
int gNumSplitImpulseRecoveries = 0 ;
2017-08-01 12:30:58 +00:00
# include "BulletDynamics/Dynamics/btRigidBody.h"
//#define VERBOSE_RESIDUAL_PRINTF 1
///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
2018-09-07 14:11:04 +00:00
static btScalar gResolveSingleConstraintRowGeneric_scalar_reference ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
btScalar deltaImpulse = c . m_rhs - btScalar ( c . m_appliedImpulse ) * c . m_cfm ;
2018-09-07 14:11:04 +00:00
const btScalar deltaVel1Dotn = c . m_contactNormal1 . dot ( bodyA . internalGetDeltaLinearVelocity ( ) ) + c . m_relpos1CrossNormal . dot ( bodyA . internalGetDeltaAngularVelocity ( ) ) ;
const btScalar deltaVel2Dotn = c . m_contactNormal2 . dot ( bodyB . internalGetDeltaLinearVelocity ( ) ) + c . m_relpos2CrossNormal . dot ( bodyB . internalGetDeltaAngularVelocity ( ) ) ;
2017-08-01 12:30:58 +00:00
// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
2019-01-03 13:26:51 +00:00
deltaImpulse - = deltaVel1Dotn * c . m_jacDiagABInv ;
deltaImpulse - = deltaVel2Dotn * c . m_jacDiagABInv ;
2017-08-01 12:30:58 +00:00
const btScalar sum = btScalar ( c . m_appliedImpulse ) + deltaImpulse ;
if ( sum < c . m_lowerLimit )
{
deltaImpulse = c . m_lowerLimit - c . m_appliedImpulse ;
c . m_appliedImpulse = c . m_lowerLimit ;
}
else if ( sum > c . m_upperLimit )
{
deltaImpulse = c . m_upperLimit - c . m_appliedImpulse ;
c . m_appliedImpulse = c . m_upperLimit ;
}
else
{
c . m_appliedImpulse = sum ;
}
2019-01-03 13:26:51 +00:00
bodyA . internalApplyImpulse ( c . m_contactNormal1 * bodyA . internalGetInvMass ( ) , c . m_angularComponentA , deltaImpulse ) ;
bodyB . internalApplyImpulse ( c . m_contactNormal2 * bodyB . internalGetInvMass ( ) , c . m_angularComponentB , deltaImpulse ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
return deltaImpulse * ( 1. / c . m_jacDiagABInv ) ;
2017-08-01 12:30:58 +00:00
}
2018-09-07 14:11:04 +00:00
static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
btScalar deltaImpulse = c . m_rhs - btScalar ( c . m_appliedImpulse ) * c . m_cfm ;
2018-09-07 14:11:04 +00:00
const btScalar deltaVel1Dotn = c . m_contactNormal1 . dot ( bodyA . internalGetDeltaLinearVelocity ( ) ) + c . m_relpos1CrossNormal . dot ( bodyA . internalGetDeltaAngularVelocity ( ) ) ;
const btScalar deltaVel2Dotn = c . m_contactNormal2 . dot ( bodyB . internalGetDeltaLinearVelocity ( ) ) + c . m_relpos2CrossNormal . dot ( bodyB . internalGetDeltaAngularVelocity ( ) ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
deltaImpulse - = deltaVel1Dotn * c . m_jacDiagABInv ;
deltaImpulse - = deltaVel2Dotn * c . m_jacDiagABInv ;
2017-08-01 12:30:58 +00:00
const btScalar sum = btScalar ( c . m_appliedImpulse ) + deltaImpulse ;
if ( sum < c . m_lowerLimit )
{
deltaImpulse = c . m_lowerLimit - c . m_appliedImpulse ;
c . m_appliedImpulse = c . m_lowerLimit ;
}
else
{
c . m_appliedImpulse = sum ;
}
2019-01-03 13:26:51 +00:00
bodyA . internalApplyImpulse ( c . m_contactNormal1 * bodyA . internalGetInvMass ( ) , c . m_angularComponentA , deltaImpulse ) ;
bodyB . internalApplyImpulse ( c . m_contactNormal2 * bodyB . internalGetInvMass ( ) , c . m_angularComponentB , deltaImpulse ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
return deltaImpulse * ( 1. / c . m_jacDiagABInv ) ;
2017-08-01 12:30:58 +00:00
}
# ifdef USE_SIMD
# include <emmintrin.h>
2019-01-03 13:26:51 +00:00
# define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e))
static inline __m128 btSimdDot3 ( __m128 vec0 , __m128 vec1 )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
__m128 result = _mm_mul_ps ( vec0 , vec1 ) ;
return _mm_add_ps ( btVecSplat ( result , 0 ) , _mm_add_ps ( btVecSplat ( result , 1 ) , btVecSplat ( result , 2 ) ) ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
# if defined(BT_ALLOW_SSE4)
2017-08-01 12:30:58 +00:00
# include <intrin.h>
2019-01-03 13:26:51 +00:00
# define USE_FMA 1
# define USE_FMA3_INSTEAD_FMA4 1
# define USE_SSE4_DOT 1
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
# define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f)
# define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f))
2017-08-01 12:30:58 +00:00
# if USE_SSE4_DOT
2019-01-03 13:26:51 +00:00
# define DOT_PRODUCT(a, b) SSE4_DP(a, b)
2017-08-01 12:30:58 +00:00
# else
2019-01-03 13:26:51 +00:00
# define DOT_PRODUCT(a, b) btSimdDot3(a, b)
2017-08-01 12:30:58 +00:00
# endif
# if USE_FMA
# if USE_FMA3_INSTEAD_FMA4
// a*b + c
2019-01-03 13:26:51 +00:00
# define FMADD(a, b, c) _mm_fmadd_ps(a, b, c)
2017-08-01 12:30:58 +00:00
// -(a*b) + c
2019-01-03 13:26:51 +00:00
# define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c)
# else // USE_FMA3
2017-08-01 12:30:58 +00:00
// a*b + c
2019-01-03 13:26:51 +00:00
# define FMADD(a, b, c) _mm_macc_ps(a, b, c)
2017-08-01 12:30:58 +00:00
// -(a*b) + c
2019-01-03 13:26:51 +00:00
# define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
2017-08-01 12:30:58 +00:00
# endif
2019-01-03 13:26:51 +00:00
# else // USE_FMA
2017-08-01 12:30:58 +00:00
// c + a*b
2019-01-03 13:26:51 +00:00
# define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b))
2017-08-01 12:30:58 +00:00
// c - a*b
2019-01-03 13:26:51 +00:00
# define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b))
2017-08-01 12:30:58 +00:00
# endif
# endif
// Project Gauss Seidel or the equivalent Sequential Impulse
2018-09-07 14:11:04 +00:00
static btScalar gResolveSingleConstraintRowGeneric_sse2 ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
__m128 cpAppliedImp = _mm_set1_ps ( c . m_appliedImpulse ) ;
2019-01-03 13:26:51 +00:00
__m128 lowerLimit1 = _mm_set1_ps ( c . m_lowerLimit ) ;
__m128 upperLimit1 = _mm_set1_ps ( c . m_upperLimit ) ;
2017-08-01 12:30:58 +00:00
btSimdScalar 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 ) ) ) ;
2018-09-07 14:11:04 +00:00
__m128 deltaVel1Dotn = _mm_add_ps ( btSimdDot3 ( c . m_contactNormal1 . mVec128 , bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 ) , btSimdDot3 ( c . m_relpos1CrossNormal . mVec128 , bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 ) ) ;
__m128 deltaVel2Dotn = _mm_add_ps ( btSimdDot3 ( c . m_contactNormal2 . mVec128 , bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 ) , btSimdDot3 ( c . m_relpos2CrossNormal . mVec128 , bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 ) ) ;
2017-08-01 12:30:58 +00:00
deltaImpulse = _mm_sub_ps ( deltaImpulse , _mm_mul_ps ( deltaVel1Dotn , _mm_set1_ps ( c . m_jacDiagABInv ) ) ) ;
deltaImpulse = _mm_sub_ps ( deltaImpulse , _mm_mul_ps ( deltaVel2Dotn , _mm_set1_ps ( c . m_jacDiagABInv ) ) ) ;
btSimdScalar sum = _mm_add_ps ( cpAppliedImp , deltaImpulse ) ;
btSimdScalar resultLowerLess , resultUpperLess ;
resultLowerLess = _mm_cmplt_ps ( sum , lowerLimit1 ) ;
resultUpperLess = _mm_cmplt_ps ( sum , upperLimit1 ) ;
__m128 lowMinApplied = _mm_sub_ps ( lowerLimit1 , cpAppliedImp ) ;
d eltaImpulse = _mm_or_ps ( _mm_and_ps ( resultLowerLess , lowMinApplied ) , _mm_andnot_ps ( resultLowerLess , deltaImpulse ) ) ;
c . m_appliedImpulse = _mm_or_ps ( _mm_and_ps ( resultLowerLess , lowerLimit1 ) , _mm_andnot_ps ( resultLowerLess , sum ) ) ;
__m128 upperMinApplied = _mm_sub_ps ( upperLimit1 , cpAppliedImp ) ;
deltaImpulse = _mm_or_ps ( _mm_and_ps ( resultUpperLess , deltaImpulse ) , _mm_andnot_ps ( resultUpperLess , upperMinApplied ) ) ;
c . m_appliedImpulse = _mm_or_ps ( _mm_and_ps ( resultUpperLess , c . m_appliedImpulse ) , _mm_andnot_ps ( resultUpperLess , upperLimit1 ) ) ;
2019-01-03 13:26:51 +00:00
__m128 linearComponentA = _mm_mul_ps ( c . m_contactNormal1 . mVec128 , bodyA . internalGetInvMass ( ) . mVec128 ) ;
__m128 linearComponentB = _mm_mul_ps ( ( c . m_contactNormal2 ) . mVec128 , bodyB . internalGetInvMass ( ) . mVec128 ) ;
2017-08-01 12:30:58 +00:00
__m128 impulseMagnitude = deltaImpulse ;
2018-09-07 14:11:04 +00:00
bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 = _mm_add_ps ( bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 , _mm_mul_ps ( linearComponentA , impulseMagnitude ) ) ;
bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 = _mm_add_ps ( bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 , _mm_mul_ps ( c . m_angularComponentA . mVec128 , impulseMagnitude ) ) ;
bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 = _mm_add_ps ( bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 , _mm_mul_ps ( linearComponentB , impulseMagnitude ) ) ;
bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 = _mm_add_ps ( bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 , _mm_mul_ps ( c . m_angularComponentB . mVec128 , impulseMagnitude ) ) ;
2019-01-03 13:26:51 +00:00
return deltaImpulse . m_floats [ 0 ] / c . m_jacDiagABInv ;
2017-08-01 12:30:58 +00:00
}
// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
2018-09-07 14:11:04 +00:00
static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3 ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
# if defined(BT_ALLOW_SSE4)
__m128 tmp = _mm_set_ps1 ( c . m_jacDiagABInv ) ;
__m128 deltaImpulse = _mm_set_ps1 ( c . m_rhs - btScalar ( c . m_appliedImpulse ) * c . m_cfm ) ;
const __m128 lowerLimit = _mm_set_ps1 ( c . m_lowerLimit ) ;
const __m128 upperLimit = _mm_set_ps1 ( c . m_upperLimit ) ;
const __m128 deltaVel1Dotn = _mm_add_ps ( DOT_PRODUCT ( c . m_contactNormal1 . mVec128 , bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 ) , DOT_PRODUCT ( c . m_relpos1CrossNormal . mVec128 , bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 ) ) ;
const __m128 deltaVel2Dotn = _mm_add_ps ( DOT_PRODUCT ( c . m_contactNormal2 . mVec128 , bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 ) , DOT_PRODUCT ( c . m_relpos2CrossNormal . mVec128 , bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 ) ) ;
deltaImpulse = FMNADD ( deltaVel1Dotn , tmp , deltaImpulse ) ;
deltaImpulse = FMNADD ( deltaVel2Dotn , tmp , deltaImpulse ) ;
tmp = _mm_add_ps ( c . m_appliedImpulse , deltaImpulse ) ; // sum
const __m128 maskLower = _mm_cmpgt_ps ( tmp , lowerLimit ) ;
const __m128 maskUpper = _mm_cmpgt_ps ( upperLimit , tmp ) ;
deltaImpulse = _mm_blendv_ps ( _mm_sub_ps ( lowerLimit , c . m_appliedImpulse ) , _mm_blendv_ps ( _mm_sub_ps ( upperLimit , c . m_appliedImpulse ) , deltaImpulse , maskUpper ) , maskLower ) ;
c . m_appliedImpulse = _mm_blendv_ps ( lowerLimit , _mm_blendv_ps ( upperLimit , tmp , maskUpper ) , maskLower ) ;
bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 = FMADD ( _mm_mul_ps ( c . m_contactNormal1 . mVec128 , bodyA . internalGetInvMass ( ) . mVec128 ) , deltaImpulse , bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 ) ;
2018-09-07 14:11:04 +00:00
bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 = FMADD ( c . m_angularComponentA . mVec128 , deltaImpulse , bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 ) ;
2019-01-03 13:26:51 +00:00
bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 = FMADD ( _mm_mul_ps ( c . m_contactNormal2 . mVec128 , bodyB . internalGetInvMass ( ) . mVec128 ) , deltaImpulse , bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 ) ;
2018-09-07 14:11:04 +00:00
bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 = FMADD ( c . m_angularComponentB . mVec128 , deltaImpulse , bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 ) ;
btSimdScalar deltaImp = deltaImpulse ;
2019-01-03 13:26:51 +00:00
return deltaImp . m_floats [ 0 ] * ( 1. / c . m_jacDiagABInv ) ;
2017-08-01 12:30:58 +00:00
# else
2019-01-03 13:26:51 +00:00
return gResolveSingleConstraintRowGeneric_sse2 ( bodyA , bodyB , c ) ;
2017-08-01 12:30:58 +00:00
# endif
}
2018-09-07 14:11:04 +00:00
static btScalar gResolveSingleConstraintRowLowerLimit_sse2 ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
__m128 cpAppliedImp = _mm_set1_ps ( c . m_appliedImpulse ) ;
2019-01-03 13:26:51 +00:00
__m128 lowerLimit1 = _mm_set1_ps ( c . m_lowerLimit ) ;
__m128 upperLimit1 = _mm_set1_ps ( c . m_upperLimit ) ;
2017-08-01 12:30:58 +00:00
btSimdScalar 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 ) ) ) ;
2018-09-07 14:11:04 +00:00
__m128 deltaVel1Dotn = _mm_add_ps ( btSimdDot3 ( c . m_contactNormal1 . mVec128 , bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 ) , btSimdDot3 ( c . m_relpos1CrossNormal . mVec128 , bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 ) ) ;
__m128 deltaVel2Dotn = _mm_add_ps ( btSimdDot3 ( c . m_contactNormal2 . mVec128 , bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 ) , btSimdDot3 ( c . m_relpos2CrossNormal . mVec128 , bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 ) ) ;
2017-08-01 12:30:58 +00:00
deltaImpulse = _mm_sub_ps ( deltaImpulse , _mm_mul_ps ( deltaVel1Dotn , _mm_set1_ps ( c . m_jacDiagABInv ) ) ) ;
deltaImpulse = _mm_sub_ps ( deltaImpulse , _mm_mul_ps ( deltaVel2Dotn , _mm_set1_ps ( c . m_jacDiagABInv ) ) ) ;
btSimdScalar sum = _mm_add_ps ( cpAppliedImp , deltaImpulse ) ;
btSimdScalar resultLowerLess , resultUpperLess ;
resultLowerLess = _mm_cmplt_ps ( sum , lowerLimit1 ) ;
resultUpperLess = _mm_cmplt_ps ( sum , upperLimit1 ) ;
__m128 lowMinApplied = _mm_sub_ps ( lowerLimit1 , cpAppliedImp ) ;
deltaImpulse = _mm_or_ps ( _mm_and_ps ( resultLowerLess , lowMinApplied ) , _mm_andnot_ps ( resultLowerLess , deltaImpulse ) ) ;
c . m_appliedImpulse = _mm_or_ps ( _mm_and_ps ( resultLowerLess , lowerLimit1 ) , _mm_andnot_ps ( resultLowerLess , sum ) ) ;
2019-01-03 13:26:51 +00:00
__m128 linearComponentA = _mm_mul_ps ( c . m_contactNormal1 . mVec128 , bodyA . internalGetInvMass ( ) . mVec128 ) ;
__m128 linearComponentB = _mm_mul_ps ( c . m_contactNormal2 . mVec128 , bodyB . internalGetInvMass ( ) . mVec128 ) ;
2017-08-01 12:30:58 +00:00
__m128 impulseMagnitude = deltaImpulse ;
2018-09-07 14:11:04 +00:00
bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 = _mm_add_ps ( bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 , _mm_mul_ps ( linearComponentA , impulseMagnitude ) ) ;
bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 = _mm_add_ps ( bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 , _mm_mul_ps ( c . m_angularComponentA . mVec128 , impulseMagnitude ) ) ;
bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 = _mm_add_ps ( bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 , _mm_mul_ps ( linearComponentB , impulseMagnitude ) ) ;
bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 = _mm_add_ps ( bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 , _mm_mul_ps ( c . m_angularComponentB . mVec128 , impulseMagnitude ) ) ;
2019-01-03 13:26:51 +00:00
return deltaImpulse . m_floats [ 0 ] / c . m_jacDiagABInv ;
2017-08-01 12:30:58 +00:00
}
// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
2018-09-07 14:11:04 +00:00
static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3 ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
# ifdef BT_ALLOW_SSE4
2019-01-03 13:26:51 +00:00
__m128 tmp = _mm_set_ps1 ( c . m_jacDiagABInv ) ;
__m128 deltaImpulse = _mm_set_ps1 ( c . m_rhs - btScalar ( c . m_appliedImpulse ) * c . m_cfm ) ;
const __m128 lowerLimit = _mm_set_ps1 ( c . m_lowerLimit ) ;
const __m128 deltaVel1Dotn = _mm_add_ps ( DOT_PRODUCT ( c . m_contactNormal1 . mVec128 , bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 ) , DOT_PRODUCT ( c . m_relpos1CrossNormal . mVec128 , bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 ) ) ;
const __m128 deltaVel2Dotn = _mm_add_ps ( DOT_PRODUCT ( c . m_contactNormal2 . mVec128 , bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 ) , DOT_PRODUCT ( c . m_relpos2CrossNormal . mVec128 , bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 ) ) ;
deltaImpulse = FMNADD ( deltaVel1Dotn , tmp , deltaImpulse ) ;
deltaImpulse = FMNADD ( deltaVel2Dotn , tmp , deltaImpulse ) ;
tmp = _mm_add_ps ( c . m_appliedImpulse , deltaImpulse ) ;
const __m128 mask = _mm_cmpgt_ps ( tmp , lowerLimit ) ;
deltaImpulse = _mm_blendv_ps ( _mm_sub_ps ( lowerLimit , c . m_appliedImpulse ) , deltaImpulse , mask ) ;
c . m_appliedImpulse = _mm_blendv_ps ( lowerLimit , tmp , mask ) ;
bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 = FMADD ( _mm_mul_ps ( c . m_contactNormal1 . mVec128 , bodyA . internalGetInvMass ( ) . mVec128 ) , deltaImpulse , bodyA . internalGetDeltaLinearVelocity ( ) . mVec128 ) ;
2018-09-07 14:11:04 +00:00
bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 = FMADD ( c . m_angularComponentA . mVec128 , deltaImpulse , bodyA . internalGetDeltaAngularVelocity ( ) . mVec128 ) ;
2019-01-03 13:26:51 +00:00
bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 = FMADD ( _mm_mul_ps ( c . m_contactNormal2 . mVec128 , bodyB . internalGetInvMass ( ) . mVec128 ) , deltaImpulse , bodyB . internalGetDeltaLinearVelocity ( ) . mVec128 ) ;
2018-09-07 14:11:04 +00:00
bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 = FMADD ( c . m_angularComponentB . mVec128 , deltaImpulse , bodyB . internalGetDeltaAngularVelocity ( ) . mVec128 ) ;
btSimdScalar deltaImp = deltaImpulse ;
2019-01-03 13:26:51 +00:00
return deltaImp . m_floats [ 0 ] * ( 1. / c . m_jacDiagABInv ) ;
2017-08-01 12:30:58 +00:00
# else
2019-01-03 13:26:51 +00:00
return gResolveSingleConstraintRowLowerLimit_sse2 ( bodyA , bodyB , c ) ;
# endif //BT_ALLOW_SSE4
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
# endif //USE_SIMD
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btScalar btSequentialImpulseConstraintSolver : : resolveSingleConstraintRowGenericSIMD ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
2018-09-07 14:11:04 +00:00
return m_resolveSingleConstraintRowGeneric ( bodyA , bodyB , c ) ;
2017-08-01 12:30:58 +00:00
}
// Project Gauss Seidel or the equivalent Sequential Impulse
2019-01-03 13:26:51 +00:00
btScalar btSequentialImpulseConstraintSolver : : resolveSingleConstraintRowGeneric ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
2018-09-07 14:11:04 +00:00
return m_resolveSingleConstraintRowGeneric ( bodyA , bodyB , c ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
btScalar btSequentialImpulseConstraintSolver : : resolveSingleConstraintRowLowerLimitSIMD ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
2018-09-07 14:11:04 +00:00
return m_resolveSingleConstraintRowLowerLimit ( bodyA , bodyB , c ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
btScalar btSequentialImpulseConstraintSolver : : resolveSingleConstraintRowLowerLimit ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
2018-09-07 14:11:04 +00:00
return m_resolveSingleConstraintRowLowerLimit ( bodyA , bodyB , c ) ;
2017-08-01 12:30:58 +00:00
}
2018-09-07 14:11:04 +00:00
static btScalar gResolveSplitPenetrationImpulse_scalar_reference (
2019-01-03 13:26:51 +00:00
btSolverBody & bodyA ,
btSolverBody & bodyB ,
const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
btScalar deltaImpulse = 0.f ;
2019-01-03 13:26:51 +00:00
if ( c . m_rhsPenetration )
{
gNumSplitImpulseRecoveries + + ;
deltaImpulse = c . m_rhsPenetration - btScalar ( c . m_appliedPushImpulse ) * c . m_cfm ;
const btScalar deltaVel1Dotn = c . m_contactNormal1 . dot ( bodyA . internalGetPushVelocity ( ) ) + c . m_relpos1CrossNormal . dot ( bodyA . internalGetTurnVelocity ( ) ) ;
const btScalar deltaVel2Dotn = c . m_contactNormal2 . dot ( bodyB . internalGetPushVelocity ( ) ) + c . m_relpos2CrossNormal . dot ( bodyB . internalGetTurnVelocity ( ) ) ;
deltaImpulse - = deltaVel1Dotn * c . m_jacDiagABInv ;
deltaImpulse - = deltaVel2Dotn * c . m_jacDiagABInv ;
const btScalar sum = btScalar ( c . m_appliedPushImpulse ) + deltaImpulse ;
if ( sum < c . m_lowerLimit )
{
deltaImpulse = c . m_lowerLimit - c . m_appliedPushImpulse ;
c . m_appliedPushImpulse = c . m_lowerLimit ;
}
else
{
c . m_appliedPushImpulse = sum ;
}
bodyA . internalApplyPushImpulse ( c . m_contactNormal1 * bodyA . internalGetInvMass ( ) , c . m_angularComponentA , deltaImpulse ) ;
bodyB . internalApplyPushImpulse ( c . m_contactNormal2 * bodyB . internalGetInvMass ( ) , c . m_angularComponentB , deltaImpulse ) ;
}
return deltaImpulse * ( 1. / c . m_jacDiagABInv ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
static btScalar gResolveSplitPenetrationImpulse_sse2 ( btSolverBody & bodyA , btSolverBody & bodyB , const btSolverConstraint & c )
2017-08-01 12:30:58 +00:00
{
# ifdef USE_SIMD
if ( ! c . m_rhsPenetration )
return 0.f ;
gNumSplitImpulseRecoveries + + ;
__m128 cpAppliedImp = _mm_set1_ps ( c . m_appliedPushImpulse ) ;
2019-01-03 13:26:51 +00:00
__m128 lowerLimit1 = _mm_set1_ps ( c . m_lowerLimit ) ;
__m128 upperLimit1 = _mm_set1_ps ( c . m_upperLimit ) ;
__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 ) ) ) ;
__m128 deltaVel1Dotn = _mm_add_ps ( btSimdDot3 ( c . m_contactNormal1 . mVec128 , bodyA . internalGetPushVelocity ( ) . mVec128 ) , btSimdDot3 ( c . m_relpos1CrossNormal . mVec128 , bodyA . internalGetTurnVelocity ( ) . mVec128 ) ) ;
__m128 deltaVel2Dotn = _mm_add_ps ( btSimdDot3 ( c . m_contactNormal2 . mVec128 , bodyB . internalGetPushVelocity ( ) . mVec128 ) , btSimdDot3 ( c . m_relpos2CrossNormal . mVec128 , bodyB . internalGetTurnVelocity ( ) . mVec128 ) ) ;
deltaImpulse = _mm_sub_ps ( deltaImpulse , _mm_mul_ps ( deltaVel1Dotn , _mm_set1_ps ( c . m_jacDiagABInv ) ) ) ;
deltaImpulse = _mm_sub_ps ( deltaImpulse , _mm_mul_ps ( deltaVel2Dotn , _mm_set1_ps ( c . m_jacDiagABInv ) ) ) ;
btSimdScalar sum = _mm_add_ps ( cpAppliedImp , deltaImpulse ) ;
btSimdScalar resultLowerLess , resultUpperLess ;
resultLowerLess = _mm_cmplt_ps ( sum , lowerLimit1 ) ;
resultUpperLess = _mm_cmplt_ps ( sum , upperLimit1 ) ;
__m128 lowMinApplied = _mm_sub_ps ( lowerLimit1 , cpAppliedImp ) ;
deltaImpulse = _mm_or_ps ( _mm_and_ps ( resultLowerLess , lowMinApplied ) , _mm_andnot_ps ( resultLowerLess , deltaImpulse ) ) ;
c . m_appliedPushImpulse = _mm_or_ps ( _mm_and_ps ( resultLowerLess , lowerLimit1 ) , _mm_andnot_ps ( resultLowerLess , sum ) ) ;
__m128 linearComponentA = _mm_mul_ps ( c . m_contactNormal1 . mVec128 , bodyA . internalGetInvMass ( ) . mVec128 ) ;
__m128 linearComponentB = _mm_mul_ps ( c . m_contactNormal2 . mVec128 , bodyB . internalGetInvMass ( ) . mVec128 ) ;
2017-08-01 12:30:58 +00:00
__m128 impulseMagnitude = deltaImpulse ;
2019-01-03 13:26:51 +00:00
bodyA . internalGetPushVelocity ( ) . mVec128 = _mm_add_ps ( bodyA . internalGetPushVelocity ( ) . mVec128 , _mm_mul_ps ( linearComponentA , impulseMagnitude ) ) ;
bodyA . internalGetTurnVelocity ( ) . mVec128 = _mm_add_ps ( bodyA . internalGetTurnVelocity ( ) . mVec128 , _mm_mul_ps ( c . m_angularComponentA . mVec128 , impulseMagnitude ) ) ;
bodyB . internalGetPushVelocity ( ) . mVec128 = _mm_add_ps ( bodyB . internalGetPushVelocity ( ) . mVec128 , _mm_mul_ps ( linearComponentB , impulseMagnitude ) ) ;
bodyB . internalGetTurnVelocity ( ) . mVec128 = _mm_add_ps ( bodyB . internalGetTurnVelocity ( ) . mVec128 , _mm_mul_ps ( c . m_angularComponentB . mVec128 , impulseMagnitude ) ) ;
2018-09-07 14:11:04 +00:00
btSimdScalar deltaImp = deltaImpulse ;
return deltaImp . m_floats [ 0 ] * ( 1. / c . m_jacDiagABInv ) ;
2017-08-01 12:30:58 +00:00
# else
2019-01-03 13:26:51 +00:00
return gResolveSplitPenetrationImpulse_scalar_reference ( bodyA , bodyB , c ) ;
2017-08-01 12:30:58 +00:00
# endif
}
btSequentialImpulseConstraintSolver : : btSequentialImpulseConstraintSolver ( )
{
2019-01-03 13:26:51 +00:00
m_btSeed2 = 0 ;
m_cachedSolverMode = 0 ;
setupSolverFunctions ( false ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
void btSequentialImpulseConstraintSolver : : setupSolverFunctions ( bool useSimd )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference ;
m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference ;
m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( useSimd )
{
2017-08-01 12:30:58 +00:00
# ifdef USE_SIMD
2019-01-03 13:26:51 +00:00
m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2 ;
m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2 ;
m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2 ;
2017-08-01 12:30:58 +00:00
# ifdef BT_ALLOW_SSE4
2019-01-03 13:26:51 +00:00
int cpuFeatures = btCpuFeatureUtility : : getCpuFeatures ( ) ;
if ( ( cpuFeatures & btCpuFeatureUtility : : CPU_FEATURE_FMA3 ) & & ( cpuFeatures & btCpuFeatureUtility : : CPU_FEATURE_SSE4_1 ) )
{
m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3 ;
m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3 ;
}
# endif //BT_ALLOW_SSE4
# endif //USE_SIMD
}
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
btSequentialImpulseConstraintSolver : : ~ btSequentialImpulseConstraintSolver ( )
{
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver : : getScalarConstraintRowSolverGeneric ( )
{
return gResolveSingleConstraintRowGeneric_scalar_reference ;
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver : : getScalarConstraintRowSolverLowerLimit ( )
{
return gResolveSingleConstraintRowLowerLimit_scalar_reference ;
}
2017-08-01 12:30:58 +00:00
# ifdef USE_SIMD
2019-01-03 13:26:51 +00:00
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver : : getSSE2ConstraintRowSolverGeneric ( )
{
return gResolveSingleConstraintRowGeneric_sse2 ;
}
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver : : getSSE2ConstraintRowSolverLowerLimit ( )
{
return gResolveSingleConstraintRowLowerLimit_sse2 ;
}
2017-08-01 12:30:58 +00:00
# ifdef BT_ALLOW_SSE4
2019-01-03 13:26:51 +00:00
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver : : getSSE4_1ConstraintRowSolverGeneric ( )
{
return gResolveSingleConstraintRowGeneric_sse4_1_fma3 ;
}
btSingleConstraintRowSolver btSequentialImpulseConstraintSolver : : getSSE4_1ConstraintRowSolverLowerLimit ( )
{
return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3 ;
}
# endif //BT_ALLOW_SSE4
# endif //USE_SIMD
2017-08-01 12:30:58 +00:00
unsigned long btSequentialImpulseConstraintSolver : : btRand2 ( )
{
2019-01-03 13:26:51 +00:00
m_btSeed2 = ( 1664525L * m_btSeed2 + 1013904223L ) & 0xffffffff ;
2017-08-01 12:30:58 +00:00
return m_btSeed2 ;
}
//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
2019-01-03 13:26:51 +00:00
int btSequentialImpulseConstraintSolver : : btRandInt2 ( int n )
2017-08-01 12:30:58 +00:00
{
// seems good; xor-fold and modulus
const unsigned long un = static_cast < unsigned long > ( n ) ;
unsigned long r = btRand2 ( ) ;
// note: probably more aggressive than it needs to be -- might be
// able to get away without one or two of the innermost branches.
2019-01-03 13:26:51 +00:00
if ( un < = 0x00010000UL )
{
2017-08-01 12:30:58 +00:00
r ^ = ( r > > 16 ) ;
2019-01-03 13:26:51 +00:00
if ( un < = 0x00000100UL )
{
2017-08-01 12:30:58 +00:00
r ^ = ( r > > 8 ) ;
2019-01-03 13:26:51 +00:00
if ( un < = 0x00000010UL )
{
2017-08-01 12:30:58 +00:00
r ^ = ( r > > 4 ) ;
2019-01-03 13:26:51 +00:00
if ( un < = 0x00000004UL )
{
2017-08-01 12:30:58 +00:00
r ^ = ( r > > 2 ) ;
2019-01-03 13:26:51 +00:00
if ( un < = 0x00000002UL )
{
2017-08-01 12:30:58 +00:00
r ^ = ( r > > 1 ) ;
}
}
}
}
}
2019-01-03 13:26:51 +00:00
return ( int ) ( r % un ) ;
2017-08-01 12:30:58 +00:00
}
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : initSolverBody ( btSolverBody * solverBody , btCollisionObject * collisionObject , btScalar timeStep )
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
btRigidBody * rb = collisionObject ? btRigidBody : : upcast ( collisionObject ) : 0 ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
solverBody - > internalGetDeltaLinearVelocity ( ) . setValue ( 0.f , 0.f , 0.f ) ;
solverBody - > internalGetDeltaAngularVelocity ( ) . setValue ( 0.f , 0.f , 0.f ) ;
solverBody - > internalGetPushVelocity ( ) . setValue ( 0.f , 0.f , 0.f ) ;
solverBody - > internalGetTurnVelocity ( ) . setValue ( 0.f , 0.f , 0.f ) ;
if ( rb )
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
solverBody - > m_worldTransform = rb - > getWorldTransform ( ) ;
solverBody - > internalSetInvMass ( btVector3 ( rb - > getInvMass ( ) , rb - > getInvMass ( ) , rb - > getInvMass ( ) ) * rb - > getLinearFactor ( ) ) ;
solverBody - > m_originalBody = rb ;
solverBody - > m_angularFactor = rb - > getAngularFactor ( ) ;
solverBody - > m_linearFactor = rb - > getLinearFactor ( ) ;
solverBody - > m_linearVelocity = rb - > getLinearVelocity ( ) ;
solverBody - > m_angularVelocity = rb - > getAngularVelocity ( ) ;
solverBody - > m_externalForceImpulse = rb - > getTotalForce ( ) * rb - > getInvMass ( ) * timeStep ;
solverBody - > m_externalTorqueImpulse = rb - > getTotalTorque ( ) * rb - > getInvInertiaTensorWorld ( ) * timeStep ;
}
else
2019-06-11 11:18:05 +00:00
{
2020-01-08 17:05:43 +00:00
solverBody - > m_worldTransform . setIdentity ( ) ;
solverBody - > internalSetInvMass ( btVector3 ( 0 , 0 , 0 ) ) ;
solverBody - > m_originalBody = 0 ;
solverBody - > m_angularFactor . setValue ( 1 , 1 , 1 ) ;
solverBody - > m_linearFactor . setValue ( 1 , 1 , 1 ) ;
solverBody - > m_linearVelocity . setValue ( 0 , 0 , 0 ) ;
solverBody - > m_angularVelocity . setValue ( 0 , 0 , 0 ) ;
solverBody - > m_externalForceImpulse . setValue ( 0 , 0 , 0 ) ;
solverBody - > m_externalTorqueImpulse . setValue ( 0 , 0 , 0 ) ;
2019-06-11 11:18:05 +00:00
}
}
2020-01-08 17:05:43 +00:00
btScalar btSequentialImpulseConstraintSolver : : restitutionCurve ( btScalar rel_vel , btScalar restitution , btScalar velocityThreshold )
2017-08-01 12:30:58 +00:00
{
//printf("rel_vel =%f\n", rel_vel);
2019-01-03 13:26:51 +00:00
if ( btFabs ( rel_vel ) < velocityThreshold )
2017-08-01 12:30:58 +00:00
return 0. ;
btScalar rest = restitution * - rel_vel ;
return rest ;
}
2019-01-03 13:26:51 +00:00
void btSequentialImpulseConstraintSolver : : applyAnisotropicFriction ( btCollisionObject * colObj , btVector3 & frictionDirection , int frictionMode )
2017-08-01 12:30:58 +00:00
{
if ( colObj & & colObj - > hasAnisotropicFriction ( frictionMode ) )
{
// transform to local coordinates
btVector3 loc_lateral = frictionDirection * colObj - > getWorldTransform ( ) . getBasis ( ) ;
const btVector3 & friction_scaling = colObj - > getAnisotropicFriction ( ) ;
//apply anisotropic friction
loc_lateral * = friction_scaling ;
// ... and transform it back to global coordinates
frictionDirection = colObj - > getWorldTransform ( ) . getBasis ( ) * loc_lateral ;
}
}
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : setupFrictionConstraint ( btSolverConstraint & solverConstraint , const btVector3 & normalAxis , int solverBodyIdA , int solverBodyIdB , btManifoldPoint & cp , const btVector3 & rel_pos1 , const btVector3 & rel_pos2 , btCollisionObject * colObj0 , btCollisionObject * colObj1 , btScalar relaxation , const btContactSolverInfo & infoGlobal , btScalar desiredVelocity , btScalar cfmSlip )
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
btSolverBody & solverBodyA = m_tmpSolverBodyPool [ solverBodyIdA ] ;
btSolverBody & solverBodyB = m_tmpSolverBodyPool [ solverBodyIdB ] ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
btRigidBody * body0 = m_tmpSolverBodyPool [ solverBodyIdA ] . m_originalBody ;
btRigidBody * bodyA = m_tmpSolverBodyPool [ solverBodyIdB ] . m_originalBody ;
2017-08-01 12:30:58 +00:00
solverConstraint . m_solverBodyIdA = solverBodyIdA ;
solverConstraint . m_solverBodyIdB = solverBodyIdB ;
solverConstraint . m_friction = cp . m_combinedFriction ;
solverConstraint . m_originalContactPoint = 0 ;
solverConstraint . m_appliedImpulse = 0.f ;
solverConstraint . m_appliedPushImpulse = 0.f ;
if ( body0 )
{
solverConstraint . m_contactNormal1 = normalAxis ;
btVector3 ftorqueAxis1 = rel_pos1 . cross ( solverConstraint . m_contactNormal1 ) ;
solverConstraint . m_relpos1CrossNormal = ftorqueAxis1 ;
2019-01-03 13:26:51 +00:00
solverConstraint . m_angularComponentA = body0 - > getInvInertiaTensorWorld ( ) * ftorqueAxis1 * body0 - > getAngularFactor ( ) ;
}
else
2017-08-01 12:30:58 +00:00
{
solverConstraint . m_contactNormal1 . setZero ( ) ;
solverConstraint . m_relpos1CrossNormal . setZero ( ) ;
2019-01-03 13:26:51 +00:00
solverConstraint . m_angularComponentA . setZero ( ) ;
2017-08-01 12:30:58 +00:00
}
2018-09-07 14:11:04 +00:00
if ( bodyA )
2017-08-01 12:30:58 +00:00
{
solverConstraint . m_contactNormal2 = - normalAxis ;
btVector3 ftorqueAxis1 = rel_pos2 . cross ( solverConstraint . m_contactNormal2 ) ;
solverConstraint . m_relpos2CrossNormal = ftorqueAxis1 ;
2019-01-03 13:26:51 +00:00
solverConstraint . m_angularComponentB = bodyA - > getInvInertiaTensorWorld ( ) * ftorqueAxis1 * bodyA - > getAngularFactor ( ) ;
}
else
2017-08-01 12:30:58 +00:00
{
solverConstraint . m_contactNormal2 . setZero ( ) ;
solverConstraint . m_relpos2CrossNormal . setZero ( ) ;
solverConstraint . m_angularComponentB . setZero ( ) ;
}
{
btVector3 vec ;
btScalar denom0 = 0.f ;
btScalar denom1 = 0.f ;
if ( body0 )
{
2019-01-03 13:26:51 +00:00
vec = ( solverConstraint . m_angularComponentA ) . cross ( rel_pos1 ) ;
2017-08-01 12:30:58 +00:00
denom0 = body0 - > getInvMass ( ) + normalAxis . dot ( vec ) ;
}
2018-09-07 14:11:04 +00:00
if ( bodyA )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
vec = ( - solverConstraint . m_angularComponentB ) . cross ( rel_pos2 ) ;
2018-09-07 14:11:04 +00:00
denom1 = bodyA - > getInvMass ( ) + normalAxis . dot ( vec ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
btScalar denom = relaxation / ( denom0 + denom1 ) ;
2017-08-01 12:30:58 +00:00
solverConstraint . m_jacDiagABInv = denom ;
}
{
btScalar rel_vel ;
2019-01-03 13:26:51 +00:00
btScalar vel1Dotn = solverConstraint . m_contactNormal1 . dot ( body0 ? solverBodyA . m_linearVelocity + solverBodyA . m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ) + solverConstraint . m_relpos1CrossNormal . dot ( body0 ? solverBodyA . m_angularVelocity : btVector3 ( 0 , 0 , 0 ) ) ;
btScalar vel2Dotn = solverConstraint . m_contactNormal2 . dot ( bodyA ? solverBodyB . m_linearVelocity + solverBodyB . m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ) + solverConstraint . m_relpos2CrossNormal . dot ( bodyA ? solverBodyB . m_angularVelocity : btVector3 ( 0 , 0 , 0 ) ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
rel_vel = vel1Dotn + vel2Dotn ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
// btScalar positionalError = 0.f;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btScalar velocityError = desiredVelocity - rel_vel ;
2017-08-01 12:30:58 +00:00
btScalar v elocityImpulse = velocityError * solverConstraint . m_jacDiagABInv ;
btScalar penetrationImpulse = btScalar ( 0 ) ;
if ( cp . m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR )
{
btScalar distance = ( cp . getPositionWorldOnA ( ) - cp . getPositionWorldOnB ( ) ) . dot ( normalAxis ) ;
2019-01-03 13:26:51 +00:00
btScalar positionalError = - distance * infoGlobal . m_frictionERP / infoGlobal . m_timeStep ;
penetrationImpulse = positionalError * s olverConstraint . m_jacDiagABInv ;
2017-08-01 12:30:58 +00:00
}
solverConstraint . m_rhs = penetrationImpulse + velocityImpulse ;
solverConstraint . m_rhsPenetration = 0.f ;
solverConstraint . m_cfm = cfmSlip ;
solverConstraint . m_lowerLimit = - solverConstraint . m_friction ;
solverConstraint . m_upperLimit = solverConstraint . m_friction ;
}
}
2019-01-03 13:26:51 +00:00
btSolverConstraint & btSequentialImpulseConstraintSolver : : addFrictionConstraint ( const btVector3 & normalAxis , int solverBodyIdA , int solverBodyIdB , int frictionIndex , btManifoldPoint & cp , const btVector3 & rel_pos1 , const btVector3 & rel_pos2 , btCollisionObject * colObj0 , btCollisionObject * colObj1 , btScalar relaxation , const btContactSolverInfo & infoGlobal , btScalar desiredVelocity , btScalar cfmSlip )
2017-08-01 12:30:58 +00:00
{
btSolverConstraint & solverConstraint = m_tmpSolverContactFrictionConstraintPool . expandNonInitializing ( ) ;
solverConstraint . m_frictionIndex = frictionIndex ;
setupFrictionConstraint ( solverConstraint , normalAxis , solverBodyIdA , solverBodyIdB , cp , rel_pos1 , rel_pos2 ,
2019-06-11 11:18:05 +00:00
colObj0 , colObj1 , relaxation , infoGlobal , desiredVelocity , cfmSlip ) ;
2017-08-01 12:30:58 +00:00
return solverConstraint ;
}
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : setupTorsionalFrictionConstraint ( btSolverConstraint & solverConstraint , const btVector3 & normalAxis1 , int solverBodyIdA , int solverBodyIdB ,
2019-06-11 11:18:05 +00:00
btManifoldPoint & cp , btScalar combinedTorsionalFriction , const btVector3 & rel_pos1 , const btVector3 & rel_pos2 ,
btCollisionObject * colObj0 , btCollisionObject * colObj1 , btScalar relaxation ,
btScalar desiredVelocity , btScalar cfmSlip )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
btVector3 normalAxis ( 0 , 0 , 0 ) ;
2017-08-01 12:30:58 +00:00
solverConstraint . m_contactNormal1 = normalAxis ;
solverConstraint . m_contactNormal2 = - normalAxis ;
2020-01-08 17:05:43 +00:00
btSolverBody & solverBodyA = m_tmpSolverBodyPool [ solverBodyIdA ] ;
btSolverBody & solverBodyB = m_tmpSolverBodyPool [ solverBodyIdB ] ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
btRigidBody * body0 = m_tmpSolverBodyPool [ solverBodyIdA ] . m_originalBody ;
btRigidBody * bodyA = m_tmpSolverBodyPool [ solverBodyIdB ] . m_originalBody ;
2017-08-01 12:30:58 +00:00
solverConstraint . m_solverBodyIdA = solverBodyIdA ;
solverConstraint . m_solverBodyIdB = solverBodyIdB ;
2019-01-03 13:26:51 +00:00
solverConstraint . m_friction = combinedTorsionalFriction ;
solverConstraint . m_originalContactPoint = 0 ;
2017-08-01 12:30:58 +00:00
solverConstraint . m_appliedImpulse = 0.f ;
solverConstraint . m_appliedPushImpulse = 0.f ;
{
btVector3 ftorqueAxis1 = - normalAxis1 ;
solverConstraint . m_relpos1CrossNormal = ftorqueAxis1 ;
2019-01-03 13:26:51 +00:00
solverConstraint . m_angularComponentA = body0 ? body0 - > getInvInertiaTensorWorld ( ) * ftorqueAxis1 * body0 - > getAngularFactor ( ) : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 12:30:58 +00:00
}
{
btVector3 ftorqueAxis1 = normalAxis1 ;
solverConstraint . m_relpos2CrossNormal = ftorqueAxis1 ;
2019-01-03 13:26:51 +00:00
solverConstraint . m_angularComponentB = bodyA ? bodyA - > getInvInertiaTensorWorld ( ) * ftorqueAxis1 * bodyA - > getAngularFactor ( ) : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 12:30:58 +00:00
}
{
2019-01-03 13:26:51 +00:00
btVector3 iMJaA = body0 ? body0 - > getInvInertiaTensorWorld ( ) * solverConstraint . m_relpos1CrossNormal : btVector3 ( 0 , 0 , 0 ) ;
btVector3 iMJaB = bodyA ? bodyA - > getInvInertiaTensorWorld ( ) * solverConstraint . m_relpos2CrossNormal : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 12:30:58 +00:00
btScalar sum = 0 ;
sum + = iMJaA . dot ( solverConstraint . m_relpos1CrossNormal ) ;
sum + = iMJaB . dot ( solverConstraint . m_relpos2CrossNormal ) ;
2019-01-03 13:26:51 +00:00
solverConstraint . m_jacDiagABInv = btScalar ( 1. ) / sum ;
2017-08-01 12:30:58 +00:00
}
{
btScalar rel_vel ;
2019-01-03 13:26:51 +00:00
btScalar vel1Dotn = solverConstraint . m_contactNormal1 . dot ( body0 ? solverBodyA . m_linearVelocity + solverBodyA . m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ) + solverConstraint . m_relpos1CrossNormal . dot ( body0 ? solverBodyA . m_angularVelocity : btVector3 ( 0 , 0 , 0 ) ) ;
btScalar vel2Dotn = solverConstraint . m_contactNormal2 . dot ( bodyA ? solverBodyB . m_linearVelocity + solverBodyB . m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ) + solverConstraint . m_relpos2CrossNormal . dot ( bodyA ? solverBodyB . m_angularVelocity : btVector3 ( 0 , 0 , 0 ) ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
rel_vel = vel1Dotn + vel2Dotn ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
// btScalar positionalError = 0.f;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btSimdScalar velocityError = desiredVelocity - rel_vel ;
btSimdScalar velocityImpulse = velocityError * btSimdScalar ( solverConstraint . m_jacDiagABInv ) ;
2017-08-01 12:30:58 +00:00
solverConstraint . m_rhs = velocityImpulse ;
solverConstraint . m_cfm = cfmSlip ;
solverConstraint . m_lowerLimit = - solverConstraint . m_friction ;
solverConstraint . m_upperLimit = solverConstraint . m_friction ;
}
}
2019-01-03 13:26:51 +00:00
btSolverConstraint & btSequentialImpulseConstraintSolver : : addTorsionalFrictionConstraint ( const btVector3 & normalAxis , int solverBodyIdA , int solverBodyIdB , int frictionIndex , btManifoldPoint & cp , btScalar combinedTorsionalFriction , const btVector3 & rel_pos1 , const btVector3 & rel_pos2 , btCollisionObject * colObj0 , btCollisionObject * colObj1 , btScalar relaxation , btScalar desiredVelocity , btScalar cfmSlip )
2017-08-01 12:30:58 +00:00
{
btSolverConstraint & solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool . expandNonInitializing ( ) ;
solverConstraint . m_frictionIndex = frictionIndex ;
2019-01-03 13:26:51 +00:00
setupTorsionalFrictionConstraint ( solverConstraint , normalAxis , solverBodyIdA , solverBodyIdB , cp , combinedTorsionalFriction , rel_pos1 , rel_pos2 ,
2019-06-11 11:18:05 +00:00
colObj0 , colObj1 , relaxation , desiredVelocity , cfmSlip ) ;
2017-08-01 12:30:58 +00:00
return solverConstraint ;
}
2019-01-03 13:26:51 +00:00
int btSequentialImpulseConstraintSolver : : getOrInitSolverBody ( btCollisionObject & body , btScalar timeStep )
2017-08-01 12:30:58 +00:00
{
# if BT_THREADSAFE
2019-01-03 13:26:51 +00:00
int solverBodyId = - 1 ;
2021-09-29 13:47:08 +00:00
const bool isRigidBodyType = btRigidBody : : upcast ( & body ) ! = NULL ;
const bool isStaticOrKinematic = body . isStaticOrKinematicObject ( ) ;
const bool isKinematic = body . isKinematicObject ( ) ;
if ( isRigidBodyType & & ! isStaticOrKinematic )
2019-01-03 13:26:51 +00:00
{
// dynamic body
// Dynamic bodies can only be in one island, so it's safe to write to the companionId
solverBodyId = body . getCompanionId ( ) ;
if ( solverBodyId < 0 )
{
solverBodyId = m_tmpSolverBodyPool . size ( ) ;
btSolverBody & solverBody = m_tmpSolverBodyPool . expand ( ) ;
initSolverBody ( & solverBody , & body , timeStep ) ;
body . setCompanionId ( solverBodyId ) ;
}
}
2021-09-29 13:47:08 +00:00
else if ( isRigidBodyType & & isKinematic )
2019-01-03 13:26:51 +00:00
{
//
// NOTE: must test for kinematic before static because some kinematic objects also
// identify as "static"
//
// Kinematic bodies can be in multiple islands at once, so it is a
// race condition to write to them, so we use an alternate method
// to record the solverBodyId
int uniqueId = body . getWorldArrayIndex ( ) ;
const int INVALID_SOLVER_BODY_ID = - 1 ;
if ( uniqueId > = m_kinematicBodyUniqueIdToSolverBodyTable . size ( ) )
{
m_kinematicBodyUniqueIdToSolverBodyTable . resize ( uniqueId + 1 , INVALID_SOLVER_BODY_ID ) ;
}
solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable [ uniqueId ] ;
// if no table entry yet,
if ( solverBodyId = = INVALID_SOLVER_BODY_ID )
{
// create a table entry for this body
solverBodyId = m_tmpSolverBodyPool . size ( ) ;
btSolverBody & solverBody = m_tmpSolverBodyPool . expand ( ) ;
initSolverBody ( & solverBody , & body , timeStep ) ;
m_kinematicBodyUniqueIdToSolverBodyTable [ uniqueId ] = solverBodyId ;
}
}
else
{
bool isMultiBodyType = ( body . getInternalType ( ) & btCollisionObject : : CO_FEATHERSTONE_LINK ) ;
// Incorrectly set collision object flags can degrade performance in various ways.
2018-09-07 14:11:04 +00:00
if ( ! isMultiBodyType )
{
2019-01-03 13:26:51 +00:00
btAssert ( body . isStaticOrKinematicObject ( ) ) ;
2018-09-07 14:11:04 +00:00
}
2019-01-03 13:26:51 +00:00
//it could be a multibody link collider
// all fixed bodies (inf mass) get mapped to a single solver id
if ( m_fixedBodyId < 0 )
{
m_fixedBodyId = m_tmpSolverBodyPool . size ( ) ;
btSolverBody & fixedBody = m_tmpSolverBodyPool . expand ( ) ;
initSolverBody ( & fixedBody , 0 , timeStep ) ;
}
solverBodyId = m_fixedBodyId ;
}
btAssert ( solverBodyId > = 0 & & solverBodyId < m_tmpSolverBodyPool . size ( ) ) ;
2017-08-01 12:30:58 +00:00
return solverBodyId ;
2019-01-03 13:26:51 +00:00
# else // BT_THREADSAFE
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
int solverBodyIdA = - 1 ;
2017-08-01 12:30:58 +00:00
if ( body . getCompanionId ( ) > = 0 )
{
//body has already been converted
solverBodyIdA = body . getCompanionId ( ) ;
2019-01-03 13:26:51 +00:00
btAssert ( solverBodyIdA < m_tmpSolverBodyPool . size ( ) ) ;
}
else
2017-08-01 12:30:58 +00:00
{
btRigidBody * rb = btRigidBody : : upcast ( & body ) ;
//convert both active and kinematic objects (for their velocity)
if ( rb & & ( rb - > getInvMass ( ) | | rb - > isKinematicObject ( ) ) )
{
solverBodyIdA = m_tmpSolverBodyPool . size ( ) ;
btSolverBody & solverBody = m_tmpSolverBodyPool . expand ( ) ;
2019-01-03 13:26:51 +00:00
initSolverBody ( & solverBody , & body , timeStep ) ;
2017-08-01 12:30:58 +00:00
body . setCompanionId ( solverBodyIdA ) ;
2019-01-03 13:26:51 +00:00
}
else
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
if ( m_fixedBodyId < 0 )
2017-08-01 12:30:58 +00:00
{
m_fixedBodyId = m_tmpSolverBodyPool . size ( ) ;
btSolverBody & fixedBody = m_tmpSolverBodyPool . expand ( ) ;
2019-01-03 13:26:51 +00:00
initSolverBody ( & fixedBody , 0 , timeStep ) ;
2017-08-01 12:30:58 +00:00
}
return m_fixedBodyId ;
2019-01-03 13:26:51 +00:00
// return 0;//assume first one is a fixed solver body
2017-08-01 12:30:58 +00:00
}
}
return solverBodyIdA ;
2019-01-03 13:26:51 +00:00
# endif // BT_THREADSAFE
2017-08-01 12:30:58 +00:00
}
# include <stdio.h>
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : setupContactConstraint ( btSolverConstraint & solverConstraint ,
2019-06-11 11:18:05 +00:00
int solverBodyIdA , int solverBodyIdB ,
btManifoldPoint & cp , const btContactSolverInfo & infoGlobal ,
btScalar & relaxation ,
const btVector3 & rel_pos1 , const btVector3 & rel_pos2 )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
// const btVector3& pos1 = cp.getPositionWorldOnA();
// const btVector3& pos2 = cp.getPositionWorldOnB();
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
btSolverBody * bodyA = & m_tmpSolverBodyPool [ solverBodyIdA ] ;
btSolverBody * bodyB = & m_tmpSolverBodyPool [ solverBodyIdB ] ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btRigidBody * rb0 = bodyA - > m_originalBody ;
btRigidBody * rb1 = bodyB - > m_originalBody ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
relaxation = infoGlobal . m_sor ;
btScalar invTimeStep = btScalar ( 1 ) / infoGlobal . m_timeStep ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
//cfm = 1 / ( dt * kp + kd )
//erp = dt * kp / ( dt * kp + kd )
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btScalar cfm = infoGlobal . m_globalCfm ;
btScalar erp = infoGlobal . m_erp2 ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( ( cp . m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM ) | | ( cp . m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP ) )
{
if ( cp . m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM )
cfm = cp . m_contactCFM ;
if ( cp . m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP )
erp = cp . m_contactERP ;
}
else
{
if ( cp . m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING )
{
btScalar denom = ( infoGlobal . m_timeStep * cp . m_combinedContactStiffness1 + cp . m_combinedContactDamping1 ) ;
if ( denom < SIMD_EPSILON )
{
denom = SIMD_EPSILON ;
}
cfm = btScalar ( 1 ) / denom ;
erp = ( infoGlobal . m_timeStep * cp . m_combinedContactStiffness1 ) / denom ;
}
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
cfm * = invTimeStep ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btVector3 torqueAxis0 = rel_pos1 . cross ( cp . m_normalWorldOnB ) ;
solverConstraint . m_angularComponentA = rb0 ? rb0 - > getInvInertiaTensorWorld ( ) * torqueAxis0 * rb0 - > getAngularFactor ( ) : btVector3 ( 0 , 0 , 0 ) ;
btVector3 torqueAxis1 = rel_pos2 . cross ( cp . m_normalWorldOnB ) ;
solverConstraint . m_angularComponentB = rb1 ? rb1 - > getInvInertiaTensorWorld ( ) * - torqueAxis1 * rb1 - > getAngularFactor ( ) : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
{
# ifdef COMPUTE_IMPULSE_DENOM
btScalar denom0 = rb0 - > computeImpulseDenominator ( pos1 , cp . m_normalWorldOnB ) ;
btScalar denom1 = rb1 - > computeImpulseDenominator ( pos2 , cp . m_normalWorldOnB ) ;
# else
btVector3 vec ;
btScalar denom0 = 0.f ;
btScalar denom1 = 0.f ;
if ( rb0 )
{
vec = ( solverConstraint . m_angularComponentA ) . cross ( rel_pos1 ) ;
denom0 = rb0 - > getInvMass ( ) + cp . m_normalWorldOnB . dot ( vec ) ;
}
if ( rb1 )
{
vec = ( - solverConstraint . m_angularComponentB ) . cross ( rel_pos2 ) ;
denom1 = rb1 - > getInvMass ( ) + cp . m_normalWorldOnB . dot ( vec ) ;
}
# endif //COMPUTE_IMPULSE_DENOM
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btScalar denom = relaxation / ( denom0 + denom1 + cfm ) ;
solverConstraint . m_jacDiagABInv = denom ;
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( rb0 )
{
solverConstraint . m_contactNormal1 = cp . m_normalWorldOnB ;
solverConstraint . m_relpos1CrossNormal = torqueAxis0 ;
}
else
{
solverConstraint . m_contactNormal1 . setZero ( ) ;
solverConstraint . m_relpos1CrossNormal . setZero ( ) ;
}
if ( rb1 )
{
solverConstraint . m_contactNormal2 = - cp . m_normalWorldOnB ;
solverConstraint . m_relpos2CrossNormal = - torqueAxis1 ;
}
else
{
solverConstraint . m_contactNormal2 . setZero ( ) ;
solverConstraint . m_relpos2CrossNormal . setZero ( ) ;
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btScalar restitution = 0.f ;
btScalar penetration = cp . getDistance ( ) + infoGlobal . m_linearSlop ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
{
btVector3 vel1 , vel2 ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
vel1 = rb0 ? rb0 - > getVelocityInLocalPoint ( rel_pos1 ) : btVector3 ( 0 , 0 , 0 ) ;
vel2 = rb1 ? rb1 - > getVelocityInLocalPoint ( rel_pos2 ) : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
// btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
btVector3 vel = vel1 - vel2 ;
btScalar rel_vel = cp . m_normalWorldOnB . dot ( vel ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
solverConstraint . m_friction = cp . m_combinedFriction ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
restitution = restitutionCurve ( rel_vel , cp . m_combinedRestitution , infoGlobal . m_restitutionVelocityThreshold ) ;
2019-01-03 13:26:51 +00:00
if ( restitution < = btScalar ( 0. ) )
{
restitution = 0.f ;
} ;
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
///warm starting (or zero if disabled)
if ( infoGlobal . m_solverMode & SOLVER_USE_WARMSTARTING )
{
solverConstraint . m_appliedImpulse = cp . m_appliedImpulse * infoGlobal . m_warmstartingFactor ;
if ( rb0 )
bodyA - > internalApplyImpulse ( solverConstraint . m_contactNormal1 * bodyA - > internalGetInvMass ( ) , solverConstraint . m_angularComponentA , solverConstraint . m_appliedImpulse ) ;
if ( rb1 )
2019-06-11 11:18:05 +00:00
bodyB - > internalApplyImpulse ( - solverConstraint . m_contactNormal2 * bodyB - > internalGetInvMass ( ) , - solverConstraint . m_angularComponentB , - ( btScalar ) solverConstraint . m_appliedImpulse ) ;
2019-01-03 13:26:51 +00:00
}
else
{
solverConstraint . m_appliedImpulse = 0.f ;
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
solverConstraint . m_appliedPushImpulse = 0.f ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
{
btVector3 externalForceImpulseA = bodyA - > m_originalBody ? bodyA - > m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ;
btVector3 externalTorqueImpulseA = bodyA - > m_originalBody ? bodyA - > m_externalTorqueImpulse : btVector3 ( 0 , 0 , 0 ) ;
btVector3 externalForceImpulseB = bodyB - > m_originalBody ? bodyB - > m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ;
btVector3 externalTorqueImpulseB = bodyB - > m_originalBody ? bodyB - > m_externalTorqueImpulse : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btScalar vel1Dotn = solverConstraint . m_contactNormal1 . dot ( bodyA - > m_linearVelocity + externalForceImpulseA ) + solverConstraint . m_relpos1CrossNormal . dot ( bodyA - > m_angularVelocity + externalTorqueImpulseA ) ;
btScalar vel2Dotn = solverConstraint . m_contactNormal2 . dot ( bodyB - > m_linearVelocity + externalForceImpulseB ) + solverConstraint . m_relpos2CrossNormal . dot ( bodyB - > m_angularVelocity + externalTorqueImpulseB ) ;
btScalar rel_vel = vel1Dotn + vel2Dotn ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btScalar positionalError = 0.f ;
btScalar velocityError = restitution - rel_vel ; // * damping;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( penetration > 0 )
{
positionalError = 0 ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
velocityError - = penetration * invTimeStep ;
}
else
{
positionalError = - penetration * erp * invTimeStep ;
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
btScalar penetrationImpulse = positionalError * solverConstraint . m_jacDiagABInv ;
btScalar velocityImpulse = velocityError * solverConstraint . m_jacDiagABInv ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( ! infoGlobal . m_splitImpulse | | ( penetration > infoGlobal . m_splitImpulsePenetrationThreshold ) )
{
//combine position and velocity into rhs
solverConstraint . m_rhs = penetrationImpulse + velocityImpulse ; //-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
solverConstraint . m_rhsPenetration = 0.f ;
}
else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint . m_rhs = velocityImpulse ;
solverConstraint . m_rhsPenetration = penetrationImpulse ;
}
solverConstraint . m_cfm = cfm * solverConstraint . m_jacDiagABInv ;
solverConstraint . m_lowerLimit = 0 ;
solverConstraint . m_upperLimit = 1e10 f ;
}
2017-08-01 12:30:58 +00:00
}
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : setFrictionConstraintImpulse ( btSolverConstraint & solverConstraint ,
2019-06-11 11:18:05 +00:00
int solverBodyIdA , int solverBodyIdB ,
btManifoldPoint & cp , const btContactSolverInfo & infoGlobal )
2017-08-01 12:30:58 +00:00
{
{
2020-01-08 17:05:43 +00:00
btSolverConstraint & frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool [ solverConstraint . m_frictionIndex ] ;
frictionConstraint1 . m_appliedImpulse = 0.f ;
2017-08-01 12:30:58 +00:00
}
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
{
2020-01-08 17:05:43 +00:00
btSolverConstraint & frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool [ solverConstraint . m_frictionIndex + 1 ] ;
frictionConstraint2 . m_appliedImpulse = 0.f ;
2017-08-01 12:30:58 +00:00
}
}
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : convertContact ( btPersistentManifold * manifold , const btContactSolverInfo & infoGlobal )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
btCollisionObject * colObj0 = 0 , * colObj1 = 0 ;
2017-08-01 12:30:58 +00:00
colObj0 = ( btCollisionObject * ) manifold - > getBody0 ( ) ;
colObj1 = ( btCollisionObject * ) manifold - > getBody1 ( ) ;
2020-01-08 17:05:43 +00:00
int solverBodyIdA = getOrInitSolverBody ( * colObj0 , infoGlobal . m_timeStep ) ;
int solverBodyIdB = getOrInitSolverBody ( * colObj1 , infoGlobal . m_timeStep ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
// btRigidBody* bodyA = btRigidBody::upcast(colObj0);
// btRigidBody* bodyB = btRigidBody::upcast(colObj1);
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
btSolverBody * solverBodyA = & m_tmpSolverBodyPool [ solverBodyIdA ] ;
btSolverBody * solverBodyB = & m_tmpSolverBodyPool [ solverBodyIdB ] ;
2017-08-01 12:30:58 +00:00
///avoid collision response between two static objects
if ( ! solverBodyA | | ( solverBodyA - > m_invMass . fuzzyZero ( ) & & ( ! solverBodyB | | solverBodyB - > m_invMass . fuzzyZero ( ) ) ) )
return ;
2019-01-03 13:26:51 +00:00
int rollingFriction = 1 ;
for ( int j = 0 ; j < manifold - > getNumContacts ( ) ; j + + )
2017-08-01 12:30:58 +00:00
{
btManifoldPoint & cp = manifold - > getContactPoint ( j ) ;
if ( cp . getDistance ( ) < = manifold - > getContactProcessingThreshold ( ) )
{
btVector3 rel_pos1 ;
btVector3 rel_pos2 ;
btScalar relaxation ;
2020-01-08 17:05:43 +00:00
int frictionIndex = m_tmpSolverContactConstraintPool . size ( ) ;
btSolverConstraint & solverConstraint = m_tmpSolverContactConstraintPool . expandNonInitializing ( ) ;
2017-08-01 12:30:58 +00:00
solverConstraint . m_solverBodyIdA = solverBodyIdA ;
solverConstraint . m_solverBodyIdB = solverBodyIdB ;
solverConstraint . m_originalContactPoint = & cp ;
const btVector3 & pos1 = cp . getPositionWorldOnA ( ) ;
const btVector3 & pos2 = cp . getPositionWorldOnB ( ) ;
rel_pos1 = pos1 - colObj0 - > getWorldTransform ( ) . getOrigin ( ) ;
rel_pos2 = pos2 - colObj1 - > getWorldTransform ( ) . getOrigin ( ) ;
btVector3 vel1 ;
btVector3 vel2 ;
2019-01-03 13:26:51 +00:00
solverBodyA - > getVelocityInLocalPointNoDelta ( rel_pos1 , vel1 ) ;
solverBodyB - > getVelocityInLocalPointNoDelta ( rel_pos2 , vel2 ) ;
btVector3 vel = vel1 - vel2 ;
2017-08-01 12:30:58 +00:00
btScalar rel_vel = cp . m_normalWorldOnB . dot ( vel ) ;
2020-01-08 17:05:43 +00:00
setupContactConstraint ( solverConstraint , solverBodyIdA , solverBodyIdB , cp , infoGlobal , relaxation , rel_pos1 , rel_pos2 ) ;
2017-08-01 12:30:58 +00:00
/////setup the friction constraints
2020-01-08 17:05:43 +00:00
solverConstraint . m_frictionIndex = m_tmpSolverContactFrictionConstraintPool . size ( ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( ( cp . m_combinedRollingFriction > 0.f ) & & ( rollingFriction > 0 ) )
2017-08-01 12:30:58 +00:00
{
{
2020-01-08 17:05:43 +00:00
addTorsionalFrictionConstraint ( cp . m_normalWorldOnB , solverBodyIdA , solverBodyIdB , frictionIndex , cp , cp . m_combinedSpinningFriction , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation ) ;
2019-01-03 13:26:51 +00:00
btVector3 axis0 , axis1 ;
btPlaneSpace1 ( cp . m_normalWorldOnB , axis0 , axis1 ) ;
2017-08-01 12:30:58 +00:00
axis0 . normalize ( ) ;
axis1 . normalize ( ) ;
2019-01-03 13:26:51 +00:00
applyAnisotropicFriction ( colObj0 , axis0 , btCollisionObject : : CF_ANISOTROPIC_ROLLING_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , axis0 , btCollisionObject : : CF_ANISOTROPIC_ROLLING_FRICTION ) ;
applyAnisotropicFriction ( colObj0 , axis1 , btCollisionObject : : CF_ANISOTROPIC_ROLLING_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , axis1 , btCollisionObject : : CF_ANISOTROPIC_ROLLING_FRICTION ) ;
if ( axis0 . length ( ) > 0.001 )
2020-01-08 17:05:43 +00:00
addTorsionalFrictionConstraint ( axis0 , solverBodyIdA , solverBodyIdB , frictionIndex , cp ,
2019-06-11 11:18:05 +00:00
cp . m_combinedRollingFriction , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation ) ;
2019-01-03 13:26:51 +00:00
if ( axis1 . length ( ) > 0.001 )
2020-01-08 17:05:43 +00:00
addTorsionalFrictionConstraint ( axis1 , solverBodyIdA , solverBodyIdB , frictionIndex , cp ,
2019-06-11 11:18:05 +00:00
cp . m_combinedRollingFriction , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation ) ;
}
2017-08-01 12:30:58 +00:00
}
///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically very frame
///based on the relative linear velocity.
///If the relative velocity it zero, it will automatically compute a friction direction.
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
///
///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
///
///The user can manually override the friction directions for certain contacts using a contact callback,
2019-01-03 13:26:51 +00:00
///and use contactPoint.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
2017-08-01 12:30:58 +00:00
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
2019-01-03 13:26:51 +00:00
if ( ! ( infoGlobal . m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING ) | | ! ( cp . m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ) )
2017-08-01 12:30:58 +00:00
{
cp . m_lateralFrictionDir1 = vel - cp . m_normalWorldOnB * rel_vel ;
btScalar lat_rel_vel = cp . m_lateralFrictionDir1 . length2 ( ) ;
if ( ! ( infoGlobal . m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) & & lat_rel_vel > SIMD_EPSILON )
{
2019-01-03 13:26:51 +00:00
cp . m_lateralFrictionDir1 * = 1.f / btSqrt ( lat_rel_vel ) ;
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
2020-01-08 17:05:43 +00:00
addFrictionConstraint ( cp . m_lateralFrictionDir1 , solverBodyIdA , solverBodyIdB , frictionIndex , cp , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
2017-08-01 12:30:58 +00:00
{
cp . m_lateralFrictionDir2 = cp . m_lateralFrictionDir1 . cross ( cp . m_normalWorldOnB ) ;
2019-01-03 13:26:51 +00:00
cp . m_lateralFrictionDir2 . normalize ( ) ; //??
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
2020-01-08 17:05:43 +00:00
addFrictionConstraint ( cp . m_lateralFrictionDir2 , solverBodyIdA , solverBodyIdB , frictionIndex , cp , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
}
else
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
btPlaneSpace1 ( cp . m_normalWorldOnB , cp . m_lateralFrictionDir1 , cp . m_lateralFrictionDir2 ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
2020-01-08 17:05:43 +00:00
addFrictionConstraint ( cp . m_lateralFrictionDir1 , solverBodyIdA , solverBodyIdB , frictionIndex , cp , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
{
2019-01-03 13:26:51 +00:00
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
2020-01-08 17:05:43 +00:00
addFrictionConstraint ( cp . m_lateralFrictionDir2 , solverBodyIdA , solverBodyIdB , frictionIndex , cp , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
}
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) & & ( infoGlobal . m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) )
{
2019-01-03 13:26:51 +00:00
cp . m_contactPointFlags | = BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ;
2017-08-01 12:30:58 +00:00
}
}
2019-01-03 13:26:51 +00:00
}
else
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
addFrictionConstraint ( cp . m_lateralFrictionDir1 , solverBodyIdA , solverBodyIdB , frictionIndex , cp , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation , infoGlobal , cp . m_contactMotion1 , cp . m_frictionCFM ) ;
2017-08-01 12:30:58 +00:00
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
2020-01-08 17:05:43 +00:00
addFrictionConstraint ( cp . m_lateralFrictionDir2 , solverBodyIdA , solverBodyIdB , frictionIndex , cp , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation , infoGlobal , cp . m_contactMotion2 , cp . m_frictionCFM ) ;
2019-06-11 11:18:05 +00:00
}
2020-01-08 17:05:43 +00:00
setFrictionConstraintImpulse ( solverConstraint , solverBodyIdA , solverBodyIdB , cp , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
}
}
}
2019-06-11 11:18:05 +00:00
2019-01-03 13:26:51 +00:00
void btSequentialImpulseConstraintSolver : : convertContacts ( btPersistentManifold * * manifoldPtr , int numManifolds , const btContactSolverInfo & infoGlobal )
2017-08-01 12:30:58 +00:00
{
int i ;
btPersistentManifold * manifold = 0 ;
2019-01-03 13:26:51 +00:00
// btCollisionObject* colObj0=0,*colObj1=0;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
for ( i = 0 ; i < numManifolds ; i + + )
2017-08-01 12:30:58 +00:00
{
manifold = manifoldPtr [ i ] ;
2019-01-03 13:26:51 +00:00
convertContact ( manifold , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
}
}
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : convertJoint ( btSolverConstraint * currentConstraintRow ,
2019-06-11 11:18:05 +00:00
btTypedConstraint * constraint ,
const btTypedConstraint : : btConstraintInfo1 & info1 ,
int solverBodyIdA ,
int solverBodyIdB ,
const btContactSolverInfo & infoGlobal )
2018-09-07 14:11:04 +00:00
{
const btRigidBody & rbA = constraint - > getRigidBodyA ( ) ;
const btRigidBody & rbB = constraint - > getRigidBodyB ( ) ;
2020-01-08 17:05:43 +00:00
const btSolverBody * bodyAPtr = & m_tmpSolverBodyPool [ solverBodyIdA ] ;
const btSolverBody * bodyBPtr = & m_tmpSolverBodyPool [ solverBodyIdB ] ;
2018-09-07 14:11:04 +00:00
int overrideNumSolverIterations = constraint - > getOverrideNumSolverIterations ( ) > 0 ? constraint - > getOverrideNumSolverIterations ( ) : infoGlobal . m_numIterations ;
2020-01-08 17:05:43 +00:00
if ( overrideNumSolverIterations > m_maxOverrideNumSolverIterations )
m_maxOverrideNumSolverIterations = overrideNumSolverIterations ;
2018-09-07 14:11:04 +00:00
2019-01-03 13:26:51 +00:00
for ( int j = 0 ; j < info1 . m_numConstraintRows ; j + + )
2018-09-07 14:11:04 +00:00
{
2019-01-03 13:26:51 +00:00
memset ( & currentConstraintRow [ j ] , 0 , sizeof ( btSolverConstraint ) ) ;
2018-09-07 14:11:04 +00:00
currentConstraintRow [ j ] . m_lowerLimit = - SIMD_INFINITY ;
currentConstraintRow [ j ] . m_upperLimit = SIMD_INFINITY ;
currentConstraintRow [ j ] . m_appliedImpulse = 0.f ;
currentConstraintRow [ j ] . m_appliedPushImpulse = 0.f ;
currentConstraintRow [ j ] . m_solverBodyIdA = solverBodyIdA ;
currentConstraintRow [ j ] . m_solverBodyIdB = solverBodyIdB ;
currentConstraintRow [ j ] . m_overrideNumSolverIterations = overrideNumSolverIterations ;
}
2019-01-03 13:26:51 +00:00
// these vectors are already cleared in initSolverBody, no need to redundantly clear again
btAssert ( bodyAPtr - > getDeltaLinearVelocity ( ) . isZero ( ) ) ;
btAssert ( bodyAPtr - > getDeltaAngularVelocity ( ) . isZero ( ) ) ;
btAssert ( bodyAPtr - > getPushVelocity ( ) . isZero ( ) ) ;
btAssert ( bodyAPtr - > getTurnVelocity ( ) . isZero ( ) ) ;
btAssert ( bodyBPtr - > getDeltaLinearVelocity ( ) . isZero ( ) ) ;
btAssert ( bodyBPtr - > getDeltaAngularVelocity ( ) . isZero ( ) ) ;
btAssert ( bodyBPtr - > getPushVelocity ( ) . isZero ( ) ) ;
btAssert ( bodyBPtr - > getTurnVelocity ( ) . isZero ( ) ) ;
2018-09-07 14:11:04 +00:00
//bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
//bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
//bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
//bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
//bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
//bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
//bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
//bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
btTypedConstraint : : btConstraintInfo2 info2 ;
2019-01-03 13:26:51 +00:00
info2 . fps = 1.f / infoGlobal . m_timeStep ;
2018-09-07 14:11:04 +00:00
info2 . erp = infoGlobal . m_erp ;
info2 . m_J1linearAxis = currentConstraintRow - > m_contactNormal1 ;
info2 . m_J1angularAxis = currentConstraintRow - > m_relpos1CrossNormal ;
info2 . m_J2linearAxis = currentConstraintRow - > m_contactNormal2 ;
info2 . m_J2angularAxis = currentConstraintRow - > m_relpos2CrossNormal ;
2019-01-03 13:26:51 +00:00
info2 . rowskip = sizeof ( btSolverConstraint ) / sizeof ( btScalar ) ; //check this
2019-06-11 11:18:05 +00:00
///the size of btSolverConstraint needs be a multiple of btScalar
2019-01-03 13:26:51 +00:00
btAssert ( info2 . rowskip * sizeof ( btScalar ) = = sizeof ( btSolverConstraint ) ) ;
2018-09-07 14:11:04 +00:00
info2 . m_constraintError = & currentConstraintRow - > m_rhs ;
currentConstraintRow - > m_cfm = infoGlobal . m_globalCfm ;
info2 . m_damping = infoGlobal . m_damping ;
info2 . cfm = & currentConstraintRow - > m_cfm ;
info2 . m_lowerLimit = & currentConstraintRow - > m_lowerLimit ;
info2 . m_upperLimit = & currentConstraintRow - > m_upperLimit ;
info2 . m_numIterations = infoGlobal . m_numIterations ;
constraint - > getInfo2 ( & info2 ) ;
///finalize the constraint setup
2019-01-03 13:26:51 +00:00
for ( int j = 0 ; j < info1 . m_numConstraintRows ; j + + )
2018-09-07 14:11:04 +00:00
{
btSolverConstraint & solverConstraint = currentConstraintRow [ j ] ;
2019-01-03 13:26:51 +00:00
if ( solverConstraint . m_upperLimit > = constraint - > getBreakingImpulseThreshold ( ) )
2018-09-07 14:11:04 +00:00
{
solverConstraint . m_upperLimit = constraint - > getBreakingImpulseThreshold ( ) ;
}
2019-01-03 13:26:51 +00:00
if ( solverConstraint . m_lowerLimit < = - constraint - > getBreakingImpulseThreshold ( ) )
2018-09-07 14:11:04 +00:00
{
solverConstraint . m_lowerLimit = - constraint - > getBreakingImpulseThreshold ( ) ;
}
solverConstraint . m_originalContactPoint = constraint ;
{
const btVector3 & ftorqueAxis1 = solverConstraint . m_relpos1CrossNormal ;
2019-01-03 13:26:51 +00:00
solverConstraint . m_angularComponentA = constraint - > getRigidBodyA ( ) . getInvInertiaTensorWorld ( ) * ftorqueAxis1 * constraint - > getRigidBodyA ( ) . getAngularFactor ( ) ;
2018-09-07 14:11:04 +00:00
}
{
const btVector3 & ftorqueAxis2 = solverConstraint . m_relpos2CrossNormal ;
2019-01-03 13:26:51 +00:00
solverConstraint . m_angularComponentB = constraint - > getRigidBodyB ( ) . getInvInertiaTensorWorld ( ) * ftorqueAxis2 * constraint - > getRigidBodyB ( ) . getAngularFactor ( ) ;
2018-09-07 14:11:04 +00:00
}
{
2019-01-03 13:26:51 +00:00
btVector3 iMJlA = solverConstraint . m_contactNormal1 * rbA . getInvMass ( ) ;
btVector3 iMJaA = rbA . getInvInertiaTensorWorld ( ) * solverConstraint . m_relpos1CrossNormal ;
btVector3 iMJlB = solverConstraint . m_contactNormal2 * rbB . getInvMass ( ) ; //sign of normal?
btVector3 iMJaB = rbB . getInvInertiaTensorWorld ( ) * solverConstraint . m_relpos2CrossNormal ;
2018-09-07 14:11:04 +00:00
btScalar sum = iMJlA . dot ( solverConstraint . m_contactNormal1 ) ;
sum + = iMJaA . dot ( solverConstraint . m_relpos1CrossNormal ) ;
sum + = iMJlB . dot ( solverConstraint . m_contactNormal2 ) ;
sum + = iMJaB . dot ( solverConstraint . m_relpos2CrossNormal ) ;
btScalar fsum = btFabs ( sum ) ;
btAssert ( fsum > SIMD_EPSILON ) ;
2019-01-03 13:26:51 +00:00
btScalar sorRelaxation = 1.f ; //todo: get from globalInfo?
solverConstraint . m_jacDiagABInv = fsum > SIMD_EPSILON ? sorRelaxation / sum : 0.f ;
2018-09-07 14:11:04 +00:00
}
{
btScalar rel_vel ;
2019-01-03 13:26:51 +00:00
btVector3 externalForceImpulseA = bodyAPtr - > m_originalBody ? bodyAPtr - > m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ;
btVector3 externalTorqueImpulseA = bodyAPtr - > m_originalBody ? bodyAPtr - > m_externalTorqueImpulse : btVector3 ( 0 , 0 , 0 ) ;
2018-09-07 14:11:04 +00:00
2019-01-03 13:26:51 +00:00
btVector3 externalForceImpulseB = bodyBPtr - > m_originalBody ? bodyBPtr - > m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ;
btVector3 externalTorqueImpulseB = bodyBPtr - > m_originalBody ? bodyBPtr - > m_externalTorqueImpulse : btVector3 ( 0 , 0 , 0 ) ;
2018-09-07 14:11:04 +00:00
2019-01-03 13:26:51 +00:00
btScalar vel1Dotn = solverConstraint . m_contactNormal1 . dot ( rbA . getLinearVelocity ( ) + externalForceImpulseA ) + solverConstraint . m_relpos1CrossNormal . dot ( rbA . getAngularVelocity ( ) + externalTorqueImpulseA ) ;
2018-09-07 14:11:04 +00:00
2019-01-03 13:26:51 +00:00
btScalar vel2Dotn = solverConstraint . m_contactNormal2 . dot ( rbB . getLinearVelocity ( ) + externalForceImpulseB ) + solverConstraint . m_relpos2CrossNormal . dot ( rbB . getAngularVelocity ( ) + externalTorqueImpulseB ) ;
2018-09-07 14:11:04 +00:00
2019-01-03 13:26:51 +00:00
rel_vel = vel1Dotn + vel2Dotn ;
2018-09-07 14:11:04 +00:00
btScalar restitution = 0.f ;
2019-01-03 13:26:51 +00:00
btScalar positionalError = solverConstraint . m_rhs ; //already filled in by getConstraintInfo2
btScalar velocityError = restitution - rel_vel * info2 . m_damping ;
btScalar penetrationImpulse = positionalError * solverConstraint . m_jacDiagABInv ;
btScalar velocityImpulse = velocityError * solverConstraint . m_jacDiagABInv ;
solverConstraint . m_rhs = penetrationImpulse + velocityImpulse ;
2018-09-07 14:11:04 +00:00
solverConstraint . m_appliedImpulse = 0.f ;
}
}
}
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : convertJoints ( btTypedConstraint * * constraints , int numConstraints , const btContactSolverInfo & infoGlobal )
2018-09-07 14:11:04 +00:00
{
2019-01-03 13:26:51 +00:00
BT_PROFILE ( " convertJoints " ) ;
for ( int j = 0 ; j < numConstraints ; j + + )
2018-09-07 14:11:04 +00:00
{
btTypedConstraint * constraint = constraints [ j ] ;
constraint - > buildJacobian ( ) ;
constraint - > internalSetAppliedImpulse ( 0.0f ) ;
}
int totalNumRows = 0 ;
2020-01-08 17:05:43 +00:00
m_tmpConstraintSizesPool . resizeNoInitialize ( numConstraints ) ;
2018-09-07 14:11:04 +00:00
//calculate the total number of contraint rows
2019-01-03 13:26:51 +00:00
for ( int i = 0 ; i < numConstraints ; i + + )
2018-09-07 14:11:04 +00:00
{
2020-01-08 17:05:43 +00:00
btTypedConstraint : : btConstraintInfo1 & info1 = m_tmpConstraintSizesPool [ i ] ;
2018-09-07 14:11:04 +00:00
btJointFeedback * fb = constraints [ i ] - > getJointFeedback ( ) ;
if ( fb )
{
fb - > m_appliedForceBodyA . setZero ( ) ;
fb - > m_appliedTorqueBodyA . setZero ( ) ;
fb - > m_appliedForceBodyB . setZero ( ) ;
fb - > m_appliedTorqueBodyB . setZero ( ) ;
}
if ( constraints [ i ] - > isEnabled ( ) )
{
constraints [ i ] - > getInfo1 ( & info1 ) ;
2019-01-03 13:26:51 +00:00
}
else
2018-09-07 14:11:04 +00:00
{
info1 . m_numConstraintRows = 0 ;
info1 . nub = 0 ;
}
totalNumRows + = info1 . m_numConstraintRows ;
}
2020-01-08 17:05:43 +00:00
m_tmpSolverNonContactConstraintPool . resizeNoInitialize ( totalNumRows ) ;
2018-09-07 14:11:04 +00:00
///setup the btSolverConstraints
int currentRow = 0 ;
2019-01-03 13:26:51 +00:00
for ( int i = 0 ; i < numConstraints ; i + + )
2018-09-07 14:11:04 +00:00
{
2020-01-08 17:05:43 +00:00
const btTypedConstraint : : btConstraintInfo1 & info1 = m_tmpConstraintSizesPool [ i ] ;
2018-09-07 14:11:04 +00:00
if ( info1 . m_numConstraintRows )
{
2019-01-03 13:26:51 +00:00
btAssert ( currentRow < totalNumRows ) ;
2018-09-07 14:11:04 +00:00
2020-01-08 17:05:43 +00:00
btSolverConstraint * currentConstraintRow = & m_tmpSolverNonContactConstraintPool [ currentRow ] ;
2018-09-07 14:11:04 +00:00
btTypedConstraint * constraint = constraints [ i ] ;
btRigidBody & rbA = constraint - > getRigidBodyA ( ) ;
btRigidBody & rbB = constraint - > getRigidBodyB ( ) ;
2020-01-08 17:05:43 +00:00
int solverBodyIdA = getOrInitSolverBody ( rbA , infoGlobal . m_timeStep ) ;
int solverBodyIdB = getOrInitSolverBody ( rbB , infoGlobal . m_timeStep ) ;
2018-09-07 14:11:04 +00:00
2020-01-08 17:05:43 +00:00
convertJoint ( currentConstraintRow , constraint , info1 , solverBodyIdA , solverBodyIdB , infoGlobal ) ;
2019-01-03 13:26:51 +00:00
}
currentRow + = info1 . m_numConstraintRows ;
2018-09-07 14:11:04 +00:00
}
}
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : convertBodies ( btCollisionObject * * bodies , int numBodies , const btContactSolverInfo & infoGlobal )
2018-09-07 14:11:04 +00:00
{
2019-01-03 13:26:51 +00:00
BT_PROFILE ( " convertBodies " ) ;
2018-09-07 14:11:04 +00:00
for ( int i = 0 ; i < numBodies ; i + + )
{
bodies [ i ] - > setCompanionId ( - 1 ) ;
}
# if BT_THREADSAFE
2020-01-08 17:05:43 +00:00
m_kinematicBodyUniqueIdToSolverBodyTable . resize ( 0 ) ;
2019-01-03 13:26:51 +00:00
# endif // BT_THREADSAFE
2018-09-07 14:11:04 +00:00
2020-01-08 17:05:43 +00:00
m_tmpSolverBodyPool . reserve ( numBodies + 1 ) ;
m_tmpSolverBodyPool . resize ( 0 ) ;
2018-09-07 14:11:04 +00:00
//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
2019-01-03 13:26:51 +00:00
//initSolverBody(&fixedBody,0);
2018-09-07 14:11:04 +00:00
2019-01-03 13:26:51 +00:00
for ( int i = 0 ; i < numBodies ; i + + )
2018-09-07 14:11:04 +00:00
{
2020-01-08 17:05:43 +00:00
int bodyId = getOrInitSolverBody ( * bodies [ i ] , infoGlobal . m_timeStep ) ;
2018-09-07 14:11:04 +00:00
btRigidBody * body = btRigidBody : : upcast ( bodies [ i ] ) ;
if ( body & & body - > getInvMass ( ) )
{
2020-01-08 17:05:43 +00:00
btSolverBody & solverBody = m_tmpSolverBodyPool [ bodyId ] ;
2019-01-03 13:26:51 +00:00
btVector3 gyroForce ( 0 , 0 , 0 ) ;
if ( body - > getFlags ( ) & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT )
2018-09-07 14:11:04 +00:00
{
gyroForce = body - > computeGyroscopicForceExplicit ( infoGlobal . m_maxGyroscopicForce ) ;
2019-01-03 13:26:51 +00:00
solverBody . m_externalTorqueImpulse - = gyroForce * body - > getInvInertiaTensorWorld ( ) * infoGlobal . m_timeStep ;
2018-09-07 14:11:04 +00:00
}
2019-01-03 13:26:51 +00:00
if ( body - > getFlags ( ) & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD )
2018-09-07 14:11:04 +00:00
{
gyroForce = body - > computeGyroscopicImpulseImplicit_World ( infoGlobal . m_timeStep ) ;
solverBody . m_externalTorqueImpulse + = gyroForce ;
}
2019-01-03 13:26:51 +00:00
if ( body - > getFlags ( ) & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY )
2018-09-07 14:11:04 +00:00
{
gyroForce = body - > computeGyroscopicImpulseImplicit_Body ( infoGlobal . m_timeStep ) ;
solverBody . m_externalTorqueImpulse + = gyroForce ;
}
}
}
}
2019-01-03 13:26:51 +00:00
btScalar btSequentialImpulseConstraintSolver : : solveGroupCacheFriendlySetup ( btCollisionObject * * bodies , int numBodies , btPersistentManifold * * manifoldPtr , int numManifolds , btTypedConstraint * * constraints , int numConstraints , const btContactSolverInfo & infoGlobal , btIDebugDraw * debugDrawer )
2017-08-01 12:30:58 +00:00
{
m_fixedBodyId = - 1 ;
BT_PROFILE ( " solveGroupCacheFriendlySetup " ) ;
( void ) debugDrawer ;
2019-01-03 13:26:51 +00:00
// if solver mode has changed,
if ( infoGlobal . m_solverMode ! = m_cachedSolverMode )
{
// update solver functions to use SIMD or non-SIMD
bool useSimd = ! ! ( infoGlobal . m_solverMode & SOLVER_SIMD ) ;
setupSolverFunctions ( useSimd ) ;
m_cachedSolverMode = infoGlobal . m_solverMode ;
}
2017-08-01 12:30:58 +00:00
m_maxOverrideNumSolverIterations = 0 ;
# ifdef BT_ADDITIONAL_DEBUG
2019-01-03 13:26:51 +00:00
//make sure that dynamic bodies exist for all (enabled) constraints
for ( int i = 0 ; i < numConstraints ; i + + )
2017-08-01 12:30:58 +00:00
{
btTypedConstraint * constraint = constraints [ i ] ;
if ( constraint - > isEnabled ( ) )
{
if ( ! constraint - > getRigidBodyA ( ) . isStaticOrKinematicObject ( ) )
{
2019-01-03 13:26:51 +00:00
bool found = false ;
for ( int b = 0 ; b < numBodies ; b + + )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
if ( & constraint - > getRigidBodyA ( ) = = bodies [ b ] )
2017-08-01 12:30:58 +00:00
{
found = true ;
break ;
}
}
btAssert ( found ) ;
}
if ( ! constraint - > getRigidBodyB ( ) . isStaticOrKinematicObject ( ) )
{
2019-01-03 13:26:51 +00:00
bool found = false ;
for ( int b = 0 ; b < numBodies ; b + + )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
if ( & constraint - > getRigidBodyB ( ) = = bodies [ b ] )
2017-08-01 12:30:58 +00:00
{
found = true ;
break ;
}
}
btAssert ( found ) ;
}
}
}
2019-01-03 13:26:51 +00:00
//make sure that dynamic bodies exist for all contact manifolds
for ( int i = 0 ; i < numManifolds ; i + + )
{
if ( ! manifoldPtr [ i ] - > getBody0 ( ) - > isStaticOrKinematicObject ( ) )
{
bool found = false ;
for ( int b = 0 ; b < numBodies ; b + + )
{
if ( manifoldPtr [ i ] - > getBody0 ( ) = = bodies [ b ] )
{
found = true ;
break ;
}
}
btAssert ( found ) ;
}
if ( ! manifoldPtr [ i ] - > getBody1 ( ) - > isStaticOrKinematicObject ( ) )
{
bool found = false ;
for ( int b = 0 ; b < numBodies ; b + + )
{
if ( manifoldPtr [ i ] - > getBody1 ( ) = = bodies [ b ] )
{
found = true ;
break ;
}
}
btAssert ( found ) ;
}
}
# endif //BT_ADDITIONAL_DEBUG
2017-08-01 12:30:58 +00:00
//convert all bodies
2019-01-03 13:26:51 +00:00
convertBodies ( bodies , numBodies , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
convertJoints ( constraints , numConstraints , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
convertContacts ( manifoldPtr , numManifolds , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
// btContactSolverInfo info = infoGlobal;
2017-08-01 12:30:58 +00:00
int numNonContactPool = m_tmpSolverNonContactConstraintPool . size ( ) ;
int numConstraintPool = m_tmpSolverContactConstraintPool . size ( ) ;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool . size ( ) ;
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
m_orderNonContactConstraintPool . resizeNoInitialize ( numNonContactPool ) ;
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
2019-01-03 13:26:51 +00:00
m_orderTmpConstraintPool . resizeNoInitialize ( numConstraintPool * 2 ) ;
2017-08-01 12:30:58 +00:00
else
m_orderTmpConstraintPool . resizeNoInitialize ( numConstraintPool ) ;
m_orderFrictionConstraintPool . resizeNoInitialize ( numFrictionPool ) ;
{
int i ;
2019-01-03 13:26:51 +00:00
for ( i = 0 ; i < numNonContactPool ; i + + )
2017-08-01 12:30:58 +00:00
{
m_orderNonContactConstraintPool [ i ] = i ;
}
2019-01-03 13:26:51 +00:00
for ( i = 0 ; i < numConstraintPool ; i + + )
2017-08-01 12:30:58 +00:00
{
m_orderTmpConstraintPool [ i ] = i ;
}
2019-01-03 13:26:51 +00:00
for ( i = 0 ; i < numFrictionPool ; i + + )
2017-08-01 12:30:58 +00:00
{
m_orderFrictionConstraintPool [ i ] = i ;
}
}
return 0.f ;
}
2020-01-08 17:05:43 +00:00
btScalar btSequentialImpulseConstraintSolver : : solveSingleIteration ( int iteration , btCollisionObject * * /*bodies */ , int /*numBodies*/ , btPersistentManifold * * /*manifoldPtr*/ , int /*numManifolds*/ , btTypedConstraint * * constraints , int numConstraints , const btContactSolverInfo & infoGlobal , btIDebugDraw * /*debugDrawer*/ )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
BT_PROFILE ( " solveSingleIteration " ) ;
2017-08-01 12:30:58 +00:00
btScalar leastSquaresResidual = 0.f ;
2020-01-08 17:05:43 +00:00
int numNonContactPool = m_tmpSolverNonContactConstraintPool . size ( ) ;
int numConstraintPool = m_tmpSolverContactConstraintPool . size ( ) ;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool . size ( ) ;
2017-08-01 12:30:58 +00:00
if ( infoGlobal . m_solverMode & SOLVER_RANDMIZE_ORDER )
{
2019-01-03 13:26:51 +00:00
if ( 1 ) // uncomment this for a bit less random ((iteration & 7) == 0)
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
for ( int j = 0 ; j < numNonContactPool ; + + j )
{
2020-01-08 17:05:43 +00:00
int tmp = m_orderNonContactConstraintPool [ j ] ;
int swapi = btRandInt2 ( j + 1 ) ;
m_orderNonContactConstraintPool [ j ] = m_orderNonContactConstraintPool [ swapi ] ;
m_orderNonContactConstraintPool [ swapi ] = tmp ;
2017-08-01 12:30:58 +00:00
}
//contact/friction constraints are not solved more than
2019-01-03 13:26:51 +00:00
if ( iteration < infoGlobal . m_numIterations )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
for ( int j = 0 ; j < numConstraintPool ; + + j )
{
2020-01-08 17:05:43 +00:00
int tmp = m_orderTmpConstraintPool [ j ] ;
int swapi = btRandInt2 ( j + 1 ) ;
m_orderTmpConstraintPool [ j ] = m_orderTmpConstraintPool [ swapi ] ;
m_orderTmpConstraintPool [ swapi ] = tmp ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
for ( int j = 0 ; j < numFrictionPool ; + + j )
{
2020-01-08 17:05:43 +00:00
int tmp = m_orderFrictionConstraintPool [ j ] ;
int swapi = btRandInt2 ( j + 1 ) ;
m_orderFrictionConstraintPool [ j ] = m_orderFrictionConstraintPool [ swapi ] ;
m_orderFrictionConstraintPool [ swapi ] = tmp ;
2017-08-01 12:30:58 +00:00
}
}
}
}
2019-01-03 13:26:51 +00:00
///solve all joint constraints
2020-01-08 17:05:43 +00:00
for ( int j = 0 ; j < m_tmpSolverNonContactConstraintPool . size ( ) ; j + + )
2019-01-03 13:26:51 +00:00
{
2020-01-08 17:05:43 +00:00
btSolverConstraint & constraint = m_tmpSolverNonContactConstraintPool [ m_orderNonContactConstraintPool [ j ] ] ;
2019-01-03 13:26:51 +00:00
if ( iteration < constraint . m_overrideNumSolverIterations )
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
btScalar residual = resolveSingleConstraintRowGeneric ( m_tmpSolverBodyPool [ constraint . m_solverBodyIdA ] , m_tmpSolverBodyPool [ constraint . m_solverBodyIdB ] , constraint ) ;
2019-01-03 13:26:51 +00:00
leastSquaresResidual = btMax ( leastSquaresResidual , residual * residual ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( iteration < infoGlobal . m_numIterations )
{
for ( int j = 0 ; j < numConstraints ; j + + )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
if ( constraints [ j ] - > isEnabled ( ) )
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
int bodyAid = getOrInitSolverBody ( constraints [ j ] - > getRigidBodyA ( ) , infoGlobal . m_timeStep ) ;
int bodyBid = getOrInitSolverBody ( constraints [ j ] - > getRigidBodyB ( ) , infoGlobal . m_timeStep ) ;
btSolverBody & bodyA = m_tmpSolverBodyPool [ bodyAid ] ;
btSolverBody & bodyB = m_tmpSolverBodyPool [ bodyBid ] ;
2019-01-03 13:26:51 +00:00
constraints [ j ] - > solveConstraintObsolete ( bodyA , bodyB , infoGlobal . m_timeStep ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
}
///solve all contact constraints
if ( infoGlobal . m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS )
{
2020-01-08 17:05:43 +00:00
int numPoolConstraints = m_tmpSolverContactConstraintPool . size ( ) ;
2019-01-03 13:26:51 +00:00
int multiplier = ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) ? 2 : 1 ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
for ( int c = 0 ; c < numPoolConstraints ; c + + )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
btScalar totalImpulse = 0 ;
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
const btSolverConstraint & solveManifold = m_tmpSolverContactConstraintPool [ m_orderTmpConstraintPool [ c ] ] ;
btScalar residual = resolveSingleConstraintRowLowerLimit ( m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdA ] , m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdB ] , solveManifold ) ;
2019-01-03 13:26:51 +00:00
leastSquaresResidual = btMax ( leastSquaresResidual , residual * residual ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
totalImpulse = solveManifold . m_appliedImpulse ;
}
bool applyFriction = true ;
if ( applyFriction )
{
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
btSolverConstraint & solveManifold = m_tmpSolverContactFrictionConstraintPool [ m_orderFrictionConstraintPool [ c * multiplier ] ] ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( totalImpulse > btScalar ( 0 ) )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
solveManifold . m_lowerLimit = - ( solveManifold . m_friction * totalImpulse ) ;
solveManifold . m_upperLimit = solveManifold . m_friction * totalImpulse ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
btScalar residual = resolveSingleConstraintRowGeneric ( m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdA ] , m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdB ] , solveManifold ) ;
2019-01-03 13:26:51 +00:00
leastSquaresResidual = btMax ( leastSquaresResidual , residual * residual ) ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS )
{
2020-01-08 17:05:43 +00:00
btSolverConstraint & solveManifold = m_tmpSolverContactFrictionConstraintPool [ m_orderFrictionConstraintPool [ c * multiplier + 1 ] ] ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( totalImpulse > btScalar ( 0 ) )
{
solveManifold . m_lowerLimit = - ( solveManifold . m_friction * totalImpulse ) ;
solveManifold . m_upperLimit = solveManifold . m_friction * totalImpulse ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
btScalar residual = resolveSingleConstraintRowGeneric ( m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdA ] , m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdB ] , solveManifold ) ;
2019-01-03 13:26:51 +00:00
leastSquaresResidual = btMax ( leastSquaresResidual , residual * residual ) ;
2017-08-01 12:30:58 +00:00
}
}
}
}
2019-01-03 13:26:51 +00:00
}
else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
2020-01-08 17:05:43 +00:00
int numPoolConstraints = m_tmpSolverContactConstraintPool . size ( ) ;
2019-01-03 13:26:51 +00:00
int j ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
for ( j = 0 ; j < numPoolConstraints ; j + + )
{
2020-01-08 17:05:43 +00:00
const btSolverConstraint & solveManifold = m_tmpSolverContactConstraintPool [ m_orderTmpConstraintPool [ j ] ] ;
btScalar residual = resolveSingleConstraintRowLowerLimit ( m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdA ] , m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdB ] , solveManifold ) ;
2019-01-03 13:26:51 +00:00
leastSquaresResidual = btMax ( leastSquaresResidual , residual * residual ) ;
}
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
///solve all friction constraints
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool . size ( ) ;
2019-01-03 13:26:51 +00:00
for ( j = 0 ; j < numFrictionPoolConstraints ; j + + )
{
2020-01-08 17:05:43 +00:00
btSolverConstraint & solveManifold = m_tmpSolverContactFrictionConstraintPool [ m_orderFrictionConstraintPool [ j ] ] ;
btScalar totalImpulse = m_tmpSolverContactConstraintPool [ solveManifold . m_frictionIndex ] . m_appliedImpulse ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( totalImpulse > btScalar ( 0 ) )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
solveManifold . m_lowerLimit = - ( solveManifold . m_friction * totalImpulse ) ;
solveManifold . m_upperLimit = solveManifold . m_friction * totalImpulse ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
btScalar residual = resolveSingleConstraintRowGeneric ( m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdA ] , m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdB ] , solveManifold ) ;
2019-01-03 13:26:51 +00:00
leastSquaresResidual = btMax ( leastSquaresResidual , residual * residual ) ;
2017-08-01 12:30:58 +00:00
}
}
2019-01-03 13:26:51 +00:00
}
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool . size ( ) ;
2019-01-03 13:26:51 +00:00
for ( int j = 0 ; j < numRollingFrictionPoolConstraints ; j + + )
{
2020-01-08 17:05:43 +00:00
btSolverConstraint & rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool [ j ] ;
btScalar totalImpulse = m_tmpSolverContactConstraintPool [ rollingFrictionConstraint . m_frictionIndex ] . m_appliedImpulse ;
2019-01-03 13:26:51 +00:00
if ( totalImpulse > btScalar ( 0 ) )
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint . m_friction * totalImpulse ;
if ( rollingFrictionMagnitude > rollingFrictionConstraint . m_friction )
rollingFrictionMagnitude = rollingFrictionConstraint . m_friction ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
rollingFrictionConstraint . m_lowerLimit = - rollingFrictionMagnitude ;
rollingFrictionConstraint . m_upperLimit = rollingFrictionMagnitude ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
btScalar residual = resolveSingleConstraintRowGeneric ( m_tmpSolverBodyPool [ rollingFrictionConstraint . m_solverBodyIdA ] , m_tmpSolverBodyPool [ rollingFrictionConstraint . m_solverBodyIdB ] , rollingFrictionConstraint ) ;
2019-01-03 13:26:51 +00:00
leastSquaresResidual = btMax ( leastSquaresResidual , residual * residual ) ;
}
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
}
2017-08-01 12:30:58 +00:00
return leastSquaresResidual ;
}
2019-01-03 13:26:51 +00:00
void btSequentialImpulseConstraintSolver : : solveGroupCacheFriendlySplitImpulseIterations ( btCollisionObject * * bodies , int numBodies , btPersistentManifold * * manifoldPtr , int numManifolds , btTypedConstraint * * constraints , int numConstraints , const btContactSolverInfo & infoGlobal , btIDebugDraw * debugDrawer )
2017-08-01 12:30:58 +00:00
{
2018-09-07 14:11:04 +00:00
BT_PROFILE ( " solveGroupCacheFriendlySplitImpulseIterations " ) ;
2017-08-01 12:30:58 +00:00
int iteration ;
if ( infoGlobal . m_splitImpulse )
{
{
2019-01-03 13:26:51 +00:00
for ( iteration = 0 ; iteration < infoGlobal . m_numIterations ; iteration + + )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
btScalar leastSquaresResidual = 0.f ;
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
int numPoolConstraints = m_tmpSolverContactConstraintPool . size ( ) ;
2017-08-01 12:30:58 +00:00
int j ;
2019-01-03 13:26:51 +00:00
for ( j = 0 ; j < numPoolConstraints ; j + + )
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
const btSolverConstraint & solveManifold = m_tmpSolverContactConstraintPool [ m_orderTmpConstraintPool [ j ] ] ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
btScalar residual = resolveSplitPenetrationImpulse ( m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdA ] , m_tmpSolverBodyPool [ solveManifold . m_solverBodyIdB ] , solveManifold ) ;
2019-01-03 13:26:51 +00:00
leastSquaresResidual = btMax ( leastSquaresResidual , residual * residual ) ;
2017-08-01 12:30:58 +00:00
}
}
2019-01-03 13:26:51 +00:00
if ( leastSquaresResidual < = infoGlobal . m_leastSquaresResidualThreshold | | iteration > = ( infoGlobal . m_numIterations - 1 ) )
2017-08-01 12:30:58 +00:00
{
# ifdef VERBOSE_RESIDUAL_PRINTF
2019-01-03 13:26:51 +00:00
printf ( " residual = %f at iteration #%d \n " , leastSquaresResidual , iteration ) ;
2017-08-01 12:30:58 +00:00
# endif
break ;
}
}
2019-01-03 13:26:51 +00:00
}
2017-08-01 12:30:58 +00:00
}
}
2019-01-03 13:26:51 +00:00
btScalar btSequentialImpulseConstraintSolver : : solveGroupCacheFriendlyIterations ( btCollisionObject * * bodies , int numBodies , btPersistentManifold * * manifoldPtr , int numManifolds , btTypedConstraint * * constraints , int numConstraints , const btContactSolverInfo & infoGlobal , btIDebugDraw * debugDrawer )
2017-08-01 12:30:58 +00:00
{
BT_PROFILE ( " solveGroupCacheFriendlyIterations " ) ;
{
///this is a special step to resolve penetrations (just for contacts)
2019-01-03 13:26:51 +00:00
solveGroupCacheFriendlySplitImpulseIterations ( bodies , numBodies , manifoldPtr , numManifolds , constraints , numConstraints , infoGlobal , debugDrawer ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal . m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal . m_numIterations ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
for ( int iteration = 0 ; iteration < maxIterations ; iteration + + )
2019-06-11 11:18:05 +00:00
//for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
m_leastSquaresResidual = solveSingleIteration ( iteration , bodies , numBodies , manifoldPtr , numManifolds , constraints , numConstraints , infoGlobal , debugDrawer ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
if ( m_leastSquaresResidual < = infoGlobal . m_leastSquaresResidualThreshold | | ( iteration > = ( maxIterations - 1 ) ) )
2017-08-01 12:30:58 +00:00
{
# ifdef VERBOSE_RESIDUAL_PRINTF
2019-01-03 13:26:51 +00:00
printf ( " residual = %f at iteration #%d \n " , m_leastSquaresResidual , iteration ) ;
2017-08-01 12:30:58 +00:00
# endif
2019-06-11 11:18:05 +00:00
m_analyticsData . m_numSolverCalls + + ;
m_analyticsData . m_numIterationsUsed = iteration + 1 ;
m_analyticsData . m_islandId = - 2 ;
if ( numBodies > 0 )
m_analyticsData . m_islandId = bodies [ 0 ] - > getCompanionId ( ) ;
m_analyticsData . m_numBodies = numBodies ;
m_analyticsData . m_numContactManifolds = numManifolds ;
m_analyticsData . m_remainingLeastSquaresResidual = m_leastSquaresResidual ;
2017-08-01 12:30:58 +00:00
break ;
}
}
}
return 0.f ;
}
2020-01-08 17:05:43 +00:00
void btSequentialImpulseConstraintSolver : : writeBackContacts ( int iBegin , int iEnd , const btContactSolverInfo & infoGlobal )
2017-08-01 12:30:58 +00:00
{
2019-01-03 13:26:51 +00:00
for ( int j = iBegin ; j < iEnd ; j + + )
{
2020-01-08 17:05:43 +00:00
const btSolverConstraint & solveManifold = m_tmpSolverContactConstraintPool [ j ] ;
2019-01-03 13:26:51 +00:00
btManifoldPoint * pt = ( btManifoldPoint * ) solveManifold . m_originalContactPoint ;
btAssert ( pt ) ;
pt - > m_appliedImpulse = solveManifold . m_appliedImpulse ;
2017-08-01 12:30:58 +00:00
// float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
2019-01-03 13:26:51 +00:00
// printf("pt->m_appliedImpulseLateral1 = %f\n", f);
2020-01-08 17:05:43 +00:00
pt - > m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool [ solveManifold . m_frictionIndex ] . m_appliedImpulse ;
2019-01-03 13:26:51 +00:00
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
{
2020-01-08 17:05:43 +00:00
pt - > m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool [ solveManifold . m_frictionIndex + 1 ] . m_appliedImpulse ;
2017-08-01 12:30:58 +00:00
}
2019-01-03 13:26:51 +00:00
//do a callback here?
}
2018-09-07 14:11:04 +00:00
}
2017-08-01 12:30:58 +00:00
2018-09-07 14:11:04 +00:00
void btSequentialImpulseConstraintSolver : : writeBackJoints ( int iBegin , int iEnd , const btContactSolverInfo & infoGlobal )
{
2019-01-03 13:26:51 +00:00
for ( int j = iBegin ; j < iEnd ; j + + )
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
const btSolverConstraint & solverConstr = m_tmpSolverNonContactConstraintPool [ j ] ;
2017-08-01 12:30:58 +00:00
btTypedConstraint * constr = ( btTypedConstraint * ) solverConstr . m_originalContactPoint ;
btJointFeedback * fb = constr - > getJointFeedback ( ) ;
if ( fb )
{
2019-01-03 13:26:51 +00:00
fb - > m_appliedForceBodyA + = solverConstr . m_contactNormal1 * solverConstr . m_appliedImpulse * constr - > getRigidBodyA ( ) . getLinearFactor ( ) / infoGlobal . m_timeStep ;
fb - > m_appliedForceBodyB + = solverConstr . m_contactNormal2 * solverConstr . m_appliedImpulse * constr - > getRigidBodyB ( ) . getLinearFactor ( ) / infoGlobal . m_timeStep ;
fb - > m_appliedTorqueBodyA + = solverConstr . m_relpos1CrossNormal * constr - > getRigidBodyA ( ) . getAngularFactor ( ) * solverConstr . m_appliedImpulse / infoGlobal . m_timeStep ;
fb - > m_appliedTorqueBodyB + = solverConstr . m_relpos2CrossNormal * constr - > getRigidBodyB ( ) . getAngularFactor ( ) * solverConstr . m_appliedImpulse / infoGlobal . m_timeStep ; /*RGM ???? */
2017-08-01 12:30:58 +00:00
}
constr - > internalSetAppliedImpulse ( solverConstr . m_appliedImpulse ) ;
2019-01-03 13:26:51 +00:00
if ( btFabs ( solverConstr . m_appliedImpulse ) > = constr - > getBreakingImpulseThreshold ( ) )
2017-08-01 12:30:58 +00:00
{
constr - > setEnabled ( false ) ;
}
}
2018-09-07 14:11:04 +00:00
}
2017-08-01 12:30:58 +00:00
2018-09-07 14:11:04 +00:00
void btSequentialImpulseConstraintSolver : : writeBackBodies ( int iBegin , int iEnd , const btContactSolverInfo & infoGlobal )
{
2019-01-03 13:26:51 +00:00
for ( int i = iBegin ; i < iEnd ; i + + )
2017-08-01 12:30:58 +00:00
{
2020-01-08 17:05:43 +00:00
btRigidBody * body = m_tmpSolverBodyPool [ i ] . m_originalBody ;
2017-08-01 12:30:58 +00:00
if ( body )
{
if ( infoGlobal . m_splitImpulse )
2020-01-08 17:05:43 +00:00
m_tmpSolverBodyPool [ i ] . writebackVelocityAndTransform ( infoGlobal . m_timeStep , infoGlobal . m_splitImpulseTurnErp ) ;
2017-08-01 12:30:58 +00:00
else
2020-01-08 17:05:43 +00:00
m_tmpSolverBodyPool [ i ] . writebackVelocity ( ) ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
m_tmpSolverBodyPool [ i ] . m_originalBody - > setLinearVelocity (
m_tmpSolverBodyPool [ i ] . m_linearVelocity +
m_tmpSolverBodyPool [ i ] . m_externalForceImpulse ) ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
m_tmpSolverBodyPool [ i ] . m_originalBody - > setAngularVelocity (
m_tmpSolverBodyPool [ i ] . m_angularVelocity +
m_tmpSolverBodyPool [ i ] . m_externalTorqueImpulse ) ;
2017-08-01 12:30:58 +00:00
if ( infoGlobal . m_splitImpulse )
2020-01-08 17:05:43 +00:00
m_tmpSolverBodyPool [ i ] . m_originalBody - > setWorldTransform ( m_tmpSolverBodyPool [ i ] . m_worldTransform ) ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
m_tmpSolverBodyPool [ i ] . m_originalBody - > setCompanionId ( - 1 ) ;
2017-08-01 12:30:58 +00:00
}
}
2018-09-07 14:11:04 +00:00
}
2020-01-08 17:05:43 +00:00
btScalar btSequentialImpulseConstraintSolver : : solveGroupCacheFriendlyFinish ( btCollisionObject * * bodies , int numBodies , const btContactSolverInfo & infoGlobal )
2018-09-07 14:11:04 +00:00
{
BT_PROFILE ( " solveGroupCacheFriendlyFinish " ) ;
if ( infoGlobal . m_solverMode & SOLVER_USE_WARMSTARTING )
{
2020-01-08 17:05:43 +00:00
writeBackContacts ( 0 , m_tmpSolverContactConstraintPool . size ( ) , infoGlobal ) ;
2018-09-07 14:11:04 +00:00
}
2020-01-08 17:05:43 +00:00
writeBackJoints ( 0 , m_tmpSolverNonContactConstraintPool . size ( ) , infoGlobal ) ;
writeBackBodies ( 0 , m_tmpSolverBodyPool . size ( ) , infoGlobal ) ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
m_tmpSolverContactConstraintPool . resizeNoInitialize ( 0 ) ;
m_tmpSolverNonContactConstraintPool . resizeNoInitialize ( 0 ) ;
m_tmpSolverContactFrictionConstraintPool . resizeNoInitialize ( 0 ) ;
m_tmpSolverContactRollingFrictionConstraintPool . resizeNoInitialize ( 0 ) ;
2017-08-01 12:30:58 +00:00
2020-01-08 17:05:43 +00:00
m_tmpSolverBodyPool . resizeNoInitialize ( 0 ) ;
2017-08-01 12:30:58 +00:00
return 0.f ;
}
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
2019-01-03 13:26:51 +00:00
btScalar btSequentialImpulseConstraintSolver : : solveGroup ( btCollisionObject * * bodies , int numBodies , btPersistentManifold * * manifoldPtr , int numManifolds , btTypedConstraint * * constraints , int numConstraints , const btContactSolverInfo & infoGlobal , btIDebugDraw * debugDrawer , btDispatcher * /*dispatcher*/ )
2017-08-01 12:30:58 +00:00
{
BT_PROFILE ( " solveGroup " ) ;
//you need to provide at least some bodies
2019-01-03 13:26:51 +00:00
solveGroupCacheFriendlySetup ( bodies , numBodies , manifoldPtr , numManifolds , constraints , numConstraints , infoGlobal , debugDrawer ) ;
2017-08-01 12:30:58 +00:00
2019-01-03 13:26:51 +00:00
solveGroupCacheFriendlyIterations ( bodies , numBodies , manifoldPtr , numManifolds , constraints , numConstraints , infoGlobal , debugDrawer ) ;
2017-08-01 12:30:58 +00:00
solveGroupCacheFriendlyFinish ( bodies , numBodies , infoGlobal ) ;
return 0.f ;
}
2019-01-03 13:26:51 +00:00
void btSequentialImpulseConstraintSolver : : reset ( )
2017-08-01 12:30:58 +00:00
{
m_btSeed2 = 0 ;
2020-01-08 17:05:43 +00:00
}