A* algorithm

길찾기 알고리즘 중 대표적인 A*알고리즘.
원리같은거야 다른 블로그에서 상세히 설명해주고있으니 그냥 c++에서의 코드만 보자.

A*의 기본개념은 젤 빠를만한 길로 가보고 막히면 다시 다른길로...    의 방법. 사람이 보기엔 당연한거겠지만, 코드로 표현하기엔 대단한 사람들의 대단한 발상에서 생겨났다.

주석을 어느정도 달긴 했지만... 코드를 보기전에 길이 찾아지는 과정을 순차적으로 이해하면 코드이해도 쉬워질것같다.

☆우선 필요한 재료를 준비한다.
'닫힌목록' 이라 불리는 리스트 1개, '열린목록'이라 불리는 우선순위큐(리스트여도 상관없음) 1개, 맵 1개,
노드들..

여기서 노드는 위치좌표와 F,G,H라는 중요한 것들, 그리고 자기 이전의 노드를 가리킬 포인터를 가지는 클래스(든 구조체든)이다.
노드의 F,G,H가 핵심속성이라고 해야하나...  뭐 그런거다.
     F 는 G+H이고,
     G 는 시작지점 - 현재지점 까지의 거리값
     H 는 현재지점 - 종료지점 까지의 거리값
이다.  여기다가 휴리스틱 값이니 해서 이동하는데 드는 비용을 위치마다 다르게 할 수도 있는데,,  그냥 모든 맵의 이동비용은 1로 계산했다.

☆다음으로 알고리즘 진행 순차를 살펴본다.
1. 시작지점을 열린목록에 넣는다.
2. 열린목록의 가장 작은 F값을 빼내어 닫힌목록에 넣는다.
3. 닫힌목록의 맨 꽁무니것의 주변을 탐색해 열린목록에 넣는다.
4. 열린목록에 더이상 노드가 없거나 닫힌목록에 종료지점이 있으면 마친다.
5. 2-4를 반복한다.

이라고 간단하게 표현되지만, 실제 코드로 옮길때는 살짝 다르다. 게다가 주변을 탐색한다는게 할 일이 가장 많은 부분이다 ㅎ
우선은 맨 처음 시작지점을 열린목록에 넣었다 바로 빼니.. 그냥 처음부터 닫힌목록에 넣고 시작하는게 좋다.
그리고 주변탐색 일은 다시 나눠진다. 주변탐색은 맵의 형태나 이동방식에 따라 조금 달라지지만, 간단하게 사각타일이고 대각선 이동이 된다고 한다면, 한 방향에 대해서
          IF 갈 수 있는 맵인가
             IF 닫힌 목록에 없는가
                F값을 계산한 후 열린목록에 추가하는데..
                   IF 열린목록에 이미 있다면
                      G값이 작은걸로 갱신
                   ELSE
                       새로 추가
 정도의 과정을 거친다..  어째 한글로 써놓으니 더 복잡해보이는군

이정도만 알고 코드를 보면 이해하는데 어려움은 없을것같다.
(소스 퍼가는건 상관없지만, 이해의 목적으로 만든 소스라 실제 사용하기엔 무리가있다는 점을 아셔야합니다.)

/*
   AStar.h
   write by alleySark
*/
#pragma once;
#include<iostream>
#include<fstream>
#include<list>
#include<vector>
#include<cmath>
using namespace std;
namespace AStar{
 enum Attribute{NORMAL=0,BLOCK=1};
 class Map{
 public:
  unsigned** mapdata;
  unsigned width;
  unsigned height;
  Map();
  void setMapData(const char* path_name);
  void setMapData(unsigned** map_data, unsigned _width, unsigned _height);
  bool isWalkable(unsigned x, unsigned y);
 };
 class Node{
 public:
  unsigned int x_pos, y_pos;
  int f_val; //g+h
  int g_val; //시작노드-현재노드
  int h_val; //현재노드-목표노드
  Node* from;
  Node();
  Node(unsigned x, unsigned y);
  Node(const Node& n);
  //direct::1번부터 북,북동,동,남동,남,남서,서,북서
  Node(Node* from, short direct);
  void initNode();
  bool compNode(const Node* n);
 };
 class pqueue{
 private:
  vector<Node*> c;
  unsigned sz;
 public:
  pqueue();
  void push(Node* const n);
  void sorting();
  void pushNode(Node* const n);
  void pop();
  Node* top();
  bool empty();
  unsigned size();
  ~pqueue();
 };
 class cAStar{
 private:
  Map map;
  Node start;
  Node finish;
  pqueue openl;
  list<Node*> closel;
  bool hasRoute;
 public:
  cAStar();
  void init();
  bool HasRoute();
  void setMapData(const char* map_file_path);
  void setMapData(unsigned** map_data, unsigned width, unsigned height);
  void setStart(unsigned int x, unsigned int y);
  void setFinish(unsigned int x, unsigned int y);
  int calcDistance(Node* from, Node* to);
  void calcF(Node* tmp);
  bool isInClosel(const Node* n);
  bool checkAround(Node* now);
  Node* findRoute(unsigned x, unsigned y, unsigned _x, unsigned _y);
  ~cAStar();
 };
}
/*
  AStar.cpp

  write by alleySark
*/

#include"AStar.h"
using AStar::cAStar;
using AStar::Map;
using AStar::Node;

using AStar::pqueue;
//enum Attribute{NORMAL=0,BLOCK=1};
Map::Map(){
 mapdata = NULL;
 width = 0;
 height = 0;
}
void Map::setMapData(const char* path_name){
 ifstream fin;
 fin.open(path_name, ios::binary | ios::in);
 //seek map attribute file position
 //read
 //set...
}
void Map::setMapData(unsigned** map_data, unsigned _width, unsigned _height){
 width = _width;
 height = _height;
 mapdata = map_data;
}
bool Map::isWalkable(unsigned x, unsigned y){
 if(x>=width || y>=height)
  return false;
 if(mapdata[y][x] == BLOCK)
  return false;
 return true;
}
Node::Node(){
 x_pos = 0;
 y_pos = 0;
 f_val = 0;
 g_val = 0;
 h_val = 0;
 from = NULL;
}
Node::Node(unsigned x, unsigned y){
 x_pos = x;
 y_pos = y;
 f_val = 0;
 g_val = 0;
 h_val = 0;
 from = NULL;
}
Node::Node(const Node& n){
 x_pos = n.x_pos;
 y_pos = n.y_pos;
 f_val = n.f_val;
 g_val = n.g_val;
 h_val = n.h_val;
 from = n.from;
}
//direct::1번부터 북,북동,동,남동,남,남서,서,북서
Node::Node(Node* from, short direct){
 switch(direct){
   case 1://북
    x_pos = from->x_pos;
    y_pos = from->y_pos-1;
    break;
   case 2://북동
    x_pos = from->x_pos+1;
    y_pos = from->y_pos-1;
    break;
   case 3://동
    x_pos = from->x_pos+1;
    y_pos = from->y_pos;
    break;
   case 4://남동
    x_pos = from->x_pos+1;
    y_pos = from->y_pos+1;
    break;
   case 5://남
    x_pos = from->x_pos;
    y_pos = from->y_pos+1;
    break;
   case 6://남서
    x_pos = from->x_pos-1;
    y_pos = from->y_pos+1;
    break;
   case 7://서
    x_pos = from->x_pos-1;
    y_pos = from->y_pos;
    break;
   case 8://북서
    x_pos = from->x_pos-1;
    y_pos = from->y_pos-1;
    break;
   default:
    break;
 }
 this->from = from;
}
void Node::initNode(){
 x_pos = 0;
 y_pos = 0;
 f_val = 0;
 g_val = 0;
 h_val = 0;
 from = NULL;
}
bool Node::compNode(const Node* n){
 return (this->x_pos == n->x_pos && this->y_pos == n->y_pos);
}
pqueue::pqueue(){
 c.reserve(100);
 sz=0;
}
void pqueue::push(Node* const n){
 c.push_back(n);
 sz++;
}
void pqueue::sorting(){
 /*for(unsigned i=1; i<sz; i++){
 for(unsigned k=0; k<i; k++){
 if(c[k]->f_val < c[i]->f_val){
 Node* tmp = c[i];
 c[i] = c[k];
 c[k] = tmp;
 }
 }
 }*/
 //f_val로 하라는데 h로하면 안되는 이유가 있나 모르겠다.
 for(unsigned i=1; i<sz; i++){
  for(unsigned k=0; k<i; k++){
   if(c[k]->h_val < c[i]->h_val){
    Node* tmp = c[i];
    c[i] = c[k];
    c[k] = tmp;
   }
  }
 }
}
void pqueue::pushNode(Node* const n){
 for(int i=sz-1; i>=0; i--){
  if(c[i]->compNode(n)){
   if( c[i]->g_val > n->g_val){
    c[i]->g_val = n->g_val;
   }
   else
    return;
  }
 }
 push(n);
}
void pqueue::pop(){
 c.pop_back();
 sz--;
}
Node* pqueue::top(){
 if(sz == 0){
  return NULL;
 }
 return c.back();
}
bool pqueue::empty(){
 return c.empty();
}
unsigned pqueue::size(){
 return sz;
}
pqueue::~pqueue(){
 for(vector<Node*>::iterator itr = c.begin(); itr!=c.end(); itr++){
  delete (*itr);
 }
}

cAStar::cAStar(){
 start.initNode();
 finish.initNode();
 closel.clear();
 hasRoute = false;
}
void cAStar::init(){
 while(!openl.empty()){
  openl.pop();
 }
 closel.clear();
 start.initNode();
 finish.initNode();
 hasRoute = false;
}
bool cAStar::HasRoute(){
 return hasRoute;
}
void cAStar::setMapData(const char* map_file_path){
 map.setMapData(map_file_path);
}
void cAStar::setMapData(unsigned** map_data, unsigned width, unsigned height){
 if(map_data == NULL)
  return;
 map.setMapData(map_data, width, height);
}
void cAStar::setStart(unsigned int x, unsigned int y){
 start.initNode();
 start.x_pos = x;
 start.y_pos = y;
}
void cAStar::setFinish(unsigned int x, unsigned int y){
 finish.initNode();
 finish.x_pos = x;
 finish.y_pos = y;
}
int cAStar::calcDistance(Node* from, Node* to){
 return (int)((sqrt(pow((double)(to->x_pos-from->x_pos),2) + pow((double)(to->y_pos-from->y_pos),2)))*10);
}
void cAStar::calcF(Node* tmp){
 tmp->g_val = calcDistance(&start, tmp);
 tmp->h_val = calcDistance(tmp, &finish);
 tmp->f_val = tmp->g_val + tmp->h_val;
}
bool cAStar::isInClosel(const Node* n){
 for(list<Node*>::iterator it = closel.begin(); it!= closel.end(); it++){
  if( (*it)->compNode(n))
   return true;
 }
 return false;
}
bool cAStar::checkAround(Node* now){
 bool isThere = false;
 if( map.isWalkable(now->x_pos, now->y_pos-1) ){//북 체크
  Node* tmp = new Node(now, 1);
  if(!isInClosel(tmp)){
   calcF(tmp);
   openl.pushNode(tmp);
   isThere = true;
  }
 }
 //if( map.isWalkable(now->x_pos+1, now->y_pos-1)){//북동
 // Node* tmp = new Node(now, 2);
 // if(!isInClosel(tmp)){
 //  calcF(tmp);
 //  openl.pushNode(tmp);
 //  isThere = true;
 // }
 //}
 if( map.isWalkable(now->x_pos+1, now->y_pos)){//동
  Node* tmp = new Node(now, 3);
  if(!isInClosel(tmp)){
   calcF(tmp);
   openl.pushNode(tmp);
   isThere = true;
  }
 }
 //if( map.isWalkable(now->x_pos+1, now->y_pos+1)){//남동
 // Node* tmp = new Node(now, 4);
 // if(!isInClosel(tmp)){
 //  calcF(tmp);
 //  openl.pushNode(tmp);
 //  isThere = true;
 // }
 //}
 if( map.isWalkable(now->x_pos, now->y_pos+1)){//남
  Node* tmp = new Node(now, 5);
  if(!isInClosel(tmp)){
   calcF(tmp);
   openl.pushNode(tmp);
   isThere = true;
  }
 }
 //if( map.isWalkable(now->x_pos-1, now->y_pos+1)){//남서
 // Node* tmp = new Node(now, 6);
 // if(!isInClosel(tmp)){
 //  calcF(tmp);
 //  openl.pushNode(tmp);
 //  isThere = true;
 // }
 //}
 if( map.isWalkable(now->x_pos-1, now->y_pos)){//서
  Node* tmp = new Node(now, 7);
  if(!isInClosel(tmp)){
   calcF(tmp);
   openl.pushNode(tmp);
   isThere = true;
  }
 }
 //if( map.isWalkable(now->x_pos-1, now->y_pos-1)){//북서
 // Node* tmp = new Node(now, 8);
 // if(!isInClosel(tmp)){
 //  calcF(tmp);
 //  isThere = true;
 // }
 //}
 return isThere;
}
Node* cAStar::findRoute(unsigned x, unsigned y, unsigned _x, unsigned _y){
 //0. initialize private datas
 init();
 //1.set start and finish node
 setStart(x,y);
 setFinish(_x, _y);
 //2. check start node's around
 closel.push_back(&start);
 checkAround(&start);

 //3. pop from open list and check around(it pushs the walkable positions to priority queue)
 Node* tmpnow;
 while(1){
  openl.sorting();
  tmpnow = openl.top();
  if(tmpnow == NULL){
   hasRoute = false;
   break;
  }
  openl.pop();
  closel.push_back(tmpnow);
  if(isInClosel(&finish)){
   hasRoute = true;
   break;
  }
  checkAround(tmpnow);
 }
 return (closel.back());
}
cAStar::~cAStar(){
}
 
/*
  main.cpp
*/
#include<iostream>
#include"AStar.h"
using namespace std;
using AStar::Attribute;
using AStar::Node;
using AStar::cAStar;
const unsigned width = 20;
const unsigned height = 20;
unsigned** map;  
unsigned _map[20][20]={
      0,0,0,0,1,0,0,0,1,0,0,1,0,0,0,0,0,1,0,0,
      0,0,0,0,1,0,0,0,1,0,0,1,0,0,0,0,0,0,1,0,
      1,0,1,0,1,0,1,0,1,0,0,0,1,0,0,1,0,0,0,1,
      1,0,1,0,1,0,1,0,1,0,0,0,0,1,0,0,1,0,0,1,
      1,1,1,0,1,0,1,0,1,0,0,1,0,0,0,0,1,0,0,1,
      0,0,0,0,1,0,1,0,1,0,0,1,0,0,0,1,0,0,0,1,
      0,0,0,1,1,0,1,0,1,0,0,1,0,0,1,0,0,0,1,0,
      0,0,1,0,0,0,1,0,0,1,0,0,1,1,0,0,0,1,0,0,
      0,1,0,0,1,1,0,0,0,0,1,0,0,1,0,0,1,0,0,0,
      0,0,0,1,0,0,0,0,0,1,0,1,0,0,1,0,0,1,0,0,
      1,1,1,0,0,0,0,0,1,0,0,0,1,0,0,1,0,0,0,0,
      1,0,0,0,1,0,0,1,0,0,0,0,0,1,0,1,1,0,0,0,
      0,0,0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,1,0,1,
      0,0,1,0,0,1,0,0,0,1,0,0,0,1,1,1,0,0,0,1,
      0,0,1,0,1,0,0,0,1,0,0,0,1,1,0,0,0,0,0,1,
      0,0,1,1,0,0,0,1,0,0,0,1,0,0,1,0,0,1,1,0,
      0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,1,1,0,0,0,
      1,1,1,1,1,1,0,0,0,1,0,0,0,1,0,1,0,0,1,0,
      0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,1,0,1,0,1,
      0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0 };
void setMapPath(unsigned x, unsigned y){
 if(x>=width || y>=height)
  return;
 map[y][x] = 2;
}
int main(){
 map = new unsigned*[height];
 for(unsigned i=0; i<height; i++){
  map[i] = new unsigned[width];
  for(unsigned k=0; k<width; k++){
   map[i][k] = _map[i][k];
  }
 }
 cAStar as;
 as.setMapData((unsigned**)map, width, height);
 Node* fin;
 fin = as.findRoute(0,0,  19,19);
 do{
  setMapPath(fin->x_pos, fin->y_pos);
  fin = fin->from;
 }while(fin != NULL);
 for(unsigned y=0; y<height; y++){
  for(unsigned x=0; x<width; x++){
   switch(map[y][x]){
    case 0:
     printf("□");
     break;
    case 1:
     printf("■");
     break;
    case 2:
     printf("◎");
     break;
    default:
     break;
   }
  
  }
  putchar('\n');
 }
 if(!as.HasRoute())
  printf("길없음\n");
 return 0;
}