基于遗传算法的越野路径导航

这里写自定义目录标题

基于遗传算法的越野路径导航

基于遗传算法的越野路径导航
在这里插入图片描述
在这里插入图片描述

Command.cpp:
#include “common.h”
#include <omp.h>
vector<Mat> Path_matrix;

Mat join_verts(long cnt, …)
{
Mat mat;
va_list params;
va_start(params, cnt);
for (int i = 0; i < cnt; i++)
{
Mat item = va_arg(params, Mat);
mat = join_vert(mat, item);
}
va_end(params);
return mat;
}

Mat join_horizs(long cnt, …)
{
Mat mat;
va_list params;
va_start(params, cnt);
for (int i = 0; i < cnt; i++)
mat = join_horiz(mat, va_arg(params, Mat));
va_end(params);
return mat;
}

field<Mat> join_field(field<Mat> A, field<Mat> B)
{
for (int i = 0; i < B.n_elem; i++)
{
A << B(i);
}
return A;
}

Mat SubMatView(Mat mat, long start, long end)
{
Mat result;
for (int i = 0; i < mat.n_elem; i++)
if (i >= start && i < end)
result << mat(i);
return result;
}
///
/// 计算六边形面积
///
/// 六边形的两条相对的边之间的距离
/// 返回的面积
double area(double x)
{
double area = 0;

area = 0.5 * 2 * ((x / 2) / sqrt(3)) * (x / 2) * 6;

return area;

}
///
/// 邻接网格的编码矩阵
///
/// 表示输入网格的编码
/// 表示输出邻接网格的编码矩阵
Mat adjoin(long code, long col1, long col2, long row, long J_sum)
{
//Mat adCode;
//Mat N = zeros<Mat>(1, 3);//没有网格的点的位置
//long x = 0;
//long y = 0;
1.计算编码所在行列号
//if (code <= J_sum)//奇数行:点编码小于等于奇数行
//{
// y = fmod(code, col1);//纵坐标取余数
// if (y == 0)//是最后一列
// {
// x = 2 * floor(code / (double)col1) - 1;//横坐标取整数部分
// y = col1;//纵坐标重新赋值
// }
// else//不是最后一列
// {
// x = 2 * floor(code / (double)col1) + 1;
// }
//}
//else//偶数行:点编码大于奇数行点的总数
//{
// long a = code - J_sum;//数行点的总数时,需要减去奇数行点数总和,进行偶数行的单独计算
// y = fmod(a, col2);
// if (y == 0)
// {
// x = 2 * floor(a / (double)col2);//偶数行最后一列计算公式
// y = col2;
// }
// else
// {
// x = 2 * (floor(a / (double)col2) + 1);//偶数行非最后一列计算公式
// }
//}
2.计算邻接点
//if (fmod(x, 2) == 0)//偶数行
//{
// //1.
// long x1 = x - 2;
// long y1 = y;
// Mat a1 = { x1,y1 };//坐标
// long item1 = J_sum + (floor(x1 / (double)2) - 1) * col2 + y1;//偶数行坐标转换为编码
// Mat b1 = { item1 };
// Mat adCode1 = join_horiz(a1, b1);//将坐标和编码连成一个行向量
// //2.
// long x2 = x - 1;
// long y2 = y;
// Mat a2 = { x2,y2 };
// long item2 = floor(x2 / 2.0) * col1 + y2;//奇数行坐标转换为编码
// Mat b2 = { item2 };
// Mat adCode2 = join_horiz(a2, b2);
// //3.
// long x3 = x - 1;
// long y3 = y + 1;
// Mat a3 = { x3,y3 };
// long item3 = floor(x3 / 2.0) * col1 + y3;//奇数行坐标转换为编码
// Mat b3 = { item3 };
// Mat adCode3 = join_horiz(a3, b3);
// //4.
// long x4 = x + 1;
// long y4 = y;
// Mat a4 = { x4,y4 };
// long item4 = floor(x4 / 2.0) * col1 + y4;
// Mat b4 = { item4 };
// Mat adCode4 = join_horiz(a4, b4);
// //5.
// long x5 = x + 1;
// long y5 = y + 1;
// Mat a5 = { x5,y5 };
// long item5 = floor(x5 / 2.0) * col1 + y5;
// Mat b5 = { item5 };
// Mat adCode5 = join_horiz(a5, b5);
// //6.
// long x6 = x + 2;
// long y6 = y;
// Mat a6 = { x6,y6 };
// long item6 = J_sum + (floor(x6 / 2.0) - 1) * col2 + y6;
// Mat b6 = { item6 };
// Mat adCode6 = join_horiz(a6, b6);
// //
// if (x == 2)//坐标点的位置是第二行
// {
// if (y == col2 && col1 == col2)//判断坐标是否是第二行最后一列,且单层奇数行的列数和偶数行的列数一致
// adCode = join_verts(6, N, adCode2, N, adCode4, N, adCode6);//六个坐标点位置和编码组合成一个矩阵
// else
// adCode = join_verts(6, N, adCode2, adCode3, adCode4, adCode5, adCode6);
// }
// else if (x == row || x == row - 1)//总行数为偶数坐标点位置是最后一行或者总行数为奇数坐标点位置是倒数第二行
// {
// if (fmod(row, 2) != 0)//总行数是奇数行
// if (y == col2 && col1 == col2)//单个奇数行的网格数等于单个偶数行的网格数时,输入的点为该行最后一个网格
// adCode = join_verts(6, adCode1, adCode2, N, adCode4, N, N);
// else
// adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, N);
// else//总行数是偶数行
// if (y == col2 && col1 == col2)
// adCode = join_verts(6, adCode1, adCode2, N, N, N, N);
// //adCode = join_verts(6, adCode1, adCode2, N, N, N, N);
// else
// adCode = join_verts(6, adCode1, adCode2, adCode3, N, N, N);
// }
// else if (x != 2 && x != row && x != row - 1)//坐标位置既不是第二行也不是最后一行也不是倒数第二行的情况
// {
// if (y == col2 && col1 == col2)
// adCode = join_verts(6, adCode1, adCode2, N, adCode4, N, adCode6);
// else
// adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, adCode6);
// }
// else
// {
// adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, adCode6);
// }
//}
//else//奇数行
//{
// //1.
// long x1 = x - 2;
// long y1 = y;
// Mat a1 = { x1, y1 };
// long item1 = floor(x1 / 2.0) * col1 + y1;// 奇数行坐标转换为编码
// Mat b1 = { item1 };
// Mat adCode1 = join_horiz(a1, b1);
// //2.
// long x2 = x - 1;
// long y2 = y - 1;
// Mat a2 = { x2, y2 };
// long item2 = J_sum + (floor(x2 / 2.0) - 1) * col2 + y2;// 偶数行坐标转换为编码
// Mat b2 = { item2 };
// MatadCode2 = join_horiz(a2, b2);
// //3.
// long x3 = x - 1;
// long y3 = y;
// Mat a3 = { x3, y3 };
// long item3 = J_sum + (floor(x3 / 2.0) - 1) * col2 + y3;// 偶数行坐标转换为编码
// Mat b3 = { item3 };
// Mat adCode3 = join_horiz(a3, b3);
// //4.
// long x4 = x + 1;
// long y4 = y - 1;
// Mat a4 = { x4, y4 };
// long item4 = J_sum + (floor(x4 / 2.0) - 1) * col2 + y4;// 偶数行坐标转换为编码
// Mat b4 = { item4 };
// Mat adCode4 = join_horiz(a4, b4);
// //5.
// long x5 = x + 1;
// long y5 = y;
// Mat a5 = { x5, y5 };
// long item5 = J_sum + (floor(x5 / 2.0) - 1) * col2 + y5;// 偶数行坐标转换为编码
// Mat b5 = { item5 };
// Mat adCode5 = join_horiz(a5, b5);
// //6.
// long x6 = x + 2;
// long y6 = y;
// Mat a6 = { x6, y6 };
// long item6 = floor(x6 / 2.0) * col1 + y6;// 奇数行坐标转换为编码
// Mat b6 = { item6 };
// Mat adCode6 = join_horiz(a6, b6);
// //
// if (x == 1)//输入点在第一行
// {
// if (y == col1 && col1 != col2)//点在第一行的最后一个位置,且单个奇数行的网格数不等于单个偶数行的网格数
// //adCode = join_verts(6, N, N, N, adCode4, adCode5, adCode6);
// adCode = join_verts(6, N, N, N, adCode4, N, adCode6);
// else if (y == 1)//输入点在第一行第一个网格位置
// adCode = join_verts(6, N, N, N, N, adCode5, adCode6);
// else //输入点在第一行其他位置
// adCode = join_verts(6, N, N, N, adCode4, adCode5, adCode6);
// }
// else if (x == row || x == row - 1)
// {
// if (fmod(row, 2) != 0) // 总行数是奇数
// if (y == col1 & col1 != col2) // 单个奇数行的网格数不等于单个偶数行的网格数时,输入的点为该行最后一个网格
// adCode = join_verts(6, adCode1, adCode2, N, N, N, N);
// else if (y == 1) // 输入点在最后一行第一个网格位置
// adCode = join_verts(6, adCode1, N, adCode3, N, N, N);
// else// 其他情况都一致
// adCode = join_verts(6, adCode1, adCode2, adCode3, N, N, N);
// else// 总行数是偶数
// if (y == col1 & col1 != col2)
// adCode = join_verts(6, adCode1, adCode2, N, adCode4, N, N);
// else if (y == 1)
// adCode = join_verts(6, adCode1, N, adCode3, N, adCode5, N);
// else
// adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, N);
// }
// else if (x != 1 && x != row && x != row - 1)//既不是第一行也不是最后一行也不是倒数第二行
// {
// if (y == col1 & col1 != col2)
// adCode = join_verts(6, adCode1, adCode2, N, adCode4, N, adCode6);
// else if (y == 1)
// adCode = join_verts(6, adCode1, N, adCode3, N, adCode5, adCode6);
// else
// adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, adCode6);
// }
// else
// {
// adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, adCode6);
// }
//}
//return adCode;
return Path_matrix[code];
}
///
/// 计算种群适应度矩阵
///
/// 初始种群矩阵
/// 参与计算的种群数量
/// 种群适应度矩阵
Mat adjust(field<Mat> vtemp, long n)
{
Mat fit;
Mat distance;//定义距离矩阵
//计算两个点之间的距离

for (int i = 0; i < n; i++)
{
	Mat<long> item = { (long)vtemp(i).n_elem };
	distance = join_vert(distance, item);
}
//计算适应度矩阵
for (int i = 0; i < n; i++)
{
	double fit1 = 1.0 / distance(i);
	Mat<double> item = { fit1 };
	fit = join_vert(fit, item);
}

return fit;

}
///
/// 根据适应度函数值选择要进入遗传过程的基因,结合适应度和轮盘赌方式进行筛选,选择每一代最优的10个进行保留,其他的通过轮盘赌进行挑选
///
/// 种群染色体矩阵
/// 每一代中最优的染色体个数
/// 每一代中剩余的染色体个数
///
bool compare(Mat a, Mat b)
{
return a(0)>b(0);
}
field<Mat> choose(field<Mat> vt, long num1, long num2)
{
srand((int)time(0));
vector<Mat> mat;
//1.
vector<Mat> f;
vector<Mat> V1;
vector<Mat> vtemp;
Mat fit1 = adjust(vt, vt.n_elem);

for (int i = 0; i < vt.size(); i++)
{
	Mat<double> mat1 = { (double)i };
	Mat<double> mat2 = { fit1(i) };
	Mat<double> mat;
	mat= join_horiz(mat2, mat1);
	vtemp.push_back(vt(i));
	f.push_back(mat);
}
//2.
sort(f.begin(), f.end(), compare);
vector<Mat<double>> topfit = vector<Mat<double>>(f.begin(), f.size() > num1 ?f.begin() + num1:f.begin()+f.size());//解决f中元素不够10个的错误
Mat<double> index;

	for (int i = 0; i < num1; i++) {
		if (i < topfit.size()) {
			Mat<double> z = { topfit[i](1) };
			//#pragma omp critical
			index = join_vert(index, z);
		}
	}


	for (int i = 0; i < num1; i++)
	{
		if (i < index.n_elem) {
			Mat<long> z = vtemp[index(i)];
			V1.push_back(z);
		}
	}

//3.
int ii = vtemp.size();
for (int i = 0; i < num1 ; i++)
{
	if(i<ii)
		vtemp[index(i)] = Mat<long>();
}
vector<Mat<long>> temp;
for (int i = 0; i < vtemp.size(); i++) {
	if (!vtemp[i].is_empty())
		temp.push_back(vtemp[i]);
}
vtemp = temp;

//5.
vector<Mat<long>> V2;
long m = vtemp.size();//计算经过交叉变异后的种群的数量
Mat<double> fit = adjust(vtemp, m);//计算各种群的适应度
fit = sort(fit);
Mat<double> sumFit = sum(fit, 0);//计算总适应度
Mat<double> accP = cumsum(fit / sumFit(0));//累积概率

for (int i = 0; i < num2; i++)
{
	uvec matrix = find(accP > random());//找到比随机数大的累积概率
	if (matrix.is_empty())
		continue;
	double value = matrix(0);
	V2.push_back(vtemp[value]);
}

for (auto t : V1)
	mat.push_back(t);
for (auto t : V2)
	mat.push_back(t);
return mat;

}

