Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1219 from lunkhound/pr-nncg

Browse Source 
NNCG solver: apply rolling friction consistently
This commit is contained in:
erwincoumans
2017-08-18 13:31:54 -07:00
committed by GitHub
parent b940a83797 b7f35000aa
commit a4f28e1589
2 changed files with 118 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

133
examples/MultiThreadedDemo/MultiThreadedDemo.cpp
View File

@@ -13,28 +13,35 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btBulletDynamicsCommon.h" #include "MultiThreadedDemo.h"
#include "LinearMath/btQuickprof.h" #include "CommonRigidBodyMTBase.h"
#include "LinearMath/btIDebugDraw.h"
#include "../CommonInterfaces/CommonParameterInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h"
#include "btBulletDynamicsCommon.h"
#include "btBulletCollisionCommon.h"
#include "LinearMath/btAlignedObjectArray.h"
#include <stdio.h> //printf debugging #include <stdio.h> //printf debugging
#include <algorithm> #include <algorithm>
class btCollisionShape;
#include "CommonRigidBodyMTBase.h"
#include "MultiThreadedDemo.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btBulletCollisionCommon.h"
static btScalar gSliderStackRows = 8.0f; static btScalar gSliderStackRows = 8.0f;
static btScalar gSliderStackColumns = 6.0f; static btScalar gSliderStackColumns = 6.0f;
static btScalar gSliderStackHeight = 10.0f; static btScalar gSliderStackHeight = 10.0f;
static btScalar gSliderStackWidth = 1.0f;
static btScalar gSliderGroundHorizontalAmplitude = 0.0f; static btScalar gSliderGroundHorizontalAmplitude = 0.0f;
static btScalar gSliderGroundVerticalAmplitude = 0.0f; static btScalar gSliderGroundVerticalAmplitude = 0.0f;
static btScalar gSliderGroundTilt = 0.0f;
static btScalar gSliderRollingFriction = 0.0f;
static bool gSpheresNotBoxes = false;
static void boolPtrButtonCallback( int buttonId, bool buttonState, void* userPointer )
{
if ( bool* val = static_cast<bool*>( userPointer ) )
{
*val = !*val;
}
}
/// MultiThreadedDemo shows how to setup and use multithreading /// MultiThreadedDemo shows how to setup and use multithreading
class MultiThreadedDemo : public CommonRigidBodyMTBase class MultiThreadedDemo : public CommonRigidBodyMTBase
@@ -50,8 +57,9 @@ class MultiThreadedDemo : public CommonRigidBodyMTBase
btRigidBody* m_groundBody; btRigidBody* m_groundBody;
btTransform m_groundStartXf; btTransform m_groundStartXf;
float m_groundMovePhase; float m_groundMovePhase;
float m_prevRollingFriction;
void createStack( const btVector3& pos, btCollisionShape* boxShape, const btVector3& halfBoxSize, int size ); void createStack( const btTransform& trans, btCollisionShape* boxShape, const btVector3& halfBoxSize, int size, int width );
void createSceneObjects(); void createSceneObjects();
void destroySceneObjects(); void destroySceneObjects();
@@ -62,10 +70,25 @@ public:
virtual ~MultiThreadedDemo() {} virtual ~MultiThreadedDemo() {}
btQuaternion getGroundRotation() const
{
btScalar tilt = gSliderGroundTilt * SIMD_2_PI / 360.0f;
return btQuaternion( btVector3( 1.0f, 0.0f, 0.0f ), tilt );
}
virtual void stepSimulation( float deltaTime ) BT_OVERRIDE virtual void stepSimulation( float deltaTime ) BT_OVERRIDE
{ {
if ( m_dynamicsWorld ) if ( m_dynamicsWorld )
{ {
if ( m_prevRollingFriction != gSliderRollingFriction )
{
m_prevRollingFriction = gSliderRollingFriction;
btCollisionObjectArray& objArray = m_dynamicsWorld->getCollisionObjectArray();
for ( int i = 0; i < objArray.size(); ++i )
{
btCollisionObject* obj = objArray[ i ];
obj->setRollingFriction( gSliderRollingFriction );
}
}
if (m_groundBody) if (m_groundBody)
{ {
// update ground // update ground
@@ -85,6 +108,7 @@ public:
offset[kUpAxis] = gndVOffset; offset[kUpAxis] = gndVOffset;
vel[kUpAxis] = gndVVel; vel[kUpAxis] = gndVVel;
xf.setOrigin(xf.getOrigin() + offset); xf.setOrigin(xf.getOrigin() + offset);
xf.setRotation( getGroundRotation() );
m_groundBody->setWorldTransform( xf ); m_groundBody->setWorldTransform( xf );
m_groundBody->setLinearVelocity( vel ); m_groundBody->setLinearVelocity( vel );
} }
@@ -117,6 +141,7 @@ MultiThreadedDemo::MultiThreadedDemo(struct GUIHelperInterface* helper)
m_cameraPitch = -30.0f; m_cameraPitch = -30.0f;
m_cameraYaw = 90.0f; m_cameraYaw = 90.0f;
m_cameraDist = 48.0f; m_cameraDist = 48.0f;
m_prevRollingFriction = -1.0f;
helper->setUpAxis( kUpAxis ); helper->setUpAxis( kUpAxis );
} }
@@ -134,6 +159,13 @@ void MultiThreadedDemo::initPhysics()
slider.m_clampToIntegers = true; slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider ); m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
} }
{
SliderParams slider( "Stack width", &gSliderStackWidth );
slider.m_minVal = 1.0f;
slider.m_maxVal = 30.0f;
slider.m_clampToIntegers = true;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{ {
SliderParams slider( "Stack rows", &gSliderStackRows ); SliderParams slider( "Stack rows", &gSliderStackRows );
slider.m_minVal = 1.0f; slider.m_minVal = 1.0f;
@@ -164,6 +196,29 @@ void MultiThreadedDemo::initPhysics()
slider.m_clampToNotches = false; slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider ); m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
} }
{
// ground tilt
SliderParams slider( "Ground tilt", &gSliderGroundTilt );
slider.m_minVal = -45.0f;
slider.m_maxVal = 45.0f;
slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
// rolling friction
SliderParams slider( "Rolling friction", &gSliderRollingFriction );
slider.m_minVal = 0.0f;
slider.m_maxVal = 1.0f;
slider.m_clampToNotches = false;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
}
{
ButtonParams button( "Spheres not boxes", 0, false );
button.m_initialState = gSpheresNotBoxes;
button.m_userPointer = &gSpheresNotBoxes;
button.m_callback = boolPtrButtonCallback;
m_guiHelper->getParameterInterface()->registerButtonParameter( button );
}
createSceneObjects(); createSceneObjects();
@@ -184,29 +239,36 @@ btRigidBody* MultiThreadedDemo::localCreateRigidBody(btScalar mass, const btTran
} }
void MultiThreadedDemo::createStack( const btVector3& center, btCollisionShape* boxShape, const btVector3& halfBoxSize, int size ) void MultiThreadedDemo::createStack( const btTransform& parentTrans, btCollisionShape* boxShape, const btVector3& halfBoxSize, int height, int width )
{ {
btTransform trans; btTransform trans;
trans.setIdentity(); trans.setIdentity();
trans.setRotation( parentTrans.getRotation() );
float halfBoxHeight = halfBoxSize.y(); float halfBoxHeight = halfBoxSize.y();
float halfBoxWidth = halfBoxSize.x(); float halfBoxWidth = halfBoxSize.x();
for ( int i = 0; i<size; i++ ) btVector3 offset = btVector3( 0, 0, -halfBoxSize.z() * (width - 1) );
for ( int iZ = 0; iZ < width; iZ++ )
{ {
// This constructs a row, from left to right offset += btVector3( 0, 0, halfBoxSize.z()*2.0f );
int rowSize = size - i; for ( int iY = 0; iY < height; iY++ )
for ( int j = 0; j< rowSize; j++ )
{ {
btVector3 pos = center + btVector3( halfBoxWidth*( 1 + j * 2 - rowSize ), // This constructs a row, from left to right
halfBoxHeight * ( 1 + i * 2), int rowSize = height - iY;
0.0f for ( int iX = 0; iX < rowSize; iX++ )
{
btVector3 pos = offset + btVector3( halfBoxWidth*( 1 + iX * 2 - rowSize ),
halfBoxHeight * ( 1 + iY * 2 ),
0.0f
); );
trans.setOrigin( pos ); trans.setOrigin( parentTrans(pos) );
btScalar mass = 1.f; btScalar mass = 1.f;
btRigidBody* body = localCreateRigidBody( mass, trans, boxShape ); btRigidBody* body = localCreateRigidBody( mass, trans, boxShape );
body->setFriction(1.0f); body->setFriction( 1.0f );
body->setRollingFriction( gSliderRollingFriction );
}
} }
} }
} }
@@ -216,10 +278,8 @@ void MultiThreadedDemo::createSceneObjects()
{ {
{ {
// create ground box // create ground box
btTransform tr; m_groundStartXf.setOrigin( btVector3( 0.f, -3.f, 0.f ) );
tr.setIdentity(); m_groundStartXf.setRotation( getGroundRotation() );
tr.setOrigin( btVector3( 0.f, -3.f, 0.f ) );
m_groundStartXf = tr;
//either use heightfield or triangle mesh //either use heightfield or triangle mesh
@@ -240,18 +300,33 @@ void MultiThreadedDemo::createSceneObjects()
int numStackRows = btMax(1, int(gSliderStackRows)); int numStackRows = btMax(1, int(gSliderStackRows));
int numStackCols = btMax(1, int(gSliderStackColumns)); int numStackCols = btMax(1, int(gSliderStackColumns));
int stackHeight = int(gSliderStackHeight); int stackHeight = int(gSliderStackHeight);
float stackZSpacing = 3.0f; int stackWidth = int( gSliderStackWidth );
float stackZSpacing = 2.0f + stackWidth*halfExtents.x()*2.0f;
float stackXSpacing = 20.0f; float stackXSpacing = 20.0f;
btBoxShape* boxShape = new btBoxShape( halfExtents ); btBoxShape* boxShape = new btBoxShape( halfExtents );
m_collisionShapes.push_back( boxShape ); m_collisionShapes.push_back( boxShape );
btSphereShape* sphereShape = new btSphereShape( 0.5f );
m_collisionShapes.push_back( sphereShape );
btCollisionShape* shape = boxShape;
if ( gSpheresNotBoxes )
{
shape = sphereShape;
}
btTransform groundTrans;
groundTrans.setIdentity();
groundTrans.setRotation( getGroundRotation() );
for ( int iX = 0; iX < numStackCols; ++iX ) for ( int iX = 0; iX < numStackCols; ++iX )
{ {
for ( int iZ = 0; iZ < numStackRows; ++iZ ) for ( int iZ = 0; iZ < numStackRows; ++iZ )
{ {
btVector3 center = btVector3( iX * stackXSpacing, 0.0f, ( iZ - numStackRows / 2 ) * stackZSpacing ); btVector3 center = btVector3( iX * stackXSpacing, 0.0f, ( iZ - numStackRows / 2 ) * stackZSpacing );
createStack( center, boxShape, halfExtents, stackHeight ); btTransform trans = groundTrans;
trans.setOrigin( groundTrans( center ) );
createStack( trans, shape, halfExtents, stackHeight, stackWidth );
} }
} }
} }

115
src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
View File

@@ -78,20 +78,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
btScalar deltaflengthsqr = 0; btScalar deltaflengthsqr = 0;
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
m_deltafNC[j] = deltaf;
deltaflengthsqr += deltaf * deltaf;
}
}
} else
{ {
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{ {
@@ -141,7 +127,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{ {
if (iteration< infoGlobal.m_numIterations) if (iteration< infoGlobal.m_numIterations)
@@ -158,7 +143,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
} }
} }
///solve all contact constraints using SIMD, if available ///solve all contact constraints
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
{ {
int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
@@ -170,7 +155,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
{ {
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[c] = deltaf; m_deltafC[c] = deltaf;
deltaflengthsqr += deltaf*deltaf; deltaflengthsqr += deltaf*deltaf;
totalImpulse = solveManifold.m_appliedImpulse; totalImpulse = solveManifold.m_appliedImpulse;
@@ -186,7 +171,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
{ {
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[c*multiplier] = deltaf; m_deltafCF[c*multiplier] = deltaf;
deltaflengthsqr += deltaf*deltaf; deltaflengthsqr += deltaf*deltaf;
} else { } else {
@@ -203,7 +188,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
{ {
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[c*multiplier+1] = deltaf; m_deltafCF[c*multiplier+1] = deltaf;
deltaflengthsqr += deltaf*deltaf; deltaflengthsqr += deltaf*deltaf;
} else { } else {
@@ -223,7 +208,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
for (j=0;j<numPoolConstraints;j++) for (j=0;j<numPoolConstraints;j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[j] = deltaf; m_deltafC[j] = deltaf;
deltaflengthsqr += deltaf*deltaf; deltaflengthsqr += deltaf*deltaf;
@@ -231,7 +215,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
///solve all friction constraints, using SIMD, if available ///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++) for (j=0;j<numFrictionPoolConstraints;j++)
@@ -244,7 +228,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[j] = deltaf; m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf; deltaflengthsqr += deltaf*deltaf;
@@ -252,10 +235,11 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
m_deltafCF[j] = 0; m_deltafCF[j] = 0;
} }
} }
}
{
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (j=0;j<numRollingFrictionPoolConstraints;j++) for (int j=0;j<numRollingFrictionPoolConstraints;j++)
{ {
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
@@ -269,87 +253,19 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
m_deltafCRF[j] = deltaf; m_deltafCRF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf; deltaflengthsqr += deltaf*deltaf;
} else { } else {
m_deltafCRF[j] = 0; m_deltafCRF[j] = 0;
} }
} }
}
}
} }
} else
{
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
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];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
for (int j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (int j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[j] = 0;
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (int j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
m_deltafCRF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCRF[j] = 0;
}
}
}
} }
@@ -362,10 +278,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j]; for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j]; for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j]; for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
{
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
}
} else } else
{ {
// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation // deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
@@ -374,9 +287,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0; for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0; for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0; for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) { for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
}
} else { } else {
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{ {
@@ -420,7 +331,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
} }
} }
if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) { {
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
{ {
btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];