源程序.docx
《源程序.docx》由会员分享，可在线阅读，更多相关《源程序.docx（76页珍藏版）》请在冰豆网上搜索。
源程序
#ifdefMACOS
#include
#include
#include
#else
#include
#include
#include
#endif
#include
#include"Dstar.h"
inthh,ww;
intwindow;
Dstar*dstar;
intscale=6;
intmbutton=0;
intmstate=0;
boolb_autoreplan=true;
voidInitGL（intWidth,intHeight）
{
hh=Height;
ww=Width;
glClearColor（0.0f,0.0f,0.0f,0.0f）;
glClearDepth（1.0）;
glViewport（0,0,Width,Height）;
glMatrixMode（GL_PROJECTION）;
glLoadIdentity（）;
glOrtho（0,Width,0,Height,-100,100）;
glMatrixMode（GL_MODELVIEW）;
}
voidReSizeGLScene（intWidth,intHeight）
{
hh=Height;
ww=Width;
glViewport（0,0,Width,Height）;
glMatrixMode（GL_PROJECTION）;
glLoadIdentity（）;
glOrtho（0,Width,0,Height,-100,100）;
glMatrixMode（GL_MODELVIEW）;
}
voidDrawGLScene（）
{
usleep（100）;
glClear（GL_DEPTH_BUFFER_BIT|GL_COLOR_BUFFER_BIT）;
glLoadIdentity（）;
glPushMatrix（）;
glScaled（scale,scale,1）;
if（b_autoreplan）dstar->replan（）;
dstar->draw（）;
glPopMatrix（）;
glutSwapBuffers（）;
}
voidkeyPressed（unsignedcharkey,intx,inty）
{
usleep（100）;
switch（key）{
case'q':
case'Q':
glutDestroyWindow（window）;
exit（0）;
break;
case'r':
case'R':
dstar->replan（）;
break;
case'a':
case'A':
b_autoreplan=!
b_autoreplan;
break;
case'c':
case'C':
dstar->init（40,50,140,90）;
break;
}
}
voidmouseFunc（intbutton,intstate,intx,inty）{
y=hh-y+scale/2;
x+=scale/2;
mbutton=button;
if（（mstate=state）==GLUT_DOWN）{
if（button==GLUT_LEFT_BUTTON）{
dstar->updateCell（x/scale,y/scale,-1）;
}elseif（button==GLUT_RIGHT_BUTTON）{
dstar->updateStart（x/scale,y/scale）;
}elseif（button==GLUT_MIDDLE_BUTTON）{
dstar->updateGoal（x/scale,y/scale）;
}
}
}
voidmouseMotionFunc（intx,inty）{
y=hh-y+scale/2;
x+=scale/2;
y/=scale;
x/=scale;
if（mstate==GLUT_DOWN）{
if（mbutton==GLUT_LEFT_BUTTON）{
dstar->updateCell（x,y,-1）;
}elseif（mbutton==GLUT_RIGHT_BUTTON）{
dstar->updateStart（x,y）;
}elseif（mbutton==GLUT_MIDDLE_BUTTON）{
dstar->updateGoal（x,y）;
}
}
}
intmain（intargc,char**argv）{
glutInit（&argc,argv）;
glutInitDisplayMode（GLUT_RGBA|GLUT_DOUBLE|GLUT_DEPTH）;
glutInitWindowSize（1000,800）;
glutInitWindowPosition（50,20）;
window=glutCreateWindow（"DstarVisualizer"）;
glutDisplayFunc（&DrawGLScene）;
glutIdleFunc（&DrawGLScene）;
glutReshapeFunc（&ReSizeGLScene）;
glutKeyboardFunc（&keyPressed）;
glutMouseFunc（&mouseFunc）;
glutMotionFunc（&mouseMotionFunc）;
InitGL（800,600）;
dstar=newDstar（）;
dstar->init（40,50,140,90）;
printf（"----------------------------------\n"）;
printf（"DstarVisualizer\n"）;
printf（"Commands:
\n"）;
printf（"[q/Q]-Quit\n"）;
printf（"[r/R]-Replan\n"）;
printf（"[a/A]-ToggleAutoReplan\n"）;
printf（"[c/C]-Clear（restart）\n"）;
printf（"leftmouseclick-makecelluntraversable（cost-1）\n"）;
printf（"middlemouseclick-movegoaltocell\n"）;
printf（"rightmouseclick-movestarttocell\n"）;
printf（"----------------------------------\n"）;
glutMainLoop（）;
return1;
}
namespaceRVO{
//-----------------------------------------------------------//
//Implementationforclass:
Agent//
//-----------------------------------------------------------//
RVOSimulator*Agent:
:
_sim=RVOSimulator:
:
Instance（）;
//-----------------------------------------------------------
Agent:
:
Agent（）{}
//-----------------------------------------------------------
Agent:
:
Agent（constVector2&p,intgoalID）{
_atGoal=false;
_subGoal=-2;
_p=p;
_goalID=goalID;
_velSampleCount=_sim->_defaultAgent->_velSampleCount;
_neighborDist=_sim->_defaultAgent->_neighborDist;
_maxNeighbors=_sim->_defaultAgent->_maxNeighbors;
_class=_sim->_defaultAgent->_class;
_r=_sim->_defaultAgent->_r;
_gR=_sim->_defaultAgent->_gR;
_prefSpeed=_sim->_defaultAgent->_prefSpeed;
_maxSpeed=_sim->_defaultAgent->_maxSpeed;
_maxAccel=_sim->_defaultAgent->_maxAccel;
_o=_sim->_defaultAgent->_o;
_safetyFactor=_sim->_defaultAgent->_safetyFactor;
_v=_sim->_defaultAgent->_v;
}
Agent:
:
Agent（constVector2&p,intgoalID,intvelSampleCount,floatneighborDist,intmaxNeighbors,intclassID,floatr,constVector2&v,floatmaxAccel,floatgR,floatprefSpeed,floatmaxSpeed,floato,floatsafetyFactor）{
_atGoal=false;
_subGoal=-2;
_p=p;
_goalID=goalID;
_velSampleCount=velSampleCount;
_neighborDist=neighborDist;
_maxNeighbors=maxNeighbors;
_class=classID;
_r=r;
_gR=gR;
_prefSpeed=prefSpeed;
_maxSpeed=maxSpeed;
_maxAccel=maxAccel;
_o=o;
_safetyFactor=safetyFactor;
_v=v;
}
//-----------------------------------------------------------
Agent:
:
~Agent（）{}
//-----------------------------------------------------------
//Searchingforthebestnewvelocity
voidAgent:
:
computeNewVelocity（）{
floatmin_penalty=RVO_INFTY;
Vector2vCand;
//Selectnum_samplescandidatevelocitieswithinthecircleofradius_maxSpeed
for（intn=0;n<_velSampleCount;++n）{
if（n==0）{
vCand=_vPref;
}else{
floatangle=rand（）*2*RVO_PI/RAND_MAX;
floatradius=sqrt（（rand（）/（float）RAND_MAX））;
vCand=Vector2（radius*_maxSpeed*cos（angle）,radius*_maxSpeed*sin（angle））;
}
floatdV;//distancebetweencandidatevelocityandpreferredvelocity
if（_collision）{
dV=0;
}else{
dV=abs（vCand-_vPref）;
}
//searchingforsmallesttimetocollision
floatct=RVO_INFTY;//timetocollision
//iterateoverneighbors
for（std:
:
set:
pair:
pair>>:
:
iteratorj=_neighbors.begin（）;j!
=_neighbors.end（）;++j）{
floatct_j;//timetocollisionwithagentj
Vector2Vab;
inttype=j->second.first;
intid=j->second.second;
if（type==AGENT）{
Agent*other=_sim->_agents[id];
Vab=2*vCand-_v-other->_v;
floattime=timeToCollision（_p,Vab,other->_p,_r+other->_r,_collision）;
if（_collision）{
ct_j=-ceil（time/_sim->_timeStep）;
ct_j-=absSq（vCand）/sqr（_maxSpeed）;
}else{
ct_j=time;
}
}elseif（type==OBSTACLE）{
Obstacle*other;
other=_sim->_obstacles[id];
floattime_1,time_2,time_a,time_b;
time_1=timeToCollision（_p,vCand,other->_p1,_r,_collision）;
time_2=timeToCollision（_p,vCand,other->_p2,_r,_collision）;
time_a=timeToCollision（_p,vCand,other->_p1+_r*other->_normal,other->_p2+_r*other->_normal,_collision）;
time_b=timeToCollision（_p,vCand,other->_p1-_r*other->_normal,other->_p2-_r*other->_normal,_collision）;
if（_collision）{
floattime=std:
:
max（std:
:
max（std:
:
max（time_1,time_2）,time_a）,time_b）;
ct_j=-ceil（time/_sim->_timeStep）;
ct_j-=absSq（vCand）/sqr（_maxSpeed）;
}else{
floattime=std:
:
min（std:
:
min（std:
:
min（time_1,time_2）,time_a）,time_b）;
if（time<_sim->_timeStep||sqr（time）ct_j=time;
}else{
ct_j=RVO_INFTY;//nopenalty
}
}
}
if（ct_jct=ct_j;
//pruningsearchifnobetterpenaltycanbeobtainedanymoreforthisvelocity
if（_safetyFactor/ct+dV>=min_penalty）{
break;
}
}
}
floatpenalty=_safetyFactor/ct+dV;
if（penaltymin_penalty=penalty;
_vNew=vCand;
}
}
}
//---------------------------------------------------------------
voidAgent:
:
insertObstacleNeighbor（intid,float&rangeSq）{
Obstacle*obstacle=_sim->_obstacles[id];
floatdistSq=distSqPointLineSegment（obstacle->_p1,obstacle->_p2,_p）;
if（distSqif（!
_collision）{
_collision=true;
_neighbors.clear（）;
rangeSq=sqr（_r）;
}
if（_neighbors.size（）==_maxNeighbors）{
_neighbors.erase（--_neighbors.end（））;
}
_neighbors.insert（std:
:
make_pair（distSq,std:
:
make_pair（OBSTACLE,id）））;
if（_neighbors.size（）==_maxNeighbors）{
rangeSq=（--_neighbors.end（））->first;
}
}elseif（!
_collision&&distSqif（_neighbors.size（）==_maxNeighbors）{
_neighbors.erase（--_neighbors.end（））;
}
_neighbors.insert（std:
:
make_pair（distSq,std:
:
make_pair（OBSTACLE,id）））;
if（_neighbors.size（）==_maxNeighbors）{
rangeSq=（--_neighbors.end（））->first;
}
}
}
voidAgent:
:
insertAgentNeighbor（intid,float&rangeSq）{
Agent*other=_sim->_agents[id];
if（this!
=other）{
floatdistSq=absSq（_p-other->_p）;
if（distSq_r）&&distSqif（!
_collision）{
_collision=true;
_neighbors.clear（）;
}
if（_neighbors.size（）==_maxNeighbors）{
_neighbors.erase（--_neighbors.end（））;
}
_neighbors.insert（std:
:
make_pair（distSq,std:
:
make_pair（AGENT,id）））;
if（_neighbors.size（）==_maxNeighbors）{
rangeSq=（--_neighbors.end（））->first;
}
}elseif（!
_collision&&distSqif（_neighbors.size（）==_maxNeighbors）{
_neighbors.erase（--_neighbors.end（））;
}
_neighbors.insert（std:
:
make_pair（distSq,std:
:
make_pair（AGENT,id）））;
if（_neighbors.size（）==_maxNeighbors）{
rangeSq=（--_neighbors.end（））->first;
}
}
}
}
voidAgent:
:
computeNeighbors（）{
//Computenewneighborsofagent;
//sortthemaccordingtodistance（optimizedeffectofpruningheuristicinsearchfornewvelocities）;
//seperatebetweencollidingandnearagents
_collision=false;
_neighbors.clear（）;
//checkobstacleneighbors
floatrangeSq=std:
:
min（sqr（_neighborDist）,sqr（std:
:
max（_sim->_timeStep,_maxSpeed/_maxAccel）*_maxSpeed+_r））;
_sim->_kdTree->computeObstacleNeighbors（this,rangeSq）;
/*for（intj=0;j<（int）_sim->_obstacles.size（）;++j）{
insertObstacleNeighbor（j,rangeSq）;
}*/
if（_collision）{
return;
}
//Checkother
升级会员 