diff --git a/examples/CommonInterfaces/CommonDeformableBodyBase.h b/examples/CommonInterfaces/CommonDeformableBodyBase.h new file mode 100644 index 000000000..456c25bfe --- /dev/null +++ b/examples/CommonInterfaces/CommonDeformableBodyBase.h @@ -0,0 +1,178 @@ + +#ifndef COMMON_DEFORMABLE_BODY_SETUP_H +#define COMMON_DEFORMABLE_BODY_SETUP_H +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "btBulletDynamicsCommon.h" +#include "CommonExampleInterface.h" +#include "CommonGUIHelperInterface.h" +#include "CommonRenderInterface.h" +#include "CommonGraphicsAppInterface.h" +#include "CommonWindowInterface.h" +#include "CommonCameraInterface.h" +#include "CommonMultiBodyBase.h" + +struct CommonDeformableBodyBase : public CommonMultiBodyBase +{ + btAlignedObjectArray m_forces; + btSoftBody* m_pickedSoftBody; + btDeformableMousePickingForce* m_mouseForce; + btScalar m_maxPickingForce; + CommonDeformableBodyBase(GUIHelperInterface* helper) + : CommonMultiBodyBase(helper), + m_pickedSoftBody(0), + m_mouseForce(0), + m_maxPickingForce(0.3) + { + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + if (getDeformableDynamicsWorld() == 0) + return false; + btCollisionWorld::ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld); + getDeformableDynamicsWorld()->rayTest(rayFromWorld, rayToWorld, rayCallback); + if (rayCallback.hasHit()) + { + btVector3 pickPos = rayCallback.m_hitPointWorld; + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); + btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); + if (body) + { + if (!(body->isStaticObject() || body->isKinematicObject())) + { + m_pickedBody = body; + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); + m_dynamicsWorld->addConstraint(p2p, true); + m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } + } + else if (psb) + { + int face_id = rayCallback.m_localShapeInfo->m_triangleIndex; + m_pickedSoftBody = psb; + psb->setActivationState(DISABLE_DEACTIVATION); + const btSoftBody::Face& f = psb->m_faces[face_id]; + btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0.2, f, m_hitPos, m_maxPickingForce); + m_mouseForce = mouse_force; + getDeformableDynamicsWorld()->addForce(psb, mouse_force); + } + else + { + btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); + if (multiCol && multiCol->m_multiBody) + { + m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); + multiCol->m_multiBody->setCanSleep(false); + + btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody, multiCol->m_link, 0, pivotInA, pickPos); + //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' + //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 + //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply + //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) + btScalar scaling = 1; + p2p->setMaxAppliedImpulse(2 * scaling); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + m_pickingMultiBodyPoint2Point = p2p; + } + } + m_oldPickingPos = rayToWorld; + m_hitPos = pickPos; + m_oldPickingDist = (pickPos - rayFromWorld).length(); + } + return false; + } + + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + if (m_pickedBody && m_pickedConstraint) + { + btPoint2PointConstraint* pickCon = static_cast(m_pickedConstraint); + if (pickCon) + { + //keep it at the same picking distance + btVector3 newPivotB; + btVector3 dir = rayToWorld - rayFromWorld; + dir.normalize(); + dir *= m_oldPickingDist; + newPivotB = rayFromWorld + dir; + pickCon->setPivotB(newPivotB); + return true; + } + } + if (m_pickingMultiBodyPoint2Point) + { + //keep it at the same picking distance + btVector3 dir = rayToWorld - rayFromWorld; + dir.normalize(); + dir *= m_oldPickingDist; + btVector3 newPivotB = rayFromWorld + dir; + m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); + } + if (m_pickedSoftBody && m_mouseForce) + { + btVector3 newPivot; + btVector3 dir = rayToWorld - rayFromWorld; + dir.normalize(); + dir *= m_oldPickingDist; + newPivot = rayFromWorld + dir; + m_mouseForce->setMousePos(newPivot); + } + return false; + } + + virtual void removePickingConstraint() + { + if (m_pickedConstraint) + { + m_dynamicsWorld->removeConstraint(m_pickedConstraint); + + if (m_pickedBody) + { + m_pickedBody->forceActivationState(ACTIVE_TAG); + m_pickedBody->activate(true); + } + delete m_pickedConstraint; + m_pickedConstraint = 0; + m_pickedBody = 0; + } + if (m_pickingMultiBodyPoint2Point) + { + m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_prevCanSleep); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_dynamicsWorld; + world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point); + delete m_pickingMultiBodyPoint2Point; + m_pickingMultiBodyPoint2Point = 0; + } + if (m_pickedSoftBody) + { + getDeformableDynamicsWorld()->removeForce(m_pickedSoftBody, m_mouseForce); + delete m_mouseForce; + m_mouseForce = 0; + m_pickedSoftBody = 0; + } + } +}; +#endif //COMMON_MULTI_BODY_SETUP_H diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h index 7736f51a5..ad6c2a4c6 100644 --- a/examples/CommonInterfaces/CommonMultiBodyBase.h +++ b/examples/CommonInterfaces/CommonMultiBodyBase.h @@ -456,6 +456,7 @@ struct CommonMultiBodyBase : public CommonExampleInterface return false; } + virtual void removePickingConstraint() { if (m_pickedConstraint) diff --git a/examples/DeformableDemo/ClothFriction.cpp b/examples/DeformableDemo/ClothFriction.cpp index e2aa2c0b3..ec1d47a3c 100644 --- a/examples/DeformableDemo/ClothFriction.cpp +++ b/examples/DeformableDemo/ClothFriction.cpp @@ -22,18 +22,17 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The ClothFriction shows the use of deformable friction. -class ClothFriction : public CommonRigidBodyBase +class ClothFriction : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; btDeformableBodySolver* m_deformableBodySolver; public: ClothFriction(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper), - m_deformableBodySolver(0) + : CommonDeformableBodyBase(helper), + m_deformableBodySolver(0) { } virtual ~ClothFriction() @@ -59,19 +58,9 @@ public: m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -210,7 +199,7 @@ void ClothFriction::initPhysics() void ClothFriction::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableClothAnchor.cpp b/examples/DeformableDemo/DeformableClothAnchor.cpp index e7fe6484b..ef2c495d9 100644 --- a/examples/DeformableDemo/DeformableClothAnchor.cpp +++ b/examples/DeformableDemo/DeformableClothAnchor.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableClothAnchor shows contact between deformable objects and rigid objects. -class DeformableClothAnchor : public CommonRigidBodyBase +class DeformableClothAnchor : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableClothAnchor(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~DeformableClothAnchor() @@ -55,24 +54,10 @@ public: float internalTimeStep = 1. / 240.f; m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } - - 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(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -186,7 +171,7 @@ void DeformableClothAnchor::initPhysics() void DeformableClothAnchor::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp index 287d150df..37107186d 100644 --- a/examples/DeformableDemo/DeformableContact.cpp +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -22,17 +22,16 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableContact shows the contact between deformable objects -class DeformableContact : public CommonRigidBodyBase +class DeformableContact : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableContact(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~DeformableContact() @@ -57,19 +56,9 @@ public: m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); @@ -218,7 +207,7 @@ void DeformableContact::initPhysics() void DeformableContact::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index 5b342aada..31dfdeda1 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -28,17 +28,16 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" -#include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableMultibody demo deformable bodies self-collision static bool g_floatingBase = true; static float friction = 1.; -class DeformableMultibody : public CommonMultiBodyBase +class DeformableMultibody : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableMultibody(struct GUIHelperInterface* helper) - : CommonMultiBodyBase(helper) + :CommonDeformableBodyBase(helper) { } @@ -65,20 +64,9 @@ public: void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); - - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonMultiBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -229,7 +217,7 @@ void DeformableMultibody::initPhysics() void DeformableMultibody::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 47ac3d9ff..f6cd1d5b8 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableRigid shows contact between deformable objects and rigid objects. -class DeformableRigid : public CommonRigidBodyBase +class DeformableRigid : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableRigid(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + :CommonDeformableBodyBase(helper) { } virtual ~DeformableRigid() @@ -115,7 +114,7 @@ public: virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -241,7 +240,7 @@ void DeformableRigid::initPhysics() void DeformableRigid::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index a5999084c..7d3db4965 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The DeformableSelfCollision shows deformable self collisions -class DeformableSelfCollision : public CommonRigidBodyBase +class DeformableSelfCollision : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: DeformableSelfCollision(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~DeformableSelfCollision() @@ -57,23 +56,9 @@ public: void addCloth(btVector3 origin); - 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(); + CommonDeformableBodyBase::renderScene(); } }; @@ -183,7 +168,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin) void DeformableSelfCollision::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index e7d63d52d..8c85083f0 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -23,7 +23,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" @@ -56,12 +56,12 @@ static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex) return canHaveMotor; } -class GraspDeformable : public CommonRigidBodyBase +class GraspDeformable : public CommonDeformableBodyBase { btAlignedObjectArray m_forces; public: GraspDeformable(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + :CommonDeformableBodyBase(helper) { } virtual ~GraspDeformable() @@ -154,7 +154,7 @@ public: virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -166,6 +166,16 @@ public: } } } + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + return false; + } + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + return false; + } + virtual void removePickingConstraint(){} }; @@ -189,7 +199,7 @@ void GraspDeformable::initPhysics() m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - + m_maxPickingForce = 0.001; // build a gripper if(1) { @@ -275,13 +285,13 @@ void GraspDeformable::initPhysics() { char absolute_path[1024]; b3BulletDefaultFileIO fileio; - fileio.findResourcePath("ditto.vtk", absolute_path, 1024); +// fileio.findResourcePath("ditto.vtk", absolute_path, 1024); // fileio.findResourcePath("banana.vtk", absolute_path, 1024); - // fileio.findResourcePath("ball.vtk", absolute_path, 1024); +// fileio.findResourcePath("ball.vtk", absolute_path, 1024); // fileio.findResourcePath("deformable_crumpled_napkin_sim.vtk", absolute_path, 1024); // fileio.findResourcePath("single_tet.vtk", absolute_path, 1024); -// fileio.findResourcePath("tube.vtk", absolute_path, 1024); - // fileio.findResourcePath("torus.vtk", absolute_path, 1024); + fileio.findResourcePath("tube.vtk", absolute_path, 1024); +// fileio.findResourcePath("torus.vtk", absolute_path, 1024); // fileio.findResourcePath("paper_roll.vtk", absolute_path, 1024); // fileio.findResourcePath("bread.vtk", absolute_path, 1024); // fileio.findResourcePath("boot.vtk", absolute_path, 1024); @@ -293,17 +303,17 @@ void GraspDeformable::initPhysics() btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), absolute_path); // psb->scale(btVector3(30, 30, 30)); // for banana - psb->scale(btVector3(.7, .7, .7)); +// psb->scale(btVector3(.7, .7, .7)); // psb->scale(btVector3(2, 2, 2)); -// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot - psb->scale(btVector3(.1, .1, .1)); // for ditto + psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot +// psb->scale(btVector3(.1, .1, .1)); // for ditto // psb->translate(btVector3(.25, 10, 0.4)); psb->getCollisionShape()->setMargin(0.0005); psb->setMaxStress(50); psb->setTotalMass(.01); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 20; + psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); @@ -311,7 +321,7 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,32, .05); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,8,.02); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } @@ -376,7 +386,7 @@ void GraspDeformable::initPhysics() void GraspDeformable::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/MultibodyClothAnchor.cpp b/examples/DeformableDemo/MultibodyClothAnchor.cpp index ba90a9e20..5efdf8d28 100644 --- a/examples/DeformableDemo/MultibodyClothAnchor.cpp +++ b/examples/DeformableDemo/MultibodyClothAnchor.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The MultibodyClothAnchor shows contact between deformable objects and rigid objects. -class MultibodyClothAnchor : public CommonRigidBodyBase +class MultibodyClothAnchor : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: MultibodyClothAnchor(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~MultibodyClothAnchor() @@ -56,23 +55,9 @@ public: m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } - 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(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -234,7 +219,7 @@ void MultibodyClothAnchor::initPhysics() void MultibodyClothAnchor::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index 07c491d89..c6b942fe3 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -22,7 +22,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The Pinch shows the frictional contact between kinematic rigid objects with deformable objects @@ -32,12 +32,11 @@ struct TetraCube #include "../SoftDemo/cube.inl" }; -class Pinch : public CommonRigidBodyBase +class Pinch : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: Pinch(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~Pinch() @@ -81,19 +80,9 @@ public: } } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -337,7 +326,7 @@ void Pinch::initPhysics() void Pinch::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp index 24db82111..16b6e30e1 100644 --- a/examples/DeformableDemo/PinchFriction.cpp +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -21,7 +21,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The PinchFriction shows the frictional contacts among volumetric deformable objects @@ -31,12 +31,12 @@ struct TetraCube #include "../SoftDemo/cube.inl" }; -class PinchFriction : public CommonRigidBodyBase +class PinchFriction : public CommonDeformableBodyBase { btAlignedObjectArray m_forces; public: PinchFriction(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~PinchFriction() @@ -80,20 +80,20 @@ public: } } - virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - - virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() - { - return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; - } - virtual void renderScene() { - CommonRigidBodyBase::renderScene(); + CommonDeformableBodyBase::renderScene(); } + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + return false; + } + virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) + { + return false; + } + virtual void removePickingConstraint(){} }; void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world) @@ -353,7 +353,7 @@ void PinchFriction::initPhysics() void PinchFriction::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/SplitImpulse.cpp b/examples/DeformableDemo/SplitImpulse.cpp index c7c8088c2..a213918d4 100644 --- a/examples/DeformableDemo/SplitImpulse.cpp +++ b/examples/DeformableDemo/SplitImpulse.cpp @@ -21,16 +21,15 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The SplitImpulse shows the effect of split impulse in deformable rigid contact. -class SplitImpulse : public CommonRigidBodyBase +class SplitImpulse : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; public: SplitImpulse(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { } virtual ~SplitImpulse() @@ -69,23 +68,9 @@ public: 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(); + CommonDeformableBodyBase::renderScene(); btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) @@ -202,7 +187,7 @@ void SplitImpulse::initPhysics() void SplitImpulse::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index 4be1e090f..d99fc53e9 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -23,27 +23,28 @@ #include "../CommonInterfaces/CommonParameterInterface.h" #include //printf debugging -#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonDeformableBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects. static btScalar E = 100; static btScalar nu = 0.3; -static btScalar damping = 0.1; +static btScalar damping = 0.01; struct TetraCube { #include "../SoftDemo/cube.inl" }; -class VolumetricDeformable : public CommonRigidBodyBase +class VolumetricDeformable : public CommonDeformableBodyBase { - btAlignedObjectArray m_forces; btDeformableNeoHookeanForce* m_neohookean; + public: VolumetricDeformable(struct GUIHelperInterface* helper) - : CommonRigidBodyBase(helper) + : CommonDeformableBodyBase(helper) { + m_neohookean = 0; } virtual ~VolumetricDeformable() { @@ -67,8 +68,8 @@ public: m_neohookean->setYoungsModulus(E); m_neohookean->setDamping(damping); //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 240.f; - m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + float internalTimeStep = 1. / 600.f; + m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); } void createStaticBox(const btVector3& halfEdge, const btVector3& translation) @@ -125,37 +126,20 @@ public: } } - 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(); + CommonDeformableBodyBase::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()); } } } - - virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); }; void VolumetricDeformable::initPhysics() @@ -237,7 +221,7 @@ void VolumetricDeformable::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.05); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.01); m_neohookean = neohookean; getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); @@ -258,7 +242,7 @@ void VolumetricDeformable::initPhysics() } { SliderParams slider("Poisson Ratio", &nu); - slider.m_minVal = 0; + slider.m_minVal = 0.1; slider.m_maxVal = 0.4; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); @@ -266,73 +250,16 @@ void VolumetricDeformable::initPhysics() { SliderParams slider("Damping", &damping); slider.m_minVal = 0.01; - slider.m_maxVal = 1; + slider.m_maxVal = 0.2; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } } -bool VolumetricDeformable::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) -{ - if (getDeformableDynamicsWorld() == 0) - return false; - - btCollisionWorld::ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld); - - rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; - getDeformableDynamicsWorld()->rayTest(rayFromWorld, rayToWorld, rayCallback); - if (rayCallback.hasHit()) - { - btVector3 pickPos = rayCallback.m_hitPointWorld; - btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); - if (body) - { - //other exclusions? - if (!(body->isStaticObject() || body->isKinematicObject())) - { - m_pickedBody = body; - m_savedState = m_pickedBody->getActivationState(); - m_pickedBody->setActivationState(DISABLE_DEACTIVATION); - //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); - btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; - btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); - m_dynamicsWorld->addConstraint(p2p, true); - m_pickedConstraint = p2p; - btScalar mousePickClamping = 30.f; - p2p->m_setting.m_impulseClamp = mousePickClamping; - //very weak constraint for picking - p2p->m_setting.m_tau = 0.001f; - } - } - btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); - if (psb) - { - m_savedState = psb->getActivationState(); - m_pickedBody->setActivationState(DISABLE_DEACTIVATION); -// btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; -// btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); -// m_dynamicsWorld->addConstraint(p2p, true); -// m_pickedConstraint = p2p; -// btScalar mousePickClamping = 30.f; -// p2p->m_setting.m_impulseClamp = mousePickClamping; -// //very weak constraint for picking -// p2p->m_setting.m_tau = 0.001f; - } - - // pickObject(pickPos, rayCallback.m_collisionObject); - m_oldPickingPos = rayToWorld; - m_hitPos = pickPos; - m_oldPickingDist = (pickPos - rayFromWorld).length(); - // printf("hit !\n"); - //add p2p - } - return false; -} - void VolumetricDeformable::exitPhysics() { //cleanup in the reverse order of creation/initialization - + removePickingConstraint(); //remove the rigidbodies from the dynamics world and delete them int i; for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 89cbb38c5..7bd03bd03 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -20,6 +20,7 @@ #include "btDeformableMassSpringForce.h" #include "btDeformableGravityForce.h" #include "btDeformableCorotatedForce.h" +#include "btDeformableMousePickingForce.h" #include "btDeformableLinearElasticityForce.h" #include "btDeformableNeoHookeanForce.h" #include "btDeformableContactProjection.h" diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index 64e80e23b..cdd31c7fd 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -26,7 +26,8 @@ enum btDeformableLagrangianForceType BT_MASSSPRING_FORCE = 2, BT_COROTATED_FORCE = 3, BT_NEOHOOKEAN_FORCE = 4, - BT_LINEAR_ELASTICITY_FORCE = 5 + BT_LINEAR_ELASTICITY_FORCE = 5, + BT_MOUSE_PICKING_FORCE = 6 }; static inline double randomDouble(double low, double high) @@ -85,6 +86,11 @@ public: m_softBodies.push_back(psb); } + virtual void removeSoftBody(btSoftBody* psb) + { + m_softBodies.remove(psb); + } + virtual void setIndices(const btAlignedObjectArray* nodes) { m_nodes = nodes; diff --git a/src/BulletSoftBody/btDeformableMousePickingForce.h b/src/BulletSoftBody/btDeformableMousePickingForce.h new file mode 100644 index 000000000..0e2ca064f --- /dev/null +++ b/src/BulletSoftBody/btDeformableMousePickingForce.h @@ -0,0 +1,143 @@ +/* + Written by Xuchen Han + + 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 BT_MOUSE_PICKING_FORCE_H +#define BT_MOUSE_PICKING_FORCE_H + +#include "btDeformableLagrangianForce.h" + +class btDeformableMousePickingForce : public btDeformableLagrangianForce +{ + // If true, the damping force will be in the direction of the spring + // If false, the damping force will be in the direction of the velocity + btScalar m_elasticStiffness, m_dampingStiffness; + const btSoftBody::Face& m_face; + btVector3 m_mouse_pos; + btScalar m_maxForce; +public: + typedef btAlignedObjectArray TVStack; + btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos), m_maxForce(maxForce) + { + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = scale * m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = scaled_stiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + force[m_face.m_n[i]->index] -= scaled_force; + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int i = 0; i < 3; ++i) + { + btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index]; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + local_scaled_df= scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir; + } + df[m_face.m_n[i]->index] -= local_scaled_df; + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btVector3 scaled_force = m_elasticStiffness * dir; + if (scaled_force.safeNorm() > m_maxForce) + { + scaled_force.safeNormalize(); + scaled_force *= m_maxForce; + } + energy += 0.5 * scaled_force.dot(dir); + } + return energy; + } + + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < 3; ++i) + { + btVector3 v_diff = m_face.m_n[i]->m_v; + btVector3 scaled_force = m_dampingStiffness * v_diff; + if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON) + { + btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized(); + scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir; + } + energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt; + } + return energy; + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + //TODO + } + + void setMousePos(const btVector3& p) + { + m_mouse_pos = p; + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MOUSE_PICKING_FORCE; + } + +}; + +#endif /* btMassSpring_h */ diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index e09cfb09c..f6a49a934 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -688,6 +688,24 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL } } +void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) +{ + btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + int removed_index = -1; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->removeSoftBody(psb); + if (forces[i]->m_softBodies.size() == 0) + removed_index = i; + break; + } + } + if (removed_index >= 0) + forces.removeAtIndex(removed_index); +} + void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) { m_softBodies.remove(body); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 5d78d4097..17512c8db 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -131,6 +131,8 @@ public: void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); + void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force); + void removeSoftBody(btSoftBody* body); void removeCollisionObject(btCollisionObject* collisionObject); @@ -267,7 +269,7 @@ public: if (softBody) { btSoftBody::sRayCast softResult; - if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) { if (softResult.fraction <= resultCallback.m_closestHitFraction) { @@ -278,8 +280,6 @@ public: btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); btVector3 normal = -rayDir; normal.normalize(); - - if (softResult.feature == btSoftBody::eFeature::Face) { normal = softBody->m_faces[softResult.index].m_normal; if (normal.dot(rayDir) > 0) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 70765a2d3..1bf68bb2a 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1936,6 +1936,25 @@ bool btSoftBody::rayTest(const btVector3& rayFrom, return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0); } +bool btSoftBody::rayFaceTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) +{ + if (m_faces.size() == 0) + return false; + else + { + if (m_fdbvt.empty()) + initializeFaceTree(); + } + + results.body = this; + results.fraction = 1.f; + results.index = -1; + + return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0); +} + // void btSoftBody::setSolver(eSolverPresets::_ preset) { @@ -2441,6 +2460,25 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo, return (cnt); } +int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, int& index) const +{ + int cnt = 0; + { /* Use dbvt */ + RayFromToCaster collider(rayFrom, rayTo, mint); + + btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider); + if (collider.m_face) + { + mint = collider.m_mint; + index = (int)(collider.m_face - &m_faces[0]); + cnt = 1; + } + } + return (cnt); +} + + // static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 8bb143681..6b3bb9800 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1031,6 +1031,11 @@ public: bool rayTest(const btVector3& rayFrom, const btVector3& rayTo, sRayCast& results); + bool rayFaceTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); + int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo, + btScalar& mint, int& index) const; /* Solver presets */ void setSolver(eSolverPresets::_ preset); /* predictMotion */