00001
00002 #ifndef INCLUDED_WaypointEngine_h_
00003 #define INCLUDED_WaypointEngine_h_
00004
00005 #include "Shared/ListMemBuf.h"
00006 #include "Shared/LoadSave.h"
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 template<unsigned int MAX_WAY>
00056 class WaypointEngine : public LoadSave {
00057 public:
00058 static const unsigned int MAX_WAYPOINTS=MAX_WAY;
00059
00060
00061 struct Waypoint {
00062
00063 enum posType_t {
00064 POSTYPE_EGOCENTRIC,
00065 POSTYPE_OFFSET,
00066 POSTYPE_ABSOLUTE
00067 };
00068 Waypoint()
00069 : x(0), y(0), angle(0), arc(), speed(), turnSpeed(), posType(), angleIsRelative(), trackPath()
00070 {}
00071 Waypoint(float xc, float yc, Waypoint::posType_t pos_rel, float ac, bool ang_rel, float spd, bool track, float turn)
00072 : x(xc), y(yc), angle(ac), arc(0), speed(spd), turnSpeed(turn), posType(pos_rel), angleIsRelative(ang_rel), trackPath(track)
00073 {}
00074 float x;
00075 float y;
00076 float angle;
00077 float arc;
00078 float speed;
00079 float turnSpeed;
00080 posType_t posType;
00081 bool angleIsRelative;
00082 bool trackPath;
00083 };
00084
00085 typedef ListMemBuf<Waypoint,MAX_WAYPOINTS> WaypointList_t;
00086 typedef typename ListMemBuf<Waypoint,MAX_WAYPOINTS>::index_t WaypointListIter_t;
00087
00088
00089 WaypointEngine()
00090 : LoadSave(), waypoints(), isRunning(false), isLooping(true), isTracking(false),
00091 curWaypoint(waypoints.end()), waypointTime(0), waypointDistance(0), pathLength(0), arcRadius(0),
00092 lastUpdateTime(0), Pcorr(.5), turnSpeed(.65)
00093 {init();}
00094
00095 WaypointEngine(char * f)
00096 : LoadSave(), waypoints(), isRunning(false), isLooping(true), isTracking(false),
00097 curWaypoint(waypoints.end()), waypointTime(0), waypointDistance(0), pathLength(0), arcRadius(0),
00098 lastUpdateTime(0), Pcorr(.5), turnSpeed(.65)
00099 {init(); LoadFile(f); }
00100
00101
00102
00103
00104 virtual unsigned int getBinSize() const;
00105 virtual unsigned int LoadBuffer(const char buf[], unsigned int len);
00106 virtual unsigned int SaveBuffer(char buf[], unsigned int len) const;
00107
00108 virtual void go();
00109 virtual void pause();
00110 virtual void unpause();
00111
00112 virtual void setIsLooping(bool isl) { isLooping=isl; }
00113 virtual bool getIsLooping() const { return isLooping; }
00114
00115 virtual WaypointList_t& getWaypointList() { return waypoints; }
00116 virtual const WaypointList_t& getWaypointList() const { return waypoints; }
00117
00118 virtual WaypointListIter_t getCurWaypointID() const { return curWaypoint; }
00119
00120 virtual float getCurX() const { return curPos[0]; }
00121 virtual float getCurY() const { return curPos[1]; }
00122 virtual float getCurA() const { return curPos[2]; }
00123
00124 virtual void setCurPos(float x, float y, float a) {
00125 curPos[0]=x; curPos[1]=y; curPos[2]=a;
00126 }
00127
00128
00129
00130 virtual bool cycle();
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141 virtual void addEgocentricWaypoint(float forward, float left, float angle, bool angleIsRelative, float speed) {
00142 waypoints.push_back(Waypoint(forward,left,Waypoint::POSTYPE_EGOCENTRIC,angle,angleIsRelative,speed,isTracking,turnSpeed));
00143 }
00144
00145
00146
00147
00148
00149
00150 virtual void addOffsetWaypoint(float x, float y, float angle, bool angleIsRelative, float speed) {
00151 waypoints.push_back(Waypoint(x,y,Waypoint::POSTYPE_OFFSET,angle,angleIsRelative,speed,isTracking,turnSpeed));
00152 }
00153
00154
00155
00156
00157
00158
00159 virtual void addAbsoluteWaypoint(float x, float y, float angle, bool angleIsRelative, float speed) {
00160 waypoints.push_back(Waypoint(x,y,Waypoint::POSTYPE_ABSOLUTE,angle,angleIsRelative,speed,isTracking,turnSpeed));
00161 }
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171 virtual void addEgocentricArc(float forward, float left, float angle, bool angleIsRelative, float speed, float arc) {
00172 addEgocentricWaypoint(forward,left,angle,angleIsRelative,speed);
00173 fixArc(arc);
00174 }
00175
00176
00177
00178
00179
00180
00181
00182
00183 virtual void addOffsetArc(float x, float y, float angle, bool angleIsRelative, float speed, float arc) {
00184 addOffsetWaypoint(x,y,angle,angleIsRelative,speed);
00185 fixArc(arc);
00186 }
00187
00188
00189
00190
00191
00192
00193
00194
00195 virtual void addAbsoluteArc(float x, float y, float angle, bool angleIsRelative, float speed, float arc) {
00196 addAbsoluteWaypoint(x,y,angle,angleIsRelative,speed);
00197 fixArc(arc);
00198 }
00199
00200
00201
00202
00203 virtual void setTargetWaypoint(WaypointListIter_t iter) {
00204
00205 bool isLoop=false;
00206 if(iter==waypoints.end()) {
00207 if(isLooping && waypoints.size()>0) {
00208 iter=waypoints.begin();
00209 for(unsigned int i=0; i<3; i++)
00210 pathStartPos[i]=curPos[i];
00211 isLoop=true;
00212 } else {
00213 isRunning=false;
00214 curWaypoint=iter;
00215 for(unsigned int i=0; i<3; i++) {
00216 sourcePos[i]=targetPos[i];
00217 targetPos[i]=curPos[i];
00218 curVel[i]=0;
00219 }
00220 return;
00221 }
00222 }
00223 if(iter==waypoints.next(curWaypoint) || isLoop)
00224 for(unsigned int i=0; i<3; i++)
00225 sourcePos[i]=targetPos[i];
00226 else
00227 for(unsigned int i=0; i<3; i++)
00228 sourcePos[i]=curPos[i];
00229
00230 Waypoint target;
00231 if(isLoop)
00232 target=calcAbsoluteCoords(iter,pathStartPos[0],pathStartPos[1],pathStartPos[2]);
00233 else
00234 target=calcAbsoluteCoords(iter);
00235 targetPos[0]=target.x;
00236 targetPos[1]=target.y;
00237 targetPos[2]=target.angle;
00238
00239 float dx=targetPos[0]-sourcePos[0];
00240 float dy=targetPos[1]-sourcePos[1];
00241 waypointDistance=sqrt(dx*dx+dy*dy);
00242 waypointTime=get_time();
00243 curWaypoint=iter;
00244
00245 float radiusRatio=sin(waypoints[iter].arc/2);
00246 arcRadius = (radiusRatio==0) ? 0 : (waypointDistance/2)/radiusRatio;
00247 pathLength = arcRadius!=0 ? arcRadius*waypoints[iter].arc : waypointDistance;
00248
00249 cout << "Target is now: ("<<targetPos[0]<<','<<targetPos[1]<<','<<targetPos[2]<<")" << endl;
00250 }
00251
00252
00253 Waypoint calcAbsoluteCoords(WaypointListIter_t it) {
00254
00255 bool isAhead=false;
00256 for(WaypointListIter_t c=curWaypoint; c!=waypoints.end(); c=waypoints.next(c))
00257 if(c==it) {
00258 isAhead=true;
00259 break;
00260 }
00261 if(!isAhead)
00262 return calcAbsoluteCoords(it,pathStartPos[0],pathStartPos[1],pathStartPos[2]);
00263 Waypoint cur(targetPos[0],targetPos[1],Waypoint::POSTYPE_ABSOLUTE,targetPos[2],false,0,isTracking,turnSpeed);
00264 if(it==curWaypoint)
00265 return cur;
00266 for(WaypointListIter_t c=waypoints.next(curWaypoint); c!=waypoints.end(); c=waypoints.next(c)) {
00267 applyWaypoint(cur,waypoints[c]);
00268 if(c==it)
00269 break;
00270 }
00271 return cur;
00272 }
00273
00274
00275 Waypoint calcAbsoluteCoords(WaypointListIter_t it,float sx, float sy, float sa) {
00276 Waypoint cur(sx,sy,Waypoint::POSTYPE_ABSOLUTE,sa,false,0,isTracking,turnSpeed);
00277 for(WaypointListIter_t c=waypoints.begin(); c!=waypoints.end(); c=waypoints.next(c)) {
00278 applyWaypoint(cur,waypoints[c]);
00279 if(c==it)
00280 break;
00281 }
00282 return cur;
00283 }
00284
00285
00286 protected:
00287 void init();
00288
00289
00290
00291 void applyWaypoint(Waypoint& cur, const Waypoint& next);
00292
00293
00294 void fixArc(float arc);
00295
00296
00297
00298 void computeCurrentPosition(unsigned int t);
00299 void checkNextWaypoint(unsigned int t);
00300 void computeIdeal(unsigned int t);
00301 void computeNewVelocity(unsigned int t);
00302
00303
00304 static float normalizeAngle(float a) {
00305 while(a>M_PI)
00306 a-=M_PI*2;
00307 while(a<-M_PI)
00308 a+=M_PI*2;
00309 return a;
00310 }
00311
00312
00313 static float clipRange(float x, float min, float max) {
00314 if(x<min)
00315 return min;
00316 else if(x>max)
00317 return max;
00318 else
00319 return x;
00320 }
00321
00322 WaypointList_t waypoints;
00323
00324 bool isRunning;
00325 bool isLooping;
00326 bool isTracking;
00327 unsigned int curWaypoint;
00328 unsigned int waypointTime;
00329 float waypointDistance;
00330 float pathLength;
00331 float arcRadius;
00332 unsigned int lastUpdateTime;
00333 float pathStartPos[3];
00334 float sourcePos[3];
00335 float targetPos[3];
00336 float idealPos[4];
00337 float curPos[3];
00338 float curVel[3];
00339 float eps[3];
00340 float Pcorr;
00341 float turnSpeed;
00342 };
00343
00344 template<unsigned int MAX_WAY>
00345 void WaypointEngine<MAX_WAY>::go() {
00346 isRunning=true;
00347 for(unsigned int i=0; i<3; i++) {
00348 curVel[i]=0;
00349 pathStartPos[i]=sourcePos[i]=curPos[i];
00350 }
00351 Waypoint target(curPos[0],curPos[1],Waypoint::POSTYPE_ABSOLUTE,curPos[2],false,0,isTracking,turnSpeed);
00352 applyWaypoint(target,waypoints.front());
00353 targetPos[0]=target.x;
00354 targetPos[1]=target.y;
00355 targetPos[2]=target.angle;
00356 lastUpdateTime=get_time();
00357 curWaypoint=waypoints.begin();
00358 setTargetWaypoint(curWaypoint);
00359 }
00360
00361 template<unsigned int MAX_WAY>
00362 void WaypointEngine<MAX_WAY>::pause() {
00363 isRunning=false;
00364 }
00365
00366 template<unsigned int MAX_WAY>
00367 void WaypointEngine<MAX_WAY>::unpause() {
00368 if(curWaypoint==waypoints.end())
00369 go();
00370 isRunning=true;
00371 for(unsigned int i=0; i<3; i++)
00372 curVel[i]=0;
00373 lastUpdateTime=get_time();
00374 }
00375
00376 template<unsigned int MAX_WAY>
00377 bool WaypointEngine<MAX_WAY>::cycle() {
00378 if(!isRunning)
00379 return false;
00380
00381 unsigned int curtime=get_time();
00382 if(curWaypoint!=waypoints.end()) {
00383 computeCurrentPosition(curtime);
00384 checkNextWaypoint(curtime);
00385 }
00386 if(curWaypoint!=waypoints.end()) {
00387 computeIdeal(curtime);
00388 computeNewVelocity(curtime);
00389 }
00390
00391 return true;
00392 }
00393
00394 template<unsigned int MAX_WAY>
00395 unsigned int WaypointEngine<MAX_WAY>::getBinSize() const {
00396 unsigned int numPrecision=9;
00397 unsigned int wpSize=0;
00398 unsigned int boilerplateSize=0;
00399 boilerplateSize+=strlen("#WyP\n");
00400 boilerplateSize+=strlen("#add_{point|arc} {ego|off|abs} x_val y_val {hold|follow} angle_val speed_val arc_val\n");
00401 wpSize+=strlen("max_turn_speed ")+numPrecision+1;
00402 wpSize+=strlen("track_path false\n");
00403 wpSize+=strlen("add_point ")+4+numPrecision*5+1*5+strlen("follow");
00404 boilerplateSize+=strlen("#END\n");
00405 return wpSize*waypoints.size()+boilerplateSize;
00406 }
00407
00408 template<unsigned int MAX_WAY>
00409 unsigned int WaypointEngine<MAX_WAY>::LoadBuffer(const char buf[], unsigned int len) {
00410 unsigned int origlen=len;
00411 waypoints.clear();
00412 if(strncmp("#WyP\n",buf,5)!=0) {
00413 return 0;
00414 }
00415
00416 float turn=turnSpeed;
00417 bool track=isTracking;
00418 char cmd[40];
00419 char posType[40];
00420 float x_val=0;
00421 float y_val=0;
00422 char angType[40];
00423 bool ang_val=0;
00424 float angle_val=0;
00425 float speed_val=0;
00426 float arc_val=0;
00427 unsigned int linenum=2;
00428 while(len<=origlen && len>0) {
00429
00430 if(buf[0]=='#' || buf[0]=='\n') {
00431 if(strncmp("#END\n",buf,5)==0)
00432 return origlen-len;
00433 else {
00434 while(*buf++!='\n') {}
00435 continue;
00436 }
00437 }
00438 int used=-1U;
00439 sscanf(buf,"%40s%n",&cmd,&used);
00440 if(!ChkAdvance(used,&buf,&len,"*** ERROR Waypoint list load corrupted - ran out of room line %d\n",linenum)) return 0;
00441 if(strncasecmp(cmd,"add_point",9)==0 || strncasecmp(cmd,"add_arc",7)==0) {
00442 sscanf(buf,"%40s %g %g %40s %g %g %g%n",posType,&x_val,&y_val,angType,&angle_val,&speed_val,&arc_val,&used);
00443 if(!ChkAdvance(used,&buf,&len,"*** ERROR Waypoint list load corrupted - bad read on add at line %d\n",linenum)) return 0;
00444 if(strncasecmp(angType,"hold",4)==0)
00445 ang_val=false;
00446 else if(strncasecmp(angType,"follow",6)==0)
00447 ang_val=true;
00448 else {
00449 printf("*** ERROR WaypointEngine: Invalid angle value type %s\n",angType);
00450 return 0;
00451 }
00452 if(strncasecmp(cmd,"add_point",9)==0) {
00453 if(strncasecmp(posType,"ego",3)==0)
00454 addEgocentricWaypoint(x_val,y_val,angle_val,ang_val,speed_val);
00455 else if(strncasecmp(posType,"off",3)==0)
00456 addOffsetWaypoint(x_val,y_val,angle_val,ang_val,speed_val);
00457 else if(strncasecmp(posType,"abs",3)==0)
00458 addAbsoluteWaypoint(x_val,y_val,angle_val,ang_val,speed_val);
00459 else {
00460 printf("*** ERROR WaypointEngine: Invalid position type %s\n",posType);
00461 return 0;
00462 }
00463 waypoints.back().arc=arc_val;
00464 } else {
00465 if(strncasecmp(posType,"ego",3)==0)
00466 addEgocentricArc(x_val,y_val,angle_val,ang_val,speed_val,arc_val);
00467 else if(strncasecmp(posType,"off",3)==0)
00468 addOffsetArc(x_val,y_val,angle_val,ang_val,speed_val,arc_val);
00469 else if(strncasecmp(posType,"abs",3)==0)
00470 addAbsoluteArc(x_val,y_val,angle_val,ang_val,speed_val,arc_val);
00471 else {
00472 printf("*** ERROR WaypointEngine: Invalid position type %s\n",posType);
00473 return 0;
00474 }
00475 }
00476 waypoints.back().trackPath=track;
00477 waypoints.back().turnSpeed=turn;
00478 } else if(strncasecmp(cmd,"track_path",10)==0) {
00479 sscanf(buf,"%d%n",&track,&used);
00480 if(!ChkAdvance(used,&buf,&len,"*** ERROR Waypoint load corrupted - bad read on track_path line %d\n",linenum)) return 0;
00481 } else if(strncasecmp(cmd,"max_turn_speed",14)==0) {
00482 sscanf(buf,"%g%n",&turn,&used);
00483 if(!ChkAdvance(used,&buf,&len,"*** ERROR Waypoint load corrupted - bad read on max_turn_speed line %d\n",linenum)) return 0;
00484 } else {
00485 printf("*** ERROR WaypointEngine: Invalid command %s\n",cmd);
00486 return 0;
00487 }
00488
00489 linenum++;
00490 }
00491 std::cout << "*** WARNING WaypointEngine: load missing #END" << std::endl;
00492 return origlen-len;
00493 }
00494
00495 template<unsigned int MAX_WAY>
00496 unsigned int WaypointEngine<MAX_WAY>::SaveBuffer(char buf[], unsigned int len) const {
00497 unsigned int origLen=len;
00498 unsigned int used;
00499 unsigned int cnt=0;
00500
00501 used=snprintf(buf,len,"#WyP\n");
00502 if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on header\n")) return 0;
00503
00504 used=snprintf(buf,len,"#add_{point|arc} {ego|off|abs} x_val y_val {hold|follow} angle_val speed_val arc_val\n");
00505 if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on header\n")) return 0;
00506
00507
00508 float turn=waypoints.front().turnSpeed-1;
00509 bool track=!waypoints.front().trackPath;
00510
00511 for(WaypointListIter_t it=waypoints.begin(); it!=waypoints.end(); it=waypoints.next(it)) {
00512 if(waypoints[it].turnSpeed!=turn) {
00513 turn=waypoints[it].turnSpeed;
00514 used=snprintf(buf,len,"max_turn_speed %g\n",turn);
00515 if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on waypoint %d turnSpeed\n",cnt)) return 0;
00516 }
00517 if(waypoints[it].trackPath!=track) {
00518 track=waypoints[it].trackPath;
00519 used=snprintf(buf,len,"track_path %d\n",track);
00520 if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on waypoint %d\n trackPath",cnt)) return 0;
00521 }
00522 const char * posType=NULL;
00523 switch(waypoints[it].posType) {
00524 case Waypoint::POSTYPE_EGOCENTRIC:
00525 posType="EGO"; break;
00526 case Waypoint::POSTYPE_OFFSET:
00527 posType="OFF"; break;
00528 case Waypoint::POSTYPE_ABSOLUTE:
00529 posType="ABS"; break;
00530 }
00531 if(waypoints[it].arc!=0)
00532 used=snprintf(buf,len,"add_point %s %g %g %s %g %g %g\n",posType,waypoints[it].x,waypoints[it].y,(waypoints[it].angleIsRelative?"FOLLOW":"HOLD"),waypoints[it].angle,waypoints[it].speed,waypoints[it].arc);
00533 else
00534 used=snprintf(buf,len,"add_point %s %g %g %s %g %g %g\n",posType,waypoints[it].x,waypoints[it].y,(waypoints[it].angleIsRelative?"FOLLOW":"HOLD"),waypoints[it].angle,waypoints[it].speed,waypoints[it].arc);
00535 if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on waypoint %d\n",cnt)) return 0;
00536 cnt++;
00537 }
00538
00539 used=snprintf(buf,len,"#END\n");
00540 if(!ChkAdvance(used,(const char**)&buf,&len,"*** ERROR Waypoint list save failed on footer\n")) return 0;
00541
00542 return origLen-len;
00543 }
00544
00545 template<unsigned int MAX_WAY>
00546 void WaypointEngine<MAX_WAY>::init() {
00547 eps[0]=eps[1]=.01;
00548 eps[2]=0.0175;
00549 for(unsigned int i=0; i<3; i++)
00550 pathStartPos[i]=targetPos[i]=sourcePos[i]=curPos[i]=curVel[i]=0;
00551 for(unsigned int i=0; i<4; i++)
00552 idealPos[i]=0;
00553 }
00554
00555 template<unsigned int MAX_WAY>
00556 void WaypointEngine<MAX_WAY>::applyWaypoint(Waypoint& cur, const Waypoint& next) {
00557 float origx=cur.x;
00558 float origy=cur.y;
00559 switch(next.posType) {
00560 case Waypoint::POSTYPE_EGOCENTRIC: {
00561 cur.x+=next.x*cos(cur.angle)-next.y*sin(cur.angle);
00562 cur.y+=next.x*sin(cur.angle)+next.y*cos(cur.angle);
00563 break;
00564 }
00565 case Waypoint::POSTYPE_OFFSET:
00566 cur.x+=next.x;
00567 cur.y+=next.y;
00568 break;
00569 case Waypoint::POSTYPE_ABSOLUTE:
00570 cur.x=next.x;
00571 cur.y=next.y;
00572 break;
00573 }
00574 float dx=cur.x-origx;
00575 float dy=cur.y-origy;
00576 if(fabs(dx)<eps[0] && fabs(dy)<eps[1]) {
00577 if(next.angleIsRelative)
00578 cur.angle+=next.angle;
00579 else
00580 cur.angle=next.angle;
00581 } else {
00582 cur.angle=next.angle;
00583 if(next.angleIsRelative)
00584 cur.angle+=atan2(dy,dx);
00585 }
00586 cur.angle+=next.arc/2;
00587 cur.angle=normalizeAngle(cur.angle);
00588 }
00589
00590 template<unsigned int MAX_WAY>
00591 void WaypointEngine<MAX_WAY>::fixArc(float arc) {
00592 Waypoint& center=waypoints.back();
00593 float cdx=center.x;
00594 float cdy=center.y;
00595 if(center.posType==Waypoint::POSTYPE_ABSOLUTE) {
00596
00597 WaypointListIter_t start_it=waypoints.prev(waypoints.prev(waypoints.end()));
00598 if(start_it!=waypoints.end()) {
00599 Waypoint start=calcAbsoluteCoords(waypoints.prev(waypoints.prev(waypoints.end())));
00600 cdx-=start.x;
00601 cdy-=start.y;
00602 }
00603 }
00604 float r=sqrt(cdx*cdx+cdy*cdy);
00605 float ca=atan2(cdy,cdx);
00606 center.x-=r*cos(ca-arc);
00607 center.y-=r*sin(ca-arc);
00608 center.arc=arc;
00609 }
00610
00611 template<unsigned int MAX_WAY>
00612 void WaypointEngine<MAX_WAY>::computeCurrentPosition(unsigned int t) {
00613 float dt=(t-lastUpdateTime)/1000.f;
00614 float df=dt*curVel[0];
00615 float ds=dt*curVel[1];
00616 float da=dt*curVel[2];
00617
00618 float avgAngle=curPos[2]+da/2;
00619 float ca=cos(avgAngle);
00620 float sa=sin(avgAngle);
00621
00622 curPos[0]+=df*ca-ds*sa;
00623 curPos[1]+=df*sa+ds*ca;
00624 curPos[2]+=da;
00625 curPos[2]=normalizeAngle(curPos[2]);
00626
00627 lastUpdateTime=t;
00628 }
00629
00630 template<unsigned int MAX_WAY>
00631 void WaypointEngine<MAX_WAY>::checkNextWaypoint(unsigned int ) {
00632 float rx=targetPos[0]-curPos[0];
00633 float ry=targetPos[1]-curPos[1];
00634 float ra=targetPos[2]-curPos[2];
00635 if(fabs(rx)<eps[0] && fabs(ry)<eps[1] && fabs(ra)<eps[2]) {
00636
00637 setTargetWaypoint(waypoints.next(curWaypoint));
00638 }
00639 }
00640
00641 template<unsigned int MAX_WAY>
00642 void WaypointEngine<MAX_WAY>::computeIdeal(unsigned int t) {
00643 Waypoint& cur=waypoints[curWaypoint];
00644 if(cur.trackPath) {
00645 float dx=targetPos[0]-sourcePos[0];
00646 float dy=targetPos[1]-sourcePos[1];
00647 float dt=(t-waypointTime)/1000.f;
00648 float ideal_travel=dt*cur.speed;
00649 float p=1;
00650 if(pathLength!=0) {
00651 p=ideal_travel/pathLength;
00652 if(p>1)
00653 p=1;
00654 }
00655 if(arcRadius==0) {
00656 idealPos[0]=sourcePos[0]+dx*p;
00657 idealPos[1]=sourcePos[1]+dy*p;
00658 idealPos[2]=targetPos[2];
00659 idealPos[3]=atan2(dy,dx);
00660 } else {
00661
00662 float bearing=atan2(dy,dx);
00663 float center_bearing=bearing+(M_PI-cur.arc)/2;
00664 float cx=sourcePos[0]+arcRadius*cos(center_bearing);
00665 float cy=sourcePos[1]+arcRadius*sin(center_bearing);
00666 float arc_bearing=center_bearing-M_PI+cur.arc*p;
00667
00668 idealPos[0]=cx+arcRadius*cos(arc_bearing);
00669 idealPos[1]=cy+arcRadius*sin(arc_bearing);
00670 idealPos[3]=arc_bearing+M_PI/2;
00671 idealPos[2]=cur.angle;
00672 if(cur.angleIsRelative)
00673 idealPos[2]+=idealPos[3];
00674 idealPos[2]=normalizeAngle(idealPos[2]);
00675 idealPos[3]=normalizeAngle(idealPos[3]);
00676 }
00677 } else {
00678 idealPos[0]=curPos[0];
00679 idealPos[1]=curPos[1];
00680 float rx=targetPos[0]-curPos[0];
00681 float ry=targetPos[1]-curPos[1];
00682 if(fabs(rx)<eps[0] && fabs(ry)<eps[1]) {
00683 idealPos[2]=targetPos[2];
00684 } else {
00685 idealPos[2]=cur.angle;
00686 if(cur.angleIsRelative) {
00687 float dx=targetPos[0]-curPos[0];
00688 float dy=targetPos[1]-curPos[1];
00689 idealPos[2]+=atan2(dy,dx);
00690 }
00691 idealPos[2]=normalizeAngle(idealPos[2]);
00692 }
00693 idealPos[3]=atan2(ry,rx);
00694 if(arcRadius!=0) {
00695 float dt=(t-waypointTime)/1000.f;
00696 float ideal_travel=dt*cur.speed;
00697 float p=1;
00698 if(pathLength!=0) {
00699 p=ideal_travel/pathLength;
00700 if(p>1)
00701 p=1;
00702 }
00703 float arc=cur.arc*(1-p)/2;
00704 idealPos[2]=normalizeAngle(idealPos[2]-arc);
00705 idealPos[3]=normalizeAngle(idealPos[3]-arc);
00706 }
00707 }
00708 }
00709
00710 template<unsigned int MAX_WAY>
00711 void WaypointEngine<MAX_WAY>::computeNewVelocity(unsigned int ) {
00712 Waypoint& cur=waypoints[curWaypoint];
00713
00714
00715
00716 float dx=targetPos[0]-idealPos[0];
00717 float dy=targetPos[1]-idealPos[1];
00718 float spd=sqrt(dx*dx+dy*dy)/(FrameTime*NumFrames)*1000;
00719 if(spd>cur.speed) {
00720
00721 curVel[0]=cur.speed*cos(idealPos[3]-curPos[2]);
00722 curVel[1]=cur.speed*sin(idealPos[3]-curPos[2]);
00723 } else {
00724
00725 curVel[0]=spd*cos(idealPos[3]-curPos[2]);
00726 curVel[1]=spd*sin(idealPos[3]-curPos[2]);
00727 }
00728 if(arcRadius==0)
00729 curVel[2]=0;
00730 else
00731 curVel[2]=cur.speed/arcRadius;
00732
00733
00734
00735
00736 float ex=idealPos[0]-curPos[0];
00737 float ey=idealPos[1]-curPos[1];
00738 float ed=sqrt(ex*ex+ey*ey);
00739 float ehead=atan2(ey,ex)-curPos[2];
00740 float ea=normalizeAngle(idealPos[2]-curPos[2]);
00741 float easpd=ea/(FrameTime*NumFrames)*1000;
00742 easpd=clipRange(easpd,-cur.turnSpeed,cur.turnSpeed);
00743 curVel[0]+=Pcorr*ed*cos(ehead);
00744 curVel[1]+=Pcorr*ed*sin(ehead);
00745 curVel[2]+=easpd;
00746 }
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759 #endif