mypathplanning.h

实现了波兰式的求解

#ifndef H_MYPATHPLANNING
#define H_MYPATHPLANNING
#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>



#define N 10
#define C 10
#define MUL 5
#define TARGET 99

 //  #define DEBUG_PANJIE

typedef struct Path
{
	int pathNode[100];
	int pathLength;
}Path;
Path AntPath[20];//每群最多五只蚂蚁

Path TempPath;

double possibility[8];
double Map[100][100];
	//delta
double DirectDistance[8] = {1.414, 1, 1.414, 1, 1,1.414, 1, 1.414};
int DirectionWeight[8] = {1,2,3,2,6,3,6,10};//w
int NextOrientation[8] = {-11,-10,-9,-1,1,9,10,11};


int HowToGet = 100;
bool successful = false;
bool IsBounder(int index)
{
	int col,row;
	col = index % N;
	row = index / N;
	if(col==0 || col==N-1 || row==0 || row==N-1)
		return true;
	return false;
}

bool IsValid(int index)
{
	if(index<0 || index>=100)
		return false;
	return true;
}

bool IsTarget(int nPosition)
{
	if(nPosition == TARGET)
		return true;
	else
		return false;
}

int GetD(int nCurrentPosition,int nTempPos)
{
	int col1,row1,col2,row2;
	col1 = nCurrentPosition%N;
	row1 = nCurrentPosition/N;
	col2 = nTempPos%N;
	row2 = nTempPos/N;
	return abs(col1-col2) + abs(row1-row2);

}
//得到两点距离的平方
int GetDistance(int nCurrentPosition,int nTempPos)
{
	int col1,row1,col2,row2;
	if(nCurrentPosition<0 || nCurrentPosition>=N*N)
		return 500;
	col1 = nCurrentPosition%N;
	row1 = nCurrentPosition/N;
	col2 = nTempPos%N;
	row2 = nTempPos/N;
	return (col1-col2)*(col1-col2) + (row1-row2)*(row1-row2);

}



void UpdateDirectionWeight(int nCurrentPos)
{
	int temp[8];
	int i;
	for(i=-11; i<=-9; i++)
	{
		temp[i+11] = GetDistance(nCurrentPos+i,TARGET);
	}
	temp[3] = GetDistance(nCurrentPos-1,TARGET);
	temp[4] = GetDistance(nCurrentPos+1,TARGET);
	for(i=9; i<=11; i++)
	{
		temp[i-4] = GetDistance(nCurrentPos+i,TARGET);
	}
	for(i=0; i<8; i++)
	{
		if(temp[i] !=0)
			DirectionWeight[i] = 400/temp[i];
		else
			DirectionWeight[i] = 500;
	}
}
//没有用随机数
int FindNext(int nCurrentPosition)
{
	int i;
	int nTempPos;
	int nNextPosition;
	for(i=0; i<8; i++)
	{
		nTempPos = nCurrentPosition + NextOrientation[i];
#ifdef DEBUG_PANJIE
	 	printf("CurrentPos = %d ,tempPos = %d\n",nCurrentPosition,nTempPos);
#endif
		if(nTempPos<0||nTempPos>=N*N || GetD(nCurrentPosition,nTempPos)>2)
		{
			possibility[i] = 0;
		}
		else
			possibility[i] = Map[nCurrentPosition][nTempPos] * DirectionWeight[i] * DirectDistance[i];	
		//printf("%f ",possibility[i]);
	}
	double sum[8];
	for(i=0;i<8;i++)
	{
		sum[i] = 0;
	}
	sum[0] = possibility[0];
	for(i=1; i<8; i++)
	{
		sum[i] = sum[i-1] + possibility[i];
	}
	for(i=0; i<8; i++)
	{
		possibility[i] = possibility[i] / sum[7]; 
#ifdef DEBUG_PANJIE
		printf("poss[%d] = %f  ",i,possibility[i]);
#endif
	}
	printf("\n");


	//srand( (unsigned)time( NULL ) );
	//int nRandNum = rand()%100;
	//double RandomNumber = nRandNum*1.0/100;
//	for(i=0;i<8;i++)
	//{
	//	if(RandomNumber<=sum[i]&&fabs(possibility[i])>0.000001)
	//	{
	///		HowToGet = i;
	//		nNextPosition = nCurrentPosition + NextOrientation[i];
	//		break;
	//	}
//	}
	double maxpossibiliyt = 0;
	int maxpossindex = 0;
	for(i=0; i<8; i++)
	{
		if(possibility[i] > maxpossibiliyt && i+HowToGet != 7) //////////////////////
		{
			maxpossibiliyt = possibility[i];
			maxpossindex = i;
		}
		
	}
	HowToGet = maxpossindex;
	nNextPosition = nCurrentPosition + NextOrientation[maxpossindex];
#ifdef DEBUG_PANJIE
	printf("Node ID:%d \n",nNextPosition);
#endif
	return nNextPosition;
}

