From e70fc6453bca2471ae2d8271a80c294e03bcfa5f Mon Sep 17 00:00:00 2001 From: jingyuc Date: Thu, 2 Sep 2021 21:32:16 -0400 Subject: [PATCH] add support for initial rotation --- examples/ReducedDeformableDemo/BasicTest.cpp | 19 +++--- .../btReducedSoftBody.cpp | 59 +++++++------------ .../BulletReducedSoftBody/btReducedSoftBody.h | 19 +++++- src/BulletSoftBody/btSoftBody.h | 6 +- 4 files changed, 51 insertions(+), 52 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index fe2bd542c..2ffc3eb8f 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -198,7 +198,7 @@ void BasicTest::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); - btVector3 gravity = btVector3(0, -10, 0); + btVector3 gravity = btVector3(0, 0, 0); reducedSoftBodySolver->setGravity(gravity); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); @@ -220,17 +220,22 @@ void BasicTest::initPhysics() getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); - // rsb->scale(btVector3(1, 1, 1)); //TODO: add back scale - rsb->translate(btVector3(0, 4, 0)); + + btTransform init_transform; + init_transform.setIdentity(); + init_transform.setOrigin(btVector3(0, 4, 0)); + init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0)); + rsb->transform(init_transform); + // rsb->setTotalMass(0.5); rsb->setStiffnessScale(100); rsb->setDamping(damping_alpha, damping_beta); // set fixed nodes - rsb->setFixedNodes(0); - rsb->setFixedNodes(1); - rsb->setFixedNodes(2); - rsb->setFixedNodes(3); + // rsb->setFixedNodes(0); + // rsb->setFixedNodes(1); + // rsb->setFixedNodes(2); + // rsb->setFixedNodes(3); rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects rsb->m_cfg.kCHR = 1; // collision hardness with rigid body diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index c7d66494e..fe15dd2ee 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -82,31 +82,12 @@ void btReducedSoftBody::setInertiaProps(const btVector3& inertia) inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); - // // CoM - // btVector3 x_com(0,0,0); - // for (int i = 0; i < m_nFull; ++i) - // { - // x_com += m_nodalMass[i] * m_nodes[i].m_x; - // } - // x_com /= m_mass; - - // btMatrix3x3 inertia_temp; - // inertia_temp.setZero(); - // for (int i = 0; i < m_nFull; ++i) - // { - // btVector3 ri = m_nodes[i].m_x - x_com; - // for (int a = 0; a < 3; ++a) - // { - // for (int b = 0; b < 3; ++b) - // { - // inertia_temp[a][b] += m_nodalMass[i] * ri[a] * ri[b]; - // } - // } - // } - // m_invInertiaLocal = inertia_temp.inverse(); // update world inertia tensor + btMatrix3x3 rotation; + rotation.setIdentity(); + updateInitialInertiaTensor(rotation); updateInertiaTensor(); m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; } @@ -363,28 +344,21 @@ void btReducedSoftBody::proceedToTransform(btScalar dt, bool end_of_time_step) m_invInertiaTensorWorld = m_interpolateInvInertiaTensorWorld; } -void btReducedSoftBody::translate(const btVector3& trs) +void btReducedSoftBody::transform(const btTransform& trs) { // translate mesh - btSoftBody::translate(trs); + btSoftBody::transform(trs); updateRestNodalPositions(); - // update rigid frame - m_rigidTransformWorld.setOrigin(trs); + // update rigid frame (No need to update the rotation. Nodes have already been updated.) + m_rigidTransformWorld.setOrigin(trs.getOrigin()); m_interpolationWorldTransform = m_rigidTransformWorld; m_initialOrigin = m_rigidTransformWorld.getOrigin(); - updateInertiaTensor(); -} -void btReducedSoftBody::rotate(const btQuaternion& rot) -{ - // translate mesh - btSoftBody::rotate(rot); - updateRestNodalPositions(); - - // update rigid frame - m_rigidTransformWorld.setRotation(rot); - m_interpolationWorldTransform = m_rigidTransformWorld; + updateLocalMomentArm(); + + // update inertia tensor + updateInitialInertiaTensor(trs.getBasis()); updateInertiaTensor(); } @@ -393,12 +367,19 @@ void btReducedSoftBody::updateRestNodalPositions() // update reset nodal position m_x0.resize(m_nFull); for (int i = 0; i < m_nFull; ++i) - m_x0[i] = m_nodes[i].m_x; + { + m_x0[i] = m_nodes[i].m_x; + } +} + +void btReducedSoftBody::updateInitialInertiaTensor(const btMatrix3x3& rotation) +{ + m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose(); } void btReducedSoftBody::updateInertiaTensor() { - m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis().scaled(m_invInertiaLocal) * m_rigidTransformWorld.getBasis().transpose(); + m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis() * m_invInertiaTensorWorldInitial * m_rigidTransformWorld.getBasis().transpose(); } void btReducedSoftBody::applyDamping(btScalar timeStep) diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index f837b9a33..44666a6c6 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -48,6 +48,7 @@ class btReducedSoftBody : public btSoftBody btVector3 m_angularFactor; btVector3 m_invInertiaLocal; btTransform m_rigidTransformWorld; + btMatrix3x3 m_invInertiaTensorWorldInitial; btMatrix3x3 m_invInertiaTensorWorld; btMatrix3x3 m_interpolateInvInertiaTensorWorld; btVector3 m_initialOrigin; // initial center of mass (original of the m_rigidTransformWorld) @@ -121,12 +122,24 @@ class btReducedSoftBody : public btSoftBody // // various internal updates // - virtual void translate(const btVector3& trs); - - virtual void rotate(const btQuaternion& rot); + virtual void transform(const btTransform& trs); + virtual void translate(const btVector3& trs) + { + btAssert(false); // use transform(). + } + virtual void rotate(const btQuaternion& rot) + { + btAssert(false); // use transform(). + } + virtual void scale(const btVector3& scl) + { + btAssert(false); // scale is NOT supported in the reduced deformable body + } void updateRestNodalPositions(); + void updateInitialInertiaTensor(const btMatrix3x3& rotation); + void updateInertiaTensor(); void updateLocalMomentArm(); diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index aa5fc5e9d..7884ff47f 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1006,13 +1006,13 @@ public: /* Transform to given pose */ void transformTo(const btTransform& trs); /* Transform */ - void transform(const btTransform& trs); + virtual void transform(const btTransform& trs); /* Translate */ virtual void translate(const btVector3& trs); /* Rotate */ - void rotate(const btQuaternion& rot); + virtual void rotate(const btQuaternion& rot); /* Scale */ - void scale(const btVector3& scl); + virtual void scale(const btVector3& scl); /* Get link resting lengths scale */ btScalar getRestLengthScale(); /* Scale resting length of all springs */