AStar寻路算法

这篇具有很好参考价值的文章主要介绍了AStar寻路算法。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

概述

AStar算法是一种图形搜索算法,常用于寻路。他是以广度优先搜索为基础,集Dijkstra算法和最佳优先(best fit)于一身的一种算法。

示例1:4向
astar算法,算法,算法

示例2:8向
astar算法,算法,算法

思路

astar算法,算法,算法
递归的通过估值函数找到最佳路径,估值函数与距离相关,也有可能与通过代价系数相关(例如平地系数为1,坡地系数为2),有三个参数:文章来源地址https://www.toymoban.com/news/detail-601263.html

  • G:起点点到当前点的代价
  • H: 当前点到终点的代价
  • F: F = G + H 与最佳路径权重负相关的参数
    过程大概:
    astar算法,算法,算法

代码示例

位置定义

public struct Vec2
{
    public int x;
    public int y;

    public Vec2(int x, int y)
    {
        this.x = x;
        this.y = y;
    }

    public static Vec2 Zero
    {
        get
        {
            return new Vec2(0, 0);
        }
    }

    public override bool Equals(object obj)
    {
        if (!(obj is Vec2))
            return false;

        var o = (Vec2)obj;
        return x == o.x && y == o.y;
    }

    public override int GetHashCode()
    {
        return x.GetHashCode() + y.GetHashCode();
    }

    public static Vec2 operator +(Vec2 a, Vec2 b)
    {
        return new Vec2(a.x + b.x, a.y + b.y);
    }

    public static Vec2 operator *(Vec2 a, int n)
    {
        return new Vec2(a.x * n, a.y * n);
    }

    public static Vec2 operator *(int n, Vec2 a)
    {
        return new Vec2(a.x * n, a.y * n);
    }

    public static bool operator ==(Vec2 a, Vec2 b)
    {
        return a.x == b.x && a.y == b.y;
    }

    public static bool operator !=(Vec2 a, Vec2 b)
    {
        return !(a.x == b.x && a.y == b.y);
    }
}

方向定义

public enum EDir
{
    Up = 0,
    Down = 1,
    Left = 2,
    Right = 3,
    UpLeft = 4,
    UpRight = 5,
    DownLeft = 6,
    DownRight = 7,
}

public abstract class CheckDirPol
{
    abstract public Dictionary<EDir, Vec2> GetDir();
}

public class CheckDir4Pol : CheckDirPol
{
    private Dictionary<EDir, Vec2> dirDict = new Dictionary<EDir, Vec2>
    {
        {EDir.Up, new Vec2(0, 1) },
        {EDir.Down, new Vec2(0, -1) },
        {EDir.Left, new Vec2(-1, 0) },
        {EDir.Right, new Vec2(1, 0) },
    };
    override public Dictionary<EDir, Vec2> GetDir()
    {
        return dirDict;
    }
}

public class CheckDir8Pol : CheckDirPol
{
    private Dictionary<EDir, Vec2> dirDict = new Dictionary<EDir, Vec2>
    {
        {EDir.Up, new Vec2(0, 1) },
        {EDir.Down, new Vec2(0, -1) },
        {EDir.Left, new Vec2(-1, 0) },
        {EDir.Right, new Vec2(1, 0) },
        {EDir.UpLeft, new Vec2(-1, 1) },
        {EDir.UpRight, new Vec2(1, 1) },
        {EDir.DownLeft, new Vec2(-1, -1) },
        {EDir.DownRight, new Vec2(1, -1) },

    };
    override public Dictionary<EDir, Vec2> GetDir()
    {
        return dirDict;
    }
}
  • 运用策略模式的技巧,以实现4向,8向搜索切换

估值函数

public abstract class EvaPol
{
	abstract public float Calc(Vec2 a, Vec2 b);
}

public class MANEvaPol : EvaPol
{
    override public float Calc(Vec2 a, Vec2 b)
    {
        return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y);
    }
}
  • 直接使用曼哈顿距离作为代价

节点定义

public class Node
{
    public int id;
    public Vec2 pos;
    public float g;
    public float h;
    public float f;
    public Vec2 prePos;
    public bool hasPrePos;

    public Node(Vec2 pos)
    {
        this.pos = pos;
    }

    public void SetPrePos(Vec2 pos)
    {
        prePos = pos;
        hasPrePos = true;
    }
}

算法上下文定义

Context context;
EvaPol disPol;
CheckDirPol checkDirPol;

public struct Context
{
    public Vec2 end;
    public Vec2 start;
    public Node[,] nodes;
    public List<Node> open;
    public List<Node> close;
    public int[,] map;
    public List<Vec2> result;
    public Vec2 size;
}

寻路算法

初始化