/*
int FindNext(int nCurrentPosition)
{
	int i,j;
	int nTempPos;
	int nNextPosition;
	UpdateDirectionWeight(nCurrentPosition);
	for(i=0; i<8; i++)
	{
		nTempPos = nCurrentPosition + NextOrientation[i];
#ifdef DEBUG_PANJIE
	 	printf("CurrentPos = %d ,tempPos = %d\n",nCurrentPosition,nTempPos);
#endif
		if(nTempPos<0||nTempPos>=N*N || GetD(nCurrentPosition,nTempPos)>2)
		{
			possibility[i] = 0;
		}
		else
			possibility[i] = Map[nCurrentPosition][nTempPos] * DirectionWeight[i] * DirectDistance[i];	
#ifdef DEBUG_PANJIE
	//	printf("possibility[%d]=%f ",i,possibility[i]);
#endif
	}
	double totle = 0;
	for(i=0; i<8; i++)
		totle = totle + possibility[i];

	for(i=0; i<8; i++)
	{
		if(fabs(totle)<0.000001)
			return -1;
		possibility[i] = possibility[i] / totle; 
#ifdef DEBUG_PANJIE
		printf("poss[%d] = %f  ",i,possibility[i]);
#endif
	}
		
	double sum[8];
	for(i=0;i<8;i++)
	{
		sum[i] = 0;
	}
	sum[0] = possibility[0];
	for(i=1; i<8; i++)
	{
		sum[i] = sum[i-1] + possibility[i];
#ifdef DEBUG_PANJIE
		printf("sum[%d]=%f ",i,sum[i]);
#endif
	}

//	printf("\n");

	
	//srand(GetTickCount());
	//for(i=0;i<10;i++)
	//	rand();
	int nRandNum = rand()%1000;
//	printf("nRandNum = %d\n",nRandNum);
	double RandomNumber = nRandNum*1.0/1000;
	for(i=0;i<8;i++)
	{
		if(RandomNumber<=sum[i]&&fabs(possibility[i])>0.000001&& i+HowToGet != 7)//????????????
		{
			HowToGet = i;
		//	printf("index=%d\n",i);
			nNextPosition = nCurrentPosition + NextOrientation[i];
			break;
		}
	}
	if(i==8)//////////////////////////////////////
	{
		//return -1;
	//	HowToGet = i-1;
	//	printf("index=%d\n",i-1);
	//	nNextPosition = nCurrentPosition + NextOrientation[i-1];
	//	return -1;
		for(j=0;j<8;j++)
		{
			if(RandomNumber<=sum[j]&&fabs(possibility[j])>0.000001)//&& j+HowToGet != 7)//????????????
			{
				HowToGet = j;
			//	printf("index=%d\n",j);
				nNextPosition = nCurrentPosition + NextOrientation[j];
				break;
			}
		}
	}
		
#ifdef DEBUG_PANJIE
	printf("RandomNumber=%f,Node ID:%d \n",RandomNumber,nNextPosition);
#endif
	return nNextPosition;
}
*/
int BackTrace(int nCurrentPosition)
{
	if(nCurrentPosition<0||nCurrentPosition>=N*N)
	{
		printf("BackTrace:position Error\n");
		return 0;
	}
	int tempPos;
	if(TempPath.pathLength>1000)
	{
		printf("path to long!\n");
		return 0;
	}
	if(nCurrentPosition == TARGET)
	{
		TempPath.pathNode[TempPath.pathLength++] = nCurrentPosition;
		printf("\nFind one Path\n");
		for(int pathindex=0;pathindex<TempPath.pathLength;pathindex++)
			printf("%d ",TempPath.pathNode[pathindex]);
		printf("\n");
		successful = true;
		return 1;
	}
	else
	{
		TempPath.pathNode[TempPath.pathLength++] = nCurrentPosition;
		tempPos = FindNext(nCurrentPosition);
		if(tempPos == -1)
		 	return 0;
		BackTrace(tempPos);
	}
	return 1;
}

