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