【游戏算法】 - 寻路算法-A星寻路

宋 游戏开发
  2020-10-19 20:22:05 2020-10-19 20:22:05 创建   2021-03-15 20:30:07 2021-03-15 20:30:07 更新      2.4k 字  14 分钟

A星算法的核心思想

A星算法的核心在于启发式评估函数，其公式为：

  • G(n)：从起点到当前节点 n 的实际代价（已知的路径成本）。
  • H(n)：从当前节点 n 到目标节点的预估代价（启发式估算成本）。
  • F(n)：总评估代价，用于决定下一步搜索的优先级。

A星算法通过不断扩展F值最小的节点，逐步逼近目标，最终找到最优路径。

算法原理与关键概念

开放列表（OpenList）与关闭列表（CloseList）

  • OpenList：待探索的节点集合，按 F值 排序。
  • CloseList：已探索过的节点集合，避免重复计算。

启发函数（Heuristic Function）

  • 曼哈顿距离：适用于只能上下左右移动的网格（如城市街区）：
  • 欧几里得距离：适用于可自由移动的连续空间：
  • 对角线距离：适用于允许斜向移动的网格：

算法流程

  • 步骤1：将起点加入 OpenList。
  • 步骤2：从 OpenList 中选择 F值最小 的节点作为当前节点。
  • 步骤3：遍历当前节点的所有邻居：
    • 如果邻居是障碍物或已在 CloseList 中，跳过。
    • 如果邻居未在 OpenList 中，计算其 G 和 H 值，并将其加入 OpenList。
    • 如果邻居已在 OpenList 中，检查通过当前节点到达邻居的路径是否更优（即 G值更小），如果是则更新邻居的 G 和 F 值。
  • 步骤4：将当前节点移入 CloseList。
  • 步骤5：重复步骤2-4，直到找到目标节点或 OpenList 为空。
    A星八方向
    A星四方向 优先选择最先遍历的点
    A星四方向 优先选择最后遍历的点

算法实现

A星算法实现 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <limits>

enum class HeuristicType {
    Manhattan,
    Euclidean,
    Diagonal
};

enum class MoveDirection {
    FourWay,
    EightWay
};

struct Node {
    int x, y;
    float g, h, f;
    Node* parent;

    Node(int x, int y) : x(x), y(y), g(0), h(0), f(0), parent(nullptr) {}

    bool operator==(const Node& other) const {
        return x == other.x && y == other.y;
    }
};

float calculateHeuristic(HeuristicType type, const Node& a, const Node& b) {
    if (type == HeuristicType::Manhattan) {
        return std::abs(a.x - b.x) + std::abs(a.y - b.y);
    } else if (type == HeuristicType::Euclidean) {
        return std::sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
    } else { // Diagonal
        return std::max(std::abs(a.x - b.x), std::abs(a.y - b.y));
    }
}

bool aStarSearch(const std::vector<std::vector<int>>& grid, 
                std::vector<std::vector<int>>& path, 
                const Node& start, const Node& goal, 
                HeuristicType heuristicType, 
                MoveDirection moveDirection) {
    std::priority_queue<Node*, std::vector<Node*>, 
                        [](Node* a, Node* b){ return a->f > b->f; }> openList;
    std::vector<std::vector<bool>> closedList(grid.size(), std::vector<bool>(grid[0].size(), false));

    Node* startNode = new Node(start.x, start.y);
    startNode->h = calculateHeuristic(heuristicType, *startNode, goal);
    startNode->f = startNode->g + startNode->h;
    openList.push(startNode);

    while (!openList.empty()) {
        Node* current = openList.top();
        openList.pop();

        if (current->x == goal.x && current->y == goal.y) {
            // Reconstruct path
            Node* node = current;
            while (node != nullptr) {
                path.push_back({node->x, node->y});
                node = node->parent;
            }
            std::reverse(path.begin(), path.end());
            return true;
        }

        closedList[current->x][current->y] = true;

        std::vector<std::pair<int, int>> directions;
        if (moveDirection == MoveDirection::FourWay) {
            directions = {{0, 1}, {1, 0}, {0, -1}, {-1, 0}};
        } else { // EightWay
            directions = {
                {0, 1}, {1, 0}, {0, -1}, {-1, 0},
                {1, 1}, {1, -1}, {-1, 1}, {-1, -1}
            };
        }

        for (const auto& [dx, dy] : directions) {
            int nx = current->x + dx;
            int ny = current->y + dy;

            if (nx >= 0 && ny >= 0 && nx < grid.size() && ny < grid[0].size() && 
                grid[nx][ny] == 0 && !closedList[nx][ny]) {
                
                Node* neighbor = new Node(nx, ny);
                neighbor->parent = current;
                neighbor->g = current->g + 1;
                neighbor->h = calculateHeuristic(heuristicType, *neighbor, goal);
                neighbor->f = neighbor->g + neighbor->h;
                
                openList.push(neighbor);
            }
        }
    }
    return false;
}

