Initial commit

src/QuadrupedRobotSimulation/SharedGeometry.cpp

@@ -0,0 +1,211 @@
+// SharedGeometry.cpp
+#include "SharedGeometry.h"
+#include <stdexcept>
+#include <cmath>
+#include <algorithm>
+
+// 如果没有定义M_PI，则定义它
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+// ==================== Vector2D 实现 ====================
+
+Vector2D::Vector2D() : X(0.0), Y(0.0) {}
+
+Vector2D::Vector2D(double x, double y) : X(x), Y(y) {}
+
+Vector2D Vector2D::operator+(const Vector2D &other) const
+{
+    return Vector2D(X + other.X, Y + other.Y);
+}
+
+Vector2D Vector2D::operator-(const Vector2D &other) const
+{
+    return Vector2D(X - other.X, Y - other.Y);
+}
+
+Vector2D Vector2D::operator*(double scalar) const
+{
+    return Vector2D(X * scalar, Y * scalar);
+}
+
+double Vector2D::distanceTo(const Vector2D &other) const
+{
+    double dx = X - other.X;
+    double dy = Y - other.Y;
+    return std::sqrt(dx * dx + dy * dy);
+}
+
+double Vector2D::length() const
+{
+    return std::sqrt(X * X + Y * Y);
+}
+
+Vector2D Vector2D::normalized() const
+{
+    double len = length();
+    if (len > 0.0)
+    {
+        return Vector2D(X / len, Y / len);
+    }
+    return Vector2D(0.0, 0.0);
+}
+
+Vector2D Vector2D::Zero()
+{
+    return Vector2D(0.0, 0.0);
+}
+
+double Vector2D::Distance(const Vector2D &v1, const Vector2D &v2)
+{
+    return std::sqrt(std::pow(v2.X - v1.X, 2) + std::pow(v2.Y - v1.Y, 2));
+}
+
+double Vector2D::Cross(const Vector2D &other) const
+{
+    return X * other.Y - Y * other.X;
+}
+
+double Vector2D::Dot(const Vector2D &other) const
+{
+    return X * other.X + Y * other.Y;
+}
+
+// ==================== Pose7 实现 ====================
+
+Pose7::Pose7() : tx(0.0), ty(0.0), tz(0.0), qx(0.0), qy(0.0), qz(0.0), qw(1.0) {}
+
+// ==================== PoseCalculator 实现 ====================
+
+Pose7 PoseCalculator::CalculatePoseAndQuaternion(const Vector2D &start, const Vector2D &end)
+{
+    Pose7 pose;
+    pose.tx = start.X;
+    pose.ty = start.Y;
+    pose.tz = 0.0;
+
+    Vector2D direction = end - start;
+    double length = direction.length();
+
+    if (length > 0.0)
+    {
+        direction = direction * (1.0 / length);
+        double angle = std::atan2(direction.Y, direction.X);
+
+        // 转换为四元数（绕Z轴旋转）
+        double halfAngle = angle * 0.5;
+        pose.qw = std::cos(halfAngle);
+        pose.qz = std::sin(halfAngle);
+        pose.qx = 0.0;
+        pose.qy = 0.0;
+    }
+    else
+    {
+        pose.qw = 1.0;
+        pose.qx = 0.0;
+        pose.qy = 0.0;
+        pose.qz = 0.0;
+    }
+
+    return pose;
+}
+
+std::vector<double> PoseCalculator::CalculatePoseAndQuaternionArray(double x1, double y1, double x2, double y2)
+{
+    double x_D = x1;
+    double y_D = y1;
+    double x_G = x2;
+    double y_G = y2;
+
+    // 方向向量
+    double dx = x_G - x_D;
+    double dy = y_G - y_D;
+
+    double v_norm = std::sqrt(dx * dx + dy * dy);
+
+    // 使用小的epsilon值而不是直接比较0
+    if (std::abs(v_norm) < 1e-10)
+    {
+        throw std::invalid_argument("两个点位置相同，无法计算姿态。");
+    }
+
+    // 单位化方向向量
+    double vx = dx / v_norm;
+    double vy = dy / v_norm;
+
+    // 四元数（绕Z轴旋转）
+    double theta = std::atan2(vy, vx);
+    double halfTheta = theta / 2.0;
+    double qw = std::cos(halfTheta);
+    double qx = 0.0;
+    double qy = 0.0;
+    double qz = std::sin(halfTheta);
+
+    // 返回长度为7的数组
+    return {x_D, y_D, 0.0, qw, qx, qy, qz};
+}
+
+double PoseCalculator::MMToM(double MM)
+{
+    return Round(MM / 1000.0, 6);
+}
+
+double PoseCalculator::Round(double value, int decimals)
+{
+    double factor = std::pow(10.0, decimals);
+    return std::round(value * factor) / factor;
+}
+
+std::string PoseCalculator::ToString(double value, int decimals)
+{
+    std::ostringstream oss;
+    oss << std::fixed << std::setprecision(decimals) << value;
+    return oss.str();
+}
+
+C_ObjStates PoseCalculator::CalculatePoseAndQuaternion_C_ObjStates(const std::string &i,
+                                                                   double x1, double y1,
+                                                                   double x2, double y2)
+{
+    // 如果需要实现这个方法，需要C_ObjStates的定义
+    // 这里先抛出一个异常，提醒需要实现
+    // throw std::runtime_error("C_ObjStates需要额外的定义，请提供C_ObjStates类的定义");
+
+    // 示例代码（需要C_ObjStates类）：
+
+    double x_D = MMToM(x1);
+    double y_D = MMToM(y1);
+    double x_G = MMToM(x2);
+    double y_G = MMToM(y2);
+
+    double dx = x_G - x_D;
+    double dy = y_G - y_D;
+    double v_norm = std::sqrt(dx * dx + dy * dy);
+
+    if (std::abs(v_norm) < 1e-10)
+    {
+        throw std::invalid_argument("两个点位置相同，无法计算姿态。");
+    }
+
+    double vx = dx / v_norm;
+    double vy = dy / v_norm;
+    double theta = std::atan2(vy, vx);
+    double halfTheta = theta / 2.0;
+    double qw = std::cos(halfTheta);
+    double qx = 0.0;
+    double qy = 0.0;
+    double qz = std::sin(halfTheta);
+
+    C_ObjStates c_ObjStates;
+    c_ObjStates.i = i;
+    c_ObjStates.tx = ToString(x_D);
+    c_ObjStates.ty = ToString(y_D);
+    c_ObjStates.tz = "0";
+    c_ObjStates.qx = ToString(qx);
+    c_ObjStates.qy = ToString(qy);
+    c_ObjStates.qz = ToString(qz);
+    c_ObjStates.qw = ToString(qw);
+
+    return c_ObjStates;
+}