void CopyData(int index)
{
	int i;
	for(i=0; i<TempPath.pathLength; i++)
		AntPath[index].pathNode[i] = TempPath.pathNode[i];
	AntPath[index].pathLength = TempPath.pathLength;

}

void UpdateMap(Path AntPath)
{
	int i;
	for(i=0; i<AntPath.pathLength-1; i++)
	{
		Map[AntPath.pathNode[i]][AntPath.pathNode[i+1]] += MUL;
	}

}
//void GetBarrier()
//{
//	Barrier[0]= ;
//}


void Initial(int Barrier[],int BarrierNum)
{
	int i;
	for(i=0; i<100; i++)
	{	
		if(IsValid(i-11))
		{
			Map[i][i-11] = C;
		}
		if(IsValid(i-10))
		{
			Map[i][i-10] = C;
		}
		if(IsValid(i-9))
		{
			Map[i][i-9] = C;
		}
		if(IsValid(i-1))
		{
			Map[i][i-1] = C;
		}
		if(IsValid(i+1))
		{
			Map[i][i+1] = C;
		}
		if(IsValid(i+9))
		{
			Map[i][i+9] = C;
		}
		if(IsValid(i+10))
		{
			Map[i][i+10] = C;
		}
		if(IsValid(i+11))
		{
			Map[i][i+11] = C;
		}		
	}
	for(i=0; i< BarrierNum; i++)
	{
		if(IsValid(Barrier[i]-11))
		{
			Map[Barrier[i]][Barrier[i]-11] = 0;
			Map[Barrier[i]-11][Barrier[i]] = 0;
		}
		if(IsValid(Barrier[i]-10))
		{
			Map[Barrier[i]][Barrier[i]-10] = 0;
			Map[Barrier[i]-10][Barrier[i]] = 0;
		}
		if(IsValid(Barrier[i]-9))
		{
			Map[Barrier[i]][Barrier[i]-9] = 0;
			Map[Barrier[i]-9][Barrier[i]] = 0;
		}
		if(IsValid(Barrier[i]-1))
		{
			Map[Barrier[i]][Barrier[i]-1] = 0;
			Map[Barrier[i]-1][Barrier[i]] = 0;
		}
		if(IsValid(Barrier[i]+1))
		{
			Map[Barrier[i]][Barrier[i]+1] = 0;
			Map[Barrier[i]+1][Barrier[i]] = 0;
		}
		if(IsValid(Barrier[i]+9))
		{
			Map[Barrier[i]][Barrier[i]+9] = 0;
			Map[Barrier[i]+9][Barrier[i]] = 0;
		}
		if(IsValid(Barrier[i]+10))
		{
			Map[Barrier[i]][Barrier[i]+10] = 0;
			Map[Barrier[i]+10][Barrier[i]] = 0;
		}
		if(IsValid(Barrier[i]+11))
		{
			Map[Barrier[i]][Barrier[i]+11] = 0;
			Map[Barrier[i]+11][Barrier[i]] = 0;
		}		
	}
}

void PathPlanning(int m,int n,int PathNode[],int& nPathLength)
{
	int i,j;
	int nCurrentPosition;//当前位置
	int MinLength,MinIndex;
	srand( (unsigned)time( NULL ) );
	for(j=0;j<n;j++)
	{
		for(i=0; i<m; i++)
		{
			//printf("The %d path:--------\n",j*m+i);
			nCurrentPosition = 0;
			HowToGet = 100;
			TempPath.pathLength = 0;
			successful = false;
			BackTrace(nCurrentPosition);		
			if(successful == true)
				CopyData(i);//save path
			else
				AntPath[i].pathLength = 1500;
		
		}
		MinLength = 1000;
		MinIndex = 0;
		for(i=0;i<m;i++)
		{
			if(AntPath[i].pathLength<MinLength)
			{
				MinLength = AntPath[i].pathLength;
				MinIndex = i;
			}	
		}
		UpdateMap(AntPath[MinIndex]);	
	}
	//output
	printf("\nThe best path length=%d,the node as follows:\n",AntPath[MinIndex].pathLength);
	for(i=0; i<MinLength; i++)
	{
		PathNode[i] = AntPath[MinIndex].pathNode[i];
	}
	nPathLength = MinLength;

}



#endif