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

separate external force solve from constraint solve and eliminate damping in external force solve

Browse Source
This commit is contained in:
Xuchen Han
2019-07-10 16:08:46 -07:00
parent c4e316f005
commit 77d670ae41
2 changed files with 295 additions and 282 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -420,6 +420,66 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
}
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
solveExternalForces(solverInfo);
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b = 0; b < bod->getNumLinks(); b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks() + 1);
m_scratch_m.resize(bod->getNumLinks() + 1);
if (bod->internalNeedsJointFeedback())
{
if (!bod->isUsingRK4Integration())
{
if (bod->internalNeedsJointFeedback())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
}
}
}
}
}
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
{
forwardKinematics();
@@ -514,10 +574,15 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
if (!bod->isUsingRK4Integration())
{
const btScalar linearDamp = bod->getLinearDamping();
// const btScalar angularDamp = bod->getAngularDamping();
bod->setLinearDamping(0);
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
bod->setLinearDamping(linearDamp);
// bod->setAngularDamping(angularDamp);
}
else
{
@@ -703,63 +768,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
} //if (!isSleeping)
}
}
/// solve all the constraints for this island
m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bool isSleeping = false;
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
{
isSleeping = true;
}
for (int b = 0; b < bod->getNumLinks(); b++)
{
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
isSleeping = true;
}
if (!isSleeping)
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v.resize(bod->getNumLinks() + 1);
m_scratch_m.resize(bod->getNumLinks() + 1);
if (bod->internalNeedsJointFeedback())
{
if (!bod->isUsingRK4Integration())
{
if (bod->internalNeedsJointFeedback())
{
bool isConstraintPass = true;
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
getSolverInfo().m_jointFeedbackInWorldSpace,
getSolverInfo().m_jointFeedbackInJointFrame);
}
}
}
}
}
}
for (int i = 0; i < this->m_multiBodies.size(); i++)
{
btMultiBody* bod = m_multiBodies[i];
bod->processDeltaVeeMultiDof2();
}
}
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
btDiscreteDynamicsWorld::integrateTransforms(timeStep);

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -112,5 +112,7 @@ public:
virtual void setConstraintSolver(btConstraintSolver* solver);
virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
virtual void solveExternalForces(btContactSolverInfo& solverInfo);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H