AStar(?)ができた

経路探索はこれでばっちり。
特にこのページを参考にした。
http://www5f.biglobe.ne.jp/~kenmo/program/SLG/astar/astar.html
またスタックの実装はWarpRogueの書き方を多く参考にした。

//CPathF.h
#ifndef __C_PATHFINDER_H_INC__
#define __C_PATHFINDER_H_INC__

#include <iostream>
#include <vector>

using namespace std;

class CNode {
public:
	int x, y, cost, heur;
	class CNode *parent;
	CNode(int x = 0, int y = 0, int c = 0, int heur = 0, class CNode *par = NULL)
	{
		this->x = x;
		this->y = y;
		this->cost = c;
		this->heur = heur;
		parent = par;
	}
	int GetScore()
	{
		return cost + heur;
	}
	void Print()
	{
		cout.width(2);
		cout << "[" << x << ',' << y << "] ";
	}
};

class CPathFinder
{
private:
	enum {
		NODECNT = 2048,
	};
	CNode forge_node[NODECNT];
	CNode forge_open[NODECNT];
	CNode *node_total_ptr;
	CNode *open_sta_ptr;
	enum {
		BLO_NONE,
		BLO_BLOCK,
	};
	vector<vector<int> > blocker;
	vector<vector<int> > closed_set;
	int sx, sy, gx, gy;
	int hgt, wid;
	bool is_ok_xy(int x, int y)
	{
		if (x < 0 || y < 0 || x >= wid || y >= hgt)
			return false;
		return true;
	}
	CNode *open_pop();
	void open_push(CNode *newitem);
	void push_adj_nodes(CNode *node);
	CNode *total_push(CNode *node);
	int iabs(int x)
	{
		if (x < 0) 
			return -x;
		return x;
	}
	int idistxyxy(int x1, int y1, int x2, int y2);
	enum {
		DIR_NONE,
		DIR_UL,
		DIR_U,
		DIR_UR,
		DIR_R,
		DIR_DR,
		DIR_D,
		DIR_DL,
		DIR_L,
		N_DIR,
	};
	int xxd[N_DIR];
	int yyd[N_DIR];
public:
	void Alloc_mapetc();
	CPathFinder(int sx, int sy, int gx, int gy, int wid = 256, int hgt = 256)
	{
		this->sx = sx;
		this->sy = sy;
		this->gx = gx;
		this->gy = gy;
		this->wid = wid;
		this->hgt = hgt;
		node_total_ptr = forge_node;
		open_sta_ptr = forge_open;
		Alloc_mapetc();
	}
	void SetBlocker_xy(int x, int y)
	{
		if (is_ok_xy(x,y))
		{
			blocker[y][x] = 1;
		}
	}
	void SetStart_xy(int x, int y)
	{
		sx = x;
		sy = y;
	}
	void SetGoal_xy(int x, int y)
	{
		gx = x;
		gy = y;
	}
	CNode *calc_path();
	bool is_block_xy(int x, int y)
	{
		if (!is_ok_xy(x,y))
			return false;
		if (blocker[y][x] == BLO_NONE)
			return false;
		return true;
	}
};


#endif //__C_PATHFINDER_H_INC__

//CPathF.cpp
#include "CPathF.h"

void CPathFinder::Alloc_mapetc()
{
	for(int i=0;i<hgt;i++)
	{
		vector<int> a;
		a.clear();
		for(int j=0;j<wid;j++)
		{
			a.push_back(0);
		}
		closed_set.push_back(a);
		blocker.push_back(a);
	}
	const int tmpxxdi[N_DIR] = {0,-1, 0, 1, 1, 1, 0,-1,-1};
	const int tmpyydi[N_DIR] = {0,-1,-1,-1, 0, 1, 1, 1, 0};
	for(int i=0;i<N_DIR;i++)
	{
		xxd[i] = tmpxxdi[i];
		yyd[i] = tmpyydi[i];
	}
}

CNode *CPathFinder::calc_path()
{
	int loopcnt = 800;
	
	CNode tmpstart(sx, sy, 0, idistxyxy(sx,sy,gx,gy));
	
	open_push(&tmpstart);

	closed_set[sy][sx] = 1;

	CNode *node = open_pop();
	
	while(loopcnt--)
	{
		if (node == NULL) break;
		
		if (node->x == gx && node->y == gy)
		{
			return node->parent;
		}
		
		node = total_push(node);
		
		push_adj_nodes(node);
		
		node = open_pop();
	}
	
	return NULL;
}

CNode *CPathFinder::total_push(CNode *node)
{
	CNode *rval;
	*node_total_ptr = *node;
	rval = node_total_ptr;
	node_total_ptr++;
	return rval;
}

CNode *CPathFinder::open_pop()
{
	if (open_sta_ptr == forge_open)
	{
		return NULL;
	}
	open_sta_ptr--;
	return open_sta_ptr;
}

