00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00037
00039
00040
00041 #ifndef PATH_H
00042 #define PATH_H
00043
00044 #ifdef MIP_HOST_APPLE
00045 #include <applePatch.h>
00046 #endif
00047
00048 #include <iostream>
00049 #include <math.h>
00050 #include <Spaces.h>
00051 #include <Time.h>
00052
00053
00054 using namespace std;
00055
00058 enum PathType{
00059 NULL_PATH,
00060 ASTEROID,
00061 CARDIOID,
00062 CLOTHOID,
00063 ELLIPSE,
00064 EIGHT,
00065 SEGMENT,
00066 PATH_NUM
00067 };
00068
00071 static const char* PathNames[PATH_NUM] = {
00072 "Null Path(NotInitialized)",
00073 "Asteroid",
00074 "Cardioid",
00075 "Clothoid",
00076 "Ellipse",
00077 "Eight",
00078 "Segment"
00079 };
00080
00081
00082
00087 class AsteroidParam{
00088 private:
00090 Decimal _A;
00091
00092 public:
00095 AsteroidParam(){
00096 setA(1.0);
00097 }
00100 AsteroidParam(Decimal A){
00101 setA(A);
00102 }
00105 AsteroidParam(const AsteroidParam& p){
00106 _A=p._A;
00107 }
00109 AsteroidParam& operator=(const AsteroidParam& p);
00110
00111
00112
00115 void setA(Decimal value){
00116 _A=value;
00117 }
00118
00119
00120
00123 Decimal A(){
00124 return _A;
00125 }
00126
00129 string print();
00130 };
00131
00136 class CardioidParam{
00137 private:
00139 Decimal _A;
00140
00141 public:
00143 CardioidParam(){
00144 setA(1.0);
00145 }
00147 CardioidParam(Decimal A){
00148 setA(A);
00149 }
00151 CardioidParam(const CardioidParam& p){
00152 _A=p._A;
00153 }
00155 CardioidParam& operator=(const CardioidParam& p);
00156
00157
00158
00160 void setA(Decimal value){
00161 _A=value;
00162 }
00163
00164
00165
00168 Decimal A(){
00169 return _A;
00170 }
00171
00174 string print();
00175 };
00176
00182 class ClothoidParam{
00183 private:
00184 Decimal _A;
00185 Decimal _tau;
00186 Decimal _sommaParziale;
00187 public:
00188 ClothoidParam(){
00189 setA(1.0);
00190 }
00191 ClothoidParam(Decimal A){
00192 setA(A);
00193 }
00194
00195 void setA(Decimal value){
00196 _A=value;
00197 }
00198
00199 void setTau(Decimal value){
00200 _tau=value;
00201 }
00202
00203 void setSommaParziale(Decimal value){
00204 _sommaParziale=value;
00205 }
00206
00207
00208 Decimal A(){
00209 return _A;
00210 }
00211
00212 Decimal tau(){
00213 return _tau;
00214 }
00215
00216 Decimal sommaParziale(){
00217 return _sommaParziale;
00218 }
00219 };
00220
00225 class EllipseParam{
00226 private:
00227 Decimal _R1;
00228 Decimal _R2;
00229 public:
00231 EllipseParam(){
00232 setR1(1);
00233 setR2(1);
00234 }
00236 EllipseParam(Decimal R1, Decimal R2){
00237 setR1(R1);
00238 setR2(R2);
00239 }
00240
00242 EllipseParam(const EllipseParam& p){
00243 _R1=p._R1;
00244 _R2=p._R2;
00245 }
00246
00248 EllipseParam& operator=(const EllipseParam& p){
00249 if (this != &p){
00250 _R1=p._R1;
00251 _R2=p._R2;
00252 }
00253 return *this;
00254 }
00255
00256
00257
00259 void setR1(Decimal value){
00260 _R1=value;
00261 };
00262
00264 void setR2(Decimal value){
00265 _R2=value;
00266 }
00267
00268
00269
00271 Decimal R1(){
00272 return _R1;
00273 }
00274
00276 Decimal R2(){
00277 return _R2;
00278 }
00279
00281 string print(){
00282 stringstream s;
00283 s.precision(3);
00284 s.setf(ios::fixed,ios::floatfield);
00285 s<<"Horizontal axis\t"<<_R1<<endl;
00286 s<<"Vertical axis\t"<<_R2<<endl;
00287 return s.str();
00288 }
00289 };
00290
00297 class EightParam{
00298 private:
00299 Decimal _R1;
00300 Decimal _R2;
00301 public:
00303 EightParam(){
00304 setR1(1);
00305 setR2(1);
00306 }
00308 EightParam(Decimal R1, Decimal R2){
00309 setR1(R1);
00310 setR2(R2);
00311 }
00312
00314 EightParam(const EightParam& p){
00315 _R1=p._R1;
00316 _R2=p._R2;
00317 }
00318
00320 EightParam& operator=(const EightParam& p){
00321 if (this != &p){
00322 _R1=p._R1;
00323 _R2=p._R2;
00324 }
00325 return *this;
00326 }
00327
00328
00329
00331 void setR1(Decimal value){
00332 _R1=value;
00333 }
00334
00336 void setR2(Decimal value){
00337 _R2=value;
00338 }
00339
00340
00341
00343 Decimal R1(){
00344 return _R1;
00345 }
00346
00348 Decimal R2(){
00349 return _R2;
00350 };
00351
00353 string print(){
00354 stringstream s;
00355 s.precision(3);
00356 s.setf(ios::fixed,ios::floatfield);
00357 s<<"Horizontal axis\t"<<_R1<<endl;
00358 s<<"Vertical axis\t"<<_R2<<endl;
00359 return s.str();
00360 }
00361 };
00362
00367 class SegmentParam{
00368 private:
00369 Position _A;
00370 Position _B;
00371 public:
00373 SegmentParam(){
00374 Position A=Position(0.0,0.0);
00375 Position B=Position(1.0,1.0);
00376 setA(A);
00377 setB(B);
00378 }
00380 SegmentParam(Position A,Position B){
00381 setA(A);
00382 setB(B);
00383 }
00384
00386 SegmentParam(const SegmentParam& p){
00387 _A=p._A;
00388 _B=p._B;
00389 }
00390
00392 SegmentParam& operator=(const SegmentParam& p){
00393 if (this != &p){
00394 _A=p._A;
00395 _B=p._B;
00396 }
00397 return *this;
00398 }
00399
00400
00401
00403 void setA(Position value){
00404 _A=value;
00405 }
00406
00408 void setB(Position value){
00409 _B=value;
00410 }
00411
00412
00413
00415 Position A(){
00416 return _A;
00417 }
00418
00420 Position B(){
00421 return _B;
00422 }
00423
00425 string print(){
00426 stringstream s;
00427 s.precision(3);
00428 s.setf(ios::fixed,ios::floatfield);
00429 s<<"Starting point\t"<<_A.print()<<endl;
00430 s<<"Ending point\t"<<_B.print()<<endl;
00431 return s.str();
00432 }
00433 };
00434
00440
00441 class Path{
00442 private:
00443 Position _center;
00444 Position _centredPos;
00445 Position _centredPosp;
00446 Position _centredPospp;
00447 Position _pos;
00448 Position _posp;
00449 Position _pospp;
00450 Angle _theta;
00451 Angle _thetap;
00452 Angle _thetapp;
00453 Angle _rotation;
00454 bool _clockwise;
00455 PathType _pathType;
00456
00457 public:
00459 Path(){
00460 Position origin=Position(0.0,0.0);
00461 setCenter(origin);
00462 setPos(origin);
00463 setPosp(origin);
00464 setPospp(origin);
00465 setTheta(0.0);
00466 setThetap(0.0);
00467 setThetapp(0.0);
00468 setRotation(0.0);
00469 unsetClockwise();
00470 setPathType(NULL_PATH);
00471 };
00472
00474 Path(const Path& path){
00475 _center=path._center;
00476 _centredPos=path._centredPos;
00477 _centredPosp=path._centredPosp;
00478 _centredPospp=path._centredPospp;
00479 _pos=path._pos;
00480 _posp=path._posp;
00481 _pospp=path._pospp;
00482 _theta=path._theta;
00483 _thetap=path._thetap;
00484 _thetapp=path._thetapp;
00485 _rotation=path._rotation;
00486 _clockwise=path._clockwise;
00487 _pathType=path._pathType;
00488
00489 }
00490
00492 Path& operator=(const Path& path){
00493 if (this != &path){
00494 _center=path._center;
00495 _centredPos=path._centredPos;
00496 _centredPosp=path._centredPosp;
00497 _centredPospp=path._centredPospp;
00498 _pos=path._pos;
00499 _posp=path._posp;
00500 _pospp=path._pospp;
00501 _theta=path._theta;
00502 _thetap=path._thetap;
00503 _thetapp=path._thetapp;
00504 _rotation=path._rotation;
00505 _clockwise=path._clockwise;
00506 _pathType=path._pathType;
00507 }
00508 return *this;
00509 }
00510
00512 bool operator!=(const Path& path){
00513 return( _center!=path._center ||
00514 _rotation!=path._rotation ||
00515 _clockwise!=path._clockwise ||
00516 _pathType!=path._pathType);
00517 }
00518
00522 virtual Position evalCentredPos(Decimal s)=0;
00523
00527 virtual Position evalCentredPosp(Decimal s)=0;
00528
00532 virtual Position evalCentredPospp(Decimal s)=0;
00533
00537 virtual Angle evalTheta(Decimal s)=0;
00538
00542 virtual Angle evalThetap(Decimal s, Decimal sp)=0;
00543
00547 virtual Angle evalThetapp(Decimal s, Decimal sp,Decimal spp)=0;
00548
00549 Position rotate(Position p, Angle theta){
00550 Position rotP = Position(p.x()*cos(theta.dCast2Pi())-p.y()*sin(theta.dCast2Pi()),
00551 p.x()*sin(theta.dCast2Pi())+p.y()*cos(theta.dCast2Pi()));
00552 return rotP;
00553 }
00554
00555
00556
00558 void setCenter(Position value){
00559 _center=value;
00560 }
00561
00563 void setCentredPos(Position value){
00564 _centredPos=value;
00565 }
00566
00568 void setCentredPosp(Position value){
00569 _centredPosp=value;
00570 }
00571
00573 void setCentredPospp(Position value){
00574 _centredPospp=value;
00575 }
00576
00578 void setPos(Position value){
00579 _pos=value;
00580 }
00581
00583 void setPosp(Position value){
00584 _posp=value;
00585 }
00586
00588 void setPospp(Position value){
00589 _pospp=value;
00590 }
00591
00593 void setTheta(Angle value){
00594 _theta=value;
00595 }
00596
00598 void setThetap(Angle value){
00599 _thetap=value;
00600 }
00601
00603 void setThetapp(Angle value){
00604 _thetapp=value;
00605 }
00606
00608 void evalPos(Decimal s){
00609 evalCentredPos(s);
00610 Position rotatedCentredPos=(Position)rotate(centredPos(),rotation());
00611 setCentredPos(rotatedCentredPos);
00612 _pos=centredPos()+center();
00613 }
00614
00616 void evalPosp(Decimal s, Decimal sp){
00617 evalCentredPosp(s);
00618 Position rotatedCentredPosp=(Position)rotate(centredPosp(),rotation());
00619 setCentredPosp(rotatedCentredPosp);
00620 _posp=centredPosp()*sp;
00621 }
00622
00624 void evalPospp(Decimal s, Decimal sp,Decimal spp){
00625 evalCentredPospp(s);
00626 Position rotatedCentredPospp=(Position)rotate(centredPospp(),rotation());
00627 setCentredPospp(rotatedCentredPospp);
00628 _pospp=centredPospp()*(sp*sp)+centredPosp()*spp;
00629 }
00630
00632 void setRotation(Angle value){
00633 _rotation=value;
00634 }
00635
00637 void setClockwise(){
00638 _clockwise=true;
00639 }
00640
00642 void unsetClockwise(){
00643 _clockwise=false;
00644 }
00645
00647 void setPathType(PathType value){
00648 _pathType=value;
00649 }
00650
00651
00652
00654 Position center(){
00655 return _center;
00656 }
00657
00659 Position centredPos(){
00660 return _centredPos;
00661 }
00662
00664 Position centredPosp(){
00665 return _centredPosp;
00666 }
00667
00669 Position centredPospp(){
00670 return _centredPospp;
00671 }
00672
00674 Position pos(){
00675 return _pos;
00676 }
00677
00679 Position posp(){
00680 return _posp;
00681 }
00682
00684 Position pospp(){
00685 return _pospp;
00686 }
00687
00689 Angle theta(){
00690 return _theta;
00691 }
00692
00694 Angle thetap(){
00695 return _thetap;
00696 }
00697
00699 Angle thetapp(){
00700 return _thetapp;
00701 }
00702
00703
00705 Angle rotation(){
00706 return _rotation;
00707 }
00708
00710 bool clockwise(){
00711 return _clockwise;
00712 }
00713
00715 PathType pathType(){
00716 return _pathType;
00717 }
00718
00720 string print(){
00721 stringstream s;
00722 s.precision(3);
00723 s.setf(ios::fixed,ios::floatfield);
00724 s<<"Path Type\t"<<PathNames[_pathType]<<endl;
00725 s<<"Center\t\t"<<_center.print()<<endl;
00726 if(_clockwise)
00727 s<<"Clockwise\tYES"<<endl;
00728 else
00729 s<<"Clockwise\tNO"<<endl;
00730 s<<"Rotation\t"<<_rotation.dCast2Pi()<<endl;
00731 return s.str();
00732 }
00733 };
00734
00741 class Asteroid : public Path{
00742 private:
00743 AsteroidParam _param;
00744 public:
00746 Asteroid(AsteroidParam param){
00747 Position origin=Position(0.0,0.0);
00748 setCenter(origin);
00749 setRotation(0.0);
00750 _param.setA(param.A());
00751 setPathType(ASTEROID);
00752 }
00754 Asteroid(Position center,AsteroidParam param){
00755 setCenter(center);
00756 _param.setA(param.A());
00757 setPathType(ASTEROID);
00758 }
00759
00761 Asteroid(Position center,Angle rotation,AsteroidParam param){
00762 setCenter(center);
00763 setRotation(rotation);
00764 _param.setA(param.A());
00765 setPathType(ASTEROID);
00766 }
00767
00770 Asteroid(const Asteroid& asteroid):Path(asteroid){
00771 _param=asteroid._param;
00772 }
00773
00776 Asteroid& operator=(const Asteroid& asteroid){
00777 if (this != &asteroid){
00778 Path::operator=(asteroid);
00779 _param=asteroid._param;
00780 }
00781 return *this;
00782 }
00783
00785 Position evalCentredPos(Decimal s);
00786
00788 Position evalCentredPosp(Decimal s);
00789
00791 Position evalCentredPospp(Decimal s);
00792
00794 Angle evalTheta(Decimal s){}
00795
00797 Angle evalThetap(Decimal s, Decimal sp){}
00798
00800 Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
00801
00802
00803
00804
00806 void setParam(AsteroidParam param){
00807 _param.setA(param.A());
00808 }
00809
00810
00811
00813 AsteroidParam param(){
00814 return _param;
00815 }
00816
00818 string print(){
00819 stringstream s;
00820 s.precision(3);
00821 s.setf(ios::fixed,ios::floatfield);
00822 s<<Path::print();
00823 s<<_param.print()<<endl;
00824 return s.str();
00825 }
00826 };
00827
00828
00833 class Cardioid : public Path{
00834 private:
00835 CardioidParam _param;
00836 public:
00838 Cardioid(CardioidParam param){
00839 Position origin=Position(0.0,0.0);
00840 setCenter(origin);
00841 setRotation(0.0);
00842 _param.setA(param.A());
00843 setPathType(CARDIOID);
00844 }
00846 Cardioid(Position center,CardioidParam param){
00847 setCenter(center);
00848 _param.setA(param.A());
00849 setPathType(CARDIOID);
00850 }
00851
00853 Cardioid(Position center,Angle rotation,CardioidParam param){
00854 setCenter(center);
00855 setRotation(rotation);
00856 _param.setA(param.A());
00857 setPathType(CARDIOID);
00858 }
00859
00862 Cardioid(const Cardioid& cardioid):Path(cardioid){
00863 _param=cardioid._param;
00864 }
00865
00868 Cardioid& operator=(const Cardioid& cardioid){
00869 if (this != &cardioid){
00870 Path::operator=(cardioid);
00871 _param=cardioid._param;
00872 }
00873 return *this;
00874 }
00875
00877 Position evalCentredPos(Decimal s);
00878
00880 Position evalCentredPosp(Decimal s);
00881
00883 Position evalCentredPospp(Decimal s);
00884
00886 Angle evalTheta(Decimal s){}
00887
00889 Angle evalThetap(Decimal s, Decimal sp){}
00890
00892 Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
00893
00894
00895
00896
00898 void setParam(CardioidParam param){
00899 _param.setA(param.A());
00900 }
00901
00902
00903
00905 CardioidParam param(){
00906 return _param;
00907 }
00908
00910 string print(){
00911 stringstream s;
00912 s.precision(3);
00913 s.setf(ios::fixed,ios::floatfield);
00914 s<<Path::print();
00915 s<<_param.print()<<endl;
00916 return s.str();
00917 }
00918 };
00919
00920
00926 class Clothoid : public Path{
00927 private:
00928 public:
00929 Decimal _A;
00930 Decimal _tauPrev;
00931 Decimal parsumX;
00932 Decimal parsumY;
00933 Decimal prevS;
00934 Decimal r;
00935
00936 Angle _alfa;
00937 Decimal _Amin;
00938 Decimal _tauMax;
00939
00940
00941
00942
00943
00944
00945
00946
00947
00949 Clothoid(){
00950 _A=0.5;
00951 parsumX=0.0;
00952 parsumY=0.0;
00953 _tauPrev=0.0;
00954 prevS=0.0;
00955 setRotation(0.0);
00956
00957
00958
00959 }
00961 Clothoid(Decimal A){
00962 _A=A;
00963 parsumX=0.0;
00964 parsumY=0.0;
00965 _tauPrev=0.0;
00966 prevS=0.0;
00967
00968
00969
00970 }
00971
00973 Clothoid(Position center,Decimal A){
00974 setCenter(center);
00975 _A=A;
00976 parsumX=0.0;
00977 parsumY=0.0;
00978 _tauPrev=0.0;
00979 prevS=0.0;
00980
00981
00982
00983 }
00984
00986 Clothoid(Position center,Angle rotation,Decimal A){
00987 setCenter(center);
00988 setRotation(rotation);
00989 _A=A;
00990 parsumX=0.0;
00991 parsumY=0.0;
00992 _tauPrev=0.0;
00993 prevS=0.0;
00994
00995
00996
00997 }
00998
00999 Decimal evalTau(Decimal s){
01000 return (pow(s,2)/(pow(_A,2)*2));
01001 }
01002
01003 void evalAmin(Decimal r){
01004 _Amin=r*sqrt(0.4*_alfa.dCast2Pi());
01005 }
01006
01007 void evalTauMax(Angle alfa){
01008 _tauMax=alfa.dCast2Pi()/3.0;
01009 }
01010
01012 Position evalCentredPos(Decimal s);
01013
01015 Position evalCentredPosp(Decimal s);
01016
01018 Position evalCentredPospp(Decimal s);
01019
01021 Angle evalTheta(Decimal s){}
01022
01024 Angle evalThetap(Decimal s, Decimal sp){}
01025
01027 Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
01028 };
01029
01030
01035 class Ellipse : public Path{
01036 private:
01037 EllipseParam _param;
01038 public:
01039 Ellipse(){
01040 setPathType(ELLIPSE);
01041 };
01043 Ellipse(EllipseParam param){
01044 Position origin=Position(0.0,0.0);
01045 setCenter(origin);
01046 setRotation(0.0);
01047 _param.setR1(param.R1());
01048 _param.setR2(param.R2());
01049 setPathType(ELLIPSE);
01050 }
01052 Ellipse(Position center,EllipseParam param){
01053 setCenter(center);
01054 _param.setR1(param.R1());
01055 _param.setR2(param.R2());
01056 setPathType(ELLIPSE);
01057 }
01058
01060 Ellipse(Position center,Angle rotation,EllipseParam param){
01061 setCenter(center);
01062 setRotation(rotation);
01063 _param.setR1(param.R1());
01064 _param.setR2(param.R2());
01065 setPathType(ELLIPSE);
01066 }
01067
01070 Ellipse(const Ellipse& ellipse):Path(ellipse){
01071 _param=ellipse._param;
01072 }
01073
01076 Ellipse& operator=(const Ellipse& ellipse){
01077 if (this != &ellipse){
01078 Path::operator=(ellipse);
01079 _param=ellipse._param;
01080 }
01081 return *this;
01082 }
01083
01085 Position evalCentredPos(Decimal s);
01086
01088 Position evalCentredPosp(Decimal s);
01089
01091 Position evalCentredPospp(Decimal s);
01092
01094 Angle evalTheta(Decimal s);
01095
01097 Angle evalThetap(Decimal s, Decimal sp);
01098
01100 Angle evalThetapp(Decimal s, Decimal sp,Decimal spp);
01101
01102
01103
01104
01106 void setParam(EllipseParam param){
01107 _param.setR1(param.R1());
01108 _param.setR2(param.R2());
01109 }
01110
01111
01112
01114 EllipseParam param(){
01115 return _param;
01116 }
01117
01119 string print(){
01120 stringstream s;
01121 s.precision(3);
01122 s.setf(ios::fixed,ios::floatfield);
01123 s<<Path::print();
01124 s<<_param.print()<<endl;
01125 return s.str();
01126 }
01127 };
01128
01133 class Eight : public Path{
01134 private:
01135 EightParam _param;
01136 public:
01138 Eight(EightParam param){
01139 Position origin=Position(0.0,0.0);
01140 setCenter(origin);
01141 setRotation(0.0);
01142 _param.setR1(param.R1());
01143 _param.setR2(param.R2());
01144 setPathType(EIGHT);
01145 }
01147 Eight(Position center,EightParam param){
01148 setCenter(center);
01149 _param.setR1(param.R1());
01150 _param.setR2(param.R2());
01151 setPathType(EIGHT);
01152 }
01153
01155 Eight(Position center,Angle rotation,EightParam param){
01156 setCenter(center);
01157 setRotation(rotation);
01158 _param.setR1(param.R1());
01159 _param.setR2(param.R2());
01160 setPathType(EIGHT);
01161 }
01162
01165 Eight(const Eight& eight):Path(eight){
01166 _param=eight._param;
01167 }
01168
01171 Eight& operator=(const Eight& eight){
01172 if (this != &eight){
01173 Path::operator=(eight);
01174 _param=eight._param;
01175 }
01176 return *this;
01177 }
01178
01180 Position evalCentredPos(Decimal s);
01181
01183 Position evalCentredPosp(Decimal s);
01184
01186 Position evalCentredPospp(Decimal s);
01187
01189 Angle evalTheta(Decimal s){}
01190
01192 Angle evalThetap(Decimal s, Decimal sp){}
01193
01195 Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
01196
01197
01198
01199
01201 void setParam(EightParam param){
01202 _param.setR1(param.R1());
01203 _param.setR2(param.R2());
01204 }
01205
01206
01207
01209 EightParam param(){
01210 return _param;
01211 }
01212
01214 string print(){
01215 stringstream s;
01216 s.precision(3);
01217 s.setf(ios::fixed,ios::floatfield);
01218 s<<Path::print();
01219 s<<_param.print()<<endl;
01220 return s.str();
01221 }
01222 };
01223
01228 class Segment : public Path{
01229 private:
01230 SegmentParam _param;
01231 public:
01232 Segment(){
01233 setPathType(SEGMENT);
01234 };
01236 Segment(SegmentParam param){
01237 Position origin=Position(0.0,0.0);
01238 setCenter(origin);
01239 setRotation(0.0);
01240 _param.setA(param.A());
01241 _param.setB(param.B());
01242 setPathType(SEGMENT);
01243 };
01244
01247 Segment(const Segment& segment):Path(segment){
01248 _param=segment._param;
01249 }
01250
01253 Segment& operator=(const Segment& segment){
01254 if (this != &segment){
01255 Path::operator=(segment);
01256 _param=segment._param;
01257 }
01258 return *this;
01259 }
01260
01262 Position evalCentredPos(Decimal s);
01263
01265 Position evalCentredPosp(Decimal s);
01266
01268 Position evalCentredPospp(Decimal s);
01269
01271 Angle evalTheta(Decimal s);
01272
01274 Angle evalThetap(Decimal s, Decimal sp);
01275
01277 Angle evalThetapp(Decimal s, Decimal sp,Decimal spp);
01278
01279
01280
01281
01283 void setParam(SegmentParam param){
01284 _param.setA(param.A());
01285 _param.setB(param.B());
01286 }
01287
01288
01289
01291 SegmentParam param(){
01292 return _param;
01293 }
01294
01296 string print(){
01297 stringstream s;
01298 s.precision(3);
01299 s.setf(ios::fixed,ios::floatfield);
01300 s<<Path::print();
01301 s<<_param.print()<<endl;
01302 return s.str();
01303 }
01304 };
01305
01306
01312 class Clothoid2Param{
01313 private:
01314 Position _A;
01315 Position _B;
01316 Position _C;
01317 Decimal _eps;
01318 Decimal _a;
01319 Decimal _d;
01320 public:
01322 Clothoid2Param(){
01323 Position A=Position(0.0,0.0);
01324 Position B=Position(1.0,1.0);
01325 Position C=Position(2.0,2.0);
01326 setA(A);
01327 setB(B);
01328 setB(C);
01329 setEps(1.0);
01330 setA(1.0);
01331 setD(1.0);
01332 }
01334 Clothoid2Param(Position A,Position B,Position C,Decimal eps,Decimal a,Decimal d){
01335 setA(A);
01336 setB(B);
01337 setC(C);
01338 setEps(eps);
01339 setA(a);
01340 setD(d);
01341 }
01342
01343
01344
01345
01347 void setA(Position value){
01348 _A=value;
01349 }
01350
01352 void setB(Position value){
01353 _B=value;
01354 }
01355
01357 void setC(Position value){
01358 _C=value;
01359 }
01360
01362 void setEps(Decimal value){
01363 _eps=value;
01364 };
01365
01367 void setA(Decimal value){
01368 _a=value;
01369 };
01370
01372 void setD(Decimal value){
01373 _d=value;
01374 };
01375
01376
01377
01378
01380 Position A(){
01381 return _A;
01382 }
01383
01385 Position B(){
01386 return _B;
01387 }
01388
01390 Position C(){
01391 return _C;
01392 }
01393
01395 Decimal eps(){
01396 return _eps;
01397 };
01398
01400 Decimal a(){
01401 return _a;
01402 };
01403
01405 Decimal d(){
01406 return _d;
01407 };
01408
01409 };
01410
01411
01412
01418 class Clothoid2 : public Path{
01419 private:
01420 Clothoid2Param _param;
01421 Decimal _X0,_Xc,_Yc,_Kc,_e;
01422 Angle _alfa,_thetac;
01423 Pose _ref;
01424 Position _start;
01425 Position _end;
01426 bool _ribalta;
01427
01429 Position Base2RegNormale(Position punto, Pose reg);
01430
01432 Position Reg2BaseRibaltato(Position punto, Pose reg);
01433
01435 Position Reg2BaseNormale(Position punto, Pose reg);
01436
01438 Position Reg2Base(Position punto, Pose reg);
01439
01441 Position Base2Reg(Position punto, Pose reg);
01442
01444 Decimal SF(Decimal z, Decimal coef);
01445
01447 Decimal CF(Decimal z, Decimal coef);
01448
01450 void Assesta();
01451 public:
01453 Clothoid2(Clothoid2Param param);
01454
01456 Position evalCentredPos(Decimal s);
01457
01459 Position evalCentredPosp(Decimal s);
01460
01462 Position evalCentredPospp(Decimal s);
01463
01465 Angle evalTheta(Decimal s){}
01466
01468 Angle evalThetap(Decimal s, Decimal sp){}
01469
01471 Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
01472
01473
01474
01475
01477 void setParam(Clothoid2Param param){
01478 _param.setA(param.A());
01479 _param.setB(param.B());
01480 _param.setC(param.C());
01481 _param.setEps(_param.eps());
01482 _param.setA(_param.a());
01483 _param.setD(_param.d());
01484 }
01485
01487 void setRibalta(){
01488 _ribalta=true;
01489 }
01490
01492 void unsetRibalta(){
01493 _ribalta=false;
01494 }
01495
01497 void setX0(Decimal value){
01498 _X0=value;
01499 }
01500
01502 void setXc(Decimal value){
01503 _Xc=value;
01504 }
01505
01507 void setYc(Decimal value){
01508 _Yc=value;
01509 }
01510
01512 void setKc(Decimal value){
01513 _Kc=value;
01514 }
01515
01517 void setE(Decimal value){
01518 _e=value;
01519 }
01520
01522 void setAlfa (Angle value){
01523 _alfa=value;
01524 }
01525
01527 void setThetac (Angle value){
01528 _thetac=value;
01529 }
01530
01532 void setStart (Position value){
01533 _start=value;
01534 }
01535
01537 void setEnd (Position value){
01538 _end=value;
01539 }
01540
01542 void setRiferimento (Pose value){
01543 _ref=value;
01544 }
01545
01546
01547
01548
01550 Clothoid2Param param(){
01551 return _param;
01552 }
01553
01555 bool ribalta(){
01556 return _ribalta;
01557 }
01558
01560 Decimal X0(){
01561 return _X0;
01562 }
01563
01565 Decimal Xc(){
01566 return _Xc;
01567 }
01568
01570 Decimal Yc(){
01571 return _Yc;
01572 }
01573
01575 Decimal Kc(){
01576 return _Kc;
01577 }
01578
01580 Decimal e(){
01581 return _e;
01582 }
01583
01585 Angle alfa (){
01586 return _alfa;
01587 }
01588
01590 Angle thetac (){
01591 return _thetac;
01592 }
01593
01595 Position start (){
01596 return _start;
01597 }
01598
01600 Position end (){
01601 return _end;
01602 }
01603
01605 Pose riferimento (){
01606 return _ref;
01607 }
01608 };
01609
01610
01616 class Bezier : public Path{
01617 private:
01618 public:
01619
01620
01621
01622
01623
01624
01625
01626 vector<Decimal> _weightAtt;
01627 vector<Decimal> _weightRep;
01628
01629
01630
01631 vector<Decimal> _xAtt;
01632 vector<Decimal> _yAtt;
01633 vector<Decimal> _xRep;
01634 vector<Decimal> _yRep;
01635 vector<int> _binomialAtt;
01636 vector<int> _binomialRep;
01637
01638
01639 int factorial(int n){
01640 int fact=1;
01641 for (unsigned int i=1;i<=n;i++){
01642 fact*=i;
01643 }
01644 return fact;
01645 }
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663 Bezier(vector<Position> pointsAtt){
01664 unsigned int sizeAtt=pointsAtt.size()-1;
01665 for (unsigned int i=0;i<=sizeAtt;i++){
01666 _binomialAtt.push_back(factorial(sizeAtt)/(factorial(i)*factorial(sizeAtt-i)));
01667 _xAtt.push_back(pointsAtt[i].x());
01668 _yAtt.push_back(pointsAtt[i].y());
01669 _weightAtt.push_back(1.0);
01670 }
01671
01672
01673
01674
01675
01676 }
01678 Position evalCentredPos(Decimal s){
01679 unsigned int size=_xAtt.size()-1;
01680 Decimal xAtt=0.0;
01681 Decimal yAtt=0.0;
01682
01683 Decimal normAtt=0.0;
01684 Decimal normRep=0.0;
01685 Decimal x;
01686 Decimal y;
01687
01688 for (unsigned int i=0;i<=size;i++){
01689 xAtt+=_weightAtt[i]*_binomialAtt[i]*pow(1.0-s, int(size-i))*pow(s,int(i))*_xAtt[i];
01690 yAtt+=_weightAtt[i]*_binomialAtt[i]*pow(1.0-s, int(size-i))*pow(s,int(i))*_yAtt[i];
01691 normAtt+=_weightAtt[i]*_binomialAtt[i]*pow(1.0-s,int(size-i))*pow(s,int(i));
01692
01693 }
01694 xAtt=xAtt/normAtt;
01695 yAtt=yAtt/normAtt;
01696
01697
01698
01699
01700
01701
01702
01703 cout<<"normAtt\t"<<normAtt<<endl;
01704
01705
01706
01707
01708 x=xAtt;
01709 y=yAtt;
01710
01711 Position p=Position(x,y);
01712 setCentredPosp(p);
01713 return p;
01714 }
01715
01717 Position evalCentredPosp(Decimal s){};
01718
01720 Position evalCentredPospp(Decimal s){};
01721
01723 Angle evalTheta(Decimal s){}
01724
01726 Angle evalThetap(Decimal s, Decimal sp){}
01727
01729 Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
01730 };
01731
01732
01733
01734 #endif
01735
01737