C#进阶12：C#全局路径规划算法_Dijkstra

葛小白12025-11-17 10:05

本节目标：

1）了解全局路径规划算法Dijkstra的基本原理；

2）使用C#实现Dijkstra算法；

3）使用Winform对Dijkstra算法进行仿真，更直观理解Dijkstra算法；

下面是Dijkstra算法的Winform实现结果，运行程序后，任意点击一个位置，该位置作为开始点，再点击一个位置作为结束点，然后程序会自动从开始点向外搜索，直到搜索到结束点后停止，然后取其中距离最短的路径作为最优路径；

运行环境：

VS2013（.net framework 4.5）

1.1 算法原理

Dijkstra是一种带权图BFS（广度优先搜索）+贪心策略，解决带非负权有向图或无向图 中单源最短路径（SSSP）问题的经典贪心算法。本节演示的是非网络的搜索示例，搜索的边长为2，斜对角线为3，如下图所示：

算法的基本流程是：

1）新建两个点集合CloseList和OpenList，CloseList用于存放搜索过的点，OpenList用于存放待向目标点搜索的点；

2）初始化两个集合，将开始点放入CloseList，将开始点周边未有障碍物的点放入OpenList；

3）先判断OpenList是否为空，为空结束未找到路径，不为空，执行第4步；

4）取OpenList中第一个路径值最小的点，将其放入CloseList中，判断其是不是目标点，如果是结束，将其路径还原出来，绘制在界面上，如果不是执行第5步；

5）依次周边的8个点搜索，判断是不是障碍物以及在没在CloseList中，如果都不是将其放入OpenList中，返回执行第3步；

流程图如下：

1.2 实现

根据上面对Dijkstra算法的分析，进行C#程序设计，程序流程图如下；

1.2.1 变量

Form1中的变量

cs 复制代码 
int paintpix;
Point[] obstacleStart = new Point[100], obstacleEnd = new Point[100];
bool bStartPointFlg;
bool bGoalPointFlg;
Point Startpoint;
Point Goalpoint;
NavDijkstra NavDiAs = new NavDijkstra();
int upvalue;

1.2.2 障碍物初始化

cs 复制代码 
void Init()
{
    //Dijkstra,Astar
    paintpix = 20;
    obstacleStart[0].X = (1 * paintpix);
    obstacleStart[0].Y = (2 * paintpix);
    obstacleEnd[0].X = (2 * paintpix);
    obstacleEnd[0].Y = (23 * paintpix);
    obstacleStart[1].X = (38 * paintpix);
    obstacleStart[1].Y = (2 * paintpix);
    obstacleEnd[1].X = (39 * paintpix);
    obstacleEnd[1].Y = (23 * paintpix);
    obstacleStart[2].X = (2 * paintpix);
    obstacleStart[2].Y = (2 * paintpix);
    obstacleEnd[2].X = (38 * paintpix);
    obstacleEnd[2].Y = (3 * paintpix);
    obstacleStart[3].X = (2 * paintpix);
    obstacleStart[3].Y = (22 * paintpix);
    obstacleEnd[3].X = (38 * paintpix);
    obstacleEnd[3].Y = (23 * paintpix);
    obstacleStart[4].X = (24 * paintpix);
    obstacleStart[4].Y = (2 * paintpix);
    obstacleEnd[4].X = (25 * paintpix);
    obstacleEnd[4].Y = (16 * paintpix);
    obstacleStart[5].X = (12 * paintpix);
    obstacleStart[5].Y = (10 * paintpix);
    obstacleEnd[5].X = (13 * paintpix);
    obstacleEnd[5].Y = (23 * paintpix);
    NavDiAs.OnSignal += this.reponsefunc;
    upvalue = 0;
}

1.2.3 重绘

