dstarview.cpp

人工智能里的A-star算法,用于机器人的路径规划和寻优.

// DStarView.cpp : implementation of the CDStarView class
//

#include "stdafx.h"
#include "DStar.h"
#include <afxtempl.h>

#include "DStarDoc.h"
#include "DStarView.h"
#include "State.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

#define WM_ENABLEPAUSE WM_USER+1
#define WM_DISABLEPAUSE WM_USER+2
#define WM_DRAWOBST WM_USER+3
/////////////////////////////////////////////////////////////////////////////
// CDStarView
/************************************************************************/
BOOL bEndThread;
int iMode = 0;

int ROWNUMS = 20;
int COLNUMS = 20;
int NextPt[8][2] = {	{-1,-1},	{-1,0},		{-1,1},
					{0,-1},				    {0, 1},
					{1,-1},		{1,0},		{1,1}   };
CPoint GoalPt;
CPoint StartPt;

CBrush ObsBrush( RGB(0, 0, 255) );    //障碍物画刷
CBrush StateBrush( RGB(255,0,0) );    //画最后的点状态

CArray<CState, CState> OpenList,CloseList;
CArray<CPoint, CPoint> Obst;
CArray<CPoint, CPoint> OutObst;

UINT ThreadSearch( LPVOID param );

BOOL IsInObst( CPoint pt );
BOOL IsInOutObst( CPoint pt );
BOOL IsCorrect( CPoint& pt );
BOOL IsDanger( CState cs );
int  InOpenList( CState& st );
int  InCloseList( CState& st );

void InsertIntoOpen( CState& cs );

/************************************************************************/
/************************************************************************/
IMPLEMENT_DYNCREATE(CDStarView, CFormView)

BEGIN_MESSAGE_MAP(CDStarView, CFormView)
	//{{AFX_MSG_MAP(CDStarView)
	ON_BN_CLICKED(IDC_SET, OnSet)
	ON_BN_CLICKED(IDC_RUN, OnRun)
	ON_BN_CLICKED(IDC_RESET, OnReset)
	ON_BN_CLICKED(IDC_LOAD, OnLoad)
	ON_BN_CLICKED(IDC_PAUSE, OnPause)
	ON_WM_LBUTTONDOWN()
	ON_BN_CLICKED(IDC_SAVE, OnSave)
	ON_BN_CLICKED(IDC_BREAK, OnBreak)
	ON_COMMAND(ID_MODENEW, OnModenew)
	ON_COMMAND(ID_MODEOLD, OnModeold)
	ON_UPDATE_COMMAND_UI(ID_MODENEW, OnUpdateModenew)
	ON_UPDATE_COMMAND_UI(ID_MODEOLD, OnUpdateModeold)
	//}}AFX_MSG_MAP
	ON_MESSAGE( WM_ENABLEPAUSE, EnablePause )
	ON_MESSAGE( WM_DISABLEPAUSE, DisablePause )
	ON_MESSAGE( WM_DRAWOBST, DrawObst )
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CDStarView construction/destruction

CDStarView::CDStarView()
	: CFormView(CDStarView::IDD)
{
	m_strFile = _T("f:\\MyVCProject\\DStar\\obstacle.txt");
	pThread = NULL;
	m_bRun = FALSE;
	GoalPt.x = 19;
	GoalPt.y = 19;
}

CDStarView::~CDStarView()
{
}

void CDStarView::DoDataExchange(CDataExchange* pDX)
{
	CFormView::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CDStarView)
		// NOTE: the ClassWizard will add DDX and DDV calls here
	//}}AFX_DATA_MAP
}

BOOL CDStarView::PreCreateWindow(CREATESTRUCT& cs)
{
	// TODO: Modify the Window class or styles here by modifying
	//  the CREATESTRUCT cs

	return CFormView::PreCreateWindow(cs);
}

void CDStarView::OnInitialUpdate()
{
	CFormView::OnInitialUpdate();
	
	CSize sizeTotal( GetSystemMetrics( SM_CXSCREEN )/1.1, GetSystemMetrics( SM_CYSCREEN )/1.2 );
	CSize sizePages( sizeTotal.cx/2, sizeTotal.cy/2 );
	CSize sizeLines( sizeTotal.cx/50, sizeTotal.cy/50 );
	
	SetScrollSizes( MM_TEXT, sizeTotal );
	GetParentFrame()->RecalcLayout();
	ResizeParentToFit();
}

/////////////////////////////////////////////////////////////////////////////
// CDStarView diagnostics

#ifdef _DEBUG
void CDStarView::AssertValid() const
{
	CFormView::AssertValid();
}

void CDStarView::Dump(CDumpContext& dc) const
{
	CFormView::Dump(dc);
}

CDStarDoc* CDStarView::GetDocument() // non-debug version is inline
{
	ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CDStarDoc)));
	return (CDStarDoc*)m_pDocument;
}
#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CDStarView message handlers