///
/// 生成一条随机路径
///
/// 为初始位置编码
/// 为目标位置编码
///
Mat initalization(long firstCode, long endCode, long col1, long col2, long row, long J_sum)
{
Mat V;
Mat randRoad = { firstCode };//路径矩阵
long cpoint = firstCode;//当前点位的编码,初始设为初始编码
while (cpoint != endCode)//如果当前节点不是最后一个
{
Mat codeSet = adjoin(cpoint, col1, col2, row, J_sum);//生成当前点的连接点位及点位编码
Mat code1 = codeSet.col(2);//读取所有连接点位的编码
uvec code2 = find(code1);//筛选出六个邻接点中不为0的编码在code1中的位置
Mat code3 = code1(code2);//提取code1相应位置的编码
Mat code4;//定义一个空的矩阵用于存放满足条件的编码

	for (int i = 0; i < code3.n_elem; i++)
	{
		if (!any(vectorise(randRoad) == code3(i)))//判断路径矩阵中是否有该编码,等于0表示没有,将不存在路径矩阵中的编码筛选出来
		{
			Mat<long> item = { code3(i) };
			code4 =join_horiz(code4,item);//将路径矩阵中没有出现过的编码存入code4中--水平加入
		}
	}
	if (code4.is_empty())//判断如果一个点周围没有满足条件的编码,即所有点都不可用,就将参数初始化,跳出循环,重新开始
	{
		randRoad = { firstCode };
		cpoint = firstCode;
		continue;
	}
	int randValue = floor(random()*numel(code4)) + 1;  //生成剩余编码个数到1之间的随机数											
	long codeFin = code4(randValue - 1);               //提取满足要求实际格网点的六边形网格的编码
	Mat<long> item = { codeFin };
	randRoad = join_horiz(randRoad, item);             //将选中的六边形格网点的编码点加入路径矩阵中
	cpoint = codeFin;          //更新当前点的编码    
}
V = randRoad;
return V;           //输出结果

}

