matrix.h

//  Copyright (c) 2019 Prashant K. Jha
//  Copyright (c) 2019 Patrick Diehl
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 
#ifndef UTIL_MATRIX_H
#define UTIL_MATRIX_H
 
#include "point.h"
 
namespace util {
 
struct Matrix3 {
 
  float d_data[3][3]{};
 
  Matrix3() {
 
    d_data[0][0] = 0.;
    d_data[0][1] = 0.;
    d_data[0][2] = 0.;
 
    d_data[1][0] = 0.;
    d_data[1][1] = 0.;
    d_data[1][2] = 0.;
 
    d_data[2][0] = 0.;
    d_data[2][1] = 0.;
    d_data[2][2] = 0.;
  };
 
  Matrix3(const util::Point3& diagonal) {
 
    d_data[0][0] = diagonal.d_x;
    d_data[0][1] = 0.;
    d_data[0][2] = 0.;
 
    d_data[1][0] = 0.;
    d_data[1][1] = diagonal.d_y;
    d_data[1][2] = 0.;
 
    d_data[2][0] = 0.;
    d_data[2][1] = 0.;
    d_data[2][2] = diagonal.d_z;
  };
 
  Matrix3(const util::Point3& a1, const util::Point3& a2, const util::Point3& a3) {
 
    d_data[0][0] = a1.d_x;
    d_data[0][1] = a1.d_y;
    d_data[0][2] = a1.d_z,
        d_data[1][0] = a2.d_x;
    d_data[1][1] = a2.d_y;
    d_data[1][2] = a2.d_z;
    d_data[2][0] = a3.d_x;
    d_data[2][1] = a3.d_y;
    d_data[2][2] = a3.d_z;
  };
 
  Matrix3(const std::vector<std::vector<double>> &m) {
    for (size_t i=0; i<3; i++)
      for (size_t j=0; j<3; j++)
        d_data[i][j] = m[i][j];
  }
 
  Matrix3(const Matrix3 &m) {
    for (size_t i=0; i<3; i++)
      for (size_t j=0; j<3; j++)
        d_data[i][j] = m(i,j);
  }
 
  std::string printStr(int nt = 0, int lvl = 0) const {
 
    std::string tabS = "";
    for (int i = 0; i < nt; i++)
      tabS += "\t";
 
    std::ostringstream oss;
    for (size_t i=0; i<3; i++)
      oss << tabS << "[" << (*this)(i, 0) << ", " << (*this)(i, 1) << ", "
          << (*this)(i, 2) << "]" << std::endl;
    oss << std::endl;
 
    return oss.str();
  }
 
  void print(int nt = 0, int lvl = 0) const { std::cout << printStr(nt, lvl); }
 
  Point3 operator()(size_t i) {
    return Point3(d_data[i]);
  }
 
  Point3 operator()(size_t i) const {
    return Point3(d_data[i]);
  }
 
  float &operator()(size_t i, size_t j) { return d_data[i][j]; }
 
  const float &operator()(size_t i, size_t j) const { return d_data[i][j]; }
 
  std::vector<double> dot(const std::vector<double> &v) const {
 
    auto r = std::vector<double>(3,0.);
    for (size_t i=0; i<3; i++)
      for (size_t j=0; j<3; j++)
        r[i] += (*this)(i,j) * v[j];
 
    return r;
  }
 
  util::Point3 dot(const util::Point3 &v) {
 
    return {(*this)(0) * v, (*this)(1) * v, (*this)(2) * v};
  }
 
  Matrix3 transpose() const {
 
    Matrix3 m = Matrix3(*this);
 
    m(0,1) = (*this)(1,0);
    m(0,2) = (*this)(2,0);
 
    m(1,0) = (*this)(0,1);
    m(1,2) = (*this)(2,1);
 
    m(2,0) = (*this)(0,2);
    m(2,1) = (*this)(1,2);
 
    return m;
  }
 
