📄 pathdoc.cpp
字号:
// PathDoc.cpp : implementation of the CPathDoc class
//
#include "stdafx.h"
#include "Path.h"
#include "PathDoc.h"
#include "math.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
/////////////////////////////////////////////////////////////////////////////
// CPathDoc
IMPLEMENT_DYNCREATE(CPathDoc, CDocument)
BEGIN_MESSAGE_MAP(CPathDoc, CDocument)
//{{AFX_MSG_MAP(CPathDoc)
// NOTE - the ClassWizard will add and remove mapping macros here.
// DO NOT EDIT what you see in these blocks of generated code!
//}}AFX_MSG_MAP
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CPathDoc construction/destruction
CPathDoc::CPathDoc()
{
// TODO: add one-time construction code here
memset(m_cBoard, 0, sizeof(m_cBoard));
m_uBrushType = 0;
m_Start.x = m_Start.y = -1;
m_End.x = m_End.y = -1;
m_cStart.x = m_cStart.y = -1;
m_cNow = m_cStart;
m_cEnd.x = m_cEnd.y = -1;
m_bDrawRoute = false;
m_cObsArray.RemoveAll();
m_cPathArray.RemoveAll();
m_cEndArray.RemoveAll();
flagUp=flagDown=flagLeft=flagRight=true;
flagLeftUp=flagRightDown=flagRightUp=flagLeftDown=true;
//KillFlag = false;
}
CPathDoc::~CPathDoc()
{
}
BOOL CPathDoc::OnNewDocument()
{
if (!CDocument::OnNewDocument())
return FALSE;
// TODO: add reinitialization code here
// (SDI documents will reuse this document)
memset(m_cBoard, 0, sizeof(m_cBoard));
m_uBrushType = 0;
m_cStart.x = m_cStart.y = -1;
m_cNow = m_cStart;
m_cEnd.x = m_cEnd.y = -1;
m_bDrawRoute = false;
m_cObsArray.RemoveAll();
m_cPathArray.RemoveAll();
m_cEndArray.RemoveAll();
return TRUE;
}
/////////////////////////////////////////////////////////////////////////////
// CPathDoc serialization
void CPathDoc::Serialize(CArchive& ar)
{
int x = MAP_BOARDX, y = MAP_BOARDY;
if (ar.IsStoring())
{
ar << x;
ar << y;
ar << m_Start;
ar << m_End;
ar << m_cStart;
ar << m_cEnd;
for (int i=0; i<x; i++) {
for (int j=0; j<y; j++) {
ar << m_cBoard[i][j];
}
}
}
else
{
ar >> x;
ar >> y;
ar >> m_Start;
ar >> m_End;
ar >> m_cStart;
ar >> m_cEnd;
for (int i=0; i<x; i++) {
for (int j=0; j<y; j++) {
ar >> m_cBoard[i][j];
}
}
}
}
/////////////////////////////////////////////////////////////////////////////
// CPathDoc diagnostics
#ifdef _DEBUG
void CPathDoc::AssertValid() const
{
CDocument::AssertValid();
}
void CPathDoc::Dump(CDumpContext& dc) const
{
CDocument::Dump(dc);
}
#endif //_DEBUG
/////////////////////////////////////////////////////////////////////////////
// CPathDoc commands
//***** APF *****
bool CPathDoc::GetMapInfo()
//获取地图的相应点信息
{
m_cObsArray.RemoveAll();
m_cPathArray.RemoveAll();
m_cEndArray.RemoveAll();
//获得障碍节点坐标
int i,j;
for(i=0;i<MAP_BOARDX;i++)
for(j=0;j<MAP_BOARDY;j++)
{
if(m_cBoard[i][j]==1)
{
CPoint pt = CPoint(i*MAP_GRIDSIZE+4,j*MAP_GRIDSIZE+4);
m_cObsArray.Add(pt);
}
}
//判断起始，终止节点
if (m_Start.x == -1 || m_End.x == -1) {
MessageBox("请确定已经设置了起始、终止节点!\n红色表示起点，蓝色表示终点.", "APF", MB_ICONERROR);
return false;
}
//存储开始，目标点
m_cStart.x=m_Start.x*MAP_GRIDSIZE+4;
m_cStart.y=m_Start.y*MAP_GRIDSIZE+4;
m_cNow=m_cStart;
m_cEnd.x=m_End.x*MAP_GRIDSIZE+4;
m_cEnd.y=m_End.y*MAP_GRIDSIZE+4;
//记入相应点数组
m_cPathArray.Add(m_cNow);
m_cEndArray.Add(m_cEnd);
return true;
}
float CPathDoc::ComDistance(CPoint x,CPoint y)
//计算距离
{
float dx=(float)(y.x - x.x);
float dy=(float)(y.y - x.y);
float dis=(float)sqrt(dx*dx+dy*dy);
return dis;
}
CVector CPathDoc::ComAngle(CPoint now,CPoint end)
//角度方向 now-->end
{
CVector t;
if(now.y==end.y && now.x<end.x)
{
t.SetX(1);//cos
t.SetY(0);//sin
return t;
}
if(now.y==end.y && now.x>end.x)
{
t.SetX(-1);//
t.SetY(0);
return t;
}
if(now.x==end.x && now.y>end.y)
{
t.SetX(0);//
t.SetY(-1);
return t;
}
if(now.x==end.x && now.y<end.y)
{
t.SetX(0);//
t.SetY(1);
return t;
}
//第四象限
if(now.x<end.x && now.y>end.y)
{
float dis=ComDistance(now,end);
float cos=abs(end.x-now.x)/dis;
float sin=abs(end.y-now.y)/dis;
t.SetX(cos);//
t.SetY(-sin);
return t;
}
//第三象限
if(now.x>end.x && now.y>end.y)
{
float dis=ComDistance(now,end);
float cos=abs(end.x-now.x)/dis;
float sin=abs(end.y-now.y)/dis;
t.SetX(-cos);//
t.SetY(-sin);
return t;
}
//第二象限
if(now.x>end.x && now.y<end.y)
{
float dis=ComDistance(now,end);
float cos=abs(end.x-now.x)/dis;
float sin=abs(end.y-now.y)/dis;
t.SetX(-cos);//
t.SetY(sin);
return t;
}
//第一象限
if(now.x<end.x && now.y<end.y)
{
float dis=ComDistance(now,end);
float cos=abs(end.x-now.x)/dis;
float sin=abs(end.y-now.y)/dis;
t.SetX(cos);
t.SetY(sin);
return t;
}
return CVector(0,0);
}
CVector CPathDoc::ComAttract(CPoint robot,CPoint goal, CVector v)
//goal是相对目标节点，robot是当前节点。向量方向 robot->goal
//v向量是力的方向，用来计算返回的向量 t 存储的X，Y轴的相应分力
{
float dis=ComDistance(robot,goal);
CVector t;
t.SetX(K_ATTRACT*dis*v.GetX());//（系数*距离）*cos（X轴分量）
t.SetY(K_ATTRACT*dis*v.GetY());//（系数*距离）*cos（Y轴分量）
return t;
}
CVector CPathDoc::ComRepulsion(CPoint now,CPoint obs,CPoint goal,CVector vobs,CVector vgoal)
//now是当前节点，obs 是障碍节点，方向 obs->now
//vobs,vgoal 存储的是 cos,sin 的值,作为方向正余玄用
{
CVector t;
float Rat,rat,Rre,rre;
Rat=(goal.x-now.x)*(goal.x-now.x)+(goal.y-now.y)*(goal.y-now.y);
rat=(float)sqrt(Rat);//与目标点距离
Rre=(obs.x-now.x)*(obs.x-now.x)+(obs.y-now.y)*(obs.y-now.y);
rre=(float)sqrt(Rre);//与障碍点距离
CVector _obs,_goal;
_obs.SetX(K_REPULSION*(1/rre-1/K_DISTANCE)*1/Rre*Rat*vobs.GetX());
_obs.SetY(K_REPULSION*(1/rre-1/K_DISTANCE)*1/Rre*Rat*vobs.GetY());
_goal.SetX(K_REPULSION*(1/rre-1/K_DISTANCE)*(1/rre-1/K_DISTANCE)*rat*vgoal.GetX());
_goal.SetY(K_REPULSION*(1/rre-1/K_DISTANCE)*(1/rre-1/K_DISTANCE)*rat*vgoal.GetY());
t=_obs+_goal;
return t;
}
void CPathDoc::MoveToNext(CVector v)
//v 里储存当前节点的合力,可以计算出 cos ,sin
{
CPoint pt=m_cEnd;
//pt.x=pt.x*MAP_GRIDSIZE+4;
//pt.y=pt.y*MAP_GRIDSIZE+4;
if(ComDistance(m_cNow,pt)<=6)
{
m_cNow=pt;
return;
}
float x=v.GetX();
float y=v.GetY();
float d=(float)sqrt(x*x+y*y);
m_cNow.x=m_cNow.x+K_MOVE*x/d;//K_MOVE 移动步长
m_cNow.y=m_cNow.y+K_MOVE*y/d;
}
bool CPathDoc::IsArrivalGoal()
{
if(abs(m_cNow.x-m_cEnd.x)<3 && abs(m_cNow.y-m_cEnd.y)<3)
return true;
else return false;
}
//**********
void CPathDoc::MessageBox(CString title, CString caption, UINT type)
{
AfxGetMainWnd()->MessageBox(title, caption, type);
}
//***** 设置中间牵引点 ******
void CPathDoc::CalculateLine1(CPoint now,CPoint end)
//水平或垂直的直线不在计算里面
{
k1 = (float)(now.y - end.y)/(now.x - end.x);
b1 = now.y - k1*now.x;
}
void CPathDoc::CheckAlongLine1(float k1,float b1,CPoint now,CPoint end,CPoint &middle)
{
bool flag;
if(end.x > now.x)
flag=true;
else
flag=false;
for(int ii=1;ii<20;ii++)
{
float px;
if(flag)
px = now.x + 6*ii;
else
px = now.x - 6*ii;
float py = k1*px + b1;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
if(m_cBoard[i][j]==1)
{
middle.x = px;
middle.y = k1*middle.x + b1;
return;
}
}
}
void CPathDoc::CalculateLine2(CPoint middle,float k1)
{
k2 = -1/k1;
b2 = middle.y - k2*middle.x;
}
void CPathDoc::MyCalculateLine2(CPoint middle,float k1)
{
k3 = -1/k1;
b3 = middle.y - k3*middle.x;
}
void CPathDoc::CheckAlongLine2(float k2,float b2,CPoint middle,CPoint end,CPoint &newend)
{
CPoint pt1,pt2;
if(k2>0)
//右上，左下
{
if(flagRightUp && !flagLeftDown)
//右上无障碍
{
for(int ii=1;ii<40;ii++)
{
float px = middle.x + 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=0;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
px=px+6;
py=k2*px+b2;
pt1 = CPoint(px,py);
//pt1 = CPoint(px+6,k2*(px+6)+b2);
ii = 40;
}
}
newend = pt1;
return;
}
if(!flagRightUp && flagLeftDown)
//左下无障碍
{
for(int ii=1;ii<40;ii++)
{
float px = middle.x - 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=0;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
px=px+6;
py=k2*px+b2;
newend = CPoint(px,py);
ii = 40;
}
}
return;
}
if(flagRightUp && flagLeftDown)
//都无障碍
{
for(int ii=1;ii<40;ii++)
{
float px = middle.x + 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=0;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
px=px+6;
py=k2*px+b2;
pt1 = CPoint(px,py);
//pt1 = CPoint(px+6,k2*(px+6)+b2);
ii = 40;
}
}
for(ii=1;ii<40;ii++)
{
float px = middle.x - 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=0;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
px=px;
py=k2*px+b2;
pt2 = CPoint(px,py);
//pt2 = CPoint(px-6,k2*(px+6)+b2);
ii = 40;
}
}
//计算两个点与中间点的距离，取小的
float ds1=ComDistance(pt1,middle);
float ds2=ComDistance(pt2,middle);
if(ds1<=ds2)
{
newend=pt1;
return;
}
else
{
newend=pt2;
return;
}
}
if(!flagRightUp && !flagLeftDown)
//都有障碍
{
CPoint pt1,pt2;
for(int ii=0;ii<40;ii++)
{
float px = middle.x + 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
if(m_cBoard[i][j])
{
pt1 = CPoint(px,py);
ii = 40;
}
}
///
for(ii=1;ii<40;ii++)
{
float px = middle.x + 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=1;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
pt1 = CPoint(px,py);
ii = 40;
}
}
////////////////////
for(ii=0;ii<40;ii++)
{
float px = middle.x - 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
if(m_cBoard[i][j])
{
pt2 = CPoint(px,py);
ii = 40;
}
}
//
for(ii=1;ii<40;ii++)
{
float px = middle.x - 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=1;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
pt2 = CPoint(px,py);
ii = 40;
}
}
///////////////////
//计算两个点与中间点的距离，取小的
float ds1,ds2;
ds1=ComDistance(pt1,middle);
ds2=ComDistance(pt2,middle);
if(ds1<=ds2)
{
newend=pt1;
}
else
{
newend=pt2;
}
return;
}
}
if(k2<0)
//左上，右下
{
if(!flagLeftUp && flagRightDown)
//右下无障碍
{
for(int ii=1;ii<40;ii++)
{
float px = middle.x + 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=0;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
px=px+6;
py=k2*px+b2;
newend = CPoint(px,py);
ii = 40;
}
}
return;
}
if(flagLeftUp && !flagRightDown)
//左上无障碍
{
for(int ii=1;ii<40;ii++)
{
float px = middle.x - 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=0;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
px=px+6;
py=k2*px+b2;
newend = CPoint(px,py);
ii = 40;
}
}
return;
}
if(flagLeftUp && flagRightDown)
//都无障碍
{
for(int ii=1;ii<40;ii++)
{
float px = middle.x + 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=0;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
px=px+6;
py=k2*px+b2;
pt1 = CPoint(px,py);
//pt1 = CPoint(px+6,k2*(px+6)+b2);
ii = 40;
}
}
for(ii=1;ii<40;ii++)
{
float px = middle.x - 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=0;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
px=px;
py=k2*px+b2;
pt2 = CPoint(px,py);
//pt2 = CPoint(px-6,k2*(px+6)+b2);
ii = 40;
}
}
//计算两个点与中间点的距离，取小的
float ds1=ComDistance(pt1,middle);
float ds2=ComDistance(pt2,middle);
if(ds1<=ds2)
{
newend=pt1;
return;
}
else
{
newend=pt2;
return;
}
}
if(!flagLeftUp && !flagRightDown)
//都有障碍
{
CPoint pt1,pt2;
for(int ii=0;ii<40;ii++)
{
float px = middle.x + 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
if(m_cBoard[i][j])
{
pt1 = CPoint(px,py);
ii = 40;
}
}
///
for(ii=1;ii<40;ii++)
{
float px = middle.x + 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=1;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
jj=5;
if(m_cBoard[i-jj][j])
jj=5;
}
if(jj==4)
{
pt1 = CPoint(px,py);
ii = 40;
}
}
////////////////////
for(ii=0;ii<40;ii++)
{
float px = middle.x - 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
if(m_cBoard[i][j])
{
pt2 = CPoint(px,py);
ii = 40;
}
}
//
for(ii=1;ii<40;ii++)
{
float px = middle.x - 6*ii;
float py = k2*px + b2;
int i=px/MAP_GRIDSIZE;
int j=py/MAP_GRIDSIZE;
for(int jj=1;jj<4;jj++)
{
if(m_cBoard[i][j+jj])
jj=5;
if(m_cBoard[i][j-jj])
jj=5;
if(m_cBoard[i+jj][j])
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -