summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp')
-rw-r--r--thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp207
1 files changed, 104 insertions, 103 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
index c8cc47923e..631fd5fbed 100644
--- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
+++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
@@ -13,131 +13,132 @@
3. This notice may not be removed or altered from any source distribution.
*/
-
#include "btDeformableMultiBodyConstraintSolver.h"
#include <iostream>
// override the iterations method to include deformable/multibody contact
-btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies,int numBodies,btCollisionObject** deformableBodies,int numDeformableBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
- {
- ///this is a special step to resolve penetrations (just for contacts)
- solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+ {
+ ///this is a special step to resolve penetrations (just for contacts)
+ solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+
+ int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
+ for (int iteration = 0; iteration < maxIterations; iteration++)
+ {
+ // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
- int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
- for (int iteration = 0; iteration < maxIterations; iteration++)
- {
- // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
-
- // solve rigid/rigid in solver body
- m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
- // solver body velocity -> rigid body velocity
- solverBodyWriteBack(infoGlobal);
- btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal);
- // update rigid body velocity in rigid/deformable contact
- m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
- // solver body velocity <- rigid body velocity
- writeToSolverBody(bodies, numBodies, infoGlobal);
-
- if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
- {
+ // solve rigid/rigid in solver body
+ m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+ // solver body velocity -> rigid body velocity
+ solverBodyWriteBack(infoGlobal);
+ btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies, numDeformableBodies, infoGlobal);
+ // update rigid body velocity in rigid/deformable contact
+ m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
+ // solver body velocity <- rigid body velocity
+ writeToSolverBody(bodies, numBodies, infoGlobal);
+
+ if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+ {
#ifdef VERBOSE_RESIDUAL_PRINTF
- printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+ if (iteration >= (maxIterations - 1))
+ printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
#endif
- m_analyticsData.m_numSolverCalls++;
- m_analyticsData.m_numIterationsUsed = iteration+1;
- m_analyticsData.m_islandId = -2;
- if (numBodies>0)
- m_analyticsData.m_islandId = bodies[0]->getCompanionId();
- m_analyticsData.m_numBodies = numBodies;
- m_analyticsData.m_numContactManifolds = numManifolds;
- m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
- break;
- }
- }
- }
- return 0.f;
+ m_analyticsData.m_numSolverCalls++;
+ m_analyticsData.m_numIterationsUsed = iteration + 1;
+ m_analyticsData.m_islandId = -2;
+ if (numBodies > 0)
+ m_analyticsData.m_islandId = bodies[0]->getCompanionId();
+ m_analyticsData.m_numBodies = numBodies;
+ m_analyticsData.m_numContactManifolds = numManifolds;
+ m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
+ break;
+ }
+ }
+ }
+ return 0.f;
}
-void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject * *bodies, int numBodies, btCollisionObject * *deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
{
- m_tmpMultiBodyConstraints = multiBodyConstraints;
- m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
-
- // inherited from MultiBodyConstraintSolver
- solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
-
- // overriden
- solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
-
- // inherited from MultiBodyConstraintSolver
- solveGroupCacheFriendlyFinish(bodies, numBodies, info);
-
- m_tmpMultiBodyConstraints = 0;
- m_tmpNumMultiBodyConstraints = 0;
+ m_tmpMultiBodyConstraints = multiBodyConstraints;
+ m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
+
+ // inherited from MultiBodyConstraintSolver
+ solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+ // overriden
+ solveDeformableGroupIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+ // inherited from MultiBodyConstraintSolver
+ solveGroupCacheFriendlyFinish(bodies, numBodies, info);
+
+ m_tmpMultiBodyConstraints = 0;
+ m_tmpNumMultiBodyConstraints = 0;
}
void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
{
- for (int i = 0; i < numBodies; i++)
- {
- int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
+ for (int i = 0; i < numBodies; i++)
+ {
+ int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
- btRigidBody* body = btRigidBody::upcast(bodies[i]);
- if (body && body->getInvMass())
- {
- btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
- solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
- solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
- }
- }
+ btRigidBody* body = btRigidBody::upcast(bodies[i]);
+ if (body && body->getInvMass())
+ {
+ btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+ solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
+ solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
+ }
+ }
}
void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
{
- for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
- {
- btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
- if (body)
- {
- m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
- m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
- }
- }
+ for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+ {
+ btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
+ if (body)
+ {
+ m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
+ m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
+ }
+ }
}
-void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
{
- BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
- int iteration;
- if (infoGlobal.m_splitImpulse)
- {
- {
-// m_deformableSolver->splitImpulseSetup(infoGlobal);
- for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
- {
- btScalar leastSquaresResidual = 0.f;
- {
- int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
- int j;
- for (j = 0; j < numPoolConstraints; j++)
- {
- const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-
- btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
- leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
- }
- // solve the position correction between deformable and rigid/multibody
-// btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
-// leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
- }
- if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
- {
+ BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
+ int iteration;
+ if (infoGlobal.m_splitImpulse)
+ {
+ {
+ for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
+ {
+ btScalar leastSquaresResidual = 0.f;
+ {
+ int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+ int j;
+ for (j = 0; j < numPoolConstraints; j++)
+ {
+ const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+
+ btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+ leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+ }
+ // solve the position correction between deformable and rigid/multibody
+ // btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
+ btScalar residual = m_deformableSolver->m_objective->m_projection.solveSplitImpulse(deformableBodies, numDeformableBodies, infoGlobal);
+ leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+ }
+ if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
+ {
#ifdef VERBOSE_RESIDUAL_PRINTF
- printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
+ if (iteration >= (infoGlobal.m_numIterations - 1))
+ printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
#endif
- break;
- }
- }
- }
- }
+ break;
+ }
+ }
+ }
+ }
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-18 03:52:26 +0000