.. _program_listing_file_numerics_math_utils.cc:

Program Listing for File math_utils.cc
======================================

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

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

.. code-block:: cpp

   
   
   #include "lupnt/numerics/math_utils.h"
   
   #include <Eigen/Cholesky>
   #include <Eigen/Eigenvalues>
   #include <Eigen/SVD>
   #include <autodiff/forward/real.hpp>
   #include <cmath>
   #include <random>
   
   #include "lupnt/core/constants.h"
   #include "lupnt/core/random_engine.h"
   
   namespace lupnt {
     template <typename T> VectorX<T> Arange(T start, T stop, T step) {
       std::vector<T> values;
       for (T value = start; value < stop; value += step) {
         values.push_back(value);
       }
       // Create a proper VectorX that owns its memory (not a Map to temporary data)
       return VectorX<T>(Eigen::Map<VectorX<T>>(values.data(), values.size()));
     }
     template VectorX<int> Arange<int>(int start, int stop, int step);
     template VectorX<double> Arange<double>(double start, double stop, double step);
     template VectorX<Real> Arange<Real>(Real start, Real stop, Real step);
   
     VecXd Subsample(const VecX& x, int step) {
       int n = x.size();
       VecX out(n / step);
       for (int i = 0; i < n; i += step) out(i / step) = x(i);
       return out;
     }
   
     MatXd Subsample(const MatX& x, int step_row, int step_col) {
       int n_row = x.rows();
       int n_col = x.cols();
       MatX out(n_row / step_row, n_col / step_col);
       for (int i = 0; i < n_row; i += step_row) {
         for (int j = 0; j < n_col; j += step_col) {
           out(i / step_row, j / step_col) = x(i, j);
         }
       }
       return out;
     }
   
     Real AngleBetweenVecs(const VecX& x, const VecX& y) {
       return 2.0
              * atan2((x.normalized() - y.normalized()).norm(),
                      (x.normalized() + y.normalized()).norm());
     }
   
     VecX AngleBetweenVecs(const MatX& x, const MatX& y) {
       return (x.rowwise().normalized().array() * y.rowwise().normalized().array())
           .rowwise()
           .sum()
           .cwiseMax(-1.0)
           .cwiseMin(1.0)
           .acos();
     }
   
     double DegMinSec2DeciDeg(double degrees, double minutes, double seconds) {
       double decimalDegrees = degrees + minutes / 60.0 + seconds / 3600.0;
       return decimalDegrees;
     }
   
     Real WrapToPi(Real angle) { return atan2(sin(angle), cos(angle)); }
     VecX WrapToPi(const VecX& angle) {
       int n = angle.size();
       VecX out(n);
       for (int i = 0; i < n; i++) out(i) = WrapToPi(angle(i));
       return out;
     }
   
     Real WrapToTwoPi(Real angle) { return angle - TWO_PI * floor(angle / TWO_PI); }
     VecX WrapToTwoPi(const VecX& angle) {
       int n = angle.size();
       VecX out(n);
       for (int i = 0; i < n; i++) out(i) = WrapToTwoPi(angle(i));
       return out;
     }
   
     Real round(Real x, int n) {
       Real y = x;
       y[0] = std::round(x.val() * std::pow(10, n)) / std::pow(10, n);
       return y;
     }
   
     Real frac(Real x) {
       Real y = x;
       y[0] = x.val() - std::floor(x.val());
       return y;
     }
   
     Real ceil(Real x) {
       Real y = x;
       y[0] = std::ceil(x.val());
       return y;
     }
   
     Real floor(Real x) {
       Real y = x;
       y[0] = std::floor(x.val());
       return y;
     }
   
     Real mod(Real x, Real y) {
       Real z = x;
       z[0] = std::fmod(x.val(), y.val());
       return z;
     }
   
     Real DecimalToDecibel(Real x) { return 10 * log10(x); }
     ArrX DecimalToDecibel(const ArrX& x) { return 10 * log10(x); }
   
     Real Max(Real x, Real y) { return x.val() > y.val() ? x : y; }
     Real Min(Real x, Real y) { return x.val() < y.val() ? x : y; }
     double MaxD(double x, double y) { return x > y ? x : y; }
     double MinD(double x, double y) { return x < y ? x : y; }
   
     Real DecibelToDecimal(Real x) { return pow(10, x / 10); }
     ArrX DecibelToDecimal(const ArrX& x) { return pow(10, x / 10); }
   
     Vec3 DegToDegMinSec(Real deg) {
       Real d = floor(deg);
       Real m = floor((deg - d) * 60);
       Real s = (deg - d - m / 60) * 3600;
       return Vec3(d, m, s);
     }
   
     Real DegMinSecToDeg(const Vec3& dms) {
       Real decdeg = dms(0) + dms(1) / 60.0 + dms(2) / 3600.0;
       return decdeg;
     }
   
     Real sind(Real x) { return sin(x * RAD); }
   
     Real cosd(Real x) { return cos(x * RAD); }
   
     Real tand(Real x) { return tan(x * RAD); }
   
     MatX SampleMvNormal(const VecX& mean, const MatX& cov, int nn, std::mt19937* rng) {
       int n = mean.size();
       Eigen::LLT<MatXd> llt(cov.cast<double>());
       MatXd L = llt.matrixL();
       std::normal_distribution<double> dist(0.0, 1.0);
       std::mt19937& gen = rng ? *rng : lupnt::RandomEngine::Get();
       MatX samples(nn, n);
       for (int i = 0; i < nn; i++) {
         VecXd z(n);
         for (int j = 0; j < n; j++) z(j) = dist(gen);
         samples.row(i) = mean + (L * z).cast<Real>();
       }
       return samples;
     }
   
     Real SampleNormal(Real mean, Real stddev, std::mt19937* rng) {
       std::normal_distribution<double> dist(0.0, 1.0);
       std::mt19937& gen = rng ? *rng : lupnt::RandomEngine::Get();
       return dist(gen) * stddev + mean;
     }
   
     Real safe_acos(Real x) {
       if (x >= 1.0) {
         return acos(x - EPS);
       } else if (x <= -1.0) {
         return acos(x + EPS);
       } else {
         return acos(x);
       }
     }
   
     Real safe_asin(Real x) {
       if (x >= 1.0) {
         return asin(x - 1e-16);
       } else if (x <= -1.0) {
         return asin(x + 1e-16);
       } else {
         return asin(x);
       }
     }
   
     Mat3 RotX(Real angle) {
       Real c = cos(angle);
       Real s = sin(angle);
       Mat3 R{
           {1.0, 0.0, 0.0},
           {0.0, c, s},
           {0.0, -s, c},
       };
       return R;
     }
   
     Mat3 RotY(Real angle) {
       Real c = cos(angle);
       Real s = sin(angle);
       Mat3 R{
           {c, 0.0, -s},
           {0.0, 1.0, 0.0},
           {s, 0.0, c},
       };
       return R;
     }
   
     Mat3 RotZ(Real angle) {
       Real c = cos(angle);
       Real s = sin(angle);
       Mat3 R{
           {c, s, 0.0},
           {-s, c, 0.0},
           {0.0, 0.0, 1.0},
       };
       return R;
     }
   
     Mat3 RotXdot(Real angle, Real angle_dot) {
       Real c = cos(angle);
       Real s = sin(angle);
       Mat3 R_dot{
           {0.0, 0.0, 0.0},
           {0.0, -s * angle_dot, c * angle_dot},
           {0.0, -c * angle_dot, -s * angle_dot},
       };
       return R_dot;
     }
   
     Mat3 RotYdot(Real angle, Real angle_dot) {
       Real c = cos(angle);
       Real s = sin(angle);
       Mat3 R_dot{
           {-s * angle_dot, 0.0, -c * angle_dot},
           {0.0, 0.0, 0.0},
           {c * angle_dot, 0.0, -s * angle_dot},
       };
       return R_dot;
     }
   
     Mat3 RotZdot(Real angle, Real angle_dot) {
       Real c = cos(angle);
       Real s = sin(angle);
       Mat3 R_dot{
           {-s * angle_dot, c * angle_dot, 0.0},
           {-c * angle_dot, -s * angle_dot, 0.0},
           {0.0, 0.0, 0.0},
       };
       return R_dot;
     }
   
     Mat3 Skew(const Vec3& x) {
       Mat3 skew{
           {0.0, -x(2), x(1)},
           {x(2), 0.0, -x(0)},
           {-x(1), x(0), 0.0},
       };
       return skew;
     }
   
     std::vector<double> EigenToVector(const VecX& x) {
       std::vector<double> y(x.size());
       for (int i = 0; i < x.size(); i++) {
         y[i] = x(i).val();
       }
       return y;
     }
   
     Real F(Real eta, Real m, Real l) {
       const double eps = 100.0 * EPS;
       Real w, W, a, n, g;
       w = m / (eta * eta) - l;
       if (abs(w) < 0.1) {  // Series expansion
         W = a = 4.0 / 3.0;
         n = 0.0;
         do {
           n += 1.0;
           a *= w * (n + 2.0) / (n + 1.5);
           W += a;
         } while (abs(a) >= eps);
       } else {
         if (w > 0.0) {
           g = 2.0 * asin(sqrt(w));
           W = (2.0 * g - sin(2.0 * g)) / pow(sin(g), 3);
         } else {
           g = 2.0 * log(sqrt(-w) + sqrt(1.0 - w));  // =2.0*arsinh(sqrt(-w))
           W = (sinh(2.0 * g) - 2.0 * g) / pow(sinh(g), 3);
         }
       }
       return (1.0 - eta + (w + l) * W);
     }
   
     Real RatioOfSectorToTriangleArea(const Vec3& r1, const Vec3& r2, Real tau) {
       const int max_it = 30;
       const double delta = 100 * EPS;
   
       Real s1 = r1.norm();
       Real s2 = r2.norm();
       Real kappa = sqrt(2.0 * (s1 * s2 + r1.dot(r2)));
       Real m = tau * tau / pow(kappa, 3);
       Real l = (s1 + s2) / (2.0 * kappa) - 0.5;
       Real eta_min = sqrt(m / (l + 1.0));
   
       // Start with Hansen's approximation
       Real eta2 = (12.0 + 10.0 * sqrt(1.0 + (44.0 / 9.0) * m / (l + 5.0 / 6.0))) / 22.0;
       Real eta1 = eta2 + 0.1;
   
       // Secant method
       Real F1 = F(eta1, m, l);
       Real F2 = F(eta2, m, l);
   
       int it = 0;
       while (abs(F2 - F1) > delta && it < max_it) {
         Real d_eta = -F2 * (eta2 - eta1) / (F2 - F1);
         eta1 = eta2;
         F1 = F2;
         while (eta2 + d_eta <= eta_min) d_eta *= 0.5;
         eta2 += d_eta;
         F2 = F(eta2, m, l);
         ++it;
       }
       if (it >= max_it) throw std::runtime_error("Hansen's method did not converge");
       return eta2;
     }
   
     VecXd SolveLinearEqSVD(const MatXd& A, const VecXd& b) {
       Eigen::JacobiSVD<MatXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
       return svd.solve(b);
     }
   
     // Compute A * X = B
     MatXd SolveLinearEqSVD(const MatXd& A, const MatXd& B) {
       Eigen::JacobiSVD<MatXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
       MatXd X(A.cols(), B.cols());
       for (int i = 0; i < B.cols(); i++) {
         X.col(i) = svd.solve(B.col(i));
       }
       return X;
     }
   
     MatXd PseudoInverse(const MatXd& A) {
       Eigen::JacobiSVD<MatXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
       double tol = 1e-6;
       MatXd S = svd.singularValues();
       MatXd S_inv = MatXd::Zero(S.rows(), S.cols());
       for (int i = 0; i < S.rows(); i++) {
         if (S(i) > tol) {
           S_inv(i, i) = 1.0 / S(i);
         }
       }
       MatXd S_pinv = svd.matrixV() * S_inv * svd.matrixU().transpose();
       return S_pinv;
     }
   
     VecX JacobianParallel(const std::function<VecX(const VecX&)>& func, const VecX& x0, MatXd& J) {
       int n = x0.size();
       std::vector<MatXd> J_vec(n);
       VecX xf;
   
   #pragma omp parallel for
       for (int i = 0; i < n; i++) {
         VecX xi;
         MatXd Ji;
         VecX x0_tmp = x0.cast<double>();
         jacobian(func, wrt(x0_tmp(i)), at(x0_tmp), xi, Ji);
         if (i == 0) xf = xi;
         J_vec[i] = Ji;
       }
   
       int m = xf.size();
       J.resize(m, n);
       for (int i = 0; i < n; i++) {
         for (int j = 0; j < m; j++) {
           J(j, i) = J_vec[i](j);
         }
       }
       return xf;
     }
   
     VecX Vech(const MatX& x) {
       int n = x.rows();
       VecX v(n * (n + 1) / 2);
       for (int i = 0; i < n; i++) {
         for (int j = 0; j <= i; j++) {
           v(i * (n + 1) / 2 + j) = x(i, j);
         }
       }
       return v;
     }
   
     Real RotationAngle(const Mat3& R) { return acos((R.trace() - 1.0) / 2.0); }
   }  // namespace lupnt