  double det() const {
    return (*this)(0,0) * ((*this)(1,1) * (*this)(2,2) - (*this)(2,1) * (*this)(1,2)) -
           (*this)(0,1) * ((*this)(1,0) * (*this)(2,2) - (*this)(2,0) * (*this)(1,2)) +
           (*this)(0,2) * ((*this)(1,0) * (*this)(2,1) - (*this)(2,0) * (*this)(1,1));
  }
 
  Matrix3 inv() const {
 
    Matrix3 m = Matrix3();
 
    auto det_inv = 1. / this->det();
 
    m(0,0) = det_inv *
             ((*this)(1,1) * (*this)(2,2) - (*this)(2,1) * (*this)(1,2));
    m(0,1) = -det_inv *
             ((*this)(0,1) * (*this)(2,2) - (*this)(2,1) * (*this)(0,2));
    m(0,2) = det_inv *
             ((*this)(0,1) * (*this)(1,2) - (*this)(1,1) * (*this)(0,2));
 
    m(1,0) = -det_inv *
             ((*this)(1,0) * (*this)(2,2) - (*this)(2,0) * (*this)(1,2));
    m(1,1) = det_inv *
             ((*this)(0,0) * (*this)(2,2) - (*this)(2,0) * (*this)(0,2));
    m(1,2) = -det_inv *
             ((*this)(0,0) * (*this)(1,2) - (*this)(1,0) * (*this)(0,2));
 
    m(2,0) = det_inv *
             ((*this)(1,0) * (*this)(2,1) - (*this)(2,0) * (*this)(1,1));
    m(2,1) = -det_inv *
             ((*this)(0,0) * (*this)(2,1) - (*this)(2,0) * (*this)(0,1));
    m(2,2) = det_inv *
             ((*this)(0,0) * (*this)(1,1) - (*this)(1,0) * (*this)(0,1));
 
    return m;
  }
};
 
struct SymMatrix3 {
 
  float d_data[6]{};
 
  SymMatrix3() {
 
    d_data[0] = 0.;
    d_data[1] = 0.;
    d_data[2] = 0.;
    d_data[3] = 0.;
    d_data[4] = 0.;
    d_data[5] = 0.;
  };
 
  SymMatrix3(const util::Point3& diagonal) {
 
    d_data[0] = diagonal.d_x;
    d_data[1] = diagonal.d_y;
    d_data[2] = diagonal.d_z;
 
    d_data[3] = 0.;
    d_data[4] = 0.;
    d_data[5] = 0.;
  };
 
  SymMatrix3(const std::vector<std::vector<double>> &m) {
 
    d_data[0] = m[0][0];
    d_data[1] = m[1][1];
    d_data[2] = m[2][2];
    d_data[3] = 0.5 * (m[1][2] + m[2][1]);
    d_data[4] = 0.5 * (m[0][2] + m[2][0]);
    d_data[5] = 0.5 * (m[0][1] + m[1][0]);
  }
 
  SymMatrix3(const std::vector<double> &m) {
 
    d_data[0] = m[0];
    d_data[1] = m[1];
    d_data[2] = m[2];
    d_data[3] = m[3];
    d_data[4] = m[4];
    d_data[5] = m[5];
  }
 
  SymMatrix3(const Matrix3 &m) {
 
    d_data[0] = m(0,0);
    d_data[1] = m(1,1);
    d_data[2] = m(2,2);
    d_data[3] = 0.5 * (m(1,2) + m(2,1));
    d_data[4] = 0.5 * (m(0,2) + m(2,0));
    d_data[5] = 0.5 * (m(0,1) + m(1,0));
  }
 
  SymMatrix3(const SymMatrix3 &m) {
 
    for (size_t i=0; i<6; i++)
      d_data[i] = m.d_data[i];
  }
 
