AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues 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
commit a4f28e1589
2 changed files with 118 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

135
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.
*/
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btIDebugDraw.h"
#include "MultiThreadedDemo.h"
#include "CommonRigidBodyMTBase.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "btBulletDynamicsCommon.h"
#include "btBulletCollisionCommon.h"
#include "LinearMath/btAlignedObjectArray.h"
#include <stdio.h> //printf debugging
#include <algorithm>
class btCollisionShape;
#include "CommonRigidBodyMTBase.h"
#include "MultiThreadedDemo.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btBulletCollisionCommon.h"
static btScalar gSliderStackRows = 8.0f;
static btScalar gSliderStackColumns = 6.0f;
static btScalar gSliderStackHeight = 10.0f;
static btScalar gSliderStackWidth = 1.0f;
static btScalar gSliderGroundHorizontalAmplitude = 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
class MultiThreadedDemo : public CommonRigidBodyMTBase
@ -50,8 +57,9 @@ class MultiThreadedDemo : public CommonRigidBodyMTBase
btRigidBody* m_groundBody;
btTransform m_groundStartXf;
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 destroySceneObjects();
@ -62,10 +70,25 @@ public:
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
{
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)
{
// update ground
@ -85,6 +108,7 @@ public:
offset[kUpAxis] = gndVOffset;
vel[kUpAxis] = gndVVel;
xf.setOrigin(xf.getOrigin() + offset);
xf.setRotation( getGroundRotation() );
m_groundBody->setWorldTransform( xf );
m_groundBody->setLinearVelocity( vel );
}
@ -117,6 +141,7 @@ MultiThreadedDemo::MultiThreadedDemo(struct GUIHelperInterface* helper)
m_cameraPitch = -30.0f;
m_cameraYaw = 90.0f;
m_cameraDist = 48.0f;
m_prevRollingFriction = -1.0f;
helper->setUpAxis( kUpAxis );
}
@ -134,6 +159,13 @@ void MultiThreadedDemo::initPhysics()
slider.m_clampToIntegers = true;
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 );
slider.m_minVal = 1.0f;
@ -164,7 +196,30 @@ void MultiThreadedDemo::initPhysics()
slider.m_clampToNotches = false;
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();
m_guiHelper->createPhysicsDebugDrawer( m_dynamicsWorld );
@ -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;
trans.setIdentity();
trans.setRotation( parentTrans.getRotation() );
float halfBoxHeight = halfBoxSize.y();
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
int rowSize = size - i;
for ( int j = 0; j< rowSize; j++ )
offset += btVector3( 0, 0, halfBoxSize.z()*2.0f );
for ( int iY = 0; iY < height; iY++ )
{
btVector3 pos = center + btVector3( halfBoxWidth*( 1 + j * 2 - rowSize ),
halfBoxHeight * ( 1 + i * 2),
0.0f
// This constructs a row, from left to right
int rowSize = height - iY;
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 );
btScalar mass = 1.f;
trans.setOrigin( parentTrans(pos) );
btScalar mass = 1.f;
btRigidBody* body = localCreateRigidBody( mass, trans, boxShape );
body->setFriction(1.0f);
btRigidBody* body = localCreateRigidBody( mass, trans, boxShape );
body->setFriction( 1.0f );
body->setRollingFriction( gSliderRollingFriction );
}
}
}
}
@ -216,10 +278,8 @@ void MultiThreadedDemo::createSceneObjects()
{
{
// create ground box
btTransform tr;
tr.setIdentity();
tr.setOrigin( btVector3( 0.f, -3.f, 0.f ) );
m_groundStartXf = tr;
m_groundStartXf.setOrigin( btVector3( 0.f, -3.f, 0.f ) );
m_groundStartXf.setRotation( getGroundRotation() );
//either use heightfield or triangle mesh
@ -240,18 +300,33 @@ void MultiThreadedDemo::createSceneObjects()
int numStackRows = btMax(1, int(gSliderStackRows));
int numStackCols = btMax(1, int(gSliderStackColumns));
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;
btBoxShape* boxShape = new btBoxShape( halfExtents );
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 iZ = 0; iZ < numStackRows; ++iZ )
{
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;
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++)
{
@ -141,7 +127,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
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)
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
@ -170,7 +155,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
{
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;
deltaflengthsqr += deltaf*deltaf;
totalImpulse = solveManifold.m_appliedImpulse;
@ -186,7 +171,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
{
solveManifold.m_lowerLimit = -(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;
deltaflengthsqr += deltaf*deltaf;
} else {
@ -203,7 +188,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
{
solveManifold.m_lowerLimit = -(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;
deltaflengthsqr += deltaf*deltaf;
} else {
@ -223,7 +208,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
for (j=0;j<numPoolConstraints;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);
m_deltafC[j] = 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();
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_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);
m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
@ -252,10 +235,11 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
m_deltafCF[j] = 0;
}
}
}
{
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (j=0;j<numRollingFrictionPoolConstraints;j++)
for (int j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
@ -269,87 +253,19 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision
rollingFrictionConstraint.m_lowerLimit = -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;
deltaflengthsqr += deltaf*deltaf;
} else {
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_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[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
{
// 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_tmpSolverContactConstraintPool.size();j++) m_pC[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 {
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);
}
}
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++)
{
btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];