Initial commit

src/FourBarMechanism/CrankRockingBlockMechanism_Forward.cpp

@@ -0,0 +1,480 @@
+#include "CrankRockingBlockMechanism_Forward.h"
+#include <cmath>
+#include <algorithm>
+#include <stdexcept>
+#include <cstdio>
+
+using namespace std;
+
+// ==================== CrankRockingBlockMechanism_Forward 实现 ====================
+
+CrankRockingBlockMechanism_Forward::CrankRockingBlockMechanism_Forward()
+    : name("曲柄摇块机构 (正解)"),
+      type(MechanismType::CrankRockingBlock_Forward),
+      frameCount(0),
+      thetaADelta(0.0),
+      thetaAValueFactor(1.0),
+      thetaAFlip(1)
+{
+    // 设置默认参数
+    parameters.CrankLength2 = 1.0;         // OA - 曲柄长度
+    parameters.ConnectingRodLength2 = 3.0; // AB - 连杆长度
+    parameters.SliderOffset2 = 3.0;        // OC - 摇块中心距
+    parameters.InputValue = 0.0;           // 曲柄角度
+    parameters.AngularVelocity = 2.0;      // 角速度
+}
+
+string CrankRockingBlockMechanism_Forward::getName() const
+{
+    return name;
+}
+
+MechanismType CrankRockingBlockMechanism_Forward::getType() const
+{
+    return type;
+}
+
+MechanismParameters CrankRockingBlockMechanism_Forward::getParameters() const
+{
+    return parameters;
+}
+
+void CrankRockingBlockMechanism_Forward::setParameters(const MechanismParameters &params)
+{
+    parameters = params;
+}
+
+void CrankRockingBlockMechanism_Forward::setLink(double OA, double AB, double OC)
+{
+    parameters.CrankLength2 = OA;
+    parameters.ConnectingRodLength2 = AB;
+    parameters.SliderOffset2 = OC;
+}
+
+void CrankRockingBlockMechanism_Forward::setOA(double length)
+{
+    parameters.CrankLength2 = length;
+}
+
+void CrankRockingBlockMechanism_Forward::setAB(double length)
+{
+    parameters.ConnectingRodLength2 = length;
+}
+
+void CrankRockingBlockMechanism_Forward::setOC(double distance)
+{
+    parameters.SliderOffset2 = distance;
+}
+
+void CrankRockingBlockMechanism_Forward::setThetaADelta(double delta)
+{
+    thetaADelta = delta;
+}
+
+void CrankRockingBlockMechanism_Forward::setThetaAValueFactor(double factor)
+{
+    thetaAValueFactor = factor;
+}
+
+void CrankRockingBlockMechanism_Forward::setThetaAFlip(int flip)
+{
+    thetaAFlip = flip;
+}
+
+MechanismState CrankRockingBlockMechanism_Forward::calculate(double crankAngleDeg)
+{
+    // 应用配置参数
+    double angleDeg = thetaADelta + crankAngleDeg * thetaAValueFactor * thetaAFlip;
+    parameters.InputValue = angleDeg;
+
+    double OA = parameters.CrankLength2;         // 曲柄长度
+    double AB = parameters.ConnectingRodLength2; // 连杆长度
+    double OC = parameters.SliderOffset2;        // 摇块中心距
+
+    double crankAngle = degreesToRadians(angleDeg);
+
+    MechanismState state;
+    state.InputValue = angleDeg;
+
+    try
+    {
+        // 验证机构参数
+        auto validation = validateParameters();
+        if (!validation.isValid())
+        {
+            state.ErrorMessage = validation.getCombinedMessage();
+            return state;
+        }
+
+        // 计算各点坐标
+        Vector2D O(0.0, 0.0); // 固定点
+        Vector2D C(OC, 0.0);  // 摇块中心
+        Vector2D A(           // 曲柄端点
+            OA * cos(crankAngle),
+            OA * sin(crankAngle));
+
+        // 计算滑块B的位置 (B点在直线AC上)
+        Vector2D B = calculatePointB(A, C, AB);
+
+        if (B.X == 0.0 && B.Y == 0.0 && AB > 1e-10)
+        {
+            state.ErrorMessage = "无法计算滑块位置，机构可能无法装配";
+            return state;
+        }
+
+        // 记录轨迹
+        trajectoryB.push_back(B);
+        crankCirclePoints.push_back(A);
+        rockerTrajectory.push_back(B);
+
+        // 限制轨迹点数
+        const size_t MAX_TRAJECTORY_POINTS = 500;
+        if (trajectoryB.size() > MAX_TRAJECTORY_POINTS)
+        {
+            trajectoryB.erase(trajectoryB.begin());
+            crankCirclePoints.erase(crankCirclePoints.begin());
+            rockerTrajectory.erase(rockerTrajectory.begin());
+        }
+
+        // 更新帧计数
+        frameCount++;
+
+        // 计算角度信息
+        double rockerAngle = atan2(B.Y - C.Y, B.X - C.X);
+        double acDistance = Vector2D::Distance(A, C);
+
+        // 设置状态
+        state.Points["O"] = O;
+        state.Points["A"] = A;
+        state.Points["B"] = B;
+        state.Points["C"] = C;
+
+        state.Angles["曲柄角度"] = normalizeAngle(angleDeg);
+        state.Angles["摇块角度"] = normalizeAngle(radiansToDegrees(rockerAngle));
+        state.Angles["AC距离"] = acDistance;
+
+        // 计算姿态
+        state.Poses["OA"] = PoseCalculator::CalculatePoseAndQuaternion(O, A);
+        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
+        state.Poses["BC"] = PoseCalculator::CalculatePoseAndQuaternion(B, C);
+
+        // TCP姿态（滑块B点）
+        Pose7 tcpPose;
+        tcpPose.tx = B.X;
+        tcpPose.ty = B.Y;
+        tcpPose.tz = 0.0;
+        tcpPose.qw = 1.0;
+        state.Poses["TCP"] = tcpPose;
+
+        // 检查极限位置
+        checkLimitPositions(state, A, B, C, OA, AB, OC);
+
+        return state;
+    }
+    catch (const exception &ex)
+    {
+        state.ErrorMessage = string("计算失败: ") + ex.what();
+        return state;
+    }
+}
+
+Vector2D CrankRockingBlockMechanism_Forward::calculatePointB(
+    const Vector2D &A, const Vector2D &C, double AB)
+{
+    // 直线方程: B在直线AC上，且|AB| = 给定长度
+    Vector2D AC = C - A;
+    double AC_length = AC.Length();
+
+    if (AC_length < 1e-10)
+        return Vector2D::Zero();
+
+    // 单位向量
+    Vector2D AC_unit = AC * (1.0 / AC_length);
+
+    // 有两个可能的解
+    Vector2D B1 = A + AC_unit * AB; // 沿AC方向
+    Vector2D B2 = A - AC_unit * AB; // 反向
+
+    // 选择合理的解（基于连续性）
+    if (trajectoryB.empty())
+    {
+        // 初始状态选择B1（通常滑块在C点附近）
+        return B1;
+    }
+    else
+    {
+        // 选择与上一帧更接近的解
+        Vector2D lastB = trajectoryB.back();
+        double dist1 = Vector2D::Distance(B1, lastB);
+        double dist2 = Vector2D::Distance(B2, lastB);
+        return (dist1 < dist2) ? B1 : B2;
+    }
+}
+
+void CrankRockingBlockMechanism_Forward::checkLimitPositions(
+    MechanismState &state, const Vector2D &A, const Vector2D &B, const Vector2D &C,
+    double OA, double AB, double OC)
+{
+    // 检查死点位置（OA与AC共线）
+    Vector2D OA_vec = A;
+    Vector2D AC_vec = C - A;
+
+    // 计算夹角
+    double dotProduct = OA_vec.Dot(AC_vec);
+    double productLength = OA_vec.Length() * AC_vec.Length();
+
+    if (productLength > 1e-10)
+    {
+        double cosAngle = dotProduct / productLength;
+        double angle = acos(fabs(cosAngle)) * 180.0 / M_PI;
+
+        if (angle < 5.0 || angle > 175.0)
+        {
+            if (!state.WarningMessage.empty())
+                state.WarningMessage += "\n";
+            state.WarningMessage += "警告：接近死点位置";
+        }
+    }
+
+    // 检查机构装配条件
+    double acDistance = Vector2D::Distance(A, C);
+    if (fabs(acDistance - AB) < 0.1)
+    {
+        if (!state.WarningMessage.empty())
+            state.WarningMessage += "\n";
+        state.WarningMessage += "警告：接近极限位置 (AC ≈ AB)";
+    }
+
+    // 检查曲柄角度范围
+    auto range = getValidCrankRange();
+    if (range.first > 0 || range.second < 360)
+    {
+        double currentAngle = normalizeAngle(parameters.InputValue);
+        if (currentAngle < range.first + 10 || currentAngle > range.second - 10)
+        {
+            char buffer[128];
+            snprintf(buffer, sizeof(buffer),
+                     "警告：接近曲柄角度极限 [%.1f°, %.1f°]",
+                     range.first, range.second);
+
+            if (!state.WarningMessage.empty())
+                state.WarningMessage += "\n";
+            state.WarningMessage += buffer;
+        }
+    }
+}
+
+double CrankRockingBlockMechanism_Forward::normalizeAngle(double angle)
+{
+    angle = fmod(angle, 360.0);
+    if (angle < 0)
+        angle += 360.0;
+    return angle;
+}
+
+pair<double, double> CrankRockingBlockMechanism_Forward::getValidCrankRange() const
+{
+    double OA = parameters.CrankLength2;
+    double AB = parameters.ConnectingRodLength2;
+    double OC = parameters.SliderOffset2;
+
+    // 检查机构能否整周旋转
+    bool canFullRotation = checkFullRotationCondition(OA, AB, OC);
+
+    if (canFullRotation)
+    {
+        return make_pair(0.0, 360.0);
+    }
+    else
+    {
+        // 计算曲柄的摆动范围
+        double cosTheta = (OA * OA + OC * OC - AB * AB) / (2 * OA * OC);
+        cosTheta = max(min(cosTheta, 1.0), -1.0);
+        double theta = acos(cosTheta) * 180.0 / M_PI;
+
+        double minAngle = 180.0 - theta;
+        double maxAngle = 180.0 + theta;
+
+        // 确保角度在0-360范围内
+        minAngle = fmod(minAngle + 360.0, 360.0);
+        maxAngle = fmod(maxAngle + 360.0, 360.0);
+
+        return make_pair(minAngle, maxAngle);
+    }
+}
+
+bool CrankRockingBlockMechanism_Forward::checkFullRotationCondition(
+    double OA, double AB, double OC) const
+{
+    // 曲柄摇块机构整周旋转条件：
+    // 1. 曲柄是最短杆
+    // 2. 满足三角形不等式
+
+    double minLength = min(min(OA, AB), OC);
+
+    // 曲柄不是最短杆
+    if (fabs(minLength - OA) > 1e-10)
+        return false;
+
+    // 检查几何约束
+    if (OA + AB < OC || fabs(OA - AB) > OC)
+        return false;
+
+    return true;
+}
+
+pair<double, double> CrankRockingBlockMechanism_Forward::getInputRange()
+{
+    return make_pair(0.0, 360.0);
+}
+
+vector<Vector2D> CrankRockingBlockMechanism_Forward::getTrajectoryPoints() const
+{
+    return trajectoryB;
+}
+
+vector<Vector2D> CrankRockingBlockMechanism_Forward::getCrankCirclePoints() const
+{
+    return crankCirclePoints;
+}
+
+vector<Vector2D> CrankRockingBlockMechanism_Forward::getRockerTrajectory() const
+{
+    return rockerTrajectory;
+}
+
+void CrankRockingBlockMechanism_Forward::clearTrajectory()
+{
+    trajectoryB.clear();
+    crankCirclePoints.clear();
+    rockerTrajectory.clear();
+    frameCount = 0;
+    timer.reset();
+}
+
+ValidationResult CrankRockingBlockMechanism_Forward::validateParameters()
+{
+    ValidationResult result;
+
+    if (parameters.CrankLength2 <= 0.0)
+        result.Errors.push_back("曲柄长度OA必须大于0");
+
+    if (parameters.ConnectingRodLength2 <= 0.0)
+        result.Errors.push_back("连杆长度AB必须大于0");
+
+    if (parameters.SliderOffset2 <= 0.0)
+        result.Errors.push_back("摇块中心距OC必须大于0");
+
+    double OA = parameters.CrankLength2;
+    double AB = parameters.ConnectingRodLength2;
+    double OC = parameters.SliderOffset2;
+
+    // 检查机构装配条件
+    if (OA + AB < OC)
+    {
+        result.Warnings.push_back("机构可能无法装配或不满足整周旋转条件");
+    }
+
+    // 检查曲柄是否为最短杆
+    double minLength = min(min(OA, AB), OC);
+    if (fabs(minLength - OA) > 1e-10)
+    {
+        result.Warnings.push_back("曲柄不是最短杆，可能无法整周旋转");
+    }
+
+    return result;
+}
+
+string CrankRockingBlockMechanism_Forward::getStatusText()
+{
+    double runTime = timer.elapsedSeconds();
+    double fps = (runTime > 0.0) ? frameCount / runTime : 0.0;
+
+    auto range = getValidCrankRange();
+    string rotationInfo;
+
+    if (range.first == 0.0 && range.second == 360.0)
+    {
+        rotationInfo = "曲柄可整周旋转";
+    }
+    else
+    {
+        char buffer[64];
+        snprintf(buffer, sizeof(buffer),
+                 "曲柄摆动范围: [%.1f°, %.1f°]",
+                 range.first, range.second);
+        rotationInfo = buffer;
+    }
+
+    char statusBuffer[512];
+    snprintf(statusBuffer, sizeof(statusBuffer),
+             "%s\n"
+             "曲柄OA: %.3f m\n"
+             "连杆AB: %.3f m\n"
+             "摇块中心OC: %.3f m\n"
+             "曲柄角度: %.1f°\n"
+             "%s\n"
+             "角速度: %.2f rad/s\n"
+             "运行时间: %.1f s\n"
+             "帧率: %.1f FPS\n"
+             "轨迹点数: %zu/500",
+             name.c_str(),
+             parameters.CrankLength2,
+             parameters.ConnectingRodLength2,
+             parameters.SliderOffset2,
+             parameters.InputValue,
+             rotationInfo.c_str(),
+             parameters.AngularVelocity,
+             runTime,
+             fps,
+             trajectoryB.size());
+
+    return string(statusBuffer);
+}
+
+double CrankRockingBlockMechanism_Forward::getThetaADelta() const
+{
+    return thetaADelta;
+}
+
+double CrankRockingBlockMechanism_Forward::getThetaAValueFactor() const
+{
+    return thetaAValueFactor;
+}
+
+int CrankRockingBlockMechanism_Forward::getThetaAFlip() const
+{
+    return thetaAFlip;
+}
+
+double CrankRockingBlockMechanism_Forward::getOA() const
+{
+    return parameters.CrankLength2;
+}
+
+double CrankRockingBlockMechanism_Forward::getAB() const
+{
+    return parameters.ConnectingRodLength2;
+}
+
+double CrankRockingBlockMechanism_Forward::getOC() const
+{
+    return parameters.SliderOffset2;
+}
+
+double CrankRockingBlockMechanism_Forward::degreesToRadians(double degrees) const
+{
+    return degrees * M_PI / 180.0;
+}
+
+double CrankRockingBlockMechanism_Forward::radiansToDegrees(double radians) const
+{
+    return radians * 180.0 / M_PI;
+}
+
+string CrankRockingBlockMechanism_Forward::formatDouble(double value, int precision) const
+{
+    char buffer[32];
+    snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
+    return string(buffer);
+}

