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

add SOLVER_USE_ARTICULATED_WARMSTARTING option and APIs

Browse Source 
fix compile
This commit is contained in:
Chuyuan Fu
2019-10-17 16:59:15 -07:00
parent ac680be673
commit 913400eba1
10 changed files with 33 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -752,6 +752,14 @@ B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandl
return 0;
}
B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_articulatedWarmStartingFactor = warmStartingFactor;
command->m_updateFlags |= SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -339,6 +339,7 @@ extern "C"
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);

6
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -9338,6 +9338,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = clientCmd.m_physSimParamArgs.m_warmStartingFactor;
}
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR)
{
m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_USE_ARTICULATED_WARMSTARTING;
m_data->m_dynamicsWorld->getSolverInfo().m_articulatedWarmstartingFactor = clientCmd.m_physSimParamArgs.m_articulatedWarmStartingFactor;
}
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
return hasStatus;

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -483,6 +483,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 1 << 25,
SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26,
SIM_PARAM_UPDATE_WARM_STARTING_FACTOR = 1 << 27,
SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR = 1 << 28,
};

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -947,6 +947,7 @@ struct b3PhysicsSimulationParameters
int m_numSimulationSubSteps;
int m_numSolverIterations;
double m_warmStartingFactor;
double m_articulatedWarmStartingFactor;
int m_useRealTimeSimulation;
int m_useSplitImpulse;
double m_splitImpulsePenetrationThreshold;

8
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -30,7 +30,8 @@ enum btSolverMode
SOLVER_SIMD = 256,
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024,
SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048
SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048,
SOLVER_USE_ARTICULATED_WARMSTARTING = 4096,
};
struct btContactSolverInfoData
@@ -54,7 +55,7 @@ struct btContactSolverInfoData
btScalar m_splitImpulseTurnErp;
btScalar m_linearSlop;
btScalar m_warmstartingFactor;
btScalar m_articulatedWarmstartingFactor;
int m_solverMode;
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
@@ -89,6 +90,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_splitImpulseTurnErp = 0.1f;
m_linearSlop = btScalar(0.0);
m_warmstartingFactor = btScalar(0.85);
m_articulatedWarmstartingFactor = btScalar(0);
//m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD; // | SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2; //unused as of 2.81
@@ -120,6 +122,7 @@ struct btContactSolverInfoDoubleData
double m_splitImpulseTurnErp;
double m_linearSlop;
double m_warmstartingFactor;
double m_articulatedWarmstartingFactor;
double m_maxGyroscopicForce; ///it is only used for 'explicit' version of gyroscopic force
double m_singleAxisRollingFrictionThreshold;
@@ -150,6 +153,7 @@ struct btContactSolverInfoFloatData
float m_linearSlop;
float m_warmstartingFactor;
float m_articulatedWarmstartingFactor;
float m_maxGyroscopicForce;
float m_singleAxisRollingFrictionThreshold;

30
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -835,7 +835,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
{
if (btFabs(cp.m_prevRHS) > 1e-5)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_warmstartingFactor;
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;
}
@@ -1438,7 +1438,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
}
// Warmstart for noncontact constraints
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
{
for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
{
@@ -1446,7 +1446,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
m_multiBodyNonContactConstraints[i];
solverConstraint.m_appliedImpulse =
solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
infoGlobal.m_warmstartingFactor;
infoGlobal.m_articulatedWarmstartingFactor;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
@@ -1603,28 +1603,10 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
}
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
{
BT_PROFILE("warm starting write back");
for (int j = 0; j < numPoolConstraints; j++)
{
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse;
} else
{
pt->m_appliedImpulseLateral2 = 0;
}
}
//do a callback here?
writeBackMultiBodyContacts(0, numPoolConstraints);
}
#if 0
//multibody joint feedback
@@ -1725,7 +1707,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
}
#endif
#endif
writeBackMultiBodyContacts(0, numPoolConstraints);
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
}