public void Init(Vec2 start, Vec2 end, int[,] map)
{
	var x = map.GetLength(0);
	var y = map.GetLength(1);
	context = new Context()
	{
		start = start,
		end = end,
		open = new List<Node>(),
		close = new List<Node>(),
		map = map,
		result = new List<Vec2>(),
		size = new Vec2(x, y),
	};

	context.nodes = new Node[x, y];
	for (int i = 0; i < x; i++)
		for (int j = 0; j < x; j++)
			context.nodes[i, j] = new Node(new Vec2(i, j));

	disPol = new MANEvaPol();
	//checkDirPol = new CheckDir4Pol();
	checkDirPol = new CheckDir8Pol();
}

获取路径

public List<Vec2> GetResult()
{
	return context.result;
}

寻路

寻路入口
public void FindPath()
{
	var s = context.start;
	var sn = context.nodes[s.x, s.y];
	sn.g = 0;
	sn.h = disPol.Calc(s, context.end);
	sn.f = sn.g + sn.h;
	context.open.Add(sn);

	FindArrangement(sn);
}
递归函数
void FindArrangement(Node node)
{
	context.close.Add(node);
	context.open.Remove(node);

	if (node.pos == context.end)
	{
		SetResult(node);
		return;
	}

	CheckRound(node);
	if (context.open.Count == 0)
		return;

	Node next = context.open[0];
	for (int i = 1; i < context.open.Count; i++)
		if (context.open[i].f < next.f)
			next = context.open[i];

	FindArrangement(next);
}
检查周围节点
void CheckRound(Node node)
{
	var dirDict = checkDirPol.GetDir();
	foreach (var pair in dirDict)
	{
		var dir = node.pos + pair.Value;

		if (IsBlock(dir))
			continue;
		var dn = context.nodes[dir.x, dir.y];

		if (context.close.Contains(dn))
			continue;

		if (context.open.Contains(dn))
			TryOverridePath(node, dn);
		else
		{
			dn.g = disPol.Calc(dn.pos, context.start);
			dn.h = disPol.Calc(dn.pos, context.end);
			dn.f = dn.g + dn.h;
			dn.SetPrePos(node.pos);
			context.open.Add(dn);
		}
	}
}

// 若是从邻节点到该节点路径更优,则替换更新
void TryOverridePath(Node a, Node b)
{
	var g = a.g + disPol.Calc(a.pos, b.pos);
	if (g < b.g)
	{
		b.g = g;
		b.SetPrePos(a.pos);
	}
}

bool IsBlock(Vec2 pos)
{
	return !InMap(pos) || context.map[pos.x, pos.y] == 1;
}

bool InMap(Vec2 pos)
{
	var x = pos.x;
	var y = pos.y;
	var size = context.size;
	return x >= 0 && x < size.x && y >= 0 && y < size.y;
}
生成路径
void SetResult(Node node)
{
	Queue<Node> q = new Queue<Node>();
	while(node.hasPrePos)
	{
		q.Enqueue(node);
		node = context.nodes[node.prePos.x, node.prePos.y];
	}
	while(q.Count > 0)
	{
		context.result.Add(q.Dequeue().pos);
	}
}

完整代码

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

public struct Vec2
{
    public int x;
    public int y;

    public Vec2(int x, int y)
    {
        this.x = x;
        this.y = y;
    }

    public static Vec2 Zero
    {
        get
        {
            return new Vec2(0, 0);
        }
    }

    public override bool Equals(object obj)
    {
        if (!(obj is Vec2))
            return false;

        var o = (Vec2)obj;
        return x == o.x && y == o.y;
    }

    public override int GetHashCode()
    {
        return x.GetHashCode() + y.GetHashCode();
    }

    public static Vec2 operator +(Vec2 a, Vec2 b)
    {
        return new Vec2(a.x + b.x, a.y + b.y);
    }

    public static Vec2 operator *(Vec2 a, int n)
    {
        return new Vec2(a.x * n, a.y * n);
    }

    public static Vec2 operator *(int n, Vec2 a)
    {
        return new Vec2(a.x * n, a.y * n);
    }

    public static bool operator ==(Vec2 a, Vec2 b)
    {
        return a.x == b.x && a.y == b.y;
    }

    public static bool operator !=(Vec2 a, Vec2 b)
    {
        return !(a.x == b.x && a.y == b.y);
    }
}

public enum EDir
{
    Up = 0,
    Down = 1,
    Left = 2,
    Right = 3,
    UpLeft = 4,
    UpRight = 5,
    DownLeft = 6,
    DownRight = 7,
}

public class AstarFindPath
{
    public class Node
    {
        public int id;
        public Vec2 pos;
        public float g;
        public float h;
        public float f;
        public Vec2 prePos;
        public bool hasPrePos;

        public Node(Vec2 pos)
        {
            this.pos = pos;
        }

        public void SetPrePos(Vec2 pos)
        {
            prePos = pos;
            hasPrePos = true;
        }
    }

