diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 4be47189f..a26a758c9 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -95,7 +95,7 @@ public: btReducedSoftBody* rsb = static_cast(static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]); if (first_step /* && !rsb->m_bUpdateRtCst*/) { - getDeformedShape(rsb, 0, 0.5); + // getDeformedShape(rsb, 0, 0.5); first_step = false; rsb->updateReducedDofs(); } @@ -133,13 +133,14 @@ void BasicTest::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); reducedSoftBodySolver->setDamping(damping_alpha, damping_beta); + btVector3 gravity = btVector3(0, 0, 0); + reducedSoftBodySolver->setGravity(gravity); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); sol->setDeformableSolver(reducedSoftBodySolver); m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver); - btVector3 gravity = btVector3(0, -10, 0); m_dynamicsWorld->setGravity(gravity); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -153,9 +154,9 @@ void BasicTest::initPhysics() btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str()); getDeformableDynamicsWorld()->addSoftBody(rsb); - // rsb->scale(btVector3(1, 1, 1)); - // rsb->translate(btVector3(0, 0, 0)); rsb->getCollisionShape()->setMargin(0.1); + // rsb->scale(btVector3(1, 1, 1)); + rsb->translate(btVector3(0, 2, 0)); //TODO: add back translate and scale // rsb->setTotalMass(0.5); rsb->setStiffnessScale(1); rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index f276c1b23..bf564b024 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -83,7 +83,6 @@ void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransfor btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); } - void btReducedSoftBody::updateReducedDofs() { btAssert(m_reducedDofs.size() == m_nReduced); @@ -91,8 +90,13 @@ void btReducedSoftBody::updateReducedDofs() { m_reducedDofs[j] = 0; for (int i = 0; i < m_nFull; ++i) + { for (int k = 0; k < 3; ++k) + { + // std::cout << m_nodes[i].m_x[k] - m_x0[i][k] << "\n"; m_reducedDofs[j] += m_modes[j][3 * i + k] * (m_nodes[i].m_x[k] - m_x0[i][k]); + } + } } } @@ -141,6 +145,26 @@ void btReducedSoftBody::setCenterOfMassTransform(const btTransform& xform) updateInertiaTensor(); } +void btReducedSoftBody::translate(const btVector3& trs) +{ + // translate mesh + btSoftBody::translate(trs); + updateRestNodalPositions(); + // for (int i = 0; i < m_nFull; ++i) + // for (int k = 0; k < 3; ++k) + // std::cout << m_nodes[i].m_x[k] << "\t" << m_x0[i][k] << "\n"; + + // update rigid frame + // m_worldTransform.setOrigin(trs); +} + +void btReducedSoftBody::updateRestNodalPositions() +{ + m_x0.resize(m_nFull); + for (int i = 0; i < m_nFull; ++i) + m_x0[i] = m_nodes[i].m_x; +} + void btReducedSoftBody::updateInertiaTensor() { m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); @@ -189,6 +213,11 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n applyImpulse(impulse, m_nodes[n_node].m_x); } +void btReducedSoftBody::applyRigidGravity(const btVector3& gravity, btScalar dt) +{ + m_linearVelocity += dt * gravity; +} + void btReducedSoftBody::applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta) { for (int r = 0; r < m_nReduced; ++r) diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index c970b3858..60883c255 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -19,7 +19,6 @@ class btReducedSoftBody : public btSoftBody // rigid frame btScalar m_mass; // total mass of the rigid frame btScalar m_inverseMass; // inverse of the total mass of the rigid frame - btVector3 m_linearVelocity; btVector3 m_angularVelocity; btVector3 m_linearFactor; btVector3 m_angularFactor; @@ -34,6 +33,7 @@ class btReducedSoftBody : public btSoftBody typedef btAlignedObjectArray tDenseArray; typedef btAlignedObjectArray > tDenseMatrix; + btVector3 m_linearVelocity; // // Fields // @@ -74,6 +74,10 @@ class btReducedSoftBody : public btSoftBody void setStiffnessScale(const btScalar ks); + virtual void translate(const btVector3& trs); + + void updateRestNodalPositions(); + void updateInertiaTensor(); void predictIntegratedTransform(btScalar step, btTransform& predictedTransform); @@ -101,6 +105,9 @@ class btReducedSoftBody : public btSoftBody // apply impulse to nodes in the full space void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force); + // apply gravity to the rigid frame + void applyRigidGravity(const btVector3& gravity, btScalar dt); + // apply reduced force void applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp index 48dae2f3c..e8989876d 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp @@ -109,11 +109,6 @@ btReducedSoftBody* btReducedSoftBodyHelpers::createFromVtkFile(btSoftBodyWorldIn fs.close(); - // get rest position - rsb->m_x0.resize(rsb->m_nodes.size()); - for (int i = 0; i < rsb->m_nodes.size(); ++i) - rsb->m_x0[i] = rsb->m_nodes[i].m_x; - return rsb; } @@ -132,6 +127,9 @@ void btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(btReducedSoftB std::string modes_file = std::string(file_path) + "modes.bin"; btReducedSoftBodyHelpers::readBinaryModes(rsb->m_modes, rsb->m_startMode, rsb->m_nReduced, 3 * rsb->m_nFull, modes_file.c_str()); // default to 3D + // get rest position + rsb->updateRestNodalPositions(); + // read in full nodal mass std::string M_file = std::string(file_path) + "M_diag_mat.bin"; btAlignedObjectArray mass_array; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 7e87002ea..edc96219d 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -13,17 +13,14 @@ void btReducedSoftBodySolver::setDamping(btScalar alpha, btScalar beta) m_dampingBeta = beta; } +void btReducedSoftBodySolver::setGravity(const btVector3& gravity) +{ + m_gravity = gravity; +} + void btReducedSoftBodySolver::predictMotion(btScalar solverdt) { - applyForce(); - - // apply rigid motion - for (int i = 0; i < m_softBodies.size(); ++i) - { - btReducedSoftBody* rsb = static_cast(m_softBodies[i]); - - // rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform()); - } + applyExplicitForce(); } void btReducedSoftBodySolver::applyForce() @@ -51,12 +48,12 @@ void btReducedSoftBodySolver::applyForce() } if (sim_time > 2 && apply_impulse == 1) { - rsb->applyFullSpaceImpulse(btVector3(0, -1.2, 0), 0, 2.0 * m_dt, reduced_force); + rsb->applyFullSpaceImpulse(btVector3(0, -1.2, 0), 0, m_dt, reduced_force); apply_impulse++; } if (sim_time > 3 && apply_impulse == 2) { - rsb->applyFullSpaceImpulse(btVector3(1.1, 0, 0), 0, 2.0 * m_dt, reduced_force); + rsb->applyFullSpaceImpulse(btVector3(1.1, 0, 0), 0, m_dt, reduced_force); apply_impulse++; } if (sim_time > 4 && apply_impulse == 3) @@ -80,6 +77,14 @@ void btReducedSoftBodySolver::applyForce() void btReducedSoftBodySolver::applyExplicitForce() { + // apply gravity to the rigid frame + for (int i = 0; i < m_softBodies.size(); ++i) + { + btReducedSoftBody* rsb = static_cast(m_softBodies[i]); + rsb->applyRigidGravity(m_gravity, m_dt); + } + + // apply internal forces and impulses applyForce(); } diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h index 6ee8b7430..ea9e73e81 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h @@ -12,6 +12,8 @@ class btReducedSoftBodySolver : public btDeformableBodySolver btScalar m_dampingAlpha; btScalar m_dampingBeta; + btVector3 m_gravity; + void applyForce(); public: @@ -20,6 +22,8 @@ class btReducedSoftBodySolver : public btDeformableBodySolver void setDamping(btScalar alpha, btScalar beta); + void setGravity(const btVector3& gravity); + virtual SolverTypes getSolverType() const { return REDUCED_DEFORMABLE_SOLVER; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 5510b96b2..d96414b00 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1006,7 +1006,7 @@ public: /* Transform */ void transform(const btTransform& trs); /* Translate */ - void translate(const btVector3& trs); + virtual void translate(const btVector3& trs); /* Rotate */ void rotate(const btQuaternion& rot); /* Scale */