astar.cpp

极度优化的ASTAR算法

#include "stdafx.h"



#include "astar.h"



template <int SIZE>
struct vector_t
{
	vector_t() :  _last(0) 
	{ 
		memset(_mappos_index, 0xff, sizeof(_mappos_index));
	}
	bool empty() { return 0 == _last; }
	int size() { return _last; }


	bool val_less(int index1, int index2)
	{
		ASSERTDEBUG (index1>=0 && index1 < _last);
		ASSERTDEBUG (index2>=0 && index2 < _last);
		return _val[index1] < _val[index2];
	}
	void clear()
	{
		memset(_mappos_index, 0xff, sizeof(_mappos_index));
		_last = 0;
	}

	// get
	void get_top(int &mappos, int &val)
	{
		ASSERTDEBUG (_last > 0);
		val = _val[0];
		mappos = _mappos[0];
	}
	int get_mappos_index(int mappos)
	{
		ASSERTDEBUG (mappos >=0 && mappos < SIZE && _mappos_index[mappos] != -1);
		return _mappos_index[mappos];
	}

	// add
	void push_back(int mappos, int val)
	{
		ASSERTDEBUG (mappos >=0 && mappos < SIZE && _mappos_index[mappos] == -1);
		ASSERTDEBUG (_last < SIZE);
		_mappos_index[mappos] = _last;
		_mappos[_last] = mappos;
		_val[_last++] = val;
	}

	// del
	void drop_top()
	{
		ASSERTDEBUG (_last > 0) ;
		_val[0] = _val[--_last];
		_mappos[0] = _mappos[_last];
		_mappos_index[_mappos[0]] = 0;
	}

	// modify
	void swap(int index1, int index2)
	{
		ASSERTDEBUG (index1 >= 0 && index1 < _last);
		ASSERTDEBUG (index2 >= 0 && index2 < _last);

		int temp;
		temp = _mappos_index[_mappos[index1]] ;
		_mappos_index[_mappos[index1]] = _mappos_index[_mappos[index2]] ;
		_mappos_index[_mappos[index2]] = temp;

		temp = _val[index1];
		_val[index1] = _val[index2];
		_val[index2] = temp;

		temp = _mappos[index1];
		_mappos[index1] = _mappos[index2];
		_mappos[index2] = temp;
	}

	bool val_less_modify(int index, int val)
	{
		ASSERTDEBUG (_val[index] != val);
		int old = _val[index];
		_val[index] = val;
		return val < old; 
	}
private:
	int _mappos[SIZE];
	int _val[SIZE];
	int _mappos_index[SIZE];
	int _last;
};



template<int SIZE>
struct open_t
{
	bool empty()
	{
		return _vec.empty();
	}

	void push(int mappos, int val)
	{
		_vec.push_back(mappos, val);
		_up(_vec.size()-1);
	}
	void pop(int &mappos, int &val)
	{
		_vec.get_top(mappos, val);
		_vec.drop_top();
		_down(0);
	}

	void modify(int mappos, int val)
	{
		int index = _vec.get_mappos_index(mappos);

		if (_vec.val_less_modify(index, val))
			_up(index);
		else
			_down(index);
	}
	void clear()
	{
		_vec.clear();
	}

private:
	void _up(int index)
	{
		while (index > 0)
		{
			int p = (index-1)/2;
			if (_vec.val_less(p, index)) break;
			_vec.swap(p, index);
			index = p;
		}
	}
	void _down(int index)
	{
		for (; ;)
		{
			int c1 = index*2 + 1;
			if (c1 >= _vec.size()) break;
			int c2 = c1+1;
			if (c2 < _vec.size() && _vec.val_less(c2, c1) ) c1 = c2;
			if (_vec.val_less(index, c1)) break;
			_vec.swap(c1, index);
			index = c1;
		}
	}

	vector_t<SIZE> _vec;
};


#define MAP_HEIGHT 514   // 两个格子的城墙
#define MAP_WIDTH 514	 // 两个格子的城墙
#define MAPSIZE  MAP_HEIGHT * MAP_WIDTH 








static int dis(int begin,  int end)
{
	int x1 = begin % MAP_WIDTH;
	int y1 = begin / MAP_WIDTH;
	int x2 = end % MAP_WIDTH;
	int y2 = end / MAP_WIDTH;
	int dif_x = x1>x2 ? x1-x2 : x2-x1;
	int dif_y = y1>y2 ? y1-y2 : y2-y1;

	if (dif_x > dif_y)
		return 10 * dif_x + 4 * dif_y;
	else
		return 10 * dif_y + 4 * dif_x;
}


static inline int get_angle(int x1, int y1, int x2, int y2)
{
	return ((y2+1-y1) << 2 ) | (x2+1-x1);
}