src/FourBarMechanism/CrankRockingBlockMechanism_Inverse.cpp

@@ -0,0 +1,261 @@
+#include "CrankRockingBlockMechanism_Inverse.h"
+#include <cmath>
+#include <algorithm>
+#include <stdexcept>
+#include <cstdio>
+
+using namespace std;
+
+// ==================== CrankRockingBlockMechanism_Inverse 实现 ====================
+
+CrankRockingBlockMechanism_Inverse::CrankRockingBlockMechanism_Inverse()
+    : name("曲柄摇块机构 (逆解)"),
+      type(MechanismType::CrankRockingBlock)
+{
+    // 设置默认参数
+    parameters.CrankLength2 = 1.0;         // OA - 曲柄长度
+    parameters.ConnectingRodLength2 = 3.0; // AB - 连杆长度
+    parameters.SliderOffset2 = 3.0;        // OC - 摇块中心距
+    parameters.InputValue = 2.5;           // AC距离
+    parameters.AngularVelocity = 1.0;      // 角速度
+}
+
+string CrankRockingBlockMechanism_Inverse::getName() const
+{
+    return name;
+}
+
+MechanismType CrankRockingBlockMechanism_Inverse::getType() const
+{
+    return type;
+}
+
+MechanismParameters CrankRockingBlockMechanism_Inverse::getParameters() const
+{
+    return parameters;
+}
+
+void CrankRockingBlockMechanism_Inverse::setParameters(const MechanismParameters &params)
+{
+    parameters = params;
+}
+
+void CrankRockingBlockMechanism_Inverse::setLink(double OA, double AB, double OC)
+{
+    parameters.CrankLength2 = OA;
+    parameters.ConnectingRodLength2 = AB;
+    parameters.SliderOffset2 = OC;
+}
+
+void CrankRockingBlockMechanism_Inverse::setOA(double length)
+{
+    parameters.CrankLength2 = length;
+}
+
+void CrankRockingBlockMechanism_Inverse::setAB(double length)
+{
+    parameters.ConnectingRodLength2 = length;
+}
+
+void CrankRockingBlockMechanism_Inverse::setOC(double distance)
+{
+    parameters.SliderOffset2 = distance;
+}
+
+MechanismState CrankRockingBlockMechanism_Inverse::calculate(double acDistance)
+{
+    parameters.InputValue = acDistance;
+
+    double OA = parameters.CrankLength2;         // 曲柄长度
+    double AB = parameters.ConnectingRodLength2; // 连杆长度
+    double OC = parameters.SliderOffset2;        // 摇块中心距
+    double AC = acDistance;
+
+    MechanismState state;
+    state.InputValue = acDistance;
+
+    try
+    {
+        // 验证AC距离范围
+        auto acRange = getACRange();
+
+        if (AC < acRange.first || AC > acRange.second)
+        {
+            char buffer[128];
+            snprintf(buffer, sizeof(buffer),
+                     "AC距离超出有效范围 [%.3f, %.3f]",
+                     acRange.first, acRange.second);
+            state.ErrorMessage = buffer;
+            return state;
+        }
+
+        // 计算角度θ1 (∠AOC)
+        double cosTheta1 = (OA * OA + OC * OC - AC * AC) / (2 * OA * OC);
+        cosTheta1 = max(min(cosTheta1, 1.0), -1.0);
+        double theta1 = acos(cosTheta1);
+
+        // 计算角度φ (∠BAC)
+        double cosPhi = (AC * AC + OC * OC - OA * OA) / (2 * AC * OC);
+        cosPhi = max(min(cosPhi, 1.0), -1.0);
+        double phi = acos(cosPhi);
+
+        // 计算坐标点
+        Vector2D O(0.0, 0.0); // 固定点
+        Vector2D C(OC, 0.0);  // 摇块中心
+        Vector2D A(           // 曲柄端点
+            OA * cos(theta1),
+            OA * sin(theta1));
+
+        // B点在AC延长线上 (沿AC方向距离A点为AB长度)
+        Vector2D AC_vec = C - A;
+        double AC_length = AC_vec.Length();
+
+        if (AC_length < 1e-10)
+        {
+            state.ErrorMessage = "A点和C点重合";
+            return state;
+        }
+
+        Vector2D AC_dir = AC_vec * (1.0 / AC_length);
+        Vector2D B = A + AC_dir * AB;
+
+        // 记录轨迹（限制点数）
+        trajectoryPoints.push_back(B);
+        const size_t MAX_TRAJECTORY_POINTS = 100;
+        if (trajectoryPoints.size() > MAX_TRAJECTORY_POINTS)
+        {
+            trajectoryPoints.erase(trajectoryPoints.begin());
+        }
+
+        // 设置状态
+        state.Points["O"] = O;
+        state.Points["A"] = A;
+        state.Points["B"] = B;
+        state.Points["C"] = C;
+
+        state.Angles["θ1"] = theta1 * 180.0 / M_PI;
+        state.Angles["φ"] = phi * 180.0 / M_PI;
+        state.Angles["AC距离"] = AC;
+
+        // 计算姿态
+        state.Poses["OA"] = PoseCalculator::CalculatePoseAndQuaternion(O, A);
+        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
+        state.Poses["BC"] = PoseCalculator::CalculatePoseAndQuaternion(B, C);
+
+        // TCP姿态（滑块B点）
+        Pose7 tcpPose;
+        tcpPose.tx = B.X;
+        tcpPose.ty = B.Y;
+        tcpPose.tz = 0.0;
+        tcpPose.qw = 1.0;
+        state.Poses["TCP"] = tcpPose;
+
+        return state;
+    }
+    catch (const exception &ex)
+    {
+        state.ErrorMessage = string("计算失败: ") + ex.what();
+        return state;
+    }
+}
+
+pair<double, double> CrankRockingBlockMechanism_Inverse::getACRange() const
+{
+    double OA = parameters.CrankLength2;
+    double OC = parameters.SliderOffset2;
+    double AB = parameters.ConnectingRodLength2;
+
+    // 理论最小和最大AC距离
+    double minAC = max(fabs(OA - OC), 0.1);
+    double maxAC = OA + OC - 0.1;
+
+    // 考虑连杆长度约束
+    minAC = max(minAC, fabs(AB - OA));
+    maxAC = min(maxAC, AB + OA);
+
+    return make_pair(minAC, maxAC);
+}
+
+pair<double, double> CrankRockingBlockMechanism_Inverse::getInputRange()
+{
+    return getACRange();
+}
+
+vector<Vector2D> CrankRockingBlockMechanism_Inverse::getTrajectoryPoints() const
+{
+    return trajectoryPoints;
+}
+
+void CrankRockingBlockMechanism_Inverse::clearTrajectory()
+{
+    trajectoryPoints.clear();
+}
+
+ValidationResult CrankRockingBlockMechanism_Inverse::validateParameters()
+{
+    ValidationResult result;
+
+    if (parameters.CrankLength2 <= 0.0)
+        result.Errors.push_back("曲柄长度OA必须大于0");
+
+    if (parameters.ConnectingRodLength2 <= 0.0)
+        result.Errors.push_back("连杆长度AB必须大于0");
+
+    if (parameters.SliderOffset2 <= 0.0)
+        result.Errors.push_back("摇块中心距OC必须大于0");
+
+    // 检查三角形不等式
+    double OA = parameters.CrankLength2;
+    double AB = parameters.ConnectingRodLength2;
+    double OC = parameters.SliderOffset2;
+
+    if (OA + AB < OC)
+    {
+        result.Errors.push_back("曲柄+连杆长度必须大于摇块中心距");
+    }
+
+    if (OA + OC < AB)
+    {
+        result.Errors.push_back("曲柄+中心距必须大于连杆长度");
+    }
+
+    if (AB + OC < OA)
+    {
+        result.Errors.push_back("连杆+中心距必须大于曲柄长度");
+    }
+
+    // 检查运动范围
+    auto range = getACRange();
+    if (range.first >= range.second)
+    {
+        result.Errors.push_back("机构参数无效，无法形成有效运动范围");
+    }
+
+    return result;
+}
+
+string CrankRockingBlockMechanism_Inverse::getStatusText()
+{
+    auto range = getACRange();
+
+    char buffer[256];
+    snprintf(buffer, sizeof(buffer),
+             "%s\n"
+             "曲柄OA: %.3f m\n"
+             "连杆AB: %.3f m\n"
+             "摇块中心OC: %.3f m\n"
+             "当前AC距离: %.3f m\n"
+             "AC有效范围: [%.3f, %.3f] m\n"
+             "角速度: %.2f rad/s\n"
+             "轨迹点数: %zu",
+             name.c_str(),
+             parameters.CrankLength2,
+             parameters.ConnectingRodLength2,
+             parameters.SliderOffset2,
+             parameters.InputValue,
+             range.first, range.second,
+             parameters.AngularVelocity,
+             trajectoryPoints.size());
+
+    return string(buffer);
+}

