源程序.docx
- 文档编号:5091690
- 上传时间:2022-12-13
- 格式:DOCX
- 页数:76
- 大小:35.99KB
源程序.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_j ct=ct_j; //pruningsearchifnobetterpenaltycanbeobtainedanymoreforthisvelocity if(_safetyFactor/ct+dV>=min_penalty){ break; } } } floatpenalty=_safetyFactor/ct+dV; if(penalty min_penalty=penalty; _vNew=vCand; } } } //--------------------------------------------------------------- voidAgent: : insertObstacleNeighbor(intid,float&rangeSq){ Obstacle*obstacle=_sim->_obstacles[id]; floatdistSq=distSqPointLineSegment(obstacle->_p1,obstacle->_p2,_p); if(distSq if(! _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&&distSq 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; } } } voidAgent: : insertAgentNeighbor(intid,float&rangeSq){ Agent*other=_sim->_agents[id]; if(this! =other){ floatdistSq=absSq(_p-other->_p); if(distSq if(! _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&&distSq 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; } } } } 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
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 源程序
![提示](https://static.bdocx.com/images/bang_tan.gif)