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

4
src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
View File

@@ -56,7 +56,7 @@ public:
m_contactPointFlags(0),
m_appliedImpulse(0.f),
m_prevRHS(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral2(0.f),
m_contactMotion1(0.f),
m_contactMotion2(0.f),
@@ -117,7 +117,7 @@ public:
btScalar m_appliedImpulse;
btScalar m_prevRHS;
btScalar m_appliedImpulseLateral1;
btScalar m_appliedImpulseLateral1;
btScalar m_appliedImpulseLateral2;
btScalar m_contactMotion1;
btScalar m_contactMotion2;

2
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
View File

@@ -326,7 +326,7 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma
const btManifoldPoint& pt = manifold->getContactPoint(i);
dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS;
dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);

2
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -280,7 +280,7 @@ struct btPersistentManifoldDoubleData
double m_pointCacheDistance[4];
double m_pointCacheAppliedImpulse[4];
double m_pointCachePrevRHS[4];
double m_pointCacheCombinedFriction[4];
double m_pointCacheCombinedFriction[4];
double m_pointCacheCombinedRollingFriction[4];
double m_pointCacheCombinedSpinningFriction[4];
double m_pointCacheCombinedRestitution[4];

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);
}