C++ 代码学习一 ----- 三维坐标周围节点搜索与优先级队

1、搜索三维空间中某个点周围的26个点

#include <iostream>
#include <vector>
#include <tuple>

// 定义一个表示三维坐标的结构体
struct Point {
    int x, y, z;
    Point(int _x, int _y, int _z) : x(_x), y(_y), z(_z) {}
};

// 获取三维空间中与给定坐标相邻的所有坐标
std::vector<Point> get_neighbors(const Point& coord) {
    std::vector<Point> neighbors;
    for (int dx = -1; dx <= 1; ++dx) {
        for (int dy = -1; dy <= 1; ++dy) {
            for (int dz = -1; dz <= 1; ++dz) {
                if (dx == 0 && dy == 0 && dz == 0) {
                    continue;  // 跳过原始坐标
                }
                Point neighbor(coord.x + dx, coord.y + dy, coord.z + dz);
                neighbors.push_back(neighbor);
            }
        }
    }
    return neighbors;
}

int main() {
    Point coord(2, 1, 3);
    std::vector<Point> neighbors = get_neighbors(coord);
    std::cout << "坐标 (" << coord.x << ", " << coord.y << ", " << coord.z << ") 周围的相邻坐标：" << std::endl;
    for (const auto& neighbor : neighbors) {
        std::cout << "(" << neighbor.x << ", " << neighbor.y << ", " << neighbor.z << ")" << std::endl;
    }
    return 0;
}

2、优先级队列使用方法

#include <iostream>
#include <queue>
#include <vector>

class Node {
public:
    int index;
    double g_score;

    Node(int _index, double _g_score) : index(_index), g_score(_g_score) {}

    void print() const {
        std::cout << "index: " << index << ", g_score: " << g_score << std::endl;
    }
};

// 定义比较函数对象
class NodeComparator0 {
public:
    bool operator() (const Node* n1, const Node* n2) const {
        return n1->g_score > n2->g_score; // 按照 g_score 降序排列
    }
};

// 定义优先队列
std::priority_queue<Node*, std::vector<Node*>, NodeComparator0> open_set;

int main() {
    // 创建几个节点
    Node node1(1, 10.0);
    Node node2(2, 5.0);
    Node node3(3, 7.0);
    Node node4(4, 20.0);

    // 将节点指针放入优先队列
    open_set.push(&node1);
    open_set.push(&node2);
    open_set.push(&node3);
    open_set.push(&node4);

    // 输出队列中的节点
    while (!open_set.empty()) {
        Node* node = open_set.top();
        node->print();
        open_set.pop();
    }

    return 0;
}

3、将列向量转置为行向量

Eigen::Vector3d column_vector(1, 2, 3);
// 将列向量转置为行向量
Eigen::RowVector3d row_vector = column_vector.transpose();