231 lines
7.5 KiB
C++
231 lines
7.5 KiB
C++
//
|
||
// Created by lgv on 11/10/25.
|
||
//
|
||
|
||
#pragma once
|
||
#include <cstdint>
|
||
#include <cmath>
|
||
#include <vector>
|
||
#include <algorithm>
|
||
#include <sstream>
|
||
#include "common/base/logging/logger.h"
|
||
#include <Eigen/Core>
|
||
|
||
class SupportFunctions {
|
||
private:
|
||
static constexpr double EPS = 1e-9;
|
||
public:
|
||
static constexpr std::int64_t absoluteDifference(const std::int32_t lhs,
|
||
const std::int32_t rhs) noexcept {
|
||
return lhs >= rhs
|
||
? static_cast<std::int64_t>(lhs) - static_cast<std::int64_t>(rhs)
|
||
: static_cast<std::int64_t>(rhs) - static_cast<std::int64_t>(lhs);
|
||
}
|
||
|
||
static constexpr std::int64_t cyclicAbsoluteDifference(
|
||
const std::int32_t lhs,
|
||
const std::int32_t rhs,
|
||
const std::int64_t period) noexcept {
|
||
const auto linear_distance = absoluteDifference(lhs, rhs);
|
||
if (period <= 0) {
|
||
return linear_distance;
|
||
}
|
||
const auto wrapped_distance = linear_distance % period;
|
||
return std::min(wrapped_distance, period - wrapped_distance);
|
||
}
|
||
|
||
static std::vector<double> eigen_to_vector(const Eigen::VectorXd &v) {
|
||
return std::vector<double>(v.data(), v.data() + v.size());
|
||
}
|
||
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) {
|
||
std::ostringstream output;
|
||
for (const auto &interval: intervals) {
|
||
output << "[" << interval.first << ", " << interval.second << "] ";
|
||
}
|
||
if (!output.str().empty()) {
|
||
CMVR_LOG(INFO) << output.str();
|
||
}
|
||
}
|
||
|
||
|
||
// 假设 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;
|
||
}
|
||
|
||
|
||
static int calcLookahead(double qd_ref,
|
||
double qdd_ref,
|
||
double v_max,
|
||
double dt,
|
||
int n_min = 1,
|
||
int n_max = 5)
|
||
{
|
||
double v0 = std::abs(qd_ref);
|
||
double a = std::abs(qdd_ref);
|
||
double vmax = std::abs(v_max);
|
||
|
||
// 1) 已经在接近最大速度的匀速阶段：直接用 1 步
|
||
const double vel_sat_ratio = 0.95; // 95% vmax 就认为是全速段
|
||
if (v0 >= vel_sat_ratio * vmax) {
|
||
return 1;
|
||
}
|
||
|
||
// 2) 其他情况再用“加速 + 饱和”的那套公式
|
||
double s_target = vmax * dt;
|
||
|
||
if (v0 < 1e-6 && a < 1e-6) {
|
||
return n_min;
|
||
}
|
||
|
||
double tau = 0.0;
|
||
|
||
if (a < 1e-6) {
|
||
// 近似匀速：v0 * tau = s_target
|
||
tau = s_target / (v0 + 1e-6);
|
||
} else {
|
||
double A = 0.5 * a;
|
||
double B = v0;
|
||
double C = -s_target;
|
||
|
||
double disc = B * B - 4.0 * A * C;
|
||
if (disc < 0.0) disc = 0.0;
|
||
double sqrt_disc = std::sqrt(disc);
|
||
|
||
double tau_quad = (-B + sqrt_disc) / (2.0 * A);
|
||
double v_at_tau = v0 + a * tau_quad;
|
||
|
||
if (v_at_tau <= vmax + 1e-9) {
|
||
tau = tau_quad;
|
||
} else {
|
||
double t_sat = (vmax - v0) / a;
|
||
double s_sat = v0 * t_sat + 0.5 * a * t_sat * t_sat;
|
||
|
||
if (s_target <= s_sat) {
|
||
tau = std::min(tau_quad, t_sat);
|
||
} else {
|
||
tau = t_sat + (s_target - s_sat) / vmax;
|
||
}
|
||
}
|
||
}
|
||
|
||
int n = (int)std::ceil(tau / dt);
|
||
if (n < n_min) n = n_min;
|
||
if (n > n_max) n = n_max;
|
||
return n;
|
||
}
|
||
};
|