2017-08-01 14:30:58 +02:00
/*
Bullet Continuous Collision Detection and Physics Library
Copyright ( c ) 2013 Erwin Coumans http : //bulletphysics.org
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 .
*/
# include "btMultiBodyConstraintSolver.h"
# include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
# include "btMultiBodyLinkCollider.h"
# include "BulletDynamics/ConstraintSolver/btSolverBody.h"
# include "btMultiBodyConstraint.h"
# include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
# include "LinearMath/btQuickprof.h"
2020-01-08 18:05:43 +01:00
# include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
# include "LinearMath/btScalar.h"
2017-08-01 14:30:58 +02:00
2019-01-03 14:26:51 +01:00
btScalar btMultiBodyConstraintSolver : : solveSingleIteration ( int iteration , btCollisionObject * * bodies , int numBodies , btPersistentManifold * * manifoldPtr , int numManifolds , btTypedConstraint * * constraints , int numConstraints , const btContactSolverInfo & infoGlobal , btIDebugDraw * debugDrawer )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver : : solveSingleIteration ( iteration , bodies , numBodies , manifoldPtr , numManifolds , constraints , numConstraints , infoGlobal , debugDrawer ) ;
2017-08-01 14:30:58 +02:00
//solve featherstone non-contact constraints
//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
2019-01-03 14:26:51 +01:00
for ( int j = 0 ; j < m_multiBodyNonContactConstraints . size ( ) ; j + + )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
int index = iteration & 1 ? j : m_multiBodyNonContactConstraints . size ( ) - 1 - j ;
2017-08-01 14:30:58 +02:00
btMultiBodySolverConstraint & constraint = m_multiBodyNonContactConstraints [ index ] ;
2019-01-03 14:26:51 +01:00
2017-08-01 14:30:58 +02:00
btScalar residual = resolveSingleConstraintRowGeneric ( constraint ) ;
2019-01-03 14:26:51 +01:00
leastSquaredResidual = btMax ( leastSquaredResidual , residual * residual ) ;
2017-08-01 14:30:58 +02:00
2019-01-03 14:26:51 +01:00
if ( constraint . m_multiBodyA )
2017-08-01 14:30:58 +02:00
constraint . m_multiBodyA - > setPosUpdated ( false ) ;
2019-01-03 14:26:51 +01:00
if ( constraint . m_multiBodyB )
2017-08-01 14:30:58 +02:00
constraint . m_multiBodyB - > setPosUpdated ( false ) ;
}
//solve featherstone normal contact
2019-01-03 14:26:51 +01:00
for ( int j0 = 0 ; j0 < m_multiBodyNormalContactConstraints . size ( ) ; j0 + + )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
int index = j0 ; //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
2017-08-01 14:30:58 +02:00
btMultiBodySolverConstraint & constraint = m_multiBodyNormalContactConstraints [ index ] ;
btScalar residual = 0.f ;
if ( iteration < infoGlobal . m_numIterations )
{
residual = resolveSingleConstraintRowGeneric ( constraint ) ;
}
2019-01-03 14:26:51 +01:00
leastSquaredResidual = btMax ( leastSquaredResidual , residual * residual ) ;
if ( constraint . m_multiBodyA )
2017-08-01 14:30:58 +02:00
constraint . m_multiBodyA - > setPosUpdated ( false ) ;
2019-01-03 14:26:51 +01:00
if ( constraint . m_multiBodyB )
2017-08-01 14:30:58 +02:00
constraint . m_multiBodyB - > setPosUpdated ( false ) ;
}
2018-09-07 16:11:04 +02:00
//solve featherstone frictional contact
2019-01-03 14:26:51 +01:00
if ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS & & ( ( infoGlobal . m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION ) = = 0 ) )
2017-08-01 14:30:58 +02:00
{
2019-06-11 13:18:05 +02:00
for ( int j1 = 0 ; j1 < this - > m_multiBodySpinningFrictionContactConstraints . size ( ) ; j1 + + )
{
if ( iteration < infoGlobal . m_numIterations )
{
int index = j1 ;
btMultiBodySolverConstraint & frictionConstraint = m_multiBodySpinningFrictionContactConstraints [ index ] ;
btScalar totalImpulse = m_multiBodyNormalContactConstraints [ frictionConstraint . m_frictionIndex ] . m_appliedImpulse ;
//adjust friction limits here
if ( totalImpulse > btScalar ( 0 ) )
{
frictionConstraint . m_lowerLimit = - ( frictionConstraint . m_friction * totalImpulse ) ;
frictionConstraint . m_upperLimit = frictionConstraint . m_friction * totalImpulse ;
btScalar residual = resolveSingleConstraintRowGeneric ( frictionConstraint ) ;
leastSquaredResidual = btMax ( leastSquaredResidual , residual * residual ) ;
if ( frictionConstraint . m_multiBodyA )
frictionConstraint . m_multiBodyA - > setPosUpdated ( false ) ;
if ( frictionConstraint . m_multiBodyB )
frictionConstraint . m_multiBodyB - > setPosUpdated ( false ) ;
}
}
}
2019-01-03 14:26:51 +01:00
for ( int j1 = 0 ; j1 < this - > m_multiBodyTorsionalFrictionContactConstraints . size ( ) ; j1 + + )
2018-09-07 16:11:04 +02:00
{
if ( iteration < infoGlobal . m_numIterations )
{
2019-01-03 14:26:51 +01:00
int index = j1 ; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
2018-09-07 16:11:04 +02:00
btMultiBodySolverConstraint & frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints [ index ] ;
btScalar totalImpulse = m_multiBodyNormalContactConstraints [ frictionConstraint . m_frictionIndex ] . m_appliedImpulse ;
2019-06-11 13:18:05 +02:00
j1 + + ;
int index2 = j1 ;
btMultiBodySolverConstraint & frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints [ index2 ] ;
2018-09-07 16:11:04 +02:00
//adjust friction limits here
2019-06-11 13:18:05 +02:00
if ( totalImpulse > btScalar ( 0 ) & & frictionConstraint . m_frictionIndex = = frictionConstraintB . m_frictionIndex )
2018-09-07 16:11:04 +02:00
{
2019-01-03 14:26:51 +01:00
frictionConstraint . m_lowerLimit = - ( frictionConstraint . m_friction * totalImpulse ) ;
frictionConstraint . m_upperLimit = frictionConstraint . m_friction * totalImpulse ;
2019-06-11 13:18:05 +02:00
frictionConstraintB . m_lowerLimit = - ( frictionConstraintB . m_friction * totalImpulse ) ;
frictionConstraintB . m_upperLimit = frictionConstraintB . m_friction * totalImpulse ;
btScalar residual = resolveConeFrictionConstraintRows ( frictionConstraint , frictionConstraintB ) ;
2019-01-03 14:26:51 +01:00
leastSquaredResidual = btMax ( leastSquaredResidual , residual * residual ) ;
2018-09-07 16:11:04 +02:00
if ( frictionConstraint . m_multiBodyA )
frictionConstraint . m_multiBodyA - > setPosUpdated ( false ) ;
if ( frictionConstraint . m_multiBodyB )
frictionConstraint . m_multiBodyB - > setPosUpdated ( false ) ;
2019-06-11 13:18:05 +02:00
if ( frictionConstraintB . m_multiBodyA )
frictionConstraintB . m_multiBodyA - > setPosUpdated ( false ) ;
if ( frictionConstraintB . m_multiBodyB )
frictionConstraintB . m_multiBodyB - > setPosUpdated ( false ) ;
2018-09-07 16:11:04 +02:00
}
}
}
for ( int j1 = 0 ; j1 < this - > m_multiBodyFrictionContactConstraints . size ( ) ; j1 + + )
2017-08-01 14:30:58 +02:00
{
2018-09-07 16:11:04 +02:00
if ( iteration < infoGlobal . m_numIterations )
{
2019-01-03 14:26:51 +01:00
int index = j1 ; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
2018-09-07 16:11:04 +02:00
btMultiBodySolverConstraint & frictionConstraint = m_multiBodyFrictionContactConstraints [ index ] ;
btScalar totalImpulse = m_multiBodyNormalContactConstraints [ frictionConstraint . m_frictionIndex ] . m_appliedImpulse ;
j1 + + ;
2019-01-03 14:26:51 +01:00
int index2 = j1 ; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
2018-09-07 16:11:04 +02:00
btMultiBodySolverConstraint & frictionConstraintB = m_multiBodyFrictionContactConstraints [ index2 ] ;
btAssert ( frictionConstraint . m_frictionIndex = = frictionConstraintB . m_frictionIndex ) ;
if ( frictionConstraint . m_frictionIndex = = frictionConstraintB . m_frictionIndex )
{
2019-01-03 14:26:51 +01:00
frictionConstraint . m_lowerLimit = - ( frictionConstraint . m_friction * totalImpulse ) ;
frictionConstraint . m_upperLimit = frictionConstraint . m_friction * totalImpulse ;
frictionConstraintB . m_lowerLimit = - ( frictionConstraintB . m_friction * totalImpulse ) ;
frictionConstraintB . m_upperLimit = frictionConstraintB . m_friction * totalImpulse ;
2018-09-07 16:11:04 +02:00
btScalar residual = resolveConeFrictionConstraintRows ( frictionConstraint , frictionConstraintB ) ;
2019-01-03 14:26:51 +01:00
leastSquaredResidual = btMax ( leastSquaredResidual , residual * residual ) ;
2018-09-07 16:11:04 +02:00
if ( frictionConstraintB . m_multiBodyA )
frictionConstraintB . m_multiBodyA - > setPosUpdated ( false ) ;
if ( frictionConstraintB . m_multiBodyB )
frictionConstraintB . m_multiBodyB - > setPosUpdated ( false ) ;
2019-01-03 14:26:51 +01:00
2018-09-07 16:11:04 +02:00
if ( frictionConstraint . m_multiBodyA )
frictionConstraint . m_multiBodyA - > setPosUpdated ( false ) ;
if ( frictionConstraint . m_multiBodyB )
frictionConstraint . m_multiBodyB - > setPosUpdated ( false ) ;
}
}
}
}
else
{
2019-01-03 14:26:51 +01:00
for ( int j1 = 0 ; j1 < this - > m_multiBodyFrictionContactConstraints . size ( ) ; j1 + + )
2018-09-07 16:11:04 +02:00
{
if ( iteration < infoGlobal . m_numIterations )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
int index = j1 ; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
2018-09-07 16:11:04 +02:00
btMultiBodySolverConstraint & frictionConstraint = m_multiBodyFrictionContactConstraints [ index ] ;
btScalar totalImpulse = m_multiBodyNormalContactConstraints [ frictionConstraint . m_frictionIndex ] . m_appliedImpulse ;
//adjust friction limits here
2019-01-03 14:26:51 +01:00
if ( totalImpulse > btScalar ( 0 ) )
2018-09-07 16:11:04 +02:00
{
2019-01-03 14:26:51 +01:00
frictionConstraint . m_lowerLimit = - ( frictionConstraint . m_friction * totalImpulse ) ;
frictionConstraint . m_upperLimit = frictionConstraint . m_friction * totalImpulse ;
2018-09-07 16:11:04 +02:00
btScalar residual = resolveSingleConstraintRowGeneric ( frictionConstraint ) ;
2019-01-03 14:26:51 +01:00
leastSquaredResidual = btMax ( leastSquaredResidual , residual * residual ) ;
2018-09-07 16:11:04 +02:00
if ( frictionConstraint . m_multiBodyA )
frictionConstraint . m_multiBodyA - > setPosUpdated ( false ) ;
if ( frictionConstraint . m_multiBodyB )
frictionConstraint . m_multiBodyB - > setPosUpdated ( false ) ;
}
2017-08-01 14:30:58 +02:00
}
}
}
return leastSquaredResidual ;
}
2019-01-03 14:26:51 +01:00
btScalar btMultiBodyConstraintSolver : : solveGroupCacheFriendlySetup ( btCollisionObject * * bodies , int numBodies , btPersistentManifold * * manifoldPtr , int numManifolds , btTypedConstraint * * constraints , int numConstraints , const btContactSolverInfo & infoGlobal , btIDebugDraw * debugDrawer )
2017-08-01 14:30:58 +02:00
{
m_multiBodyNonContactConstraints . resize ( 0 ) ;
m_multiBodyNormalContactConstraints . resize ( 0 ) ;
m_multiBodyFrictionContactConstraints . resize ( 0 ) ;
2018-09-07 16:11:04 +02:00
m_multiBodyTorsionalFrictionContactConstraints . resize ( 0 ) ;
2019-06-11 13:18:05 +02:00
m_multiBodySpinningFrictionContactConstraints . resize ( 0 ) ;
2019-01-03 14:26:51 +01:00
2017-08-01 14:30:58 +02:00
m_data . m_jacobians . resize ( 0 ) ;
m_data . m_deltaVelocitiesUnitImpulse . resize ( 0 ) ;
m_data . m_deltaVelocities . resize ( 0 ) ;
2019-01-03 14:26:51 +01:00
for ( int i = 0 ; i < numBodies ; i + + )
2017-08-01 14:30:58 +02:00
{
const btMultiBodyLinkCollider * fcA = btMultiBodyLinkCollider : : upcast ( bodies [ i ] ) ;
if ( fcA )
{
fcA - > m_multiBody - > setCompanionId ( - 1 ) ;
}
}
2019-01-03 14:26:51 +01:00
btScalar val = btSequentialImpulseConstraintSolver : : solveGroupCacheFriendlySetup ( bodies , numBodies , manifoldPtr , numManifolds , constraints , numConstraints , infoGlobal , debugDrawer ) ;
2017-08-01 14:30:58 +02:00
return val ;
}
2019-01-03 14:26:51 +01:00
void btMultiBodyConstraintSolver : : applyDeltaVee ( btScalar * delta_vee , btScalar impulse , int velocityIndex , int ndof )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
for ( int i = 0 ; i < ndof ; + + i )
m_data . m_deltaVelocities [ velocityIndex + i ] + = delta_vee [ i ] * impulse ;
2017-08-01 14:30:58 +02:00
}
btScalar btMultiBodyConstraintSolver : : resolveSingleConstraintRowGeneric ( const btMultiBodySolverConstraint & c )
{
2019-01-03 14:26:51 +01:00
btScalar deltaImpulse = c . m_rhs - btScalar ( c . m_appliedImpulse ) * c . m_cfm ;
2018-09-07 16:11:04 +02:00
btScalar deltaVelADotn = 0 ;
btScalar deltaVelBDotn = 0 ;
2017-08-01 14:30:58 +02:00
btSolverBody * bodyA = 0 ;
btSolverBody * bodyB = 0 ;
2018-09-07 16:11:04 +02:00
int ndofA = 0 ;
int ndofB = 0 ;
2017-08-01 14:30:58 +02:00
if ( c . m_multiBodyA )
{
2018-09-07 16:11:04 +02:00
ndofA = c . m_multiBodyA - > getNumDofs ( ) + 6 ;
for ( int i = 0 ; i < ndofA ; + + i )
deltaVelADotn + = m_data . m_jacobians [ c . m_jacAindex + i ] * m_data . m_deltaVelocities [ c . m_deltaVelAindex + i ] ;
}
else if ( c . m_solverBodyIdA > = 0 )
2017-08-01 14:30:58 +02:00
{
bodyA = & m_tmpSolverBodyPool [ c . m_solverBodyIdA ] ;
2018-09-07 16:11:04 +02:00
deltaVelADotn + = c . m_contactNormal1 . dot ( bodyA - > internalGetDeltaLinearVelocity ( ) ) + c . m_relpos1CrossNormal . dot ( bodyA - > internalGetDeltaAngularVelocity ( ) ) ;
2017-08-01 14:30:58 +02:00
}
if ( c . m_multiBodyB )
{
2018-09-07 16:11:04 +02:00
ndofB = c . m_multiBodyB - > getNumDofs ( ) + 6 ;
for ( int i = 0 ; i < ndofB ; + + i )
deltaVelBDotn + = m_data . m_jacobians [ c . m_jacBindex + i ] * m_data . m_deltaVelocities [ c . m_deltaVelBindex + i ] ;
}
else if ( c . m_solverBodyIdB > = 0 )
2017-08-01 14:30:58 +02:00
{
bodyB = & m_tmpSolverBodyPool [ c . m_solverBodyIdB ] ;
2018-09-07 16:11:04 +02:00
deltaVelBDotn + = c . m_contactNormal2 . dot ( bodyB - > internalGetDeltaLinearVelocity ( ) ) + c . m_relpos2CrossNormal . dot ( bodyB - > internalGetDeltaAngularVelocity ( ) ) ;
2017-08-01 14:30:58 +02:00
}
2019-01-03 14:26:51 +01:00
deltaImpulse - = deltaVelADotn * c . m_jacDiagABInv ; //m_jacDiagABInv = 1./denom
deltaImpulse - = deltaVelBDotn * c . m_jacDiagABInv ;
2017-08-01 14:30:58 +02:00
const btScalar sum = btScalar ( c . m_appliedImpulse ) + deltaImpulse ;
2018-09-07 16:11:04 +02:00
2017-08-01 14:30:58 +02:00
if ( sum < c . m_lowerLimit )
{
2018-09-07 16:11:04 +02:00
deltaImpulse = c . m_lowerLimit - c . m_appliedImpulse ;
2017-08-01 14:30:58 +02:00
c . m_appliedImpulse = c . m_lowerLimit ;
}
2018-09-07 16:11:04 +02:00
else if ( sum > c . m_upperLimit )
2017-08-01 14:30:58 +02:00
{
2018-09-07 16:11:04 +02:00
deltaImpulse = c . m_upperLimit - c . m_appliedImpulse ;
2017-08-01 14:30:58 +02:00
c . m_appliedImpulse = c . m_upperLimit ;
}
else
{
c . m_appliedImpulse = sum ;
}
2019-01-03 14:26:51 +01:00
2017-08-01 14:30:58 +02:00
if ( c . m_multiBodyA )
{
2018-09-07 16:11:04 +02:00
applyDeltaVee ( & m_data . m_deltaVelocitiesUnitImpulse [ c . m_jacAindex ] , deltaImpulse , c . m_deltaVelAindex , ndofA ) ;
2017-08-01 14:30:58 +02:00
# ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
2018-09-07 16:11:04 +02:00
c . m_multiBodyA - > applyDeltaVeeMultiDof2 ( & m_data . m_deltaVelocitiesUnitImpulse [ c . m_jacAindex ] , deltaImpulse ) ;
2019-01-03 14:26:51 +01:00
# endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
2018-09-07 16:11:04 +02:00
}
else if ( c . m_solverBodyIdA > = 0 )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
bodyA - > internalApplyImpulse ( c . m_contactNormal1 * bodyA - > i nternalGetInvMass ( ) , c . m_angularComponentA , deltaImpulse ) ;
2017-08-01 14:30:58 +02:00
}
if ( c . m_multiBodyB )
{
2018-09-07 16:11:04 +02:00
applyDeltaVee ( & m_data . m_deltaVelocitiesUnitImpulse [ c . m_jacBindex ] , deltaImpulse , c . m_deltaVelBindex , ndofB ) ;
2017-08-01 14:30:58 +02:00
# ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
2018-09-07 16:11:04 +02:00
c . m_multiBodyB - > applyDeltaVeeMultiDof2 ( & m_data . m_deltaVelocitiesUnitImpulse [ c . m_jacBindex ] , deltaImpulse ) ;
2019-01-03 14:26:51 +01:00
# endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
2018-09-07 16:11:04 +02:00
}
else if ( c . m_solverBodyIdB > = 0 )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
bodyB - > internalApplyImpulse ( c . m_contactNormal2 * bodyB - > internalGetInvMass ( ) , c . m_angularComponentB , deltaImpulse ) ;
2017-08-01 14:30:58 +02:00
}
2019-01-03 14:26:51 +01:00
btScalar deltaVel = deltaImpulse / c . m_jacDiagABInv ;
2018-09-07 16:11:04 +02:00
return deltaVel ;
}
2019-01-03 14:26:51 +01:00
btScalar btMultiBodyConstraintSolver : : resolveConeFrictionConstraintRows ( const btMultiBodySolverConstraint & cA1 , const btMultiBodySolverConstraint & cB )
2018-09-07 16:11:04 +02:00
{
2019-01-03 14:26:51 +01:00
int ndofA = 0 ;
int ndofB = 0 ;
2018-09-07 16:11:04 +02:00
btSolverBody * bodyA = 0 ;
btSolverBody * bodyB = 0 ;
btScalar deltaImpulseB = 0.f ;
btScalar sumB = 0.f ;
{
2019-01-03 14:26:51 +01:00
deltaImpulseB = cB . m_rhs - btScalar ( cB . m_appliedImpulse ) * cB . m_cfm ;
btScalar deltaVelADotn = 0 ;
btScalar deltaVelBDotn = 0 ;
2018-09-07 16:11:04 +02:00
if ( cB . m_multiBodyA )
{
2019-01-03 14:26:51 +01:00
ndofA = cB . m_multiBodyA - > getNumDofs ( ) + 6 ;
for ( int i = 0 ; i < ndofA ; + + i )
deltaVelADotn + = m_data . m_jacobians [ cB . m_jacAindex + i ] * m_data . m_deltaVelocities [ cB . m_deltaVelAindex + i ] ;
}
else if ( cB . m_solverBodyIdA > = 0 )
2018-09-07 16:11:04 +02:00
{
bodyA = & m_tmpSolverBodyPool [ cB . m_solverBodyIdA ] ;
2019-01-03 14:26:51 +01:00
deltaVelADotn + = cB . m_contactNormal1 . dot ( bodyA - > internalGetDeltaLinearVelocity ( ) ) + cB . m_relpos1CrossNormal . dot ( bodyA - > internalGetDeltaAngularVelocity ( ) ) ;
2018-09-07 16:11:04 +02:00
}
if ( cB . m_multiBodyB )
{
2019-01-03 14:26:51 +01:00
ndofB = cB . m_multiBodyB - > getNumDofs ( ) + 6 ;
for ( int i = 0 ; i < ndofB ; + + i )
deltaVelBDotn + = m_data . m_jacobians [ cB . m_jacBindex + i ] * m_data . m_deltaVelocities [ cB . m_deltaVelBindex + i ] ;
}
else if ( cB . m_solverBodyIdB > = 0 )
2018-09-07 16:11:04 +02:00
{
bodyB = & m_tmpSolverBodyPool [ cB . m_solverBodyIdB ] ;
2019-01-03 14:26:51 +01:00
deltaVelBDotn + = cB . m_contactNormal2 . dot ( bodyB - > internalGetDeltaLinearVelocity ( ) ) + cB . m_relpos2CrossNormal . dot ( bodyB - > internalGetDeltaAngularVelocity ( ) ) ;
2018-09-07 16:11:04 +02:00
}
2019-01-03 14:26:51 +01:00
deltaImpulseB - = deltaVelADotn * cB . m_jacDiagABInv ; //m_jacDiagABInv = 1./denom
deltaImpulseB - = deltaVelBDotn * cB . m_jacDiagABInv ;
2018-09-07 16:11:04 +02:00
sumB = btScalar ( cB . m_appliedImpulse ) + deltaImpulseB ;
}
btScalar deltaImpulseA = 0.f ;
btScalar sumA = 0.f ;
const btMultiBodySolverConstraint & cA = cA1 ;
{
{
2019-01-03 14:26:51 +01:00
deltaImpulseA = cA . m_rhs - btScalar ( cA . m_appliedImpulse ) * cA . m_cfm ;
btScalar deltaVelADotn = 0 ;
btScalar deltaVelBDotn = 0 ;
2018-09-07 16:11:04 +02:00
if ( cA . m_multiBodyA )
{
2019-01-03 14:26:51 +01:00
ndofA = cA . m_multiBodyA - > getNumDofs ( ) + 6 ;
for ( int i = 0 ; i < ndofA ; + + i )
deltaVelADotn + = m_data . m_jacobians [ cA . m_jacAindex + i ] * m_data . m_deltaVelocities [ cA . m_deltaVelAindex + i ] ;
}
else if ( cA . m_solverBodyIdA > = 0 )
2018-09-07 16:11:04 +02:00
{
bodyA = & m_tmpSolverBodyPool [ cA . m_solverBodyIdA ] ;
2019-01-03 14:26:51 +01:00
deltaVelADotn + = cA . m_contactNormal1 . dot ( bodyA - > internalGetDeltaLinearVelocity ( ) ) + cA . m_relpos1CrossNormal . dot ( bodyA - > internalGetDeltaAngularVelocity ( ) ) ;
2018-09-07 16:11:04 +02:00
}
if ( cA . m_multiBodyB )
{
2019-01-03 14:26:51 +01:00
ndofB = cA . m_multiBodyB - > getNumDofs ( ) + 6 ;
for ( int i = 0 ; i < ndofB ; + + i )
deltaVelBDotn + = m_data . m_jacobians [ cA . m_jacBindex + i ] * m_data . m_deltaVelocities [ cA . m_deltaVelBindex + i ] ;
}
else if ( cA . m_solverBodyIdB > = 0 )
2018-09-07 16:11:04 +02:00
{
bodyB = & m_tmpSolverBodyPool [ cA . m_solverBodyIdB ] ;
2019-01-03 14:26:51 +01:00
deltaVelBDotn + = cA . m_contactNormal2 . dot ( bodyB - > internalGetDeltaLinearVelocity ( ) ) + cA . m_relpos2CrossNormal . dot ( bodyB - > internalGetDeltaAngularVelocity ( ) ) ;
2018-09-07 16:11:04 +02:00
}
2019-01-03 14:26:51 +01:00
deltaImpulseA - = deltaVelADotn * cA . m_jacDiagABInv ; //m_jacDiagABInv = 1./denom
deltaImpulseA - = deltaVelBDotn * cA . m_jacDiagABInv ;
2018-09-07 16:11:04 +02:00
sumA = btScalar ( cA . m_appliedImpulse ) + deltaImpulseA ;
}
}
2019-01-03 14:26:51 +01:00
if ( sumA * sumA + sumB * sumB > = cA . m_lowerLimit * cB . m_lowerLimit )
2018-09-07 16:11:04 +02:00
{
2019-01-03 14:26:51 +01:00
btScalar angle = btAtan2 ( sumA , sumB ) ;
btScalar sumAclipped = btFabs ( cA . m_lowerLimit * btSin ( angle ) ) ;
btScalar sumBclipped = btFabs ( cB . m_lowerLimit * btCos ( angle ) ) ;
2018-09-07 16:11:04 +02:00
if ( sumA < - sumAclipped )
{
deltaImpulseA = - sumAclipped - cA . m_appliedImpulse ;
cA . m_appliedImpulse = - sumAclipped ;
}
else if ( sumA > sumAclipped )
{
deltaImpulseA = sumAclipped - cA . m_appliedImpulse ;
cA . m_appliedImpulse = sumAclipped ;
}
else
{
cA . m_appliedImpulse = sumA ;
}
if ( sumB < - sumBclipped )
{
deltaImpulseB = - sumBclipped - cB . m_appliedImpulse ;
cB . m_appliedImpulse = - sumBclipped ;
}
else if ( sumB > sumBclipped )
{
deltaImpulseB = sumBclipped - cB . m_appliedImpulse ;
cB . m_appliedImpulse = sumBclipped ;
}
else
{
cB . m_appliedImpulse = sumB ;
}
//deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
//cA.m_appliedImpulse = sumAclipped;
//deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
//cB.m_appliedImpulse = sumBclipped;
2019-01-03 14:26:51 +01:00
}
2018-09-07 16:11:04 +02:00
else
{
cA . m_appliedImpulse = sumA ;
cB . m_appliedImpulse = sumB ;
}
2019-01-03 14:26:51 +01:00
2018-09-07 16:11:04 +02:00
if ( cA . m_multiBodyA )
{
2019-01-03 14:26:51 +01:00
applyDeltaVee ( & m_data . m_deltaVelocitiesUnitImpulse [ cA . m_jacAindex ] , deltaImpulseA , cA . m_deltaVelAindex , ndofA ) ;
2018-09-07 16:11:04 +02:00
# ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
2019-01-03 14:26:51 +01:00
cA . m_multiBodyA - > applyDeltaVeeMultiDof2 ( & m_data . m_deltaVelocitiesUnitImpulse [ cA . m_jacAindex ] , deltaImpulseA ) ;
# endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
}
else if ( cA . m_solverBodyIdA > = 0 )
2018-09-07 16:11:04 +02:00
{
2019-01-03 14:26:51 +01:00
bodyA - > internalApplyImpulse ( cA . m_contactNormal1 * bodyA - > internalGetInvMass ( ) , cA . m_angularComponentA , deltaImpulseA ) ;
2018-09-07 16:11:04 +02:00
}
if ( cA . m_multiBodyB )
{
2019-01-03 14:26:51 +01:00
applyDeltaVee ( & m_data . m_deltaVelocitiesUnitImpulse [ cA . m_jacBindex ] , deltaImpulseA , cA . m_deltaVelBindex , ndofB ) ;
2018-09-07 16:11:04 +02:00
# ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
2019-01-03 14:26:51 +01:00
cA . m_multiBodyB - > applyDeltaVeeMultiDof2 ( & m_data . m_deltaVelocitiesUnitImpulse [ cA . m_jacBindex ] , deltaImpulseA ) ;
# endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
}
else if ( cA . m_solverBodyIdB > = 0 )
2018-09-07 16:11:04 +02:00
{
2019-01-03 14:26:51 +01:00
bodyB - > internalApplyImpulse ( cA . m_contactNormal2 * bodyB - > internalGetInvMass ( ) , cA . m_angularComponentB , deltaImpulseA ) ;
2018-09-07 16:11:04 +02:00
}
if ( cB . m_multiBodyA )
{
2019-01-03 14:26:51 +01:00
applyDeltaVee ( & m_data . m_deltaVelocitiesUnitImpulse [ cB . m_jacAindex ] , deltaImpulseB , cB . m_deltaVelAindex , ndofA ) ;
2018-09-07 16:11:04 +02:00
# ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
2019-01-03 14:26:51 +01:00
cB . m_multiBodyA - > applyDeltaVeeMultiDof2 ( & m_data . m_deltaVelocitiesUnitImpulse [ cB . m_jacAindex ] , deltaImpulseB ) ;
# endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
}
else if ( cB . m_solverBodyIdA > = 0 )
2018-09-07 16:11:04 +02:00
{
2019-01-03 14:26:51 +01:00
bodyA - > internalApplyImpulse ( cB . m_contactNormal1 * bodyA - > internalGetInvMass ( ) , cB . m_angularComponentA , deltaImpulseB ) ;
2018-09-07 16:11:04 +02:00
}
if ( cB . m_multiBodyB )
{
2019-01-03 14:26:51 +01:00
applyDeltaVee ( & m_data . m_deltaVelocitiesUnitImpulse [ cB . m_jacBindex ] , deltaImpulseB , cB . m_deltaVelBindex , ndofB ) ;
2018-09-07 16:11:04 +02:00
# ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
2019-01-03 14:26:51 +01:00
cB . m_multiBodyB - > applyDeltaVeeMultiDof2 ( & m_data . m_deltaVelocitiesUnitImpulse [ cB . m_jacBindex ] , deltaImpulseB ) ;
# endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
}
else if ( cB . m_solverBodyIdB > = 0 )
2018-09-07 16:11:04 +02:00
{
2019-01-03 14:26:51 +01:00
bodyB - > internalApplyImpulse ( cB . m_contactNormal2 * bodyB - > internalGetInvMass ( ) , cB . m_angularComponentB , deltaImpulseB ) ;
2018-09-07 16:11:04 +02:00
}
2019-01-03 14:26:51 +01:00
btScalar deltaVel = deltaImpulseA / cA . m_jacDiagABInv + deltaImpulseB / cB . m_jacDiagABInv ;
return deltaVel ;
2017-08-01 14:30:58 +02:00
}
2020-01-08 18:05:43 +01:00
void btMultiBodyConstraintSolver : : setupMultiBodyContactConstraint ( btMultiBodySolverConstraint & solverConstraint , const btVector3 & contactNormal , const btScalar & appliedImpulse , btManifoldPoint & cp , const btContactSolverInfo & infoGlobal , btScalar & relaxation , bool isFriction , btScalar desiredVelocity , btScalar cfmSlip )
2017-08-01 14:30:58 +02:00
{
BT_PROFILE ( " setupMultiBodyContactConstraint " ) ;
btVector3 rel_pos1 ;
btVector3 rel_pos2 ;
btMultiBody * multiBodyA = solverConstraint . m_multiBodyA ;
btMultiBody * multiBodyB = solverConstraint . m_multiBodyB ;
const btVector3 & pos1 = cp . getPositionWorldOnA ( ) ;
const btVector3 & pos2 = cp . getPositionWorldOnB ( ) ;
btSolverBody * bodyA = multiBodyA ? 0 : & m_tmpSolverBodyPool [ solverConstraint . m_solverBodyIdA ] ;
btSolverBody * bodyB = multiBodyB ? 0 : & m_tmpSolverBodyPool [ solverConstraint . m_solverBodyIdB ] ;
btRigidBody * rb0 = multiBodyA ? 0 : bodyA - > m_originalBody ;
btRigidBody * rb1 = multiBodyB ? 0 : bodyB - > m_originalBody ;
if ( bodyA )
2019-01-03 14:26:51 +01:00
rel_pos1 = pos1 - bodyA - > getWorldTransform ( ) . getOrigin ( ) ;
2017-08-01 14:30:58 +02:00
if ( bodyB )
rel_pos2 = pos2 - bodyB - > getWorldTransform ( ) . getOrigin ( ) ;
relaxation = infoGlobal . m_sor ;
2019-01-03 14:26:51 +01:00
btScalar invTimeStep = btScalar ( 1 ) / infoGlobal . m_timeStep ;
//cfm = 1 / ( dt * kp + kd )
//erp = dt * kp / ( dt * kp + kd )
btScalar cfm ;
2017-08-01 14:30:58 +02:00
btScalar erp ;
if ( isFriction )
{
cfm = infoGlobal . m_frictionCFM ;
erp = infoGlobal . m_frictionERP ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
cfm = infoGlobal . m_globalCfm ;
erp = infoGlobal . m_erp2 ;
2019-01-03 14:26:51 +01:00
if ( ( cp . m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM ) | | ( cp . m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP ) )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
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
2017-08-01 14:30:58 +02:00
{
if ( cp . m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING )
{
2019-01-03 14:26:51 +01:00
btScalar denom = ( infoGlobal . m_timeStep * cp . m_combinedContactStiffness1 + cp . m_combinedContactDamping1 ) ;
2017-08-01 14:30:58 +02:00
if ( denom < SIMD_EPSILON )
{
denom = SIMD_EPSILON ;
}
2019-01-03 14:26:51 +01:00
cfm = btScalar ( 1 ) / denom ;
2017-08-01 14:30:58 +02:00
erp = ( infoGlobal . m_timeStep * cp . m_combinedContactStiffness1 ) / denom ;
}
}
}
cfm * = invTimeStep ;
if ( multiBodyA )
{
2019-01-03 14:26:51 +01:00
if ( solverConstraint . m_linkA < 0 )
2017-08-01 14:30:58 +02:00
{
rel_pos1 = pos1 - multiBodyA - > getBasePos ( ) ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
rel_pos1 = pos1 - multiBodyA - > getLink ( solverConstraint . m_linkA ) . m_cachedWorldTransform . getOrigin ( ) ;
}
2019-01-03 14:26:51 +01:00
const int ndofA = multiBodyA - > getNumDofs ( ) + 6 ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_deltaVelAindex = multiBodyA - > getCompanionId ( ) ;
2019-01-03 14:26:51 +01:00
if ( solverConstraint . m_deltaVelAindex < 0 )
2017-08-01 14:30:58 +02:00
{
solverConstraint . m_deltaVelAindex = m_data . m_deltaVelocities . size ( ) ;
multiBodyA - > setCompanionId ( solverConstraint . m_deltaVelAindex ) ;
2019-01-03 14:26:51 +01:00
m_data . m_deltaVelocities . resize ( m_data . m_deltaVelocities . size ( ) + ndofA ) ;
}
else
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
btAssert ( m_data . m_deltaVelocities . size ( ) > = solverConstraint . m_deltaVelAindex + ndofA ) ;
2017-08-01 14:30:58 +02:00
}
solverConstraint . m_jacAindex = m_data . m_jacobians . size ( ) ;
2019-01-03 14:26:51 +01:00
m_data . m_jacobians . resize ( m_data . m_jacobians . size ( ) + ndofA ) ;
m_data . m_deltaVelocitiesUnitImpulse . resize ( m_data . m_deltaVelocitiesUnitImpulse . size ( ) + ndofA ) ;
2017-08-01 14:30:58 +02:00
btAssert ( m_data . m_jacobians . size ( ) = = m_data . m_deltaVelocitiesUnitImpulse . size ( ) ) ;
2019-01-03 14:26:51 +01:00
btScalar * jac1 = & m_data . m_jacobians [ solverConstraint . m_jacAindex ] ;
2017-08-01 14:30:58 +02:00
multiBodyA - > fillContactJacobianMultiDof ( solverConstraint . m_linkA , cp . getPositionWorldOnA ( ) , contactNormal , jac1 , m_data . scratch_r , m_data . scratch_v , m_data . scratch_m ) ;
btScalar * delta = & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacAindex ] ;
2019-01-03 14:26:51 +01:00
multiBodyA - > calcAccelerationDeltasMultiDof ( & m_data . m_jacobians [ solverConstraint . m_jacAindex ] , delta , m_data . scratch_r , m_data . scratch_v ) ;
2017-08-01 14:30:58 +02:00
btVector3 torqueAxis0 = rel_pos1 . cross ( contactNormal ) ;
solverConstraint . m_relpos1CrossNormal = torqueAxis0 ;
solverConstraint . m_contactNormal1 = contactNormal ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
btVector3 torqueAxis0 = rel_pos1 . cross ( contactNormal ) ;
solverConstraint . m_relpos1CrossNormal = torqueAxis0 ;
solverConstraint . m_contactNormal1 = contactNormal ;
2019-01-03 14:26:51 +01:00
solverConstraint . m_angularComponentA = rb0 ? rb0 - > getInvInertiaTensorWorld ( ) * torqueAxis0 * rb0 - > getAngularFactor ( ) : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 14:30:58 +02:00
}
if ( multiBodyB )
{
2019-01-03 14:26:51 +01:00
if ( solverConstraint . m_linkB < 0 )
2017-08-01 14:30:58 +02:00
{
rel_pos2 = pos2 - multiBodyB - > getBasePos ( ) ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
rel_pos2 = pos2 - multiBodyB - > getLink ( solverConstraint . m_linkB ) . m_cachedWorldTransform . getOrigin ( ) ;
}
2019-01-03 14:26:51 +01:00
const int ndofB = multiBodyB - > getNumDofs ( ) + 6 ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_deltaVelBindex = multiBodyB - > getCompanionId ( ) ;
2019-01-03 14:26:51 +01:00
if ( solverConstraint . m_deltaVelBindex < 0 )
2017-08-01 14:30:58 +02:00
{
solverConstraint . m_deltaVelBindex = m_data . m_deltaVelocities . size ( ) ;
multiBodyB - > setCompanionId ( solverConstraint . m_deltaVelBindex ) ;
2019-01-03 14:26:51 +01:00
m_data . m_deltaVelocities . resize ( m_data . m_deltaVelocities . size ( ) + ndofB ) ;
2017-08-01 14:30:58 +02:00
}
solverConstraint . m_jacBindex = m_data . m_jacobians . size ( ) ;
2019-01-03 14:26:51 +01:00
m_data . m_jacobians . resize ( m_data . m_jacobians . size ( ) + ndofB ) ;
m_data . m_deltaVelocitiesUnitImpulse . resize ( m_data . m_deltaVelocitiesUnitImpulse . size ( ) + ndofB ) ;
2017-08-01 14:30:58 +02:00
btAssert ( m_data . m_jacobians . size ( ) = = m_data . m_deltaVelocitiesUnitImpulse . size ( ) ) ;
multiBodyB - > fillContactJacobianMultiDof ( solverConstraint . m_linkB , cp . getPositionWorldOnB ( ) , - contactNormal , & m_data . m_jacobians [ solverConstraint . m_jacBindex ] , m_data . scratch_r , m_data . scratch_v , m_data . scratch_m ) ;
2019-01-03 14:26:51 +01:00
multiBodyB - > calcAccelerationDeltasMultiDof ( & m_data . m_jacobians [ solverConstraint . m_jacBindex ] , & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacBindex ] , m_data . scratch_r , m_data . scratch_v ) ;
btVector3 torqueAxis1 = rel_pos2 . cross ( contactNormal ) ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_relpos2CrossNormal = - torqueAxis1 ;
solverConstraint . m_contactNormal2 = - contactNormal ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
btVector3 torqueAxis1 = rel_pos2 . cross ( contactNormal ) ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_relpos2CrossNormal = - torqueAxis1 ;
solverConstraint . m_contactNormal2 = - contactNormal ;
2019-01-03 14:26:51 +01:00
solverConstraint . m_angularComponentB = rb1 ? rb1 - > getInvInertiaTensorWorld ( ) * - torqueAxis1 * rb1 - > getAngularFactor ( ) : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 14:30:58 +02:00
}
{
btVector3 vec ;
btScalar denom0 = 0.f ;
btScalar denom1 = 0.f ;
btScalar * jacB = 0 ;
btScalar * jacA = 0 ;
2019-01-03 14:26:51 +01:00
btScalar * lambdaA = 0 ;
btScalar * lambdaB = 0 ;
int ndofA = 0 ;
2017-08-01 14:30:58 +02:00
if ( multiBodyA )
{
2019-01-03 14:26:51 +01:00
ndofA = multiBodyA - > getNumDofs ( ) + 6 ;
2017-08-01 14:30:58 +02:00
jacA = & m_data . m_jacobians [ solverConstraint . m_jacAindex ] ;
lambdaA = & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacAindex ] ;
for ( int i = 0 ; i < ndofA ; + + i )
{
2019-01-03 14:26:51 +01:00
btScalar j = jacA [ i ] ;
btScalar l = lambdaA [ i ] ;
denom0 + = j * l ;
2017-08-01 14:30:58 +02:00
}
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
if ( rb0 )
{
2019-01-03 14:26:51 +01:00
vec = ( solverConstraint . m_angularComponentA ) . cross ( rel_pos1 ) ;
2017-08-01 14:30:58 +02:00
denom0 = rb0 - > getInvMass ( ) + contactNormal . dot ( vec ) ;
}
}
if ( multiBodyB )
{
2019-01-03 14:26:51 +01:00
const int ndofB = multiBodyB - > getNumDofs ( ) + 6 ;
2017-08-01 14:30:58 +02:00
jacB = & m_data . m_jacobians [ solverConstraint . m_jacBindex ] ;
lambdaB = & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacBindex ] ;
for ( int i = 0 ; i < ndofB ; + + i )
{
2019-01-03 14:26:51 +01:00
btScalar j = jacB [ i ] ;
btScalar l = lambdaB [ i ] ;
denom1 + = j * l ;
2017-08-01 14:30:58 +02:00
}
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
if ( rb1 )
{
2019-01-03 14:26:51 +01:00
vec = ( - solverConstraint . m_angularComponentB ) . cross ( rel_pos2 ) ;
2017-08-01 14:30:58 +02:00
denom1 = rb1 - > getInvMass ( ) + contactNormal . dot ( vec ) ;
}
}
2019-01-03 14:26:51 +01:00
btScalar d = denom0 + denom1 + cfm ;
if ( d > SIMD_EPSILON )
{
solverConstraint . m_jacDiagABInv = relaxation / ( d ) ;
}
else
{
2017-08-01 14:30:58 +02:00
//disable the constraint row to handle singularity/redundant constraint
2019-01-03 14:26:51 +01:00
solverConstraint . m_jacDiagABInv = 0.f ;
}
2017-08-01 14:30:58 +02:00
}
//compute rhs and remaining solverConstraint fields
btScalar restitution = 0.f ;
2019-01-03 14:26:51 +01:00
btScalar distance = 0 ;
if ( ! isFriction )
{
distance = cp . getDistance ( ) + infoGlobal . m_linearSlop ;
}
else
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
if ( cp . m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR )
{
distance = ( cp . getPositionWorldOnA ( ) - cp . getPositionWorldOnB ( ) ) . dot ( contactNormal ) ;
}
}
2017-08-01 14:30:58 +02:00
2019-01-03 14:26:51 +01:00
btScalar rel_vel = 0.f ;
int ndofA = 0 ;
int ndofB = 0 ;
{
btVector3 vel1 , vel2 ;
2017-08-01 14:30:58 +02:00
if ( multiBodyA )
{
2019-01-03 14:26:51 +01:00
ndofA = multiBodyA - > getNumDofs ( ) + 6 ;
2017-08-01 14:30:58 +02:00
btScalar * jacA = & m_data . m_jacobians [ solverConstraint . m_jacAindex ] ;
2019-01-03 14:26:51 +01:00
for ( int i = 0 ; i < ndofA ; + + i )
2017-08-01 14:30:58 +02:00
rel_vel + = multiBodyA - > getVelocityVector ( ) [ i ] * jacA [ i ] ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
if ( rb0 )
{
2019-01-03 14:26:51 +01:00
rel_vel + = ( rb0 - > getVelocityInLocalPoint ( rel_pos1 ) +
( rb0 - > getTotalTorque ( ) * rb0 - > getInvInertiaTensorWorld ( ) * infoGlobal . m_timeStep ) . cross ( rel_pos1 ) +
rb0 - > getTotalForce ( ) * rb0 - > getInvMass ( ) * infoGlobal . m_timeStep )
. dot ( solverConstraint . m_contactNormal1 ) ;
2017-08-01 14:30:58 +02:00
}
}
if ( multiBodyB )
{
2019-01-03 14:26:51 +01:00
ndofB = multiBodyB - > getNumDofs ( ) + 6 ;
2017-08-01 14:30:58 +02:00
btScalar * jacB = & m_data . m_jacobians [ solverConstraint . m_jacBindex ] ;
2019-01-03 14:26:51 +01:00
for ( int i = 0 ; i < ndofB ; + + i )
2017-08-01 14:30:58 +02:00
rel_vel + = multiBodyB - > getVelocityVector ( ) [ i ] * jacB [ i ] ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
if ( rb1 )
{
2019-01-03 14:26:51 +01:00
rel_vel + = ( rb1 - > getVelocityInLocalPoint ( rel_pos2 ) +
( rb1 - > getTotalTorque ( ) * rb1 - > getInvInertiaTensorWorld ( ) * infoGlobal . m_timeStep ) . cross ( rel_pos2 ) +
rb1 - > getTotalForce ( ) * rb1 - > getInvMass ( ) * infoGlobal . m_timeStep )
. dot ( solverConstraint . m_contactNormal2 ) ;
2017-08-01 14:30:58 +02:00
}
}
solverConstraint . m_friction = cp . m_combinedFriction ;
2019-01-03 14:26:51 +01:00
if ( ! isFriction )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
restitution = restitutionCurve ( rel_vel , cp . m_combinedRestitution , infoGlobal . m_restitutionVelocityThreshold ) ;
2017-08-01 14:30:58 +02:00
if ( restitution < = btScalar ( 0. ) )
{
restitution = 0.f ;
}
}
}
{
btScalar positionalError = 0.f ;
2019-01-03 14:26:51 +01:00
btScalar velocityError = restitution - rel_vel ; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
2017-08-01 14:30:58 +02:00
if ( isFriction )
{
2019-01-03 14:26:51 +01:00
positionalError = - distance * erp / infoGlobal . m_timeStep ;
}
else
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
if ( distance > 0 )
2017-08-01 14:30:58 +02:00
{
positionalError = 0 ;
velocityError - = distance / infoGlobal . m_timeStep ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
positionalError = - distance * erp / infoGlobal . m_timeStep ;
2017-08-01 14:30:58 +02:00
}
}
2019-01-03 14:26:51 +01:00
btScalar penetrationImpulse = positionalError * solverConstraint . m_jacDiagABInv ;
btScalar velocityImpulse = velocityError * solverConstraint . m_jacDiagABInv ;
2017-08-01 14:30:58 +02:00
2019-01-03 14:26:51 +01:00
if ( ! isFriction )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
// if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
2017-08-01 14:30:58 +02:00
{
//combine position and velocity into rhs
2019-01-03 14:26:51 +01:00
solverConstraint . m_rhs = penetrationImpulse + velocityImpulse ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_rhsPenetration = 0.f ;
}
2019-01-03 14:26:51 +01:00
/*else
2017-08-01 14:30:58 +02:00
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint . m_rhs = velocityImpulse ;
solverConstraint . m_rhsPenetration = penetrationImpulse ;
}
*/
solverConstraint . m_lowerLimit = 0 ;
solverConstraint . m_upperLimit = 1e10 f ;
}
else
{
2019-01-03 14:26:51 +01:00
solverConstraint . m_rhs = penetrationImpulse + velocityImpulse ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_rhsPenetration = 0.f ;
solverConstraint . m_lowerLimit = - solverConstraint . m_friction ;
solverConstraint . m_upperLimit = solverConstraint . m_friction ;
}
2019-01-03 14:26:51 +01:00
solverConstraint . m_cfm = cfm * solverConstraint . m_jacDiagABInv ;
2017-08-01 14:30:58 +02:00
}
2020-01-08 18:05:43 +01:00
if ( infoGlobal . m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING )
{
if ( btFabs ( cp . m_prevRHS ) > 1e-5 & & cp . m_prevRHS < 2 * solverConstraint . m_rhs & & solverConstraint . m_rhs < 2 * cp . m_prevRHS )
{
solverConstraint . m_appliedImpulse = isFriction ? 0 : cp . m_appliedImpulse / cp . m_prevRHS * solverConstraint . m_rhs * infoGlobal . m_articulatedWarmstartingFactor ;
if ( solverConstraint . m_appliedImpulse < 0 )
solverConstraint . m_appliedImpulse = 0 ;
}
else
{
solverConstraint . m_appliedImpulse = 0.f ;
}
if ( solverConstraint . m_appliedImpulse )
{
if ( multiBodyA )
{
btScalar impulse = solverConstraint . m_appliedImpulse ;
btScalar * deltaV = & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacAindex ] ;
multiBodyA - > applyDeltaVeeMultiDof2 ( deltaV , impulse ) ;
applyDeltaVee ( deltaV , impulse , solverConstraint . m_deltaVelAindex , ndofA ) ;
}
else
{
if ( rb0 )
bodyA - > internalApplyImpulse ( solverConstraint . m_contactNormal1 * bodyA - > internalGetInvMass ( ) * rb0 - > getLinearFactor ( ) , solverConstraint . m_angularComponentA , solverConstraint . m_appliedImpulse ) ;
}
if ( multiBodyB )
{
btScalar impulse = solverConstraint . m_appliedImpulse ;
btScalar * deltaV = & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacBindex ] ;
multiBodyB - > applyDeltaVeeMultiDof2 ( deltaV , impulse ) ;
applyDeltaVee ( deltaV , impulse , solverConstraint . m_deltaVelBindex , ndofB ) ;
}
else
{
if ( rb1 )
bodyB - > internalApplyImpulse ( - solverConstraint . m_contactNormal2 * bodyB - > internalGetInvMass ( ) * rb1 - > getLinearFactor ( ) , - solverConstraint . m_angularComponentB , - ( btScalar ) solverConstraint . m_appliedImpulse ) ;
}
}
}
else
{
solverConstraint . m_appliedImpulse = 0.f ;
solverConstraint . m_appliedPushImpulse = 0.f ;
}
2017-08-01 14:30:58 +02:00
}
void btMultiBodyConstraintSolver : : setupMultiBodyTorsionalFrictionConstraint ( btMultiBodySolverConstraint & solverConstraint ,
2019-01-03 14:26:51 +01:00
const btVector3 & constraintNormal ,
btManifoldPoint & cp ,
btScalar combinedTorsionalFriction ,
const btContactSolverInfo & infoGlobal ,
btScalar & relaxation ,
bool isFriction , btScalar desiredVelocity , btScalar cfmSlip )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
BT_PROFILE ( " setupMultiBodyRollingFrictionConstraint " ) ;
btVector3 rel_pos1 ;
btVector3 rel_pos2 ;
btMultiBody * multiBodyA = solverConstraint . m_multiBodyA ;
btMultiBody * multiBodyB = solverConstraint . m_multiBodyB ;
const btVector3 & pos1 = cp . getPositionWorldOnA ( ) ;
const btVector3 & pos2 = cp . getPositionWorldOnB ( ) ;
btSolverBody * bodyA = multiBodyA ? 0 : & m_tmpSolverBodyPool [ solverConstraint . m_solverBodyIdA ] ;
btSolverBody * bodyB = multiBodyB ? 0 : & m_tmpSolverBodyPool [ solverConstraint . m_solverBodyIdB ] ;
btRigidBody * rb0 = multiBodyA ? 0 : bodyA - > m_originalBody ;
btRigidBody * rb1 = multiBodyB ? 0 : bodyB - > m_originalBody ;
if ( bodyA )
rel_pos1 = pos1 - bodyA - > getWorldTransform ( ) . getOrigin ( ) ;
if ( bodyB )
rel_pos2 = pos2 - bodyB - > getWorldTransform ( ) . getOrigin ( ) ;
relaxation = infoGlobal . m_sor ;
// btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
if ( multiBodyA )
{
if ( solverConstraint . m_linkA < 0 )
{
rel_pos1 = pos1 - multiBodyA - > getBasePos ( ) ;
}
else
{
rel_pos1 = pos1 - multiBodyA - > getLink ( solverConstraint . m_linkA ) . m_cachedWorldTransform . getOrigin ( ) ;
}
const int ndofA = multiBodyA - > getNumDofs ( ) + 6 ;
solverConstraint . m_deltaVelAindex = multiBodyA - > getCompanionId ( ) ;
if ( solverConstraint . m_deltaVelAindex < 0 )
{
solverConstraint . m_deltaVelAindex = m_data . m_deltaVelocities . size ( ) ;
multiBodyA - > setCompanionId ( solverConstraint . m_deltaVelAindex ) ;
m_data . m_deltaVelocities . resize ( m_data . m_deltaVelocities . size ( ) + ndofA ) ;
}
else
{
btAssert ( m_data . m_deltaVelocities . size ( ) > = solverConstraint . m_deltaVelAindex + ndofA ) ;
}
solverConstraint . m_jacAindex = m_data . m_jacobians . size ( ) ;
m_data . m_jacobians . resize ( m_data . m_jacobians . size ( ) + ndofA ) ;
m_data . m_deltaVelocitiesUnitImpulse . resize ( m_data . m_deltaVelocitiesUnitImpulse . size ( ) + ndofA ) ;
btAssert ( m_data . m_jacobians . size ( ) = = m_data . m_deltaVelocitiesUnitImpulse . size ( ) ) ;
btScalar * jac1 = & m_data . m_jacobians [ solverConstraint . m_jacAindex ] ;
multiBodyA - > fillConstraintJacobianMultiDof ( solverConstraint . m_linkA , cp . getPositionWorldOnA ( ) , constraintNormal , btVector3 ( 0 , 0 , 0 ) , jac1 , m_data . scratch_r , m_data . scratch_v , m_data . scratch_m ) ;
btScalar * delta = & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacAindex ] ;
multiBodyA - > calcAccelerationDeltasMultiDof ( & m_data . m_jacobians [ solverConstraint . m_jacAindex ] , delta , m_data . scratch_r , m_data . scratch_v ) ;
2020-01-08 18:05:43 +01:00
btVector3 torqueAxis0 = constraintNormal ;
2019-01-03 14:26:51 +01:00
solverConstraint . m_relpos1CrossNormal = torqueAxis0 ;
solverConstraint . m_contactNormal1 = btVector3 ( 0 , 0 , 0 ) ;
}
else
{
2020-01-08 18:05:43 +01:00
btVector3 torqueAxis0 = constraintNormal ;
2019-01-03 14:26:51 +01:00
solverConstraint . m_relpos1CrossNormal = torqueAxis0 ;
solverConstraint . m_contactNormal1 = btVector3 ( 0 , 0 , 0 ) ;
solverConstraint . m_angularComponentA = rb0 ? rb0 - > getInvInertiaTensorWorld ( ) * torqueAxis0 * rb0 - > getAngularFactor ( ) : btVector3 ( 0 , 0 , 0 ) ;
}
if ( multiBodyB )
{
if ( solverConstraint . m_linkB < 0 )
{
rel_pos2 = pos2 - multiBodyB - > getBasePos ( ) ;
}
else
{
rel_pos2 = pos2 - multiBodyB - > getLink ( solverConstraint . m_linkB ) . m_cachedWorldTransform . getOrigin ( ) ;
}
const int ndofB = multiBodyB - > getNumDofs ( ) + 6 ;
solverConstraint . m_deltaVelBindex = multiBodyB - > getCompanionId ( ) ;
if ( solverConstraint . m_deltaVelBindex < 0 )
{
solverConstraint . m_deltaVelBindex = m_data . m_deltaVelocities . size ( ) ;
multiBodyB - > setCompanionId ( solverConstraint . m_deltaVelBindex ) ;
m_data . m_deltaVelocities . resize ( m_data . m_deltaVelocities . size ( ) + ndofB ) ;
}
solverConstraint . m_jacBindex = m_data . m_jacobians . size ( ) ;
m_data . m_jacobians . resize ( m_data . m_jacobians . size ( ) + ndofB ) ;
m_data . m_deltaVelocitiesUnitImpulse . resize ( m_data . m_deltaVelocitiesUnitImpulse . size ( ) + ndofB ) ;
btAssert ( m_data . m_jacobians . size ( ) = = m_data . m_deltaVelocitiesUnitImpulse . size ( ) ) ;
multiBodyB - > fillConstraintJacobianMultiDof ( solverConstraint . m_linkB , cp . getPositionWorldOnB ( ) , - constraintNormal , btVector3 ( 0 , 0 , 0 ) , & m_data . m_jacobians [ solverConstraint . m_jacBindex ] , m_data . scratch_r , m_data . scratch_v , m_data . scratch_m ) ;
multiBodyB - > calcAccelerationDeltasMultiDof ( & m_data . m_jacobians [ solverConstraint . m_jacBindex ] , & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacBindex ] , m_data . scratch_r , m_data . scratch_v ) ;
2020-01-08 18:05:43 +01:00
btVector3 torqueAxis1 = - constraintNormal ;
2019-01-03 14:26:51 +01:00
solverConstraint . m_relpos2CrossNormal = torqueAxis1 ;
solverConstraint . m_contactNormal2 = - btVector3 ( 0 , 0 , 0 ) ;
}
else
{
2020-01-08 18:05:43 +01:00
btVector3 torqueAxis1 = - constraintNormal ;
2019-01-03 14:26:51 +01:00
solverConstraint . m_relpos2CrossNormal = torqueAxis1 ;
solverConstraint . m_contactNormal2 = - btVector3 ( 0 , 0 , 0 ) ;
solverConstraint . m_angularComponentB = rb1 ? rb1 - > getInvInertiaTensorWorld ( ) * torqueAxis1 * rb1 - > getAngularFactor ( ) : btVector3 ( 0 , 0 , 0 ) ;
}
{
btScalar denom0 = 0.f ;
btScalar denom1 = 0.f ;
btScalar * jacB = 0 ;
btScalar * jacA = 0 ;
btScalar * lambdaA = 0 ;
btScalar * lambdaB = 0 ;
int ndofA = 0 ;
if ( multiBodyA )
{
ndofA = multiBodyA - > getNumDofs ( ) + 6 ;
jacA = & m_data . m_jacobians [ solverConstraint . m_jacAindex ] ;
lambdaA = & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacAindex ] ;
for ( int i = 0 ; i < ndofA ; + + i )
{
btScalar j = jacA [ i ] ;
btScalar l = lambdaA [ i ] ;
denom0 + = j * l ;
}
}
else
{
if ( rb0 )
{
btVector3 iMJaA = rb0 ? rb0 - > getInvInertiaTensorWorld ( ) * solverConstraint . m_relpos1CrossNormal : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 14:30:58 +02:00
denom0 = iMJaA . dot ( solverConstraint . m_relpos1CrossNormal ) ;
2019-01-03 14:26:51 +01:00
}
}
if ( multiBodyB )
{
const int ndofB = multiBodyB - > getNumDofs ( ) + 6 ;
jacB = & m_data . m_jacobians [ solverConstraint . m_jacBindex ] ;
lambdaB = & m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacBindex ] ;
for ( int i = 0 ; i < ndofB ; + + i )
{
btScalar j = jacB [ i ] ;
btScalar l = lambdaB [ i ] ;
denom1 + = j * l ;
}
}
else
{
if ( rb1 )
{
btVector3 iMJaB = rb1 ? rb1 - > getInvInertiaTensorWorld ( ) * solverConstraint . m_relpos2CrossNormal : btVector3 ( 0 , 0 , 0 ) ;
2017-08-01 14:30:58 +02:00
denom1 = iMJaB . dot ( solverConstraint . m_relpos2CrossNormal ) ;
2019-01-03 14:26:51 +01:00
}
}
btScalar d = denom0 + denom1 + infoGlobal . m_globalCfm ;
if ( d > SIMD_EPSILON )
{
solverConstraint . m_jacDiagABInv = relaxation / ( d ) ;
}
else
{
//disable the constraint row to handle singularity/redundant constraint
solverConstraint . m_jacDiagABInv = 0.f ;
}
}
//compute rhs and remaining solverConstraint fields
btScalar restitution = 0.f ;
btScalar penetration = isFriction ? 0 : cp . getDistance ( ) ;
btScalar rel_vel = 0.f ;
int ndofA = 0 ;
int ndofB = 0 ;
{
btVector3 vel1 , vel2 ;
if ( multiBodyA )
{
ndofA = multiBodyA - > getNumDofs ( ) + 6 ;
btScalar * jacA = & m_data . m_jacobians [ solverConstraint . m_jacAindex ] ;
for ( int i = 0 ; i < ndofA ; + + i )
rel_vel + = multiBodyA - > getVelocityVector ( ) [ i ] * jacA [ i ] ;
}
else
{
if ( rb0 )
{
2017-08-01 14:30:58 +02:00
btSolverBody * solverBodyA = & m_tmpSolverBodyPool [ solverConstraint . m_solverBodyIdA ] ;
2019-01-03 14:26:51 +01:00
rel_vel + = solverConstraint . m_contactNormal1 . dot ( rb0 ? solverBodyA - > m_linearVelocity + solverBodyA - > m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ) + solverConstraint . m_relpos1CrossNormal . dot ( rb0 ? solverBodyA - > m_angularVelocity : btVector3 ( 0 , 0 , 0 ) ) ;
}
}
if ( multiBodyB )
{
ndofB = multiBodyB - > getNumDofs ( ) + 6 ;
btScalar * jacB = & m_data . m_jacobians [ solverConstraint . m_jacBindex ] ;
for ( int i = 0 ; i < ndofB ; + + i )
rel_vel + = multiBodyB - > getVelocityVector ( ) [ i ] * jacB [ i ] ;
}
else
{
if ( rb1 )
{
2017-08-01 14:30:58 +02:00
btSolverBody * solverBodyB = & m_tmpSolverBodyPool [ solverConstraint . m_solverBodyIdB ] ;
2019-01-03 14:26:51 +01:00
rel_vel + = solverConstraint . m_contactNormal2 . dot ( rb1 ? solverBodyB - > m_linearVelocity + solverBodyB - > m_externalForceImpulse : btVector3 ( 0 , 0 , 0 ) ) + solverConstraint . m_relpos2CrossNormal . dot ( rb1 ? solverBodyB - > m_angularVelocity : btVector3 ( 0 , 0 , 0 ) ) ;
}
}
solverConstraint . m_friction = combinedTorsionalFriction ;
if ( ! isFriction )
{
restitution = restitutionCurve ( rel_vel , cp . m_combinedRestitution , infoGlobal . m_restitutionVelocityThreshold ) ;
if ( restitution < = btScalar ( 0. ) )
{
restitution = 0.f ;
}
}
}
solverConstraint . m_appliedImpulse = 0.f ;
solverConstraint . m_appliedPushImpulse = 0.f ;
{
btScalar velocityError = 0 - rel_vel ; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
btScalar velocityImpulse = velocityError * solverConstraint . m_jacDiagABInv ;
solverConstraint . m_rhs = velocityImpulse ;
solverConstraint . m_rhsPenetration = 0.f ;
solverConstraint . m_lowerLimit = - solverConstraint . m_friction ;
solverConstraint . m_upperLimit = solverConstraint . m_friction ;
solverConstraint . m_cfm = infoGlobal . m_globalCfm * solverConstraint . m_jacDiagABInv ;
}
2017-08-01 14:30:58 +02:00
}
2020-01-08 18:05:43 +01:00
btMultiBodySolverConstraint & btMultiBodyConstraintSolver : : addMultiBodyFrictionConstraint ( const btVector3 & normalAxis , const btScalar & appliedImpulse , btPersistentManifold * manifold , int frictionIndex , btManifoldPoint & cp , btCollisionObject * colObj0 , btCollisionObject * colObj1 , btScalar relaxation , const btContactSolverInfo & infoGlobal , btScalar desiredVelocity , btScalar cfmSlip )
2017-08-01 14:30:58 +02:00
{
BT_PROFILE ( " addMultiBodyFrictionConstraint " ) ;
btMultiBodySolverConstraint & solverConstraint = m_multiBodyFrictionContactConstraints . expandNonInitializing ( ) ;
2019-01-03 14:26:51 +01:00
solverConstraint . m_orgConstraint = 0 ;
solverConstraint . m_orgDofIndex = - 1 ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_frictionIndex = frictionIndex ;
bool isFriction = true ;
const btMultiBodyLinkCollider * fcA = btMultiBodyLinkCollider : : upcast ( manifold - > getBody0 ( ) ) ;
const btMultiBodyLinkCollider * fcB = btMultiBodyLinkCollider : : upcast ( manifold - > getBody1 ( ) ) ;
2019-01-03 14:26:51 +01:00
btMultiBody * mbA = fcA ? fcA - > m_multiBody : 0 ;
btMultiBody * mbB = fcB ? fcB - > m_multiBody : 0 ;
int solverBodyIdA = mbA ? - 1 : getOrInitSolverBody ( * colObj0 , infoGlobal . m_timeStep ) ;
int solverBodyIdB = mbB ? - 1 : getOrInitSolverBody ( * colObj1 , infoGlobal . m_timeStep ) ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_solverBodyIdA = solverBodyIdA ;
solverConstraint . m_solverBodyIdB = solverBodyIdB ;
solverConstraint . m_multiBodyA = mbA ;
if ( mbA )
solverConstraint . m_linkA = fcA - > m_link ;
solverConstraint . m_multiBodyB = mbB ;
if ( mbB )
solverConstraint . m_linkB = fcB - > m_link ;
solverConstraint . m_originalContactPoint = & cp ;
2020-01-08 18:05:43 +01:00
setupMultiBodyContactConstraint ( solverConstraint , normalAxis , 0 , cp , infoGlobal , relaxation , isFriction , desiredVelocity , cfmSlip ) ;
2017-08-01 14:30:58 +02:00
return solverConstraint ;
}
2019-01-03 14:26:51 +01:00
btMultiBodySolverConstraint & btMultiBodyConstraintSolver : : addMultiBodyTorsionalFrictionConstraint ( const btVector3 & normalAxis , btPersistentManifold * manifold , int frictionIndex , btManifoldPoint & cp ,
btScalar combinedTorsionalFriction ,
btCollisionObject * colObj0 , btCollisionObject * colObj1 , btScalar relaxation , const btContactSolverInfo & infoGlobal , btScalar desiredVelocity , btScalar cfmSlip )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
BT_PROFILE ( " addMultiBodyRollingFrictionConstraint " ) ;
bool useTorsionalAndConeFriction = ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS & & ( ( infoGlobal . m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION ) = = 0 ) ) ;
btMultiBodySolverConstraint & solverConstraint = useTorsionalAndConeFriction ? m_multiBodyTorsionalFrictionContactConstraints . expandNonInitializing ( ) : m_multiBodyFrictionContactConstraints . expandNonInitializing ( ) ;
solverConstraint . m_orgConstraint = 0 ;
solverConstraint . m_orgDofIndex = - 1 ;
solverConstraint . m_frictionIndex = frictionIndex ;
bool isFriction = true ;
const btMultiBodyLinkCollider * fcA = btMultiBodyLinkCollider : : upcast ( manifold - > getBody0 ( ) ) ;
const btMultiBodyLinkCollider * fcB = btMultiBodyLinkCollider : : upcast ( manifold - > getBody1 ( ) ) ;
btMultiBody * mbA = fcA ? fcA - > m_multiBody : 0 ;
btMultiBody * mbB = fcB ? fcB - > m_multiBody : 0 ;
int solverBodyIdA = mbA ? - 1 : getOrInitSolverBody ( * colObj0 , infoGlobal . m_timeStep ) ;
int solverBodyIdB = mbB ? - 1 : getOrInitSolverBody ( * colObj1 , infoGlobal . m_timeStep ) ;
solverConstraint . m_solverBodyIdA = solverBodyIdA ;
solverConstraint . m_solverBodyIdB = solverBodyIdB ;
solverConstraint . m_multiBodyA = mbA ;
if ( mbA )
solverConstraint . m_linkA = fcA - > m_link ;
solverConstraint . m_multiBodyB = mbB ;
if ( mbB )
solverConstraint . m_linkB = fcB - > m_link ;
solverConstraint . m_originalContactPoint = & cp ;
setupMultiBodyTorsionalFrictionConstraint ( solverConstraint , normalAxis , cp , combinedTorsionalFriction , infoGlobal , relaxation , isFriction , desiredVelocity , cfmSlip ) ;
return solverConstraint ;
2017-08-01 14:30:58 +02:00
}
2019-06-11 13:18:05 +02:00
btMultiBodySolverConstraint & btMultiBodyConstraintSolver : : addMultiBodySpinningFrictionConstraint ( const btVector3 & normalAxis , btPersistentManifold * manifold , int frictionIndex , btManifoldPoint & cp ,
btScalar combinedTorsionalFriction ,
btCollisionObject * colObj0 , btCollisionObject * colObj1 , btScalar relaxation , const btContactSolverInfo & infoGlobal , btScalar desiredVelocity , btScalar cfmSlip )
{
BT_PROFILE ( " addMultiBodyRollingFrictionConstraint " ) ;
btMultiBodySolverConstraint & solverConstraint = m_multiBodySpinningFrictionContactConstraints . expandNonInitializing ( ) ;
solverConstraint . m_orgConstraint = 0 ;
solverConstraint . m_orgDofIndex = - 1 ;
solverConstraint . m_frictionIndex = frictionIndex ;
bool isFriction = true ;
const btMultiBodyLinkCollider * fcA = btMultiBodyLinkCollider : : upcast ( manifold - > getBody0 ( ) ) ;
const btMultiBodyLinkCollider * fcB = btMultiBodyLinkCollider : : upcast ( manifold - > getBody1 ( ) ) ;
btMultiBody * mbA = fcA ? fcA - > m_multiBody : 0 ;
btMultiBody * mbB = fcB ? fcB - > m_multiBody : 0 ;
int solverBodyIdA = mbA ? - 1 : getOrInitSolverBody ( * colObj0 , infoGlobal . m_timeStep ) ;
int solverBodyIdB = mbB ? - 1 : getOrInitSolverBody ( * colObj1 , infoGlobal . m_timeStep ) ;
solverConstraint . m_solverBodyIdA = solverBodyIdA ;
solverConstraint . m_solverBodyIdB = solverBodyIdB ;
solverConstraint . m_multiBodyA = mbA ;
if ( mbA )
solverConstraint . m_linkA = fcA - > m_link ;
solverConstraint . m_multiBodyB = mbB ;
if ( mbB )
solverConstraint . m_linkB = fcB - > m_link ;
solverConstraint . m_originalContactPoint = & cp ;
setupMultiBodyTorsionalFrictionConstraint ( solverConstraint , normalAxis , cp , combinedTorsionalFriction , infoGlobal , relaxation , isFriction , desiredVelocity , cfmSlip ) ;
return solverConstraint ;
}
2019-01-03 14:26:51 +01:00
void btMultiBodyConstraintSolver : : convertMultiBodyContact ( btPersistentManifold * manifold , const btContactSolverInfo & infoGlobal )
2017-08-01 14:30:58 +02:00
{
const btMultiBodyLinkCollider * fcA = btMultiBodyLinkCollider : : upcast ( manifold - > getBody0 ( ) ) ;
const btMultiBodyLinkCollider * fcB = btMultiBodyLinkCollider : : upcast ( manifold - > getBody1 ( ) ) ;
2019-01-03 14:26:51 +01:00
btMultiBody * mbA = fcA ? fcA - > m_multiBody : 0 ;
btMultiBody * mbB = fcB ? fcB - > m_multiBody : 0 ;
btCollisionObject * colObj0 = 0 , * colObj1 = 0 ;
2017-08-01 14:30:58 +02:00
colObj0 = ( btCollisionObject * ) manifold - > getBody0 ( ) ;
colObj1 = ( btCollisionObject * ) manifold - > getBody1 ( ) ;
2019-01-03 14:26:51 +01:00
int solverBodyIdA = mbA ? - 1 : getOrInitSolverBody ( * colObj0 , infoGlobal . m_timeStep ) ;
int solverBodyIdB = mbB ? - 1 : getOrInitSolverBody ( * colObj1 , infoGlobal . m_timeStep ) ;
2017-08-01 14:30:58 +02:00
2019-01-03 14:26:51 +01:00
// btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
// btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
2017-08-01 14:30:58 +02:00
///avoid collision response between two static objects
2019-01-03 14:26:51 +01:00
// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
2017-08-01 14:30:58 +02:00
// return;
2019-01-03 14:26:51 +01:00
//only a single rollingFriction per manifold
int rollingFriction = 1 ;
2017-08-01 14:30:58 +02:00
2019-01-03 14:26:51 +01:00
for ( int j = 0 ; j < manifold - > getNumContacts ( ) ; j + + )
{
2017-08-01 14:30:58 +02:00
btManifoldPoint & cp = manifold - > getContactPoint ( j ) ;
if ( cp . getDistance ( ) < = manifold - > getContactProcessingThreshold ( ) )
{
btScalar relaxation ;
int frictionIndex = m_multiBodyNormalContactConstraints . size ( ) ;
btMultiBodySolverConstraint & solverConstraint = m_multiBodyNormalContactConstraints . expandNonInitializing ( ) ;
2019-01-03 14:26:51 +01:00
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint . m_orgConstraint = 0 ;
solverConstraint . m_orgDofIndex = - 1 ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_solverBodyIdA = solverBodyIdA ;
solverConstraint . m_solverBodyIdB = solverBodyIdB ;
solverConstraint . m_multiBodyA = mbA ;
if ( mbA )
solverConstraint . m_linkA = fcA - > m_link ;
solverConstraint . m_multiBodyB = mbB ;
if ( mbB )
solverConstraint . m_linkB = fcB - > m_link ;
solverConstraint . m_originalContactPoint = & cp ;
bool isFriction = false ;
2020-01-08 18:05:43 +01:00
setupMultiBodyContactConstraint ( solverConstraint , cp . m_normalWorldOnB , cp . m_appliedImpulse , cp , infoGlobal , relaxation , isFriction ) ;
2017-08-01 14:30:58 +02:00
2019-01-03 14:26:51 +01:00
// const btVector3& pos1 = cp.getPositionWorldOnA();
// const btVector3& pos2 = cp.getPositionWorldOnB();
2017-08-01 14:30:58 +02:00
/////setup the friction constraints
# define ENABLE_FRICTION
# ifdef ENABLE_FRICTION
2019-01-03 14:26:51 +01:00
solverConstraint . m_frictionIndex = m_multiBodyFrictionContactConstraints . size ( ) ;
2017-08-01 14:30:58 +02:00
///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically every frame
///based on the relative linear velocity.
///If the relative velocity is zero, it will automatically compute a friction direction.
2019-01-03 14:26:51 +01:00
2017-08-01 14:30:58 +02:00
///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.
///
2019-01-03 14:26:51 +01:00
///The user can manually override the friction directions for certain contacts using a contact callback,
2017-08-01 14:30:58 +02:00
///and set the cp.m_lateralFrictionInitialized to true
///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 14:26:51 +01:00
btPlaneSpace1 ( cp . m_normalWorldOnB , cp . m_lateralFrictionDir1 , cp . m_lateralFrictionDir2 ) ;
2017-08-01 14:30:58 +02:00
cp . m_lateralFrictionDir1 . normalize ( ) ;
cp . m_lateralFrictionDir2 . normalize ( ) ;
2019-01-03 14:26:51 +01:00
if ( rollingFriction > 0 )
{
if ( cp . m_combinedSpinningFriction > 0 )
{
2019-06-11 13:18:05 +02:00
addMultiBodySpinningFrictionConstraint ( cp . m_normalWorldOnB , manifold , frictionIndex , cp , cp . m_combinedSpinningFriction , colObj0 , colObj1 , relaxation , infoGlobal ) ;
2019-01-03 14:26:51 +01:00
}
if ( cp . m_combinedRollingFriction > 0 )
{
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_ROLLING_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_ROLLING_FRICTION ) ;
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_ROLLING_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_ROLLING_FRICTION ) ;
2019-06-11 13:18:05 +02:00
addMultiBodyTorsionalFrictionConstraint ( cp . m_lateralFrictionDir1 , manifold , frictionIndex , cp , cp . m_combinedRollingFriction , colObj0 , colObj1 , relaxation , infoGlobal ) ;
addMultiBodyTorsionalFrictionConstraint ( cp . m_lateralFrictionDir2 , manifold , frictionIndex , cp , cp . m_combinedRollingFriction , colObj0 , colObj1 , relaxation , infoGlobal ) ;
2019-01-03 14:26:51 +01:00
}
rollingFriction - - ;
}
if ( ! ( infoGlobal . m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING ) | | ! ( cp . m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ) )
{ /*
2017-08-01 14:30:58 +02: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 )
{
cp . m_lateralFrictionDir1 * = 1.f / btSqrt ( lat_rel_vel ) ;
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
{
cp . m_lateralFrictionDir2 = cp . m_lateralFrictionDir1 . cross ( cp . m_normalWorldOnB ) ;
cp . m_lateralFrictionDir2 . normalize ( ) ; //??
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
addMultiBodyFrictionConstraint ( cp . m_lateralFrictionDir2 , solverBodyIdA , solverBodyIdB , frictionIndex , cp , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation ) ;
}
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
addMultiBodyFrictionConstraint ( cp . m_lateralFrictionDir1 , solverBodyIdA , solverBodyIdB , frictionIndex , cp , rel_pos1 , rel_pos2 , colObj0 , colObj1 , relaxation ) ;
} else
*/
{
2019-01-03 14:26:51 +01:00
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir1 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
2020-01-08 18:05:43 +01:00
addMultiBodyFrictionConstraint ( cp . m_lateralFrictionDir1 , cp . m_appliedImpulseLateral1 , manifold , frictionIndex , cp , colObj0 , colObj1 , relaxation , infoGlobal ) ;
2017-08-01 14:30:58 +02:00
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
{
2019-01-03 14:26:51 +01:00
applyAnisotropicFriction ( colObj0 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
applyAnisotropicFriction ( colObj1 , cp . m_lateralFrictionDir2 , btCollisionObject : : CF_ANISOTROPIC_FRICTION ) ;
2020-01-08 18:05:43 +01:00
addMultiBodyFrictionConstraint ( cp . m_lateralFrictionDir2 , cp . m_appliedImpulseLateral2 , manifold , frictionIndex , cp , colObj0 , colObj1 , relaxation , infoGlobal ) ;
2017-08-01 14:30:58 +02:00
}
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) & & ( infoGlobal . m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) )
{
2019-01-03 14:26:51 +01:00
cp . m_contactPointFlags | = BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ;
2017-08-01 14:30:58 +02:00
}
}
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
2020-01-08 18:05:43 +01:00
addMultiBodyFrictionConstraint ( cp . m_lateralFrictionDir1 , cp . m_appliedImpulseLateral1 , manifold , frictionIndex , cp , colObj0 , colObj1 , relaxation , infoGlobal , cp . m_contactMotion1 , cp . m_frictionCFM ) ;
2017-08-01 14:30:58 +02:00
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
2020-01-08 18:05:43 +01:00
addMultiBodyFrictionConstraint ( cp . m_lateralFrictionDir2 , cp . m_appliedImpulseLateral2 , manifold , frictionIndex , cp , colObj0 , colObj1 , relaxation , infoGlobal , cp . m_contactMotion2 , cp . m_frictionCFM ) ;
2017-08-01 14:30:58 +02:00
solverConstraint . m_appliedImpulse = 0.f ;
solverConstraint . m_appliedPushImpulse = 0.f ;
2020-01-08 18:05:43 +01:00
}
2017-08-01 14:30:58 +02:00
2019-01-03 14:26:51 +01:00
# endif //ENABLE_FRICTION
2017-08-01 14:30:58 +02:00
}
2020-01-08 18:05:43 +01:00
else
{
// Reset quantities related to warmstart as 0.
cp . m_appliedImpulse = 0 ;
cp . m_prevRHS = 0 ;
}
2017-08-01 14:30:58 +02:00
}
}
2019-01-03 14:26:51 +01:00
void btMultiBodyConstraintSolver : : convertContacts ( btPersistentManifold * * manifoldPtr , int numManifolds , const btContactSolverInfo & infoGlobal )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
for ( int i = 0 ; i < numManifolds ; i + + )
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
btPersistentManifold * manifold = manifoldPtr [ i ] ;
2017-08-01 14:30:58 +02:00
const btMultiBodyLinkCollider * fcA = btMultiBodyLinkCollider : : upcast ( manifold - > getBody0 ( ) ) ;
const btMultiBodyLinkCollider * fcB = btMultiBodyLinkCollider : : upcast ( manifold - > getBody1 ( ) ) ;
if ( ! fcA & & ! fcB )
{
//the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
2019-01-03 14:26:51 +01:00
convertContact ( manifold , infoGlobal ) ;
}
else
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
convertMultiBodyContact ( manifold , infoGlobal ) ;
2017-08-01 14:30:58 +02:00
}
}
//also convert the multibody constraints, if any
2019-01-03 14:26:51 +01:00
for ( int i = 0 ; i < m_tmpNumMultiBodyConstraints ; i + + )
2017-08-01 14:30:58 +02:00
{
btMultiBodyConstraint * c = m_tmpMultiBodyConstraints [ i ] ;
m_data . m_solverBodyPool = & m_tmpSolverBodyPool ;
m_data . m_fixedBodyId = m_fixedBodyId ;
2019-01-03 14:26:51 +01:00
c - > createConstraintRows ( m_multiBodyNonContactConstraints , m_data , infoGlobal ) ;
}
2020-01-08 18:05:43 +01:00
// Warmstart for noncontact constraints
if ( infoGlobal . m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING )
{
for ( int i = 0 ; i < m_multiBodyNonContactConstraints . size ( ) ; i + + )
{
btMultiBodySolverConstraint & solverConstraint =
m_multiBodyNonContactConstraints [ i ] ;
solverConstraint . m_appliedImpulse =
solverConstraint . m_orgConstraint - > getAppliedImpulse ( solverConstraint . m_orgDofIndex ) *
infoGlobal . m_articulatedWarmstartingFactor ;
btMultiBody * multiBodyA = solverConstraint . m_multiBodyA ;
btMultiBody * multiBodyB = solverConstraint . m_multiBodyB ;
if ( solverConstraint . m_appliedImpulse )
{
if ( multiBodyA )
{
int ndofA = multiBodyA - > getNumDofs ( ) + 6 ;
btScalar * deltaV =
& m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacAindex ] ;
btScalar impulse = solverConstraint . m_appliedImpulse ;
multiBodyA - > applyDeltaVeeMultiDof2 ( deltaV , impulse ) ;
applyDeltaVee ( deltaV , impulse , solverConstraint . m_deltaVelAindex , ndofA ) ;
}
if ( multiBodyB )
{
int ndofB = multiBodyB - > getNumDofs ( ) + 6 ;
btScalar * deltaV =
& m_data . m_deltaVelocitiesUnitImpulse [ solverConstraint . m_jacBindex ] ;
btScalar impulse = solverConstraint . m_appliedImpulse ;
multiBodyB - > applyDeltaVeeMultiDof2 ( deltaV , impulse ) ;
applyDeltaVee ( deltaV , impulse , solverConstraint . m_deltaVelBindex , ndofB ) ;
}
}
}
}
else
{
for ( int i = 0 ; i < m_multiBodyNonContactConstraints . size ( ) ; i + + )
{
btMultiBodySolverConstraint & solverConstraint = m_multiBodyNonContactConstraints [ i ] ;
solverConstraint . m_appliedImpulse = 0 ;
}
}
2017-08-01 14:30:58 +02:00
}
2019-01-03 14:26:51 +01:00
btScalar btMultiBodyConstraintSolver : : solveGroup ( btCollisionObject * * bodies , int numBodies , btPersistentManifold * * manifold , int numManifolds , btTypedConstraint * * constraints , int numConstraints , const btContactSolverInfo & info , btIDebugDraw * debugDrawer , btDispatcher * dispatcher )
2017-08-01 14:30:58 +02:00
{
2018-09-07 16:11:04 +02:00
//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
2019-01-03 14:26:51 +01:00
return btSequentialImpulseConstraintSolver : : solveGroup ( bodies , numBodies , manifold , numManifolds , constraints , numConstraints , info , debugDrawer , dispatcher ) ;
2017-08-01 14:30:58 +02:00
}
#if 0
static void applyJointFeedback ( btMultiBodyJacobianData & data , const btMultiBodySolverConstraint & solverConstraint , int jacIndex , btMultiBody * mb , btScalar appliedImpulse )
{
if ( appliedImpulse ! = 0 & & mb - > internalNeedsJointFeedback ( ) )
{
//todo: get rid of those temporary memory allocations for the joint feedback
btAlignedObjectArray < btScalar > forceVector ;
int numDofsPlusBase = 6 + mb - > getNumDofs ( ) ;
forceVector . resize ( numDofsPlusBase ) ;
for ( int i = 0 ; i < numDofsPlusBase ; i + + )
{
forceVector [ i ] = data . m_jacobians [ jacIndex + i ] * appliedImpulse ;
}
btAlignedObjectArray < btScalar > output ;
output . resize ( numDofsPlusBase ) ;
bool applyJointFeedback = true ;
mb - > calcAccelerationDeltasMultiDof ( & forceVector [ 0 ] , & output [ 0 ] , data . scratch_r , data . scratch_v , applyJointFeedback ) ;
}
}
# endif
void btMultiBodyConstraintSolver : : writeBackSolverBodyToMultiBody ( btMultiBodySolverConstraint & c , btScalar deltaTime )
{
2019-01-03 14:26:51 +01:00
# if 1
2017-08-01 14:30:58 +02:00
//bod->addBaseForce(m_gravity * bod->getBaseMass());
//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
if ( c . m_orgConstraint )
{
2019-01-03 14:26:51 +01:00
c . m_orgConstraint - > internalSetAppliedImpulse ( c . m_orgDofIndex , c . m_appliedImpulse ) ;
2017-08-01 14:30:58 +02:00
}
if ( c . m_multiBodyA )
{
c . m_multiBodyA - > setCompanionId ( - 1 ) ;
2019-01-03 14:26:51 +01:00
btVector3 force = c . m_contactNormal1 * ( c . m_appliedImpulse / deltaTime ) ;
btVector3 torque = c . m_relpos1CrossNormal * ( c . m_appliedImpulse / deltaTime ) ;
if ( c . m_linkA < 0 )
2017-08-01 14:30:58 +02:00
{
c . m_multiBodyA - > addBaseConstraintForce ( force ) ;
c . m_multiBodyA - > addBaseConstraintTorque ( torque ) ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
2019-01-03 14:26:51 +01:00
c . m_multiBodyA - > addLinkConstraintForce ( c . m_linkA , force ) ;
//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
c . m_multiBodyA - > addLinkConstraintTorque ( c . m_linkA , torque ) ;
2017-08-01 14:30:58 +02:00
}
}
2019-01-03 14:26:51 +01:00
2017-08-01 14:30:58 +02:00
if ( c . m_multiBodyB )
{
{
c . m_multiBodyB - > setCompanionId ( - 1 ) ;
2019-01-03 14:26:51 +01:00
btVector3 force = c . m_contactNormal2 * ( c . m_appliedImpulse / deltaTime ) ;
btVector3 torque = c . m_relpos2CrossNormal * ( c . m_appliedImpulse / deltaTime ) ;
if ( c . m_linkB < 0 )
2017-08-01 14:30:58 +02:00
{
c . m_multiBodyB - > addBaseConstraintForce ( force ) ;
c . m_multiBodyB - > addBaseConstraintTorque ( torque ) ;
2019-01-03 14:26:51 +01:00
}
else
2017-08-01 14:30:58 +02:00
{
{
2019-01-03 14:26:51 +01:00
c . m_multiBodyB - > addLinkConstraintForce ( c . m_linkB , force ) ;
2017-08-01 14:30:58 +02:00
//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
2019-01-03 14:26:51 +01:00
c . m_multiBodyB - > addLinkConstraintTorque ( c . m_linkB , torque ) ;
2017-08-01 14:30:58 +02:00
}
}
}
}
# endif
# ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
if ( c . m_multiBodyA )
{
2019-01-03 14:26:51 +01:00
c . m_multiBodyA - > applyDeltaVeeMultiDof ( & m_data . m_deltaVelocitiesUnitImpulse [ c . m_jacAindex ] , c . m_appliedImpulse ) ;
2017-08-01 14:30:58 +02:00
}
2019-01-03 14:26:51 +01:00
2017-08-01 14:30:58 +02:00
if ( c . m_multiBodyB )
{
2019-01-03 14:26:51 +01:00
c . m_multiBodyB - > applyDeltaVeeMultiDof ( & m_data . m_deltaVelocitiesUnitImpulse [ c . m_jacBindex ] , c . m_appliedImpulse ) ;
2017-08-01 14:30:58 +02:00
}
# endif
}
2019-01-03 14:26:51 +01:00
btScalar btMultiBodyConstraintSolver : : solveGroupCacheFriendlyFinish ( btCollisionObject * * bodies , int numBodies , const btContactSolverInfo & infoGlobal )
2017-08-01 14:30:58 +02:00
{
BT_PROFILE ( " btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish " ) ;
int numPoolConstraints = m_multiBodyNormalContactConstraints . size ( ) ;
2019-01-03 14:26:51 +01:00
//write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
2017-08-01 14:30:58 +02:00
//or as applied force, so we can measure the joint reaction forces easier
2019-01-03 14:26:51 +01:00
for ( int i = 0 ; i < numPoolConstraints ; i + + )
2017-08-01 14:30:58 +02:00
{
btMultiBodySolverConstraint & solverConstraint = m_multiBodyNormalContactConstraints [ i ] ;
2019-01-03 14:26:51 +01:00
writeBackSolverBodyToMultiBody ( solverConstraint , infoGlobal . m_timeStep ) ;
2017-08-01 14:30:58 +02:00
2019-01-03 14:26:51 +01:00
writeBackSolverBodyToMultiBody ( m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] , infoGlobal . m_timeStep ) ;
2017-08-01 14:30:58 +02:00
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
{
2019-01-03 14:26:51 +01:00
writeBackSolverBodyToMultiBody ( m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] , infoGlobal . m_timeStep ) ;
2017-08-01 14:30:58 +02:00
}
}
2019-01-03 14:26:51 +01:00
for ( int i = 0 ; i < m_multiBodyNonContactConstraints . size ( ) ; i + + )
2017-08-01 14:30:58 +02:00
{
btMultiBodySolverConstraint & solverConstraint = m_multiBodyNonContactConstraints [ i ] ;
2019-01-03 14:26:51 +01:00
writeBackSolverBodyToMultiBody ( solverConstraint , infoGlobal . m_timeStep ) ;
2017-08-01 14:30:58 +02:00
}
2020-01-08 18:05:43 +01:00
2017-08-01 14:30:58 +02:00
{
BT_PROFILE ( " warm starting write back " ) ;
2019-01-03 14:26:51 +01:00
for ( int j = 0 ; j < numPoolConstraints ; j + + )
2017-08-01 14:30:58 +02:00
{
const btMultiBodySolverConstraint & solverConstraint = m_multiBodyNormalContactConstraints [ j ] ;
2019-01-03 14:26:51 +01:00
btManifoldPoint * pt = ( btManifoldPoint * ) solverConstraint . m_originalContactPoint ;
2017-08-01 14:30:58 +02:00
btAssert ( pt ) ;
pt - > m_appliedImpulse = solverConstraint . m_appliedImpulse ;
2020-01-08 18:05:43 +01:00
pt - > m_prevRHS = solverConstraint . m_rhs ;
2017-08-01 14:30:58 +02:00
pt - > m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_appliedImpulse ;
2019-01-03 14:26:51 +01:00
2017-08-01 14:30:58 +02:00
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
{
2019-01-03 14:26:51 +01:00
pt - > m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_appliedImpulse ;
} else
{
pt - > m_appliedImpulseLateral2 = 0 ;
2017-08-01 14:30:58 +02:00
}
}
}
2020-01-08 18:05:43 +01:00
2017-08-01 14:30:58 +02:00
#if 0
//multibody joint feedback
{
BT_PROFILE ( " multi body joint feedback " ) ;
for ( int j = 0 ; j < numPoolConstraints ; j + + )
{
const btMultiBodySolverConstraint & solverConstraint = m_multiBodyNormalContactConstraints [ j ] ;
//apply the joint feedback into all links of the btMultiBody
//todo: double-check the signs of the applied impulse
if ( solverConstraint . m_multiBodyA & & solverConstraint . m_multiBodyA - > isMultiDof ( ) )
{
applyJointFeedback ( m_data , solverConstraint , solverConstraint . m_jacAindex , solverConstraint . m_multiBodyA , solverConstraint . m_appliedImpulse * btSimdScalar ( 1. / infoGlobal . m_timeStep ) ) ;
}
if ( solverConstraint . m_multiBodyB & & solverConstraint . m_multiBodyB - > isMultiDof ( ) )
{
applyJointFeedback ( m_data , solverConstraint , solverConstraint . m_jacBindex , solverConstraint . m_multiBodyB , solverConstraint . m_appliedImpulse * btSimdScalar ( - 1. / infoGlobal . m_timeStep ) ) ;
}
#if 0
if ( m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_multiBodyA & & m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_multiBodyA - > isMultiDof ( ) )
{
applyJointFeedback ( m_data , m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_jacAindex ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_multiBodyA ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_appliedImpulse * btSimdScalar ( 1. / infoGlobal . m_timeStep ) ) ;
}
if ( m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_multiBodyB & & m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_multiBodyB - > isMultiDof ( ) )
{
applyJointFeedback ( m_data , m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_jacBindex ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_multiBodyB ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex ] . m_appliedImpulse * btSimdScalar ( - 1. / infoGlobal . m_timeStep ) ) ;
}
if ( ( infoGlobal . m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) )
{
if ( m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_multiBodyA & & m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_multiBodyA - > isMultiDof ( ) )
{
applyJointFeedback ( m_data , m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_jacAindex ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_multiBodyA ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_appliedImpulse * btSimdScalar ( 1. / infoGlobal . m_timeStep ) ) ;
}
if ( m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_multiBodyB & & m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_multiBodyB - > isMultiDof ( ) )
{
applyJointFeedback ( m_data , m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_jacBindex ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_multiBodyB ,
m_multiBodyFrictionContactConstraints [ solverConstraint . m_frictionIndex + 1 ] . m_appliedImpulse * btSimdScalar ( - 1. / infoGlobal . m_timeStep ) ) ;
}
}
# endif
}
for ( int i = 0 ; i < m_multiBodyNonContactConstraints . size ( ) ; i + + )
{
const btMultiBodySolverConstraint & solverConstraint = m_multiBodyNonContactConstraints [ i ] ;
if ( solverConstraint . m_multiBodyA & & solverConstraint . m_multiBodyA - > isMultiDof ( ) )
{
applyJointFeedback ( m_data , solverConstraint , solverConstraint . m_jacAindex , solverConstraint . m_multiBodyA , solverConstraint . m_appliedImpulse * btSimdScalar ( 1. / infoGlobal . m_timeStep ) ) ;
}
if ( solverConstraint . m_multiBodyB & & solverConstraint . m_multiBodyB - > isMultiDof ( ) )
{
applyJointFeedback ( m_data , solverConstraint , solverConstraint . m_jacBindex , solverConstraint . m_multiBodyB , solverConstraint . m_appliedImpulse * btSimdScalar ( 1. / infoGlobal . m_timeStep ) ) ;
}
}
}
numPoolConstraints = m_multiBodyNonContactConstraints . size ( ) ;
#if 0
//@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
for ( int i = 0 ; i < numPoolConstraints ; i + + )
{
const btMultiBodySolverConstraint & c = m_multiBodyNonContactConstraints [ i ] ;
btTypedConstraint * constr = ( btTypedConstraint * ) c . m_originalContactPoint ;
btJointFeedback * fb = constr - > getJointFeedback ( ) ;
if ( fb )
{
fb - > m_appliedForceBodyA + = c . m_contactNormal1 * c . m_appliedImpulse * constr - > getRigidBodyA ( ) . getLinearFactor ( ) / infoGlobal . m_timeStep ;
fb - > m_appliedForceBodyB + = c . m_contactNormal2 * c . m_appliedImpulse * constr - > getRigidBodyB ( ) . getLinearFactor ( ) / infoGlobal . m_timeStep ;
fb - > m_appliedTorqueBodyA + = c . m_relpos1CrossNormal * constr - > getRigidBodyA ( ) . getAngularFactor ( ) * c . m_appliedImpulse / infoGlobal . m_timeStep ;
fb - > m_appliedTorqueBodyB + = c . m_relpos2CrossNormal * constr - > getRigidBodyB ( ) . getAngularFactor ( ) * c . m_appliedImpulse / infoGlobal . m_timeStep ; /*RGM ???? */
}
constr - > internalSetAppliedImpulse ( c . m_appliedImpulse ) ;
if ( btFabs ( c . m_appliedImpulse ) > = constr - > getBreakingImpulseThreshold ( ) )
{
constr - > setEnabled ( false ) ;
}
}
2019-01-03 14:26:51 +01:00
# endif
2017-08-01 14:30:58 +02:00
# endif
2019-01-03 14:26:51 +01:00
return btSequentialImpulseConstraintSolver : : solveGroupCacheFriendlyFinish ( bodies , numBodies , infoGlobal ) ;
2017-08-01 14:30:58 +02:00
}
2019-01-03 14:26:51 +01:00
void btMultiBodyConstraintSolver : : solveMultiBodyGroup ( btCollisionObject * * bodies , int numBodies , btPersistentManifold * * manifold , int numManifolds , btTypedConstraint * * constraints , int numConstraints , btMultiBodyConstraint * * multiBodyConstraints , int numMultiBodyConstraints , const btContactSolverInfo & info , btIDebugDraw * debugDrawer , btDispatcher * dispatcher )
2017-08-01 14:30:58 +02:00
{
2018-09-07 16:11:04 +02:00
//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
2017-08-01 14:30:58 +02:00
//printf("solveMultiBodyGroup start\n");
m_tmpMultiBodyConstraints = multiBodyConstraints ;
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints ;
2019-01-03 14:26:51 +01:00
btSequentialImpulseConstraintSolver : : solveGroup ( bodies , numBodies , manifold , numManifolds , constraints , numConstraints , info , debugDrawer , dispatcher ) ;
2017-08-01 14:30:58 +02:00
m_tmpMultiBodyConstraints = 0 ;
m_tmpNumMultiBodyConstraints = 0 ;
}