///
///依据 DEM 判断爬坡高度是否超过能力阈值
///
///DEM文件存放路径
///爬坡阈值
///路径
///返回0:超过阈值;1:未超过阈值
bool isAcceptable(Mat DEMdata, double Threshold, Mat RoadPath)
{
long N_nodes = RoadPath.n_cols; //列数就是一条路径的节点个数量
for (int i = 1; i < N_nodes; i++)
{
long tempT = DEMdata(RoadPath(i) - 1, 0) - DEMdata(RoadPath(i - 1) - 1, 0); // 计算路径爬坡
if (tempT > Threshold)
{
return false;
}
}
DEMdata.clear();
return true;
}

//ismember函数用来检测元素是否在集合中
bool isMember(long t1, Mat t2)
{
bool flag;
for (int i = 0; i < t2.n_elem; i++)
{
if (t1 == t2(i))
{
flag = true;
break;
}
else
{
flag = false;
}
}
return flag;
}

///
///该函数用于优化尖角,以及去除转一圈回到路径中某个节点的邻接节点的情况
///
///被选中做遗传的种群
///文件路径
///爬坡阈值
///
field<Mat> optimize(field<Mat> vtemp, Mat DEMdata, double Threshold, long col1, long col2, long row, long J_sum)
{
vector<Mat> V;
vector<Mat> V2;
for (auto r : vtemp)
V2.push_back®;

for (int i = 0; i < vtemp.n_elem; i++)
{
	
	Mat<long> vtemp1 = vtemp(i);//读取元胞中的单条染色体
	Mat<long> qchild;//用于进行去环处理的矩阵
			for (int j = 0; j < vtemp1.n_elem; j++)
			{
				Mat<long> t;
				t << vtemp1(j);
				qchild = join_horiz(qchild, t);//往去环操作矩阵中一次加入输入的单条染色体的每个元素
				long l = qchild.n_elem;//计算去环矩阵的长度 
				if (l >= 3)//当去环矩阵长度大于3的时候就要判断第三个元素是否是第一个元素的邻接元素
				{
					for (int m = 0; m < qchild.n_elem - 2; m++)//从离新加入元素往前面数的第二个元素开始判断
					{

						Mat<long> codeSet = adjoin(qchild(m), col1, col2, row, J_sum);//依次往前计算是否是去环矩阵中已经存在元素的邻接元素
						Mat<long> codes = codeSet.col(2);//codes取了第三列,对应的就是六角格网对应的ID
						if (isMember(vtemp1(j), codes))
						{
							Mat<long> a, b;
							for (int k = 0; k <= m; k++)//如果是就将新加入元素于邻接元素之间的所有元素都消除
							{
								b << qchild(k);
								a = join_horiz(a, b);
							}
							a = join_horiz(a, t);
							qchild = a;
							break;
						}
					}
				}
			}
		

			if (isAcceptable(DEMdata, Threshold, qchild))//爬坡高度没有超过阈值,则记录
			{
				V.push_back(qchild);//将交叉变换去环后的子代加入到原始种群中
			}
			else
			{
				V.push_back(vtemp1);
			}
	
}
return V;

}
///
/// 变异操作
///
field<Mat> mutation(field<Mat> vtemp, double Pm, long startpoint, long endpoint, Mat DEMdata, long Threshold, long col1, long col2, long row, long J_sum) {

srand((int)time(0));
vector<Mat<long>> V1;
for (auto t : vtemp)
	V1.push_back(t);
for (int i = 0; i < numel(vtemp); i++) {
	// 随机数小于变异率就进行变异
	if (random() < Pm) {
		Mat<long> parent = vtemp(i);
		long mLength = numel(parent);
		long k = floor(random() * mLength); // 随机产生变异位置
		long mcode = parent(k);  // 获取变异位置的编码
		if (k == mLength - 1) {
			cout << endl;
		}
		Mat<long> ranroad1;
		ranroad1 = parent.cols(0, k);
		Mat<long> ranroad2;
		while (true) {
			ranroad2 = initalization(mcode, endpoint, col1, col2, row, J_sum); // 产生变异点到终止点的路径
			if (ranroad2.n_elem < 2) {
				cout << "mutation错误" << endl;
			}
			if (isAcceptable(DEMdata, Threshold, ranroad2)) // 爬坡高度没有超过阈值, 则记录
				break;
			ranroad2.clear();
		}

		//这里出现过bug,ranroad2里面是否会只有一个值
		Mat<long> ranroad3;
		if (ranroad2.n_elem >= 2) {
			ranroad2 = ranroad2.cols(1, numel(ranroad2) - 1);
			ranroad3 = join_horiz(ranroad1, ranroad2); // 连接路径
		}
		else
			ranroad3 = ranroad1;

		for (int j = 0; j < numel(ranroad3); j++) {
			uvec sameindex = find(ranroad3 == ranroad3(j));
			if (numel(sameindex) == 2) {
				Mat<long> temp1 = ranroad3.cols(0, sameindex(0));
				Mat<long> temp2 = ranroad3.cols(sameindex(1) + 1, numel(ranroad3) - 1);
				ranroad3 = join_horiz(temp1, temp2);
			}
		}
		if (ranroad3.n_elem < 2) {
			cout << "mutation错误" << endl;
		}
		V1.push_back(ranroad3);
	}
}
return V1;

}