src/FourBarMechanism/CrankSliderMechanism.cpp

@@ -0,0 +1,474 @@
+// mechanism_simulation.cpp
+#include "CrankSliderMechanism.h"
+#include <stdexcept>
+#include <algorithm>
+#include <cstdio>
+
+// ==================== MechanismParameters 实现 ====================
+
+MechanismParameters::MechanismParameters() : CrankLength2(1.0),
+                                             ConnectingRodLength2(3.0),
+                                             SliderOffset2(0.5),
+                                             L1(1.0),
+                                             L2(3.0),
+                                             L3(2.5),
+                                             L4(3.5),
+                                             InputValue(0.0),
+                                             AngularVelocity(2.0) {}
+
+// ==================== MechanismState 实现 ====================
+
+MechanismState::MechanismState() : InputValue(0.0) {}
+
+bool MechanismState::hasError() const
+{
+    return !ErrorMessage.empty();
+}
+
+bool MechanismState::hasWarning() const
+{
+    return !WarningMessage.empty();
+}
+
+// ==================== ValidationResult 实现 ====================
+
+bool ValidationResult::isValid() const
+{
+    return Errors.empty();
+}
+
+std::string ValidationResult::getCombinedMessage() const
+{
+    std::string message;
+
+    if (!Errors.empty())
+    {
+        message += "错误:\n";
+        for (const auto &error : Errors)
+        {
+            message += "  • " + error + "\n";
+        }
+    }
+
+    if (!Warnings.empty())
+    {
+        if (!message.empty())
+            message += "\n";
+        message += "警告:\n";
+        for (const auto &warning : Warnings)
+        {
+            message += "  • " + warning + "\n";
+        }
+    }
+
+    return message;
+}
+
+// ==================== SimpleTimer 实现 ====================
+
+SimpleTimer::SimpleTimer()
+{
+    reset();
+}
+
+void SimpleTimer::reset()
+{
+    start = std::chrono::steady_clock::now();
+}
+
+double SimpleTimer::elapsedSeconds() const
+{
+    auto now = std::chrono::steady_clock::now();
+    std::chrono::duration<double> elapsed = now - start;
+    return elapsed.count();
+}
+
+// ==================== CrankSliderMechanism 私有辅助方法 ====================
+
+double CrankSliderMechanism::degreesToRadians(double degrees) const
+{
+    return degrees * M_PI / 180.0;
+}
+
+double CrankSliderMechanism::radiansToDegrees(double radians) const
+{
+    return radians * 180.0 / M_PI;
+}
+
+std::string CrankSliderMechanism::formatDouble(double value, int precision) const
+{
+    char buffer[32];
+    snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
+    return std::string(buffer);
+}
+
+// ==================== CrankSliderMechanism 公有方法实现 ====================
+
+CrankSliderMechanism::CrankSliderMechanism() : name("曲柄滑块机构 (正解)"),
+                                               type(MechanismType::CrankSlider),
+                                               thetaADelta(0.0),
+                                               thetaAValueFactor(1.0),
+                                               thetaAFlip(1),
+                                               frameCount(0)
+{
+
+    parameters.CrankLength2 = 0.5;
+    parameters.ConnectingRodLength2 = 2.0;
+    parameters.SliderOffset2 = 0.0;
+    parameters.InputValue = 0.0;
+    parameters.AngularVelocity = 2.0;
+}
+
+std::string CrankSliderMechanism::getName() const
+{
+    return name;
+}
+
+MechanismType CrankSliderMechanism::getType() const
+{
+    return type;
+}
+
+MechanismParameters CrankSliderMechanism::getParameters() const
+{
+    return parameters;
+}
+
+void CrankSliderMechanism::setName(const std::string &newName)
+{
+    name = newName;
+}
+
+void CrankSliderMechanism::setParameters(const MechanismParameters &params)
+{
+    parameters = params;
+}
+
+void CrankSliderMechanism::setLink(double crankLength, double rodLength, double sliderOffset)
+{
+    parameters.CrankLength2 = crankLength;
+    parameters.ConnectingRodLength2 = rodLength;
+    parameters.SliderOffset2 = sliderOffset;
+}
+
+// 配置方法
+void CrankSliderMechanism::setThetaADelta(double delta)
+{
+    thetaADelta = delta;
+}
+
+void CrankSliderMechanism::setThetaAValueFactor(double factor)
+{
+    thetaAValueFactor = factor;
+}
+
+void CrankSliderMechanism::setThetaAFlip(int flip)
+{
+    thetaAFlip = flip;
+}
+
+void CrankSliderMechanism::setL_AB(double length)
+{
+    parameters.CrankLength2 = length;
+}
+
+void CrankSliderMechanism::setL_BS(double length)
+{
+    parameters.ConnectingRodLength2 = length;
+}
+
+void CrankSliderMechanism::setS_OFS(double offset)
+{
+    parameters.SliderOffset2 = offset;
+}
+
+void CrankSliderMechanism::setCodeBody(const std::string &code)
+{
+    codeBody = code;
+}
+
+void CrankSliderMechanism::setL1ABModelName(const std::string &modelName)
+{
+    l1ABModelName = modelName;
+}
+
+void CrankSliderMechanism::setL2BSModelName(const std::string &modelName)
+{
+    l2BSModelName = modelName;
+}
+
+void CrankSliderMechanism::setL3SModelName(const std::string &modelName)
+{
+    l3SModelName = modelName;
+}
+
+MechanismState CrankSliderMechanism::calculate(double _angleDeg)
+{
+    double angleDeg = thetaADelta + _angleDeg * thetaAValueFactor * thetaAFlip;
+    parameters.InputValue = angleDeg;
+
+    double angle = degreesToRadians(angleDeg);
+    double crankLength = parameters.CrankLength2;
+    double rodLength = parameters.ConnectingRodLength2;
+    double sliderOffset = parameters.SliderOffset2;
+
+    Vector2D A(0.0, 0.0);
+    MechanismState state;
+    state.InputValue = angleDeg;
+
+    try
+    {
+        // 验证Grashof条件
+        bool grashofCondition = crankLength + rodLength > std::abs(sliderOffset);
+        if (!grashofCondition)
+        {
+            state.ErrorMessage = "错误：不满足Grashof条件\n(曲柄+连杆长度必须大于滑块偏移量的绝对值)";
+            return state;
+        }
+
+        // 检查滑块偏移量
+        double maxOffset = rodLength - crankLength;
+        if (std::abs(sliderOffset) > maxOffset)
+        {
+            state.ErrorMessage = "错误：滑块偏移量过大\n最大允许值: ±" + formatDouble(maxOffset) + "m";
+            return state;
+        }
+        else if (std::abs(sliderOffset) > 0.8 * maxOffset)
+        {
+            state.WarningMessage = "警告：滑块偏移量接近极限值\n建议值: ±" + formatDouble(0.8 * maxOffset) + "m以内";
+        }
+
+        // 计算B点位置
+        Vector2D B = A + Vector2D(
+                             crankLength * std::cos(angle),
+                             crankLength * std::sin(angle));
+
+        // 计算滑块位置（S点）
+        double Bx = B.X;
+        double By = B.Y;
+        double y_s = sliderOffset;
+        double L = rodLength;
+
+        // 解二次方程：x_s^2 - 2*Bx*x_s + Bx^2 + (y_s - By)^2 - L^2 = 0
+        double a = 1.0;
+        double b = -2.0 * Bx;
+        double c = Bx * Bx + (y_s - By) * (y_s - By) - L * L;
+
+        double discriminant = b * b - 4.0 * a * c;
+
+        if (discriminant < 0.0)
+        {
+            state.ErrorMessage = "错误：无法找到滑块位置，机构可能无法装配";
+            return state;
+        }
+
+        double sqrtDiscriminant = std::sqrt(discriminant);
+        double x1 = (-b + sqrtDiscriminant) / (2.0 * a);
+        double x2 = (-b - sqrtDiscriminant) / (2.0 * a);
+
+        // 选择正确的解（基于连续性）
+        // 修正：改进解的选择逻辑
+        double x_s;
+        if (!sliderTrajectory.empty())
+        {
+            // 使用连续性原则：选择与上一个位置最接近的解
+            double lastX = sliderTrajectory.back().X;
+            x_s = (std::abs(x1 - lastX) < std::abs(x2 - lastX)) ? x1 : x2;
+        }
+        else
+        {
+            x_s = (x1 > x2) ? x1 : x2; // 选择较大的解（通常对应正向运动）
+        }
+
+        Vector2D S(x_s, y_s);
+
+        // 记录轨迹点
+        // 记录轨迹点
+        trajectoryB.push_back(B);
+        sliderTrajectory.push_back(S);
+        crankCirclePoints.push_back(B);
+
+        // 限制轨迹点数量
+        const size_t MAX_TRAJECTORY_POINTS = 1000;
+        if (trajectoryB.size() > MAX_TRAJECTORY_POINTS)
+        {
+            trajectoryB.erase(trajectoryB.begin());
+            sliderTrajectory.erase(sliderTrajectory.begin());
+            crankCirclePoints.erase(crankCirclePoints.begin());
+        }
+
+        // 更新帧计数
+        frameCount++;
+
+        // 设置状态
+        state.Points["A"] = A;
+        state.Points["B"] = B;
+        state.Points["S"] = S;
+
+        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
+        state.Poses["BS"] = PoseCalculator::CalculatePoseAndQuaternion(B, S);
+
+        // TCP姿态
+        Pose7 tcpPose;
+        tcpPose.tx = S.X;
+        tcpPose.ty = S.Y;
+        tcpPose.tz = 0.0;
+        tcpPose.qw = 1.0;
+        state.Poses["TCP"] = tcpPose;
+
+        // 角度信息
+        state.Angles["曲柄角度"] = std::fmod(angleDeg, 360.0);
+        state.Angles["滑块位置X"] = x_s;
+
+        return state;
+    }
+    catch (const std::exception &ex)
+    {
+        state.ErrorMessage = std::string("计算失败: ") + ex.what();
+        return state;
+    }
+}
+
+ValidationResult CrankSliderMechanism::validateParameters()
+{
+    ValidationResult result;
+
+    if (parameters.CrankLength2 <= 0.0)
+    {
+        result.Errors.push_back("曲柄长度必须大于0");
+    }
+
+    if (parameters.ConnectingRodLength2 <= 0.0)
+    {
+        result.Errors.push_back("连杆长度必须大于0");
+    }
+
+    if (parameters.CrankLength2 + parameters.ConnectingRodLength2 <= std::abs(parameters.SliderOffset2))
+    {
+        result.Errors.push_back("不满足Grashof条件: 曲柄+连杆长度必须大于滑块偏移量的绝对值");
+    }
+
+    double maxOffset = parameters.ConnectingRodLength2 - parameters.CrankLength2;
+    if (std::abs(parameters.SliderOffset2) > maxOffset)
+    {
+        result.Errors.push_back("滑块偏移量过大，最大允许值: ±" + formatDouble(maxOffset) + "m");
+    }
+    else if (std::abs(parameters.SliderOffset2) > 0.8 * maxOffset)
+    {
+        result.Warnings.push_back("滑块偏移量接近极限值，建议值: ±" + formatDouble(0.8 * maxOffset) + "m以内");
+    }
+
+    return result;
+}
+
+std::pair<double, double> CrankSliderMechanism::getInputRange()
+{
+    return std::make_pair(0.0, 360.0);
+}
+
+std::vector<Vector2D> CrankSliderMechanism::getTrajectoryPoints()
+{
+    return trajectoryB;
+}
+
+std::vector<Vector2D> CrankSliderMechanism::getSliderTrajectory() const
+{
+    return sliderTrajectory;
+}
+
+std::vector<Vector2D> CrankSliderMechanism::getCrankCirclePoints() const
+{
+    return crankCirclePoints;
+}
+
+void CrankSliderMechanism::clearTrajectory()
+{
+    trajectoryB.clear();
+    sliderTrajectory.clear();
+    crankCirclePoints.clear();
+    frameCount = 0;
+    timer.reset();
+}
+
+std::string CrankSliderMechanism::getStatusText()
+{
+    double runTime = timer.elapsedSeconds();
+    double fps = (runTime > 0.0) ? frameCount / runTime : 0.0;
+
+    return name + "\n" +
+           "曲柄长度: " + formatDouble(parameters.CrankLength2) + "m\n" +
+           "连杆长度: " + formatDouble(parameters.ConnectingRodLength2) + "m\n" +
+           "滑块偏移: " + formatDouble(parameters.SliderOffset2) + "m\n" +
+           "角速度: " + formatDouble(parameters.AngularVelocity, 1) + "rad/s\n" +
+           "运行时间: " + formatDouble(runTime, 1) + "s\n" +
+           "帧率: " + formatDouble(fps, 1) + " FPS\n" +
+           "轨迹点数: " + std::to_string(trajectoryB.size()) + "/1000";
+}
+
+// 获取配置参数
+double CrankSliderMechanism::getThetaADelta() const
+{
+    return thetaADelta;
+}
+
+double CrankSliderMechanism::getThetaAValueFactor() const
+{
+    return thetaAValueFactor;
+}
+
+int CrankSliderMechanism::getThetaAFlip() const
+{
+    return thetaAFlip;
+}
+
+double CrankSliderMechanism::getL_AB() const
+{
+    return parameters.CrankLength2;
+}
+
+double CrankSliderMechanism::getL_BS() const
+{
+    return parameters.ConnectingRodLength2;
+}
+
+double CrankSliderMechanism::getS_OFS() const
+{
+    return parameters.SliderOffset2;
+}
+
+std::string CrankSliderMechanism::getCodeBody() const
+{
+    return codeBody;
+}
+
+std::string CrankSliderMechanism::getL1ABModelName() const
+{
+    return l1ABModelName;
+}
+
+std::string CrankSliderMechanism::getL2BSModelName() const
+{
+    return l2BSModelName;
+}
+
+std::string CrankSliderMechanism::getL3SModelName() const
+{
+    return l3SModelName;
+}
+
+// ==================== 工厂函数实现 ====================
+
+CrankSliderMechanism *createCrankSliderMechanism()
+{
+    return new CrankSliderMechanism();
+}
+
+void deleteCrankSliderMechanism(CrankSliderMechanism *mechanism)
+{
+    delete mechanism;
+}
+
+CrankSliderMechanismPtr createCrankSliderMechanismSmart()
+{
+    return std::make_shared<CrankSliderMechanism>();
+}

