A* 알고리즘 / 최단경로탐색

이번에는 C++로 A* 알고리즘을 구현해보았다.

A* 알고리즘이란 최단경로탐색에 쓰이는 길찾기 알고리즘인데 가중치를 부여하여 최단경로를 찾는다.

인터넷 검색하면 관련 자료가 많은데 나는 이 사이트를 참고하였다.

http://cozycoz.egloos.com/9748811

아직 완전 구현 다한건 아니고 한번 길을 찾는정도까지 만들었는데 기록용으로 올림.

#include "stdafx.h"
#include <iostream>
#include <list>
#include <stack>
 
using namespace std;
 
class Amap
{
public:
    Amap* parent = NULL;
 
    int x = 0;
    int y = 0;
 
    int f = 0;
    int g = 0;
    int h = 0;
};
 
Amap map[10][10];
// 1시작, 2도착, 3장애물
 
int posX = 0;
int posY = 0;
int startX = 0;
int startY = 0;
int endX = 0;
int endY = 0;
 
list<int> openlist;
list<int> closelist;
list<int> barrierlist;
 
stack<int> findroute;
 
void openlist_save(int x, int y)
{
    openlist.push_back(y * 10 + x);
}
 
void closelist_save(int x, int y)
{
    closelist.push_back(y * 10 + x);
}
 
void barrierlist_save(int x, int y)
{
    barrierlist.push_back(y * 10 + x);
}
 
int distance(int x, int y)
{
    int absX = abs(endX - x) * 10;
    int absY = abs(endY - y) * 10;
    return absX + absY;
}
 
void print()
{
    for (int y = 0; y < 10; y++)
    {
        for (int x = 0; x < 10; x ++)
        {
            //현재위치
            if (x == posX && y == posY)
                cout << "★";
            //시작
            else if (map[x][y].f == 1)
                cout << "◎";
            //도착
            else if (map[x][y].f == 2)
                cout << "●";
            //장애물
            else if (map[x][y].f == 3)
                cout << "■";
            else
                cout << "□";
        }
        cout << endl;
    }
    cout << endl;
}
 
void print2()
{
    for (int y = 0; y < 10; y++)
    {
        for (int x = 0; x < 10; x++)
        {
            cout << map[x][y].f << "\t";
        }
        cout << endl;
    }
    cout << endl;
}
 
void start(int x, int y)
{
    map[x][y].x = x;
    map[x][y].y = y;
 
    map[x][y].f = 1;
 
    startX = x;
    startY = y;
 
    posX = startX;
    posY = startY;
 
    openlist_save(x, y);
 
    //시작점에서 인접 열린목록을 만듬
    // 주변 8칸을 검사해 열린목록에 추가
    list<int>::iterator iter = openlist.begin();
    for (int i = y - 1; i <= y + 1; i++)
    {
        for (int j = x - 1; j <= x + 1; j++)
        {
            // 이미 열린목록에 있는 값이라면 저장하지않음
            bool find = false;
            iter = openlist.begin();
            while (iter != openlist.end())
            {
                int dat = *iter;
 
                if (dat == i * 10 + j)
                {
                    find = true;
                    break;
                }
                iter++;
            }
 
            // 저장되지 않은값(시작값이 아니면)은 열린목록에 추가
            if (find == false)
            {
                openlist_save(j, i);
 
                map[j][i].parent = &map[posX][posY];
 
                if (posX == j || posY == i)
                    map[j][i].g = 10;
                else
                    map[j][i].g = 14;
 
                map[j][i].h = distance(j, i);
                map[j][i].f = map[j][i].g + map[j][i].h;
 
                map[j][i].x = j;
                map[j][i].y = i;
            }
        }
    }
 
    // 시작지점을 닫힌목록에 넣음
    iter = openlist.begin();
    while (iter != openlist.end())
    {
        int dat = *iter;
        if (dat == posY * 10 + posX)
        {
            iter = openlist.erase(iter);
            closelist.push_back(dat);
            break;
        }
        iter++;
    }
}
 
void end(int x, int y)
{
    map[x][y].f = 2;
 
    endX = x;
    endY = y;
}
 
void barrier(int x, int y)
{
    map[x][y].f = 3;
    barrierlist_save(x, y);
}
 
