diff --git a/examples/DeformableDemo/SplitImpulse.cpp b/examples/DeformableDemo/SplitImpulse.cpp new file mode 100644 index 000000000..6b99e9523 --- /dev/null +++ b/examples/DeformableDemo/SplitImpulse.cpp @@ -0,0 +1,259 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ +#include "SplitImpulse.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +#define USE_SPLIT_IMPULSE 1 +///The SplitImpulse shows the effect of split impulse in deformable rigid contact. +class SplitImpulse : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + SplitImpulse(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~SplitImpulse() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + void Ctor_RbUpStack(int count) + { + float mass = 0.2; + + btCollisionShape* shape[] = { + new btBoxShape(btVector3(1, 1, 1)), + }; + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0, 0.7, 0)); + createRigidBody(mass, startTransform, shape[0]); + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void SplitImpulse::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); +// deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + btVector3 gravity = btVector3(0, -50, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25); + getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.Reset(); + +// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -32, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(1); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + +#ifdef USE_SPLIT_IMPULSE + getDeformableDynamicsWorld()->getSolverInfo().m_erp = 0.03; +#else + getDeformableDynamicsWorld()->getSolverInfo().m_erp = 0.0; +#endif + + // create a piece of cloth + { + const btScalar s = 4; + 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); +// 0, true); + + + psb->getCollisionShape()->setMargin(0.15); + psb->generateBendingConstraints(2); + psb->setTotalMass(1); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 2; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true); + getDeformableDynamicsWorld()->addForce(psb, mass_spring); + m_forces.push_back(mass_spring); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + m_forces.push_back(gravity_force); + // add a few rigid bodies + Ctor_RbUpStack(1); + } + getDeformableDynamicsWorld()->setImplicit(false); + getDeformableDynamicsWorld()->setLineSearch(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void SplitImpulse::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* SplitImpulseCreateFunc(struct CommonExampleOptions& options) +{ + return new SplitImpulse(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/SplitImpulse.h b/examples/DeformableDemo/SplitImpulse.h new file mode 100644 index 000000000..441f9106f --- /dev/null +++ b/examples/DeformableDemo/SplitImpulse.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _SPLIT_IMPULSE_H +#define _SPLIT_IMPULSE_H + +class CommonExampleInterface* SplitImpulseCreateFunc(struct CommonExampleOptions& options); + +#endif //_SPLIT_IMPULSE_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 56eecf328..fc34707fc 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -369,6 +369,8 @@ SET(BulletExampleBrowser_SRCS ../DeformableDemo/DeformableMultibody.h ../DeformableDemo/DeformableRigid.cpp ../DeformableDemo/DeformableRigid.h + ../DeformableDemo/SplitImpulse.cpp + ../DeformableDemo/SplitImpulse.h ../DeformableDemo/VolumetricDeformable.cpp ../DeformableDemo/VolumetricDeformable.h ../DeformableDemo/DeformableClothAnchor.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 191d62e58..6bc8b589b 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -45,6 +45,7 @@ #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../DeformableDemo/DeformableRigid.h" +#include "../DeformableDemo/SplitImpulse.h" #include "../DeformableDemo/ClothFriction.h" #include "../DeformableDemo/Pinch.h" #include "../DeformableDemo/DeformableSelfCollision.h" @@ -197,6 +198,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Cloth Friction", "Cloth friction contact", ClothFrictionCreateFunc), ExampleEntry(1, "Deformable-Deformable Friction Contact", "Deformable friction contact", PinchFrictionCreateFunc), ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc), + ExampleEntry(1, "Split Impulse Contact", "Split impulse test", SplitImpulseCreateFunc), ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc), ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 705331b88..0a345c2ef 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -332,6 +332,48 @@ public: } } } + + void applyPushImpulse(const btVector3& impulse, const btVector3& rel_pos) + { + if (m_inverseMass != btScalar(0.)) + { + applyCentralPushImpulse(impulse); + if (m_angularFactor) + { + applyTorqueTurnImpulse(rel_pos.cross(impulse * m_linearFactor)); + } + } + } + + btVector3 getPushVelocity() + { + return m_pushVelocity; + } + + btVector3 getTurnVelocity() + { + return m_turnVelocity; + } + + void setPushVelocity(const btVector3& v) + { + m_pushVelocity = v; + } + + void setTurnVelocity(const btVector3& v) + { + m_turnVelocity = v; + } + + void applyCentralPushImpulse(const btVector3& impulse) + { + m_pushVelocity += impulse * m_linearFactor * m_inverseMass; + } + + void applyTorqueTurnImpulse(const btVector3& torque) + { + m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + } void clearForces() { diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 9af8e3f77..a76c6ce79 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -36,7 +36,7 @@ btDeformableBodySolver::~btDeformableBodySolver() void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { - BT_PROFILE("solveConstraints"); + BT_PROFILE("solveDeformableConstraints"); if (!m_implicit) { m_objective->computeResidual(solverdt, m_residual); @@ -241,6 +241,16 @@ btScalar btDeformableBodySolver::solveContactConstraints() return maxSquaredResidual; } +btScalar btDeformableBodySolver::solveSplitImpulse(const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("solveSplitImpulse"); + return m_objective->m_projection.solveSplitImpulse(infoGlobal); +} + +void btDeformableBodySolver::splitImpulseSetup(const btContactSolverInfo& infoGlobal) +{ + m_objective->m_projection.splitImpulseSetup(infoGlobal); +} void btDeformableBodySolver::updateVelocity() { @@ -321,7 +331,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit) } else m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; - psb->m_nodes[j].m_v = m_backupVelocity[counter]; + psb->m_nodes[j].m_v = m_backupVelocity[counter] + psb->m_nodes[j].m_vsplit; ++counter; } } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index bc56b3736..64c7dcfbe 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -66,6 +66,12 @@ public: // solve the contact between deformable and rigid as well as among deformables btScalar solveContactConstraints(); + + // solve the position error between deformable and rigid as well as among deformables; + btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); + + // set up the position error in split impulse + void splitImpulseSetup(const btContactSolverInfo& infoGlobal); // resize/clear data structures void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 5764e961a..e8219dc50 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -140,11 +140,14 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS { m_total_normal_dv.setZero(); m_total_tangent_dv.setZero(); + // penetration is non-positive. The magnitude of penetration is the depth of penetration. + m_penetration = btMin(btScalar(0), c.m_cti.m_offset); } btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) : m_contact(other.m_contact) , btDeformableContactConstraint(other) +, m_penetration(other.m_penetration) { m_total_normal_dv = other.m_total_normal_dv; m_total_tangent_dv = other.m_total_tangent_dv; @@ -285,6 +288,36 @@ btScalar btDeformableRigidContactConstraint::solveConstraint() return residualSquare; } +btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) +{ + const btSoftBody::sCti& cti = m_contact->m_cti; + const btScalar dn = m_penetration; + if (dn != 0) + { + const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep)); + // one iteration of the position impulse corrects all the position error at this timestep + m_penetration -= dn; + // apply impulse to deformable nodes involved and change their position + applySplitImpulse(impulse); + // apply impulse to the rigid/multibodies involved and change their position + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyPushImpulse(impulse, m_contact->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + // todo xuchenhan@ + } + return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep); + } + return 0; +} + /* ================ Node vs. Rigid =================== */ btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact) : m_node(contact.m_node) @@ -316,6 +349,13 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul contact->m_node->m_v -= dv; } +void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableNodeRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + contact->m_node->m_vsplit -= dv; +}; + /* ================ Face vs. Rigid =================== */ btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact) : m_face(contact.m_face) @@ -386,6 +426,26 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul v2 += dv2; } +void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) +{ + const btSoftBody::DeformableFaceRigidContact* contact = getContact(); + btVector3 dv = impulse * contact->m_c2; + btSoftBody::Face* face = contact->m_face; + + btVector3& v0 = face->m_n[0]->m_vsplit; + btVector3& v1 = face->m_n[1]->m_vsplit; + btVector3& v2 = face->m_n[2]->m_vsplit; + const btScalar& im0 = face->m_n[0]->m_im; + const btScalar& im1 = face->m_n[1]->m_im; + const btScalar& im2 = face->m_n[2]->m_im; + if (im0 > 0) + v0 -= dv * contact->m_weights[0]; + if (im1 > 0) + v1 -= dv * contact->m_weights[1]; + if (im2 > 0) + v2 -= dv * contact->m_weights[2]; +} + /* ================ Face vs. Node =================== */ btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact) : m_node(contact.m_node) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index da16c51e5..912119e7c 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -50,6 +50,9 @@ public: // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact virtual btScalar solveConstraint() = 0; + // solve the position error by applying an inelastic impulse that changes only the position (not velocity) + virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0; + // get the velocity of the object A in the contact virtual btVector3 getVa() const = 0; @@ -61,6 +64,12 @@ public: // apply impulse to the soft body node and/or face involved virtual void applyImpulse(const btVector3& impulse) = 0; + + // apply position based impulse to the soft body node and/or face involved + virtual void applySplitImpulse(const btVector3& impulse) = 0; + + // scale the penetration depth by erp + virtual void setPenetrationScale(btScalar scale) = 0; }; // @@ -90,6 +99,11 @@ public: return 0; } + virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) + { + return 0; + } + virtual btVector3 getVa() const { return btVector3(0,0,0); @@ -106,6 +120,8 @@ public: } virtual void applyImpulse(const btVector3& impulse){} + virtual void applySplitImpulse(const btVector3& impulse){} + virtual void setPenetrationScale(btScalar scale){} }; // @@ -122,6 +138,11 @@ public: { } virtual btScalar solveConstraint(); + virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) + { + // todo xuchenhan@ + return 0; + } // object A is the rigid/multi body, and object B is the deformable node/face virtual btVector3 getVa() const; // get the velocity of the deformable node in contact @@ -131,6 +152,11 @@ public: return btVector3(0,0,0); } virtual void applyImpulse(const btVector3& impulse); + virtual void applySplitImpulse(const btVector3& impulse) + { + // todo xuchenhan@ + }; + virtual void setPenetrationScale(btScalar scale){} }; @@ -141,6 +167,7 @@ class btDeformableRigidContactConstraint : public btDeformableContactConstraint public: btVector3 m_total_normal_dv; btVector3 m_total_tangent_dv; + btScalar m_penetration; const btSoftBody::DeformableRigidContact* m_contact; btDeformableRigidContactConstraint(){} @@ -154,6 +181,13 @@ public: virtual btVector3 getVa() const; virtual btScalar solveConstraint(); + + virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); + + virtual void setPenetrationScale(btScalar scale) + { + m_penetration *= scale; + } }; // @@ -185,6 +219,7 @@ public: } virtual void applyImpulse(const btVector3& impulse); + virtual void applySplitImpulse(const btVector3& impulse); }; // @@ -214,6 +249,7 @@ public: } virtual void applyImpulse(const btVector3& impulse); + virtual void applySplitImpulse(const btVector3& impulse); }; // @@ -235,6 +271,12 @@ public: virtual btScalar solveConstraint(); + virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) + { + // todo: xuchenhan@ + return 0; + } + // get the velocity of the object A in the contact virtual btVector3 getVa() const; @@ -251,5 +293,10 @@ public: } virtual void applyImpulse(const btVector3& impulse); + virtual void applySplitImpulse(const btVector3& impulse) + { + // todo xuchenhan@ + } + virtual void setPenetrationScale(btScalar scale){} }; #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index d438d6eec..dc00e7520 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -51,6 +51,57 @@ btScalar btDeformableContactProjection::update() return residualSquare; } +void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal) +{ + // node constraints + for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) + { + btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); + for (int i = 0; i < constraints.size(); ++i) + { + constraints[i].setPenetrationScale(infoGlobal.m_erp); + } + } + + // face constraints + for (int index = 0; index < m_allFaceConstraints.size(); ++index) + { + btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; + constraint->setPenetrationScale(infoGlobal.m_erp); + } +} + +btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal) +{ + btScalar residualSquare = 0; + // node constraints + for (int index = 0; index < m_nodeRigidConstraints.size(); ++index) + { + btAlignedObjectArray& constraints = *m_nodeRigidConstraints.getAtIndex(index); + for (int i = 0; i < constraints.size(); ++i) + { + btScalar localResidualSquare = constraints[i].solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + } + + // anchor constraints + for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) + { + btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index); + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + + // face constraints + for (int index = 0; index < m_allFaceConstraints.size(); ++index) + { + btDeformableContactConstraint* constraint = m_allFaceConstraints[index]; + btScalar localResidualSquare = constraint->solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } + return residualSquare; +} void btDeformableContactProjection::setConstraints() { diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 90386b0c9..406eeba3c 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -60,9 +60,12 @@ public: // add friction force to the rhs of the linear solve virtual void applyDynamicFriction(TVStack& f); - // update the constraints + // update and solve the constraints virtual btScalar update(); + // solve the position error using split impulse + virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); + // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict. virtual void setConstraints(); @@ -70,5 +73,7 @@ public: virtual void setProjection(); virtual void reinitialize(bool nodeUpdated); + + virtual void splitImpulseSetup(const btContactSolverInfo& infoGlobal); }; #endif /* btDeformableContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 1e0d53a41..9c5390d1f 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -104,3 +104,40 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS } } } + +void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, 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)) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration); +#endif + break; + } + } + } + } +} diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index cac82f98b..10f1d3f4b 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -44,6 +44,8 @@ protected: // write the velocity of the underlying rigid body to the the the solver body void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); + public: BT_DECLARE_ALIGNED_ALLOCATOR(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 03a884139..814c2d687 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -118,9 +118,32 @@ void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() } } +void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep) +{ + // correct the position of rigid bodies with temporary velocity generated from split impulse + btContactSolverInfo infoGlobal; + btVector3 zero(0,0,0); + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btVector3 pushVelocity = rb->getPushVelocity(); + btVector3 turnVelocity = rb->getTurnVelocity(); + if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0) + { + btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform); + rb->setWorldTransform(newTransform); + rb->setPushVelocity(zero); + rb->setTurnVelocity(zero); + } + } +} + void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); + positionCorrection(timeStep); btMultiBodyDynamicsWorld::integrateTransforms(timeStep); for (int i = 0; i < m_softBodies.size(); ++i) { @@ -142,6 +165,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) } } node.m_x = node.m_x + timeStep * node.m_v; + node.m_v -= node.m_vsplit; + node.m_vsplit.setZero(); node.m_q = node.m_x; node.m_vn = node.m_v; } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index d894d5cf5..38cc162d2 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -257,6 +257,7 @@ public: btVector3 m_x; // Position btVector3 m_q; // Previous step position/Test position btVector3 m_v; // Velocity + btVector3 m_vsplit; // Temporary Velocity in addintion to velocity used in split impulse btVector3 m_vn; // Previous step velocity btVector3 m_f; // Force accumulator btVector3 m_n; // Normal