dstarview.cpp

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

	else
	{
		pBtn->SetWindowText("Pause");
		pThread->ResumeThread();
	}
}

void CDStarView::OnBreak() 
{
	if ( !bEndThread )
		bEndThread = TRUE;	
}

void CDStarView::EnablePause()
{
	GetDlgItem( IDC_PAUSE )->EnableWindow( TRUE );
	GetDlgItem( IDC_BREAK )->EnableWindow( TRUE );
	GetDlgItem( IDC_RUN )->EnableWindow( FALSE );
	GetDlgItem( IDC_SET )->EnableWindow( FALSE );
	GetDlgItem( IDC_RESET )->EnableWindow( FALSE );
	GetDlgItem( IDC_LOAD )->EnableWindow( FALSE );
	GetDlgItem( IDC_SAVE )->EnableWindow( FALSE );
}

void CDStarView::DisablePause()
{
	GetDlgItem( IDC_PAUSE )->EnableWindow( FALSE );
	GetDlgItem( IDC_BREAK )->EnableWindow( FALSE );
	GetDlgItem( IDC_RUN )->EnableWindow( TRUE );
	GetDlgItem( IDC_SET )->EnableWindow( TRUE );
	GetDlgItem( IDC_RESET )->EnableWindow( TRUE );
	GetDlgItem( IDC_LOAD )->EnableWindow( TRUE );
	GetDlgItem( IDC_SAVE )->EnableWindow( TRUE );
	m_bRun = FALSE;
}

void CDStarView::OnLButtonDown(UINT nFlags, CPoint point) 
{
	CRect rect( 0, 0, COLNUMS*10, ROWNUMS*10 );

	if ( rect.PtInRect( point ) )
	{
		int xPt = 0, yPt = 0;
		CDC* pDC = GetDC();
		xPt = point.x/10;
		yPt = point.y/10;
		
		int nObstLength = Obst.GetSize();
		for ( int i = 0; i < nObstLength; i++ )
		{
			if ( CPoint(xPt,yPt) == Obst.GetAt(i) )
			{	nObstLength--;		break;	}	
		}
			
		CBrush* pOldBrush = new CBrush;
		if  ( nObstLength == Obst.GetSize() )
		{
			pOldBrush = (CBrush*)pDC->SelectObject( &ObsBrush );
			pDC->Rectangle( xPt*10, yPt*10, xPt*10+10, yPt*10+10 );
			pDC->SelectObject( pOldBrush );
			Obst.Add( CPoint(xPt, yPt) );
		}
		else
		{
			CPen newPen( PS_SOLID, 0, RGB( 0, 0 ,0 ) );
			CPen* pOldPen = pDC->SelectObject( &newPen );

			CBrush newBrush( RGB(236, 233, 216) );
			pOldBrush = (CBrush*)pDC->SelectObject( &newBrush );

			pDC->Rectangle( xPt*10, yPt*10, xPt*10+11, yPt*10+11 );
			pDC->SelectObject( pOldBrush );
			pDC->SelectObject( pOldPen );

			Obst.RemoveAt( i );
		}
	}
		
	CFormView::OnLButtonDown(nFlags, point);
}

void CDStarView::OnModenew() 
{	iMode = 1;	}

void CDStarView::OnModeold() 
{	iMode = 0;	}

void CDStarView::OnUpdateModenew(CCmdUI* pCmdUI) 
{
	if ( iMode == 1 )
		pCmdUI->Enable( FALSE );
	else
		pCmdUI->Enable( TRUE );
}

void CDStarView::OnUpdateModeold(CCmdUI* pCmdUI) 
{
	if ( iMode == 0 )
		pCmdUI->Enable( FALSE );
	else
		pCmdUI->Enable( TRUE );	
}


