From 60dfe1fe697c92f95d7f18a3df35ad372a6cac73 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 16 Oct 2019 19:23:01 -0700 Subject: [PATCH] add support for anchor constraint between deformable and rigid --- .../DeformableDemo/DeformableClothAnchor.cpp | 236 ++++++++++++++++++ .../DeformableDemo/DeformableClothAnchor.h | 19 ++ examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 2 + .../btDeformableContactConstraint.cpp | 121 +++++++++ .../btDeformableContactConstraint.h | 26 ++ .../btDeformableContactProjection.cpp | 29 ++- .../btDeformableContactProjection.h | 2 + .../btDeformableMultiBodyDynamicsWorld.cpp | 7 + src/BulletSoftBody/btSoftBody.cpp | 35 +++ src/BulletSoftBody/btSoftBody.h | 10 +- 11 files changed, 487 insertions(+), 2 deletions(-) create mode 100644 examples/DeformableDemo/DeformableClothAnchor.cpp create mode 100644 examples/DeformableDemo/DeformableClothAnchor.h diff --git a/examples/DeformableDemo/DeformableClothAnchor.cpp b/examples/DeformableDemo/DeformableClothAnchor.cpp new file mode 100644 index 000000000..49fd3184b --- /dev/null +++ b/examples/DeformableDemo/DeformableClothAnchor.cpp @@ -0,0 +1,236 @@ +/* + 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 "DeformableClothAnchor.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" + +///The DeformableClothAnchor shows contact between deformable objects and rigid objects. +class DeformableClothAnchor : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + DeformableClothAnchor(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~DeformableClothAnchor() + { + } + 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); + } + + 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 DeformableClothAnchor::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, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + // 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, -50, 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); + } + + // create a piece of cloth + { + const btScalar s = 4; + const btScalar h = 6; + const int r = 9; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), r, r, 4 + 8, true); + psb->getCollisionShape()->setMargin(0.1); + 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(100,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); + + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0, h, -(s + 3.5))); + btRigidBody* body = createRigidBody(2, startTransform, new btBoxShape(btVector3(s, 1, 3))); + psb->appendDeformableAnchor(0, body); + psb->appendDeformableAnchor(r - 1, body); + } + getDeformableDynamicsWorld()->setImplicit(false); + getDeformableDynamicsWorld()->setLineSearch(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableClothAnchor::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* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableClothAnchor(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/DeformableClothAnchor.h b/examples/DeformableDemo/DeformableClothAnchor.h new file mode 100644 index 000000000..4e8388647 --- /dev/null +++ b/examples/DeformableDemo/DeformableClothAnchor.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 DEFORMABLE_CLOTH_ANCHOR_H +#define DEFORMABLE_CLOTH_ANCHOR_H + +class CommonExampleInterface* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options); + +#endif //DEFORMABLE_CLOTH_ANCHOR_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 7c31c9d84..56aaf625b 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -371,6 +371,8 @@ SET(BulletExampleBrowser_SRCS ../DeformableDemo/DeformableRigid.h ../DeformableDemo/VolumetricDeformable.cpp ../DeformableDemo/VolumetricDeformable.h + ../DeformableDemo/DeformableClothAnchor.cpp + ../DeformableDemo/DeformableClothAnchor.h ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h ../RigidBody/RigidBodySoftContact.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index e705fd075..887fb2fc5 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -53,6 +53,7 @@ #include "../DeformableDemo/VolumetricDeformable.h" #include "../DeformableDemo/GraspDeformable.h" #include "../DeformableDemo/DeformableContact.h" +#include "../DeformableDemo/DeformableClothAnchor.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsClientExample.h" @@ -198,6 +199,7 @@ static ExampleEntry gDefaultExamples[] = 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), + ExampleEntry(1, "Deformable Cloth Anchor", "Deformable Anchor test", DeformableClothAnchorCreateFunc), ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc), // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index ce5018f2f..3bb20a6e1 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -15,6 +15,127 @@ #include "btDeformableContactConstraint.h" +/* ================ Deformable Node Anchor =================== */ +btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a) +: m_anchor(&a) +, btDeformableContactConstraint(a.m_cti.m_normal) +{ +} + +btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other) +: m_anchor(other.m_anchor) +, btDeformableContactConstraint(other) +{ +} + +btVector3 btDeformableNodeAnchorConstraint::getVa() const +{ + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + // multibody anchor not supported yet + btAssert(false); + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_t1[k]; + } + va += m_anchor->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_t2[k]; + } + va += m_anchor->t2 * vel; + } + } + } + return va; +} + +btScalar btDeformableNodeAnchorConstraint::solveConstraint() +{ + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = (vb - va); + // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0 + const btScalar dn = btDot(vr, cti.m_normal); + // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt + btScalar residualSquare = dn*dn; + btVector3 impulse = m_anchor->m_c0 * vr; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_anchor->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1)); + const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2)); + } + } + return residualSquare; +} + +btVector3 btDeformableNodeAnchorConstraint::getVb() const +{ + return m_anchor->m_node->m_v; +} + +void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse) +{ + btVector3 dv = impulse * m_anchor->m_c2; + m_anchor->m_node->m_v -= dv; +} + /* ================ Deformable vs. Rigid =================== */ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c) : m_contact(&c) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index f79bc6065..da16c51e5 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -108,6 +108,32 @@ public: virtual void applyImpulse(const btVector3& impulse){} }; +// +// Anchor Constraint between rigid and deformable node +class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint +{ +public: + const btSoftBody::DeformableNodeRigidAnchor* m_anchor; + + btDeformableNodeAnchorConstraint(){} + btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c); + btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); + virtual ~btDeformableNodeAnchorConstraint() + { + } + virtual btScalar solveConstraint(); + // 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 + virtual btVector3 getVb() const; + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0,0,0); + } + virtual void applyImpulse(const btVector3& impulse); +}; + + // // Constraint between rigid/multi body and deformable objects class btDeformableRigidContactConstraint : public btDeformableContactConstraint diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index cf0a5e896..d438d6eec 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -32,6 +32,14 @@ btScalar btDeformableContactProjection::update() } } + // anchor constraints + for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) + { + btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index); + btScalar localResidualSquare = constraint.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + // face constraints for (int index = 0; index < m_allFaceConstraints.size(); ++index) { @@ -73,6 +81,24 @@ void btDeformableContactProjection::setConstraints() continue; } + // set up deformable anchors + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j]; + // skip fixed points + if (anchor.m_node->m_im == 0) + { + continue; + } + + if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL) + { + anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; + btDeformableNodeAnchorConstraint constraint(anchor); + m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint); + } + } + // set Deformable Node vs. Rigid constraint for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { @@ -262,7 +288,7 @@ void btDeformableContactProjection::setProjection() bool existStaticConstraint = false; btVector3 averagedNormal(0,0,0); btAlignedObjectArray normals; - if (m_staticConstraints.find(index) != NULL) + if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL) { existStaticConstraint = true; hasConstraint = true; @@ -451,6 +477,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) void btDeformableContactProjection::reinitialize(bool nodeUpdated) { m_staticConstraints.clear(); + m_nodeAnchorConstraints.clear(); m_nodeRigidConstraints.clear(); m_faceRigidConstraints.clear(); m_deformableConstraints.clear(); diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index fa5eb662b..90386b0c9 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -36,6 +36,8 @@ public: btHashMap > m_faceRigidConstraints; // map from node index to deformable constraint btHashMap > m_deformableConstraints; + // map from node index to node anchor constraint + btHashMap m_nodeAnchorConstraints; // all constraints involving face btAlignedObjectArray m_allFaceConstraints; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index ded9893ce..242f84bee 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -140,6 +140,13 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) node.m_q = node.m_x; node.m_vn = node.m_v; } + // enforce anchor constraints + for (int j = 0; j < psb->m_deformableAnchors.size();++j) + { + btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; + btSoftBody::Node* n = a.m_node; + n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + } psb->interpolateRenderMesh(); } } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index b05b28ca4..c25f4441b 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -411,6 +411,41 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca m_anchors.push_back(a); } +// +void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body) +{ + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + const btScalar imb = body->getInvMass(); + btVector3 nrm; + const btCollisionShape* shp = body->getCollisionShape(); + const btTransform& wtr = body->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + + c.m_cti.m_colObj = body; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * body->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + m_deformableAnchors.push_back(c); +} + // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 195434c06..8aca75e1e 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -354,6 +354,12 @@ public: Node* m_node; // Owner node }; + class DeformableNodeRigidAnchor : public DeformableNodeRigidContact + { + public: + btVector3 m_local; // Anchor position in body space + }; + class DeformableFaceRigidContact : public DeformableRigidContact { public: @@ -774,6 +780,7 @@ public: btAlignedObjectArray m_tetraScratches; btAlignedObjectArray m_tetraScratchesTn; tAnchorArray m_anchors; // Anchors + btAlignedObjectArray m_deformableAnchors; tRContactArray m_rcontacts; // Rigid contacts btAlignedObjectArray m_nodeRigidContacts; btAlignedObjectArray m_faceNodeContacts; @@ -897,7 +904,8 @@ public: Material* mat = 0); /* Append anchor */ - void appendAnchor(int node, + void appendDeformableAnchor(int node, btRigidBody* body); + void appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); /* Append linear joint */