From 05bc3870817e155b498663b42017103383d9eeb5 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 31 Oct 2019 14:31:51 -0700 Subject: [PATCH 1/5] update example to use absolute path --- examples/DeformableDemo/GraspDeformable.cpp | 40 +++++++++++---------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index ddb47954b..d96394435 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -28,6 +28,7 @@ #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../Utils/b3BulletDefaultFileIO.h" #include "../CommonInterfaces/CommonMultiBodyBase.h" #include "../CommonInterfaces/CommonGraphicsAppInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h" @@ -270,25 +271,26 @@ void GraspDeformable::initPhysics() } // create a soft block - if(1) - { - char relative_path[1024]; -// b3FileUtils::findFile("banana.vtk", relative_path, 1024); -// b3FileUtils::findFile("ball.vtk", relative_path, 1024); -// b3FileUtils::findFile("deformable_crumpled_napkin_sim.vtk", relative_path, 1024); -// b3FileUtils::findFile("single_tet.vtk", relative_path, 1024); -// b3FileUtils::findFile("tube.vtk", relative_path, 1024); -// b3FileUtils::findFile("torus.vtk", relative_path, 1024); -// b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024); -// b3FileUtils::findFile("bread.vtk", relative_path, 1024); - b3FileUtils::findFile("ditto.vtk", relative_path, 1024); -// b3FileUtils::findFile("boot.vtk", relative_path, 1024); -// btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), -// TetraCube::getElements(), -// 0, -// TetraCube::getNodes(), -// false, true, true); - btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), relative_path); + if (1) + { + // b3FileUtils::findFile("banana.vtk", relative_path, 1024); + // b3FileUtils::findFile("ball.vtk", relative_path, 1024); + // b3FileUtils::findFile("deformable_crumpled_napkin_sim.vtk", relative_path, 1024); + // b3FileUtils::findFile("single_tet.vtk", relative_path, 1024); + // b3FileUtils::findFile("tube.vtk", relative_path, 1024); + // b3FileUtils::findFile("torus.vtk", relative_path, 1024); + // b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024); + // b3FileUtils::findFile("bread.vtk", relative_path, 1024); + char absolute_path[1024]; + b3BulletDefaultFileIO fileio; + fileio.findResourcePath("ditto.vtk", absolute_path, 1024); + // b3FileUtils::findFile("boot.vtk", relative_path, 1024); + // btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + // TetraCube::getElements(), + // 0, + // TetraCube::getNodes(), + // false, true, true); + btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), absolute_path); // psb->scale(btVector3(30, 30, 30)); // for banana psb->scale(btVector3(.7, .7, .7)); From 09ca930ca8b6f737b7b34efdbbfaababaf29e2d4 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 31 Oct 2019 15:07:10 -0700 Subject: [PATCH 2/5] make consistent for other models --- examples/DeformableDemo/GraspDeformable.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index d96394435..a1952169d 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -273,18 +273,18 @@ void GraspDeformable::initPhysics() // create a soft block if (1) { - // b3FileUtils::findFile("banana.vtk", relative_path, 1024); - // b3FileUtils::findFile("ball.vtk", relative_path, 1024); - // b3FileUtils::findFile("deformable_crumpled_napkin_sim.vtk", relative_path, 1024); - // b3FileUtils::findFile("single_tet.vtk", relative_path, 1024); - // b3FileUtils::findFile("tube.vtk", relative_path, 1024); - // b3FileUtils::findFile("torus.vtk", relative_path, 1024); - // b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024); - // b3FileUtils::findFile("bread.vtk", relative_path, 1024); char absolute_path[1024]; b3BulletDefaultFileIO fileio; fileio.findResourcePath("ditto.vtk", absolute_path, 1024); - // b3FileUtils::findFile("boot.vtk", relative_path, 1024); + // fileio.findResourcePath("banana.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("paper_roll.vtk", absolute_path, 1024); + // fileio.findResourcePath("bread.vtk", absolute_path, 1024); + // fileio.findResourcePath("boot.vtk", absolute_path, 1024); // btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), // TetraCube::getElements(), // 0, From fb85b2e05f26a77146d3906c3b2f1912442492dd Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 4 Nov 2019 13:10:17 -0800 Subject: [PATCH 3/5] add a linear elasticity model --- .../btDeformableBackwardEulerObjective.h | 1 + .../btDeformableLagrangianForce.h | 3 +- .../btDeformableLinearElasticityForce.h | 340 ++++++++++++++++++ 3 files changed, 343 insertions(+), 1 deletion(-) create mode 100644 src/BulletSoftBody/btDeformableLinearElasticityForce.h diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 7699a195e..05ab42ff0 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 "btDeformableLinearElasticityForce.h" #include "btDeformableNeoHookeanForce.h" #include "btDeformableContactProjection.h" #include "btPreconditioner.h" diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index cab48837d..64e80e23b 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -25,7 +25,8 @@ enum btDeformableLagrangianForceType BT_GRAVITY_FORCE = 1, BT_MASSSPRING_FORCE = 2, BT_COROTATED_FORCE = 3, - BT_NEOHOOKEAN_FORCE = 4 + BT_NEOHOOKEAN_FORCE = 4, + BT_LINEAR_ELASTICITY_FORCE = 5 }; static inline double randomDouble(double low, double high) diff --git a/src/BulletSoftBody/btDeformableLinearElasticityForce.h b/src/BulletSoftBody/btDeformableLinearElasticityForce.h new file mode 100644 index 000000000..106dc10ad --- /dev/null +++ b/src/BulletSoftBody/btDeformableLinearElasticityForce.h @@ -0,0 +1,340 @@ +/* + 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_LINEAR_ELASTICITY_H +#define BT_LINEAR_ELASTICITY_H + +#include "btDeformableLagrangianForce.h" +#include "LinearMath/btQuickprof.h" +class btDeformableLinearElasticityForce : public btDeformableLagrangianForce +{ +public: + typedef btAlignedObjectArray TVStack; + btScalar m_mu, m_lambda; + btScalar m_mu_damp, m_lambda_damp; + btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1) + { + btScalar damping = 0.05; + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; + } + + btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) + { + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; + } + + virtual void addScaledForces(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + 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; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual double totalElasticEnergy(btScalar dt) + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetraScratches.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::TetraScratch& s = psb->m_tetraScratches[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(s); + } + } + return energy; + } + + // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual double totalDampingEnergy(btScalar dt) + { + double energy = 0; + int sz = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + sz = btMax(sz, psb->m_nodes[j].index); + } + } + TVStack dampingForce; + dampingForce.resize(sz+1); + for (int i = 0; i < dampingForce.size(); ++i) + dampingForce[i].setZero(); + addScaledDampingForce(0.5, dampingForce); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + energy -= dampingForce[node.index].dot(node.m_v) / dt; + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::TetraScratch& s) + { + double density = 0; + btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2()); + density += m_lambda * trace * trace * 0.5; + return density; + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j],P); +#if USE_SVD + if (max_p > 0) + { + // since we want to clamp the principal stress to max_p, we only need to + // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p + btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2()); + if (trPTP > max_p * max_p) + { + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; + P = U * Sigma * V.transpose(); + } + } +#endif + // 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(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + + 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 + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * force_on_node0; + force[id1] -= scale1 * force_on_node123.getColumn(0); + force[id2] -= scale1 * force_on_node123.getColumn(1); + force[id3] -= scale1 * force_on_node123.getColumn(2); + } + } + } + + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + if (m_mu_damp == 0 && m_lambda_damp == 0) + return; + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + 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; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + btMatrix3x3 I; + I.setIdentity(); + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; + // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()); + btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + 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; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); + // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + + void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) + { + btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; + P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let P be the first piola stress. + // This function calculates the dP = dP/dF * dF + void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; + } + + // Let Q be the damping stress. + // This function calculates the dP = dQ/dF * dF + void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); + dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace; + } + + virtual btDeformableLagrangianForceType getForceType() + { + return BT_LINEAR_ELASTICITY_FORCE; + } + +}; +#endif /* BT_LINEAR_ELASTICITY_H */ From 13314360a8e21f81b2ea49bd313d5ac123a9ae9a Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 5 Nov 2019 18:07:32 -0800 Subject: [PATCH 4/5] add option for deformable rigid split impulse --- examples/DeformableDemo/SplitImpulse.cpp | 259 ++++++++++++++++++ examples/DeformableDemo/SplitImpulse.h | 19 ++ examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 2 + src/BulletDynamics/Dynamics/btRigidBody.h | 42 +++ src/BulletSoftBody/btDeformableBodySolver.cpp | 14 +- src/BulletSoftBody/btDeformableBodySolver.h | 6 + .../btDeformableContactConstraint.cpp | 60 ++++ .../btDeformableContactConstraint.h | 47 ++++ .../btDeformableContactProjection.cpp | 51 ++++ .../btDeformableContactProjection.h | 7 +- .../btDeformableMultiBodyConstraintSolver.cpp | 37 +++ .../btDeformableMultiBodyConstraintSolver.h | 2 + .../btDeformableMultiBodyDynamicsWorld.cpp | 25 ++ src/BulletSoftBody/btSoftBody.h | 1 + 15 files changed, 571 insertions(+), 3 deletions(-) create mode 100644 examples/DeformableDemo/SplitImpulse.cpp create mode 100644 examples/DeformableDemo/SplitImpulse.h 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 From b55ebac2d917bfb3c1476a95721771a9a78860e6 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 5 Nov 2019 18:09:38 -0800 Subject: [PATCH 5/5] update CMakeLists --- src/BulletSoftBody/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 408d5b5d8..f20fb4c0a 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -47,6 +47,7 @@ SET(BulletSoftBody_HDRS btDeformableMassSpringForce.h btDeformableCorotatedForce.h btDeformableNeoHookeanForce.h + btDeformableLinearElasticityForce.h btDeformableLagrangianForce.h btPreconditioner.h