    public abstract class EvaPol
    {
        abstract public float Calc(Vec2 a, Vec2 b);
    }

    public class MANEvaPol : EvaPol
    {
        override public float Calc(Vec2 a, Vec2 b)
        {
            return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y);
        }
    }

    public abstract class CheckDirPol
    {
        abstract public Dictionary<EDir, Vec2> GetDir();
    }

    public class CheckDir4Pol : CheckDirPol
    {
        private Dictionary<EDir, Vec2> dirDict = new Dictionary<EDir, Vec2>
        {
            {EDir.Up, new Vec2(0, 1) },
            {EDir.Down, new Vec2(0, -1) },
            {EDir.Left, new Vec2(-1, 0) },
            {EDir.Right, new Vec2(1, 0) },
        };
        override public Dictionary<EDir, Vec2> GetDir()
        {
            return dirDict;
        }
    }

    public class CheckDir8Pol : CheckDirPol
    {
        private Dictionary<EDir, Vec2> dirDict = new Dictionary<EDir, Vec2>
        {
            {EDir.Up, new Vec2(0, 1) },
            {EDir.Down, new Vec2(0, -1) },
            {EDir.Left, new Vec2(-1, 0) },
            {EDir.Right, new Vec2(1, 0) },
            {EDir.UpLeft, new Vec2(-1, 1) },
            {EDir.UpRight, new Vec2(1, 1) },
            {EDir.DownLeft, new Vec2(-1, -1) },
            {EDir.DownRight, new Vec2(1, -1) },

        };
        override public Dictionary<EDir, Vec2> GetDir()
        {
            return dirDict;
        }
    }

    public struct Context
    {
        public Vec2 end;
        public Vec2 start;
        public Node[,] nodes;
        public List<Node> open;
        public List<Node> close;
        public int[,] map;
        public List<Vec2> result;
        public Vec2 size;
    }

    Context context;
    EvaPol disPol;
    CheckDirPol checkDirPol;

    public void Init(Vec2 start, Vec2 end, int[,] map)
    {
        var x = map.GetLength(0);
        var y = map.GetLength(1);
        context = new Context()
        {
            start = start,
            end = end,
            open = new List<Node>(),
            close = new List<Node>(),
            map = map,
            result = new List<Vec2>(),
            size = new Vec2(x, y),
        };
        
        context.nodes = new Node[x, y];
        for (int i = 0; i < x; i++)
            for (int j = 0; j < x; j++)
                context.nodes[i, j] = new Node(new Vec2(i, j));

        disPol = new MANEvaPol();
        //checkDirPol = new CheckDir4Pol();
        checkDirPol = new CheckDir8Pol();
    }

    public void FindPath()
    {
        var s = context.start;
        var sn = context.nodes[s.x, s.y];
        sn.g = 0;
        sn.h = disPol.Calc(s, context.end);
        sn.f = sn.g + sn.h;
        context.open.Add(sn);
        
        FindArrangement(sn);
    }

    public List<Vec2> GetResult()
    {
        return context.result;
    }

    void FindArrangement(Node node)
    {
        context.close.Add(node);
        context.open.Remove(node);

        if (node.pos == context.end)
        {
            SetResult(node);
            return;
        }

        CheckRound(node);
        if (context.open.Count == 0)
            return;

        Node next = context.open[0];
        for (int i = 1; i < context.open.Count; i++)
            if (context.open[i].f < next.f)
                next = context.open[i];
        
        FindArrangement(next);
    }

    void SetResult(Node node)
    {
        Queue<Node> q = new Queue<Node>();
        while(node.hasPrePos)
        {
            q.Enqueue(node);
            node = context.nodes[node.prePos.x, node.prePos.y];
        }
        while(q.Count > 0)
        {
            context.result.Add(q.Dequeue().pos);
        }
    }

    void CheckRound(Node node)
    {
        var dirDict = checkDirPol.GetDir();
        foreach (var pair in dirDict)
        {
            var dir = node.pos + pair.Value;

            if (IsBlock(dir))
                continue;
            var dn = context.nodes[dir.x, dir.y];

            if (context.close.Contains(dn))
                continue;

            if (context.open.Contains(dn))
                TryOverridePath(node, dn);
            else
            {
                dn.g = disPol.Calc(dn.pos, context.start);
                dn.h = disPol.Calc(dn.pos, context.end);
                dn.f = dn.g + dn.h;
                dn.SetPrePos(node.pos);
                context.open.Add(dn);
            }
        }
    }

    void TryOverridePath(Node a, Node b)
    {
        var g = a.g + disPol.Calc(a.pos, b.pos);
        if (g < b.g)
        {
            b.g = g;
            b.SetPrePos(a.pos);
        }
    }

    bool IsBlock(Vec2 pos)
    {
        return !InMap(pos) || context.map[pos.x, pos.y] == 1;
    }

    bool InMap(Vec2 pos)
    {
        var x = pos.x;
        var y = pos.y;
        var size = context.size;
        return x >= 0 && x < size.x && y >= 0 && y < size.y;
    }
}