int main() {
    std::vector<std::vector<int>> grid = {
        {0, 0, 0, 0, 0},
        {0, 1, 1, 0, 0},
        {0, 0, 0, 0, 0},
        {0, 0, 1, 1, 0},
        {0, 0, 0, 0, 0}
    };
    
    std::vector<std::vector<int>> path;
    Node start(0, 0);
    Node goal(4, 4);
    
    if (aStarSearch(grid, path, start, goal, HeuristicType::Manhattan, MoveDirection::FourWay)) {
        std::cout << "4-Direction Path found:\n";
        for (auto& p : path) {
            std::cout << "(" << p[0] << ", " << p[1] << ")\n";
        }
    } else {
        std::cout << "No 4-Direction path found\n";
    }

    if (aStarSearch(grid, path, start, goal, HeuristicType::Diagonal, MoveDirection::EightWay)) {
        std::cout << "8-Direction Path found:\n";
        for (auto& p : path) {
            std::cout << "(" << p[0] << ", " << p[1] << ")\n";
        }
    } else {
        std::cout << "No 8-Direction path found\n";
    }
    
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
using System;
using System.Collections.Generic;
using System.Linq;

namespace AStar
{
    enum HeuristicType
    {
        Manhattan,
        Euclidean,
        Diagonal
    }

    enum MoveDirection
    {
        FourWay,
        EightWay
    }

    class Node : IComparable<Node>
    {
        public int X { get; set; }
        public int Y { get; set; }
        public float G { get; set; }
        public float H { get; set; }
        public float F => G + H;
        public Node Parent { get; set; }

        public Node(int x, int y)
        {
            X = x;
            Y = y;
        }

        public int CompareTo(Node other)
        {
            return F.CompareTo(other.F);
        }

        public override bool Equals(object obj)
        {
            if (obj is Node n)
                return X == n.X && Y == n.Y;
            return false;
        }

        public override int GetHashCode()
        {
            return X.GetHashCode() ^ Y.GetHashCode();
        }
    }

    class AStar
    {
        static float CalculateHeuristic(HeuristicType type, Node a, Node b)
        {
            switch (type)
            {
                case HeuristicType.Manhattan:
                    return Math.Abs(a.X - b.X) + Math.Abs(a.Y - b.Y);
                case HeuristicType.Euclidean:
                    return (float)Math.Sqrt((a.X - b.X) * (a.X - b.X) + (a.Y - b.Y) * (a.Y - b.Y));
                default: // Diagonal
                    return Math.Max(Math.Abs(a.X - b.X), Math.Abs(a.Y - b.Y));
            }
        }

        public static List<(int x, int y)> FindPath(int[,] grid, Node start, Node goal, 
                                                  HeuristicType heuristicType, 
                                                  MoveDirection moveDirection)
        {
            var openList = new SortedSet<Node>(Comparer<Node>.Create((a, b) => a.F.CompareTo(b.F)));
            var closedSet = new HashSet<(int, int)>();
            var directions = new List<(int dx, int dy)>();

            if (moveDirection == MoveDirection.FourWay)
            {
                directions = new List<(int, int)> {
                    (0, 1), (1, 0), (0, -1), (-1, 0)
                };
            }
            else // EightWay
            {
                directions = new List<(int, int)> {
                    (0, 1), (1, 0), (0, -1), (-1, 0),
                    (1, 1), (1, -1), (-1, 1), (-1, -1)
                };
            }

            start.H = CalculateHeuristic(heuristicType, start, goal);
            start.F = start.G + start.H;
            openList.Add(start);

            while (openList.Count > 0)
            {
                var current = openList.Min;
                openList.Remove(current);
                closedSet.Add((current.X, current.Y));

                if (current.Equals(goal))
                {
                    var path = new List<(int x, int y)>();
                    var node = current;
                    while (node != null)
                    {
                        path.Add((node.X, node.Y));
                        node = node.Parent;
                    }
                    path.Reverse();
                    return path;
                }

                foreach (var (dx, dy) in directions)
                {
                    int nx = current.X + dx;
                    int ny = current.Y + dy;

                    if (nx >= 0 && ny >= 0 && nx < grid.GetLength(0) && ny < grid.GetLength(1) &&
                        grid[nx, ny] == 0 && !closedSet.Contains((nx, ny)))
                    {
                        var neighbor = new Node(nx, ny) { Parent = current };
                        neighbor.G = current.G + 1;
                        neighbor.H = CalculateHeuristic(heuristicType, neighbor, goal);
                        neighbor.F = neighbor.G + neighbor.H;

                        openList.Add(neighbor);
                    }
                }
            }
            return null;
        }

        static void Main()
        {
            int[,] grid = {
                {0, 0, 0, 0, 0},
                {0, 1, 1, 0, 0},
                {0, 0, 0, 0, 0},
                {0, 0, 1, 1, 0},
                {0, 0, 0, 0, 0}
            };

            var start = new Node(0, 0);
            var goal = new Node(4, 4);

            var fourWayPath = FindPath(grid, start, goal, HeuristicType.Manhattan, MoveDirection.FourWay);
            var eightWayPath = FindPath(grid, start, goal, HeuristicType.Diagonal, MoveDirection.EightWay);

            if (fourWayPath != null)
            {
                Console.WriteLine("4-Direction Path found:");
                foreach (var p in fourWayPath)
                {
                    Console.WriteLine($"({p.x}, {p.y})");
                }
            }
            else
            {
                Console.WriteLine("No 4-Direction path found");
            }

            if (eightWayPath != null)
            {
                Console.WriteLine("8-Direction Path found:");
                foreach (var p in eightWayPath)
                {
                    Console.WriteLine($"({p.x}, {p.y})");
                }
            }
            else
            {
                Console.WriteLine("No 8-Direction path found");
            }
        }
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
enum HeuristicType {
    Manhattan,
    Euclidean,
    Diagonal
}

enum MoveDirection {
    FourWay,
    EightWay
}

interface Node {
    x: number;
    y: number;
    g: number;
    h: number;
    f: number;
    parent: Node | null;
}

function calculateHeuristic(type: HeuristicType, a: Node, b: Node): number {
    if (type === HeuristicType.Manhattan) {
        return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
    } else if (type === HeuristicType.Euclidean) {
        return Math.sqrt((a.x - b.x) ** 2 + (a.y - b.y) ** 2);
    } else { // Diagonal
        return Math.max(Math.abs(a.x - b.x), Math.abs(a.y - b.y));
    }
}

function aStarSearch(grid: number[][], start: Node, goal: Node, 
                    heuristicType: HeuristicType, 
                    moveDirection: MoveDirection): Node[] | null {
    const openList: Node[] = [];
    const closedSet: Set<string> = new Set();
    const directions: [number, number][] = [];

    if (moveDirection === MoveDirection.FourWay) {
        directions.push([0, 1], [1, 0], [0, -1], [-1, 0]);
    } else { // EightWay
        directions.push(
            [0, 1], [1, 0], [0, -1], [-1, 0],
            [1, 1], [1, -1], [-1, 1], [-1, -1]
        );
    }

    const startNode: Node = {
        x: start.x,
        y: start.y,
        g: 0,
        h: calculateHeuristic(heuristicType, start, goal),
        f: start.g + start.h,
        parent: null
    };

    openList.push(startNode);
    openList.sort((a, b) => a.f - b.f);

    while (openList.length > 0) {
        const current = openList.shift()!;
        const key = `${current.x},${current.y}`;
        closedSet.add(key);

        if (current.x === goal.x && current.y === goal.y) {
            const path: Node[] = [];
            let node: Node | null = current;
            while (node) {
                path.push(node);
                node = node.parent;
            }
            return path.reverse();
        }

        for (const [dx, dy] of directions) {
            const nx = current.x + dx;
            const ny = current.y + dy;
            const keyNeighbor = `${nx},${ny}`;

            if (nx >= 0 && ny >= 0 && 
                nx < grid.length && ny < grid[0].length && 
                grid[nx][ny] === 0 && 
                !closedSet.has(keyNeighbor)) {
                
                const neighbor: Node = {
                    x: nx,
                    y: ny,
                    g: current.g + 1,
                    h: calculateHeuristic(heuristicType, {x: nx, y: ny}, goal),
                    f: current.g + 1 + calculateHeuristic(heuristicType, {x: nx, y: ny}, goal),
                    parent: current
                };

                openList.push(neighbor);
                openList.sort((a, b) => a.f - b.f);
            }
        }
    }

    return null;
}

// Example usage
const grid: number[][] = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 0, 0],
    [0, 0, 0, 0, 0],
    [0, 0, 1, 1, 0],
    [0, 0, 0, 0, 0]
];

const start: Node = { x: 0, y: 0, g: 0, h: 0, f: 0, parent: null };
const goal: Node = { x: 4, y: 4, g: 0, h: 0, f: 0, parent: null };

const fourWayPath = aStarSearch(grid, start, goal, HeuristicType.Manhattan, MoveDirection.FourWay);
const eightWayPath = aStarSearch(grid, start, goal, HeuristicType.Diagonal, MoveDirection.EightWay);

if (fourWayPath) {
    console.log("4-Direction Path found:");
    fourWayPath.forEach(p => console.log(`(${p.x}, ${p.y})`));
} else {
    console.log("No 4-Direction path found");
}

if (eightWayPath) {
    console.log("8-Direction Path found:");
    eightWayPath.forEach(p => console.log(`(${p.x}, ${p.y})`));
} else {
    console.log("No 8-Direction path found");
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
HeuristicType = {
    Manhattan = 1,
    Euclidean = 2,
    Diagonal = 3
}

MoveDirection = {
    FourWay = 1,
    EightWay = 2
}

function calculateHeuristic(type, a, b)
    if type == HeuristicType.Manhattan then
        return math.abs(a.x - b.x) + math.abs(a.y - b.y)
    elseif type == HeuristicType.Euclidean then
        return math.sqrt((a.x - b.x)^2 + (a.y - b.y)^2)
    else -- Diagonal
        return math.max(math.abs(a.x - b.x), math.abs(a.y - b.y))
    end
end

function aStarSearch(grid, start, goal, heuristicType, moveDirection)
    local openList = {}
    local closedSet = {}
    local directions = {}
    
    if moveDirection == MoveDirection.FourWay then
        directions = {
            {0,1}, {1,0}, {0,-1}, {-1,0}
        }
    else -- EightWay
        directions = {
            {0,1}, {1,0}, {0,-1}, {-1,0},
            {1,1}, {1,-1}, {-1,1}, {-1,-1}
        }
    end
    
    table.insert(openList, {
        x = start.x, 
        y = start.y, 
        g = 0, 
        h = calculateHeuristic(heuristicType, start, goal), 
        f = 0 + calculateHeuristic(heuristicType, start, goal), 
        parent = nil
    })
    
    while #openList > 0 do
        -- Sort by F value
        table.sort(openList, function(a,b) return a.f < b.f end)
        local current = table.remove(openList, 1)
        local key = current.x .. "," .. current.y
        closedSet[key] = true
        
        if current.x == goal.x and current.y == goal.y then
            local path = {}
            local node = current
            while node do
                table.insert(path, 1, {x=node.x, y=node.y})
                node = node.parent
            end
            return path
        end
        
        for _, dir in ipairs(directions) do
            local nx = current.x + dir[1]
            local ny = current.y + dir[2]
            local keyNeighbor = nx .. "," .. ny
            
            if nx >= 0 and ny >= 0 and 
               nx < #grid and ny < #grid[1] and 
               grid[nx+1][ny+1] == 0 and 
               not closedSet[keyNeighbor] then
                
                table.insert(openList, {
                    x = nx, 
                    y = ny, 
                    g = current.g + 1, 
                    h = calculateHeuristic(heuristicType, {x=nx, y=ny}, goal), 
                    f = current.g + 1 + calculateHeuristic(heuristicType, {x=nx, y=ny}, goal), 
                    parent = current
                })
            end
        end
    end
    return nil
end

-- Example usage
local grid = {
    {0, 0, 0, 0, 0},
    {0, 1, 1, 0, 0},
    {0, 0, 0, 0, 0},
    {0, 0, 1, 1, 0},
    {0, 0, 0, 0, 0}
}

local start = {x=0, y=0}
local goal = {x=4, y=4}

local fourWayPath = aStarSearch(grid, start, goal, HeuristicType.Manhattan, MoveDirection.FourWay)
local eightWayPath = aStarSearch(grid, start, goal, HeuristicType.Diagonal, MoveDirection.EightWay)

if fourWayPath then
    print("4-Direction Path found:")
    for _, p in ipairs(fourWayPath) do
        print("(" .. p.x .. ", " .. p.y .. ")")
    end
else
    print("No 4-Direction path found")
end

if eightWayPath then
    print("8-Direction Path found:")
    for _, p in ipairs(eightWayPath) do
        print("(" .. p.x .. ", " .. p.y .. ")")
    end
else
    print("No 8-Direction path found")
end
  • 标题: 【游戏算法】 - 寻路算法-A星寻路
  • 作者: 宋
  • 创建于 : 2020-10-19 20:22:05
  • 更新于 : 2021-03-15 20:30:07
  • 链接: https://sxl-space.tk/2020/10/19/002_Find-Path-2-AStar/
  • 版权声明: 版权所有 © 宋，禁止转载。
目录
【游戏算法】 - 寻路算法-A星寻路
  1. 1. A星算法的核心思想
  2. 2. 算法原理与关键概念
  3. 3. 算法实现