///
/// 交叉操作
///
/// 被选中做遗传操作的种群
/// 交叉率
/// 起点
/// 终点
///
field<Mat> crossover(field<Mat> vtemp, double pc, long startpoint, long endpoint) {

srand((int)time(0));
vector<Mat<long>> vtempnow;
vector<Mat<long>> V1;

for (auto t : vtemp) {
	vtempnow.push_back(t);
	if (t.n_elem <= 2)
		continue;
	V1.push_back(t);
}

for (int i = 1; i < numel(vtemp); i += 2)
{
	int r1 = floor(random() * vtempnow.size()); // 产生一个随机数,随机选择一条父代1号染色体
	Mat<long> parent1 = vtempnow[r1];//提取父代1号
	vtempnow.erase(vtempnow.begin() + r1);      //将父代1号染色体移除,避免再次交叉选中
	int r2 = floor(random() *  vtempnow.size()); // 产生一个随机数,随机选择一条父代2号染色体
	Mat<long> parent2 = vtempnow[r2];//提取父代2号
	vtempnow.erase(vtempnow.begin() + r2);        //将父代2号染色体移除,避免再次交叉选中

	Mat<long> T;
	uvec iparent1, iparent2;

	intersect(T, iparent1, iparent2, parent1, parent2);

	int size = numel(iparent1);
	Mat<long> B(size, 1), C(size, 1);

	for (int k = 0; k < numel(iparent1); k++) {
		B(k) = iparent1(k);
		C(k) = iparent2(k);
	}

	Mat<long> tA, A;
	tA = join_horiz(T.t(), B, C);
	Mat<uword> index_start = find(tA.col(0) == startpoint);//找到起始点在A中所在的位置
	Mat<uword> index_end = find(tA.col(0) == endpoint);//找到终止点在A中所在的位置
	int index = 0;
	for (int j = 0; j < tA.n_rows; j++) {//去除起始点和终止点
		if (j != index_start(0) && j != index_end(0)) {
			A.insert_rows(index, tA.row(j));
			index++;
		}
	}

	if (A.is_empty()) continue;

	Mat<long> child1;
	Mat<long> child2;
	if (random() < pc)
	{
		long num = A.n_rows > A.n_cols ? A.n_rows : A.n_cols;
		Mat<long> a = A.col(1);
		Mat<long> b = A.col(2);
		long k = floor(random() * a.n_elem);
		if (k > num) continue;
		//child1

		child1 = join_horiz(parent1.submat(0, 0, parent1.n_rows - 1, a(k)), parent2.submat(0, b(k) + 1, parent2.n_rows - 1, parent2.n_cols - 1));
		//child2

		child2 = join_horiz(parent2.submat(0, 0, parent2.n_rows - 1, b(k)), parent1.submat(0, a(k) + 1, parent1.n_rows - 1, parent1.n_cols - 1));
		//去环

		for (int j = 0; j < child1.n_elem; j++)
		{

			Mat<uword> sameindex = find(child1 == child1(j));
			if (sameindex.n_elem >= 2)

				child1 = join_horiz(child1.submat(0, 0, child1.n_rows - 1, sameindex(0)), child1.submat(0, sameindex(1) + 1, child1.n_rows - 1, child1.n_cols - 1));
		}

		for (int j = 0; j < child2.n_elem; j++)
		{
			Mat<uword> sameindex = find(child2 == child2(j));
			if (sameindex.n_elem >= 2)

				child2 = join_horiz(child2.submat(0, 0, child2.n_rows - 1, sameindex(0)), child2.submat(0, sameindex(1) + 1, child2.n_rows - 1, child2.n_cols - 1));
		}
		if (child1.n_elem <= 2 || child2.n_elem <= 2) {
			cout << "crossover错误" << endl;
			continue;
		}
		V1.push_back(child1);
		V1.push_back(child2);
	}
}

// 奇数
if (vtemp.n_elem % 2 != 0) {
	V1.push_back(vtemp(vtemp.n_elem - 1));
}

return V1;

}

struct myComp
{
//仿函数
bool operator()(const Mat a, const Mat b)
{
if (a.n_cols != b.n_cols)
return true;
else {
for (int i = 0; i < a.n_cols; i++) {
if (a(i) != b(i))
return true;
}

		return false;
	}
}

};
///
/// 去重操作
///
/// 被选中做去重操作的种群
///
typedef struct {
Mat aa;
}MMAT;

field<Mat> remove_duplication(field<Mat> vtemp) {

vector<Mat<long>> V1;

//set<Mat<long>, myComp> mat_set;
//for (auto i : vtemp)
//	mat_set.insert(i);

//for (auto i : mat_set)
//	V1.push_back(i);

// 去重


if (vtemp.is_empty())
	return vtemp;
		for (int i = 0; i < vtemp.n_elem; i++) {
			Mat<long> temp = vtemp[i];
			bool flag = true;

			for (int j = 0; j < V1.size(); j++) {
				Mat<long> A = V1[j];
				if (A.n_cols == temp.n_cols) {
					//continue;
				//else {
					bool flagA = false;
					for (int k = 0; k < temp.n_cols; k++) {
						if (A(k) != temp(k)) {

							flagA = true;
							break;
						}
					}
					if (!flagA)

						flag = false;
				}

			}
			if (flag)
			{
				V1.push_back(temp);
				
			}
		}



return V1;

}

