1 #include "btReducedDeformableBodyHelpers.h"
2 #include "../btSoftBodyHelpers.h"
7 btReducedDeformableBody* btReducedDeformableBodyHelpers::createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only) {
8 std::string filename = file_path + vtk_file;
9 btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createFromVtkFile(worldInfo, filename.c_str());
11 rsb->setReducedModes(num_modes, rsb->m_nodes.size());
12 btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(rsb, file_path.c_str());
14 rsb->disableReducedModes(rigid_only);
19 btReducedDeformableBody* btReducedDeformableBodyHelpers::createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
25 typedef btAlignedObjectArray<int> Index;
27 btAlignedObjectArray<btVector3> X;
29 btAlignedObjectArray<Index> indices;
30 bool reading_points = false;
31 bool reading_tets = false;
35 size_t indices_count = 0;
36 while (std::getline(fs, line))
38 std::stringstream ss(line);
39 if (line.size() == (size_t)(0))
42 else if (line.substr(0, 6) == "POINTS")
44 reading_points = true;
46 ss.ignore(128, ' '); // ignore "POINTS"
50 else if (line.substr(0, 5) == "CELLS")
52 reading_points = false;
54 ss.ignore(128, ' '); // ignore "CELLS"
56 indices.resize(n_tets);
58 else if (line.substr(0, 10) == "CELL_TYPES")
60 reading_points = false;
63 else if (reading_points)
72 //printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ());
73 X[x_count++] = position;
75 else if (reading_tets)
81 printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
85 ss.ignore(128, ' '); // ignore "4"
88 for (size_t i = 0; i < 4; i++)
91 //printf("%d ", tet[i]);
94 indices[indices_count++] = tet;
97 btReducedDeformableBody* rsb = new btReducedDeformableBody(&worldInfo, n_points, &X[0], 0);
99 for (int i = 0; i < n_tets; ++i)
101 const Index& ni = indices[i];
102 rsb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
104 rsb->appendLink(ni[0], ni[1], 0, true);
105 rsb->appendLink(ni[1], ni[2], 0, true);
106 rsb->appendLink(ni[2], ni[0], 0, true);
107 rsb->appendLink(ni[0], ni[3], 0, true);
108 rsb->appendLink(ni[1], ni[3], 0, true);
109 rsb->appendLink(ni[2], ni[3], 0, true);
113 btSoftBodyHelpers::generateBoundaryFaces(rsb);
114 rsb->initializeDmInverse();
115 rsb->m_tetraScratches.resize(rsb->m_tetras.size());
116 rsb->m_tetraScratchesTn.resize(rsb->m_tetras.size());
117 printf("Nodes: %u\r\n", rsb->m_nodes.size());
118 printf("Links: %u\r\n", rsb->m_links.size());
119 printf("Faces: %u\r\n", rsb->m_faces.size());
120 printf("Tetras: %u\r\n", rsb->m_tetras.size());
127 void btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path)
129 // read in eigenmodes, stiffness and mass matrices
130 std::string eigenvalues_file = std::string(file_path) + "eigenvalues.bin";
131 btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_eigenvalues, rsb->m_nReduced, eigenvalues_file.c_str());
133 std::string Kr_file = std::string(file_path) + "K_r_diag_mat.bin";
134 btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Kr, rsb->m_nReduced, Kr_file.c_str());
136 // std::string Mr_file = std::string(file_path) + "M_r_diag_mat.bin";
137 // btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Mr, rsb->m_nReduced, Mr_file.c_str());
139 std::string modes_file = std::string(file_path) + "modes.bin";
140 btReducedDeformableBodyHelpers::readBinaryMat(rsb->m_modes, rsb->m_nReduced, 3 * rsb->m_nFull, modes_file.c_str());
142 // read in full nodal mass
143 std::string M_file = std::string(file_path) + "M_diag_mat.bin";
144 btAlignedObjectArray<btScalar> mass_array;
145 btReducedDeformableBodyHelpers::readBinaryVec(mass_array, rsb->m_nFull, M_file.c_str());
146 rsb->setMassProps(mass_array);
148 // calculate the inertia tensor in the local frame
149 rsb->setInertiaProps();
151 // other internal initialization
152 rsb->internalInitialization();
155 // read in a vector from the binary file
156 void btReducedDeformableBodyHelpers::readBinaryVec(btReducedDeformableBody::tDenseArray& vec,
157 const unsigned int n_size, // #entries read
160 std::ifstream f_in(file, std::ios::in | std::ios::binary);
163 f_in.read((char*)&size, 4);//sizeof(unsigned int));
164 btAssert(size >= n_size); // make sure the #requested mode is smaller than the #available modes
169 for (unsigned int i = 0; i < n_size; ++i)
171 f_in.read((char*)&temp, sizeof(double));
172 vec[i] = btScalar(temp);
177 // read in a matrix from the binary file
178 void btReducedDeformableBodyHelpers::readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat,
179 const unsigned int n_modes, // #modes, outer array size
180 const unsigned int n_full, // inner array size
183 std::ifstream f_in(file, std::ios::in | std::ios::binary);
185 unsigned int v_size=0;
186 f_in.read((char*)&v_size, 4);//sizeof(unsigned int));
187 btAssert(v_size >= n_modes * n_full); // make sure the #requested mode is smaller than the #available modes
191 for (int i = 0; i < n_modes; ++i)
193 for (int j = 0; j < n_full; ++j)
196 f_in.read((char*)&temp, sizeof(double));
198 if (mat[i].size() != n_modes)
199 mat[i].resize(n_full);
200 mat[i][j] = btScalar(temp);
206 void btReducedDeformableBodyHelpers::calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin)
208 btScalar lx = btScalar(2.) * (half_extents[0] + margin[0]);
209 btScalar ly = btScalar(2.) * (half_extents[1] + margin[1]);
210 btScalar lz = btScalar(2.) * (half_extents[2] + margin[2]);
212 inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
213 mass / (btScalar(12.0)) * (lx * lx + lz * lz),
214 mass / (btScalar(12.0)) * (lx * lx + ly * ly));