  std::string printStr(int nt = 0, int lvl = 0) const {
 
    std::string tabS = "";
    for (int i = 0; i < nt; i++)
      tabS += "\t";
 
    std::ostringstream oss;
    for (size_t i=0; i<3; i++)
      oss << tabS << "[" << (*this)(i, 0) << ", " << (*this)(i, 1) << ", "
          << (*this)(i, 2) << "]" << std::endl;
    oss << std::endl;
 
    return oss.str();
  }
 
 
  void print(int nt = 0, int lvl = 0) const { std::cout << printStr(nt, lvl); }
 
 
  Point3 operator()(size_t i) {
    return {(*this)(i, 0), (*this)(i, 1), (*this)(i, 2)};
  }
 
  Point3 operator()(size_t i) const {
    return {(*this)(i, 0), (*this)(i, 1), (*this)(i, 2)};
  }
 
  float &operator()(size_t i, size_t j) {
    return d_data[i == j ? i : 6 - i - j];
  }
 
 
  const float &operator()(size_t i, size_t j) const {
    return d_data[i == j ? i : 6 - i - j];
  }
 
  const float &get(size_t i) const {
    return d_data[i];
  }
 
  float &get(size_t i) {
    return d_data[i];
  }
 
  void copy(double m[6]) const {
 
    for (size_t i=0; i<6; i++)
      m[i] = d_data[i];
  }
 
  std::vector<double> dot(const std::vector<double> &v) const {
 
    auto r = std::vector<double>(3,0.);
    for (size_t i=0; i<3; i++)
      for (size_t j=0; j<3; j++)
        r[i] += (*this)(i,j) * v[j];
 
    return r;
  }
 
  util::Point3 dot(const util::Point3 &v) {
 
    return {(*this)(0) * v, (*this)(1) * v, (*this)(2) * v};
  }
 
  SymMatrix3 transpose() const {
 
    return (*this);
  }
 
  double det() const {
    return (*this)(0,0) * ((*this)(1,1) * (*this)(2,2) - (*this)(2,1) * (*this)(1,2)) -
           (*this)(0,1) * ((*this)(1,0) * (*this)(2,2) - (*this)(2,0) * (*this)(1,2)) +
           (*this)(0,2) * ((*this)(1,0) * (*this)(2,1) - (*this)(2,0) * (*this)(1,1));
  }
 
  SymMatrix3 inv() const {
 
    SymMatrix3 m = SymMatrix3();
 
    auto det_inv = 1. / this->det();
 
    m(0,0) = det_inv *
             ((*this)(1,1) * (*this)(2,2) - (*this)(2,1) * (*this)(1,2));
    m(0,1) = -det_inv *
             ((*this)(0,1) * (*this)(2,2) - (*this)(2,1) * (*this)(0,2));
    m(0,2) = det_inv *
             ((*this)(0,1) * (*this)(1,2) - (*this)(1,1) * (*this)(0,2));
 
    m(1,0) = -det_inv *
             ((*this)(1,0) * (*this)(2,2) - (*this)(2,0) * (*this)(1,2));
    m(1,1) = det_inv *
             ((*this)(0,0) * (*this)(2,2) - (*this)(2,0) * (*this)(0,2));
    m(1,2) = -det_inv *
             ((*this)(0,0) * (*this)(1,2) - (*this)(1,0) * (*this)(0,2));
 
    m(2,0) = det_inv *
             ((*this)(1,0) * (*this)(2,1) - (*this)(2,0) * (*this)(1,1));
    m(2,1) = -det_inv *
             ((*this)(0,0) * (*this)(2,1) - (*this)(2,0) * (*this)(0,1));
    m(2,2) = det_inv *
             ((*this)(0,0) * (*this)(1,1) - (*this)(1,0) * (*this)(0,1));
 
    return m;
  }
};
 
bool checkMatrix(const std::vector<std::vector<double>> &m);
 
std::vector<double> dot(const std::vector<std::vector<double>> &m, const
std::vector<double> &v);
 
std::vector<std::vector<double>> transpose(const
                                           std::vector<std::vector<double>> &m);
 
double det(const std::vector<std::vector<double>> &m);
 
std::vector<std::vector<double>>
inv(const std::vector<std::vector<double>> &m);
 
} // namespace util
 
#endif // UTIL_MATRIX_H