写一个简单的寻路算法

最简单的寻路算法之一是广度优先搜索（BFS）。它的基本思想是从起点开始，按照深度优先的顺序搜索每一个结点，直到找到终点。

算法流程如下：
1. 将起点结点加入队列。
2. 当队列不为空时，取出队列首结点。
3. 如果该结点为终点，则找到路径。
4. 否则将该结点的所有相邻结点加入队列。
5. 重复步骤2 ~ 4，直到找到终点或队列为空。

该算法的时间复杂度为O(V+E)

代码如下：

def BFS(graph, start, end):
    queue = []
    queue.append(start)
    while queue:
        node = queue.pop(0)
        if node == end:
            return True
        for neighbor in graph[node]:
            if neighbor not in queue:
                queue.append(neighbor)
    return False

这是一个简单的例子，实际应用中需要根据实际情况进行修改和完善。

相关问题

写个unity的寻路算法

Unity中的寻路算法通常使用A*算法。下面是一个简单的示例代码：

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Pathfinding : MonoBehaviour
{
    public Transform seeker, target; // 寻路起点和终点
    Grid grid; // 网格

    void Awake()
    {
        grid = GetComponent<Grid>();
    }

    void Update()
    {
        FindPath(seeker.position, target.position);
    }

    void FindPath(Vector3 startPos, Vector3 targetPos)
    {
        Node startNode = grid.NodeFromWorldPoint(startPos);
        Node targetNode = grid.NodeFromWorldPoint(targetPos);

        List<Node> openSet = new List<Node>();
        HashSet<Node> closedSet = new HashSet<Node>();
        openSet.Add(startNode);

        while (openSet.Count > 0)
        {
            Node currentNode = openSet[0];
            for (int i = 1; i < openSet.Count; i++)
            {
                if (openSet[i].fCost < currentNode.fCost || (openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost))
                {
                    currentNode = openSet[i];
                }
            }

            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            if (currentNode == targetNode)
            {
                RetracePath(startNode, targetNode);
                return;
            }

            foreach (Node neighbor in grid.GetNeighbors(currentNode))
            {
                if (!neighbor.walkable || closedSet.Contains(neighbor))
                {
                    continue;
                }

                int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor);
                if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
                {
                    neighbor.gCost = newMovementCostToNeighbor;
                    neighbor.hCost = GetDistance(neighbor, targetNode);
                    neighbor.parent = currentNode;

                    if (!openSet.Contains(neighbor))
                    {
                        openSet.Add(neighbor);
                    }
                }
            }
        }
    }

    void RetracePath(Node startNode, Node endNode)
    {
        List<Node> path = new List<Node>();
        Node currentNode = endNode;

        while (currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.parent;
        }
        path.Reverse();

        grid.path = path;
    }

    int GetDistance(Node nodeA, Node nodeB)
    {
        int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
        int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);

        if (dstX > dstY)
        {
            return 14 * dstY + 10 * (dstX - dstY);
        }
        else
        {
            return 14 * dstX + 10 * (dstY - dstX);
        }
    }
}

该脚本需要与一个名为“Grid”的脚本一起使用，该脚本负责生成网格和节点。使用A*算法寻找路径时，需要首先根据起点和终点获取起点和终点所在的节点，然后使用openSet和closedSet分别存储已经遍历的节点和未遍历的节点。进入while循环后，每次从openSet中获取fCost最小的节点，然后检查是否到达终点，如果是，则回溯路径；如果不是，则将该节点周围的节点加入openSet。在加入openSet之前，需要计算每个节点的gCost和hCost，分别表示从起点到当前节点的实际代价和从当前节点到终点的估算代价。最终得到的路径是一系列节点的列表，可以在Grid脚本中进行渲染。

用C++写一个A*寻路算法

好的，以下是一个简单的C语言实现的A*寻路算法：

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>

#define ROW 5
#define COL 5

typedef struct {
    int x, y;
} Point;

typedef struct {
    Point parent;
    int f, g, h;
} Node;

Node map[ROW][COL];
Point start, end;

bool is_valid(int x, int y) {
    return x >= 0 && x < ROW && y >= 0 && y < COL;
}

bool is_obstacle(int x, int y) {
    return map[x][y].f == -1;
}

int heuristic(int x, int y) {
    return abs(x - end.x) + abs(y - end.y);
}

void print_path() {
    Point path[ROW * COL];
    int count = 0;
    Point current = end;
    while (current.x != start.x || current.y != start.y) {
        path[count++] = current;
        current = map[current.x][current.y].parent;
    }
    path[count++] = start;
    printf("Path: ");
    for (int i = count - 1; i >= 0; i--) {
        printf("(%d, %d) ", path[i].x, path[i].y);
    }
    printf("\n");
}

void a_star() {
    int dx[8] = {-1, -1, 0, 1, 1, 1, 0, -1};
    int dy[8] = {0, 1, 1, 1, 0, -1, -1, -1};
    map[start.x][start.y].f = 0;
    map[start.x][start.y].g = 0;
    map[start.x][start.y].h = heuristic(start.x, start.y);
    Node open_set[ROW * COL];
    int open_set_size = 1;
    open_set[0] = map[start.x][start.y];
    while (open_set_size > 0) {
        int current_index = 0;
        for (int i = 1; i < open_set_size; i++) {
            if (open_set[i].f < open_set[current_index].f) {
                current_index = i;
            }
        }
        Node current = open_set[current_index];
        if (current.parent.x == end.x && current.parent.y == end.y) {
            print_path();
            return;
        }
        open_set[current_index] = open_set[open_set_size - 1];
        open_set_size--;
        for (int i = 0; i < 8; i++) {
            int new_x = current.parent.x + dx[i];
            int new_y = current.parent.y + dy[i];
            if (!is_valid(new_x, new_y) || is_obstacle(new_x, new_y)) {
                continue;
            }
            int new_g = current.g + 1;
            int new_h = heuristic(new_x, new_y);
            int new_f = new_g + new_h;
            if (map[new_x][new_y].f == -2 || new_f < map[new_x][new_y].f) {
                map[new_x][new_y].parent = current.parent;
                map[new_x][new_y].f = new_f;
                map[new_x][new_y].g = new_g;
                map[new_x][new_y].h = new_h;
                if (map[new_x][new_y].f == -2) {
                    open_set[open_set_size++] = map[new_x][new_y];
                }
            }
        }
    }
    printf("No path found!\n");
}

int main() {
    // initialize map
    for (int i = 0; i < ROW; i++) {
        for (int j = 0; j < COL; j++) {
            map[i][j].parent.x = i;
            map[i][j].parent.y = j;
            map[i][j].f = -2;
            map[i][j].g = -2;
            map[i][j].h = -2;
        }
    }
    // set obstacles
    map[1][1].f = -1;
    map[1][2].f = -1;
    map[1][3].f = -1;
    map[2][3].f = -1;
    map[3][3].f = -1;
    map[3][2].f = -1;
    // set start and end points
    start.x = 0;
    start.y = 0;
    end.x = 4;
    end.y = 4;
    // run A* algorithm
    a_star();
    return 0;
}

这个程序实现了一个简单的A*寻路算法，可以在一个5x5的地图上找到从起点到终点的最短路径。其中，-1表示障碍物，-2表示未访问过的节点，其它数字表示已访问过的节点的f值。