searchlib.cpp

在VC的环境下

					break;
				}
			}
			list<VERTEXTYPE>::iterator iterClosedFind = CLOSED.begin();
			int nClosedFound = 0;
			for(; iterClosedFind!=CLOSED.end(); iterClosedFind++)
			{
				VERTEXTYPE & node = *iterClosedFind;
				if(node.nIndex == graph.vertices[ni.nAdjvex].data.nIndex)
				{
					nClosedFound = 1;
					break;
				}
			}
			
			if(!nClosedFound && !nOpenFound)
			{
				// 5a.如果ni不在OPEN和CLOSED表里，估计h(ni)，并计算
				//    f(ni)=g(ni)+h(ni), g(ni)=g(np)+c(np, ni), g(ns)=0,并放入OPEN表，
				//    并赋上回溯指针；
				graph.vertices[ni.nAdjvex].data.h = FuncH(graph, ni.nAdjvex, ng);
				graph.vertices[ni.nAdjvex].data.g = FuncG(graph, ni.nAdjvex, np.nIndex);
				graph.vertices[ni.nAdjvex].data.nFatherIndex = np.nIndex;
				OPEN.push_back(graph.vertices[ni.nAdjvex].data);
			}
			else
			{
				// 5b.如果ni已经在OPEN或CLOSED表里，使它的回溯指针指向g(ni)值最小的路  
				//    径。（这一点保证当前节点ni回溯指针所指的路径ns→np的路径代价最
				//    小）。
				g = FuncG(graph, ni.nAdjvex, np.nIndex);
				if(graph.vertices[ni.nAdjvex].data.g > g)
				{
					graph.vertices[ni.nAdjvex].data.g = g;
					graph.vertices[ni.nAdjvex].data.nFatherIndex = np.nIndex;

					// 5c.如果ni的回溯指针调整了且ni在CLOSED表中，则从CLOSED表中移出放入
					//    OPEN表中。
					if(nClosedFound)
					{
						CLOSED.remove(graph.vertices[ni.nAdjvex].data);
						OPEN.push_back(graph.vertices[ni.nAdjvex].data);
					}
				}
				
			}

		}		
	}

	// 返回失败
	return 0;
}

int Dual_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;
	}

	// 定义所需的变量
	const int START = 0;
	const int TARGET = 1;
	list<VERTEXTYPE> OPEN[2];
	list<VERTEXTYPE> CLOSED[2];
	double g = 0.0;
	int i = 0;

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

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

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

		// 4.搜索重合，成功退出，
		//   并通过回溯指针返回最短路径；
		//	 在OPEN[(i+1)%2]表中查找np
		list<VERTEXTYPE>::iterator iterOpenFind = OPEN[(i+1)%2].begin();
		for(; iterOpenFind!=OPEN[(i+1)%2].end(); iterOpenFind++)
		{
			VERTEXTYPE & npOther = *iterOpenFind;
			if(npOther.nIndex == np.nIndex)
			{// 搜索重合退出
				// 记录最短路径长度
				resultPath.dCostSumOnASelectedPath = np.g + npOther.g;
				
				// 设置最短路径
				if(START == i)
				{
					listShortestPath.push_front(np.nIndex);
					while(np.nFatherIndex >= 0)
					{
						listShortestPath.push_front(np.nFatherIndex);
						np = graph.vertices[np.nFatherIndex].data;
					}
					while(npOther.nFatherIndex >= 0)
					{
						listShortestPath.push_back(npOther.nFatherIndex);
						npOther = graph.vertices[npOther.nFatherIndex].data;
					}
				}
				else
				{
					listShortestPath.push_back(np.nIndex);
					while(np.nFatherIndex >= 0)
					{
						listShortestPath.push_back(np.nFatherIndex);
						np = graph.vertices[np.nFatherIndex].data;
					}
					while(npOther.nFatherIndex >= 0)
					{
						listShortestPath.push_front(npOther.nFatherIndex);
						npOther = graph.vertices[npOther.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[i].begin();
			int nOpenFound = 0;
			for(; iterOpenFind!=OPEN[i].end(); iterOpenFind++)
			{
				VERTEXTYPE & node = *iterOpenFind;
				if(node.nIndex == graph.vertices[ni.nAdjvex].data.nIndex)
				{
					nOpenFound = 1;
					break;
				}
			}
			list<VERTEXTYPE>::iterator iterClosedFind = CLOSED[i].begin();
			int nClosedFound = 0;
			for(; iterClosedFind!=CLOSED[i].end(); iterClosedFind++)
			{
				VERTEXTYPE & node = *iterClosedFind;
				if(node.nIndex == graph.vertices[ni.nAdjvex].data.nIndex)
				{
					nClosedFound = 1;
					break;
				}
			}
			
			if(!nClosedFound && !nOpenFound)
			{
				// 5a.如果ni不在OPEN和CLOSED表里，估计h(ni)，并计算
				//    f(ni)=g(ni)+h(ni), g(ni)=g(np)+c(np, ni), g(ns)=0,并放入OPEN表，
				//    并赋上回溯指针；
				graph.vertices[ni.nAdjvex].data.h = FuncH(graph, ni.nAdjvex, ng);
				graph.vertices[ni.nAdjvex].data.g = FuncG(graph, ni.nAdjvex, np.nIndex);
				graph.vertices[ni.nAdjvex].data.nFatherIndex = np.nIndex;
				OPEN[i].push_back(graph.vertices[ni.nAdjvex].data);
			}
			else
			{
				// 5b.如果ni已经在OPEN或CLOSED表里，使它的回溯指针指向g(ni)值最小的路  
				//    径。（这一点保证当前节点ni回溯指针所指的路径ns→np的路径代价最
				//    小）。
				g = FuncG(graph, ni.nAdjvex, np.nIndex);
				if(graph.vertices[ni.nAdjvex].data.g > g)
				{
					graph.vertices[ni.nAdjvex].data.g = g;
					graph.vertices[ni.nAdjvex].data.nFatherIndex = np.nIndex;

					// 5c.如果ni的回溯指针调整了且ni在CLOSED表中，则从CLOSED表中移出放入
					//    OPEN表中。
					if(nClosedFound)
					{
						CLOSED[i].remove(graph.vertices[ni.nAdjvex].data);
						OPEN[i].push_back(graph.vertices[ni.nAdjvex].data);
					}
				}
				
			}

		}
		
		// 始点和终点OPEN表指针移动
		i = (i+1) % 2;
	}

	// 返回失败
	return 0;
}

double FuncG(ALGRAPH & graph, int nNi, int nFatherNode)
{
	VERTEXTYPE * pChildNode = &(graph.vertices[nNi].data);
	const VERTEXTYPE * pFatherNode = &(graph.vertices[nFatherNode].data);
	const ARCNODE * pFatherToChild = 
		FindArc(graph, pFatherNode->nIndex, pChildNode->nIndex);
	if(pFatherToChild)
	{
		return (pFatherNode->g + pFatherToChild->info.dWeight);
	}
	
	return 0.0;
}

double FuncH(ALGRAPH & graph, int nNp, int nNg)
{
	return sqrt((graph.vertices[nNp].data.x - graph.vertices[nNg].data.x) 
				* (graph.vertices[nNp].data.x - graph.vertices[nNg].data.x)
				+ (graph.vertices[nNp].data.y - graph.vertices[nNg].data.y)
				* (graph.vertices[nNp].data.y - graph.vertices[nNg].data.y))
				* R;
}

int Random()
{
	return rand();
}