pathdoc.h

本程序主要是设计了一个人工势能场机器人运动规划算法。并提出了一个新的斥力场函数。

// PathDoc.h : interface of the CPathDoc class
//
/////////////////////////////////////////////////////////////////////////////

#if !defined(AFX_PATHDOC_H__9DA68E54_E0D6_48F4_A807_61689EFDFA35__INCLUDED_)
#define AFX_PATHDOC_H__9DA68E54_E0D6_48F4_A807_61689EFDFA35__INCLUDED_

#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000

//***** 界面设计 *****
#define  MAP_GRIDSIZE  8
#define  MAP_BOARDX    320
#define  MAP_BOARDY    240

//***** APF *****
#define  K_ATTRACT		10	//引力计算系数
#define	 K_REPULSION	120	//斥力计算系数
#define	 K_DISTANCE		25	//斥力作用距离
#define	 K_MOVE			4	//移动步长

//***** 方向变量 *****
enum Direction{
LEFT,UP,RIGHT,DOWN
};

#include "Vector.h"

class CPathDoc : public CDocument
{
protected: // create from serialization only
	CPathDoc();
	DECLARE_DYNCREATE(CPathDoc)

// Attributes
public:
	CArray<CPoint,CPoint>  m_cObsArray;//障碍物点
	CArray<CPoint,CPoint>  m_cPathArray;//路径点
	CArray<CPoint,CPoint>  m_cEndArray;//中间目标点
// Operations
public:
	char		*GetBoard() { return &(m_cBoard[0][0]); }//得到界面方格的设置
	void		GetStartEnd(CPoint &x, CPoint &y) { x = m_Start, y = m_End; }//获取首末节点
	void		SetStartEnd(CPoint x, CPoint y) { m_Start = x, m_End = y; }//设置首末节点
	void		MyGetStartEnd(CPoint &x, CPoint &y) { x = m_cStart, y = m_cEnd; }//获取首末节点
	void		GetNowPoint(CPoint &x) { x = m_cNow; }//获取当前点的坐标
	void		SetNewEndPoint(CPoint x) { m_cEnd = x; }//设置中间目标节点时用到
	bool		DrawRoute() { return m_bDrawRoute; }//画路径
	void		SetDrawRoute() { m_bDrawRoute=true;}
	//****** APF 函数 ******************
	bool		GetMapInfo();
	float		ComDistance(CPoint pt1,CPoint pt2);//计算点距，多为机器人和障碍间的距离
	CVector		ComAngle(CPoint now,CPoint end);//计算相对角度
	CVector		ComAttract(CPoint robot,CPoint goal,CVector vSave);//计算引力大小
	CVector		ComRepulsion(CPoint robot,CPoint obs,CPoint goal,CVector vobs,CVector vgoal);//计算斥力大小
	void		MoveToNext(CVector v);//移动到下一点
	bool		IsArrivalGoal();//到达目标
	//*************************
	//****** 局部稳定点解决方案函数 *****
	//(1) 因为网格分解了整个地图，参照迷宫算法实现走出局部稳定点问题
	int			m_iAllow[4];//为 1 ，说明该方向不可行
	Direction	m_dDirection;
	
	void		ResetArray();//第一次执行时清空，
	void		CheckCurrentPoint(CPoint now);//检测当前点，设置相应标志位
	bool		KillFlag;
	void		SetFlag(CPoint now);//检测当前点周围是否都没有障碍
	CPoint		GetNextPoint(CPoint now);//根据标志位，得到另一节点
	//*************************

	//(2) 设置中间目标点，参照直线方程
	float		k1,b1,k2,b2,k3,b3;
	CPoint		m_cMiddle,m_cNewEnd;
	char		m_cBoard[MAP_BOARDX][MAP_BOARDY];//记录地图信息，界面应用
	void		CalculateLine1(CPoint now,CPoint end);//计算出当前点指向目标点的直线方程系数
	void		CalculateLine2(CPoint middle,float k1);//计算出某点处法线的直线方程系数
	void		MyCalculateLine2(CPoint now,float k1);//计算出某点处法线的直线方程系数
	void		CheckAlongLine (float k2,float b2,CPoint now);//判断当前点在的直线上有无障碍物
	void		CheckAlongLine1(float k1,float b1,CPoint now,CPoint end,CPoint &middle);//检测当前点和目标点间直线上的点
	void		CheckAlongLine2(float k2,float b2,CPoint middle,CPoint end,CPoint &newend);//检测直线，获得中间目标点坐标
	//***********************************

	//(3) 问题的最终解决方案--设置中间目标点
	//******* 水平，垂直方向时的处理方法：**************
	bool		flagRight,flagLeft,flagUp,flagDown;//记录相应方向上是否有障碍物阻挡
	void		ResetAllFlag();
	void		CheckUpDown(CPoint now);//判断当前点竖直方向上是否有阻挡
	void		GetUDEndPoint(CPoint now,CPoint end,CPoint &newend);
	void		CheckRightLeft(CPoint now);//判断当前点水平方向上是否有阻挡
	void		GetRLEndPoint(CPoint now,CPoint end,CPoint &newend);
	//******* 倾斜直线的处理方法：**********************
	bool		flagLeftUp,flagRightDown,flagRightUp,flagLeftDown;
	void		ResetAllFlag2();
	void		CheckLUpRDown(CPoint now);
	void		GetLUpRDownPoint(CPoint now,CPoint end,CPoint &newend);
	void		CheckRUpLDown(CPoint now);
	void		GetRUpLDownPoint(CPoint now,CPoint end,CPoint &newend);
	//**************************************************

	//**********************

	inline void SetBrushType(UINT b) { m_uBrushType = b; }//设置画刷的颜色
	inline void	MessageBox(CString, CString, UINT);
// Overrides
	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CPathDoc)
	public:
	virtual BOOL OnNewDocument();
	virtual void Serialize(CArchive& ar);
	//}}AFX_VIRTUAL

// Implementation
public:
	virtual ~CPathDoc();
#ifdef _DEBUG
	virtual void AssertValid() const;
	virtual void Dump(CDumpContext& dc) const;
#endif

protected:
	//***** 界面需要 *****
	//char		m_cBoard[MAP_BOARDX][MAP_BOARDY];//记录地图信息，界面应用
	CPoint		m_Start,m_End;//开始，目标点
	//**********
	bool		m_bDrawRoute;//画路径
	CPoint      m_cStart,m_cEnd;//起始,目标点坐标
	CPoint      m_cNow;//当前坐标
	CPoint		m_cNext;
	//CPoint		m_cMiddle;//临时目标点坐标

	UINT		m_uBrushType;//画刷颜色

// Generated message map functions
protected:
	//{{AFX_MSG(CPathDoc)
		// NOTE - the ClassWizard will add and remove member functions here.
		//    DO NOT EDIT what you see in these blocks of generated code !
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
};

/////////////////////////////////////////////////////////////////////////////

//{{AFX_INSERT_LOCATION}}
// Microsoft Visual C++ will insert additional declarations immediately before the previous line.

#endif // !defined(AFX_PATHDOC_H__9DA68E54_E0D6_48F4_A807_61689EFDFA35__INCLUDED_)