utils_math.hpp

Go to the documentation of this file.

#pragma once
 
#include "utils_distributions.hpp"
#include "utils_macros.hpp"
#include "utils_operators.hpp"
#include <cmath>
#include <ctime>
#include <iostream>
#include <tuple>
 
namespace netdem {
 
class Math {
public:
  constexpr static double PI = 3.1415926535897932384626433832795028841971;
  constexpr static double Infinity = 1.0e15;
 
  template <typename T> static int Sign(T val);
 
  static double NormL2(Vec2d const &val);
 
  static double NormL2(Vec3d const &val);
 
  static double NormL2(double val_0, double val_1);
 
  static double NormL2(double val_0, double val_1, double val_2);
 
  static double NormL2(double val_0, double val_1, double val_2, double val_3);
 
  template <size_t N> static double NormL2(VecNd<N> const &val);
 
  static double NormL2(VecXd const &val);
 
  static double Determinant(Mat2d const &mat);
 
  static double Determinant(Mat3d const &mat);
 
  static Mat2d Inverse(Mat2d const &m_val);
 
  static Mat3d Inverse(Mat3d const &m_val);
 
  template <size_t r, size_t cr, size_t c>
  static MatNd<r, c> Dot(MatNd<r, cr> const &m_1, MatNd<cr, c> const &m_2);
 
  template <size_t r, size_t cr, size_t c>
  static MatNd<r, c> DotTransportLHS(MatNd<cr, r> const &m_1,
                                     MatNd<cr, c> const &m_2);
 
  template <size_t r, size_t cr, size_t c>
  static MatNd<r, c> DotTransportRHS(MatNd<r, cr> const &m_1,
                                     MatNd<c, cr> const &m_2);
 
  static Vec3d Cross(Vec3d const &val_1, Vec3d const &val_2);
 
  static double Dot(Vec3d const &val_1, Vec3d const &val_2);
 
  static double Dot(VecXT<double> const &val_1, VecXT<double> const &val_2);
 
  static void Normalize(Vec3d *const val);
 
  static Vec3d Rotate(Vec3d const &val_old, double rot_angle_cos,
                      double rot_angle_sin, Vec3d const &rot_axis);
 
  static Vec3d Rotate(Vec3d const &val_old, double rot_angle,
                      Vec3d const &rot_axis);
 
  static Vec3d Rotate(Vec3d const &val_old, Vec4d const &quat);
 
  static Vec3d Rotate(Vec3d const &val_old, Mat3d const &rot_mat);
 
  static Vec3d CartesianToSpherical(Vec3d const &vert_cart);
 
  static Vec3d SphericalToCartesian(Vec3d const &vert_sph);
 
  class Quaternion {
  public:
    static Vec4d FromRodrigues(double rot_angle, Vec3d const &rot_axis);
 
    static std::tuple<double, Vec3d> ToRodrigues(Vec4d const &quat);
 
    static Vec4d FromMatrix(Mat3d const &rot_mat);
 
    static Mat3d ToMatrix(Vec4d const &quat);
 
    static Vec4d Multiply(Vec4d const &p, Vec4d const &q);
 
    static Vec4d Add(Vec4d const &p, Vec4d const &q);
 
    static Vec4d Conjugate(Vec4d const &p);
 
    static void Normalize(Vec4d *const q);
  };
};
 
template <typename T> int Math::Sign(T val) {
  return (T(0) < val) - (val < T(0));
}
 
template <size_t N> double Math::NormL2(VecNd<N> const &val) {
  double res{0.0};
  for (size_t i = 0; i < N; i++) {
    res += val[i] * val[i];
  }
  return std::sqrt(res);
}
 
template <size_t r, size_t cr, size_t c>
MatNd<r, c> Math::Dot(MatNd<r, cr> const &m_1, MatNd<cr, c> const &m_2) {
  MatNd<r, c> m_res;
 
  for (size_t i = 0; i < r; i++) {
    for (size_t j = 0; j < c; j++) {
      m_res[i][j] = 0;
    }
  }
 
  for (size_t i = 0; i < r; i++) {
    for (size_t k = 0; k < cr; k++) {
      for (size_t j = 0; j < c; j++) {
        m_res[i][j] += m_1[i][k] * m_2[k][j];
      }
    }
  }
 
  return m_res;
}
 
template <size_t r, size_t cr, size_t c>
MatNd<r, c> Math::DotTransportLHS(MatNd<cr, r> const &m_1,
                                  MatNd<cr, c> const &m_2) {
  MatNd<r, c> m_res;
 
  for (size_t i = 0; i < r; i++) {
    for (size_t j = 0; j < c; j++) {
      m_res[i][j] = 0;
    }
  }
 
  for (size_t i = 0; i < r; i++) {
    for (size_t k = 0; k < cr; k++) {
      for (size_t j = 0; j < c; j++) {
        m_res[i][j] += m_1[k][i] * m_2[k][j];
      }
    }
  }
 
  return m_res;
}
 
template <size_t r, size_t cr, size_t c>
MatNd<r, c> Math::DotTransportRHS(MatNd<r, cr> const &m_1,
                                  MatNd<c, cr> const &m_2) {
  MatNd<r, c> m_res;
 
  for (size_t i = 0; i < r; i++) {
    for (size_t j = 0; j < c; j++) {
      m_res[i][j] = 0;
    }
  }
 
  for (size_t i = 0; i < r; i++) {
    for (size_t j = 0; j < c; j++) {
      for (size_t k = 0; k < cr; k++) {
        m_res[i][j] += m_1[i][k] * m_2[j][k];
      }
    }
  }
 
  return m_res;
}
 
} // namespace netdem