基于蚁群算法的机器人路径规划C语言代码

基于蚁群算法的机器人路径规划C语言代码

#include “StdAfx.h“
#include “.\ant.h“

CAnt::CAnt（void）
{
	m_pMapAry=Create2DArray（N_MAX_MAP_WIDTHN_MAX_MAP_HEIGHT）;
}

CAnt::~CAnt（void）
{
	Free2DArray（m_pMapAryN_MAX_MAP_WIDTH）;
}


void CAnt::Init（）
{
	//清空蚂蚁走的路径
	m_tabuAry.RemoveAll（）; 

	//设置蚂蚁走过的地方为地图初始状态
	for （int i=0;i	{
		for （int j=0;j		{
			m_pMapAry[i][j]=g_pMapAry[i][j];
		}
	}

	//加入起始点，并设置起始点为去过了
	m_ptCurrent=g_ptStart;
	m_tabuAry.Add（m_ptCurrent）; 
	m_pMapAry[m_ptCurrent.x][m_ptCurrent.y]=1; 

	//设置完成搜索标志为false
	m_blComplete=false;

	 //蚂蚁走过的路径长度设置为0
	m_dbPathLength=0.0;

}


//检查从（x0y0）到（xy）是否允许
bool CAnt::CheckXY（int x0int y0int xint y）
{
	//越界
	if （（x<0） || （x>=N_MAP_WIDTH））
	{
		return false;
	}

	//越界
	if （（y<0） || （y>=N_MAP_HEIGHT））
	{
		return false;
	}

	//该位置是障碍或者去过了
	if （（m_pMapAry[x][y] == BARRIER） || （m_pMapAry[x][y] == MOVED））
	{
		return false;
	}

	//如果没有越界且位置为空，要检查如果是斜穿，斜穿点的两边是否有障碍

	if （（x-x0 == -1） && （y-y0 ==-1）） //新位置在左上角
	{
		if （（m_pMapAry[x+1][y] == BARRIER） && （m_pMapAry[x][y+1] == BARRIER））
		{
			return false;
		}
	}

	if （（x-x0 == 1） && （y-y0 ==-1）） //新位置在右上角
	{
		if （（m_pMapAry[x-1][y] == BARRIER） && （m_pMapAry[x][y+1] == BARRIER））
		{
			return false;
		}
	}

	if （（x-x0 == -1） && （y-y0 ==1）） //新位置在左下角
	{
		if （（m_pMapAry[x][y-1] == BARRIER） && （m_pMapAry[x+1][y] == BARRIER））
		{
			return false;
		}
	}

	if （（x-x0 == 1） && （y-y0 ==1）） //新位置在右下角
	{
		if （（m_pMapAry[x-1][y] == BARRIER） && （m_pMapAry[x][y-1] == BARRIER））
		{
			return false;
		}
	}

	//上面都不返回，则可以到达
	return true;
}

//选择下一个点
//返回值 为点的坐标位置
CPoint CAnt::ChooseNextCity（）
{

	CPoint pt（-1-1）;  //返回结果，先暂时把其设置为-1-1

	//==============================================================================
	//计算当前点和周围8个点的信息素总和
	//为了用循环进行处理，编码方便，当前所在点也计算一下
	double dbTotal=0.0;	
	double prob[9]={-1-1-1-1-1-1-1-1-1}; // 保存城市被选中的概率

	int nIndex=-1;
	int nCount=0; //周围有几个点可以去

	
	for （int x=m_ptCurrent.x-1;x<=m_ptCurrent.x+1;x++）
	{
		for （int y=m_ptCurrent.y-1;y<=m_ptCurrent.y+1;y++）
		{
			nIndex++;
			if （CheckXY（m_ptCurrent.xm_ptCurrent.yxy） == true）
			{
				prob[nIndex]=g_pMapTrail[x][y]/（g_pMapLen[x][y]*g_pMapLen[x][y]）;
				
				dbTotal=dbTotal+prob[nIndex];
				nCount++;
			}
		}
	}

	//周围没有点可以去，则直接返回
	if （nCount ==0）
	{
		return pt;
	}


	////==============================================================================
	//对周围点进行轮盘选择

	nIndex=-1; //记录周围那个点被选择
	int nIndexFirst=-1; //记录第一个可以去的点
	double dbTemp=rnd（0.0dbTotal）; //取一个随机数
	for （int i=0;i<9;i++）
	{
		if （prob[i]>=0） //点可以去
		{
			dbTemp=dbTemp-prob[i]; //这个操作相当于转动轮盘
			if （dbTemp <= 0.0） //轮盘停止转动，记下点位置，直接跳出循环
			{
				nIndex=i;
				break;
			}

			//保存第一个可以去的点
			//如果没有点选中就用这个点作为结果返回
			if （nIndexFirst == -1） 
			{
				nIndexFirst=i;
			}
		}
	}

	//如果经过上面操作没有点被选中
	//就把第一个可去的点作为结果返回
	if （nIndex == -1）
	{
		nInd

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        880  2011-06-16 21:05  Robot.sln

    ..A..H.     70144  2011-06-23 18:47  Robot.suo

     文件      11127  2011-06-23 14:47  Robot\Ant.cpp

     文件        949  2011-06-20 15:29  Robot\Ant.h

     文件       1516  2011-06-23 14:30  Robot\Common.cpp

     文件       2441  2011-06-23 14:36  Robot\Common.h

     文件       1426  2011-06-21 19:44  Robot\DlgNewMap.cpp

     文件        615  2011-06-21 19:30  Robot\DlgNewMap.h

     文件      13416  2011-06-23 18:45  Robot\MapView.cpp

     文件       1136  2011-06-23 18:39  Robot\MapView.h

     文件       7955  2011-01-19 20:07  Robot\MyButton.cpp

     文件       1250  2011-01-12 13:30  Robot\MyButton.h

     文件       2855  2011-06-16 21:05  Robot\ReadMe.txt

     文件       7734  2011-06-21 19:33  Robot\res\btnbmp.bmp

     文件       3126  2011-06-21 13:12  Robot\res\rbt.bmp

     文件       5430  2011-06-21 18:24  Robot\res\robot.ico

     文件        361  2011-06-16 21:05  Robot\res\Robot.rc2

     文件       1592  2011-06-23 16:26  Robot\resource.h

     文件       1636  2011-06-16 21:05  Robot\Robot.cpp

     文件        435  2011-06-16 21:05  Robot\Robot.h

     文件       6561  2011-06-23 16:28  Robot\Robot.rc

     文件       6628  2011-06-21 19:03  Robot\Robot.vcproj

     文件      13427  2011-06-23 16:28  Robot\RobotDlg.cpp

     文件       1623  2011-06-23 16:26  Robot\RobotDlg.h

     文件        136  2011-06-16 21:05  Robot\stdafx.cpp

     文件       2508  2011-06-16 21:13  Robot\stdafx.h

     文件       4597  2011-06-23 14:48  Robot\Tsp.cpp

     文件        364  2011-06-23 13:41  Robot\Tsp.h

     目录          0  2011-06-21 19:03  Robot\res

     目录          0  2011-06-23 18:47  Robot

............此处省略3个文件信息