From 362bc6d9a3da283ba68028ec101e9c3dc60deb26 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 8 Nov 2019 16:54:02 -0800 Subject: [PATCH] fix the bug that prevents the pd control forces/torques being added --- .../PhysicsServerCommandProcessor.cpp | 4 +- src/BulletDynamics/Dynamics/btRigidBody.cpp | 8 ++ src/BulletDynamics/Dynamics/btRigidBody.h | 2 + .../btDeformableMultiBodyDynamicsWorld.cpp | 115 +++++++++++++++++- .../btDeformableMultiBodyDynamicsWorld.h | 6 +- 5 files changed, 127 insertions(+), 8 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index af85e9c04..4558a421a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9261,8 +9261,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st { } }; - -#ifdef SKIP_DEFORMABLE_BODY + if (newSolver) { delete oldSolver; @@ -9271,7 +9270,6 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st m_data->m_solver = newSolver; printf("switched solver\n"); } -#endif } } diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index f4bcabada..9e8705b00 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -206,6 +206,14 @@ void btRigidBody::applyGravity() applyCentralForce(m_gravity); } +void btRigidBody::clearGravity() +{ + if (isStaticOrKinematicObject()) + return; + + applyCentralForce(-m_gravity); +} + void btRigidBody::proceedToTransform(const btTransform& newTrans) { setCenterOfMassTransform(newTrans); diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 0a345c2ef..39d47cbbd 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -205,6 +205,8 @@ public: void saveKinematicState(btScalar step); void applyGravity(); + + void clearGravity(); void setGravity(const btVector3& acceleration); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 814c2d687..3eb7248b3 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -383,8 +383,6 @@ void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep // when there are multiple substeps - clearForces(); - clearMultiBodyForces(); btMultiBodyDynamicsWorld::applyGravity(); // integrate rigid body gravity for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) @@ -437,8 +435,48 @@ void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep } } } - clearForces(); - clearMultiBodyForces(); + clearGravity(); +} + +void btDeformableMultiBodyDynamicsWorld::clearGravity() +{ + BT_PROFILE("btMultiBody clearGravity"); + // clear rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + if (body->isActive()) + { + body->clearGravity(); + } + } + // clear multibody gravity + 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) + { + bod->addBaseForce(-m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j)); + } + } + } } void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) @@ -499,3 +537,72 @@ void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject else btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); } + + +int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) +{ + startProfiling(timeStep); + + int numSimulationSubSteps = 0; + + if (maxSubSteps) + { + //fixed timestep with interpolation + m_fixedTimeStep = fixedTimeStep; + m_localTime += timeStep; + if (m_localTime >= fixedTimeStep) + { + numSimulationSubSteps = int(m_localTime / fixedTimeStep); + m_localTime -= numSimulationSubSteps * fixedTimeStep; + } + } + else + { + //variable timestep + fixedTimeStep = timeStep; + m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; + m_fixedTimeStep = 0; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } + else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + //process some debugging flags + if (getDebugDrawer()) + { + btIDebugDraw* debugDrawer = getDebugDrawer(); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + } + if (numSimulationSubSteps) + { + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; + + saveKinematicState(fixedTimeStep * clampedSimulationSteps); + + for (int i = 0; i < clampedSimulationSteps; i++) + { + internalSingleStepSimulation(fixedTimeStep); + synchronizeMotionStates(); + } + } + else + { + synchronizeMotionStates(); + } + + clearForces(); + +#ifndef BT_NO_PROFILE + CProfileManager::Increment_Frame_Counter(); +#endif //BT_NO_PROFILE + + return numSimulationSubSteps; +} diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 834c6c8b1..d00183504 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -63,7 +63,9 @@ protected: void solveConstraints(btScalar timeStep); void updateActivationState(btScalar timeStep); - + + void clearGravity(); + public: btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), @@ -90,6 +92,8 @@ public: m_selfCollision = true; } + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); + void setSolverCallback(btSolverCallback cb) { m_solverCallback = cb;