cs 复制代码 
protected override void OnPaint(PaintEventArgs e)
{
    base.OnPaint(e);
    Graphics g = e.Graphics;
    Point xx = new Point(), yy = new Point();
    //开始点
    if (bStartPointFlg)
    {
        //Point xx = new Point(), yy = new Point();
        xx.X = ((Startpoint.X / paintpix)*paintpix);
        xx.Y = ((Startpoint.Y / paintpix)*paintpix);
        yy.X = ((Startpoint.X / paintpix)*paintpix + paintpix);
        yy.Y = ((Startpoint.Y / paintpix)*paintpix + paintpix);
        rect(xx, yy, Color.Red,g);
    }

    //障碍物
    for (int m = 0; m <= 5; m++)
        rect(obstacleStart[m], obstacleEnd[m], Color.Black,g);
    //路过点显示
    if (NavDiAs.bPassPointShowFlg)
    {
        for (int k = 0; k<NavDiAs.iPassNum; k++)
        {
            //Point xx = new Point(), yy = new Point();
            xx.X = (NavDiAs.pPassPoint[k].X*paintpix);
            xx.Y = (NavDiAs.pPassPoint[k].Y*paintpix);
            yy.X = (NavDiAs.pPassPoint[k].X*paintpix + paintpix);
            yy.Y = (NavDiAs.pPassPoint[k].Y*paintpix + paintpix);
            rect(xx, yy, Color.YellowGreen, g);
        }
    }
    if (NavDiAs.bFinalPassShowFlg)
    {
        for (int k = 0; k<NavDiAs.iFinalPassNum; k++)
        {
            
            xx.X = (NavDiAs.pFinalPassPoint[k].X*paintpix);
            xx.Y = (NavDiAs.pFinalPassPoint[k].Y*paintpix);
            yy.X = (NavDiAs.pFinalPassPoint[k].X*paintpix + paintpix);
            yy.Y = (NavDiAs.pFinalPassPoint[k].Y*paintpix + paintpix);
            rect(xx, yy, Color.Blue,g);
        }
    }
    //结束点
    if (bGoalPointFlg)
    {
        //Point xx = new Point(), yy = new Point();
        xx.X=((Goalpoint.X / paintpix)*paintpix);
        xx.Y=((Goalpoint.Y / paintpix)*paintpix);
        yy.X=((Goalpoint.X / paintpix)*paintpix + paintpix);
        yy.Y=((Goalpoint.Y / paintpix)*paintpix + paintpix);
        rect(xx, yy, Color.Green, g);
    }
}
void rect(Point startp, Point endp, Color color,Graphics g)
{
    SolidBrush brush = new SolidBrush(color);
    g.FillRectangle(brush, startp.X, startp.Y, Math.Abs(endp.X - startp.X), Math.Abs(endp.Y - startp.Y));
}

void reponsefunc() //槽函数
{
    this.Invalidate();
    //update();
}

1.2.4 鼠标事件

cs 复制代码 
private void Form1_MouseClick(object sender, MouseEventArgs e)
{
    if (e.Button == MouseButtons.Left && upvalue == 0)
    {
        Startpoint.X = (e.X);
        Startpoint.Y = (e.Y);
        bStartPointFlg = true;
        upvalue = 1;
        this.Invalidate();
    }
    else if (e.Button == MouseButtons.Left && upvalue == 1)
    {
        Goalpoint.X = (e.X);
        Goalpoint.Y = (e.Y);
        bGoalPointFlg = true;
        upvalue = 2;
        
        NavDiAs.startpoint.X = Startpoint.X / paintpix;
        NavDiAs.startpoint.Y = Startpoint.Y / paintpix;
        NavDiAs.goalpoint.X = Goalpoint.X / paintpix;
        NavDiAs.goalpoint.Y = Goalpoint.Y / paintpix;
        for (int m = 0; m <= 5; m++)
        {
            NavDiAs.obstacleStart[m].X = obstacleStart[m].X / paintpix;
            NavDiAs.obstacleStart[m].Y = obstacleStart[m].Y / paintpix;
            NavDiAs.obstacleEnd[m].X = obstacleEnd[m].X / paintpix;
            NavDiAs.obstacleEnd[m].Y = obstacleEnd[m].Y / paintpix;
        }
        StartDijkstra();
    }
    else if (e.Button == MouseButtons.Left && upvalue == 2)
    {
        bStartPointFlg = false;
        bGoalPointFlg = false;
        NavDiAs.bPassPointShowFlg = false;
        NavDiAs.bFinalPassShowFlg = false;
        upvalue = 0;
        this.Invalidate();
    }
}

