From bf8cf19665c0f947ade07b9dca9a8a8db1270709 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 24 Jun 2020 17:48:43 -0700 Subject: [PATCH] support implicit integration in picking force --- .../CommonDeformableBodyBase.h | 6 ++++-- .../DeformableDemo/VolumetricDeformable.cpp | 3 +++ .../btDeformableMousePickingForce.h | 19 ++++++++++++++++++- src/BulletSoftBody/btSoftBodyInternals.h | 8 ++++++-- 4 files changed, 31 insertions(+), 5 deletions(-) diff --git a/examples/CommonInterfaces/CommonDeformableBodyBase.h b/examples/CommonInterfaces/CommonDeformableBodyBase.h index 2a5e3b687..90dbf8f06 100644 --- a/examples/CommonInterfaces/CommonDeformableBodyBase.h +++ b/examples/CommonInterfaces/CommonDeformableBodyBase.h @@ -21,11 +21,13 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase btAlignedObjectArray m_forces; btSoftBody* m_pickedSoftBody; btDeformableMousePickingForce* m_mouseForce; - btScalar m_maxPickingForce; + btScalar m_pickingForceElasticStiffness, m_pickingForceDampingStiffness, m_maxPickingForce; CommonDeformableBodyBase(GUIHelperInterface* helper) : CommonMultiBodyBase(helper), m_pickedSoftBody(0), m_mouseForce(0), + m_pickingForceElasticStiffness(100), + m_pickingForceDampingStiffness(0.0), m_maxPickingForce(0.3) { } @@ -115,7 +117,7 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase m_pickedSoftBody = psb; psb->setActivationState(DISABLE_DEACTIVATION); const btSoftBody::Face& f = psb->m_faces[face_id]; - btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0, f, m_hitPos, m_maxPickingForce); + btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(m_pickingForceElasticStiffness, m_pickingForceDampingStiffness, f, m_hitPos, m_maxPickingForce); m_mouseForce = mouse_force; getDeformableDynamicsWorld()->addForce(psb, mouse_force); } diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index b8cd3e1b3..bfc51a156 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -46,6 +46,9 @@ public: : CommonDeformableBodyBase(helper) { m_linearElasticity = 0; + m_pickingForceElasticStiffness = 100; + m_pickingForceDampingStiffness = 0; + m_maxPickingForce = 1e10; // allow large picking force with implicit scheme. } virtual ~VolumetricDeformable() { diff --git a/src/BulletSoftBody/btDeformableMousePickingForce.h b/src/BulletSoftBody/btDeformableMousePickingForce.h index 392bf5ac2..d218d9621 100644 --- a/src/BulletSoftBody/btDeformableMousePickingForce.h +++ b/src/BulletSoftBody/btDeformableMousePickingForce.h @@ -128,7 +128,24 @@ public: virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) { - //TODO + 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); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + int id = m_face.m_n[i]->index; + btVector3 dx_diff = dx[id]; + btScalar r = 0; // rest length is 0 for picking spring + btVector3 scaled_df = btVector3(0, 0, 0); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff; + } + df[id] += scaled_df; + } } void setMousePos(const btVector3& p) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index f660c2bde..8d13b39bd 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1012,12 +1012,16 @@ static inline btMatrix3x3 ImpulseMatrix(btScalar dt, // static inline btMatrix3x3 ImpulseMatrix(btScalar dt, - const btMatrix3x3& effective_mass, + const btMatrix3x3& effective_mass_inv, btScalar imb, const btMatrix3x3& iwi, const btVector3& r) { - return (Diagonal(1 / dt) * Add(effective_mass, MassMatrix(imb, iwi, r)).inverse()); + return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse()); + // btMatrix3x3 iimb = MassMatrix(imb, iwi, r); + // if (iimb.determinant() == 0) + // return effective_mass_inv.inverse(); + // return effective_mass_inv.inverse() * Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse(); } //