void CDStarView::DrawObst()
{
	int iPos = Obst.GetSize();
	int x = Obst.GetAt( iPos-1 ).x;
	int y = Obst.GetAt( iPos-1 ).y;
	CDC* pDC = GetDC();
				
	CPen newPen( PS_SOLID, 0, RGB( 0, 0 ,0 ) );
	CPen* pOldPen = pDC->SelectObject( &newPen );
	CBrush newBrush( RGB(236, 233, 100) );
	CBrush* pOldBrush = (CBrush*)pDC->SelectObject( &newBrush );
	pDC->Rectangle( x*10, y*10, x*10+11, y*10+11 );
	pDC->SelectObject( pOldBrush );
	pDC->SelectObject( pOldPen );
}

/*************************************************************************************/
/*******************       全局函数   ************************************************/
/*************************************************************************************/
double GetDist( CPoint& pt1, CPoint& pt2 )
{
	return sqrt( (pt1.x-pt2.x)*(pt1.x-pt2.x) + (pt1.y-pt2.y)*(pt1.y-pt2.y) );
}

BOOL IsInObst( CPoint pt )
{
	int ObstNums = Obst.GetSize();
	for ( int i = 0; i < ObstNums; i++ )
	{		if ( Obst.GetAt(i) == pt )
				return TRUE;
	}
	return FALSE;
}

BOOL IsInOutObst( CPoint pt )
{
	int ObstNums = OutObst.GetSize();
	for ( int i = 0; i < ObstNums; i++ )
	{		if ( OutObst.GetAt(i) == pt )
				return TRUE;
	}
	return FALSE;
}

BOOL IsCorrect( CPoint& pt )
{
	if ( pt.x >= 0 && pt.x < COLNUMS &&
		 pt.y >= 0 && pt.y < ROWNUMS )
		return TRUE;
	else 
		return FALSE;
}

BOOL IsDanger( CState cs )
{
	CPoint pt = cs.m_CurPt;
	if ( (IsInObst( CPoint(pt.x+1, pt.y+1) ) || IsInOutObst( CPoint(pt.x+1, pt.y+1) ) &&
		 IsInObst( CPoint(pt.x  , pt.y+1) ) || IsInOutObst( CPoint(pt.x  , pt.y+1) ) &&
		 IsInObst( CPoint(pt.x+1, pt.y  ) ) || IsInOutObst( CPoint(pt.x+1, pt.y  ) ) )
		 ||
		 (IsInObst( CPoint(pt.x-1, pt.y-1) ) || IsInOutObst( CPoint(pt.x-1, pt.y-1) ) &&
		 IsInObst( CPoint(pt.x-1, pt.y  ) ) || IsInOutObst( CPoint(pt.x-1, pt.y  ) ) &&
		 IsInObst( CPoint(pt.x  , pt.y-1) ) || IsInOutObst( CPoint(pt.x  , pt.y-1) ) )
		 )
		return TRUE;
	return FALSE;
}

int InOpenList( CState& st )
{
	int nOpenLength = OpenList.GetSize();
	if ( nOpenLength == 0 )		return -1;
	for ( int i = 0; i < nOpenLength; i++ )
	{
		if ( OpenList.GetAt(i).m_CurPt == st.m_CurPt )
			return i;
	}
	return -1;
}

int InCloseList( CState& st )
{

	return -1;
}

void InsertIntoOpen( CState& cs )
{
	int nOpenLength = OpenList.GetSize();
	if ( nOpenLength == 0 )
		{	OpenList.Add( cs );		return;		}
	else
	{
		for ( int i = 0; i < nOpenLength; i++ )
		{
			if ( OpenList.GetAt(i).m_dCost > cs.m_dCost )
			{	OpenList.InsertAt( i, cs );		return;		}
		}
		OpenList.Add( cs );
	}
}

/************************************************************************/
/*           搜索线程入口                                               */
/************************************************************************/