#include “common.h”
#include “pathnessAPIs.h”
#include
#include
#include <direct.h>
#include “…\tinyxml\tinyxml.h”
#include “omp.h”
#include
#include
using namespace std;
//
static string g_config = “config.xml”;
//
void Navigation_Config_Init()
{
char dir[_MAX_PATH];
memset(dir, 0, sizeof(char)_MAX_PATH);
_getcwd(dir, _MAX_PATH);
string path = dir;
path.append("\");
path.append(g_config);
g_config = path;
}
//
void Navigation_Config_GetBase(int &nsize, int &iterator)
{
TiXmlDocument doc;
if (doc.LoadFile(g_config.c_str()))
{
TiXmlElement
root = doc.RootElement();
TiXmlElement* it = root->FirstChildElement();
while (it != NULL)
{
string name = it->Value();
if (name.compare(“nsize”) == 0)
{
const char* val = it->Attribute(“value”);
nsize = atoi(val);
}
else if (name.compare(“iterator”) == 0)
{
const char* val = it->Attribute(“value”);
iterator = atoi(val);
}
it = it->NextSiblingElement();
}
}

}
//
void Navigation_Config_GetRate(double &crossrate, double &mutationrate)
{
TiXmlDocument doc;
if (doc.LoadFile(g_config.c_str()))
{
TiXmlElement* root = doc.RootElement();
TiXmlElement* it = root->FirstChildElement();
while (it != NULL)
{
string name = it->Value();
if (name.compare(“crossrate”) == 0)
{
const char* val = it->Attribute(“value”);
crossrate = atoi(val);
}
else if (name.compare(“mutationrate”) == 0)
{
const char* val = it->Attribute(“value”);
mutationrate = atoi(val);
}
it = it->NextSiblingElement();
}
}
}
//
void Navigation_Config_GetThreshold(double &threshold)
{
TiXmlDocument doc;
if (doc.LoadFile(g_config.c_str()))
{
TiXmlElement* root = doc.RootElement();
TiXmlElement* it = root->FirstChildElement();
while (it != NULL)
{
string name = it->Value();
if (name.compare(“threshold”) == 0)
{
const char* val = it->Attribute(“value”);
threshold = atoi(val);
break;
}
it = it->NextSiblingElement();
}
}
}
//
void Navigation_Config_GetData(long &oddcol, long &evencol, long &rows, long &oddpoint)
{
TiXmlDocument doc;

if (doc.LoadFile(g_config.c_str()))
{
	TiXmlElement* root = doc.RootElement();
	TiXmlElement* it = root->FirstChildElement();
	while (it != NULL)
	{
		string name = it->Value();
		if (name.compare("oddcol") == 0)
		{
			const char* val = it->Attribute("value");
			oddcol = atoi(val);
		}
		else if (name.compare("evencol") == 0)
		{
			const char* val = it->Attribute("value");
			evencol = atoi(val);
		}
		else if (name.compare("rows") == 0)
		{
			const char* val = it->Attribute("value");
			rows = atoi(val);
		}
		else if (name.compare("oddpoint") == 0)
		{
			const char* val = it->Attribute("value");
			oddpoint = atoi(val);
		}
		it = it->NextSiblingElement();
	}
}

}
//-----计算均值、方差、标准差----------

Mat Myadjoin(Mat DEMdata, long code, long col1, long col2, long row, long J_sum)
{
Mat adCode;
Mat N = zeros<Mat>(1, 3);//没有网格的点的位置
long x = 0;
long y = 0;
//1.计算编码所在行列号
if (code <= J_sum)//奇数行:点编码小于等于奇数行
{
y = fmod(code, col1);//纵坐标取余数
if (y == 0)//是最后一列
{
x = 2 * floor(code / (double)col1) - 1;//横坐标取整数部分
y = col1;//纵坐标重新赋值
}
else//不是最后一列
{
x = 2 * floor(code / (double)col1) + 1;
}
}
else//偶数行:点编码大于奇数行点的总数
{
long a = code - J_sum;//数行点的总数时,需要减去奇数行点数总和,进行偶数行的单独计算
y = fmod(a, col2);
if (y == 0)
{
x = 2 * floor(a / (double)col2);//偶数行最后一列计算公式
y = col2;
}
else
{
x = 2 * (floor(a / (double)col2) + 1);//偶数行非最后一列计算公式
}
}
//2.计算邻接点
if (fmod(x, 2) == 0)//偶数行
{
//1.
long x1 = x - 2;
long y1 = y;
Mat a1 = { x1,y1 };//坐标
long item1 = J_sum + (floor(x1 / (double)2) - 1) * col2 + y1;//偶数行坐标转换为编码
Mat b1 = { item1 };
Mat adCode1 = join_horiz(a1, b1);//将坐标和编码连成一个行向量
//2.
long x2 = x - 1;
long y2 = y;
Mat a2 = { x2,y2 };
long item2 = floor(x2 / 2.0) * col1 + y2;//奇数行坐标转换为编码
Mat b2 = { item2 };
Mat adCode2 = join_horiz(a2, b2);
//3.
long x3 = x - 1;
long y3 = y + 1;
Mat a3 = { x3,y3 };
long item3 = floor(x3 / 2.0) * col1 + y3;//奇数行坐标转换为编码
Mat b3 = { item3 };
Mat adCode3 = join_horiz(a3, b3);
//4.
long x4 = x + 1;
long y4 = y;
Mat a4 = { x4,y4 };
long item4 = floor(x4 / 2.0) * col1 + y4;
Mat b4 = { item4 };
Mat adCode4 = join_horiz(a4, b4);
//5.
long x5 = x + 1;
long y5 = y + 1;
Mat a5 = { x5,y5 };
long item5 = floor(x5 / 2.0) * col1 + y5;
Mat b5 = { item5 };
Mat adCode5 = join_horiz(a5, b5);
//6.
long x6 = x + 2;
long y6 = y;
Mat a6 = { x6,y6 };
long item6 = J_sum + (floor(x6 / 2.0) - 1) * col2 + y6;
Mat b6 = { item6 };
Mat adCode6 = join_horiz(a6, b6);
//
if (x == 2)//坐标点的位置是第二行
{
if (y == col2 && col1 == col2)//判断坐标是否是第二行最后一列,且单层奇数行的列数和偶数行的列数一致
adCode = join_verts(6, N, adCode2, N, adCode4, N, adCode6);//六个坐标点位置和编码组合成一个矩阵
else
adCode = join_verts(6, N, adCode2, adCode3, adCode4, adCode5, adCode6);
}
else if (x == row || x == row - 1)//总行数为偶数坐标点位置是最后一行或者总行数为奇数坐标点位置是倒数第二行
{
if (fmod(row, 2) != 0)//总行数是奇数行
if (y == col2 && col1 == col2)//单个奇数行的网格数等于单个偶数行的网格数时,输入的点为该行最后一个网格
adCode = join_verts(6, adCode1, adCode2, N, adCode4, N, N);
else
adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, N);
else//总行数是偶数行
if (y == col2 && col1 == col2)
adCode = join_verts(6, adCode1, adCode2, N, N, N, N);
//adCode = join_verts(6, adCode1, adCode2, N, N, N, N);
else
adCode = join_verts(6, adCode1, adCode2, adCode3, N, N, N);
}
else if (x != 2 && x != row && x != row - 1)//坐标位置既不是第二行也不是最后一行也不是倒数第二行的情况
{
if (y == col2 && col1 == col2)
adCode = join_verts(6, adCode1, adCode2, N, adCode4, N, adCode6);
else
adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, adCode6);
}
else
{
adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, adCode6);
}
}
else//奇数行
{
//1.
long x1 = x - 2;
long y1 = y;
Mat a1 = { x1, y1 };
long item1 = floor(x1 / 2.0) * col1 + y1;// 奇数行坐标转换为编码
Mat b1 = { item1 };
Mat adCode1 = join_horiz(a1, b1);
//2.
long x2 = x - 1;
long y2 = y - 1;
Mat a2 = { x2, y2 };
long item2 = J_sum + (floor(x2 / 2.0) - 1) * col2 + y2;// 偶数行坐标转换为编码
Mat b2 = { item2 };
MatadCode2 = join_horiz(a2, b2);
//3.
long x3 = x - 1;
long y3 = y;
Mat a3 = { x3, y3 };
long item3 = J_sum + (floor(x3 / 2.0) - 1) * col2 + y3;// 偶数行坐标转换为编码
Mat b3 = { item3 };
Mat adCode3 = join_horiz(a3, b3);
//4.
long x4 = x + 1;
long y4 = y - 1;
Mat a4 = { x4, y4 };
long item4 = J_sum + (floor(x4 / 2.0) - 1) * col2 + y4;// 偶数行坐标转换为编码
Mat b4 = { item4 };
Mat adCode4 = join_horiz(a4, b4);
//5.
long x5 = x + 1;
long y5 = y;
Mat a5 = { x5, y5 };
long item5 = J_sum + (floor(x5 / 2.0) - 1) * col2 + y5;// 偶数行坐标转换为编码
Mat b5 = { item5 };
Mat adCode5 = join_horiz(a5, b5);
//6.
long x6 = x + 2;
long y6 = y;
Mat a6 = { x6, y6 };
long item6 = floor(x6 / 2.0) * col1 + y6;// 奇数行坐标转换为编码
Mat b6 = { item6 };
Mat adCode6 = join_horiz(a6, b6);
//
if (x == 1)//输入点在第一行
{
if (y == col1 && col1 != col2)//点在第一行的最后一个位置,且单个奇数行的网格数不等于单个偶数行的网格数
//adCode = join_verts(6, N, N, N, adCode4, adCode5, adCode6);
adCode = join_verts(6, N, N, N, adCode4, N, adCode6);
else if (y == 1)//输入点在第一行第一个网格位置
adCode = join_verts(6, N, N, N, N, adCode5, adCode6);
else //输入点在第一行其他位置
adCode = join_verts(6, N, N, N, adCode4, adCode5, adCode6);
}
else if (x == row || x == row - 1)
{
if (fmod(row, 2) != 0) // 总行数是奇数
if (y == col1 && col1 != col2) // 单个奇数行的网格数不等于单个偶数行的网格数时,输入的点为该行最后一个网格
adCode = join_verts(6, adCode1, adCode2, N, N, N, N);
else if (y == 1) // 输入点在最后一行第一个网格位置
adCode = join_verts(6, adCode1, N, adCode3, N, N, N);
else// 其他情况都一致
adCode = join_verts(6, adCode1, adCode2, adCode3, N, N, N);
else// 总行数是偶数
if (y == col1 && col1 != col2)
adCode = join_verts(6, adCode1, adCode2, N, adCode4, N, N);
else if (y == 1)
adCode = join_verts(6, adCode1, N, adCode3, N, adCode5, N);
else
adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, N);
}
else if (x != 1 && x != row && x != row - 1)//既不是第一行也不是最后一行也不是倒数第二行
{
if (y == col1 && col1 != col2)
adCode = join_verts(6, adCode1, adCode2, N, adCode4, N, adCode6);
else if (y == 1)
adCode = join_verts(6, adCode1, N, adCode3, N, adCode5, adCode6);
else
adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, adCode6);
}
else
{
adCode = join_verts(6, adCode1, adCode2, adCode3, adCode4, adCode5, adCode6);
}
}
Mat temp;
for (int i = 0; i < 6; i++) {

	if (adCode(i, 2) != 0 && DEMdata(adCode(i, 2) - 1) != emptyValue)
		temp = join_vert(temp, adCode.row(i));
	else {
		temp = join_vert(temp, N);
	}
}
return temp;

}

