Initial commit

inc/QuadrupedRobotSimulation/graph_utils.hpp

@@ -0,0 +1,109 @@
+// graph_utils.hpp
+#pragma once
+#include <iostream>
+#include <map>
+#include <string>
+#include <vector>
+
+class GraphUtils
+{
+private:
+    // 定义边类型
+    using Edge = std::pair<std::string, std::string>;
+    using EdgeMap = std::map<std::string, Edge>;
+
+    // 静态数据成员
+    static EdgeMap graphData;
+
+    // 初始化静态数据
+    static EdgeMap initGraphData()
+    {
+        EdgeMap data;
+        data["AB"] = {"A", "B"};
+        data["AB1"] = {"A", "B1"};
+        data["B1B2"] = {"B1", "B2"};
+        data["BC"] = {"B", "C"};
+        data["C2C3"] = {"C2", "C3"};
+        data["C4C3"] = {"C4", "C3"};
+        data["CC4"] = {"C", "C4"};
+        return data;
+    }
+
+public:
+    // 静态方法 - 获取所有边
+    static const EdgeMap &getGraph()
+    {
+        static EdgeMap instance = initGraphData();
+        return instance;
+    }
+
+    // 获取边的两个节点
+    static std::pair<std::string, std::string> getNodes(const std::string &edgeName)
+    {
+        const auto &graph = getGraph();
+        auto it = graph.find(edgeName);
+        if (it != graph.end())
+        {
+            return it->second;
+        }
+        return {"", ""}; // 返回空值表示未找到
+    }
+
+    // 获取所有边名
+    static std::vector<std::string> getEdgeNames()
+    {
+        std::vector<std::string> names;
+        const auto &graph = getGraph();
+        names.reserve(graph.size());
+        for (const auto &pair : graph)
+        {
+            names.push_back(pair.first);
+        }
+        return names;
+    }
+
+    // 导出为字符串
+    static std::string toString()
+    {
+        const auto &graph = getGraph();
+        std::string result = "{\n";
+        for (const auto &pair : graph)
+        {
+            result += "    \"" + pair.first + "\": [\"" +
+                      pair.second.first + "\", \"" +
+                      pair.second.second + "\"],\n";
+        }
+        // 移除最后一个逗号
+        if (!graph.empty())
+        {
+            result.pop_back(); // 移除换行符
+            result.pop_back(); // 移除逗号
+            result += "\n";
+        }
+        result += "}";
+        return result;
+    }
+
+    // 查找包含节点的边
+    static std::vector<std::string> findEdgesWithNode(const std::string &node)
+    {
+        std::vector<std::string> result;
+        const auto &graph = getGraph();
+
+        for (const auto &pair : graph)
+        {
+            if (pair.second.first == node || pair.second.second == node)
+            {
+                result.push_back(pair.first);
+            }
+        }
+        return result;
+    }
+
+    // 显示图形
+    static void display()
+    {
+        std::cout << "Graph Structure:" << std::endl;
+        std::cout << toString() << std::endl;
+    }
+};