UINT ThreadSearch( LPVOID param )
{
	::PostMessage( (HWND)param, WM_ENABLEPAUSE, 0, 0 );
	CString strWarn;
	if ( IsInObst(GoalPt) == TRUE )
	{	strWarn = "Failure, GoalPt in the Obstacle Sets!!";			
		AfxMessageBox( strWarn );
		::PostMessage( (HWND)param, WM_DISABLEPAUSE, 0, 0 );
		return 0;	
	}
	
	HDC hDc = GetDC((HWND)param);
	CDC* pDC = CDC::FromHandle( hDc );
	COLORREF bkColor = GetBkColor( hDc );
	CBrush newBrush( RGB(200, 105, 55) );
	CBrush* pOldBrush = new CBrush;
	OpenList.RemoveAll();	OpenList.FreeExtra();
	CloseList.RemoveAll();  CloseList.FreeExtra();
		
	CState CurState( StartPt );
	CurState.m_dCosted = 0.000;
	CurState.SetCost( iMode, GetDist( CurState.m_CurPt, GoalPt ) );
	OpenList.Add( CurState );

	int iDepth = 0;
	CState cs = CurState;
	CPoint pt;

	while ( TRUE )
	{
		if ( OpenList.GetSize() == 0 )
		{	strWarn = "Failure...";		goto END;	}  //失败
		CurState = OpenList.GetAt(0);
		CRect rect;
		rect.SetRect( CurState.m_CurPt.x*10, CurState.m_CurPt.y*10, (CurState.m_CurPt.x+1)*10, (CurState.m_CurPt.y+1)*10 );
		pOldBrush = (CBrush*)pDC->SelectObject( &newBrush );
		pDC->Ellipse( rect );       //画当前点的状态
		if ( CurState.m_CurPt == GoalPt )
		{	strWarn = "Success...";		goto END;	}  //成功
		
		OpenList.RemoveAt(0);	OpenList.FreeExtra();	CloseList.Add( CurState );
		
		Sleep(20);
		/* 产生8个后继结点*/
		for ( int i = 0; i < 8; i++ )
		{
			cs = CurState;
			pt.x = CurState.m_CurPt.x + NextPt[i][0];
			pt.y = CurState.m_CurPt.y + NextPt[i][1];
			if ( !IsCorrect(pt) || IsInObst(pt) )
				continue;
				
			cs.SetCurPt( pt );	  
			cs.SetForPt( CurState.m_CurPt );	 
			if ( NextPt[i][0] !=0 && NextPt[i][1] != 0 )
				cs.m_dCosted = CurState.m_dCosted + sqrt(2);
			else
				cs.m_dCosted = CurState.m_dCosted + 1.00;
			cs.SetCost( iMode, GetDist( cs.m_CurPt, GoalPt ) );
			
			if ( OpenList.GetSize() == 0 )
			{	OpenList.Add( cs );		continue;	}
			if ( InOpenList( cs ) != -1 )
			{
				if ( OpenList.GetAt(i).m_dCost > cs.m_dCost )
					OpenList.SetAt( i, cs );
			}
			else 
			{	if ( InCloseList( cs ) != -1 )
				{	if ( CloseList.GetAt(i).m_dCost > cs.m_dCost )
					{	CloseList.SetAt( i, cs );   InsertIntoOpen( cs );  }
				}
				else
				{	InsertIntoOpen( cs ); 	}
			}
		}	
		
		CBrush bsh( RGB( 150,150,255 ) );
		pDC->SelectObject( &bsh );
		rect.SetRect( CurState.m_CurPt.x*10, CurState.m_CurPt.y*10, (CurState.m_CurPt.x+1)*10, (CurState.m_CurPt.y+1)*10 );
		pDC->Rectangle( rect );
		iDepth++;
	}
	
END:
	pDC->SelectObject( pOldBrush );
	AfxMessageBox( strWarn );
	::PostMessage( (HWND)param, WM_DISABLEPAUSE, 0, 0 );
	return 0;
}