namespace MYNAMESPACE { ;


bool astar8way(const byte *map, int x1, int y1, int x2, int y2, std::list<STWay> &ways, float radius)
{
	struct node
	{
		enum
		{
			null, open, close, 
		} state;
		int parent;
		int F, G, H;
	};
	
	node *nodes = NEW node[MAPSIZE];
	open_t<MAPSIZE> *opens = NEW open_t<MAPSIZE>;


	opens->clear();
	memset (nodes, 0,  MAPSIZE * sizeof(node));

	++x1; ++y1; ++x2; ++y2; // game to map 

	int mappos_begin = MAP_WIDTH*y1 + x1;
	int mappos_end = MAP_WIDTH*y2 + x2;

	radius *= 10;
	if  (0 != map[mappos_end] && radius == 0)
		radius = 10;			// dest can't arrive. return the neast node.


	node &n = nodes[mappos_begin];
	n.parent = -1;
	n.state = node::open;
	n.G = 0;
	n.H = dis(mappos_begin, mappos_end);
	n.F = n.G + n.H;
	opens->push(mappos_begin, n.F);

	int __query_count = 0;
	int __open_count = 1;
	int __close_count = 0;


	while( !opens->empty() )
	{
		__query_count ++;

		int mappos_test, val;
		opens->pop(mappos_test, val);
		node &n_test = nodes[mappos_test];

		if (mappos_test == mappos_end || 
			(radius!=0 &&  n_test.H <= radius )
			)
		{
			// found
			ways.clear();

			node *nodelink = &n_test;
			int mappos_path = mappos_test;

			// 先计算最后两个点的方向
			int path_x_last = mappos_path % MAP_WIDTH - 1;
			int path_y_last = mappos_path / MAP_WIDTH - 1;
			mappos_path = nodelink->parent;
			if (-1 == mappos_path) 
			{
				delete []nodes;
				delete opens;
				return true;	// 只有一个点， 起点就是目标点， 返回空路径
			}
			nodelink = &nodes[mappos_path];
			int path_x_end = mappos_path % MAP_WIDTH - 1;
			int path_y_end =  mappos_path / MAP_WIDTH - 1;
			int path_angle = get_angle(path_x_end, path_y_end, path_x_last, path_y_last);
			int path_step = 1;

			for(; ;)
			{
				mappos_path = nodelink->parent;
				if (-1 == mappos_path) break;
				nodelink = &nodes[mappos_path];

				int path_x_temp = mappos_path % MAP_WIDTH - 1;
				int path_y_temp = mappos_path / MAP_WIDTH - 1;
				int path_angle_temp = get_angle(path_x_temp, path_y_temp, path_x_end, path_y_end);
				if (path_angle_temp == path_angle)
				{
					path_step ++;
				}
				else
				{					
					ways.push_front( STWay(path_angle, path_step) );
					path_step = 1;
					path_angle = path_angle_temp;
				}
				path_x_end = path_x_temp;
				path_y_end = path_y_temp;
			}
			ways.push_front( STWay(path_angle, path_step) );	// 加载第一步

			delete []nodes;
			delete opens;
			return true;
		}
		n_test.state = node::close;
		__close_count ++;


		static int __offset[] = 
		{
			-MAP_WIDTH-1, -MAP_WIDTH, -MAP_WIDTH+1, -1, +1, +MAP_WIDTH-1, +MAP_WIDTH, +MAP_WIDTH+1,
		};
		static int __diff[] = 
		{
			14, 10, 14, 10, 10, 14, 10, 14,
		};
		static int __relation[] = 
		{
			1,3,  -1,-1,  1,4,  -1,-1,  -1,-1,  3,6,  -1,-1,  4,6,
		};
		for (int i=0; i<sizeof(__offset)/sizeof(__offset[0]); ++i)
		{
			int mappos_once = mappos_test+__offset[i];
			node &n_once = nodes[mappos_once];
			if (-1 != __relation[i*2])
				if (  map[mappos_test+__offset[__relation[i*2]]]  ||
					map[mappos_test+__offset[__relation[i*2+1]]] ) 
				{
					continue;
				}

				if ((map[mappos_once]) ) continue;  // map是0表示可以走， 否则不能走
				if (node::null == n_once.state)
				{
					// new node
					n_once.state = node::open;

					n_once.parent = mappos_test;
					n_once.G = n_test.G + __diff[i];
					n_once.H = dis(mappos_once, mappos_end);
					n_once.F = n_once.G + n_once.H;
					opens->push(mappos_once, n_once.F);
					__open_count ++;
				}
				else if ( n_test.G + __diff[i] < n_once.G )
				{

					n_once.parent = mappos_test;
					n_once.G = n_test.G + __diff[i];
					n_once.F = n_once.G + n_once.H;
					if (n_once.state == node::close)
					{
						// reopen it
						n_once.state = node::open;
						opens->push(mappos_once, n_once.F);
						__close_count --;
						__open_count ++;
					}
					else
					{
						// modify
						opens->modify(mappos_once, n_once.F);
					}
				}
		}

	}
	delete []nodes;
	delete opens;
	return false; // not found
}


}