void StartDijkstra()
{
    Task.Run(() =>
    {
        NavDiAs.Nva_init();
        NavDiAs.Dijkstra();
    });
}

1.2.5 算法

算法的原理分析见1.1节。

NavDijkstra.cs

cs 复制代码 
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Winform_Dijkstra
{
    class NavDijkstra
    {
        public bool bPassPointShowFlg = false;//
        public Point[] pFinalPassPoint = new Point[200];
        public int iPassNum = 0;
        public Point[] pPassPoint = new Point[2000];
        public bool bFinalPassShowFlg = false;//Dijkstra&Astar最终路径
        public int iFinalPassNum = 0;
        public event Action OnSignal;//触发更新

        public Point[] obstacleStart = new Point[10], obstacleEnd = new Point[10];
        public Point goalpoint, startpoint;
        int openlistnum = 0;
        int closelistnum = 0;
        liststruct openlist = new liststruct(1000);
        liststruct closelist = new liststruct(1000);
        static int delaytime = 10; //延迟时间

        public struct liststruct
        {
            //public const int num = 1000;
            public int[] f;
            public Point[] parrant;
            public Point[] curent;
            public int[] value;//路径值
            public string[] path;//记录路径
            public liststruct(int size)
            {
                f = new int[size];
                parrant = new Point[size];
                curent = new Point[size];
                value = new int[size];
                path = new string[size];
            }
        }        
        public void Dijkstra()
        {	

	        Point curentpoint = new Point();//中心点
	        int curentvalue;//中心点路程值
	        string curentpath;//中心点路程
	        int minvalue,minnum;
	        while (openlistnum!=0)
	        {
		        //取openlist中路径最小的点为中心点
		        minvalue = openlist.value[0];
		        minnum = 0;
		        for(int i=1;i<openlistnum;i++)
		        {
			        if (minvalue>openlist.value[i])
			        {
				        minvalue=openlist.value[i];
				        minnum = i;
			        }
		        }
		        //将最小路径的点放入closelist中
		        closelist.curent[closelistnum]=openlist.curent[minnum];
		        closelist.f[closelistnum]=openlist.f[minnum];
		        closelist.parrant[closelistnum]=openlist.parrant[minnum];
		        closelist.value[closelistnum]=openlist.value[minnum];
		        closelist.path[closelistnum]=openlist.path[minnum];
		        closelistnum++;

		        curentpoint.X = openlist.curent[minnum].X;
		        curentpoint.Y = openlist.curent[minnum].Y;
		        curentvalue = openlist.value[minnum];
		        curentpath = openlist.path[minnum];
		        //删掉openlist中的最小路径的点
		        for(int i=minnum;i<openlistnum;i++)
		        {
			        openlist.curent[i]=openlist.curent[i+1];
			        openlist.f[i]=openlist.f[i+1];
			        openlist.parrant[i]=openlist.parrant[i+1];
			        openlist.value[i] = openlist.value[i+1];
			        openlist.path[i] = openlist.path[i+1];
		        }
		        openlistnum--;
		        //如果中心点是目标点,结束
		        if (curentpoint.X==goalpoint.X&&curentpoint.Y==goalpoint.Y)
		        {
			        bFinalPassShowFlg = true;
			        string aa;
			        Point pp = new Point();
			        pp = startpoint;
			        int length = curentpath.Length;
			        for(int j=0;j<length;j++)
			        {
				        aa = curentpath.Substring(j, 1);
				        if(aa=="A")
				        {
					        pp.X = pp.X-1;
					        pp.Y = pp.Y-1;
				        }
				        if(aa=="B")
				        {
					        pp.X = pp.X;
					        pp.Y = pp.Y-1;
				        }
				        if(aa=="C")
				        {
					        pp.X = pp.X+1;
					        pp.Y = pp.Y-1;
				        }
				        if(aa=="D")
				        {
					        pp.X = pp.X+1;
					        pp.Y = pp.Y;
				        }
				        if(aa=="E")
				        {
					        pp.X = pp.X+1;
					        pp.Y = pp.Y+1;
				        }
				        if(aa=="F")
				        {
					        pp.X = pp.X;
					        pp.Y = pp.Y+1;
				        }
				        if(aa=="G")
				        {
					        pp.X = pp.X-1;
					        pp.Y = pp.Y+1;
				        }
				        if(aa=="H")
				        {
					        pp.X = pp.X-1;
					        pp.Y = pp.Y;
				        }
				        pFinalPassPoint[iFinalPassNum] = pp;
				        iFinalPassNum++;
				        //emit ASignal();
                        OnSignal.Invoke();
			        }
			        break;
		        }
		        else
		        {
			        ABCDEFGH(curentpoint,curentvalue,"B",curentpath);
			        ABCDEFGH(curentpoint,curentvalue,"D",curentpath);
			        ABCDEFGH(curentpoint,curentvalue,"F",curentpath);
			        ABCDEFGH(curentpoint,curentvalue,"H",curentpath);
			        ABCDEFGH(curentpoint,curentvalue,"A",curentpath);
			        ABCDEFGH(curentpoint,curentvalue,"C",curentpath);
			        ABCDEFGH(curentpoint,curentvalue,"E",curentpath);
			        ABCDEFGH(curentpoint,curentvalue,"G",curentpath);		
		        }
                System.Threading.Thread.Sleep(delaytime);	
	        }
        }
        void ABCDEFGH(Point centerpoint,int curentvalue,string str,string centerpath)
        {
	        bool isclose = false;//在closelist中
	        bool isopen = false;//在openlist中
	        Point targetpoint = new Point();
	        int centervalue=0;
	        string targetpath;
	        /*
	        A B C
	        H S D
	        G F E
	        */
	        targetpath = centerpath + str;
	        if (str=="B")
	        {
		        targetpoint.X=centerpoint.X;
		        targetpoint.Y=centerpoint.Y-1;
		        centervalue = curentvalue+2;
	        }
	        if (str=="D")
	        {
		        targetpoint.X=centerpoint.X+1;
		        targetpoint.Y=centerpoint.Y;
		        centervalue = curentvalue+2;
	        }
	        if (str=="F")
	        {
		        targetpoint.X=centerpoint.X;
		        targetpoint.Y=centerpoint.Y+1;
		        centervalue = curentvalue+2;
	        }
	        if (str=="H")
	        {
		        targetpoint.X=centerpoint.X-1;
		        targetpoint.Y=centerpoint.Y;
		        centervalue = curentvalue+2;
	        }
	        if (str=="A")
	        {
		        targetpoint.X=centerpoint.X-1;
		        targetpoint.Y=centerpoint.Y-1;
		        centervalue = curentvalue+3;
	        }
	        if (str=="C")
	        {
		        targetpoint.X=centerpoint.X+1;
		        targetpoint.Y=centerpoint.Y-1;
		        centervalue = curentvalue+3;
	        }
	        if (str=="E")
	        {
		        targetpoint.X=centerpoint.X+1;
		        targetpoint.Y=centerpoint.Y+1;
		        centervalue = curentvalue+3;
	        }
	        if (str=="G")
	        {
		        targetpoint.X=centerpoint.X-1;
		        targetpoint.Y=centerpoint.Y+1;
		        centervalue = curentvalue+3;
	        }
	        for(int i=0;i<closelistnum;i++)
	        {
		        if((targetpoint.X==closelist.curent[i].X)&&(targetpoint.Y==closelist.curent[i].Y))
			        isclose = true;
	        }
	        //碰撞检测
	        bool ishit=false;

	        if(!isclose&&targetpoint.X>=0&&targetpoint.Y>=0 &&!obstacleTest(targetpoint))
	        {
		        int isopennum = 0;
		        isopen = false;
		        for(int i=0;i<openlistnum;i++)
		        {
			        if((targetpoint.X==openlist.curent[i].X)&&(targetpoint.Y==openlist.curent[i].Y))
			        {
				        isopen = true;
				        isopennum = i;
			        }
		        }
		        if(!isopen)
		        {
			        openlist.f[openlistnum]=0;
			        openlist.parrant[openlistnum].X = centerpoint.X;
			        openlist.parrant[openlistnum].Y = centerpoint.Y;
			        openlist.curent[openlistnum].X = targetpoint.X;
			        openlist.curent[openlistnum].Y = targetpoint.Y;
			        openlist.value[openlistnum] = centervalue;
			        openlist.path[openlistnum] = targetpath;
			        pPassPoint[iPassNum] = targetpoint;
			        iPassNum ++;
                    OnSignal.Invoke();
			        openlistnum ++;
		        }
		        else
		        {
			        if(centervalue<openlist.value[isopennum])
			        {
				        openlist.f[isopennum]=0;
				        openlist.parrant[isopennum].X = centerpoint.X;
				        openlist.parrant[isopennum].Y = centerpoint.Y;
				        openlist.curent[isopennum].X = targetpoint.X;
				        openlist.curent[isopennum].Y = targetpoint.Y;
				        openlist.value[isopennum] = centervalue;
				        openlist.path[openlistnum] = targetpath;
			        }
		        }
	        }
        }
        bool obstacleTest(Point targetpoint) //碰撞检测
        {
	        bool ishit=false;
	        for(int m=0;m<=5;m++)
	        {
		        if(targetpoint.X>=obstacleStart[m].X && targetpoint.X<obstacleEnd[m].X 
			        && targetpoint.Y>=obstacleStart[m].Y && targetpoint.Y<obstacleEnd[m].Y)
			        ishit = true;
	        }
	        return ishit;
        }
        public void Nva_init()
        {
        //	point aroundpoint;//四周目标点
	        openlistnum = 0;
	        closelistnum = 0;
	        bPassPointShowFlg = true;
	        iPassNum = 0;
	        iFinalPassNum = 0;
	        ABCDEFGH_Init(startpoint,"B");
	        ABCDEFGH_Init(startpoint,"D");
	        ABCDEFGH_Init(startpoint,"F");
	        ABCDEFGH_Init(startpoint,"G");
	        ABCDEFGH_Init(startpoint,"A");
	        ABCDEFGH_Init(startpoint,"C");
	        ABCDEFGH_Init(startpoint,"E");
	        ABCDEFGH_Init(startpoint,"H");

	        closelist.curent[closelistnum].X=startpoint.X;
	        closelist.curent[closelistnum].Y=startpoint.Y;
	        closelist.value[closelistnum]=0;
	        closelistnum++;
            
        }
        void ABCDEFGH_Init(Point centerpoint,string str)
        {
	        Point targetpoint = new Point();
	        int ab = 0;
	        if (str=="B")
	        {
		        targetpoint.X=centerpoint.X;
		        targetpoint.Y=centerpoint.Y-1;
		        ab = 2;
	        }
	        if (str=="D")
	        {
		        targetpoint.X=centerpoint.X+1;
		        targetpoint.Y=centerpoint.Y;
		        ab = 2;
	        }
	        if (str=="F")
	        {
		        targetpoint.X=centerpoint.X;
		        targetpoint.Y=centerpoint.Y+1;
		        ab = 2;
	        }
	        if (str=="H")
	        {
		        targetpoint.X=centerpoint.X-1;
		        targetpoint.Y=centerpoint.Y;
		        ab = 2;
	        }
	        if (str=="A")
	        {
		        targetpoint.X=centerpoint.X-1;
		        targetpoint.Y=centerpoint.Y-1;
		        ab = 3;
	        }
	        if (str=="C")
	        {
		        targetpoint.X=centerpoint.X+1;
		        targetpoint.Y=centerpoint.Y-1;
		        ab = 3;
	        }
	        if (str=="E")
	        {
		        targetpoint.X=centerpoint.X+1;
		        targetpoint.Y=centerpoint.Y+1;
		        ab = 3;
	        }
	        if (str=="G")
	        {
		        targetpoint.X=centerpoint.X-1;
		        targetpoint.Y=centerpoint.Y+1;
		        ab = 3;
	        }
	        //碰撞检测
	        //bool ishit = obstacleTest(targetpoint);
	        if (targetpoint.X>=0 && targetpoint.Y>=0 &&!obstacleTest(targetpoint))
	        {		
		        openlist.f[openlistnum]=ab+Manhattan(targetpoint,goalpoint);
		        openlist.parrant[openlistnum].X=startpoint.X;
		        openlist.parrant[openlistnum].Y=startpoint.Y;
		        openlist.curent[openlistnum].X=targetpoint.X;
		        openlist.curent[openlistnum].Y=targetpoint.Y;
		        openlist.value[openlistnum]=ab;
		        openlist.path[openlistnum]=str;
		        //rect(HWnd,hDC,NewBrush,ps,targetpoint,"yellow");
		        pPassPoint[iPassNum] = targetpoint;
		        iPassNum ++;
                OnSignal.Invoke();
		        //emit ASignal();
		        openlistnum++;
	        }
        }
        int Manhattan(Point p1, Point p2)
        {
	        int m;
            m = Math.Abs(p1.X - p2.X) + Math.Abs(p1.Y - p2.Y);
            return m*2 ;
        }

    }
}