到了这里,关于AStar寻路算法的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • Unity 中 A*寻路(AStar,A星)的优化,二叉堆,双向队列,哈希表

    前篇:A星寻路的简单实现 A星寻路,在2D地图下使用频率较高 本篇基于上一篇文章实现的A星寻路进一步优化。利用二叉堆代替了原先openList的数据结构, 改进了path返回时的操作,以及在搜索时的性能开销。 c#中的Sort函数,在实现方面采用的是快速排序。 在日常的使用上,好

    2024年02月03日
    浏览(40)
  • 自动驾驶路径规划——A*(Astar)算法

         最佳优先搜索(BFS) ,又称A算法,是一种启发式搜索算法(Heuristic Algorithm)。[不是广度优先搜索算法( Breadth First Search , BFS )]     BFS算法在广度优先搜索的基础上, 用启发估价函数对将要被遍历到的点进行估价 ,然后选择代价小的进行遍历,直到找到目标节点

    2024年02月01日
    浏览(44)
  • 【Unity】一篇文章搞定AStar(A*)算法

    AStar(A*)算法,是一种在静态网格中求解最短路径直接有效的搜索方法。在游戏开发中,A*算法常应用于部分RPG游戏和策略战棋类游戏。对于Unity开发者来说,掌握A*算法也是十分有必要的。不过在了解A*算法之前,有必要先回顾一下深度优先算法(DFS)、广度优先算法(BFS)

    2024年02月02日
    浏览(54)
  • 【ROS-Navigation】—— Astar路径规划算法解析

        最近在学习ROS的navigation部分,写些东西作为笔记,方便理解与日后查看。本文从Astar算法入手,对navigation源码进行解析。 PS:ros navigation源码版本https://gitcode.net/mirrors/ros-planning/navigation/-/tree/noetic-devel     对于Astar算法,想必大家都非常熟悉了。具体算法原理就不

    2024年01月18日
    浏览(46)
  • C#,人工智能,机器人,路径规划,A*(AStar Algorithm)算法、源代码及计算数据可视化

    Peter Hart  Nils Nilsson  Bertram Raphael  参考: C#,人工智能(AI)机器人路径规划(Path Planning)的ARA*(Anytime Replanning A* Algorithm)算法与源程序 https://blog.csdn.net/beijinghorn/article/details/125464754 A*算法最初由斯坦福研究院(Stanford Institute)的  Peter Hart,Nils Nilsson,Bertram Raphael  发表于

    2024年01月18日
    浏览(64)
  • 算法修养--A*寻路算法

    广度优先算法搜索以广度做为优先级进行搜索。 从起点开始,首先遍历起点周围邻近的点,然后再遍历已经遍历过的点邻近的点,逐步的向外扩散,直到找到终点。 这种算法就像洪水(Flood fill)一样向外扩张。直至洪水将整张地图都漫延。在访问节点时候,每个点需要记录

    2024年02月08日
    浏览(26)
  • A*寻路算法

    深度优先算法(BFS)在所有方向上平等的探索,是最好写的寻路算法 Dijkstra算法优先考虑低成本的路径,在游戏中可以规定远离敌人、避开森林需要更高的成本,所以当移动成本发生变化的时候我们使用该算法而不是BFS A*寻路是对Dijkstra算法的修改,针对单个目标时进行了优

    2023年04月26日
    浏览(31)
  • Unity寻路A星算法

    在Unity中实现A星(A*,A-Star)算法是一种用于寻找两点之间最短路径的广泛应用的技术。该算法结合了启发式搜索与图论中的Dijkstra算法,通过评估每个节点到起点和终点的成本来确定最优路径。 以下是Unity中使用A*寻路算法的一个简要步骤和实例: 实现步骤概览: 构建网格

    2024年01月17日
    浏览(36)
  • 探索迷局:解密广度寻路算法

    ================================ 专栏文章,自下而上 数据结构与算法——二叉搜索树 数据结构与算法——深度寻路算法 数据结构与算法——二叉树+实现表达式树 数据结构与算法——树(三指针描述一棵树) 数据结构与算法——栈和队列<也不过如此> 数据结构与算法——八大经

    2024年02月06日
    浏览(30)
  • “ 探索迷局:解密广度寻路算法 “

    ================================ 专栏文章,自下而上 数据结构与算法——二叉搜索树 数据结构与算法——深度寻路算法 数据结构与算法——二叉树+实现表达式树 数据结构与算法——树(三指针描述一棵树) 数据结构与算法——栈和队列<也不过如此> 数据结构与算法——八大经

    2024年02月04日
    浏览(32)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包