From 0983d36effaf77cbb7d6f4d0db5e8858e535dbd3 Mon Sep 17 00:00:00 2001 From: jyc-n Date: Mon, 26 Jul 2021 21:48:57 -0400 Subject: [PATCH] rotation is working --- examples/ReducedDeformableDemo/BasicTest.cpp | 29 ++++++------------- .../btReducedSoftBody.cpp | 22 ++++++++++---- .../BulletReducedSoftBody/btReducedSoftBody.h | 9 +++--- .../btReducedSoftBodyHelpers.cpp | 16 +++------- .../btReducedSoftBodySolver.cpp | 5 +++- 5 files changed, 38 insertions(+), 43 deletions(-) diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 13835c741..e8e965548 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -27,12 +27,12 @@ #include "../Utils/b3ResourcePath.h" ///The BasicTest shows the contact between volumetric deformable objects and rigid objects. -static btScalar E = 50; -static btScalar nu = 0.3; +// static btScalar E = 50; +// 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.0; static btScalar COLLIDING_VELOCITY = 0; class BasicTest : public CommonDeformableBodyBase @@ -69,7 +69,7 @@ public: void resetCamera() { - float dist = 20; + float dist = 10; float pitch = 0; float yaw = 90; float targetPos[3] = {0, 3, 0}; @@ -89,28 +89,17 @@ public: void stepSimulation(float deltaTime) { - btReducedSoftBody* rsb = static_cast(static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]); - // 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) { // getDeformedShape(rsb, 0, 0.5); first_step = false; rsb->updateReducedDofs(); - // std::cout << rsb->m_reducedDofs[0] << "\n"; } - - // compute reduced dofs - sim_time += deltaTime; - // std::cout << rsb->m_eigenvalues[0] << "\t" << sim_time << "\t" << deltaTime << "\t" << sin(rsb->m_eigenvalues[0] * sim_time) << "\n"; float internalTimeStep = 1. / 60.f; m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); - // float internalTimeStep = 1; - // m_dynamicsWorld->stepSimulation(1, 1, internalTimeStep); - - // map reduced dof back to full - rsb->updateFullDofs(); } virtual void renderScene() @@ -161,10 +150,10 @@ void BasicTest::initPhysics() btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str()); getDeformableDynamicsWorld()->addSoftBody(rsb); - rsb->scale(btVector3(2, 2, 2)); - rsb->translate(btVector3(0, 7, 0)); + // rsb->scale(btVector3(1, 1, 1)); + // rsb->translate(btVector3(0, 0, 0)); rsb->getCollisionShape()->setMargin(0.1); - rsb->setTotalMass(0.5); + // rsb->setTotalMass(0.5); 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; diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 31b67ea08..9587d2638 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -31,10 +31,19 @@ void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_ m_nodalMass.resize(full_size, 0); } -void btReducedSoftBody::setMass(btScalar m) +void btReducedSoftBody::setMass(const tDenseArray& mass_array) { - m_mass = m; - m_inverseMass = m > 0 ? 1.0 / m : 0; + // nodal mass + btScalar total_mass = 0; + for (int i = 0; i < m_nFull; ++i) + { + m_nodalMass[i] = mass_array[3 * i]; + m_nodes[i].m_im = mass_array[3 * i] > 0 ? mass_array[3 * i] : 0; + total_mass += mass_array[3 * i]; + } + // total rigid body mass + m_mass = total_mass; + m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0; } void btReducedSoftBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) @@ -51,7 +60,7 @@ void btReducedSoftBody::updateReducedDofs() m_reducedDofs[j] = 0; for (int i = 0; i < m_nFull; ++i) for (int k = 0; k < 3; ++k) - m_reducedDofs[j] += m_modes[j][3 * i + k] * (m_nodes[i].m_x[k] - m_x0[3 * i + k]); + m_reducedDofs[j] += m_modes[j][3 * i + k] * (m_nodes[i].m_x[k] - m_x0[i][k]); } } @@ -61,6 +70,7 @@ void btReducedSoftBody::updateFullDofs() btAlignedObjectArray delta_x; delta_x.resize(m_nFull); btVector3 origin = getWorldTransform().getOrigin(); + btMatrix3x3 rotation = getWorldTransform().getBasis(); for (int i = 0; i < m_nFull; ++i) { @@ -72,9 +82,9 @@ void btReducedSoftBody::updateFullDofs() { delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j]; } - // get new coordinates - m_nodes[i].m_x[k] = m_x0[3 * i + k] + delta_x[i][k] + origin[k]; //TODO: assume the initial origin is at (0,0,0) } + // get new coordinates + m_nodes[i].m_x = rotation * (m_x0[i] + delta_x[i]) + origin; //TODO: assume the initial origin is at (0,0,0) } } diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 7a1d935b8..58a7df1c3 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -50,18 +50,19 @@ class btReducedSoftBody : public btSoftBody tDenseArray m_Mr; // reduced mass matrix //TODO: do we need this? // full space - tDenseArray m_x0; // Rest position + TVStack m_x0; // Rest position tDenseArray m_nodalMass; // Mass on each node // // Api // btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m); + ~btReducedSoftBody() {} void setReducedModes(int start_mode, int num_modes, int full_size); - void setMass(btScalar m); + void setMass(const tDenseArray& mass_array); void predictIntegratedTransform(btScalar step, btTransform& predictedTransform); @@ -75,9 +76,9 @@ class btReducedSoftBody : public btSoftBody // compute full degree of freedoms void updateFullDofs(); - + // // rigid motion related - + // void applyCentralImpulse(const btVector3& impulse); void applyTorqueImpulse(const btVector3& torque); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp index ed6a751ed..386b60473 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp @@ -110,10 +110,9 @@ btReducedSoftBody* btReducedSoftBodyHelpers::createFromVtkFile(btSoftBodyWorldIn fs.close(); // get rest position - rsb->m_x0.resize(3 * rsb->m_nodes.size()); + rsb->m_x0.resize(rsb->m_nodes.size()); for (int i = 0; i < rsb->m_nodes.size(); ++i) - for (int k = 0; k < 3; ++k) - rsb->m_x0[3 * i + k] = rsb->m_nodes[i].m_x[k]; + rsb->m_x0[i] = rsb->m_nodes[i].m_x; return rsb; } @@ -133,18 +132,11 @@ 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 + // read in full nodal mass 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()); - // assign mass to nodes - btScalar mass = 0; - for (int i = 0; i < rsb->m_nodes.size(); ++i) - { - rsb->m_nodalMass[i] = mass_array[3 * i]; - rsb->m_nodes[i].m_im = mass_array[3 * i] > 0 ? mass_array[3 * i] : 0; - mass += mass_array[3 * i]; - } - rsb->setMass(mass); + rsb->setMass(mass_array); } // read in binary files diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index f7a1ff3a8..f9c1c3cf2 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -41,7 +41,7 @@ void btReducedSoftBodySolver::applyForce() // apply impulses to reduced deformable objects static btScalar sim_time = 0; - static btScalar target_vel = 5; + static btScalar target_vel = 1; static int apply_impulse = 0; if (rsb->m_reducedModel && apply_impulse < 4) { @@ -114,5 +114,8 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep) rsb->predictIntegratedTransform(timeStep, rsb->getInterpolationWorldTransform()); rsb->proceedToTransform(rsb->getInterpolationWorldTransform()); + + // map reduced dof back to full space + rsb->updateFullDofs(); } } \ No newline at end of file