void CPathFinder::open_push(CNode *newitem)
{
	*open_sta_ptr = *newitem;
	
	CNode *node = open_sta_ptr;
	
	open_sta_ptr++;
	
	int node_score = node->GetScore();
	
	while(node != forge_open)
	{
		CNode *node_below;
		
		node_below = node - 1;
		int node_below_score = node_below->GetScore();
		
		if (node_below_score < node_score)
		{
			//swap
			CNode tmp = *node;
			*node = *node_below;
			*node_below = tmp;
			
			node--;
		}
		else
		{
			break;
		}
	}
}

void CPathFinder::push_adj_nodes(CNode *node)
{
	for(int d=DIR_UL;d<N_DIR;d++)
	{
		CNode adj_node;
		int xm = xxd[d], ym = yyd[d];

		adj_node.x = node->x + xm;
		adj_node.y = node->y + ym;
		adj_node.cost = node->cost + 1;
		adj_node.heur = idistxyxy(adj_node.x, adj_node.y, gx, gy);
		
		if (!is_ok_xy(adj_node.x, adj_node.y))
		{
			continue;
		}
		if (closed_set[adj_node.y][adj_node.x])
		{
			continue;
		}
		if (is_block_xy(adj_node.x, adj_node.y))
		{
			continue;
		}
		//斜め移動でスキマを通り抜けない
		if ((ym && is_block_xy(node->x, adj_node.y)) && (xm && is_block_xy(adj_node.x, node->y)))
		{
			continue;
		}
		closed_set[adj_node.y][adj_node.x] = 1;
		adj_node.parent = node;
		open_push(&adj_node);
	}
}

int CPathFinder::idistxyxy(int x1, int y1, int x2, int y2)
{
	int r1 = iabs(x1-x2), r2 = iabs(y1-y2);
	if (r1 > r2)
	{
		return r1;
	}
	return r2;
}

使い方の例

#include <iostream>
#include <vector>

#include "CPathF.h"

using namespace std;

#define HGT 20
char *map[HGT+1] = {
	"##########",
	"#...#.#..#",
	"#.#.#.#..#",
	"#G#......#",
	"#.#.####.#",
	"#........#",
	"#..#####.#",
	"#.#.#....#",
	"#..#..####",
	"####.#...#",
	"#....#..S#",
	"#....#...#",
	"#........#",
	"##########",
	0,
};

enum {
	M_NONE,
	M_WALL,
	M_START,
	M_GOAL,
	M_STEP,
};

vector<vector<int> > X_all; //ベクタのベクタ

void view()
{
	for (unsigned int i= 0; i<X_all.size(); i++){
		for (unsigned int c= 0; c<X_all[i].size(); c++){
			char ch = '#';
			int masu = X_all[i][c];
			if (masu >= '0' && masu <= '9')
			{
				ch = (char) masu;
			}
			else
			{
			switch(masu)
			{
			case M_NONE: ch = '.'; break;
			case M_WALL: ch = '#'; break;
			case M_START: ch = '@'; break;
			case M_GOAL: ch = '>'; break;
			case M_STEP: ch = 'a'; break;
			}
			}
			cout << ch;
		}
		cout << endl;
	}
}

int main()
{
	int iSutaX = 1, iSutaY = 1;
	int iGoalX = 2, iGoalY = 2;
	int MWidth = 2;
	int MHeight = 2;

	for (int i = 0; ; i++)
	{
		MHeight = i;
		if (!map[i]) break;
		vector<int> vectorX;
		vectorX.clear();
		int c;
		for (c = 0; ; c++)
		{
			if (!map[i][c]) break;
			int X = M_NONE;
			switch(map[i][c])
			{
			case '#':
				X = M_WALL;
				break;
			case 'S':
				iSutaX = c;
				iSutaY = i;
				X = M_START;
				break;
			case 'G':
				iGoalX = c;
				iGoalY = i;
				X = M_GOAL;
				break;
			default:
				X = M_NONE;
				break;
			}
			vectorX.push_back(X);
		}
		if (c > MWidth)
		{
			MWidth = c;
		}
		X_all.push_back(vectorX);
	}
	cout << endl;

	CPathFinder pf(iSutaX,iSutaY,iGoalX,iGoalY,MWidth,MHeight);
	for(int i=0;i<MHeight;i++)
	{
		for(int j=0;j<MWidth;j++)
		{
			if (X_all[i][j] == M_WALL)
			{
				pf.SetBlocker_xy(j,i);
			}
		}
	}
	CNode *miti = pf.calc_path();

	if (!miti)
	{
		cout << "miti is NULL." << endl;
	}
	else
	{
		cout << "miti done." << endl;
		char cnt = '0';
		for(CNode *ptr=miti; ptr != NULL ;ptr = ptr->parent)
		{
			ptr->Print();
			if (X_all[ptr->y][ptr->x] == M_NONE)
			{
				X_all[ptr->y][ptr->x] = cnt;
			}
			cnt++;
			if (cnt > '9') cnt = '0';
		}
		cout << "its miti." << endl;
	}
	
	view();
}

保証はないけどすきにつかってね。
const 付けろのツッコミはいつでもウェルカム、
あなたのそのconst付けろを心から応援するものです。