A* Algorithm

이전에 A* Algorithm에 대해 글을 쓴적이 있다.

그 때는 이론적인 부분만 설명을 했고

이번엔 직접 구현을 해서 설명을 하려한다.

아이디어는 이전 글을 참고하자.

http://keicoon15.tistory.com/entry/a-search

빨간 선의 왼쪽(시작 지점)과 오른쪽(끝 지점)을 기준으로 최단경로(A* Algorithm)의 결과이다.

이 때, 마우스로 검은색 사각형영역(장애물)을 생성하면 최단경로가 갱신된다.

프로그램은 c#에서 작성했으며

Pixel 단위로 영역을 정의했다.

F = G + H를 계산할 때는 다른 참고 글에서(가로, 세로 : 10, 대각선 : 14)를 사용했는데

이번 구현에서는 Distance를 직접 구했다(sqrt를 사용하므로 속도가 느리다)

차후 수정이 필요하지만 완성되었으므로 코드를 올린다.

거리를 계산할 때 맨하탄방식을 적용해서 수정함(2015.12.15)

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace AlgorithmTest
{
    public partial class Form1 : Form
    {
        public struct node
        {
            public float G;
            public Point self, parent;
            public node(float _G, Point P1, Point P2)
            {
                this.G = _G;
                this.self = P1; this.parent = P2;
            }
        }
        Point MouseDownPoint;
        private const int Width = 278, Height = 244;
        Point src, des;
        private Boolean[,] renderMAP;
        private Boolean[,] astarMAP;
        public Form1()
        {
            InitializeComponent();
        }
        private void Form1_MouseDown(object sender, MouseEventArgs e)
        {
            MouseDownPoint = new Point(e.X, Height - e.Y);
        }
        private void Form1_MouseUp(object sender, MouseEventArgs e)
        {
            int sX = (MouseDownPoint.X < e.X)?MouseDownPoint.X:e.X; 
            int bX = (MouseDownPoint.X > e.X)?MouseDownPoint.X:e.X;
            int sY = (MouseDownPoint.Y < Height - e.Y)?MouseDownPoint.Y:Height - e.Y;
            int bY = (MouseDownPoint.Y > Height - e.Y)?MouseDownPoint.Y:Height - e.Y;;
            
            for (int h = sY; h <= bY; h++)
                for (int w = sX; w <= bX; w++)
                    renderMAP[h, w] = true;
            A_Star();
            Invalidate();
        }
        private void Form1_Load(object sender, EventArgs e)
        {
            renderMAP = new Boolean[Height, Width];
            Array.Clear(renderMAP, 0, renderMAP.Length);
            astarMAP = new Boolean[Height, Width];
            Array.Clear(astarMAP, 0, astarMAP.Length);

            src = new Point(30, Height / 2);
            des = new Point(150, Height / 2);
            A_Star();
        }
        private void Form1_Paint(object sender, PaintEventArgs e)
        {
            Graphics gr = e.Graphics;
            Pen myPen = new Pen(Color.Black);
            Pen myPen2R = new Pen(Color.Red);

            for (int h = 0; h < Height; h++)
                for (int w = 0; w < Width; w++) { 
                    if (renderMAP[h, w]) gr.DrawRectangle(myPen, w, Height - h, 1, 1);
                    if (astarMAP[h, w]) gr.DrawRectangle(myPen2R, w, Height - h, 1, 1);
                }
        }
        private float Distance_Manhatan(Point P1, Point P2)
        {
            return (Math.abs(P1.X-P2.X)+Math.abs(P1.Y-P2.Y))*10;
        }
        private int isFind(List list, Point P)
        {
            for (int i = 0; i < list.Count; i++)
            { 
                node n = list[i];
                if (n.self == P)
                    return i;
            }
            return -1;
        }
        private void A_Star()
        {
            Array.Clear(astarMAP, 0, astarMAP.Length);

            List openlist = new List();
            List closedlist = new List();
            openlist.Add(new node(0, src, src));

            node cp;
            while (true)
            {
                int index = 0;
                float lowestF = 999999999;
                for (int i = 0; i < openlist.Count; i++){
                    float H = Distance_Manhatan(des, openlist[i].self);
                    float F = openlist[i].G + H;
                    if (lowestF > F) { 
                        lowestF = F;
                        index = i;
                    }
                }
                cp = openlist[index];
                openlist.Remove(cp);
                if (cp.self == des) break;
                //top-left
                Point np = new Point(cp.self.X - 1, cp.self.Y + 1);
                if (!renderMAP[cp.self.Y + 1, cp.self.X - 1] && isFind(closedlist, np) == -1)
                {
                    float G = cp.G + 14;
                    int i = isFind(openlist, np);
                    if(i != -1)
                    {
                        node n = openlist[i];
                        if (n.G > G){
                            n.G = G;
                            n.parent = np;
                        }
                    }
                    else
                        openlist.Add(new node(G, np, cp.self));
                }
                //top
                np = new Point(cp.self.X, cp.self.Y + 1);
                if (!renderMAP[cp.self.Y + 1, cp.self.X] && isFind(closedlist, np) == -1)
                {
                    float G = cp.G + 10;
                    int i = isFind(openlist, np);
                    if(i != -1)
                    {
                        node n = openlist[i];
                        if (n.G > G){
                            n.G = G;
                            n.parent = np;
                        }
                    }
                    else
                        openlist.Add(new node(G, np, cp.self));
                }
                //top-right
                np = new Point(cp.self.X + 1, cp.self.Y + 1);
                if (!renderMAP[cp.self.Y + 1, cp.self.X + 1] && isFind(closedlist, np) == -1)
                {
                    float G = cp.G + 14;
                    int i = isFind(openlist, np);
                    if(i != -1)
                    {
                        node n = openlist[i];
                        if (n.G > G){
                            n.G = G;
                            n.parent = np;
                        }
                    }
                    else
                        openlist.Add(new node(G, np, cp.self));
                }
                //left
                np = new Point(cp.self.X - 1, cp.self.Y);
                if (!renderMAP[cp.self.Y, cp.self.X - 1] && isFind(closedlist, np) == -1)
                {
                    float G = cp.G + 10;
                    int i = isFind(openlist, np);
                    if(i != -1)
                    {
                        node n = openlist[i];
                        if (n.G > G){
                            n.G = G;
                            n.parent = np;
                        }
                    }
                    else
                        openlist.Add(new node(G, np, cp.self));
                }
                //right
                np = new Point(cp.self.X + 1, cp.self.Y);
                if (!renderMAP[cp.self.Y, cp.self.X + 1] && isFind(closedlist, np) == -1)
                {
                    float G = cp.G + 10;
                    int i = isFind(openlist, np);
                    if(i != -1)
                    {
                        node n = openlist[i];
                        if (n.G > G){
                            n.G = G;
                            n.parent = np;
                        }
                    }
                    else
                        openlist.Add(new node(G, np, cp.self));
                }
                //bottom-left
                np = new Point(cp.self.X - 1, cp.self.Y - 1);
                if (!renderMAP[cp.self.Y - 1, cp.self.X - 1] && isFind(closedlist, np) == -1)
                {
                    float G = cp.G + 14;
                    int i = isFind(openlist, np);
                    if(i != -1)
                    {
                        node n = openlist[i];
                        if (n.G > G){
                            n.G = G;
                            n.parent = np;
                        }
                    }
                    else
                        openlist.Add(new node(G, np, cp.self));
                }
                //bottom
                np = new Point(cp.self.X, cp.self.Y - 1);
                if (!renderMAP[cp.self.Y - 1, cp.self.X] && isFind(closedlist, np) == -1)
                {
                    float G = cp.G + 10;
                    int i = isFind(openlist, np);
                    if(i != -1)
                    {
                        node n = openlist[i];
                        if (n.G > G){
                            n.G = G;
                            n.parent = np;
                        }
                    }
                    else
                        openlist.Add(new node(G, np, cp.self));
                }
                //bottom-right
                np = new Point(cp.self.X + 1, cp.self.Y - 1);
                if (!renderMAP[cp.self.Y - 1, cp.self.X + 1] && isFind(closedlist, np) == -1)
                {
                    float G = cp.G + 14;
                    int i = isFind(openlist, np);
                    if(i != -1)
                    {
                        node n = openlist[i];
                        if (n.G > G){
                            n.G = G;
                            n.parent = np;
                        }
                    }
                    else
                        openlist.Add(new node(G, np, cp.self));
                }
                closedlist.Add(cp);
            }
            //Draw
            Point reverse = cp.parent;
            while (reverse != src)
            {
                astarMAP[reverse.Y, reverse.X] = true;
                int i = isFind(closedlist, reverse);
                reverse = closedlist[i].parent;
            }
        }
    }
}