void init_Path_matrix(Mat DEMdata, long oddcol, long evencol, long rows, long oddpoint) {
int totalPoints = oddpoint + evencol * (rows - oddpoint / oddcol);
Path_matrix = vector<Mat>(totalPoints + 1);
for (int i = 1; i <= totalPoints; i++) {
Path_matrix[i] = Myadjoin(DEMdata, i, oddcol, evencol, rows, oddpoint);
}
}

void remove_duplication(vector<Mat>& vtemp, Mat target) {

// 去重
for (int i = 0; i < vtemp.size(); i++) {
	Mat<long> temp = vtemp[i];
	if (target.n_cols != temp.n_cols)
		continue;
	else {
		bool flag = true;
		for (int k = 0; k < temp.n_cols; k++) {
			if (target(k) != temp(k))
				break;
		}
		if (flag) {
			return;
		}
	}
}
vtemp.push_back(target);

}

///
///入口函数
///
///文件路径
///初始点编码
///终止点编码>
///
void Navigation(const char* dem_path, long SP, long EP, vector &codes)
{
//环境初始化
Navigation_Config_Init();
//初始参数设置
int n = 100;//初始种群规模
int ger = 30;//迭代次数
Navigation_Config_GetBase(n, ger);
double pc = 0.9;//交叉概率
double pm = 0.45;//变异概率
//Navigation_Config_GetRate(pc, pm);
long startpoint = SP;//初始点编码
long endpoint = EP;//终止点编码
double Threshold = 100;//爬坡阈值
Navigation_Config_GetThreshold(Threshold);
string DEM_filepath = dem_path;//高程数据文件路径
long col1 = 44;//奇数行的列数
long col2 = 44;//偶数行的列数
long row = 126;//总行数
long J_sum = 2772;//奇数行点的总数
Navigation_Config_GetData(col1, col2, row, J_sum);
//初始化矩阵数据
Mat DEMdata;
DEMdata.load(dem_path, csv_ascii);//读取DEM 从文件中加载矩阵
double st = omp_get_wtime();
// 初始化栅格矩阵
init_Path_matrix(DEMdata, col1, col2, row, J_sum);
//生成初始种群
field<Mat> V;
V.reset(); //置空
vector<Mat> vec;
//初始化时间种子
srand((unsigned int)time(NULL));

//vector<thread>mythreaeds;        //用一个容器来管理线程
//for(int i=10; i <10; i++)
//{
//	mythreaeds.push_back(thread()); //创建10个线程,同时这10个线程已经开始执行
//}
//for (auto iter=mythreaeds.begin(); iter!= mythreaeds.end(); iter++)
//{
//	iter->join();                  //等待10个线程都返回
//}

//种群初始化部分种群规模为n(染色体个数)
while (n > 0) 
{
	Mat<long> s = initalization(startpoint, endpoint, col1, col2, row, J_sum);
	if(isAcceptable(DEMdata,Threshold, s))//爬坡高度没有超过阈值,则记录
	{		
		vec.push_back(s);
		V=remove_duplication(vec); //去除初始重复染色体
	} 
	if (V.size() == n)    //n=100相当于要有100条染色体
	{
		break;
	}

}


Mat<double> fit = adjust(V, n);//计算初始n个种群的适应度
Mat<double> std_Deviation = stddev(fit);//计算出第一代的标准差
Mat<double> media = mean(fit);//计算出第一代的适应度均值
							
Mat<double> vmfit = { media };        //存放适应度的均值
Mat<double> vsfit = { std_Deviation };//存放标准差的均值
Mat<double> vx;                    //存放每个子代适应度的最大值
double sol;                     //最大值
rowvec indb;                    //对应的行数
int it = 1;                     //控制迭代数目
while (it <= ger)  //ger=30相当于进化30次
{
	/*Crossover 交叉原则:将种群中的染色体按一定的比例取出,然后两两组成一对,将这对染色体中从某一列开始到最后互换
	a b c d e f g h
eg: 第一条染色体为  1 0 3 4 0 5 2 6
	第二条染色体为  1 0 2 4 5 0 3 6
	根据交叉原则互换后为  从第 3 列(c)开始互换
	重新排序
	第一条染色体    a g C d f h
	第二条染色体    a C g d e h
	交叉
	第一条染色体    a g g d e h
	第二条染色体    a C C d f h
	去除环路
	第一条染色体    a g d e h
	第二条染色体    a C d f h
	*/
	field<Mat<long>> vtemp1 = crossover(V, pc, startpoint, endpoint);
	vector<Mat<long>> vec;
	for (auto t : vtemp1)
		vec.push_back(t);
	vtemp1 = remove_duplication(vtemp1);//去除重复的染色体

	//Mutation 变异原则:根据M=rand(N,L)<=pm;产生一个随机矩阵,其中N为种群数,L为染色体元素个数,pm为变异概率
	field<Mat<long>> vtemp2 = mutation(vtemp1, pm, startpoint, endpoint, DEMdata, Threshold, col1, col2, row, J_sum);
	vtemp2 = remove_duplication(vtemp2);//去除重复的染色体

	//根据适应度函数值选择要进入遗传过程的基因
	field<Mat<long>> vtemp3;

		vtemp3 = choose(vtemp2, vtemp2.n_elem / 2, vtemp2.n_elem - vtemp2.n_elem / 2); // 这里
		//调用去环优化函数

		V = optimize(vtemp3, DEMdata, Threshold, col1, col2, row, J_sum);
	
		fit = adjust(V, V.n_elem); // 这里
	
	//得到fit最大值,并返回,sol为最大值,indb为其对应的行数
	sol = fit.max(); //最大值
	indb = fit.index_max();//对应的行数

	Mat<double> media = mean(fit);//得到fit矩阵的平均值

	Mat<double> item1 = { sol };
	vx = join_horiz(vx, item1);
	//平均适应度矩阵
	Mat<double> item2 = { media };
	vmfit = join_horiz(vmfit, item2);
	it = it + 1;
}
//构造返回值

if (V.n_elem > 0)
{
	Mat<long> matCode = V[indb[0]];
	for (int i = 0; i < matCode.n_rows; i++)
	{
		for (int j = 0; j < matCode.n_cols; j++)
			codes.push_back(matCode.at(i, j));      //获取值
	}
}
double ends = omp_get_wtime();
ofstream outfile("out.txt", ios::trunc);
outfile << ends - st << endl;
outfile.close();

}

