RigidElasticSim/static_sim/Utils.hpp

//
// Created by Wei Chen on 3/2/22
//

#ifndef UTILS_HPP
#define UTILS_HPP

#include <fstream>
#include <Eigen/Eigen>
#include "LinSysSolver.hpp"

namespace ssim{

class Utils{
public:

template<typename T>
static void writeMatrix(const std::string &path, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &M){
    std::ofstream ofs(path);
    for (int i = 0; i < M.rows(); ++i) {
        for (int j = 0; j < M.cols(); ++j) {
            ofs << M(i, j) << " ";
        }
        ofs << std::endl;
    }
    ofs.close();
}

template<typename T>
static void readMatrix(const std::string &path, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &M){
    std::vector<std::vector<T>> data;
    std::ifstream ifs(path);
    std::string line;
    int row = 0;
    int col = 0;
    while(std::getline(ifs, line)){
        std::stringstream ss(line);
        std::vector<T> row_data;
        col = 0;
        T v;
        while(ss >> v){
            col++;
            row_data.push_back(v);
        }
        data.push_back(row_data);
        row++;
    }

    M.resize(row, col);
    for (int i = 0; i < row; ++i) {
        for (int j = 0; j < col; ++j) {
            M(i, j) = data[i][j];
        }
    }
    ifs.close();
}

static bool writeMatrixXd(const std::string &filePath, const Eigen::MatrixXd &mat);
static bool writeMatrixXi(const std::string &filePath, const Eigen::MatrixXi &mat);

// write mesh information to '.txt', prepare for meshio
// V.size(): number of elements in mesh
// m = 1: V[i] is (1, 3), point mesh
// m = 2: V[i] is (2, 3), line mesh
// m = 4: V[i] is (4, 3), quad mesh
// m = 8: V[i] is (8, 3), hex mesh
static bool writeMesh(const std::string &filePath, const std::vector<Eigen::MatrixXd> &V,
                      int m);


static void writeTriVTK(const std::string &path, const Eigen::MatrixXd &V, const Eigen::MatrixXi &T,
                        const std::vector<double> &cell_data = {}, const std::vector<double> &v_data = {});

static void writeTetVTK(const std::string &path, const Eigen::MatrixXd &V, const Eigen::MatrixXi &T,
                        const std::vector<double> &cell_data = {}, const std::vector<double> &v_data = {});

static void writePntVTK(const std::string &path, const Eigen::MatrixXd &V);

// find (vertex indices of) surface triangle from tetrahedral
static void find_surfTri_from_tet(const Eigen::MatrixXi& TT, Eigen::MatrixXi& SF);

// read TV, TT, SF from .msh file
static bool readTetMesh(const std::string& filePath, Eigen::MatrixXd& TV, Eigen::MatrixXi& TT,
                        Eigen::MatrixXi& SF);

// write .obj file
static void writeOBJ(const std::string &path, const Eigen::MatrixXd &V, const Eigen::MatrixXi &F);

static void elasticMatrix(double YM, double PR, Eigen::Matrix<double, 6, 6> &D);

static double vonStress(const Eigen::VectorXd &stress);

template <int dim>
static void addBlockToMatrix(const Eigen::MatrixXd& block,
                      const Eigen::VectorXi& index,
                      int rowIndI,
                      std::shared_ptr<LinSysSolver<Eigen::VectorXi, Eigen::VectorXd>> linSysSolver)
{
    int rowStart = index[rowIndI] * dim;
    if (rowStart < 0) { // DBC node
        rowStart = -rowStart - dim;
        linSysSolver->setCoeff(rowStart, rowStart, 1.0);
        linSysSolver->setCoeff(rowStart + 1, rowStart + 1, 1.0);
        if constexpr (dim == 3) {
            linSysSolver->setCoeff(rowStart + 2, rowStart + 2, 1.0);
        }
        return;
    }

    int eleNodeNum = static_cast<int>(index.size());
    for (int _i = 0; _i < eleNodeNum; ++_i) {
        if (index[_i] >= 0) {
            int _Idim = _i * dim;
            int _dimIndexI = index[_i] * dim;
            linSysSolver->addCoeff(rowStart, _dimIndexI, block(0, _Idim));
            linSysSolver->addCoeff(rowStart, _dimIndexI + 1, block(0, _Idim + 1));
            linSysSolver->addCoeff(rowStart + 1, _dimIndexI, block(1, _Idim));
            linSysSolver->addCoeff(rowStart + 1, _dimIndexI + 1, block(1, _Idim + 1));
            if constexpr (dim == 3) {
                linSysSolver->addCoeff(rowStart, _dimIndexI + 2, block(0, _Idim + 2));
                linSysSolver->addCoeff(rowStart + 1, _dimIndexI + 2, block(1, _Idim + 2));
                linSysSolver->addCoeff(rowStart + 2, _dimIndexI, block(2, _Idim));
                linSysSolver->addCoeff(rowStart + 2, _dimIndexI + 1, block(2, _Idim + 1));
                linSysSolver->addCoeff(rowStart + 2, _dimIndexI + 2, block(2, _Idim + 2));
            }
        }
    }

}

static Eigen::MatrixXd SubMatrix(const Eigen::MatrixXd &original,
                                 const Eigen::VectorXi &rowIdx,
                                 const Eigen::VectorXi &colIdx);

static Eigen::VectorXd SubVector(const Eigen::VectorXd &original,
                                 const Eigen::VectorXi &index);

};

} // namespace SIM

#endif // UTILS_HPP