From 67b58428e872826140d9c85c7caf5acb0f4e77b3 Mon Sep 17 00:00:00 2001 From: jingyuc Date: Tue, 27 Jul 2021 15:59:25 -0400 Subject: [PATCH] clean up. now the inertia tensor is calculated based on geometry, and it is updated everytime step --- examples/ReducedDeformableDemo/BasicTest.cpp | 24 ++++++---- .../btReducedSoftBody.cpp | 44 ++++++++++++++++++- .../BulletReducedSoftBody/btReducedSoftBody.h | 26 +++++++++-- .../btReducedSoftBodyHelpers.cpp | 20 ++++++++- .../btReducedSoftBodyHelpers.h | 3 ++ .../btReducedSoftBodySolver.cpp | 30 +++++++------ .../btReducedSoftBodySolver.h | 11 ++--- 7 files changed, 123 insertions(+), 35 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index e8e965548..4be47189f 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -31,9 +31,11 @@ // static btScalar nu = 0.3; // static btScalar damping_alpha = 0.1; // static btScalar damping_beta = 0.01; -// static btScalar damping_alpha = 0.0; -// static btScalar damping_beta = 0.0; +static btScalar damping_alpha = 0.0; +static btScalar damping_beta = 0.01; static btScalar COLLIDING_VELOCITY = 0; +static int start_mode = 6; +static int num_modes = 2; class BasicTest : public CommonDeformableBodyBase { @@ -91,9 +93,9 @@ public: { // TODO: remove this. very hacky way of adding initial deformation btReducedSoftBody* rsb = static_cast(static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]); - if (first_step && !rsb->m_bUpdateRtCst) + if (first_step /* && !rsb->m_bUpdateRtCst*/) { - // getDeformedShape(rsb, 0, 0.5); + getDeformedShape(rsb, 0, 0.5); first_step = false; rsb->updateReducedDofs(); } @@ -130,6 +132,7 @@ void BasicTest::initPhysics() m_broadphase = new btDbvtBroadphase(); btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver(); + reducedSoftBodySolver->setDamping(damping_alpha, damping_beta); btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); sol->setDeformableSolver(reducedSoftBodySolver); @@ -146,7 +149,7 @@ void BasicTest::initPhysics() std::string filename = filepath + "mesh.vtk"; btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str()); - rsb->setReducedModes(6, 2, rsb->m_nodes.size()); + rsb->setReducedModes(start_mode, num_modes, rsb->m_nodes.size()); btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str()); getDeformableDynamicsWorld()->addSoftBody(rsb); @@ -154,6 +157,7 @@ void BasicTest::initPhysics() // rsb->translate(btVector3(0, 0, 0)); rsb->getCollisionShape()->setMargin(0.1); // rsb->setTotalMass(0.5); + rsb->setStiffnessScale(1); rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects rsb->m_cfg.kCHR = 1; // collision hardness with rigid body rsb->m_cfg.kDF = 0; @@ -162,11 +166,13 @@ void BasicTest::initPhysics() rsb->m_sleepingThreshold = 0; btSoftBodyHelpers::generateBoundaryFaces(rsb); - rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); + // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); + // rsb->setRigidVelocity(btVector3(0, 1, 0)); + // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); - btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); - getDeformableDynamicsWorld()->addForce(rsb, gravity_force); - m_forces.push_back(gravity_force); + // btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + // getDeformableDynamicsWorld()->addForce(rsb, gravity_force); + // m_forces.push_back(gravity_force); } getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 0afde723b..f276c1b23 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -1,4 +1,5 @@ #include "btReducedSoftBody.h" +#include "btReducedSoftBodyHelpers.h" #include "LinearMath/btTransformUtil.h" #include @@ -11,12 +12,15 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co m_nReduced = 0; m_nFull = 0; + m_ksScale = 1.0; + m_rhoScale = 1.0; + // rigid motion m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_angularVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_angularFactor.setValue(1, 1, 1); m_linearFactor.setValue(1, 1, 1); - m_invInertiaLocal.setValue(1, 1, 1); //TODO: get correct inertia through shape + m_invInertiaLocal.setValue(1, 1, 1); m_mass = 0.0; m_inverseMass = 0.0; } @@ -31,7 +35,7 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_ m_nodalMass.resize(full_size, 0); } -void btReducedSoftBody::setMass(const tDenseArray& mass_array) +void btReducedSoftBody::setMassProps(const tDenseArray& mass_array) { // nodal mass btScalar total_mass = 0; @@ -46,6 +50,34 @@ void btReducedSoftBody::setMass(const tDenseArray& mass_array) m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0; } +void btReducedSoftBody::setInertiaProps(const btVector3& inertia) +{ + // TODO: only support box shape now + // set local intertia + m_invInertiaLocal.setValue( + inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), + 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)); + + // update world inertia tensor + updateInertiaTensor(); +} + +void btReducedSoftBody::setRigidVelocity(const btVector3& v) +{ + m_linearVelocity = v; +} + +void btReducedSoftBody::setRigidAngularVelocity(const btVector3& omega) +{ + m_angularVelocity = omega; +} + +void btReducedSoftBody::setStiffnessScale(const btScalar ks) +{ + m_ksScale = ks; +} + void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) { btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); @@ -155,4 +187,12 @@ void btReducedSoftBody::applyFullSpaceImpulse(const btVector3& target_vel, int n } // impulse causes rigid motion applyImpulse(impulse, m_nodes[n_node].m_x); +} + +void btReducedSoftBody::applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta) +{ + for (int r = 0; r < m_nReduced; ++r) + { + reduced_force[r] += m_ksScale * m_Kr[r] * (m_reducedDofs[r] + damping_beta * m_reducedVelocity[r]); + } } \ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 40b5b4385..c970b3858 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -11,7 +11,9 @@ class btReducedSoftBody : public btSoftBody { private: - // + // scaling factors + btScalar m_rhoScale; // mass density scale + btScalar m_ksScale; // stiffness scale protected: // rigid frame @@ -62,7 +64,17 @@ class btReducedSoftBody : public btSoftBody void setReducedModes(int start_mode, int num_modes, int full_size); - void setMass(const tDenseArray& mass_array); + void setMassProps(const tDenseArray& mass_array); + + void setInertiaProps(const btVector3& inertia); + + void setRigidVelocity(const btVector3& v); + + void setRigidAngularVelocity(const btVector3& omega); + + void setStiffnessScale(const btScalar ks); + + void updateInertiaTensor(); void predictIntegratedTransform(btScalar step, btTransform& predictedTransform); @@ -85,15 +97,21 @@ class btReducedSoftBody : public btSoftBody // apply impulse to the rigid frame void applyImpulse(const btVector3& impulse, const btVector3& rel_pos); - + // apply impulse to nodes in the full space void applyFullSpaceImpulse(const btVector3& target_vel, int n_node, btScalar dt, tDenseArray& reduced_force); + // apply reduced force + void applyReducedInternalForce(tDenseArray& reduced_force, const btScalar damping_alpha, const btScalar damping_beta); + void proceedToTransform(const btTransform& newTrans); void setCenterOfMassTransform(const btTransform& xform); - void updateInertiaTensor(); + btScalar getTotalMass() const + { + return m_mass; + } const btVector3& getLinearVelocity() const { diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp index 386b60473..48dae2f3c 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp @@ -136,7 +136,12 @@ void btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(btReducedSoftB std::string M_file = std::string(file_path) + "M_diag_mat.bin"; btAlignedObjectArray mass_array; btReducedSoftBodyHelpers::readBinary(mass_array, 0, 3 * rsb->m_nFull, 3 * rsb->m_nFull, M_file.c_str()); - rsb->setMass(mass_array); + rsb->setMassProps(mass_array); + + // calculate the inertia tensor in the local frame + btVector3 inertia(0, 0, 0); + calculateLocalInertia(inertia, rsb->getTotalMass(), btVector3(4, 1, 0.5), btVector3(0, 0, 0)); + rsb->setInertiaProps(inertia); } // read in binary files @@ -226,4 +231,15 @@ void btReducedSoftBodyHelpers::readBinaryModes(btReducedSoftBody::tDenseMatrix& } } f_in.close(); -} \ No newline at end of file +} + +void btReducedSoftBodyHelpers::calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin) +{ + btScalar lx = btScalar(2.) * (half_extents[0] + margin[0]); + btScalar ly = btScalar(2.) * (half_extents[1] + margin[1]); + btScalar lz = btScalar(2.) * (half_extents[2] + margin[2]); + + inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + lz * lz), + mass / (btScalar(12.0)) * (lx * lx + ly * ly)); +} diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h index a1fb67b0b..38519e205 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h @@ -15,6 +15,9 @@ struct btReducedSoftBodyHelpers static void readBinaryMat(btReducedSoftBody::tDenseMatrix& mat, const unsigned int n_start, const unsigned int n_modes, const unsigned int n_full, const char* file); // read in modes file (different version of read in matrix) static void readBinaryModes(btReducedSoftBody::tDenseMatrix& mat, const unsigned int n_start, const unsigned int n_modes, const unsigned int n_full, const char* file); + + // calculate the local inertia tensor for a box shape reduced deformable object + static void calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin); }; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 024c022e7..7e87002ea 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -1,6 +1,18 @@ #include "btReducedSoftBodySolver.h" #include "../btDeformableMultiBodyDynamicsWorld.h" +btReducedSoftBodySolver::btReducedSoftBodySolver() +{ + m_dampingAlpha = 0; + m_dampingBeta = 0; +} + +void btReducedSoftBodySolver::setDamping(btScalar alpha, btScalar beta) +{ + m_dampingAlpha = alpha; + m_dampingBeta = beta; +} + void btReducedSoftBodySolver::predictMotion(btScalar solverdt) { applyForce(); @@ -25,15 +37,7 @@ void btReducedSoftBodySolver::applyForce() reduced_force.resize(rsb->m_reducedDofs.size(), 0); // add internal force (elastic force & damping force) - for (int r = 0; r < rsb->m_reducedDofs.size(); ++r) - { - // map all force to reduced - // for (int i = 0; i < force.size(); ++i) - // for (int k = 0; k < 3; ++k) - // reduced_force[r] += scale * rsb->m_modes[r][3 * i + k] * force[i][k]; - - reduced_force[r] += rsb->m_Kr[r] * (rsb->m_reducedDofs[r] + 0.1 * rsb->m_reducedVelocity[r]); - } + rsb->applyReducedInternalForce(reduced_force, m_dampingAlpha, m_dampingBeta); // apply impulses to reduced deformable objects static btScalar sim_time = 0; @@ -42,22 +46,22 @@ void btReducedSoftBodySolver::applyForce() { if (sim_time > 1 && apply_impulse == 0) { - rsb->applyFullSpaceImpulse(btVector3(0, 1, 0), 0, m_dt, reduced_force); + rsb->applyFullSpaceImpulse(btVector3(0, 1, 0), 0, 2.0 * m_dt, reduced_force); apply_impulse++; } if (sim_time > 2 && apply_impulse == 1) { - rsb->applyFullSpaceImpulse(btVector3(0, -1, 0), 0, m_dt, reduced_force); + rsb->applyFullSpaceImpulse(btVector3(0, -1.2, 0), 0, 2.0 * m_dt, reduced_force); apply_impulse++; } if (sim_time > 3 && apply_impulse == 2) { - rsb->applyFullSpaceImpulse(btVector3(1, 0, 0), 0, m_dt, reduced_force); + rsb->applyFullSpaceImpulse(btVector3(1.1, 0, 0), 0, 2.0 * m_dt, reduced_force); apply_impulse++; } if (sim_time > 4 && apply_impulse == 3) { - rsb->applyFullSpaceImpulse(btVector3(-1, 0, 0), 0, m_dt, reduced_force); + rsb->applyFullSpaceImpulse(btVector3(-1, 0, 0), 0, 2.0 * m_dt, reduced_force); apply_impulse++; } } diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h index 7a1e53366..6ee8b7430 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h @@ -9,21 +9,22 @@ class btReducedSoftBody; class btReducedSoftBodySolver : public btDeformableBodySolver { protected: + btScalar m_dampingAlpha; + btScalar m_dampingBeta; + void applyForce(); public: - btReducedSoftBodySolver() {} + btReducedSoftBodySolver(); ~btReducedSoftBodySolver() {} + void setDamping(btScalar alpha, btScalar beta); + virtual SolverTypes getSolverType() const { return REDUCED_DEFORMABLE_SOLVER; } - // virtual void predictMotion(btScalar solver_dt); - - // virtual void solveConstraints(btScalar solver_dt); - virtual void predictMotion(btScalar solverdt); virtual void applyExplicitForce();