1.3 运行

运行程序后，任意点击一个位置，该位置作为开始点，再点击一个位置作为结束点，然后程序会自动从开始点向外搜索，搜索期间遇到障碍物自动避开搜索，直到搜索到结束点后停止，然后取其中距离最短的路径作为最优路径。

本节的目的是为了介绍路径规划算法，并且使用代码实现算法，并将其直观的绘制出来，更容易理解。

相关推荐
前端小L3 小时前
图论专题(五)：图遍历的“终极考验”——深度「克隆图」
数据结构·算法·深度优先·图论·宽度优先
CoovallyAIHub3 小时前
超越像素的视觉：亚像素边缘检测原理、方法与实战
深度学习·算法·计算机视觉
CoovallyAIHub3 小时前
中科大西工大提出RSKT-Seg：精度速度双提升，开放词汇分割不再难
深度学习·算法·计算机视觉
gugugu.3 小时前
算法：位运算类型题目练习与总结
算法
百***97643 小时前
【语义分割】12个主流算法架构介绍、数据集推荐、总结、挑战和未来发展
算法·架构
代码不停3 小时前
Java分治算法题目练习(快速/归并排序)
java·数据结构·算法
yivifu3 小时前
精益求精，支持处理嵌套表格的Word表格转HTML表格
开发语言·c#·word
bubiyoushang8884 小时前
基于MATLAB的马尔科夫链蒙特卡洛（MCMC）模拟实现方法
人工智能·算法·matlab
玖剹4 小时前
穷举 VS 暴搜 VS 深搜 VS 回溯 VS 剪枝
c语言·c++·算法·深度优先·剪枝·深度优先遍历
热门推荐
01GitHub 镜像站点02UV安装并设置国内源03BongoCat - 跨平台键盘猫动画工具04安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）05Linux下V2Ray安装配置指南06《大数据技术原理与应用》实验报告三 熟悉HBase常用操作07综合整理：pdf预览显示：你尝试预览的文件可能对你的计算机有害。如果你信任此文件以及其来源，请打开此文件以看其内容，如何解决以正常预览文件08jdk21下载、安装（Windows、Linux、macOS）092025 最新教程：注册并切换到美区 Apple ID10智能库存管理的需求预测模型：从业务痛点到落地代码的完整实践