searchlib.cpp

在VC的环境下

#include "searchlib.h"

int VERTEXTYPE::operator < (const VERTEXTYPE & right)
{
	return ((g+h)<(right.g+right.h));
}

int VERTEXTYPE::operator == (const VERTEXTYPE & right)
{
	return (nIndex == right.nIndex);
}

void InsertNode(ALGRAPH * graph = NULL, const VERTEXTYPE * node = NULL, int nIndex = -1)
{

}

void InitNodes(ALGRAPH & graph)
{
	int i = 0;
	
	for(i=0; i<MAX_VERTEX_NUM; i++)
	{
		graph.vertices[i].data.nIndex = i;
	}
}

int CreateRandomGraph(ALGRAPH & graph)
{
	InitNodes(graph);

	SetCoordinates(graph);

	Link(graph);

	SetArcWeight(graph);

	return 1;	
}

int SetCoordinates(ALGRAPH & graph)
{
	int i = 0;
	int j = 0;

	// 赋随机x坐标值
	double nPrePointCoordinateX = 0.0;
	for(i=VERTEX_HEIGHT-1; i>=0; i--)
	{
		nPrePointCoordinateX = 0.0;
		for(j=0; j<VERTEX_WIDTH; j++)
		{
			graph.vertices[i*VERTEX_WIDTH+j].data.x = nPrePointCoordinateX + X_MARCH 
				+ Random() % RANDOM_X_SCALE;
			nPrePointCoordinateX = graph.vertices[i*VERTEX_WIDTH+j].data.x;
		}
	}

	// 赋随机y坐标值
	double nPrePointCoordinateY = 0.0;
	for(j=0; j<VERTEX_WIDTH; j++)
	{
		nPrePointCoordinateY = 0.0;
		for(i=VERTEX_HEIGHT-1; i>=0; i--)
		{
			graph.vertices[i*VERTEX_WIDTH+j].data.y = nPrePointCoordinateY + Y_MARCH 
				+ Random() % RANDOM_Y_SCALE;
			nPrePointCoordinateY = graph.vertices[i*VERTEX_WIDTH+j].data.y;
		}
	}

	return 1;
}

int Link(ALGRAPH & graph)
{
	int i = 0;
	int j = 0;

	// 连接上下左右节点
	int nTab[MAX_LINK_NUM] = {-VERTEX_WIDTH, VERTEX_WIDTH, -1, 1};
	int nAdjvex = 0;
	for(i=0; i<MAX_VERTEX_NUM; i++)
	{
		for(j=0; j<MAX_LINK_NUM; j++)
		{
			nAdjvex = i + nTab[j];
			if(nAdjvex>=0 && nAdjvex<MAX_VERTEX_NUM)
			{
				if(!((i+1)%VERTEX_WIDTH) && 3==j)
				{// 右边界节点无右邻接节点
					continue;
				}
				if(!(i%VERTEX_WIDTH) && 2==j)
				{// 左边界节点无左边界节点
					continue;
				}
				// 连接
				graph.vertices[i].ArcList.push_back(ARCNODE(nAdjvex, -1.0, VALIDATE_WEIGHT_DOWN_LIMIT-1.0));
			}
		}
	}

	graph.nVexNum = MAX_VERTEX_NUM;
	graph.nArcNum = (VERTEX_WIDTH - 1) * VERTEX_WIDTH
		+ (VERTEX_HEIGHT - 1) * VERTEX_HEIGHT;

	return 1;
}

int SetArcWeight(ALGRAPH & graph)
{
	int i = 0;
	int j = 0;

	// 连接上下左右的节点
	double dCurX = 0.0;
	double dCurY = 0.0;
	for(i=0; i<MAX_VERTEX_NUM; i++)
	{
		list<ARCNODE>::iterator iter = graph.vertices[i].ArcList.begin();
		for(; iter!=graph.vertices[i].ArcList.end(); iter++)
		{
			ARCNODE & arcNode = *iter;
			// 查询该弧的权值是否已经计算出来
			ARCNODE * pArcNode = NULL;
			if(pArcNode = FindArc(graph, arcNode.nAdjvex, i))
			{
				if(!(pArcNode->info.dDistance < VALIDATE_WEIGHT_DOWN_LIMIT))
				{
					arcNode.info.dDistance = pArcNode->info.dDistance;
					arcNode.info.dWeight = pArcNode->info.dWeight;
					continue;
				}
			}

			// 计算新的权值 
			dCurX = graph.vertices[arcNode.nAdjvex].data.x;
			dCurY = graph.vertices[arcNode.nAdjvex].data.y;
			arcNode.info.dDistance = sqrt((graph.vertices[i].data.x - dCurX) 
				* (graph.vertices[i].data.x - dCurX)
				+ (graph.vertices[i].data.y - dCurY)
				* (graph.vertices[i].data.y - dCurY)); // + rand() % WEIGHT_SCALE;
			arcNode.info.dWeight = arcNode.info.dDistance * 
				(1.2 + ((double)(20 - (Random() % WEIGHT_SCALE))) / 100.0);
		}

	}

	return 1;
}

