69 lines
4.2 KiB
C++
69 lines
4.2 KiB
C++
#pragma once
|
||
// math_tools.hpp
|
||
// 内联实现 solver.cpp 所需的全部数学工具函数，无外部依赖。
|
||
|
||
#include <cmath>
|
||
#include <Eigen/Dense>
|
||
|
||
namespace tools
|
||
{
|
||
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
// square(x): 返回 x 的平方
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
inline double square(double x) { return x * x; }
|
||
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
// limit_rad(angle): 将弧度角限制在 (-π, π] 范围内
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
inline double limit_rad(double angle)
|
||
{
|
||
constexpr double two_pi = 2.0 * M_PI;
|
||
angle = std::fmod(angle + M_PI, two_pi);
|
||
if (angle < 0.0) angle += two_pi;
|
||
return angle - M_PI;
|
||
}
|
||
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
// eulers(R, ax0, ax1, ax2): 从旋转矩阵 R 提取欧拉角（内禀旋转，按 ax0->ax1->ax2 顺序）
|
||
//
|
||
// 常用调用方式：eulers(R, 2, 1, 0) 对应 ZYX（Yaw-Pitch-Roll）顺序。
|
||
// 返回 Vector3d { angle_ax0, angle_ax1, angle_ax2 }
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
inline Eigen::Vector3d eulers(const Eigen::Matrix3d & R, int ax0, int ax1, int ax2)
|
||
{
|
||
// 使用 Eigen 内置的欧拉角接口（内禀旋转）
|
||
return R.eulerAngles(ax0, ax1, ax2);
|
||
}
|
||
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
// xyz2ypd(xyz): 将三维空间坐标转换为 [yaw, pitch, distance]
|
||
//
|
||
// 坐标系约定（与云台/世界坐标系一致）：
|
||
// x 轴：前方（射击方向）
|
||
// y 轴：左方
|
||
// z 轴：上方
|
||
//
|
||
// 返回 Vector3d { yaw, pitch, distance }（单位：弧度，弧度，米）
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d & xyz)
|
||
{
|
||
double distance = xyz.norm();
|
||
double yaw = std::atan2(xyz.y(), xyz.x());
|
||
double pitch = std::atan2(xyz.z(), std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y()));
|
||
return Eigen::Vector3d(yaw, pitch, distance);
|
||
}
|
||
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
// get_abs_angle(a, b): 返回两个二维向量夹角的绝对值（弧度）
|
||
// ──────────────────────────────────────────────────────────────────────────────
|
||
inline double get_abs_angle(const Eigen::Vector2d & a, const Eigen::Vector2d & b)
|
||
{
|
||
double cos_angle = a.normalized().dot(b.normalized());
|
||
// 数值钳位避免 acos 域错误
|
||
cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
|
||
return std::acos(cos_angle);
|
||
}
|
||
|
||
} // namespace tools
|