dali-physics/third-party/bullet3/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp

   1 #include "btReducedDeformableBodyHelpers.h"
   2 #include "../btSoftBodyHelpers.h"
   3 #include <iostream>
   4 #include <string>
   5 #include <sstream>
   6
   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());
  10
  11         rsb->setReducedModes(num_modes, rsb->m_nodes.size());
  12         btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(rsb, file_path.c_str());
  13
  14         rsb->disableReducedModes(rigid_only);
  15
  16         return rsb;
  17 }
  18
  19 btReducedDeformableBody* btReducedDeformableBodyHelpers::createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
  20 {
  21         std::ifstream fs;
  22         fs.open(vtk_file);
  23         btAssert(fs);
  24
  25         typedef btAlignedObjectArray<int> Index;
  26         std::string line;
  27         btAlignedObjectArray<btVector3> X;
  28         btVector3 position;
  29         btAlignedObjectArray<Index> indices;
  30         bool reading_points = false;
  31         bool reading_tets = false;
  32         size_t n_points = 0;
  33         size_t n_tets = 0;
  34         size_t x_count = 0;
  35         size_t indices_count = 0;
  36         while (std::getline(fs, line))
  37         {
  38                 std::stringstream ss(line);
  39                 if (line.size() == (size_t)(0))
  40                 {
  41                 }
  42                 else if (line.substr(0, 6) == "POINTS")
  43                 {
  44                         reading_points = true;
  45                         reading_tets = false;
  46                         ss.ignore(128, ' ');  // ignore "POINTS"
  47                         ss >> n_points;
  48                         X.resize(n_points);
  49                 }
  50                 else if (line.substr(0, 5) == "CELLS")
  51                 {
  52                         reading_points = false;
  53                         reading_tets = true;
  54                         ss.ignore(128, ' ');  // ignore "CELLS"
  55                         ss >> n_tets;
  56                         indices.resize(n_tets);
  57                 }
  58                 else if (line.substr(0, 10) == "CELL_TYPES")
  59                 {
  60                         reading_points = false;
  61                         reading_tets = false;
  62                 }
  63                 else if (reading_points)
  64                 {
  65                         btScalar p;
  66                         ss >> p;
  67                         position.setX(p);
  68                         ss >> p;
  69                         position.setY(p);
  70                         ss >> p;
  71                         position.setZ(p);
  72                         //printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ());
  73                         X[x_count++] = position;
  74                 }
  75                 else if (reading_tets)
  76                 {
  77                         int d;
  78                         ss >> d;
  79                         if (d != 4)
  80                         {
  81                                 printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
  82                                 fs.close();
  83                                 return 0;
  84                         }
  85                         ss.ignore(128, ' ');  // ignore "4"
  86                         Index tet;
  87                         tet.resize(4);
  88                         for (size_t i = 0; i < 4; i++)
  89                         {
  90                                 ss >> tet[i];
  91                                 //printf("%d ", tet[i]);
  92                         }
  93                         //printf("\n");
  94                         indices[indices_count++] = tet;
  95                 }
  96         }
  97         btReducedDeformableBody* rsb = new btReducedDeformableBody(&worldInfo, n_points, &X[0], 0);
  98
  99         for (int i = 0; i < n_tets; ++i)
 100         {
 101                 const Index& ni = indices[i];
 102                 rsb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
 103                 {
 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);
 110                 }
 111         }
 112
 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());
 121
 122         fs.close();
 123
 124         return rsb;
 125 }
 126
 127 void btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path)
 128 {
 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());
 132
 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());
 135
 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());
 138
 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());
 141
 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);
 147
 148         // calculate the inertia tensor in the local frame
 149         rsb->setInertiaProps();
 150
 151         // other internal initialization
 152         rsb->internalInitialization();
 153 }
 154
 155 // read in a vector from the binary file
 156 void btReducedDeformableBodyHelpers::readBinaryVec(btReducedDeformableBody::tDenseArray& vec,
 157                                                                                                                                                                          const unsigned int n_size,                             // #entries read
 158                                                                                                                                                                                  const char* file)
 159 {
 160         std::ifstream f_in(file, std::ios::in | std::ios::binary);
 161         // first get size
 162         unsigned int size=0;
 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
 165
 166         // read data
 167         vec.resize(n_size);
 168         double temp;
 169         for (unsigned int i = 0; i < n_size; ++i)
 170         {
 171                 f_in.read((char*)&temp, sizeof(double));
 172                 vec[i] = btScalar(temp);
 173         }
 174   f_in.close();
 175 }
 176
 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
 181                                                                                                                                                                                  const char* file)
 182 {
 183         std::ifstream f_in(file, std::ios::in | std::ios::binary);
 184         // first get size
 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
 188
 189         // read data
 190         mat.resize(n_modes);
 191         for (int i = 0; i < n_modes; ++i)
 192         {
 193                 for (int j = 0; j < n_full; ++j)
 194                 {
 195                         double temp;
 196                         f_in.read((char*)&temp, sizeof(double));
 197
 198                         if (mat[i].size() != n_modes)
 199                                 mat[i].resize(n_full);
 200                         mat[i][j] = btScalar(temp);
 201                 }
 202         }
 203   f_in.close();
 204 }
 205
 206 void btReducedDeformableBodyHelpers::calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin)
 207 {
 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]);
 211
 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));
 215 }