cmvr-es/src/common/utils/math/support_functions.h

//
// Created by lgv on 11/10/25.
//

#pragma once
#include <cmath>
#include <vector>
#include <algorithm>
#include <iostream>
#include <Eigen/Core>

class SupportFunctions {
private:
    static constexpr double EPS = 1e-9;
public:
    static double normalize_angle(double angle) {
        double a = std::fmod(angle, 2.0 * M_PI);
        if (a < -M_PI) a += 2.0 * M_PI;
        if (a > M_PI) a -= 2.0 * M_PI;

        if (std::abs(a - M_PI) < EPS) return M_PI;
        if (std::abs(a + M_PI) < EPS) return -M_PI;
        return a;
    }

    static bool angle_in_wrap(double x, double L, double U) {
        x = normalize_angle(x);
        L = normalize_angle(L);
        U = normalize_angle(U);
        if (L <= U) return (x > L - EPS && x < U + EPS);
        return (x > L - EPS || x < U + EPS);
    }

    static std::vector<std::pair<double, double> >
    union_intervals(const std::vector<std::pair<double, double> > &in) {
        if (in.empty()) return {};
        std::vector<std::pair<double, double> > v = in;
        std::sort(v.begin(), v.end(), [](auto &a, auto &b) {
            return (a.first < b.first) || (a.first == b.first && a.second < b.second);
        });
        std::vector<std::pair<double, double> > out;
        double L = v[0].first, R = v[0].second;
        for (size_t i = 1; i < v.size(); ++i) {
            if (v[i].first <= R + EPS) R = std::max(R, v[i].second);
            else {
                out.push_back({L, R});
                L = v[i].first;
                R = v[i].second;
            }
        }
        out.push_back({L, R});
        return out;
    }


    static std::vector<std::pair<double, double> > intersect_intervals(
        const std::vector<std::pair<double, double> > &A, const std::vector<std::pair<double, double> > &B) {
        if (A.empty() || B.empty()) return {};

        // 先复制并排序（按起点）
        auto SA = A, SB = B;
        std::sort(SA.begin(), SA.end(),
                  [](auto &x, auto &y) { return x.first < y.first; });
        std::sort(SB.begin(), SB.end(),
                  [](auto &x, auto &y) { return x.first < y.first; });

        // 双指针求交
        std::vector<std::pair<double, double> > out;
        size_t i = 0, j = 0;
        while (i < SA.size() && j < SB.size()) {
            double L = std::max(SA[i].first, SB[j].first);
            double R = std::min(SA[i].second, SB[j].second);
            if (R > L) out.emplace_back(L, R);

            // 谁先结束谁前进
            if (SA[i].second < SB[j].second) ++i;
            else ++j;
        }
        // 合并可能相邻/重叠的小段
        if (out.empty()) return out;

        std::vector<std::pair<double, double> > merged;
        merged.reserve(out.size());
        std::sort(out.begin(), out.end(),
                  [](auto &x, auto &y) { return x.first < y.first; });

        merged.push_back(out[0]);
        for (size_t k = 1; k < out.size(); ++k) {
            if (out[k].first <= merged.back().second + EPS) {
                merged.back().second = std::max(merged.back().second, out[k].second);
            } else {
                merged.push_back(out[k]);
            }
        }
        return merged;
    }


    static bool wraps(double L, double U) {
        L = normalize_angle(L); // [-π, π]
        U = normalize_angle(U); // [-π, π]
        return (L > U); // 在 [-π, π] 规范下仍成立
    }

    static double deg2rad(double deg) { return deg * M_PI / 180.0; }
    static double rad2deg(double rad) { return rad * 180.0 / M_PI; }

    template<class T>
    static constexpr int sign(T x, T eps) {
        return (x > eps) - (x < -eps);
    }

    template<class T>
    static constexpr int sign(T x) {
        return sign(x, T(0));
    }

    template<typename T>
    static T clamp(T v, T lo, T hi) {
        return std::max(lo, std::min(hi, v));
    }

    static void print_intervals(const std::vector<std::pair<double, double> > &intervals) {
        for (const auto &interval: intervals) {
            std::cout << "[" << interval.first << ", " << interval.second << "] ";
        }
        std::cout << std::endl;
    }


    // 假设 T 是合法的刚体变换（旋转正交、det≈+1）
    static  Eigen::Matrix4d invertHomogeneous(const Eigen::Matrix4d& T) {
        Eigen::Matrix3d R = T.block<3,3>(0,0);
        Eigen::Vector3d t = T.block<3,1>(0,3);
        Eigen::Matrix4d Ti = Eigen::Matrix4d::Identity();
        Eigen::Matrix3d Rt = R.transpose();
        Ti.block<3,3>(0,0) = Rt;
        Ti.block<3,1>(0,3) = -Rt * t;
        return Ti;
    }
};