void CDStarView::OnDraw(CDC* pDC) 
{
	GetDlgItem( IDC_PAUSE )->EnableWindow( m_bRun );
	GetDlgItem( IDC_BREAK )->ShowWindow( FALSE );
	/*******************  画目标点  ******************************************/
	CBrush pBrush( RGB(255,0,0) );
	CBrush* pBsh = (CBrush*)pDC->SelectObject( &pBrush );
	pDC->Ellipse( GoalPt.x*10, GoalPt.y*10, GoalPt.x*10+10, GoalPt.y*10+10 );
	pDC->SelectObject( pBsh );
	/************************************************************************/
	/*******************    画图    **********************************/
	CPen newPen( PS_SOLID, 1, RGB(0, 0, 0) );
	CPen* pOldPen = pDC->SelectObject( &newPen );
	for ( int nRow = 0; nRow <= ROWNUMS; nRow++ )
	{
		pDC->MoveTo( 0, 10*nRow );
		pDC->LineTo( COLNUMS*10, 10*nRow );
	}
	
	for ( int nCol = 0; nCol <= COLNUMS; nCol++ )
	{
		pDC->MoveTo( 10*nCol, 0 );
		pDC->LineTo( 10*nCol, ROWNUMS*10 );
	}
	/**********************************************************************/

	/***********************画障碍物***************************************/
	CStdioFile stdFile;
	if ( !stdFile.Open( m_strFile, CFile::modeRead ) )
	{
		AfxMessageBox("Error: Cann't find the File!");
		return;
	}
	stdFile.SeekToBegin();
	CString strRead("");
	int iPosRead = 0, iObsNum = 0;
	CBrush* pOldBrush = pDC->SelectObject( &ObsBrush );
	
	while( stdFile.ReadString( strRead ) )
	{
		int xPt,yPt;
		iPosRead = strRead.Find( ',' );
		xPt = atoi( strRead.Left( iPosRead ) );
		yPt = atoi( strRead.Right( strRead.GetLength() - iPosRead - 1 ) );
		if ( xPt >= 0 && xPt < COLNUMS && yPt >= 0 && yPt < ROWNUMS )
		{
			Obst.Add( CPoint(xPt, yPt) );
			pDC->Rectangle( xPt*10, yPt*10, xPt*10 + 10, yPt * 10 + 10 );		
		}
	}
	stdFile.Close();	
	/**********************************************************************/
	/****************     OutObst   ***************************************/
	if ( OutObst.GetSize() == 0 )
	{
		for ( int xObsLeft = 0; xObsLeft <= COLNUMS-1; xObsLeft++ )
			OutObst.Add( CPoint( xObsLeft, -1 ) );
		for ( int yObsUp = 0; yObsUp <= ROWNUMS-1; yObsUp++ )
			OutObst.Add( CPoint( -1, yObsUp ) );
		for ( int xObsRight = 0; xObsRight <= COLNUMS-1; xObsRight++ )
			OutObst.Add( CPoint( xObsRight, ROWNUMS ) );
		for ( int yObsDown = 0; yObsDown <= ROWNUMS-1; yObsDown++ )
			OutObst.Add( CPoint( COLNUMS, yObsDown ) );
	}
	/**********************************************************************/
	pDC->SelectObject( pOldBrush );
	pDC->SelectObject( pOldPen );
}

void CDStarView::OnSet() 
{
	CSetDlg setDlg;
	if ( setDlg.DoModal() == IDOK )
	{
		ROWNUMS = setDlg.m_RowNum;
		COLNUMS = setDlg.m_ColNum;		
		GoalPt.x = setDlg.m_xPt;
		GoalPt.y = setDlg.m_yPt;
		if ( IsInObst(GoalPt) == TRUE )
			AfxMessageBox("The Goal Point is in the Obstacle Sets");
		Invalidate();
	}
}

void CDStarView::OnRun()
{
	HWND hWnd = GetSafeHwnd();
	pThread =  AfxBeginThread( ThreadSearch, (LPVOID)hWnd, THREAD_PRIORITY_NORMAL );
	if ( pThread )
		m_bRun = TRUE;
}

void CDStarView::OnReset() 
{
	OpenList.RemoveAll();
	OpenList.FreeExtra();
	CloseList.RemoveAll();
	CloseList.FreeExtra();
	Obst.RemoveAll();
	Obst.FreeExtra();
	m_bRun = FALSE;
	Invalidate();
}

void CDStarView::OnSave() 
{
	CFileDialog fileDlg( FALSE, NULL, NULL, 
						OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT, 
		             "All Files(*.txt)|*.txt||", NULL );		

	if ( fileDlg.DoModal() == IDOK )
	{
		CString strWrite = fileDlg.GetPathName();
	    if ( strWrite.Find('.') == -1 )
			strWrite += ".txt";
		CStdioFile fileSave( strWrite, CFile::modeWrite | CFile::modeCreate );
		CString strBuffer;
		fileSave.SeekToBegin();
		int nPos = 0;
		int nObsNums = Obst.GetSize();
		while ( nPos < nObsNums )
		{
			strBuffer.Format( "%d,%d\n", Obst.GetAt( nPos ).x, Obst.GetAt( nPos ).y );
			fileSave.WriteString( strBuffer );
			nPos++;
		}
		fileSave.Close();
		AfxMessageBox("Save Success!");
	}
}

void CDStarView::OnLoad() 
{
	CFileDialog fileDlg( TRUE, NULL, NULL, 
		             OFN_HIDEREADONLY | OFN_HIDEREADONLY, 
		             "All Files(*.txt)|*.txt||", NULL );

	if ( fileDlg.DoModal() == IDOK )
	{
		m_strFile = fileDlg.GetPathName();
		Invalidate();
		OnReset();
	}
}

void CDStarView::OnPause() 
{
	CButton* pBtn = (CButton*)GetDlgItem( IDC_PAUSE );
	CString strText;
	pBtn->GetWindowText( strText );

	if ( strText == "Pause" )
	{
		pBtn->SetWindowText("Go On");
		pThread->SuspendThread();
	}