src/FourBarMechanism/FourBarMechanism.cpp

@@ -0,0 +1,329 @@
+// FourBarMechanism.cpp
+#include "FourBarMechanism.h"
+#include <stdexcept>
+#include <algorithm>
+#include <cstdio>
+#include <sstream>
+
+// ==================== FourBarMechanism 实现 ====================
+
+FourBarMechanism::FourBarMechanism() : name("四杆机构 (正解)"),
+                                       frameCount(0),
+                                       startTime(std::chrono::steady_clock::now())
+{
+    parameters.L1 = 1.0;
+    parameters.L2 = 3.0;
+    parameters.L3 = 2.5;
+    parameters.L4 = 3.5;
+    parameters.InputValue = 0.0;
+    parameters.AngularVelocity = 2.0;
+}
+
+double FourBarMechanism::degreesToRadians(double degrees) const
+{
+    return degrees * M_PI / 180.0;
+}
+
+double FourBarMechanism::radiansToDegrees(double radians) const
+{
+    return radians * 180.0 / M_PI;
+}
+
+std::string FourBarMechanism::formatDouble(double value, int precision) const
+{
+    char buffer[32];
+    snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
+    return std::string(buffer);
+}
+
+std::string FourBarMechanism::getName() const
+{
+    return name;
+}
+
+MechanismType FourBarMechanism::getType() const
+{
+    return MechanismType::FourBar;
+}
+
+MechanismParameters FourBarMechanism::getParameters() const
+{
+    return parameters;
+}
+
+void FourBarMechanism::setName(const std::string &newName)
+{
+    name = newName;
+}
+
+void FourBarMechanism::setParameters(const MechanismParameters &params)
+{
+    parameters = params;
+}
+
+void FourBarMechanism::setLink(double l1, double l2, double l3, double l4)
+{
+    parameters.L1 = l1;
+    parameters.L2 = l2;
+    parameters.L3 = l3;
+    parameters.L4 = l4;
+}
+
+bool FourBarMechanism::calculatePointC(const Vector2D &A, const Vector2D &D,
+                                       const Vector2D &B, double l2, double l3,
+                                       Vector2D &C) const
+{
+    C = Vector2D::Zero();
+
+    Vector2D BD = D - B;
+    double d = BD.Length();
+
+    // 检查三角形不等式
+    if (d > l2 + l3 || d < std::abs(l2 - l3))
+        return false;
+
+    // 计算C点位置（几何法）
+    double a = (l2 * l2 - l3 * l3 + d * d) / (2.0 * d);
+    double h_sq = l2 * l2 - a * a;
+
+    if (h_sq < 0)
+        return false;
+
+    double h = std::sqrt(h_sq);
+
+    // 单位向量
+    Vector2D u = BD * (1.0 / d); // u = BD / d
+    Vector2D v(-u.Y, u.X);       // 垂直向量
+
+    // 两个可能的C点
+    Vector2D C1 = B + u * a + v * h;
+    Vector2D C2 = B + u * a - v * h;
+
+    // 选择正确的解（基于连续性）
+    if (trajectoryC.empty())
+    {
+        // 首次计算，根据输入角度选择
+        C = (parameters.InputValue <= 180.0) ? C1 : C2;
+    }
+    else
+    {
+        // 选择与上一个位置最接近的解
+        Vector2D lastC = trajectoryC.back();
+        double dist1 = Vector2D::Distance(C1, lastC);
+        double dist2 = Vector2D::Distance(C2, lastC);
+        C = (dist1 < dist2) ? C1 : C2;
+    }
+
+    return true;
+}
+
+bool FourBarMechanism::checkGrashofCondition() const
+{
+    double lengths[4] = {parameters.L1, parameters.L2, parameters.L3, parameters.L4};
+    std::sort(lengths, lengths + 4);
+
+    double s = lengths[0];
+    double l = lengths[3];
+    double p = lengths[1];
+    double q = lengths[2];
+
+    // Grashof条件：最短杆+最长杆 ≤ 其他两杆之和
+    return (s + l) <= (p + q);
+}
+
+bool FourBarMechanism::isShortestLinkCrank() const
+{
+    double lengths[4] = {parameters.L1, parameters.L2, parameters.L3, parameters.L4};
+    double minLength = *std::min_element(lengths, lengths + 4);
+
+    // 判断最短杆是否为L1（曲柄）
+    return std::abs(minLength - parameters.L1) < 1e-10;
+}
+
+MechanismState FourBarMechanism::calculate(double angleDeg)
+{
+    parameters.InputValue = angleDeg;
+    double angle = degreesToRadians(angleDeg);
+
+    double l1 = parameters.L1;
+    double l2 = parameters.L2;
+    double l3 = parameters.L3;
+    double l4 = parameters.L4;
+
+    Vector2D A(0.0, 0.0);
+    Vector2D D(l4, 0.0);
+
+    MechanismState state;
+    state.InputValue = angleDeg;
+
+    try
+    {
+        // 验证Grashof条件
+        if (!checkGrashofCondition())
+        {
+            state.ErrorMessage = "不满足四杆机构装配条件: s + l > p + q";
+            return state;
+        }
+
+        // 计算B点位置（曲柄末端）
+        Vector2D B = A + Vector2D(
+                             l1 * std::cos(angle),
+                             l1 * std::sin(angle));
+
+        // 计算C点位置（连杆末端）
+        Vector2D C;
+        if (!calculatePointC(A, D, B, l2, l3, C))
+        {
+            state.ErrorMessage = "无法找到连杆位置，机构可能无法装配";
+            return state;
+        }
+
+        // 记录轨迹点
+        trajectoryB.push_back(B);
+        trajectoryC.push_back(C);
+        crankCirclePoints.push_back(B);
+
+        // 限制轨迹点数量
+        const size_t MAX_TRAJECTORY_POINTS = 500;
+        if (trajectoryB.size() > MAX_TRAJECTORY_POINTS)
+        {
+            trajectoryB.erase(trajectoryB.begin());
+            trajectoryC.erase(trajectoryC.begin());
+            crankCirclePoints.erase(crankCirclePoints.begin());
+        }
+
+        // 更新帧计数
+        frameCount++;
+
+        // 计算摇杆角度
+        Vector2D CD = D - C;
+        double rockerAngle = std::atan2(CD.Y, CD.X);
+
+        // 设置状态
+        state.Points["A"] = A;
+        state.Points["B"] = B;
+        state.Points["C"] = C;
+        state.Points["D"] = D;
+
+        // 计算各杆的姿态
+        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
+        state.Poses["BC"] = PoseCalculator::CalculatePoseAndQuaternion(B, C);
+        state.Poses["CD"] = PoseCalculator::CalculatePoseAndQuaternion(C, D);
+
+        // 角度信息
+        state.Angles["曲柄角度"] = std::fmod(angleDeg, 360.0);
+        state.Angles["摇杆角度"] = radiansToDegrees(rockerAngle);
+
+        // 检查最短杆是否为曲柄
+        if (!isShortestLinkCrank())
+        {
+            state.WarningMessage = "警告: 最短杆不是曲柄，可能无法做整周旋转";
+        }
+
+        return state;
+    }
+    catch (const std::exception &ex)
+    {
+        state.ErrorMessage = std::string("计算失败: ") + ex.what();
+        return state;
+    }
+}
+
+ValidationResult FourBarMechanism::validateParameters()
+{
+    ValidationResult result;
+
+    // 检查杆件长度
+    if (parameters.L1 <= 0.0)
+        result.Errors.push_back("L1（曲柄）长度必须大于0");
+    if (parameters.L2 <= 0.0)
+        result.Errors.push_back("L2（连杆）长度必须大于0");
+    if (parameters.L3 <= 0.0)
+        result.Errors.push_back("L3（摇杆）长度必须大于0");
+    if (parameters.L4 <= 0.0)
+        result.Errors.push_back("L4（机架）长度必须大于0");
+
+    // 检查Grashof条件
+    if (!checkGrashofCondition())
+    {
+        result.Errors.push_back("不满足四杆机构装配条件: s + l > p + q");
+    }
+
+    // 检查最短杆是否为曲柄
+    if (!isShortestLinkCrank())
+    {
+        result.Warnings.push_back("警告: 最短杆不是曲柄，可能无法做整周旋转");
+    }
+
+    return result;
+}
+
+std::pair<double, double> FourBarMechanism::getInputRange()
+{
+    return std::make_pair(0.0, 360.0);
+}
+
+std::vector<Vector2D> FourBarMechanism::getTrajectoryPoints() const
+{
+    return trajectoryB;
+}
+
+std::vector<Vector2D> FourBarMechanism::getTrajectoryC() const
+{
+    return trajectoryC;
+}
+
+std::vector<Vector2D> FourBarMechanism::getCrankCirclePoints() const
+{
+    return crankCirclePoints;
+}
+
+void FourBarMechanism::clearTrajectory()
+{
+    trajectoryB.clear();
+    trajectoryC.clear();
+    crankCirclePoints.clear();
+    frameCount = 0;
+    startTime = std::chrono::steady_clock::now();
+}
+
+std::string FourBarMechanism::getStatusText()
+{
+    auto now = std::chrono::steady_clock::now();
+    std::chrono::duration<double> elapsed = now - startTime;
+    double runTime = elapsed.count();
+    double fps = (runTime > 0.0) ? frameCount / runTime : 0.0;
+
+    std::ostringstream oss;
+    oss << name << "\n"
+        << "L1=" << formatDouble(parameters.L1) << "m, L2=" << formatDouble(parameters.L2) << "m\n"
+        << "L3=" << formatDouble(parameters.L3) << "m, L4=" << formatDouble(parameters.L4) << "m\n"
+        << "角速度: " << formatDouble(parameters.AngularVelocity, 1) << "rad/s\n"
+        << "运行时间: " << formatDouble(runTime, 1) << "s\n"
+        << "帧率: " << formatDouble(fps, 1) << " FPS\n"
+        << "轨迹点数: " << trajectoryB.size() << "/500";
+
+    return oss.str();
+}
+
+double FourBarMechanism::getL1() const { return parameters.L1; }
+double FourBarMechanism::getL2() const { return parameters.L2; }
+double FourBarMechanism::getL3() const { return parameters.L3; }
+double FourBarMechanism::getL4() const { return parameters.L4; }
+
+// ==================== 工厂函数实现 ====================
+
+FourBarMechanism *createFourBarMechanism()
+{
+    return new FourBarMechanism();
+}
+
+void deleteFourBarMechanism(FourBarMechanism *mechanism)
+{
+    delete mechanism;
+}
+
+FourBarMechanismPtr createFourBarMechanismSmart()
+{
+    return std::make_shared<FourBarMechanism>();
+}

