📄 pathplan.cpp
字号:
// Pathplan.cpp: implementation of the Pathplan class.
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "RobotView.h"
#include "Pathplan.h"
#include "RobotViewView.h"
#include "MainFrm.h"
#include "Route.h"
#include<iostream>
#include<stdio.h>
using namespace std;
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
Pathplan::Pathplan()
{
}
Pathplan::~Pathplan()
{
}
///////////////////////////////////////////////////////////////
path Pathplan:: GetPath()
{
return m_resultpath;
}
void Pathplan::GetVmap(int map[MH][NW])
{
for(int i=0;i<MH;i++)
for(int j=0;j<NW;j++)
map[i][j]=m_VMap[i][j];
}
void Pathplan::removeOfItem(int i)//m_removeItem[]中存放要从8路径表S中删除的路径的索引
{
int j,flag=0;
for(j=0;j<m_removeNum;j++){
if(m_removeItem[j]==i){
flag=1;
break;
}
}
if(flag==0){
m_removeItem[m_removeNum]=i;
m_removeNum++;
}
}
int Pathplan::pathEqual(path p,path q)// 判断两条路径是否相同，如果p与q相同，则返回1，否则，返回0
{
int i;
if(p.pathlength!=q.pathlength)
return 0;
else{
for(i=0;i<p.pathlength;i++){
if(!(p.pos[i].x==q.pos[i].x&&p.pos[i].y==q.pos[i].y))
return 0;
}
return 1;
}
}
//利用从图库传来的指针，把地图库中的相关信息传给m_VMap，m_VMap中就得到了初始的地图。
void Pathplan::plan(int map[MH][NW])
{
int i,j;
for(i=0;i<MH;i++)
{
for(j=0;j<NW;j++){
m_VMap[i][j]=map[i][j];//pinitmap->getEachRouteMap(i,j);
}
}
}
//设置初始地图中所有障碍物的位置，放入一数组中，并把障碍物的个数放入m_onum
void Pathplan::setOposition()
{
int i,j;
m_onum=0;
for(i=0;i<MH;i++)
{
for(j=0;j<NW;j++)
{
if(m_VMap[i][j]==1)
{
m_Oposition[m_onum].x=i;
m_Oposition[m_onum].y=j;
m_Oposition[m_onum].height=1;
m_onum++;
}
}
}
}
//////////////////////////////////////////////////////////////////////////////
//把初始地图转化为具有虚拟高度的虚拟地图
void Pathplan::setVmap(float Kg, float r, int onum, position Oposition[NW*MH])
{
int Kobs=20;
int i,j,k;
int Mid[MAX],temp;
for(i=0;i<MH;i++)
{
for(j=0;j<NW;j++)
{
Mid[0]=Kg*(sqrt((m_aimpos.x-i)*(m_aimpos.x-i))+sqrt((m_aimpos.y-j)*(m_aimpos.y-j)));//目标点对栅格的影响
for(k=1;k<=onum;k++)//各个障碍物对栅格的影响
{
Mid[k]=r*(80-(sqrt((m_aimpos.x-Oposition[k-1].x)*(m_aimpos.x-Oposition[k-1].x))+sqrt((m_aimpos.y-Oposition[k-1].y)*(m_aimpos.y-Oposition[k-1].y))))/(sqrt((Oposition[k-1].x-i)*(Oposition[k-1].x-i))+sqrt((Oposition[k-1].y-j)*(Oposition[k-1].y-j)));
}
if(m_onum==0)
temp=0;
else
temp=Mid[1];
for(int l=1;l<=onum;l++){
if(temp<Mid[l])
temp=Mid[l];
}
m_VMap[i][j]=temp+Mid[0];//取最大值与目的点对栅格的影响为栅格的虚拟高度
}
}
for(i=0;i<onum;i++){
Oposition[i].height=80;
m_VMap[Oposition[i].x][Oposition[i].y]=80;
}
}
///////////////////////////////////////////////////////////////////////////
//从虚拟地图中得到危险临界值
int Pathplan::getKVH(position Oposition[MH*NW])
{
int i;
int temp= Oposition[0].height;
for(i=0;i<m_onum;i++)
{
if(Oposition[i].height<temp)//有障碍物的栅格中最小的虚拟高度
{
temp=Oposition[i].height;
}
}
m_Kvh=temp;
return m_Kvh ;
}
//设置机器人目前的位置，根据定位模块得到的接口
void Pathplan::setRobotPos(int x,int y)
{
m_robotpos.x=x;
m_robotpos.y=y;
m_robotpos.height=m_VMap[x][y];
}
//把一条路径放入路径表中
void Pathplan::pathIntoPlist(path S[MAX], path p,int &num)
{
S[num]=p;
num++;
}
//得到一个栅格周围的栅格及个数
void Pathplan::getAround(position u)
{
if(u.x==0&&u.y==0)
{
m_pos[0].x=1;
m_pos[0].y=0;
m_pos[0].height=m_VMap[1][0];
m_pos[1].x=1;
m_pos[1].y=1;
m_pos[1].height=m_VMap[1][1];
m_pos[2].x=0;
m_pos[2].y=1;
m_pos[2].height=m_VMap[0][1];
m_posnum=3; //u为左下角栅格时
}
else if(u.x==0&&u.y==NW-1)
{
m_pos[0].x=0;
m_pos[0].y=NW-2;
m_pos[0].height=m_VMap[0][NW-2];
m_pos[1].x=1;
m_pos[1].y=NW-2;
m_pos[1].height=m_VMap[1][NW-2];
m_pos[2].x=1;
m_pos[2].y=NW-1;
m_pos[2].height=m_VMap[1][NW-1];
m_posnum=3; //u为右下角栅格时
}else if(u.x==MH-1&&u.y==NW-1)
{
m_pos[0].x=MH-1;
m_pos[0].y=NW-2;
m_pos[0].height=m_VMap[MH-1][NW-2];
m_pos[1].x=MH-2;
m_pos[1].y=NW-2;
m_pos[1].height=m_VMap[MH-2][NW-2];
m_pos[2].x=MH-2;
m_pos[2].y=NW-1;
m_pos[2].height=m_VMap[MH-2][NW-1];
m_posnum=3; //u为右上角栅格时
}else if(u.x==MH-1&&u.y==0)
{
m_pos[0].x=MH-1;
m_pos[0].y=1;
m_pos[0].height=m_VMap[MH-1][1];
m_pos[1].x=MH-2;
m_pos[1].y=1;
m_pos[1].height=m_VMap[MH-2][1];
m_pos[2].x=MH-2;
m_pos[2].y=0;
m_pos[2].height=m_VMap[MH-2][0];
m_posnum=3; //u为左上角栅格时
}
else if(u.x==0)
{
m_pos[0].x=0;
m_pos[0].y=u.y-1;
m_pos[0].height=m_VMap[0][u.y-1];
m_pos[1].x=1;
m_pos[1].y=u.y-1;
m_pos[1].height=m_VMap[1][u.y-1];
m_pos[2].x=1;
m_pos[2].y=u.y;
m_pos[2].height=m_VMap[1][u.y];
m_pos[3].x=1;
m_pos[3].y=u.y+1;
m_pos[3].height=m_VMap[1][u.y+1];
m_pos[4].x=0;
m_pos[4].y=u.y+1;
m_pos[4].height=m_VMap[0][u.y+1];
m_posnum=5; //u为最下行栅格时
}else if(u.y==0)
{
m_pos[0].x=u.x-1;
m_pos[0].y=0;
m_pos[0].height=m_VMap[u.x-1][0];
m_pos[1].x=u.x-1;
m_pos[1].y=1;
m_pos[1].height=m_VMap[u.x-1][1];
m_pos[2].x=u.x;
m_pos[2].y=1;
m_pos[2].height=m_VMap[u.x][1];
m_pos[3].x=u.x+1;
m_pos[3].y=1;
m_pos[3].height=m_VMap[u.x+1][1];
m_pos[4].x=u.x+1;
m_pos[4].y=0;
m_pos[4].height=m_VMap[u.x+1][0];
m_posnum=5; //u为最左行栅格时
}else if(u.x==MH-1)
{
m_pos[0].x=MH-1;
m_pos[0].y=u.y-1;
m_pos[0].height=m_VMap[MH-1][u.y-1];
m_pos[1].x=MH-2;
m_pos[1].y=u.y-1;
m_pos[1].height=m_VMap[MH-2][u.y-1];
m_pos[2].x=MH-2;
m_pos[2].y=u.y;
m_pos[2].height=m_VMap[MH-2][u.y];
m_pos[3].x=MH-2;
m_pos[3].y=u.y+1;
m_pos[3].height=m_VMap[MH-2][u.y+1];
m_pos[4].x=MH-1;
m_pos[4].y=u.y+1;
m_pos[4].height=m_VMap[MH-1][u.y+1];
m_posnum=5; //u为最上行栅格时
}else if(u.y==NW-1)
{
m_pos[0].x=u.x-1;
m_pos[0].y=NW-1;
m_pos[0].height=m_VMap[u.x-1][NW-1];
m_pos[1].x=u.x-1;
m_pos[1].y=NW-2;
m_pos[1].height=m_VMap[u.x-1][NW-2];
m_pos[2].x=u.x;
m_pos[2].y=NW-2;
m_pos[2].height=m_VMap[u.x][NW-2];
m_pos[3].x=u.x+1;
m_pos[3].y=NW-2;
m_pos[3].height=m_VMap[u.x+1][NW-2];
m_pos[4].x=u.x+1;
m_pos[4].y=NW-1;
m_pos[4].height=m_VMap[u.x+1][NW-1];
m_posnum=5; //u为最右行栅格时
}
else
{
m_pos[0].x=u.x;
m_pos[0].y=u.y-1;
m_pos[0].height=m_VMap[u.x][u.y-1];
m_pos[1].x=u.x+1;
m_pos[1].y=u.y-1;
m_pos[1].height=m_VMap[u.x+1][u.y-1];
m_pos[2].x=u.x+1;
m_pos[2].y=u.y;
m_pos[2].height=m_VMap[u.x+1][u.y];
m_pos[3].x=u.x+1;
m_pos[3].y=u.y+1;
m_pos[3].height=m_VMap[u.x+1][u.y+1];
m_pos[4].x=u.x;
m_pos[4].y=u.y+1;
m_pos[4].height=m_VMap[u.x][u.y+1];
m_pos[5].x=u.x-1;
m_pos[5].y=u.y+1;
m_pos[5].height=m_VMap[u.x-1][u.y+1];
m_pos[6].x=u.x-1;
m_pos[6].y=u.y;
m_pos[6].height=m_VMap[u.x-1][u.y];
m_pos[7].x=u.x-1;
m_pos[7].y=u.y-1;
m_pos[7].height=m_VMap[u.x-1][u.y-1];
m_posnum=8; //对中间的栅格做的操作
}
}
//得到一个栅格周围栅格的最小虚拟高度
int Pathplan::getMinHeight(position u)
{
getAround(u);
int i=0;
int heig;
heig=m_pos[0].height;
for(i=0;i<m_posnum;i++)
{
if (m_pos[i].height<heig)
{
heig=m_pos[i].height;
}
}
return heig;
}
//对一栅格进行储水操作
void Pathplan::reserveWater(position &u, int min)
{
while(u.height<= min)
{
u.height=u.height+1;//幅度为1
}
m_VMap[u.x][u.y]=u.height;
}
//得到一栅格u周围的某些栅格，这些栅格具有以下性质：虚拟高度小于m_Kvh，也小于u的虚拟高度，保存在midpos[]中，个数保存在midnum中
void Pathplan::setMidPos(position u)
{
int i;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -