A*算法实现

游戏开发中会有寻路功能，目前应用比较广泛的就是A*算法，算法描述： http://blog.vckbase.com/panic/archive/2005/03/20/3778.html，已经说的相当清楚了，实现起来不算特别麻烦，自己按照描述实现了一个算法，只是一个简单实现，实际项目中有待优化：

#ifndef _A_STAR_H_
#define _A_STAR_H_

#include <iostream>
#include <math.h>
#include <windows.h>

#define NULL 0

#define IN_OPEN_LIST 1
#define IN_CLOSED_LIST 0
#define NOT_IN_ANY_LIST -1

#define HEIGHT 5
#define WIDTH   7

/*********************************************
/地图示意图，可通过，不可通过，程序中由 A到B
                        ---------------
                        |1|1|1|1|1|1|1|
                        |1|1|1|0|1|1|1|
                        |1|B|1|0|1|A|1|
                        |1|1|1|0|1|1|1|
                        |1|1|1|1|1|1|1|
                        ---------------
**********************************************/

struct point
{
       int x ;
       int y ;

       point(){ }
       point(int _x, int _y ):x( _x), y (_y){}

       friend point operator / ( const point &_pt, const int _rate)
      {
             return point (_pt. x / _rate , _pt. y / _rate );
      }

       friend point operator + ( const point &_p1, const point &_p2)
      {
             return point (_p1. x + _p2 .x, _p1.y + _p2. y);
      }

       friend point operator + ( const point &_pt, const int _value)
      {
             return point (_pt. x + _value , _pt. y + _value );
      }

       friend point operator - ( const point &_pt, const int _value)
      {
             return point (_pt. x - _value , _pt. y - _value );
      }
};

struct map_node
{
       bool       cross_status;    //是否有障碍
       int        node_g;          //节点G 值
       int        node_h;          //节点H 值
       int        node_f;          //节点F 值
       int        list_status;     //节点在哪个列表中(-1未访问 , 0关闭列表，开启列表)
       point      node_pos ;        //节点位置
       map_node*  parent ;          //节点父节点

};

class astar
{
public:
       astar()
      {
             _open_list_size = 0;
      }
      ~ astar(){}

       void InitWordMap ()
      {
             _open_list_size = 0;

             for(int i = 0; i < HEIGHT ; i ++)
            {
                   for(int j = 0; j < WIDTH ; j ++)
                  {
                         if(j == 3 && i >= 0 && i <= 4)
                               _word_map[j ][i]. cross_status = false ;
                         else
                               _word_map[j ][i]. cross_status = true ;
                         _word_map[j ][i]. list_status = NOT_IN_ANY_LIST ;
                         _word_map[j ][i]. node_g = 0;
                         _word_map[j ][i]. node_h = 0;
                         _word_map[j ][i]. node_f = 0;
                         _word_map[j ][i]. node_pos.x = j * 10;
                         _word_map[j ][i]. node_pos.y = i * 10;
                         _word_map[j ][i]. parent = NULL ;
                  }
            }
      }

       void search_path (point _src, point _des)
      {
             point _pos = _src / 10;
             insert_open_list(_word_map [_pos. x][_pos .y]);
             while(_open_list_size )
            {
                   _pos = delete_open_list ();
                   calc_near_ghf(_word_map [_pos. x][_pos .y], _des);
                   if(_word_map [_des. x / 10][_des .y / 10]. list_status == IN_OPEN_LIST)
                         break;
            }
      }

       void calc_near_ghf (map_node & _parent, const point& _des)
      {
             point _pos = _parent. node_pos / 10;
            
             if(_pos .x + 1 < WIDTH)                         //正右边格子
            {
                   judge_ghf(_parent , point( _pos.x + 1, _pos. y), _des );
            }

             if(_pos .x + 1 < WIDTH && _pos .y + 1 < HEIGHT)  // 右下方格子
            {
                   judge_ghf(_parent , _pos + 1, _des);
            }

             if(_pos .y + 1 < HEIGHT)                        //正下方格子
            {
                   judge_ghf(_parent , point( _pos.x , _pos. y + 1), _des );
            }

             if(_pos .x - 1 >= 0 && _pos.y + 1 < HEIGHT)     //左下方格子
            {
                   judge_ghf(_parent , point( _pos.x - 1, _pos. y + 1), _des);
            }

             if(_pos .x - 1 >= 0)                                        //正左边格子
            {
                   judge_ghf(_parent , point( _pos.x - 1, _pos. y), _des );
            }

             if(_pos .x - 1 >= 0 && _pos.y - 1 >= 0)         //左上边格子
            {
                   judge_ghf(_parent , _pos - 1, _des);
            }

             if(_pos .y - 1 >= 0)                            //正上方格子
            {
                   judge_ghf(_parent , point( _pos.x , _pos. y - 1), _des );
            }

             if(_pos .x + 1 >= 0 && _pos.y - 1 >= 0)         //右上边格子
            {
                   judge_ghf(_parent , point( _pos.x + 1, _pos. y - 1), _des);
            }

      }

       void judge_ghf (map_node & _parent, const point & _pos, const point& _des)
      {
             if(!_word_map [_pos. x][_pos .y]. cross_status)
                   return ;
             switch(_word_map [_pos. x][_pos .y]. list_status)
            {
             case IN_OPEN_LIST :
                   calcG(_parent , _word_map[ _pos.x ][_pos. y]);
                   break;
             case NOT_IN_ANY_LIST :
                   calcG(_parent , _word_map[ _pos.x ][_pos. y]);
                   calcH(_word_map [_pos. x][_pos .y], _word_map[_des .x / 10][_des.y / 10]);
                   calcF(_word_map [_pos. x][_pos .y]);
                   insert_open_list(_word_map [_pos. x][_pos .y]);
                   break;
             case IN_CLOSED_LIST :
                   break;
             default:
                   break;
            }
      }

       int calcG (map_node & _parent, map_node &_child)
      {
             int tmp_g = _parent. node_g + distance (_parent. node_pos, _child.node_pos );

             if(_child .list_status == NOT_IN_ANY_LIST)
            {
                   _child.node_g = tmp_g;
                   _child.parent = &_parent;
            }
            
             else if (_child. node_g > tmp_g )
            {
                   _child.node_g = tmp_g;
                   _child.parent = &_parent;
            }

             return _child .node_g;
      }

       int calcH (map_node & _src, const map_node & _des)
      {
             return _src .node_h = abs(_src .node_pos. x - _des .node_pos. x) + abs(_src .node_pos. y - _des .node_pos. y);
      }

       int calcF (map_node & _node)
      {
             return _node .node_f = _node.node_g + _node. node_h;
      }

       void insert_open_list (map_node & _node)
      {
             _node.list_status = IN_OPEN_LIST;
             if(_open_list_size == 0)
            {
                   _open_list[0] = &_node ;
                   _open_list_size ++;
                   return ;
            }

             int _sort_hight = _open_list_size - 1;
             int _sort_low    = 0;
             int _middle      = 0;
             while(_sort_hight >= _sort_low)
            {
                   _middle = (_sort_hight + _sort_low) / 2;
                   if(_open_list [_middle]-> node_f < _node .node_f)
                  {
                         _sort_hight = _middle - 1;
                  }
                   else if (_open_list[ _middle]->node_f > _node. node_f)
                  {
                         _sort_low = _middle + 1;
                  }
                   else  {  break ;  }
            }
            
             _middle = _sort_low > _middle ? _sort_low : _middle ;
             _middle = _sort_hight < _middle ? _sort_hight : _middle ;

             int i = _open_list_size;
             for(; i > _middle; i --)
            {
                   _open_list[i ]  = _open_list[ i - 1];
            }

             _open_list[i + 1] = &_node;
             _open_list_size ++;
      }

       point delete_open_list ()
      {
             _open_list[_open_list_size - 1]->list_status = IN_CLOSED_LIST;
             return _open_list [--_open_list_size]-> node_pos / 10;
      }

       int distance (const point &_X , const point &_Y )
      {
             return (int )sqrt(( long double )((_X. x - _Y .x) * ( _X.x - _Y. x) + (_X.y - _Y. y) * (_X .y - _Y.y )));
      }

       void print_path (const point &_des )
      {
             if(_open_list_size == 0)
            {
                   std::cout << "not search path";
                   return ;
            }

             map_node* _node = &_word_map[ _des.x / 10][_des. y / 10];
             while(_node != NULL)
            {
                   std::cout << "[" << _node->node_pos .x << "," << _node->node_pos .y << "]";
                   if(_node ->parent != NULL) 
                  {
                         std::cout << "—— >";
                  }
                   _node = _node ->parent;
            }
      }
private:
       map_node             _word_map[ WIDTH][HEIGHT ];
    map_node*            _open_list[ WIDTH * HEIGHT ];
       int                  _open_list_size;
};

#endif;

A*算法中最耗时的是对估计值F排序，使用二叉堆进行插入排序，下面是二叉堆算法描述： http://blog.vckbase.com/panic/archive/2005/03/28/4144.html，算法实现：

#ifndef _BINARY_HEAP_H_
#define _BINARY_HEAP_H_

#include <iostream>

enum
{
       NODE_NUM = 100,
};

class binary_heap
{
public:
       binary_heap()
      {
             _node_count = 0;
      }

      ~ binary_heap()
      {
      }

       template<class T>
       void swap (T& _x, T & _y)
      {
             T _tmp = _x;
             _x = _y ;
             _y = _tmp ;
      }

       void insert_node (int value)
      {
             node[++_node_count ] = value;
             int _node_index = _node_count;
             while(_node_index > 1
                  && node[_node_index / 2] > node[ _node_index])
            {
                   swap<int >(node[ _node_index / 2], node [_node_index]);
                   _node_index /= 2;
            }
      }

       void delete_node ()
      {
             node[1] = node [_node_count--];
             int _node_index = 1;
             while(_node_index < _node_count)
            {
                   if(node [_node_index] > node[_node_index * 2])
                  {
                         swap<int >(node[ _node_index], node [_node_index * 2]);
                         _node_index *= 2;
                  }
                   else if (_node_index * 2 + 1 <= _node_count
                            && node[_node_index ] > node[ _node_index * 2 + 1])
                  {
                         swap<int >(node[ _node_index], node [_node_index * 2]);
                         _node_index = _node_index * 2 + 1;
                  }
                   else  {   break ;   }
            }
      }

       void print_node ()
      {
             for(int i = 1; i <= _node_count ; i ++)
                   std::cout << node[ i] << " " ;
             std::cout << std:: endl;
      }
private:
       int node [NODE_NUM];
       int _node_count ;
};

#endif