Initial commit

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>();
+}