diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index e1a75f07a..6a99e08db 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -118,8 +118,9 @@ void DeformableMultibody::initPhysics() m_broadphase = new btDbvtBroadphase(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); - btMultiBodyConstraintSolver* sol; - sol = new btMultiBodyConstraintSolver; + btDeformableMultiBodyConstraintSolver* sol; + sol = new btDeformableMultiBodyConstraintSolver; + sol->setDeformableSolver(deformableBodySolver); m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); @@ -223,6 +224,7 @@ void DeformableMultibody::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = .1; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->setCollisionFlags(0); getDeformableDynamicsWorld()->addSoftBody(psb); btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false); diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 5c149ad19..576803cc8 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -161,7 +161,8 @@ void DeformableRigid::initPhysics() btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) - btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); @@ -208,10 +209,12 @@ void DeformableRigid::initPhysics() { bool onGround = false; const btScalar s = 4; - btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), - btVector3(+s, 0, -s), - btVector3(-s, 0, +s), - btVector3(+s, 0, +s), + const btScalar h = 0; + + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), // 3,3, 20,20, 1 + 2 + 4 + 8, true); diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 31cc97734..6b01bc66d 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -187,7 +187,8 @@ void GraspDeformable::initPhysics() m_broadphase = new btDbvtBroadphase(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); - btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index 733a60f08..3685869a0 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -245,7 +245,8 @@ void Pinch::initPhysics() m_broadphase = new btDbvtBroadphase(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); - btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index 8bdafc5e9..b6657de0c 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -178,7 +178,8 @@ void VolumetricDeformable::initPhysics() m_broadphase = new btDbvtBroadphase(); btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); - btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 950edb1a2..cd1bad089 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -169,218 +169,6 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) btDiscreteDynamicsWorld::updateActivationState(timeStep); } -SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); - return islandId; -} - -class btSortConstraintOnIslandPredicate2 -{ -public: - bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const - { - int rIslandId0, lIslandId0; - rIslandId0 = btGetConstraintIslandId2(rhs); - lIslandId0 = btGetConstraintIslandId2(lhs); - return lIslandId0 < rIslandId0; - } -}; - -SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) -{ - int islandId; - - int islandTagA = lhs->getIslandIdA(); - int islandTagB = lhs->getIslandIdB(); - islandId = islandTagA >= 0 ? islandTagA : islandTagB; - return islandId; -} - -class btSortMultiBodyConstraintOnIslandPredicate -{ -public: - bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const - { - int rIslandId0, lIslandId0; - rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); - lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; - - -struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback -{ - btContactSolverInfo* m_solverInfo; - btMultiBodyConstraintSolver* m_solver; - btMultiBodyConstraint** m_multiBodySortedConstraints; - int m_numMultiBodyConstraints; - - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; - - btAlignedObjectArray m_bodies; - btAlignedObjectArray m_manifolds; - btAlignedObjectArray m_constraints; - btAlignedObjectArray m_multiBodyConstraints; - - btAlignedObjectArray m_islandAnalyticsData; - - MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver, - btDispatcher* dispatcher) - : m_solverInfo(NULL), - m_solver(solver), - m_multiBodySortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - } - - MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) - { - m_islandAnalyticsData.clear(); - btAssert(solverInfo); - m_solverInfo = solverInfo; - - m_multiBodySortedConstraints = sortedMultiBodyConstraints; - m_numMultiBodyConstraints = numMultiBodyConstraints; - m_sortedConstraints = sortedConstraints; - m_numConstraints = numConstraints; - - m_debugDrawer = debugDrawer; - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } - - void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) - { - m_solver = solver; - } - - virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) - { - if (islandId < 0) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_solverInfo->m_reportSolverAnalytics&1) - { - m_solver->m_analyticsData.m_islandId = islandId; - m_islandAnalyticsData.push_back(m_solver->m_analyticsData); - } - } - else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - btMultiBodyConstraint** startMultiBodyConstraint = 0; - - int numCurConstraints = 0; - int numCurMultiBodyConstraints = 0; - - int i; - - //find the first constraint for this island - - for (i = 0; i < m_numConstraints; i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - startConstraint = &m_sortedConstraints[i]; - break; - } - } - //count the number of constraints in this island - for (; i < m_numConstraints; i++) - { - if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) - { - numCurConstraints++; - } - } - - for (i = 0; i < m_numMultiBodyConstraints; i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; - break; - } - } - //count the number of multi body constraints in this island - for (; i < m_numMultiBodyConstraints; i++) - { - if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) - { - numCurMultiBodyConstraints++; - } - } - - //if (m_solverInfo->m_minimumSolverBatchSize<=1) - //{ - // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - //} else - { - for (i = 0; i < numBodies; i++) - m_bodies.push_back(bodies[i]); - for (i = 0; i < numManifolds; i++) - m_manifolds.push_back(manifolds[i]); - for (i = 0; i < numCurConstraints; i++) - m_constraints.push_back(startConstraint[i]); - - for (i = 0; i < numCurMultiBodyConstraints; i++) - m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); - - if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(islandId); - } - else - { - //printf("deferred\n"); - } - } - } - } - void processConstraints(int islandId=-1) - { - btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; - btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; - btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; - btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; - - //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); - - m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); - if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) - { - m_solver->m_analyticsData.m_islandId = islandId; - m_islandAnalyticsData.push_back(m_solver->m_analyticsData); - } - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - m_multiBodyConstraints.resize(0); - } -}; - void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray& islandAnalyticsData) const { islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData; @@ -442,18 +230,47 @@ void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& sol /// 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() -{ + { + 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]; @@ -461,51 +278,6 @@ void btMultiBodyDynamicsWorld::processDeltaVee() } } -void btMultiBodyDynamicsWorld::calculateJointForce(btContactSolverInfo& solverInfo) -{ - { - 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); - } - } - } - } - } - } -} - void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo) { forwardKinematics(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index a03687859..9ac46f4b6 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define BT_MULTIBODY_DYNAMICS_WORLD_H #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h" #define BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY @@ -119,8 +120,5 @@ public: virtual void solveExternalForces(btContactSolverInfo& solverInfo); virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); void buildIslands(); - void calculateJointForce(btContactSolverInfo& solverInfo); - void processDeltaVee(); - void processConstraintsAndDeltaVee(); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h b/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h new file mode 100644 index 000000000..accf7c50b --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h @@ -0,0 +1,229 @@ +// +// btMultiBodyInplaceSolverCallback.h +// BulletDynamics +// +// Created by Xuchen Han on 8/22/19. +// + +#ifndef BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H +#define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H + +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "btMultiBodyConstraintSolver.h" + +SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); + return islandId; +} +class btSortConstraintOnIslandPredicate2 +{ +public: + bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetConstraintIslandId2(rhs); + lIslandId0 = btGetConstraintIslandId2(lhs); + return lIslandId0 < rIslandId0; + } +}; + +SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) +{ + int islandId; + + int islandTagA = lhs->getIslandIdA(); + int islandTagB = lhs->getIslandIdB(); + islandId = islandTagA >= 0 ? islandTagA : islandTagB; + return islandId; +} + +class btSortMultiBodyConstraintOnIslandPredicate +{ +public: + bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); + lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + +struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +{ + + btContactSolverInfo* m_solverInfo; + btMultiBodyConstraintSolver* m_solver; + btMultiBodyConstraint** m_multiBodySortedConstraints; + int m_numMultiBodyConstraints; + + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + + btAlignedObjectArray m_bodies; + btAlignedObjectArray m_manifolds; + btAlignedObjectArray m_constraints; + btAlignedObjectArray m_multiBodyConstraints; + + btAlignedObjectArray m_islandAnalyticsData; + + MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver, + btDispatcher* dispatcher) + : m_solverInfo(NULL), + m_solver(solver), + m_multiBodySortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) + { + } + + MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + + SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) + { + m_islandAnalyticsData.clear(); + btAssert(solverInfo); + m_solverInfo = solverInfo; + + m_multiBodySortedConstraints = sortedMultiBodyConstraints; + m_numMultiBodyConstraints = numMultiBodyConstraints; + m_sortedConstraints = sortedConstraints; + m_numConstraints = numConstraints; + + m_debugDrawer = debugDrawer; + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } + + void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) + { + m_solver = solver; + } + + virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) + { + if (islandId < 0) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + if (m_solverInfo->m_reportSolverAnalytics&1) + { + m_solver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } + } + else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + btMultiBodyConstraint** startMultiBodyConstraint = 0; + + int numCurConstraints = 0; + int numCurMultiBodyConstraints = 0; + + int i; + + //find the first constraint for this island + + for (i = 0; i < m_numConstraints; i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + startConstraint = &m_sortedConstraints[i]; + break; + } + } + //count the number of constraints in this island + for (; i < m_numConstraints; i++) + { + if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) + { + numCurConstraints++; + } + } + + for (i = 0; i < m_numMultiBodyConstraints; i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; + break; + } + } + //count the number of multi body constraints in this island + for (; i < m_numMultiBodyConstraints; i++) + { + if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) + { + numCurMultiBodyConstraints++; + } + } + + //if (m_solverInfo->m_minimumSolverBatchSize<=1) + //{ + // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + //} else + { + for (i = 0; i < numBodies; i++) + m_bodies.push_back(bodies[i]); + for (i = 0; i < numManifolds; i++) + m_manifolds.push_back(manifolds[i]); + for (i = 0; i < numCurConstraints; i++) + m_constraints.push_back(startConstraint[i]); + + for (i = 0; i < numCurMultiBodyConstraints; i++) + m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); + + if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) + { + processConstraints(islandId); + } + else + { + //printf("deferred\n"); + } + } + } + } + + virtual void processConstraints(int islandId=-1) + { + btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; + btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; + btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; + btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; + + //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); + + m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); + if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1)) + { + m_solver->m_analyticsData.m_islandId = islandId; + m_islandAnalyticsData.push_back(m_solver->m_analyticsData); + } + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + m_multiBodyConstraints.resize(0); + } +}; + + +#endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */ diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 40d07bd66..1e9edbd91 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -16,6 +16,7 @@ SET(BulletSoftBody_SRCS btSoftMultiBodyDynamicsWorld.cpp btSoftSoftCollisionAlgorithm.cpp btDefaultSoftBodySolver.cpp + btDeformableMultiBodyConstraintSolver.cpp btDeformableBackwardEulerObjective.cpp btDeformableBodySolver.cpp @@ -50,6 +51,7 @@ SET(BulletSoftBody_HDRS btDeformableBackwardEulerObjective.h btDeformableBodySolver.h + btDeformableMultiBodyConstraintSolver.h btDeformableContactProjection.h btDeformableMultiBodyDynamicsWorld.h diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index c5a1bb55b..d074ca6ed 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -80,7 +80,7 @@ public: virtual void setConstraints() = 0; // update the constraints - virtual void update() = 0; + virtual btScalar update() = 0; virtual void reinitialize(bool nodeUpdated) { diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index f7a26e01e..0e26e2c6d 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -74,12 +74,13 @@ void btDeformableBodySolver::setConstraints() m_objective->setConstraints(); } -void btDeformableBodySolver::solveContactConstraints() +btScalar btDeformableBodySolver::solveContactConstraints() { BT_PROFILE("setConstraint"); - m_objective->projection.update(); + btScalar maxSquaredResidual = m_objective->projection.update(); m_objective->enforceConstraint(m_dv); m_objective->updateVelocity(m_dv); + return maxSquaredResidual; } @@ -142,7 +143,7 @@ bool btDeformableBodySolver::updateNodes() } -void btDeformableBodySolver::predictMotion(btScalar solverdt) +void btDeformableBodySolver::predictMotion(float solverdt) { for (int i = 0; i < m_softBodySet.size(); ++i) { diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 55f940328..9839df7f2 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -63,7 +63,7 @@ public: virtual void solveDeformableConstraints(btScalar solverdt); - void solveContactConstraints(); + btScalar solveContactConstraints(); virtual void solveConstraints(float dt){} @@ -81,7 +81,7 @@ public: void computeStep(TVStack& dv, const TVStack& residual); - virtual void predictMotion(btScalar solverdt); + virtual void predictMotion(float solverdt); virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index f9d4b7d99..25c14b5b9 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,10 +17,9 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include #include -void btDeformableContactProjection::update() +btScalar btDeformableContactProjection::update() { -// m_world->getSolverInfo().m_numIterations = 1; - + btScalar residualSquare = 0; // loop through constraints to set constrained values for (int index = 0; index < m_constraints.size(); ++index) { @@ -129,21 +128,28 @@ void btDeformableContactProjection::update() } } impulse = impulse_normal + impulse_tangent; + + // dn is the normal component of velocity diffrerence. Approximates the residual. + residualSquare = btMax(residualSquare, dn*dn); + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { if (rigidCol) + { rigidCol->applyImpulse(impulse, c->m_c1); + } } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { if (multibodyLinkCol) { + multibodyLinkCol->m_multiBody->processDeltaVeeMultiDof2(); // make sure velocity is up-to-date; // apply normal component of the impulse multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); if (impulse_tangent.norm() > SIMD_EPSILON) { // apply tangential component of the impulse - const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1)); const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2)); @@ -153,6 +159,7 @@ void btDeformableContactProjection::update() } } } + return residualSquare; } void btDeformableContactProjection::setConstraints() diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index f9bd371d7..774da1ae3 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -44,7 +44,7 @@ public: virtual void enforceConstraint(TVStack& x); // update the constraints - virtual void update(); + virtual btScalar update(); virtual void setConstraints(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp new file mode 100644 index 000000000..a331d1ba2 --- /dev/null +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -0,0 +1,44 @@ +// +// btDeformableMultiBodyConstraintSolver.cpp +// BulletSoftBody +// +// Created by Xuchen Han on 8/23/19. +// + +#include "btDeformableMultiBodyConstraintSolver.h" +#include +// override the iterations method to include deformable/multibody contact +btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,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); + + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + for (int iteration = 0; iteration < maxIterations; iteration++) + { + m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + + solverBodyWriteBack(infoGlobal); + m_leastSquaresResidual = btMax(m_leastSquaresResidual, m_deformableSolver->solveContactConstraints()); + 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); +#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; +} diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h new file mode 100644 index 000000000..c5c37cefc --- /dev/null +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -0,0 +1,98 @@ +// +// btDeformableMultiBodyConstraintSolver.h +// BulletSoftBody +// +// Created by Xuchen Han on 8/22/19. +// + +#ifndef BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H +#define BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H + +#include "btDeformableBodySolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" + +class btDeformableBodySolver; + +ATTRIBUTE_ALIGNED16(class) +btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver +{ + btDeformableBodySolver* m_deformableSolver; + +protected: + // override the iterations method to include deformable/multibody contact + virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + void 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); + } + } + } + + void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) + { + btSISolverSingleIterationData siData(m_tmpSolverBodyPool, + m_tmpSolverContactConstraintPool, + m_tmpSolverNonContactConstraintPool, + m_tmpSolverContactFrictionConstraintPool, + m_tmpSolverContactRollingFrictionConstraintPool, + m_orderTmpConstraintPool, + m_orderNonContactConstraintPool, + m_orderFrictionConstraintPool, + m_tmpConstraintSizesPool, + m_resolveSingleConstraintRowGeneric, + m_resolveSingleConstraintRowLowerLimit, + m_resolveSplitPenetrationImpulse, + m_kinematicBodyUniqueIdToSolverBodyTable, + m_btSeed2, + m_fixedBodyId, + m_maxOverrideNumSolverIterations); + + for (int i = 0; i < numBodies; i++) + { + int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); + + btRigidBody* body = btRigidBody::upcast(bodies[i]); + if (body && body->getInvMass()) + { + btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId]; + solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; + solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; + } + } + } + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + void setDeformableSolver(btDeformableBodySolver* deformableSolver) + { + m_deformableSolver = deformableSolver; + } + + virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, 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 + solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); + + // inherited from MultiBodyConstraintSolver + solveGroupCacheFriendlyFinish(bodies, numBodies, info); + + m_tmpMultiBodyConstraints = 0; + m_tmpNumMultiBodyConstraints = 0; + } +}; + +#endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 95f9b48b5..58907fcd3 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -33,6 +33,7 @@ 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"); @@ -176,27 +177,108 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) // save v_{n+1}^* velocity after explicit forces m_deformableBodySolver->backupVelocity(); - // setup constraints among multibodies and between multibodies and deformable bodies + // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); - solveMultiBodyConstraints(); + solveMultiBodyRelatedConstraints(); m_deformableBodySolver->solveDeformableConstraints(timeStep); } void btDeformableMultiBodyDynamicsWorld::setupConstraints() { + // set up constraints between multibody and deformable bodies m_deformableBodySolver->setConstraints(); - m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); + + // set up constraints among multibodies + { + sortConstraints(); + // setup the solver callback + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + m_solverMultiBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + + // build islands + m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); + // write the constraint information of each island to the callback, and also setup the constraints in solver + } } -void btDeformableMultiBodyDynamicsWorld::solveMultiBodyConstraints() +void btDeformableMultiBodyDynamicsWorld::sortConstraints() { - for (int i = 0; i < m_contact_iterations; ++i) + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) { - m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), (btSimulationIslandManager::IslandCallback*)m_solverMultiBodyIslandCallback); - btMultiBodyDynamicsWorld::processConstraintsAndDeltaVee(); - m_deformableBodySolver->solveContactConstraints(); + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i = 0; i < m_multiBodyConstraints.size(); i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); +} + + +void btDeformableMultiBodyDynamicsWorld::solveMultiBodyRelatedConstraints() +{ + // process constraints on each island + m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); + + // process deferred + m_solverMultiBodyIslandCallback->processConstraints(); + m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer); + + // write joint feedback + { + 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(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + } + } + } + + // todo : may not be necessary + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + bod->processDeltaVeeMultiDof2(); } - // todo @xuchenhan: add joint force feedback } void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) @@ -244,8 +326,51 @@ void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep btRigidBody* rb = m_nonStaticRigidBodies[i]; rb->integrateVelocities(timeStep); } + // integrate multibody gravity - btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo()); + { + forwardKinematics(); + clearMultiBodyConstraintForces(); + { + 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) + { + m_scratch_r.resize(bod->getNumLinks() + 1); + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + bool isConstraintPass = false; + { + if (!bod->isUsingRK4Integration()) + { + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + else + { + btAssert(" RK4Integration is not supported" ); + } + } + } + } + } + } clearForces(); clearMultiBodyForces(); } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 1b1806bac..a91944c55 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -20,6 +20,7 @@ #include "btDeformableLagrangianForce.h" #include "btDeformableMassSpringForce.h" #include "btDeformableBodySolver.h" +#include "btDeformableMultiBodyConstraintSolver.h" #include "btSoftBodyHelpers.h" #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" #include @@ -28,6 +29,7 @@ typedef btAlignedObjectArray btSoftBodyArray; class btDeformableBodySolver; class btDeformableLagrangianForce; struct MultiBodyInplaceSolverIslandCallback; +class btDeformableMultiBodyConstraintSolver; typedef btAlignedObjectArray btSoftBodyArray; @@ -59,8 +61,8 @@ protected: void solveConstraints(btScalar timeStep); public: - btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) - : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), + btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) + : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) { m_drawFlags = fDrawFlags::Std; @@ -70,7 +72,7 @@ public: m_sbi.m_broadphase = pairCache; m_sbi.m_dispatcher = dispatcher; m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.setDefaultVoxelsz(0.0025); + m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025); m_sbi.m_sparsesdf.Reset(); m_sbi.air_density = (btScalar)1.2; @@ -78,10 +80,7 @@ public: m_sbi.water_offset = 0; m_sbi.water_normal = btVector3(0, 0, 0); m_sbi.m_gravity.setValue(0, -10, 0); - - m_sbi.m_sparsesdf.Initialize(); m_internalTime = 0.0; - m_contact_iterations = 1; } void setSolverCallback(btSolverCallback cb) @@ -148,7 +147,12 @@ public: void setDrawFlags(int f) { m_drawFlags = f; } void setupConstraints(); + void solveMultiBodyConstraints(); + + void solveMultiBodyRelatedConstraints(); + + void sortConstraints(); }; #endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H