void check(int x, int y)
{
    // 열린목록에서 f값이 가장 낮은값으로 옮겨감 -> 주변 8칸중에서만 검색
    int min = 0;
    int minX = 0;
    int minY = 0;
 
    list<int>::iterator iter = openlist.begin();
    while (iter != openlist.end())
    {
        int dat = *iter;
        int y = dat / 10;
        int x = dat % 10;
 
        if (x >= posX - 1 && x <= posX + 1)
        {
            if (y >= posY - 1 && y <= posY + 1)
            {
                // 배열범위밖을 탐색하여 에러가나지않도록 예외처리
                if (x >= 0 && x <= 10 && y >= 0 && y <= 10)
                {
                    if (min == 0)
                    {
                        min = map[x][y].f;
                        minX = x;
                        minY = y;
                    }
                    else
                    {
                        if (map[x][y].f <= min)
                        {
                            min = map[x][y].f;
                            minX = x;
                            minY = y;
                        }
                    }
                }
            }
        }
        iter++;
    }
 
    posX = minX;
    posY = minY;
 
    // 옮겨간 값을 닫힌목록에 넣음
    iter = openlist.begin();
    while (iter != openlist.end())
    {
        int dat = *iter;
        if (dat == posY * 10 + posX)
        {
            iter = openlist.erase(iter);
            closelist.push_back(dat);
            break;
        }
        iter++;
    }
 
 
    // 주변 8칸을 검사해 열린목록에 추가
    list<int>::iterator iter2 = closelist.begin();
    list<int>::iterator iter3 = barrierlist.begin();
    for (int i = posY - 1; i <= posY + 1; i++)
    {
        for (int j = posX - 1; j <= posX + 1; j++)
        {
            // 배열범위밖을 탐색하여 에러가나지않도록 예외처리
            if (j >= 0 && j <= 10 && i >= 0 && i <= 10)
            {
                bool find = false;
                // 닫힌목록에 있는 값이면 무시
                iter2 = closelist.begin();
                while (iter2 != closelist.end())
                {
                    int dat = *iter2;
 
                    if (dat == i * 10 + j)
                    {
                        find = true;
                        break;
                    }
                    iter2++;
                }
 
                // 배리어에 있는 값이면 무시
                iter3 = barrierlist.begin();
                while (iter3 != barrierlist.end())
                {
                    int dat = *iter3;
 
                    if (dat == i * 10 + j)
                    {
                        find = true;
                        break;
                    }
                    iter3++;
                }
 
                // 이미 열린목록에 있는 값이라면 G값을 비교하여 g비용이 더 작으면 부모를 바꿈
                iter = openlist.begin();
                while (iter != openlist.end())
                {
                    int dat = *iter;
 
                    if (dat == i * 10 + j)
                    {
                        find = true;
 
                        // 열린목록의 현재 g값보다 지금위치의 g값이 낮으면 부모를 바꿈
                        if (map[j][i].g < map[posX][posY].g)
                        {
                            map[j][i].parent = &map[posX][posY];
 
                            if (posX == j || posY == i)
                                map[j][i].g = map[j][i].parent->g + 10;
                            else
                                map[j][i].g = map[j][i].parent->g + 14;
                        }
                        map[j][i].h = distance(j, i);
                        map[j][i].f = map[j][i].g + map[j][i].h;
 
                        map[j][i].x = j;
                        map[j][i].y = i;
                        break;
                    }
                    iter++;
                }
 
                // 저장되지 않은값이라면 열린목록에 추가
                if (find == false)
                {
                    openlist_save(j, i);
 
                    map[j][i].parent = &map[posX][posY];
 
                    if (posX == j || posY == i)
                        map[j][i].g = map[j][i].parent->g + 10;
                    else
                        map[j][i].g = map[j][i].parent->g + 14;
 
                    map[j][i].h = distance(j, i);
                    map[j][i].f = map[j][i].g + map[j][i].h;
 
                    map[j][i].x = j;
                    map[j][i].y = i;
                }
            }    
        }
    }
    
    // 열린목록에 도착지가 있으면 종료
    bool finish = false;
    iter = openlist.begin();
    while (iter != openlist.end())
    {
        int dat = *iter;
        if (dat == endY * 10 + endX)
        {
            finish = true;
            map[endX][endY].parent = &map[posX][posY];
            map[endX][endY].f = 2;
            break;
        }
        iter++;
    }
 
    // 열린목록이 비어있으면(길이없으면 종료) /////////////////////////////////////////
    if (openlist.size() == 0)
    {
        cout << "열린목록이 없습니다." << endl;
        return;
    }
 
    // 도착지가 없으면 반복
    print();
    print2();
 
    if(!finish)
        check(posX, posY);
    else
    {
        posX = endX;
        posY = endY;
        print();
        cout << "목표에 도착했습니다." << endl;
    }
}
 
void fastroute()
{
    //cout << map[posX][posY].x << "," << map[posX][posY].y << endl;
    findroute.push(posY * 10 + posX);
 
    int parentX = map[posX][posY].parent->x;
    int parentY = map[posX][posY].parent->y;
    posX = parentX;
    posY = parentY;
 
    bool finish = false;
    if (posX == startX && posY == startY)
    {
        finish = true;
        //cout << map[posX][posY].x << "," << map[posX][posY].y << endl;
        findroute.push(posY * 10 + posX);
    }
 
    if(!finish)
        fastroute();
}
 
void printRoute()
{
    if (findroute.size() > 0)
    {
        int num = findroute.top();
        findroute.pop();
 
        int y = num / 10;
        int x = num % 10;
 
        cout << x << "," << y << endl;
        printRoute();
    }
}
 
int main()
{
    end(6, 2);
    start(2, 2);
 
    barrier(4, 1);
    barrier(4, 2);
    barrier(4, 3);
    barrier(5, 3);
    barrier(6, 3);
 
    print();
    print2();
 
    check(startX, startY);
 
    //뒤에서 앞으로 찾으면서 스택에 넣기
    fastroute();
 
    //역으로 빼내서 최적화 길 출력
    printRoute();
 
    return 0;
}

print함수는 위치를 표시

print2함수는 가중치를 보여준다.

도착시 최단경로를 보여준다.

원래는 반복하면서 여러개의 길을 찾아서 그중에서 가장빠른 길을 보여줘야하지만

현재는 처음 길을 찾으면 종료되게 되어있다.

조만간 완성하여 갱신할 것.