Program Listing for File math_utils.h

↰ Return to documentation for file (numerics/math_utils.h)

#pragma once

#include <random>
#include <tuple>
#include <utility>

#include "lupnt/core/definitions.h"

namespace lupnt {

  template <typename Vec, std::size_t... Indices>
  auto UnpackImpl(const Vec& vec, std::index_sequence<Indices...>) {
    return std::make_tuple(vec(Indices)...);
  }

  template <typename T, int Size> auto Unpack(const Eigen::Matrix<T, Size, 1>& vec) {
    static_assert(Size > 0, "Cannot unpack a zero-sized vector.");
    return UnpackImpl(vec, std::make_index_sequence<Size>{});
  }

  template <typename T> VectorX<T> Arange(T start, T stop, T step = 1);

  VecXd Subsample(const VecX& x, int step);

  MatXd Subsample(const MatX& x, int step_row, int step_col);

  Real AngleBetweenVecs(const VecX& x, const VecX& y);

  VecX AngleBetweenVecs(const MatX& x, const MatX& y);

  double DegMinSec2DeciDeg(double degrees, double minutes, double seconds);

  Real WrapToPi(Real angle);

  Real WrapToTwoPi(Real angle);

  VecX WrapToPi(const VecX& angle);

  VecX WrapToTwoPi(const VecX& angle);

  Real DecimalToDecibel(Real x);

  ArrX DecimalToDecibel(const ArrX& x);

  Real DecibelToDecimal(Real x);

  ArrX DecibelToDecimal(const ArrX& x);

  Real Max(Real x, Real y);

  Real Min(Real x, Real y);

  double MaxD(double x, double y);

  double MinD(double x, double y);

  Real round(Real x, int n = 0);

  Real frac(Real x);

  Real ceil(Real x);

  Real floor(Real x);

  Real mod(Real x, Real y);

  Vec3 DegToDegMinSec(Real deg);

  Real DegMinSecToDeg(const Vec3& hms);

  Real sind(Real x);

  Real cosd(Real x);

  Real tand(Real x);

  Real safe_acos(Real x);

  Real safe_asin(Real x);

  template <typename T> T J0Bessel(T x) {
    // double J0 = 0.0;
    T y = 1.0;
    T sum = 1.0;
    for (int i = 1; i < 10; i++) {
      y = y * x * x / (4 * i * i);
      sum += y;
    }
    return sum;
  }

  template <typename T> T J1Bessel(T x) {
    // double J1 = 0.0;
    T y = 1.0;
    T sum = 1.0;
    for (int i = 1; i < 10; i++) {
      y = y * x / (2 * i * (2 * i + 1));
      sum += y;
    }
    return sum;
  }
  template <typename T> typename T::Scalar RootMeanSquare(const DenseBase<T>& x) {
    return sqrt(x.derived().squaredNorm() / x.size());
  }

  template <typename T> typename T::Scalar Percentile(const DenseBase<T>& x, double p) {
    auto evaluated = x.eval();
    std::vector<typename T::Scalar> data(evaluated.data(), evaluated.data() + evaluated.size());
    std::sort(data.begin(), data.end());
    size_t index = std::ceil(p * (data.size() - 1));
    return data[index];
  }

  template <typename T> typename T::Scalar Std(const DenseBase<T>& x) {
    auto mean_val = x.mean();
    return sqrt((x.derived().array() - mean_val).square().sum() / (x.size() - 1));
  }

  template <typename T> T erfc(T x) { return 1 - erf(x.val()); }

  template <typename T> T qfunc(T x) { return 0.5 * erfc(x / sqrt(2)); }

  MatX SampleMvNormal(const VecX& mean, const MatX& cov, int nn = 1, std::mt19937* rng = nullptr);

  Real SampleNormal(Real mean = 0.0, Real std = 1.0, std::mt19937* rng = nullptr);

  template <typename T, int N1, int M1, int N2, int M2>
  Matrix<T, N1 + N2, M1 + N2> BlockDiagonal(const Matrix<T, N1, M1>& A,
                                            const Matrix<T, N2, M2>& B) {
    Matrix<T, N1 + N2, M1 + M2> C;
    C << A, Matrix<T, N1, M2>::Zero(), Matrix<T, N2, M1>::Zero(), B;
    return C;
  }

  Mat3 RotX(Real angle);

  Mat3 RotY(Real angle);

  Mat3 RotZ(Real angle);

  Mat3 RotXdot(Real angle, Real angle_dot);

  Mat3 RotYdot(Real angle, Real angle_dot);

  Mat3 RotZdot(Real angle, Real angle_dot);

  Mat3 Skew(const Vec3& x);

  template <typename T, typename Derived>
  std::vector<T> EigenToVector(const Eigen::DenseBase<Derived>& x) {
    std::vector<T> y(x.size());
    for (int i = 0; i < x.size(); i++) y[i] = static_cast<T>(x(i));
    return y;
  }

  template <typename T, int N, typename Derived>
  std::array<T, N> EigenToArray(const Eigen::Matrix<T, N, 1>& x) {
    std::array<T, N> y;
    for (int i = 0; i < N; i++) y[i] = static_cast<T>(x(i));
    return y;
  }

  Real RatioOfSectorToTriangleArea(const Vec3& r1, const Vec3& r2, Real tau);

  VecXd SolveLinearEqSVD(const MatXd& A, const VecXd& b);  // Ax = b

  MatXd SolveLinearEqSVD(const MatXd& A, const MatXd& B);  // AX = B

  MatXd PseudoInverse(const MatXd& A);

  VecX JacobianParallel(const std::function<VecX(const VecX&)>& func, const VecX& x0, MatXd& J);

  VecX Vech(const MatX& x);

  Real RotationAngle(const Mat3& R);

}  // namespace lupnt