diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index f9c527b3c..084ff5a04 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -122,7 +122,7 @@ void DeformableMultibody::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); - deformableBodySolver->setWorld(getDeformableDynamicsWorld()); +// deformableBodySolver->setWorld(getDeformableDynamicsWorld()); btVector3 gravity = btVector3(0, -10, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index f6b0c3184..9b546086d 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -164,7 +164,7 @@ void DeformableRigid::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); - deformableBodySolver->setWorld(getDeformableDynamicsWorld()); +// deformableBodySolver->setWorld(getDeformableDynamicsWorld()); // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality btVector3 gravity = btVector3(0, -10, 0); m_dynamicsWorld->setGravity(gravity); diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 10b273a2e..d07c3cb01 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -190,7 +190,7 @@ void GraspDeformable::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); - deformableBodySolver->setWorld(getDeformableDynamicsWorld()); +// deformableBodySolver->setWorld(getDeformableDynamicsWorld()); btVector3 gravity = btVector3(0, -9.81, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; @@ -357,6 +357,7 @@ void GraspDeformable::initPhysics() // linear damping getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.5,0.04, true)); +// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(3,0.04, true)); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); getDeformableDynamicsWorld()->addForce(psb, new btDeformableNeoHookeanForce(2,10)); } diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index 7bfbc7712..e1c3fd348 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -248,7 +248,7 @@ void Pinch::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); - deformableBodySolver->setWorld(getDeformableDynamicsWorld()); +// deformableBodySolver->setWorld(getDeformableDynamicsWorld()); // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality btVector3 gravity = btVector3(0, -10, 0); m_dynamicsWorld->setGravity(gravity); diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index 55331b5d5..9796ab963 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -182,7 +182,7 @@ void VolumetricDeformable::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); - deformableBodySolver->setWorld(getDeformableDynamicsWorld()); +// deformableBodySolver->setWorld(getDeformableDynamicsWorld()); // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality btVector3 gravity = btVector3(0, -10, 0); m_dynamicsWorld->setGravity(gravity); diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index 62c4c574c..197bb457c 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -57,9 +57,11 @@ public: }; void buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); - void processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); + void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld); + void processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); + bool getSplitIslands() { return m_splitIslands; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 386f1dfd1..950edb1a2 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -428,27 +428,41 @@ void btMultiBodyDynamicsWorld::forwardKinematics() void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { solveExternalForces(solverInfo); - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); + buildIslands(); solveInternalConstraints(solverInfo); } void btMultiBodyDynamicsWorld::buildIslands() { - m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); -} - -void btMultiBodyDynamicsWorld::processIslands() -{ - m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); } void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo) { /// solve all the constraints for this island m_solverMultiBodyIslandCallback->processConstraints(); - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); + calculateJointForce(solverInfo); + processDeltaVee(); +} +void btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld::processConstraintsAndDeltaVee() +{ + m_solverMultiBodyIslandCallback->processConstraints(); + processDeltaVee(); +} + +void btMultiBodyDynamicsWorld::processDeltaVee() +{ + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->processDeltaVeeMultiDof2(); + } +} + +void btMultiBodyDynamicsWorld::calculateJointForce(btContactSolverInfo& solverInfo) +{ { BT_PROFILE("btMultiBody stepVelocities"); for (int i = 0; i < this->m_multiBodies.size(); i++) @@ -489,13 +503,7 @@ void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& sol } } } - } - - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - bod->processDeltaVeeMultiDof2(); - } + } } void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo) @@ -1087,8 +1095,8 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) } } } - -void btMultiBodyDynamicsWorld::setSplitIslands(bool split) -{ - m_islandManager->setSplitIslands(split); -} +// +//void btMultiBodyDynamicsWorld::setSplitIslands(bool split) +//{ +// m_islandManager->setSplitIslands(split); +//} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 605141bc6..a03687859 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -57,6 +57,7 @@ public: virtual ~btMultiBodyDynamicsWorld(); virtual void solveConstraints(btContactSolverInfo& solverInfo); + virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter); virtual void removeMultiBody(btMultiBody* body); @@ -118,8 +119,8 @@ public: virtual void solveExternalForces(btContactSolverInfo& solverInfo); virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); void buildIslands(); - void processIslands(); - void setSplitIslands(bool split); - + void calculateJointForce(btContactSolverInfo& solverInfo); + void processDeltaVee(); + void processConstraintsAndDeltaVee(); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index bdc219945..c5a1bb55b 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -20,7 +20,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" -class btDeformableMultiBodyDynamicsWorld; +//class btDeformableMultiBodyDynamicsWorld; struct DeformableContactConstraint { @@ -62,7 +62,6 @@ public: typedef btAlignedObjectArray > TVArrayStack; typedef btAlignedObjectArray > TArrayStack; btAlignedObjectArray& m_softBodies; - btDeformableMultiBodyDynamicsWorld* m_world; const btScalar& m_dt; btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) @@ -86,11 +85,6 @@ public: virtual void reinitialize(bool nodeUpdated) { } - - virtual void setWorld(btDeformableMultiBodyDynamicsWorld* world) - { - m_world = world; - } }; diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index bf88ae550..1dee17be8 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -151,10 +151,6 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack //set constraints as projections void btDeformableBackwardEulerObjective::setConstraints() { - // build islands for multibody solve - m_world->btMultiBodyDynamicsWorld::buildIslands(); - // for repeated constraint solve, splitIslands has to be set to true - m_world->setSplitIslands(true); projection.setConstraints(); } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 7e1bae29f..cf8ba9920 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -26,13 +26,13 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include "LinearMath/btQuickprof.h" -class btDeformableMultiBodyDynamicsWorld; +//class btDeformableMultiBodyDynamicsWorld; class btDeformableBackwardEulerObjective { public: typedef btAlignedObjectArray TVStack; btScalar m_dt; - btDeformableMultiBodyDynamicsWorld* m_world; +// btDeformableMultiBodyDynamicsWorld* m_world; btAlignedObjectArray m_lf; btAlignedObjectArray& m_softBodies; Preconditioner* m_preconditioner; @@ -98,13 +98,7 @@ public: { m_preconditioner->operator()(x,b); } - - virtual void setWorld(btDeformableMultiBodyDynamicsWorld* world) - { - m_world = world; - projection.setWorld(world); - } - + virtual void updateId() { size_t id = 0; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index d36c19718..03a9be07e 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -30,14 +30,14 @@ btDeformableBodySolver::~btDeformableBodySolver() delete m_objective; } -void btDeformableBodySolver::solveConstraints(float solverdt) +void btDeformableBodySolver::solveDeformableConstraints(float solverdt) { BT_PROFILE("solveConstraints"); // save v_{n+1}^* velocity after explicit forces - backupVelocity(); +// backupVelocity(); // add constraints to the solver - setConstraints(); +// setConstraints(); m_objective->computeResidual(solverdt, m_residual); m_objective->applyDynamicFriction(m_residual); computeStep(m_dv, m_residual); @@ -47,7 +47,6 @@ void btDeformableBodySolver::solveConstraints(float solverdt) void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual) { - btScalar tolerance = std::numeric_limits::epsilon() * 16 * m_objective->computeNorm(residual); m_cg.solve(*m_objective, dv, residual, tolerance); } @@ -78,23 +77,17 @@ void btDeformableBodySolver::setConstraints() { BT_PROFILE("setConstraint"); m_objective->setConstraints(); - // m_contact_iterations takes over m_numIterations - m_contact_iterations = m_objective->m_world->getSolverInfo().m_numIterations; - for (int i = 0; i < m_contact_iterations; ++i) - { - m_objective->projection.update(); - m_objective->enforceConstraint(m_dv); - m_objective->updateVelocity(m_dv); - } - // recover m_numIterations from m_contact_iterations - m_objective->m_world->getSolverInfo().m_numIterations = m_contact_iterations; } -void btDeformableBodySolver::setWorld(btDeformableMultiBodyDynamicsWorld* world) +void btDeformableBodySolver::solveContactConstraints() { - m_objective->setWorld(world); + BT_PROFILE("setConstraint"); + m_objective->projection.update(); + m_objective->enforceConstraint(m_dv); + m_objective->updateVelocity(m_dv); } + void btDeformableBodySolver::updateVelocity() { int counter = 0; diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 205086d84..55f940328 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -61,7 +61,11 @@ public: void extracted(float solverdt); - virtual void solveConstraints(float solverdt); + virtual void solveDeformableConstraints(btScalar solverdt); + + void solveContactConstraints(); + + virtual void solveConstraints(float dt){} void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); @@ -77,7 +81,7 @@ public: void computeStep(TVStack& dv, const TVStack& residual); - virtual void predictMotion(float solverdt); + virtual void predictMotion(btScalar solverdt); virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} @@ -90,8 +94,9 @@ public: softBody->defaultCollisionHandler(otherSoftBody); } virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false){} + virtual bool checkInitialized(){return true;} - virtual void setWorld(btDeformableMultiBodyDynamicsWorld* world); + }; #endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index ba8f74144..f9d4b7d99 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -19,13 +19,8 @@ #include void btDeformableContactProjection::update() { - ///solve rigid body constraints - { - // m_numIterations get temporarily to 1 so that we interleave one step of multibody solve with one step of multibody/deformable contact solve - m_world->getSolverInfo().m_numIterations = 1; - m_world->processIslands(); - m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo()); // process constraints deferred in the previous step - } +// m_world->getSolverInfo().m_numIterations = 1; + // loop through constraints to set constrained values for (int index = 0; index < m_constraints.size(); ++index) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 05e1c1ac9..825e7cd66 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -33,7 +33,6 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed #include "btDeformableMultiBodyDynamicsWorld.h" #include "btDeformableBodySolver.h" #include "LinearMath/btQuickprof.h" - void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { BT_PROFILE("internalSingleStepSimulation"); @@ -52,7 +51,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t beforeSolverCallbacks(timeStep); ///solve deformable bodies constraints - solveDeformableBodiesConstraints(timeStep); + solveConstraints(timeStep); afterSolverCallbacks(timeStep); @@ -172,9 +171,31 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) m_deformableBodySolver->revertVelocity(); } -void btDeformableMultiBodyDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep) +void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - m_deformableBodySolver->solveConstraints(timeStep); + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); + + // setup constraints among multibodies and between multibodies and deformable bodies + setupConstraints(); + solveMultiBodyConstraints(); + m_deformableBodySolver->solveDeformableConstraints(timeStep); +} + +void btDeformableMultiBodyDynamicsWorld::setupConstraints() +{ + m_deformableBodySolver->setConstraints(); + m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); +} + +void btDeformableMultiBodyDynamicsWorld::solveMultiBodyConstraints() +{ + for (int i = 0; i < m_contact_iterations; ++i) + { + m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), (btSimulationIslandManager::IslandCallback*)m_solverMultiBodyIslandCallback); + btMultiBodyDynamicsWorld::processConstraintsAndDeltaVee(); + m_deformableBodySolver->solveContactConstraints(); + } } void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index f8d368c5f..b4f7dbe5a 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -21,11 +21,14 @@ #include "btDeformableMassSpringForce.h" #include "btDeformableBodySolver.h" #include "btSoftBodyHelpers.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" #include typedef btAlignedObjectArray btSoftBodyArray; class btDeformableBodySolver; class btDeformableLagrangianForce; +struct MultiBodyInplaceSolverIslandCallback; + typedef btAlignedObjectArray btSoftBodyArray; class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld @@ -41,6 +44,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld bool m_drawClusterTree; btSoftBodyWorldInfo m_sbi; btScalar m_internalTime; + int m_contact_iterations; typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); btSolverCallback m_solverCallback; @@ -52,7 +56,7 @@ protected: void positionCorrection(btScalar timeStep); - void solveDeformableBodiesConstraints(btScalar timeStep); + void solveConstraints(btScalar timeStep); public: btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) @@ -76,6 +80,7 @@ public: m_sbi.m_sparsesdf.Initialize(); m_internalTime = 0.0; + m_contact_iterations = 1; } void setSolverCallback(btSolverCallback cb) @@ -140,6 +145,9 @@ public: int getDrawFlags() const { return (m_drawFlags); } void setDrawFlags(int f) { m_drawFlags = f; } + + void setupConstraints(); + void solveMultiBodyConstraints(); }; #endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H