98 lines
2.3 KiB
C++
98 lines
2.3 KiB
C++
// SharedGeometry.h
|
||
#ifndef SHARED_GEOMETRY_H
|
||
#define SHARED_GEOMETRY_H
|
||
|
||
#include <string>
|
||
#include <vector>
|
||
#include <cmath>
|
||
#include <sstream>
|
||
#include <iomanip>
|
||
#include "QuadrupedRobotSimulation/OPERATION.h"
|
||
// 2D向量类
|
||
class Vector2D
|
||
{
|
||
public:
|
||
double X;
|
||
double Y;
|
||
|
||
Vector2D();
|
||
Vector2D(double x, double y);
|
||
|
||
// 运算符重载
|
||
Vector2D operator+(const Vector2D &other) const;
|
||
Vector2D operator-(const Vector2D &other) const;
|
||
Vector2D operator*(double scalar) const;
|
||
|
||
// 几何运算
|
||
double distanceTo(const Vector2D &other) const;
|
||
double length() const;
|
||
Vector2D normalized() const;
|
||
|
||
// 静态方法
|
||
static Vector2D Zero();
|
||
static double Distance(const Vector2D &v1, const Vector2D &v2);
|
||
double Length() const { return length(); }
|
||
Vector2D Normalize() const { return normalized(); }
|
||
|
||
// 叉积和点积
|
||
double Cross(const Vector2D &other) const;
|
||
double Dot(const Vector2D &other) const;
|
||
};
|
||
|
||
// 姿态类（简化版）
|
||
class Pose7
|
||
{
|
||
public:
|
||
double tx;
|
||
double ty;
|
||
double tz;
|
||
double qx;
|
||
double qy;
|
||
double qz;
|
||
double qw;
|
||
|
||
Pose7();
|
||
};
|
||
|
||
// 姿态计算器 - 兼顾两个程序的需求
|
||
class PoseCalculator
|
||
{
|
||
public:
|
||
/**
|
||
* 计算两个2D点之间的位置和姿态（四元数）
|
||
* 适用于CrankSliderMechanism
|
||
*/
|
||
static Pose7 CalculatePoseAndQuaternion(const Vector2D &start, const Vector2D &end);
|
||
|
||
/**
|
||
* 计算两个2D点之间的位置和姿态（四元数）
|
||
* 返回double[7]数组，顺序为: [x, y, z, qw, qx, qy, qz]
|
||
* 适用于BaseClass程序
|
||
*/
|
||
static std::vector<double> CalculatePoseAndQuaternionArray(double x1, double y1, double x2, double y2);
|
||
|
||
/**
|
||
* 毫米转米
|
||
*/
|
||
static double MMToM(double MM);
|
||
|
||
/**
|
||
* 四舍五入辅助函数
|
||
*/
|
||
static double Round(double value, int decimals = 6);
|
||
|
||
/**
|
||
* 数字转字符串
|
||
*/
|
||
static std::string ToString(double value, int decimals = 6);
|
||
|
||
/**
|
||
* 计算姿态并返回C_ObjStates对象（如果需要）
|
||
*/
|
||
static C_ObjStates CalculatePoseAndQuaternion_C_ObjStates(const std::string &i,
|
||
double x1, double y1,
|
||
double x2, double y2);
|
||
};
|
||
|
||
#endif // SHARED_GEOMETRY_H
|