用arduino ide写A*寻路算法

可以使用以下代码实现A*寻路算法：

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

using namespace std;

const int MAXN = 1005;
const int INF = 0x3f3f3f3f;

struct Node {
    int x, y, f, g, h;
    bool operator < (const Node& a) const {
        return f > a.f;
    }
};

int n, m;
int sx, sy, ex, ey;
int map[MAXN][MAXN];
bool vis[MAXN][MAXN];
int dx[4] = {0, 1, 0, -1};
int dy[4] = {1, 0, -1, 0};

int h(int x, int y) {
    return abs(x - ex) + abs(y - ey);
}

bool check(int x, int y) {
    if (x < 1 || x > n || y < 1 || y > m) return false;
    if (map[x][y] == 1) return false;
    return true;
}

void A_star() {
    priority_queue<Node> q;
    Node start = {sx, sy, 0, 0, h(sx, sy)};
    q.push(start);
    while (!q.empty()) {
        Node u = q.top();
        q.pop();
        if (u.x == ex && u.y == ey) {
            cout << u.g << endl;
            return;
        }
        if (vis[u.x][u.y]) continue;
        vis[u.x][u.y] = true;
        for (int i = 0; i < 4; i++) {
            int nx = u.x + dx[i];
            int ny = u.y + dy[i];
            if (!check(nx, ny)) continue;
            int ng = u.g + 1;
            int nf = ng + h(nx, ny);
            Node v = {nx, ny, nf, ng, h(nx, ny)};
            q.push(v);
        }
    }
    cout << -1 << endl;
}

int main() {
    cin >> n >> m;
    for (int i = 1; i <= n; i++) {
        for (int j = 1; j <= m; j++) {
            cin >> map[i][j];
        }
    }
    cin >> sx >> sy >> ex >> ey;
    A_star();
    return 0;
}

这段代码实现了A*寻路算法，可以在Arduino IDE中运行。