NavigationTest.cpp

// NavigationTest.cpp : 定义应用程序的类行为。
//

#include “stdafx.h”
#include “NavigationTest.h”
#include “NavigationTestDlg.h”

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

// CNavigationTestApp

BEGIN_MESSAGE_MAP(CNavigationTestApp, CWinApp)
ON_COMMAND(ID_HELP, &CWinApp::OnHelp)
END_MESSAGE_MAP()

// CNavigationTestApp 构造

CNavigationTestApp::CNavigationTestApp()
{
// TODO: 在此处添加构造代码,
// 将所有重要的初始化放置在 InitInstance 中
}

// 唯一的一个 CNavigationTestApp 对象

CNavigationTestApp theApp;

// CNavigationTestApp 初始化

BOOL CNavigationTestApp::InitInstance()
{
CWinApp::InitInstance();

// 创建 shell 管理器,以防对话框包含
// 任何 shell 树视图控件或 shell 列表视图控件。
CShellManager *pShellManager = new CShellManager;

// 激活“Windows Native”视觉管理器,以便在 MFC 控件中启用主题
CMFCVisualManager::SetDefaultManager(RUNTIME_CLASS(CMFCVisualManagerWindows));

// 标准初始化
// 如果未使用这些功能并希望减小
// 最终可执行文件的大小,则应移除下列
// 不需要的特定初始化例程
// 更改用于存储设置的注册表项
// TODO: 应适当修改该字符串,
// 例如修改为公司或组织名
SetRegistryKey(_T("应用程序向导生成的本地应用程序"));

CNavigationTestDlg dlg;
m_pMainWnd = &dlg;
INT_PTR nResponse = dlg.DoModal();
if (nResponse == IDOK)
{
	// TODO: 在此放置处理何时用
	//  “确定”来关闭对话框的代码
}
else if (nResponse == IDCANCEL)
{
	// TODO: 在此放置处理何时用
	//  “取消”来关闭对话框的代码
}
else if (nResponse == -1)
{
	TRACE(traceAppMsg, 0, "警告: 对话框创建失败,应用程序将意外终止。\n");
	TRACE(traceAppMsg, 0, "警告: 如果您在对话框上使用 MFC 控件,则无法 #define _AFX_NO_MFC_CONTROLS_IN_DIALOGS。\n");
}

// 删除上面创建的 shell 管理器。
if (pShellManager != NULL)
{
	delete pShellManager;
}

#ifndef _AFXDLL
ControlBarCleanUp();
#endif

// 由于对话框已关闭,所以将返回 FALSE 以便退出应用程序,
//  而不是启动应用程序的消息泵。
return FALSE;

}

NavigationTestDlg.cpp

// NavigationTestDlg.cpp : 实现文件
//