ARCNODE * FindArc(ALGRAPH & graph, int nArcStart, int nArcEnd)
{
	if(nArcStart<0 || nArcStart>=MAX_VERTEX_NUM
		|| nArcEnd<0 || nArcEnd>=MAX_VERTEX_NUM)
	{
		return NULL;
	}

	list<ARCNODE>::iterator iterArc = 
		graph.vertices[nArcStart].ArcList.begin();
	for(; iterArc!=graph.vertices[nArcStart].ArcList.end(); iterArc++)
	{
		ARCNODE & arcNode = *iterArc;
		if(nArcEnd == arcNode.nAdjvex)
		{
			return &arcNode;
		}
	}

	return NULL;
}

int ShortestPath_AStart(ALGRAPH & graph, int ns, int ng, 
						list<int> & listShortestPath, STUDYRESULT & resultPath)
{
	resultPath.nNodeNumberOnASelectedPath = 0;
	resultPath.nNumberOfExpandedNode = 0;

	// 1.将开始节点ns放入OPEN表；
    // 2.如果OPEN表为空，返回错误，并退出；
    // 3.在OPEN队列中选择f(np)最小的节点np移出放入CLOSED表；
    // 4.如果np是终点ng，成功退出，并通过回溯指针返回最短路径；
    // 5.否则，扩展节点np周围所有的节点ni，并给每个节点赋上指向父节点   
    //   的指针。
    // 5a.如果ni不在OPEN或CLOSED表里，估计h(ni)，并计算
	//    f(ni)=g(ni)+h(ni), g(ni)=g(np)+c(np, ni), g(ns)=0,并放入OPEN表；
	// 5b.如果ni已经在OPEN或CLOSED表里，使它的回溯指针指向g(ni)值最小的路  
	//    径。（这一点保证当前节点ni回溯指针所指的路径ns→np的路径代价最
	//    小）。
	// 5c.如果ni的回溯指针调整了且ni在CLOSED表中，则从CLOSED表中移出放入
    //    OPEN表中。
    // 6.返回第2步。

	// 初始化
	int nNodeNo = 0;
	for(nNodeNo=0; nNodeNo<graph.nVexNum; nNodeNo++)
	{
		graph.vertices[nNodeNo].data.h = -1.0;
		graph.vertices[nNodeNo].data.g = -1.0;
		graph.vertices[nNodeNo].data.nFatherIndex = -1;
	}

	// 定义所需的变量
	list<VERTEXTYPE> OPEN;
	list<VERTEXTYPE> CLOSED;
	double g = 0.0;

	// 1.将开始节点ns放入OPEN表；
	graph.vertices[ns].data.g = 0.0;
	OPEN.push_back(graph.vertices[ns].data);

	// 2.如果OPEN表为空，返回错误，并退出；
	while (!OPEN.empty())
	{
		// 对OPEN排序
		OPEN.sort();

		// 3.在OPEN队列中选择f(np)最小的节点np移出放入CLOSED表；
		VERTEXTYPE np = *OPEN.begin();
		CLOSED.push_back(np);
		OPEN.pop_front();

		// 4.如果np是终点ng，成功退出，并通过回溯指针返回最短路径；
		if(np.nIndex == ng)
		{
			// 记录最短路径长度
			resultPath.dCostSumOnASelectedPath = np.g;

			// 设置最短路径
			listShortestPath.push_front(np.nIndex);
			while(np.nFatherIndex >= 0)
			{
				listShortestPath.push_front(np.nFatherIndex);
				np = graph.vertices[np.nFatherIndex].data;
			}
			
			// 记录最短路径长度
			resultPath.nNodeNumberOnASelectedPath = (int) listShortestPath.size();

			// 返回成功
			return 1;
		}

		// 5.否则，扩展节点np周围所有的节点ni，并给每个节点赋上指向父节点的回溯指针。   
		list<ARCNODE>::iterator iterNi = graph.vertices[np.nIndex].ArcList.begin();
		for(; iterNi!=graph.vertices[np.nIndex].ArcList.end(); iterNi++)
		{
			ARCNODE & ni = *iterNi;

			// 节点ni是np的父节点，不处理
			if(ni.nAdjvex == np.nFatherIndex)
			{
				continue;
			}

			// 记录扩展的节点数
			resultPath.nNumberOfExpandedNode++;
			
			// 在OPEN表和CLOSED表中查找ni
			list<VERTEXTYPE>::iterator iterOpenFind = OPEN.begin();
			int nOpenFound = 0;
			for(; iterOpenFind!=OPEN.end(); iterOpenFind++)
			{
				VERTEXTYPE & node = *iterOpenFind;
				if(node.nIndex == graph.vertices[ni.nAdjvex].data.nIndex)
				{
					nOpenFound = 1;