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