#include “stdafx.h”
#include “NavigationTest.h”
#include “NavigationTestDlg.h”
#include “afxdialogex.h”
#include
#include
#include
#include “…/NavigationAlgorithms/pathnessAPIs.h”

#pragma comment(lib,"…/build/NavigationAlgorithms.lib")

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

using namespace std;
// CNavigationTestDlg 对话框

CNavigationTestDlg::CNavigationTestDlg(CWnd* pParent / =NULL/)
CDialogEx(IDD_NAVIGATIONTEST_DIALOG, pParent)
{
m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CNavigationTestDlg::DoDataExchange(CDataExchange* pDX)
{
CDialogEx::DoDataExchange(pDX);
DDX_Text(pDX, IDC_EDIT1, m_startCode);
DDX_Text(pDX, IDC_EDIT2, m_endCode);
DDX_Text(pDX, IDC_EDIT3, m_demFile);
DDX_Text(pDX, IDC_EDIT4, m_planResult);
}

BEGIN_MESSAGE_MAP(CNavigationTestDlg, CDialogEx)
ON_WM_PAINT()
ON_WM_QUERYDRAGICON()
ON_BN_CLICKED(IDOK, &CNavigationTestDlg::OnBnClickedOk)
ON_BN_CLICKED(IDCANCEL, &CNavigationTestDlg::OnBnClickedCancel)
ON_BN_CLICKED(ID_PLANPATH_TEST, &CNavigationTestDlg::OnBnClickedPlanpathTest)
END_MESSAGE_MAP()

// CNavigationTestDlg 消息处理程序

BOOL CNavigationTestDlg::OnInitDialog()
{
CDialogEx::OnInitDialog();

// 设置此对话框的图标。  当应用程序主窗口不是对话框时,框架将自动
//  执行此操作
SetIcon(m_hIcon, TRUE);			// 设置大图标
SetIcon(m_hIcon, FALSE);		// 设置小图标

// TODO: 在此添加额外的初始化代码
m_startCode = "46";
m_endCode = "91";
char szdir[MAX_PATH] = { 0 };
::GetCurrentDirectory(MAX_PATH, szdir);
m_demFile = szdir;
m_demFile = m_demFile.Mid(0, m_demFile.ReverseFind('\\')+1);
m_demFile.Append("build");
m_demFile.Append("\\DEM.csv");
m_planResult = "";
UpdateData(FALSE);

return TRUE;  // 除非将焦点设置到控件,否则返回 TRUE

}

// 如果向对话框添加最小化按钮,则需要下面的代码
// 来绘制该图标。 对于使用文档/视图模型的 MFC 应用程序,
// 这将由框架自动完成。

void CNavigationTestDlg::OnPaint()
{
if (IsIconic())
{
CPaintDC dc(this); // 用于绘制的设备上下文

	SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);

	// 使图标在工作区矩形中居中
	int cxIcon = GetSystemMetrics(SM_CXICON);
	int cyIcon = GetSystemMetrics(SM_CYICON);
	CRect rect;
	GetClientRect(&rect);
	int x = (rect.Width() - cxIcon + 1) / 2;
	int y = (rect.Height() - cyIcon + 1) / 2;

	// 绘制图标
	dc.DrawIcon(x, y, m_hIcon);
}
else
{
	CDialogEx::OnPaint();
}

}

//当用户拖动最小化窗口时系统调用此函数取得光标
//显示。
HCURSOR CNavigationTestDlg::OnQueryDragIcon()
{
return static_cast(m_hIcon);
}

void CNavigationTestDlg::OnBnClickedOk()
{
// TODO: 在此添加控件通知处理程序代码
CDialogEx::OnOK();
}

void CNavigationTestDlg::OnBnClickedCancel()
{
// TODO: 在此添加控件通知处理程序代码
CDialogEx::OnCancel();
}

void CNavigationTestDlg::OnBnClickedPlanpathTest()
{
UpdateData(TRUE);
//
std::vector path;
std::stringstream strfmt;
long startCode = atoi(m_startCode.GetBuffer());
long endCode = atoi(m_endCode.GetBuffer());
char* demFile = m_demFile.GetBuffer();
//
Navigation(demFile, startCode, endCode,path);
if (path.size() > 0)
{
for (int i = 0; i < path.size(); i++)
strfmt << path[i] << “;”;
std::string txt;
strfmt >> txt;
m_planResult = “”;
m_planResult.Append(txt.c_str());
}
UpdateData(FALSE);
}
dllmain.cpp

// dllmain.cpp : 定义 DLL 应用程序的入口点。
#include “framework.h”

BOOL APIENTRY DllMain( HMODULE hModule,
DWORD ul_reason_for_call,
LPVOID lpReserved
)
{
switch (ul_reason_for_call)
{
case DLL_PROCESS_ATTACH:
case DLL_THREAD_ATTACH:
case DLL_THREAD_DETACH:
case DLL_PROCESS_DETACH:
break;
}
return TRUE;
}

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
A*算法路径平滑处理是为了使生成的路径更加平滑和自然,减少路径中的拐点数。在传统A*算法中,生成的路径是由一系列节点组成的,这些节点连接起来形成路径。然而,这些节点之间可能存在较大的角度变化,导致路径不够平滑。 为了解决这个问题,可以采用不同的方法对A*算法生成的路径进行平滑处理。例如,可以引入方向变化惩罚和局部区域复杂度惩罚来优化A*算法,使得路径更加平滑。此外,还可以采用改进的Floyd算法路径进行双向平滑,通过适当调整路径中的节点位置,使得路径更加平滑和自然。 通过使用这些路径平滑处理方法,可以在保证路径安全可靠性的同时,减少路径中的拐点数,提高算法的效率和路径的质量。例如,在越野场景路径规划问题中,采用栅格法建立障碍物、威胁物和越野道路模型,引入方向变化惩罚和局部区域复杂度惩罚来优化A*算法,通过改进的Floyd算法路径进行双向平滑,可以规划出更平滑、高效和安全的路径。 总之,路径平滑处理是为了使A*算法生成的路径更加平滑、自然和高效,减少拐点数并提高路径质量。通过引入惩罚项和采用改进的平滑算法,可以实现路径的平滑处理。<span class="em">1</span><span class="em">2</span><span class="em">3</span> #### 引用[.reference_title] - *1* *2* [改进的 A*算法路径规划(路径规划+代码+毕业设计)](https://blog.csdn.net/ALiLiLiYa/article/details/129232865)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v92^chatsearchT0_1"}}] [.reference_item style="max-width: 50%"] - *3* [A*寻路之路径平滑(二)](https://blog.csdn.net/u014028063/article/details/80922658)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v92^chatsearchT0_1"}}] [.reference_item style="max-width: 50%"] [ .reference_list ]

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值