From deb7c152c41d3b3111338d96bbbfb5f64862985a Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 8 Aug 2019 10:14:18 -0700 Subject: [PATCH] add corotated model to lagrangian forces --- examples/ExampleBrowser/ExampleEntries.cpp | 2 +- examples/GraspDeformable/GraspDeformable.cpp | 5 +- .../btDeformableBackwardEulerObjective.h | 1 + .../btDeformableCorotatedForce.h | 143 ++++++++++++++++++ .../btDeformableLagrangianForce.h | 3 +- src/BulletSoftBody/btSoftBody.cpp | 19 +++ src/BulletSoftBody/btSoftBody.h | 4 + src/BulletSoftBody/btSoftBodyHelpers.cpp | 1 + 8 files changed, 174 insertions(+), 4 deletions(-) create mode 100644 src/BulletSoftBody/btDeformableCorotatedForce.h diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 030f4eef1..ae40afec9 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -197,7 +197,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(0, "Deformabe Body"), ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc), ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), - ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableaCreateFunc), + ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc), ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc), // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), diff --git a/examples/GraspDeformable/GraspDeformable.cpp b/examples/GraspDeformable/GraspDeformable.cpp index b291dbafb..7d42ffa43 100644 --- a/examples/GraspDeformable/GraspDeformable.cpp +++ b/examples/GraspDeformable/GraspDeformable.cpp @@ -332,14 +332,15 @@ void GraspDeformable::initPhysics() psb->translate(btVector3(0, 0, 0)); psb->getCollisionShape()->setMargin(0.1); psb->setTotalMass(1); - psb->setSpringStiffness(2); - psb->setDampingCoefficient(0.02); + psb->setSpringStiffness(0); + psb->setDampingCoefficient(0.04); 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; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(2,2)); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index f87f7f509..2f44bf016 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -17,6 +17,7 @@ #include "btDeformableLagrangianForce.h" #include "btDeformableMassSpringForce.h" #include "btDeformableGravityForce.h" +#include "btDeformableCorotatedForce.h" #include "btDeformableContactProjection.h" #include "btPreconditioner.h" #include "btDeformableRigidDynamicsWorld.h" diff --git a/src/BulletSoftBody/btDeformableCorotatedForce.h b/src/BulletSoftBody/btDeformableCorotatedForce.h new file mode 100644 index 000000000..f4d42453f --- /dev/null +++ b/src/BulletSoftBody/btDeformableCorotatedForce.h @@ -0,0 +1,143 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 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_COROTATED_H +#define BT_COROTATED_H + +#include "btDeformableLagrangianForce.h" +#include "LinearMath/btPolarDecomposition.h" + +static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) +{ + static const btPolarDecomposition polar; + return polar.decompose(m, q, s); +} + +class btDeformableCorotatedForce : public btDeformableLagrangianForce +{ +public: + typedef btAlignedObjectArray TVStack; + btScalar m_mu, m_lambda; + btDeformableCorotatedForce(): m_mu(1), m_lambda(1) + { + + } + + btDeformableCorotatedForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda) + { + } + + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()) + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + updateDs(tetra); + btMatrix3x3 F = tetra.m_ds * tetra.m_Dm_inverse; + btMatrix3x3 P; + firstPiola(F,P); + btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); + btVector3 force_on_node0 = P * tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col; + btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + + // elastic force + // explicit elastic force + force[id0] -= scale * tetra.m_element_measure * force_on_node0; + force[id1] -= scale * tetra.m_element_measure * force_on_node123.getColumn(0); + force[id2] -= scale * tetra.m_element_measure * force_on_node123.getColumn(1); + force[id3] -= scale * tetra.m_element_measure * force_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) + { + btMatrix3x3 R,S; + btScalar J = F.determinant(); + if (J < 1024 * SIMD_EPSILON) + R.setIdentity(); + else + PolarDecompose(F, R, S); // this QR is not robust, consider using implicit shift svd + /*https://fuchuyuan.github.io/research/svd/paper.pdf*/ + + btMatrix3x3 JFinvT = F.adjoint(); + P = JFinvT * (m_lambda * (J-1)) + (F-R) * 2 * m_mu; + } + void updateDs(btSoftBody::Tetra& t) + { + btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; + btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; + btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; + btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_ds = Ds; + } + + virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + btScalar scaled_k_damp = psb->m_dampingCoefficient * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); + df[id1] += local_scaled_df; + df[id2] -= local_scaled_df; + } + } + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_COROTATED_FORCE; + } + +}; + + +#endif /* btCorotated_h */ diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index eb4d032bf..2146d4ec5 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -20,7 +20,8 @@ enum btDeformableLagrangianForceType { BT_GRAVITY_FORCE = 1, - BT_MASSSPRING_FORCE = 2 + BT_MASSSPRING_FORCE = 2, + BT_COROTATED_FORCE = 3 }; class btDeformableLagrangianForce diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 86a48bf62..f65851dec 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -854,6 +854,7 @@ void btSoftBody::scale(const btVector3& scl) updateNormals(); updateBounds(); updateConstants(); + initializeDmInverse(); } // @@ -2811,6 +2812,24 @@ void btSoftBody::setSpringStiffness(btScalar k) } } +void btSoftBody::initializeDmInverse() +{ + btScalar unit_simplex_measure = 1./6.; + + for (int i = 0; i < m_tetras.size(); ++i) + { + Tetra &t = m_tetras[i]; + btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; + btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; + btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; + btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + t.m_element_measure = Dm.determinant() * unit_simplex_measure; + t.m_Dm_inverse = Dm.inverse(); + } +} + // void btSoftBody::Joint::Prepare(btScalar dt, int) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index f2934c94a..0ee9e827d 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -292,6 +292,9 @@ public: btVector3 m_c0[4]; // gradients btScalar m_c1; // (4*kVST)/(im0+im1+im2+im3) btScalar m_c2; // m_c1/sum(|g0..3|^2) + btMatrix3x3 m_Dm_inverse; // rest Dm^-1 + btMatrix3x3 m_ds; + btScalar m_element_measure; }; /* RContact */ struct RContact @@ -1023,6 +1026,7 @@ public: void applyClusters(bool drift); void dampClusters(); void setSpringStiffness(btScalar k); + void initializeDmInverse(); void applyForces(); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index d0a9921d8..b3c5c88a9 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1221,6 +1221,7 @@ if(face&&face[0]) } } } + psb->initializeDmInverse(); printf("Nodes: %u\r\n", psb->m_nodes.size()); printf("Links: %u\r\n", psb->m_links.size()); printf("Faces: %u\r\n", psb->m_faces.size());