CODE:

bool	PathFinder::GetShortestPath(Vec2D target,
									std::list& path)
{
	//ターゲットまで障害物がなければ、経路は探索しない。
	if(!WallManager::Instance().
		 IntersectWalls(m_pUnit->Pos(),
					    target))
	{
		path.push_back(target);
		return true;
	}

	//自身から一番近いノードを得る。
	int from = GetClosestNode(m_pUnit->Pos());
	//見つからなかった！
	if(from == -1) return false;

	//ターゲットから一番近いノードを得る
	int to = GetClosestNode(target);
	if(to == -1) return false;

	//経路を計算
	AstarSearch AS(m_pGraph, from, to);
	//経路を得る
	std::list path_idx;
	path_idx = AS.GetShortestPath();

	//経路が見つからなかった！
	if(path_idx.empty()) return false;

	std::list::iterator it;
	for(it = path_idx.begin(); it != path_idx.end(); ++it)
	{
		path.push_front( m_pGraph->GetNodePos(*it) );
	}
	//最後にターゲットを追加
	path.push_back( target );
	return true;
}



int		PathFinder::GetClosestNode(Vec2D pos)
{
	int closest = -1;
	double minDist = 255*255;

	//全ノードについて探索
	for(std::vector::iterator it
		    = m_pGraph->GetNode().begin();
		it != m_pGraph->GetNode().end();
		++it)
	{
		//間に遮るものはないか
		if(WallManager::Instance().
			IntersectWalls(pos,
						   it->Pos()))
		{
			continue;
		}
		double dist = (pos-it->Pos()).LengthSQ();
		if( dist ID();
		}
	}
	
	return closest;
}

CODE:

class	AstarSearch
{

	//グラフへの参照
	Graph* m_pGraph;

	//インデックスはノード
	std::vector	m_GCosts; //実際のコスト
	std::vector	m_FCosts; //推定コストF=G+H
	std::vector	m_ShortestPath; //計算された最短距離
	std::vector	m_UnsearchArea; //未探索領域

	int	m_From;		//探索開始地点
	int	m_Target;	//ターゲット


	void	Search(); //実際の探索

public:

	AstarSearch(Graph* graph, int from, int target);


	//最短経路を得る
	std::list	GetShortestPath();

};

CODE:

AstarSearch::AstarSearch(Graph* graph, int from, int target)
	:m_pGraph(graph),
	 m_GCosts(graph->NumNodes(), 0.0),
	 m_FCosts(graph->NumNodes(), 0.0),
	 m_ShortestPath(graph->NumNodes(), NULL),
	 m_UnsearchArea(graph->NumNodes(), NULL),
	 m_From(from),
	 m_Target(target)
{
	Search();
}



void	AstarSearch::Search()
{
	//コストでソートされ、インデックスがキーとなる。
	std::multimap Cmap;


	//探索開始地点をマップに加える
	Cmap.insert(std::make_pair(m_FCosts[m_From], m_From));


	while(!Cmap.empty())
	{
		//最も少ないノードを得る(それは削除される)
		int	closestNode = Cmap.begin()->second;
		Cmap.erase(Cmap.begin());

		m_ShortestPath[closestNode] = m_UnsearchArea[closestNode];

		//ターゲットの場合終了
		if(closestNode == m_Target) return;

		//さきのノードに接続するリストをテスト
		std::list& clinks = m_pGraph->GetLink(closestNode);
		std::list::iterator it;

		for(it = clinks.begin(); it != clinks.end(); ++it)
		{
			//ヒューリスティックコストを計算(直線距離)
			double Hcost = (m_pGraph->GetNodePos(m_Target)
							- m_pGraph->GetNodePos(it->To())).Length();

			//実際のコスト
			double Gcost = m_GCosts[closestNode] + it->Cost();

			//未探索領域に追加しコストを更新
			if(m_UnsearchArea[it->To()] == NULL)
			{
				m_FCosts[it->To()] = Gcost + Hcost;
				m_GCosts[it->To()] = Gcost;

				Cmap.insert(std::make_pair(m_FCosts[it->To()], it->To()));

				m_UnsearchArea[it->To()] = &(*it);
			}
			//未探索領域に既に追加されていた場合、
			//そのコストが今までのものよりも少なければ、
			//マップを更新する
			else if(Gcost To()]
					&& m_ShortestPath[it->To()] == NULL)
			{
				m_FCosts[it->To()] = Gcost + Hcost;
				m_GCosts[it->To()] = Gcost;

				//全要素を辿り、要素を削除し、追加する
				std::multimap::iterator mit;
				for(mit = Cmap.begin(); mit != Cmap.end(); ++mit)
				{
					if(mit->second == it->To())
					{
						mit = Cmap.erase(mit);
						break;
					}
				}

				Cmap.insert(std::make_pair(m_FCosts[it->To()], it->To()));

				m_UnsearchArea[it->To()] = &(*it);
			}
		}
	}


}


std::list	AstarSearch::GetShortestPath()
{
	std::list path;
	//ターゲットから遡っていく
	path.push_back(m_Target);
	int node = m_Target;

	while( node != m_From
		&& m_ShortestPath[node] != NULL)
	{
		node = m_ShortestPath[node]->From();

		path.push_back(node);
	}

	return path;
}