src/FourBarMechanism/SliderCrankMechanism.cpp

@@ -0,0 +1,374 @@
+// SliderCrankMechanism.cpp
+#include "SliderCrankMechanism.h"
+#include <stdexcept>
+#include <algorithm>
+#include <cstdio>
+
+// ==================== MechanismParametersExt 实现 ====================
+
+MechanismParametersExt::MechanismParametersExt() : CrankLength(1.0),
+                                                   ConnectingRodLength(3.0),
+                                                   SliderOffset(0.5),
+                                                   InputValue(0.0),
+                                                   AngularVelocity(1.0),
+                                                   SolutionMode(SolutionMode::Auto),
+                                                   CrankAngleRange1(0.0, 0.0),
+                                                   CrankAngleRange2(0.0, 0.0) {}
+
+// ==================== SliderCrankMechanism 实现 ====================
+
+SliderCrankMechanism::SliderCrankMechanism() : name("曲柄滑块机构 (逆解)")
+{
+    parameters.CrankLength = 1.0;
+    parameters.ConnectingRodLength = 3.0;
+    parameters.SliderOffset = 0.5;
+    parameters.InputValue = 0.0;
+    parameters.AngularVelocity = 1.0;
+    parameters.SolutionMode = SolutionMode::Auto;
+}
+
+// 辅助方法
+double SliderCrankMechanism::degreesToRadians(double degrees) const
+{
+    return degrees * M_PI / 180.0;
+}
+
+double SliderCrankMechanism::radiansToDegrees(double radians) const
+{
+    return radians * 180.0 / M_PI;
+}
+
+std::string SliderCrankMechanism::formatDouble(double value, int precision) const
+{
+    char buffer[32];
+    snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
+    return std::string(buffer);
+}
+
+double SliderCrankMechanism::clamp(double value, double min, double max) const
+{
+    if (value < min)
+        return min;
+    if (value > max)
+        return max;
+    return value;
+}
+
+// 基本属性访问
+std::string SliderCrankMechanism::getName() const
+{
+    return name;
+}
+
+MechanismType SliderCrankMechanism::getType() const
+{
+    return MechanismType::CrankSlider; // 使用相同的机构类型
+}
+
+MechanismParametersExt SliderCrankMechanism::getParameters() const
+{
+    return parameters;
+}
+
+void SliderCrankMechanism::setName(const std::string &newName)
+{
+    name = newName;
+}
+
+void SliderCrankMechanism::setParameters(const MechanismParametersExt &params)
+{
+    parameters = params;
+}
+
+void SliderCrankMechanism::setLink(double crankLength, double rodLength, double sliderOffset)
+{
+    parameters.CrankLength = crankLength;
+    parameters.ConnectingRodLength = rodLength;
+    parameters.SliderOffset = sliderOffset;
+}
+
+void SliderCrankMechanism::setSolutionMode(SolutionMode mode)
+{
+    parameters.SolutionMode = mode;
+}
+
+// 计算滑块范围
+std::pair<double, double> SliderCrankMechanism::calculateSliderRange()
+{
+    double L1 = parameters.CrankLength;
+    double L2 = parameters.ConnectingRodLength;
+    double h = parameters.SliderOffset;
+
+    // 检查Grashof条件
+    if ((L1 + L2) < std::abs(h))
+        return std::make_pair(0.0, 0.0);
+
+    double maxX = 0.0;
+    if ((L1 + L2) * (L1 + L2) > h * h)
+    {
+        maxX = std::sqrt((L1 + L2) * (L1 + L2) - h * h);
+    }
+
+    double minX = 0.0;
+    if ((L2 - L1) * (L2 - L1) > h * h)
+    {
+        minX = std::sqrt((L2 - L1) * (L2 - L1) - h * h);
+    }
+    else
+    {
+        minX = -std::sqrt((L1 + L2) * (L1 + L2) - h * h);
+    }
+
+    // 计算曲柄角度范围
+    if ((L1 + L2) * (L1 + L2) > h * h)
+    {
+        double theta_max = M_PI - std::asin(h / (L1 + L2));
+        double theta_min;
+
+        if ((L2 - L1) * (L2 - L1) > h * h)
+        {
+            theta_min = std::asin(h / (L1 + L2));
+        }
+        else
+        {
+            theta_min = -M_PI + std::asin(h / (L1 + L2));
+        }
+
+        parameters.CrankAngleRange1 = std::make_pair(theta_min, theta_max);
+        parameters.CrankAngleRange2 = std::make_pair(-theta_max, -theta_min);
+    }
+
+    return std::make_pair(minX, maxX);
+}
+
+// 主要计算函数（逆解）
+MechanismState SliderCrankMechanism::calculate(double sliderX)
+{
+    parameters.InputValue = sliderX;
+
+    double crankLength = parameters.CrankLength;
+    double rodLength = parameters.ConnectingRodLength;
+    double sliderOffset = parameters.SliderOffset;
+    auto solutionMode = parameters.SolutionMode;
+
+    Vector2D A(0.0, 0.0);
+    Vector2D S(sliderX, sliderOffset);
+
+    MechanismState state;
+    state.InputValue = sliderX;
+
+    try
+    {
+        // 计算A到S的向量和距离
+        Vector2D AS_vec = S - A;
+        double AS_distance = AS_vec.length();
+
+        // 检查机构是否可以装配
+        if (AS_distance > crankLength + rodLength ||
+            AS_distance < std::abs(crankLength - rodLength))
+        {
+            state.ErrorMessage = "滑块位置 " + formatDouble(sliderX) + " 导致机构无法装配";
+            return state;
+        }
+
+        // 使用余弦定理计算角度
+        double cosTheta = (crankLength * crankLength + AS_distance * AS_distance -
+                           rodLength * rodLength) /
+                          (2.0 * crankLength * AS_distance);
+        cosTheta = clamp(cosTheta, -1.0, 1.0);
+        double theta = std::acos(cosTheta);
+
+        // 计算基准角度
+        double alpha = std::atan2(AS_vec.Y, AS_vec.X);
+
+        // 计算两个可能的解
+        double angle1 = alpha + theta;
+        double angle2 = alpha - theta;
+
+        Vector2D B1 = A + Vector2D(
+                              crankLength * std::cos(angle1),
+                              crankLength * std::sin(angle1));
+
+        Vector2D B2 = A + Vector2D(
+                              crankLength * std::cos(angle2),
+                              crankLength * std::sin(angle2));
+
+        Vector2D B, B_alt;
+
+        // 默认选择解1
+        B = B1;
+        B_alt = B2;
+
+        // 根据解模式选择
+        if (solutionMode == SolutionMode::Solution2)
+        {
+            B = B2;
+            B_alt = B1;
+        }
+        else if (solutionMode == SolutionMode::Auto)
+        {
+            if (!trajectoryB.empty())
+            {
+                Vector2D lastB = trajectoryB.back();
+                double dist1 = Vector2D::Distance(B1, lastB);
+                double dist2 = Vector2D::Distance(B2, lastB);
+                if (dist2 < dist1)
+                {
+                    B = B2;
+                    B_alt = B1;
+                }
+            }
+        }
+
+        // 记录轨迹
+        trajectoryB.push_back(B);
+        trajectoryBAlt.push_back(B_alt);
+        sliderTrajectory.push_back(S);
+
+        // 限制轨迹点数量
+        const size_t MAX_TRAJECTORY_POINTS = 200;
+        if (trajectoryB.size() > MAX_TRAJECTORY_POINTS)
+        {
+            trajectoryB.erase(trajectoryB.begin());
+            trajectoryBAlt.erase(trajectoryBAlt.begin());
+            sliderTrajectory.erase(sliderTrajectory.begin());
+        }
+
+        // 计算角度
+        double crankAngle = std::atan2(B.Y, B.X) * 180.0 / M_PI;
+        double crankAngleAlt = std::atan2(B_alt.Y, B_alt.X) * 180.0 / M_PI;
+
+        // 检查机构是否可以装配
+        auto range = calculateSliderRange();
+        if (std::abs(range.first - range.second) < 1e-10)
+        {
+            state.WarningMessage = "警告：机构无法装配！(曲柄+连杆长度小于滑块偏移量)";
+        }
+
+        // 设置状态
+        state.Points["A"] = A;
+        state.Points["B"] = B;
+        state.Points["B_alt"] = B_alt;
+        state.Points["S"] = S;
+
+        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
+        state.Poses["BS"] = PoseCalculator::CalculatePoseAndQuaternion(B, S);
+
+        // TCP姿态（滑块处）
+        Pose7 tcpPose;
+        tcpPose.tx = S.X;
+        tcpPose.ty = S.Y;
+        tcpPose.tz = 0.0;
+        tcpPose.qw = 1.0;
+        state.Poses["TCP"] = tcpPose;
+
+        // 角度信息
+        state.Angles["当前解角度"] = crankAngle;
+        state.Angles["备选解角度"] = crankAngleAlt;
+        state.Angles["滑块位置"] = sliderX;
+
+        return state;
+    }
+    catch (const std::exception &ex)
+    {
+        state.ErrorMessage = std::string("计算失败: ") + ex.what();
+        return state;
+    }
+}
+
+// 参数验证
+ValidationResult SliderCrankMechanism::validateParameters()
+{
+    ValidationResult result;
+
+    if (parameters.CrankLength <= 0.0)
+    {
+        result.Errors.push_back("曲柄长度必须大于0");
+    }
+
+    if (parameters.ConnectingRodLength <= 0.0)
+    {
+        result.Errors.push_back("连杆长度必须大于0");
+    }
+
+    if (parameters.CrankLength + parameters.ConnectingRodLength <= std::abs(parameters.SliderOffset))
+    {
+        result.Errors.push_back("不满足Grashof条件: 曲柄+连杆长度必须大于滑块偏移量的绝对值");
+    }
+
+    return result;
+}
+
+// 获取输入范围
+std::pair<double, double> SliderCrankMechanism::getInputRange()
+{
+    return calculateSliderRange();
+}
+
+// 获取轨迹点
+std::vector<Vector2D> SliderCrankMechanism::getTrajectoryPoints()
+{
+    return trajectoryB;
+}
+
+std::vector<Vector2D> SliderCrankMechanism::getSliderTrajectory() const
+{
+    return sliderTrajectory;
+}
+
+std::vector<Vector2D> SliderCrankMechanism::getAlternativeTrajectory() const
+{
+    return trajectoryBAlt;
+}
+
+// 清除轨迹
+void SliderCrankMechanism::clearTrajectory()
+{
+    trajectoryB.clear();
+    trajectoryBAlt.clear();
+    sliderTrajectory.clear();
+}
+
+// 获取状态文本
+std::string SliderCrankMechanism::getStatusText()
+{
+    auto range = calculateSliderRange();
+    std::string status = name + "\n";
+
+    if (std::abs(range.first - range.second) < 1e-10)
+    {
+        status += "⚠️ 警告：机构无法装配！(曲柄+连杆长度小于滑块偏移量)\n";
+    }
+    else
+    {
+        status += "曲柄=" + formatDouble(parameters.CrankLength) + "m, ";
+        status += "连杆=" + formatDouble(parameters.ConnectingRodLength) + "m\n";
+        status += "偏移=" + formatDouble(parameters.SliderOffset) + "m\n";
+        status += "滑块范围: [" + formatDouble(range.first) + ", " + formatDouble(range.second) + "]m\n";
+        status += "行程: " + formatDouble(range.second - range.first) + "m\n";
+
+        status += "解1角度范围: [" + formatDouble(parameters.CrankAngleRange1.first * 180.0 / M_PI, 1) + "°, ";
+        status += formatDouble(parameters.CrankAngleRange1.second * 180.0 / M_PI, 1) + "°]\n";
+        status += "解2角度范围: [" + formatDouble(parameters.CrankAngleRange2.first * 180.0 / M_PI, 1) + "°, ";
+        status += formatDouble(parameters.CrankAngleRange2.second * 180.0 / M_PI, 1) + "°]";
+    }
+
+    return status;
+}
+
+// ==================== 工厂函数实现 ====================
+
+SliderCrankMechanism *createSliderCrankMechanism()
+{
+    return new SliderCrankMechanism();
+}
+
+void deleteSliderCrankMechanism(SliderCrankMechanism *mechanism)
+{
+    delete mechanism;
+}
+
+SliderCrankMechanismPtr createSliderCrankMechanismSmart()
+{
+    return std::make_shared<SliderCrankMechanism>();
+}