.. _program_listing_file_numerics_math_utils.h:

Program Listing for File math_utils.h
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file_numerics_math_utils.h>` (``numerics/math_utils.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #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