diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp index e3f2356b6..13835c741 100644 --- a/examples/ReducedDeformableDemo/BasicTest.cpp +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -37,54 +37,9 @@ static btScalar COLLIDING_VELOCITY = 0; class BasicTest : public CommonDeformableBodyBase { - typedef btAlignedObjectArray tNodeArray; - - // btDeformableLinearElasticityForce* m_linearElasticity; - // btDeformableMassSpringForce* m_massSpring; - - static const unsigned int m_startMode = 6; // actual mode# should +1 - static const unsigned int m_nReduced = 2; - static const unsigned int selected_mode = 0; - unsigned int m_nFull; btScalar sim_time; bool first_step; - // compute reduced degree of freedoms - void mapToReducedDofs(btReducedSoftBody* rsb) - { - btAssert(rsb->m_reducedDofs.size() == m_nReduced); - for (int j = 0; j < m_nReduced; ++j) - { - rsb->m_reducedDofs[j] = 0; - for (int i = 0; i < m_nFull; ++i) - for (int k = 0; k < 3; ++k) - rsb->m_reducedDofs[j] += rsb->m_modes[j][3 * i + k] * (rsb->m_nodes[i].m_x[k] - rsb->m_x0[3 * i + k]); - } - } - - // compute full degree of freedoms - void mapToFullDofs(btReducedSoftBody* rsb) - { - btAssert(rsb->m_nodes.size() == m_nFull); - btAlignedObjectArray delta_x; - delta_x.resize(m_nFull); - btVector3 origin = rsb->getWorldTransform().getOrigin(); - - for (int i = 0; i < m_nFull; ++i) - { - for (int k = 0; k < 3; ++k) - { - // compute displacement - delta_x[i][k] = 0; - for (int j = 0; j < m_nReduced; ++j) - delta_x[i][k] += rsb->m_modes[j][3 * i + k] * rsb->m_reducedDofs[j]; - // get new coordinates - rsb->m_nodes[i].m_x[k] = rsb->m_x0[3 * i + k] + delta_x[i][k] + origin[k]; //TODO: assume the initial origin is at (0,0,0) - } - } - - } - // get deformed shape void getDeformedShape(btReducedSoftBody* rsb, const int mode_n, const btScalar scale = 1) { @@ -97,7 +52,6 @@ public: BasicTest(struct GUIHelperInterface* helper) : CommonDeformableBodyBase(helper) { - m_nFull = 0; sim_time = 0; first_step = true; } @@ -142,13 +96,11 @@ public: { // getDeformedShape(rsb, 0, 0.5); first_step = false; - mapToReducedDofs(rsb); + rsb->updateReducedDofs(); // std::cout << rsb->m_reducedDofs[0] << "\n"; } // compute reduced dofs - rsb->m_reducedDofs.resize(m_nReduced); - rsb->m_reducedVelocity.resize(m_nReduced); sim_time += deltaTime; // std::cout << rsb->m_eigenvalues[0] << "\t" << sim_time << "\t" << deltaTime << "\t" << sin(rsb->m_eigenvalues[0] * sim_time) << "\n"; @@ -158,7 +110,7 @@ public: // m_dynamicsWorld->stepSimulation(1, 1, internalTimeStep); // map reduced dof back to full - mapToFullDofs(rsb); + rsb->updateFullDofs(); } virtual void renderScene() @@ -200,30 +152,13 @@ void BasicTest::initPhysics() m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); // create volumetric soft body - { - std::string filename("../../../examples/SoftDemo/mesh.vtk"); - btReducedSoftBody* rsb = btReducedSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str()); - m_nFull = rsb->m_nodes.size(); - rsb->m_reducedModel = true; - - // read in eigenmodes, stiffness and mass matrices - std::string eigenvalues_file("../../../examples/SoftDemo/eigenvalues.bin"); - btReducedSoftBodyHelpers::readBinary(rsb->m_eigenvalues, m_startMode, m_nReduced, 3 * m_nFull, eigenvalues_file.c_str()); - - std::string Kr_file("../../../examples/SoftDemo/K_r_diag_mat.bin"); - btReducedSoftBodyHelpers::readBinary(rsb->m_Kr, m_startMode, m_nReduced, 3 * m_nFull, Kr_file.c_str()); - - std::string Mr_file("../../../examples/SoftDemo/M_r_diag_mat.bin"); - btReducedSoftBodyHelpers::readBinary(rsb->m_Mr, m_startMode, m_nReduced, 3 * m_nFull, Mr_file.c_str()); - - std::string modes_file("../../../examples/SoftDemo/modes.bin"); - btReducedSoftBodyHelpers::readBinaryModes(rsb->m_modes, m_startMode, m_nReduced, 3 * m_nFull, modes_file.c_str()); // default to 3D - - // get rest position - rsb->m_x0.resize(3 * 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]; + { + std::string filepath("../../../examples/SoftDemo/"); + std::string filename = filepath + "mesh.vtk"; + btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str()); + + rsb->setReducedModes(6, 2, rsb->m_nodes.size()); + btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str()); getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->scale(btVector3(2, 2, 2)); @@ -237,18 +172,6 @@ void BasicTest::initPhysics() rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; rsb->m_sleepingThreshold = 0; btSoftBodyHelpers::generateBoundaryFaces(rsb); - - std::string M_file("../../../examples/SoftDemo/M_diag_mat.bin"); - btAlignedObjectArray mass_array; - btReducedSoftBodyHelpers::readBinary(mass_array, 0, 3 * m_nFull, 3 * 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_nodes[i].m_im = mass_array[3 * i]; // here we use m_im as the actual mass not the mass inverse - mass += mass_array[3 * i]; - } - rsb->setMass(mass); rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index f56ef7b30..31b67ea08 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -1,11 +1,15 @@ #include "btReducedSoftBody.h" #include "LinearMath/btTransformUtil.h" +#include btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) : btSoftBody(worldInfo, node_count, x, m) { - // model flag + // reduced deformable m_reducedModel = true; + m_startMode = 0; + m_nReduced = 0; + m_nFull = 0; // rigid motion m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); @@ -17,6 +21,16 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co m_inverseMass = 0.0; } +void btReducedSoftBody::setReducedModes(int start_mode, int num_modes, int full_size) +{ + m_startMode = start_mode; + m_nReduced = num_modes; + m_nFull = full_size; + m_reducedDofs.resize(m_nReduced, 0); + m_reducedVelocity.resize(m_nReduced, 0); + m_nodalMass.resize(full_size, 0); +} + void btReducedSoftBody::setMass(btScalar m) { m_mass = m; @@ -28,6 +42,42 @@ 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); + for (int j = 0; j < m_nReduced; ++j) + { + 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]); + } +} + +void btReducedSoftBody::updateFullDofs() +{ + btAssert(m_nodes.size() == m_nFull); + btAlignedObjectArray delta_x; + delta_x.resize(m_nFull); + btVector3 origin = getWorldTransform().getOrigin(); + + for (int i = 0; i < m_nFull; ++i) + { + for (int k = 0; k < 3; ++k) + { + // compute displacement + delta_x[i][k] = 0; + for (int j = 0; j < m_nReduced; ++j) + { + 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) + } + } +} + void btReducedSoftBody::proceedToTransform(const btTransform& newTrans) { setCenterOfMassTransform(newTrans); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index 357db6936..7a1d935b8 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -14,8 +14,15 @@ class btReducedSoftBody : public btSoftBody // protected: - // - + // 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; + btVector3 m_invInertiaLocal; + btMatrix3x3 m_invInertiaTensorWorld; public: // @@ -32,25 +39,19 @@ class btReducedSoftBody : public btSoftBody bool m_reducedModel; // Reduced deformable model flag // reduced space + int m_startMode; + int m_nReduced; + int m_nFull; tDenseMatrix m_modes; // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes tDenseArray m_reducedDofs; // Reduced degree of freedom tDenseArray m_reducedVelocity; // Reduced velocity array tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model tDenseArray m_Kr; // reduced stiffness matrix tDenseArray m_Mr; // reduced mass matrix //TODO: do we need this? - - // rigid frame - btScalar m_mass; - btScalar m_inverseMass; - btVector3 m_linearVelocity; - btVector3 m_angularVelocity; - btVector3 m_linearFactor; - btVector3 m_angularFactor; - btVector3 m_invInertiaLocal; - btMatrix3x3 m_invInertiaTensorWorld; // full space tDenseArray m_x0; // Rest position + tDenseArray m_nodalMass; // Mass on each node // // Api @@ -58,10 +59,23 @@ class btReducedSoftBody : public btSoftBody 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 predictIntegratedTransform(btScalar step, btTransform& predictedTransform); + // + // reduced dof related + // + + // compute reduced degree of freedoms + void updateReducedDofs(); + + // compute full degree of freedoms + void updateFullDofs(); + + // rigid motion related void applyCentralImpulse(const btVector3& impulse); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp index 02c582280..ed6a751ed 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.cpp @@ -4,7 +4,7 @@ #include #include -btReducedSoftBody* btReducedSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) +btReducedSoftBody* btReducedSoftBodyHelpers::createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) { std::ifstream fs; fs.open(vtk_file); @@ -108,9 +108,45 @@ btReducedSoftBody* btReducedSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldIn printf("Tetras: %u\r\n", rsb->m_tetras.size()); fs.close(); + + // get rest position + rsb->m_x0.resize(3 * 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]; + return rsb; } +void btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(btReducedSoftBody* rsb, const char* file_path) +{ + // read in eigenmodes, stiffness and mass matrices + std::string eigenvalues_file = std::string(file_path) + "eigenvalues.bin"; + btReducedSoftBodyHelpers::readBinary(rsb->m_eigenvalues, rsb->m_startMode, rsb->m_nReduced, 3 * rsb->m_nFull, eigenvalues_file.c_str()); + + std::string Kr_file = std::string(file_path) + "K_r_diag_mat.bin"; + btReducedSoftBodyHelpers::readBinary(rsb->m_Kr, rsb->m_startMode, rsb->m_nReduced, 3 * rsb->m_nFull, Kr_file.c_str()); + + std::string Mr_file = std::string(file_path) + "M_r_diag_mat.bin"; + btReducedSoftBodyHelpers::readBinary(rsb->m_Mr, rsb->m_startMode, rsb->m_nReduced, 3 * rsb->m_nFull, Mr_file.c_str()); + + 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 + + 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); +} + // read in binary files void btReducedSoftBodyHelpers::readBinary(btReducedSoftBody::tDenseArray& vec, const unsigned int n_start, // starting index diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h index 35de7675d..a1fb67b0b 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h @@ -5,11 +5,10 @@ struct btReducedSoftBodyHelpers { - static btReducedSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); - - // TODO: no world info passed in here. may be required in the future - // static btReducedSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); - + // read in geometry info from Vtk file + static btReducedSoftBody* createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); + // read in all reduced files + static void readReducedDeformableInfoFromFiles(btReducedSoftBody* rsb, const char* file_path); // read in a binary vector static void readBinary(btReducedSoftBody::tDenseArray& vec, const unsigned int n_start, const unsigned int n_modes, const unsigned int n_full, const char* file); // read in a binary matrix diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index 8794c4b70..f7a1ff3a8 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -43,37 +43,37 @@ void btReducedSoftBodySolver::applyForce() static btScalar sim_time = 0; static btScalar target_vel = 5; static int apply_impulse = 0; - if (rsb->m_reducedModel && apply_impulse < 4 && sim_time > 1) + if (rsb->m_reducedModel && apply_impulse < 4) { btScalar f_imp = 0; btVector3 imp(0, 0, 0); if (sim_time > 1 && apply_impulse == 0) { - f_imp = rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; + f_imp = rsb->m_nodalMass[0] * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; imp[1] = f_imp; apply_impulse++; - std::cout << "hit" << "\n"; + std::cout << "hit" << f_imp << "\n"; } if (sim_time > 2 && apply_impulse == 1) { - f_imp = - 1.2 * rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; + f_imp = - 1.2 * rsb->m_nodalMass[0] * (target_vel - rsb->m_nodes[0].m_v[1]) / m_dt; imp[1] = f_imp; apply_impulse++; - std::cout << "hit2" << "\n"; + std::cout << "hit2" << f_imp << "\n"; } if (sim_time > 3 && apply_impulse == 2) { - f_imp = 1 * rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[0]) / m_dt; + f_imp = 1 * rsb->m_nodalMass[0] * (target_vel - rsb->m_nodes[0].m_v[0]) / m_dt; imp[2] = f_imp; apply_impulse++; - std::cout << "hit3" << "\n"; + std::cout << "hit3" << f_imp << "\n"; } if (sim_time > 4 && apply_impulse == 3) { - f_imp = -1.1 * rsb->m_nodes[i].m_im * (target_vel - rsb->m_nodes[0].m_v[0]) / m_dt; + f_imp = -1.1 * rsb->m_nodalMass[0] * (target_vel - rsb->m_nodes[0].m_v[0]) / m_dt; imp[2] = f_imp; apply_impulse++; - std::cout << "hit4" << "\n"; + std::cout << "hit4" << f_imp << "\n"; } for (int i = 0; i < rsb->m_reducedDofs.size(); ++i) {