rrt算法引用.cpp

足球机器人避障算法

// rrt算法引用.cpp : Defines the entry point for the application.
//

#include "stdafx.h"
#include "resource.h"
#include <cmath>
#include <vector>
using namespace std;
#define MAX_LOADSTRING 100
/******************************/
int flag=0;//0 画起点，1 终点 2 障碍物
struct pos{double x,y;pos(){x=-20,y=-20;}};
vector< pos> data;
pos begin;
pos end;

vector<pos>rPath;
vector<pos>wayPoint;
const double minDis=20;
const double minPath=1;
/*************************************/
void draw(HWND hwnd);
double getDis(pos a,pos b);
pos getNextPos(pos a);
void find_rrt();
void draw_rPath(HWND hwnd);
/***************************************/
HINSTANCE hInst;								// current instance
LRESULT CALLBACK	dlg(HWND, UINT, WPARAM, LPARAM);
int APIENTRY WinMain(HINSTANCE hInstance,
                     HINSTANCE hPrevInstance,
                     LPSTR     lpCmdLine,
                     int       nCmdShow)
{
 
 DialogBox(hInst, (LPCTSTR)IDD_ABOUTBOX, 0, (DLGPROC)dlg);
	return 0;
}


LRESULT CALLBACK dlg(HWND hDlg, UINT message, WPARAM wParam, LPARAM lParam)
{
	switch (message)
	{
		case WM_INITDIALOG:
			   	return TRUE;
        case WM_PAINT:{
	     	draw(hDlg);
					  }break;

		case WM_COMMAND:
			switch(LOWORD(wParam))
			{
			case IDOK :
			case  IDCANCEL: 
				{
				EndDialog(hDlg, LOWORD(wParam));
				return TRUE;
				}break;
			case IDC_BUTTON1:find_rrt();draw_rPath(hDlg);break;
			case IDC_RADIO1:flag=0;break;
            case IDC_RADIO2:flag=1;break;
            case IDC_RADIO3:flag=2;break;
			}break;
		case WM_LBUTTONDOWN :
			{
		     switch(flag)
			 {
			 case 0:
				    if(LOWORD(lParam)>20&&LOWORD(lParam)<600&&HIWORD(lParam)>40&&HIWORD(lParam)<400)
					{
					begin.x=LOWORD(lParam);begin.y=HIWORD(lParam);InvalidateRect(hDlg,NULL,true);UpdateWindow(hDlg);draw(hDlg);
					}break;
             case 1:
			    	 if(LOWORD(lParam)>20&&LOWORD(lParam)<600&&HIWORD(lParam)>40&&HIWORD(lParam)<400)
					 {
						end.x=LOWORD(lParam);end.y=HIWORD(lParam);InvalidateRect(hDlg,NULL,true);UpdateWindow(hDlg);draw(hDlg);
					 }break;
			 case 2:  if(LOWORD(lParam)>20&&LOWORD(lParam)<600&&HIWORD(lParam)>40&&HIWORD(lParam)<400)
					  {
					   pos temp;temp.x=LOWORD(lParam);temp.y=HIWORD(lParam);data.push_back(temp);InvalidateRect(hDlg,NULL,true);UpdateWindow(hDlg) ; draw(hDlg);
					  }break;
			 }
			}break;
        case WM_RBUTTONDOWN :
			if(flag==2&&data.size()>0){data.pop_back();InvalidateRect(hDlg,NULL,true);UpdateWindow(hDlg) ;draw(hDlg);}
			break;
	}
    return FALSE;
}
void draw(HWND hwnd)
{
HDC hdc=GetDC(hwnd);
Ellipse(hdc,begin.x-20,begin.y-20,begin.x+20,begin.y+20); 
Ellipse(hdc,end.x-20,end.y-20,end.x+20,end.y+20); 
for(int i=0;i<data.size();i++)
Ellipse(hdc,data[i].x-10,data[i].y-10,data[i].x+10,data[i].y+10); 
ReleaseDC(NULL,hdc); 

}
void draw_rPath(HWND hwnd)
{
HDC hdc=GetDC(hwnd);
MoveToEx(hdc,rPath[0].x,rPath[0].y,0);
pos temp=rPath[0];
for(int i=1;i<rPath.size();i++)
LineTo(hdc,rPath[i].x,rPath[i].y);
ReleaseDC(NULL,hdc); 

}
void find_rrt()
{
   rPath.clear();
   pos temp=begin;
   int cnt=0;
   while(getDis(temp,end)>minPath)
   {
     pos aPos=getNextPos(temp);
	 bool flag=true;
	 int i=0;
	 for(i=0;i<data.size();i++)
	 {
		 if(getDis(aPos,data[i])<minDis)
		 {
			 flag=false;
			 cnt++;
			 break;
		 }
	 }
	 if(cnt>100000)break;;
	 if(flag){rPath.push_back(temp);temp=aPos;}
   }
 rPath.push_back(temp);
 if(cnt<=100000)rPath.push_back(end);
}
double getDis(pos a,pos b)
{
return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
}
pos getNextPos(pos a)
{
 double rnd=rand()%356*1.0;
 double k=rand()%100;
 double xDet=cos(rnd*3.1421592654/180.0)*minPath;
 double yDet=sin(rnd*3.1421592654/180.0)*minPath;
 double len=getDis(a,end);
 pos temp;
 temp.x=(end.x-a.x)*minPath/len;
 temp.y=(end.y-a.y)*minPath/len;
 a.x+=(xDet*(100-k)+temp.x*k)/100;
 a.y+=(yDet*(100-k)+temp.y*k)/100;
 return a;
}