C++ 代码学习二 ---- 简易A*算法

#include <iostream>
#include <queue>
#include <vector>
#include <cmath>
#include <Eigen/Dense>



struct Vector3i {
    int x, y, z;
    Vector3i(int x = 0, int y = 0, int z = 0) : x(x), y(y), z(z) {}
    Vector3i operator+(const Vector3i& other) const {
        return Vector3i(x + other.x, y + other.y, z + other.z);
    }
};

struct GridNode {
    Vector3i index;
    int rounds = 0;  // Default to 0
    double gScore = INFINITY;
    double fScore = INFINITY;
    GridNode* cameFrom = nullptr;
    bool isObstacle = false;
};

struct NodeComparator {
    bool operator()(const GridNode* lhs, const GridNode* rhs) const {
        return lhs->fScore > rhs->fScore;
    }
};

int main() {
    // 网格设置
    int size = 5;
    std::vector<std::vector<GridNode>> grid(size, std::vector<GridNode>(size));
    for (int i = 0; i < size; ++i) {
        for (int j = 0; j < size; ++j) {
            grid[i][j].index = Vector3i(i, j, 0);
            std::cout << "grid[" << i << "][" << j << "].index = ("
                << grid[i][j].index.x << ", "
                << grid[i][j].index.y << ", "
                << grid[i][j].index.z << ")" << std::endl;
        }
    }

    // 初始化起点、终点
    Vector3i start_idx(0, 0, 0);
    Vector3i end_idx(4, 4, 0);

    // A* setup
    int rounds_ = 1;
    std::cout << "rounds_ = " << rounds_ << std::endl;
    GridNode* startPtr = &grid[start_idx.x][start_idx.y];
    GridNode* endPtr = &grid[end_idx.x][end_idx.y];
    std::cout << "startPtr->index[x y z] = ("
        << startPtr->index.x << ", "
        << startPtr->index.y << ", "
        << startPtr->index.z << ")" << std::endl;
    startPtr->gScore = 0;
    startPtr->fScore = std::sqrt(32);  // heuristic distance to end point
    startPtr->rounds = rounds_;
    std::cout << "startPtr->rounds =  " << startPtr->rounds << std::endl;
    std::priority_queue<GridNode*, std::vector<GridNode*>, NodeComparator> openSet_;
    openSet_.push(startPtr);
    while (!openSet_.empty()) {
        GridNode* current = openSet_.top();
        openSet_.pop();
        
        if (current->index.x == end_idx.x && current->index.y == end_idx.y) {
            std::cout << "Reached the goal!\n";
            std::cout << "current 包含了以下几个点，分别为：" << std::endl;       
            std::vector<GridNode*> path;
            GridNode* temp = current;

            while (temp != nullptr) {
                path.push_back(temp);// 将temp元素添加在容器末尾
                temp = temp->cameFrom;
                std::cout << "current 节点的父节点地址为：" << temp << std::endl;
            }
            for (int i = path.size() - 1; i >= 0; --i) {
                std::cout << "索引 [x y z] = (" << path[i]->index.x << ", " << path[i]->index.y << ", " << path[i]->index.z << ")" << std::endl;
                std::cout << "rounds = " << path[i]->rounds << std::endl;
                std::cout << "gScore = " << path[i]->gScore << std::endl;
                std::cout << "fScore = " << path[i]->fScore << std::endl;
                std::cout << "cameFrom = " << path[i]->cameFrom << std::endl;
                std::cout << "isObstacle = " << path[i]->isObstacle << std::endl;
                std::cout << std::endl;
            }
            break;
        }
        std::cout << "开始计算[" << current ->index.x<<"," << current->index.y << "," << current->index.z << "]周围邻居节点" << std::endl;
        for (int dx = -1; dx <= 1; ++dx) {
            for (int dy = -1; dy <= 1; ++dy) {
                if (dx == 0 && dy == 0) continue;
                
                Vector3i neighborIdx = current->index + Vector3i(dx, dy, 0);
                std::cout << "周围的邻居节点为：[" << neighborIdx.x<<"," << neighborIdx.y << "," << neighborIdx.z << "]" << std::endl;
                if (neighborIdx.x < 0 || neighborIdx.x >= size || neighborIdx.y < 0 || neighborIdx.y >= size) {
                    std::cout << "超出边界！" << std::endl;
                    continue;  // Out of bounds
                    
                }

                GridNode* neighborPtr = &grid[neighborIdx.x][neighborIdx.y];
                std::cout << "neighborPtr->rounds[x y z ] =  " << "[" << neighborPtr->index.x << "," << neighborPtr->index.y << "," << neighborPtr->index.z << "]，并且neighborPtr->rounds = " << neighborPtr->rounds << std::endl;
                
                if (neighborPtr->rounds == rounds_) {
                    std::cout << "这个已经访问过！节点计数为：" << neighborPtr->rounds << "\t" << "节点索引为：[" << neighborPtr->index.x << neighborPtr->index.y << neighborPtr->index.z << "]" << std::endl;
                    continue;  // Already processed in this round
                   
                }
                std::cout << "这个节点未访问过！计数为0"<< std::endl;
                neighborPtr->rounds = rounds_;
                std::cout << "将计数更新为：" << neighborPtr->rounds << std::endl;
                double tentative_gScore = current->gScore + std::sqrt(dx * dx + dy * dy);
                if (tentative_gScore < neighborPtr->gScore) {
                    std::cout << "当前节点的父节点地址为：" << current << std::endl;
                    neighborPtr->cameFrom = current;
                    neighborPtr->gScore = tentative_gScore;
                    neighborPtr->fScore = tentative_gScore + std::sqrt(std::pow(end_idx.x - neighborIdx.x, 2) + std::pow(end_idx.y - neighborIdx.y, 2));
                    openSet_.push(neighborPtr);
                }
            }
        }
    }

    return 0;
}

这部分代码没有进行障碍物检测