diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index 3072cebd4..db2469380 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -42,45 +42,53 @@ class BasicTest : public CommonDeformableBodyBase // btDeformableLinearElasticityForce* m_linearElasticity; btDeformableMassSpringForce* m_massSpring; - unsigned int m_nReduced; + static const unsigned int m_startMode = 6; // actual mode# should +1 + static const unsigned int m_nReduced = 1; + static const unsigned int selected_mode = 0; unsigned int m_nFull; + btScalar sim_time; bool first_step; - btScalar sim_time = 0; // compute reduced degree of freedoms - void mapToReducedDofs(const btAlignedObjectArray >& modes, - const btAlignedObjectArray& full_mass, - const tNodeArray& full_nodes, - btAlignedObjectArray& reduced_dof, - btAlignedObjectArray& reduced_vel) + void mapToReducedDofs(btSoftBody* psb) { - btAssert(reduced_dof.size() == m_nReduced); - for (int j = 0; j < 1; ++j) - for (int i = 0; i < m_nFull; ++i) - for (int k = 0; k < 3; ++k) - { - int idx_f = 3 * i + k; - reduced_dof[j] = modes[j][idx_f] * full_mass[idx_f] * full_nodes[i].m_x[k]; - reduced_vel[j] = modes[j][idx_f] * full_mass[idx_f] * full_nodes[i].m_v[k]; - } + btAssert(psb->m_reducedDofs.size() == m_nReduced); + for (int j = 0; j < m_nReduced; ++j) { + if (j == selected_mode) + psb->m_reducedDofs[j] = sin(psb->m_eigenvalues[j] * sim_time); + else + psb->m_reducedDofs[j] = 0; + } + // for (int j = 0; j < m_nReduced; ++j) + // { + // // if (j != selected_mode) + // // continue; + // for (int i = 0; i < m_nFull; ++i) + // for (int k = 0; k < 3; ++k) + // { + // int idx_f = 3 * i + k; + // psb->m_reducedNodes[j] = psb->m_modes[j][idx_f] * (psb->m_nodes[i].m_x[k] - psb->m_x0[idx_f]); + // psb->m_reducedVelocity[j] = psb->m_modes[j][idx_f] * psb->m_nodes[i].m_v[k]; + // } + // } } // compute full degree of freedoms - void mapToFullDofs(const btAlignedObjectArray& m_x0, - const btAlignedObjectArray >& modes, - const btAlignedObjectArray& reduced_dof, - const btAlignedObjectArray& reduced_vel, - tNodeArray& full_nodes) + void mapToFullDofs(btSoftBody* psb) { - btAssert(full_nodes.size() == m_nFull); - for (int j = 0; j < 1; ++j) + btAssert(psb->m_nodes.size() == m_nFull); + for (int j = 0; j < m_nReduced; ++j) + { + // if (j != selected_mode) + // continue; for (int i = 0; i < m_nFull; ++i) for (int k = 0; k < 3; ++k) { int idx_f = 3 * i + k; - full_nodes[i].m_x[k] = m_x0[idx_f] + modes[j][idx_f] * reduced_dof[j]; - full_nodes[i].m_v[k] = m_x0[idx_f] + modes[j][idx_f] * reduced_vel[j]; + psb->m_nodes[i].m_x[k] = psb->m_x0[idx_f] + psb->m_modes[j][idx_f] * psb->m_reducedDofs[j]; + psb->m_nodes[i].m_v[k] = psb->m_modes[j][idx_f] * psb->m_reducedVelocity[j]; } + } } // get deformed shape @@ -97,8 +105,8 @@ public: { // m_linearElasticity = 0; m_massSpring = nullptr; - m_nReduced = 0; m_nFull = 0; + sim_time = 0; first_step = true; } virtual ~BasicTest() @@ -135,27 +143,31 @@ public: void stepSimulation(float deltaTime) { - // TODO: remove this. very hacky way of adding initial deformation btSoftBody* psb = static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]; - if (first_step && !psb->m_bUpdateRtCst) { + + // TODO: remove this. very hacky way of adding initial deformation + if (first_step && !psb->m_bUpdateRtCst) + { getDeformedShape(psb, 0); - first_step = false; } // compute reduced dofs - psb->m_reducedNodes.resize(m_nReduced); + psb->m_reducedDofs.resize(m_nReduced); psb->m_reducedVelocity.resize(m_nReduced); - mapToReducedDofs(psb->m_modes, psb->m_M, psb->m_nodes, psb->m_reducedNodes, psb->m_reducedVelocity); - sim_time += deltaTime; - psb->m_reducedNodes[1] = sin(psb->m_eigenvalues[1] * sim_time); - + // std::cout << psb->m_eigenvalues[0] << "\t" << sim_time << "\t" << deltaTime << "\t" << sin(psb->m_eigenvalues[0] * sim_time) << "\n"; + mapToReducedDofs(psb); + float internalTimeStep = 1. / 60.f; m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + // for (int i = 0; i < m_nReduced; ++i) + // std::cout << psb->m_reducedDofs[i] << "\t"; + // std::cout << "\n"; + // map reduced dof back to full - mapToFullDofs(psb->m_x0, psb->m_modes, psb->m_reducedNodes, psb->m_reducedVelocity, psb->m_nodes); + mapToFullDofs(psb); } virtual void renderScene() @@ -203,25 +215,23 @@ void BasicTest::initPhysics() m_nFull = psb->m_nodes.size(); // read in eigenmodes, stiffness and mass matrices - unsigned int buffer_n_modes = 0; - - // TODO: save all modes, but only use the selected ones here - std::string eigenvalues_file("../../../examples/SoftDemo/eigenvalues.bin"); - btSoftBodyHelpers::readBinary(psb->m_eigenvalues, buffer_n_modes, eigenvalues_file.c_str()); - m_nReduced = buffer_n_modes; + btSoftBodyHelpers::readBinary(psb->m_eigenvalues, m_startMode, m_nReduced, 3 * m_nFull, eigenvalues_file.c_str()); std::string Kr_file("../../../examples/SoftDemo/K_r_diag_mat.bin"); - btSoftBodyHelpers::readBinary(psb->m_Kr, buffer_n_modes, Kr_file.c_str()); + btSoftBodyHelpers::readBinary(psb->m_Kr, m_startMode, m_nReduced, 3 * m_nFull, Kr_file.c_str()); std::string Mr_file("../../../examples/SoftDemo/M_r_diag_mat.bin"); - btSoftBodyHelpers::readBinary(psb->m_Mr, buffer_n_modes, Mr_file.c_str()); + btSoftBodyHelpers::readBinary(psb->m_Mr, m_startMode, m_nReduced, 3 * m_nFull, Mr_file.c_str()); std::string modes_file("../../../examples/SoftDemo/modes.bin"); - btSoftBodyHelpers::readBinaryMat(psb->m_modes, 3 * psb->m_nodes.size(), buffer_n_modes, modes_file.c_str()); // default to 3D + btSoftBodyHelpers::readBinaryMat(psb->m_modes, m_startMode, m_nReduced, 3 * m_nFull, modes_file.c_str()); // default to 3D - std::string M_file("../../../examples/SoftDemo/M_diag_mat.bin"); - btSoftBodyHelpers::readBinary(psb->m_M, buffer_n_modes, M_file.c_str()); + // for (int i = 0; i < 3*m_nFull; ++i) + // std::cout << psb->m_modes[0][i] << '\n'; + + // std::string M_file("../../../examples/SoftDemo/M_diag_mat.bin"); + // btSoftBodyHelpers::readBinary(psb->m_M, buffer_n_modes, M_file.c_str()); // get rest position psb->m_x0.resize(3 * psb->m_nodes.size()); diff --git a/examples/SoftDemo/K_r_diag_mat.bin b/examples/SoftDemo/K_r_diag_mat.bin index af8b7d78e..f3f0e6922 100644 Binary files a/examples/SoftDemo/K_r_diag_mat.bin and b/examples/SoftDemo/K_r_diag_mat.bin differ diff --git a/examples/SoftDemo/M_r_diag_mat.bin b/examples/SoftDemo/M_r_diag_mat.bin index c1c2e634f..e4e3106d0 100644 Binary files a/examples/SoftDemo/M_r_diag_mat.bin and b/examples/SoftDemo/M_r_diag_mat.bin differ diff --git a/examples/SoftDemo/eigenvalues.bin b/examples/SoftDemo/eigenvalues.bin index 5b82cc8a5..870103f7c 100644 Binary files a/examples/SoftDemo/eigenvalues.bin and b/examples/SoftDemo/eigenvalues.bin differ diff --git a/examples/SoftDemo/modes.bin b/examples/SoftDemo/modes.bin index b2d19ef74..5ff7d8e54 100644 Binary files a/examples/SoftDemo/modes.bin and b/examples/SoftDemo/modes.bin differ diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 9a634f710..10c9fb25a 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -173,7 +173,7 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero { if (m_reducedModel) { - // // get reduced force + // get reduced force // btAlignedObjectArray reduced_force; // reduced_force.resize(psb->m_reducedNodes.size()); // for (int r = 0; r < psb->m_reducedNodes.size(); ++r) @@ -183,8 +183,13 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero // for (int r = 0; r < psb->m_reducedNodes.size(); ++r) // TODO: reduced soft body // { // btScalar mass_inv = (psb->m_Mr[r] == 0) ? 0 : 1.0 / psb->m_Mr[r]; - // psb->m_reducedVelocity[r] += m_dt * mass_inv * reduced_force[r]; + // // btScalar delta_v = m_dt * mass_inv * reduced_force[r]; + // btScalar delta_v = 1.0; + + // psb->m_reducedVelocity[r] += delta_v; + // std::cout << psb->m_reducedVelocity[r] << "\t"; // } + // std::cout << "\n"; } else { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 2cc44eb0d..97bec6062 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -308,8 +308,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) btSoftBody* psb = m_softBodies[i]; if (m_reducedModel) { - for (int r = 0; r < psb->m_reducedNodes.size(); ++r) - psb->m_reducedNodes[r] += timeStep * psb->m_reducedVelocity[r]; + for (int r = 0; r < psb->m_reducedDofs.size(); ++r) + psb->m_reducedDofs[r] += timeStep * psb->m_reducedVelocity[r]; } else { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index ca904c83b..7e91dde3b 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -855,7 +855,7 @@ public: btScalar m_restLengthScale; - btAlignedObjectArray m_reducedNodes; // Reduced degree of freedom + btAlignedObjectArray m_reducedDofs; // Reduced degree of freedom btAlignedObjectArray m_reducedVelocity; // Reduced velocity array btAlignedObjectArray m_x0; // Rest position btAlignedObjectArray m_eigenvalues; // eigenvalues of the reduce deformable model diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index fd6d5920b..dd2d8a684 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1488,40 +1488,57 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) } // read in binary files -void btSoftBodyHelpers::readBinary(btAlignedObjectArray& vec, unsigned int& size, const char* file) +void btSoftBodyHelpers::readBinary(btAlignedObjectArray& vec, + const unsigned int n_start, // starting index + const unsigned int n_modes, // #entries read + const unsigned int n_full, // array size + const char* file) { std::ifstream f_in(file, std::ios::in | std::ios::binary); // first get size + unsigned int size; f_in.read((char*)&size, sizeof(uint32_t)); + btAssert(size == n_full); // read data - vec.resize(size); + vec.resize(n_modes); double temp; - for (int i = 0; i < size; i++) { + for (unsigned int i = 0; i < n_start + n_modes; ++i) + { f_in.read((char*)&temp, sizeof(double)); - vec[i] = btScalar(temp); + if (i >= n_start) + vec[i - n_start] = btScalar(temp); } f_in.close(); } -void btSoftBodyHelpers::readBinaryMat(btAlignedObjectArray >& mat, const unsigned int n_row, const unsigned int n_col, const char* file) +void btSoftBodyHelpers::readBinaryMat(btAlignedObjectArray >& mat, + const unsigned int n_start, // starting mode index + const unsigned int n_modes, // #modes, outer array size + const unsigned int n_full, // inner array size + const char* file) { std::ifstream f_in(file, std::ios::in | std::ios::binary); // first get size unsigned int v_size; f_in.read((char*)&v_size, sizeof(uint32_t)); - btAssert(v_size == n_row * n_col); + btAssert(v_size == n_full * n_full); // read data - mat.resize(n_col); - for (int i = 0; i < n_col; ++i) + mat.resize(n_modes); + for (int i = 0; i < n_start + n_modes; ++i) { - mat[i].resize(n_row); - for (int j = 0; j < n_row; ++j) + for (int j = 0; j < n_full; ++j) { double temp; f_in.read((char*)&temp, sizeof(double)); - mat[i][j] = btScalar(temp); + + if (i >= n_start) + { + if (mat[i - n_start].size() != n_full) + mat[i - n_start].resize(n_full); + mat[i - n_start][j] = btScalar(temp); + } } } f_in.close(); diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index c3a4309cf..59c959ec3 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -145,9 +145,9 @@ struct btSoftBodyHelpers static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); // read in a binary vector - static void readBinary(btAlignedObjectArray& vec, unsigned int& size, const char* file); + static void readBinary(btAlignedObjectArray& vec, const unsigned int n_start, const unsigned int n_modes, const unsigned int n_full, const char* file); // read in a binary matrix (must provide matrix size) - static void readBinaryMat(btAlignedObjectArray >& mat, const unsigned int n_row, const unsigned int n_col, const char* file); + static void readBinaryMat(btAlignedObjectArray >& mat, const unsigned int n_start, const unsigned int n_modes, const unsigned int n_full, const char* file); static void writeObj(const char* file, const btSoftBody* psb);