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
00027
00028 #ifndef LIGHTWEIGHT_GEOM_DATA_H
00029 #define LIGHTWEIGHT_GEOM_DATA_H
00030
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/utils/stl_extensions.h>
00033
00034 #include <mrpt/math/math_frwds.h>
00035
00036
00037
00038 namespace mrpt {
00039 namespace poses {
00040 template <class DERIVEDCLASS> class CPoseOrPoint;
00041 class CPoint2D;
00042 class CPoint3D;
00043 class CPose2D;
00044 class CPose3D;
00045 }
00046 namespace utils { class CStream; }
00047 }
00048
00049 namespace mrpt {
00050 namespace math {
00051 using namespace mrpt::utils;
00052
00053 struct TPoint2D;
00054 struct TPose2D;
00055 struct TPoint3D;
00056 struct TPose3D;
00057 struct TPose3DQuat;
00058
00059
00060 #pragma pack(push,1)
00061
00062
00063
00064
00065
00066
00067 struct BASE_IMPEXP TPoint2D {
00068
00069
00070
00071 double x;
00072
00073
00074
00075 double y;
00076
00077
00078
00079
00080 explicit TPoint2D(const TPose2D &p);
00081
00082
00083
00084
00085 explicit TPoint2D(const TPoint3D &p);
00086
00087
00088
00089
00090 explicit TPoint2D(const TPose3D &p);
00091
00092
00093
00094
00095 template <class DERIVEDCLASS>
00096 explicit TPoint2D(const mrpt::poses::CPoseOrPoint<DERIVEDCLASS> &p) :x(p.x()),y(p.y()) {}
00097
00098
00099 inline TPoint2D(const mrpt::utils::TPixelCoordf &p) :x(p.x),y(p.y) {}
00100
00101
00102 TPoint2D(const mrpt::poses::CPoint2D &p);
00103
00104
00105
00106 inline TPoint2D(double xx,double yy):x(xx),y(yy) {}
00107
00108
00109
00110 inline TPoint2D() {}
00111
00112
00113
00114 inline double &operator[](size_t i) {
00115 return (&x)[i];
00116 }
00117
00118
00119
00120 inline const double &operator[](size_t i) const {
00121 return (&x)[i];
00122 }
00123
00124
00125
00126 inline void getAsVector(vector_double &v) const {
00127 v.resize(2);
00128 v[0]=x; v[1]=y;
00129 }
00130
00131 bool operator<(const TPoint2D &p) const;
00132
00133 inline TPoint2D &operator+=(const TPoint2D &p) {
00134 x+=p.x;
00135 y+=p.y;
00136 return *this;
00137 }
00138
00139 inline TPoint2D &operator-=(const TPoint2D &p) {
00140 x-=p.x;
00141 y-=p.y;
00142 return *this;
00143 }
00144
00145 inline TPoint2D &operator*=(double d) {
00146 x*=d;
00147 y*=d;
00148 return *this;
00149 }
00150
00151 inline TPoint2D &operator/=(double d) {
00152 x/=d;
00153 y/=d;
00154 return *this;
00155 }
00156
00157 inline TPoint2D operator+(const TPoint2D &p) const {
00158 TPoint2D r(*this);
00159 return r+=p;
00160 }
00161
00162 inline TPoint2D operator-(const TPoint2D &p) const {
00163 TPoint2D r(*this);
00164 return r-=p;
00165 }
00166
00167 inline TPoint2D operator*(double d) const {
00168 TPoint2D r(*this);
00169 return r*=d;
00170 }
00171
00172 inline TPoint2D operator/(double d) const {
00173 TPoint2D r(*this);
00174 return r/=d;
00175 }
00176
00177
00178
00179 void asString(std::string &s) const { s = mrpt::format("[%f %f]",x,y); }
00180 inline std::string asString() const { std::string s; asString(s); return s; }
00181
00182
00183
00184
00185
00186 void fromString(const std::string &s);
00187 static size_t size() { return 2; }
00188 };
00189
00190
00191
00192
00193
00194 struct BASE_IMPEXP TPose2D {
00195
00196
00197
00198 double x;
00199
00200
00201
00202 double y;
00203
00204
00205
00206 double phi;
00207
00208
00209
00210
00211 TPose2D(const TPoint2D &p);
00212
00213
00214
00215
00216 explicit TPose2D(const TPoint3D &p);
00217
00218
00219
00220
00221 explicit TPose2D(const TPose3D &p);
00222
00223
00224
00225
00226 TPose2D(const mrpt::poses::CPose2D &p);
00227
00228
00229
00230 inline TPose2D(double xx,double yy,double pphi):x(xx),y(yy),phi(pphi) {}
00231
00232
00233
00234 inline TPose2D() {}
00235
00236
00237
00238 inline double &operator[](size_t i) {
00239 return (&x)[i];
00240 }
00241
00242
00243
00244 inline const double &operator[](size_t i) const {
00245 return (&x)[i];
00246 }
00247
00248
00249
00250 inline void getAsVector(vector_double &v) const {
00251 v.resize(3);
00252 v[0]=x; v[1]=y; v[2]=phi;
00253 }
00254
00255
00256
00257 void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,RAD2DEG(phi)); }
00258 inline std::string asString() const { std::string s; asString(s); return s; }
00259
00260
00261
00262
00263
00264 void fromString(const std::string &s);
00265 static size_t size() { return 3; }
00266 };
00267
00268
00269
00270
00271 struct BASE_IMPEXP TPoint3Df
00272 {
00273 float x;
00274 float y;
00275 float z;
00276
00277 inline TPoint3Df() { }
00278 inline TPoint3Df(const float xx,const float yy,const float zz) : x(xx), y(yy),z(zz) { }
00279 inline TPoint3Df & operator +=(const TPoint3Df &p) { x+=p.x; y+=p.y; z+=p.z; return *this; }
00280 inline TPoint3Df operator *(const float s) { return TPoint3Df(x*s,y*s,z*s); }
00281 };
00282
00283
00284
00285
00286
00287 struct BASE_IMPEXP TPoint3D {
00288 double x;
00289 double y;
00290 double z;
00291
00292
00293 inline TPoint3D(double xx,double yy,double zz):x(xx),y(yy),z(zz) {}
00294
00295 inline TPoint3D() {}
00296
00297 explicit inline TPoint3D(const TPoint3Df &p):x(p.x),y(p.y),z(p.z) {}
00298
00299
00300
00301
00302 TPoint3D(const TPoint2D &p);
00303
00304
00305
00306
00307 explicit TPoint3D(const TPose2D &p);
00308
00309
00310
00311
00312 explicit TPoint3D(const TPose3D &p);
00313
00314
00315
00316
00317 TPoint3D(const mrpt::poses::CPoint3D &p);
00318
00319
00320
00321
00322 explicit TPoint3D(const mrpt::poses::CPose3D &p);
00323
00324
00325
00326 inline double &operator[](size_t i) {
00327 return (&x)[i];
00328 }
00329
00330
00331
00332 inline const double &operator[](size_t i) const {
00333 return (&x)[i];
00334 }
00335
00336
00337
00338 inline double distanceTo(const TPoint3D &p) const {
00339 return sqrt(square(p.x-x)+square(p.y-y)+square(p.z-z));
00340 }
00341
00342
00343
00344 inline double sqrDistanceTo(const TPoint3D &p) const {
00345 return square(p.x-x)+square(p.y-y)+square(p.z-z);
00346 }
00347
00348
00349
00350 inline double norm() const {
00351 return sqrt(square(x)+square(y)+square(z));
00352 }
00353
00354
00355
00356 inline TPoint3D &operator*=(const double f) {
00357 x*=f;y*=f;z*=f;
00358 return *this;
00359 }
00360
00361
00362
00363 void getAsVector(vector_double &v) const {
00364 v.resize(3);
00365 v[0]=x; v[1]=y; v[2]=z;
00366 }
00367
00368
00369
00370 inline TPoint3D &operator+=(const TPoint3D &p) {
00371 x+=p.x;
00372 y+=p.y;
00373 z+=p.z;
00374 return *this;
00375 }
00376
00377
00378
00379 inline TPoint3D &operator-=(const TPoint3D &p) {
00380 x-=p.x;
00381 y-=p.y;
00382 z-=p.z;
00383 return *this;
00384 }
00385
00386
00387
00388 inline TPoint3D operator+(const TPoint3D &p) const {
00389 return TPoint3D(x+p.x,y+p.y,z+p.z);
00390 }
00391
00392
00393
00394 inline TPoint3D operator-(const TPoint3D &p) const {
00395 return TPoint3D(x-p.x,y-p.y,z-p.z);
00396 }
00397
00398 inline TPoint3D operator*(double d) const {
00399 return TPoint3D(x*d,y*d,z*d);
00400 }
00401
00402 inline TPoint3D operator/(double d) const {
00403 return TPoint3D(x/d,y/d,z/d);
00404 }
00405
00406 bool operator<(const TPoint3D &p) const;
00407
00408
00409
00410
00411 void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x,y,z); }
00412 inline std::string asString() const { std::string s; asString(s); return s; }
00413
00414
00415
00416
00417
00418 void fromString(const std::string &s);
00419 static size_t size() { return 3; }
00420 };
00421
00422
00423
00424
00425
00426 struct BASE_IMPEXP TPose3D {
00427
00428
00429
00430 double x;
00431
00432
00433
00434 double y;
00435
00436
00437
00438 double z;
00439
00440
00441
00442 double yaw;
00443
00444
00445
00446 double pitch;
00447
00448
00449
00450 double roll;
00451
00452
00453
00454
00455 TPose3D(const TPoint2D &p);
00456
00457
00458
00459
00460 TPose3D(const TPose2D &p);
00461
00462
00463
00464
00465 TPose3D(const TPoint3D &p);
00466
00467
00468
00469
00470 TPose3D(const mrpt::poses::CPose3D &p);
00471
00472
00473
00474 TPose3D(double _x,double _y,double _z,double _yaw,double _pitch,double _roll):x(_x),y(_y),z(_z),yaw(_yaw),pitch(_pitch),roll(_roll) {}
00475
00476
00477
00478 inline TPose3D() {}
00479
00480
00481
00482 inline double &operator[](size_t i) {
00483 return (&x)[i];
00484 }
00485
00486
00487
00488 inline const double &operator[](size_t i) const {
00489 return (&x)[i];
00490 }
00491
00492
00493
00494 double norm() const {
00495 return sqrt(square(x)+square(y)+square(z));
00496 }
00497
00498
00499
00500 void getAsVector(vector_double &v) const {
00501 v.resize(6);
00502 v[0]=x; v[1]=y; v[2]=z; v[3]=yaw; v[4]=pitch; v[5]=roll;
00503 }
00504
00505
00506
00507 void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f]",x,y,z,RAD2DEG(yaw),RAD2DEG(pitch),RAD2DEG(roll)); }
00508 inline std::string asString() const { std::string s; asString(s); return s; }
00509
00510
00511
00512
00513
00514 void fromString(const std::string &s);
00515 static size_t size() { return 6; }
00516 };
00517
00518
00519
00520
00521 struct BASE_IMPEXP TPose3DQuat {
00522 double x;
00523 double y;
00524 double z;
00525 double qr;
00526 double qx;
00527 double qy;
00528 double qz;
00529
00530
00531 inline TPose3DQuat(double _x,double _y,double _z,double _qr,double _qx, double _qy, double _qz):x(_x),y(_y),z(_z),qr(_qr),qx(_qx),qy(_qy),qz(_qz) { }
00532
00533 inline TPose3DQuat() {}
00534
00535 TPose3DQuat(const mrpt::poses::CPose3DQuat &p);
00536
00537
00538 inline double &operator[](size_t i) {
00539 return (&x)[i];
00540 }
00541
00542 inline const double &operator[](size_t i) const {
00543 return (&x)[i];
00544 }
00545
00546 double norm() const {
00547 return sqrt(square(x)+square(y)+square(z));
00548 }
00549
00550 void getAsVector(vector_double &v) const {
00551 v.resize(7);
00552 for (size_t i=0;i<7;i++) v[i]=(*this)[i];
00553 }
00554
00555
00556
00557 void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",x,y,z,qr,qx,qy,qz); }
00558 inline std::string asString() const { std::string s; asString(s); return s; }
00559
00560
00561
00562
00563
00564 void fromString(const std::string &s);
00565 static size_t size() { return 7; }
00566 };
00567 #pragma pack(pop)
00568
00569
00570 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint2D & p);
00571 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPoint3D & p);
00572 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose2D & p);
00573 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3D & p);
00574 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const TPose3DQuat & p);
00575
00576
00577
00578
00579
00580 inline TPoint3D operator-(const TPoint3D &p1) {
00581 return TPoint3D(-p1.x,-p1.y,-p1.z);
00582 }
00583
00584
00585
00586 inline bool operator==(const TPoint2D &p1,const TPoint2D &p2) {
00587 return (p1.x==p2.x)&&(p1.y==p2.y);
00588 }
00589
00590
00591
00592 inline bool operator!=(const TPoint2D &p1,const TPoint2D &p2) {
00593 return (p1.x!=p2.x)||(p1.y!=p2.y);
00594 }
00595
00596
00597
00598 inline bool operator==(const TPoint3D &p1,const TPoint3D &p2) {
00599 return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z);
00600 }
00601
00602
00603
00604 inline bool operator!=(const TPoint3D &p1,const TPoint3D &p2) {
00605 return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z);
00606 }
00607
00608
00609
00610 inline bool operator==(const TPose2D &p1,const TPose2D &p2) {
00611 return (p1.x==p2.x)&&(p1.y==p2.y)&&(mrpt::math::wrapTo2Pi(p1.phi)==mrpt::math::wrapTo2Pi(p2.phi));
00612 }
00613
00614
00615
00616 inline bool operator!=(const TPose2D &p1,const TPose2D &p2) {
00617 return (p1.x!=p2.x)||(p1.y!=p2.y)||(mrpt::math::wrapTo2Pi(p1.phi)!=mrpt::math::wrapTo2Pi(p2.phi));
00618 }
00619
00620
00621
00622 inline bool operator==(const TPose3D &p1,const TPose3D &p2) {
00623 return (p1.x==p2.x)&&(p1.y==p2.y)&&(p1.z==p2.z)&&(mrpt::math::wrapTo2Pi(p1.yaw)==mrpt::math::wrapTo2Pi(p2.yaw))&&(mrpt::math::wrapTo2Pi(p1.pitch)==mrpt::math::wrapTo2Pi(p2.pitch))&&(mrpt::math::wrapTo2Pi(p1.roll)==mrpt::math::wrapTo2Pi(p2.roll));
00624 }
00625
00626
00627
00628 inline bool operator!=(const TPose3D &p1,const TPose3D &p2) {
00629 return (p1.x!=p2.x)||(p1.y!=p2.y)||(p1.z!=p2.z)||(mrpt::math::wrapTo2Pi(p1.yaw)!=mrpt::math::wrapTo2Pi(p2.yaw))||(mrpt::math::wrapTo2Pi(p1.pitch)!=mrpt::math::wrapTo2Pi(p2.pitch))||(mrpt::math::wrapTo2Pi(p1.roll)!=mrpt::math::wrapTo2Pi(p2.roll));
00630 }
00631
00632 struct BASE_IMPEXP TSegment3D;
00633 struct BASE_IMPEXP TLine3D;
00634 class BASE_IMPEXP TPolygon3D;
00635 struct BASE_IMPEXP TObject3D;
00636
00637
00638 #pragma pack(push,1)
00639
00640
00641
00642
00643 struct BASE_IMPEXP TSegment2D {
00644 public:
00645
00646
00647
00648 TPoint2D point1;
00649
00650
00651
00652 TPoint2D point2;
00653
00654
00655
00656 double length() const;
00657
00658
00659
00660 double distance(const TPoint2D &point) const;
00661
00662
00663
00664 double signedDistance(const TPoint2D &point) const;
00665
00666
00667
00668 bool contains(const TPoint2D &point) const;
00669
00670
00671
00672 inline TPoint2D &operator[](size_t i) {
00673 return (&point1)[i];
00674 }
00675
00676
00677
00678 inline const TPoint2D &operator[](size_t i) const {
00679 return (&point1)[i];
00680 }
00681
00682
00683
00684 void generate3DObject(TSegment3D &s) const;
00685
00686
00687
00688 inline void getCenter(TPoint2D &p) const {
00689 p.x=(point1.x+point2.x)/2;
00690 p.y=(point1.y+point2.y)/2;
00691 }
00692
00693
00694
00695 TSegment2D(const TPoint2D &p1,const TPoint2D &p2):point1(p1),point2(p2) {}
00696
00697
00698
00699 TSegment2D() {}
00700
00701
00702
00703 explicit TSegment2D(const TSegment3D &s);
00704
00705 bool operator<(const TSegment2D &s) const;
00706 };
00707
00708
00709
00710
00711 struct BASE_IMPEXP TSegment3D {
00712 public:
00713
00714
00715
00716 TPoint3D point1;
00717
00718
00719
00720 TPoint3D point2;
00721
00722
00723
00724 double length() const;
00725
00726
00727
00728 double distance(const TPoint3D &point) const;
00729
00730
00731
00732 bool contains(const TPoint3D &point) const;
00733
00734
00735
00736 inline TPoint3D &operator[](size_t i) {
00737 return (&point1)[i];
00738 }
00739
00740
00741
00742 inline const TPoint3D &operator[](size_t i) const {
00743 return (&point1)[i];
00744 }
00745
00746
00747
00748 inline void generate2DObject(TSegment2D &s) const {
00749 s=TSegment2D(*this);
00750 }
00751
00752
00753
00754 inline void getCenter(TPoint3D &p) const {
00755 p.x=(point1.x+point2.x)/2;
00756 p.y=(point1.y+point2.y)/2;
00757 p.z=(point1.z+point2.z)/2;
00758 }
00759
00760
00761
00762 TSegment3D(const TPoint3D &p1,const TPoint3D &p2):point1(p1),point2(p2) {}
00763
00764
00765
00766 TSegment3D() {}
00767
00768
00769
00770 TSegment3D(const TSegment2D &s):point1(s.point1),point2(s.point2) {}
00771
00772 bool operator<(const TSegment3D &s) const;
00773 };
00774 #pragma pack(pop)
00775
00776 inline bool operator==(const TSegment2D &s1,const TSegment2D &s2) {
00777 return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
00778 }
00779
00780 inline bool operator!=(const TSegment2D &s1,const TSegment2D &s2) {
00781 return (s1.point1!=s1.point1)||(s1.point2!=s2.point2);
00782 }
00783
00784 inline bool operator==(const TSegment3D &s1,const TSegment3D &s2) {
00785 return (s1.point1==s2.point1)&&(s1.point2==s2.point2);
00786 }
00787
00788 inline bool operator!=(const TSegment3D &s1,const TSegment3D &s2) {
00789 return (s1.point1!=s1.point1)||(s1.point2!=s2.point2);
00790 }
00791
00792
00793
00794
00795
00796 struct BASE_IMPEXP TLine2D {
00797 public:
00798
00799
00800
00801 double coefs[3];
00802
00803
00804
00805 double evaluatePoint(const TPoint2D &point) const;
00806
00807
00808
00809 bool contains(const TPoint2D &point) const;
00810
00811
00812
00813 double distance(const TPoint2D &point) const;
00814
00815
00816
00817 double signedDistance(const TPoint2D &point) const;
00818
00819
00820
00821 void getNormalVector(double (&vector)[2]) const;
00822
00823
00824
00825 void unitarize();
00826
00827
00828
00829 inline void getUnitaryNormalVector(double (&vector)[2]) {
00830 unitarize();
00831 getNormalVector(vector);
00832 }
00833
00834
00835
00836 void getDirectorVector(double (&vector)[2]) const;
00837
00838
00839
00840 inline void getUnitaryDirectorVector(double (&vector)[2]) {
00841 unitarize();
00842 getDirectorVector(vector);
00843 }
00844
00845
00846
00847 void generate3DObject(TLine3D &l) const;
00848
00849
00850
00851
00852 void getAsPose2D(mrpt::poses::CPose2D &outPose) const;
00853
00854
00855
00856
00857
00858 void getAsPose2DForcingOrigin(const TPoint2D &origin,mrpt::poses::CPose2D &outPose) const;
00859
00860
00861
00862
00863 TLine2D(const TPoint2D &p1,const TPoint2D &p2) throw(std::logic_error);
00864
00865
00866
00867 explicit TLine2D(const TSegment2D &s);
00868
00869
00870
00871 TLine2D() {}
00872
00873
00874
00875 inline TLine2D(double A,double B,double C) {
00876 coefs[0]=A;
00877 coefs[1]=B;
00878 coefs[2]=C;
00879 }
00880
00881
00882
00883
00884 explicit TLine2D(const TLine3D &l);
00885 };
00886
00887
00888
00889
00890
00891 struct BASE_IMPEXP TLine3D {
00892 public:
00893
00894
00895
00896 TPoint3D pBase;
00897
00898
00899
00900 double director[3];
00901
00902
00903
00904 bool contains(const TPoint3D &point) const;
00905
00906
00907
00908 double distance(const TPoint3D &point) const;
00909
00910
00911
00912 void unitarize();
00913
00914
00915
00916 inline void getDirectorVector(double (&vector)[3]) const {
00917 for (size_t i=0;i<3;i++) vector[i]=director[i];
00918 }
00919
00920
00921
00922 inline void getUnitaryDirectorVector(double (&vector)[3]) {
00923 unitarize();
00924 getDirectorVector(vector);
00925 }
00926
00927
00928
00929
00930 inline void generate2DObject(TLine2D &l) const {
00931 l=TLine2D(*this);
00932 }
00933
00934
00935
00936
00937 TLine3D(const TPoint3D &p1,const TPoint3D &p2) throw(std::logic_error);
00938
00939
00940
00941 explicit TLine3D(const TSegment3D &s);
00942
00943
00944
00945 TLine3D() {}
00946
00947
00948
00949 TLine3D(const TLine2D &l);
00950 };
00951
00952
00953
00954
00955
00956 struct BASE_IMPEXP TPlane {
00957 public:
00958
00959
00960
00961 double coefs[4];
00962
00963
00964
00965 double evaluatePoint(const TPoint3D &point) const;
00966
00967
00968
00969 bool contains(const TPoint3D &point) const;
00970
00971
00972
00973 inline bool contains(const TSegment3D &segment) const {
00974 return contains(segment.point1)&&contains(segment.point2);
00975 }
00976
00977
00978
00979 bool contains(const TLine3D &line) const;
00980
00981
00982
00983 double distance(const TPoint3D &point) const;
00984
00985
00986
00987 double distance(const TLine3D &line) const;
00988
00989
00990
00991 void getNormalVector(double (&vec)[3]) const;
00992
00993
00994
00995 void unitarize();
00996
00997
00998
00999 inline void getUnitaryNormalVector(double (&vec)[3]) {
01000 unitarize();
01001 getNormalVector(vec);
01002 }
01003
01004
01005
01006 void getAsPose3D(mrpt::poses::CPose3D &outPose);
01007
01008
01009
01010 inline void getAsPose3D(mrpt::poses::CPose3D &outPose) const {
01011 TPlane p=*this;
01012 p.getAsPose3D(outPose);
01013 }
01014
01015
01016
01017
01018 void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose);
01019
01020
01021
01022
01023 inline void getAsPose3DForcingOrigin(const TPoint3D &newOrigin,mrpt::poses::CPose3D &pose) const {
01024 TPlane p=*this;
01025 p.getAsPose3DForcingOrigin(newOrigin,pose);
01026 }
01027
01028
01029
01030
01031 TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) throw(std::logic_error);
01032
01033
01034
01035
01036 TPlane(const TPoint3D &p1,const TLine3D &r2) throw(std::logic_error);
01037
01038
01039
01040
01041 TPlane(const TLine3D &r1,const TLine3D &r2) throw(std::logic_error);
01042
01043
01044
01045 TPlane() {}
01046
01047
01048
01049 inline TPlane(double A,double B,double C,double D) {
01050 coefs[0]=A;
01051 coefs[1]=B;
01052 coefs[2]=C;
01053 coefs[3]=D;
01054 }
01055
01056
01057
01058 inline TPlane(const double (&vec)[4]) {
01059 for (size_t i=0;i<4;i++) coefs[i]=vec[i];
01060 }
01061 };
01062
01063 typedef TPlane TPlane3D;
01064
01065
01066
01067
01068
01069 class BASE_IMPEXP TPolygon2D:public std::vector<TPoint2D> {
01070 public:
01071
01072
01073
01074 double distance(const TPoint2D &point) const;
01075
01076
01077
01078 bool contains(const TPoint2D &point) const;
01079
01080
01081
01082 void getAsSegmentList(std::vector<TSegment2D> &v) const;
01083
01084
01085
01086 void generate3DObject(TPolygon3D &p) const;
01087
01088
01089
01090 void getCenter(TPoint2D &p) const;
01091
01092
01093
01094 bool isConvex() const;
01095
01096
01097
01098
01099 void removeRepeatedVertices();
01100
01101
01102
01103
01104 void removeRedundantVertices();
01105
01106
01107
01108
01109 void getPlotData(std::vector<double> &x,std::vector<double> &y) const;
01110
01111
01112
01113 TPolygon2D():std::vector<TPoint2D>() {}
01114
01115
01116
01117 explicit TPolygon2D(size_t N):std::vector<TPoint2D>(N) {}
01118
01119
01120
01121 TPolygon2D(const std::vector<TPoint2D> &v):std::vector<TPoint2D>(v) {}
01122
01123
01124
01125 explicit TPolygon2D(const TPolygon3D &p);
01126
01127
01128
01129
01130 static void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly);
01131
01132
01133
01134
01135 static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly,const mrpt::poses::CPose2D &pose);
01136 };
01137
01138
01139
01140
01141
01142 class BASE_IMPEXP TPolygon3D:public std::vector<TPoint3D> {
01143 public:
01144
01145
01146
01147 double distance(const TPoint3D &point) const;
01148
01149
01150
01151 bool contains(const TPoint3D &point) const;
01152
01153
01154
01155 void getAsSegmentList(std::vector<TSegment3D> &v) const;
01156
01157
01158
01159 bool getPlane(TPlane &p) const;
01160
01161
01162
01163
01164 void getBestFittingPlane(TPlane &p) const;
01165
01166
01167
01168
01169 inline void generate2DObject(TPolygon2D &p) const {
01170 p=TPolygon2D(*this);
01171 }
01172
01173
01174
01175 void getCenter(TPoint3D &p) const;
01176
01177
01178
01179
01180 bool isSkew() const;
01181
01182
01183
01184 void removeRepeatedVertices();
01185
01186
01187
01188 void removeRedundantVertices();
01189
01190
01191
01192 TPolygon3D():std::vector<TPoint3D>() {}
01193
01194
01195
01196 explicit TPolygon3D(size_t N):std::vector<TPoint3D>(N) {}
01197
01198
01199
01200 TPolygon3D(const std::vector<TPoint3D> &v):std::vector<TPoint3D>(v) {}
01201
01202
01203
01204 TPolygon3D(const TPolygon2D &p);
01205
01206
01207
01208
01209 static void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly);
01210
01211
01212
01213
01214 static inline void createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly,const mrpt::poses::CPose3D &pose);
01215 };
01216
01217
01218
01219
01220
01221 const unsigned char GEOMETRIC_TYPE_POINT=0;
01222
01223
01224
01225
01226 const unsigned char GEOMETRIC_TYPE_SEGMENT=1;
01227
01228
01229
01230
01231 const unsigned char GEOMETRIC_TYPE_LINE=2;
01232
01233
01234
01235
01236 const unsigned char GEOMETRIC_TYPE_POLYGON=3;
01237
01238
01239
01240
01241 const unsigned char GEOMETRIC_TYPE_PLANE=4;
01242
01243
01244
01245
01246 const unsigned char GEOMETRIC_TYPE_UNDEFINED=255;
01247
01248
01249
01250
01251
01252 #ifdef TOBJECTS_USE_UNIONS
01253 struct BASE_IMPEXP TObject2D {
01254 private:
01255
01256
01257
01258 unsigned char type;
01259
01260
01261
01262 union {
01263 TPoint2D *point;
01264 TSegment2D *segment;
01265 TLine2D *line;
01266 TPolygon2D *polygon;
01267 } data;
01268
01269
01270
01271 void destroy() {
01272 switch(type) {
01273 case GEOMETRIC_TYPE_POINT:
01274 delete data.point;
01275 break;
01276 case GEOMETRIC_TYPE_SEGMENT:
01277 delete data.segment;
01278 break;
01279 case GEOMETRIC_TYPE_LINE:
01280 delete data.line;
01281 break;
01282 case GEOMETRIC_TYPE_POLYGON:
01283 delete data.polygon;
01284 break;
01285 }
01286 type=GEOMETRIC_TYPE_UNDEFINED;
01287 }
01288 public:
01289
01290
01291
01292 TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT) {
01293 data.point=new TPoint2D(p);
01294 }
01295
01296
01297
01298 TObject2D(const TSegment2D &s):type(GEOMETRIC_TYPE_SEGMENT) {
01299 data.segment=new TSegment2D(s);
01300 }
01301
01302
01303
01304 TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE) {
01305 data.line=new TLine2D(r);
01306 }
01307
01308
01309
01310 TObject2D(const TPolygon2D &p):type(GEOMETRIC_TYPE_POLYGON) {
01311 data.polygon=new TPolygon2D(p);
01312 }
01313
01314
01315
01316 TObject2D():type(GEOMETRIC_TYPE_UNDEFINED) {}
01317
01318
01319
01320 ~TObject2D() {
01321 destroy();
01322 }
01323
01324
01325
01326 inline bool isPoint() const {
01327 return type==GEOMETRIC_TYPE_POINT;
01328 }
01329
01330
01331
01332 inline bool isSegment() const {
01333 return type==GEOMETRIC_TYPE_SEGMENT;
01334 }
01335
01336
01337
01338 inline bool isLine() const {
01339 return type==GEOMETRIC_TYPE_LINE;
01340 }
01341
01342
01343
01344 inline bool isPolygon() const {
01345 return type==GEOMETRIC_TYPE_POLYGON;
01346 }
01347
01348
01349
01350 inline unsigned char getType() const {
01351 return type;
01352 }
01353
01354
01355
01356 inline bool getPoint(TPoint2D &p) const {
01357 if (isPoint()) {
01358 p=*(data.point);
01359 return true;
01360 } else return false;
01361 }
01362
01363
01364
01365 inline bool getSegment(TSegment2D &s) const {
01366 if (isSegment()) {
01367 s=*(data.segment);
01368 return true;
01369 } else return false;
01370 }
01371
01372
01373
01374 inline bool getLine(TLine2D &r) const {
01375 if (isLine()) {
01376 r=*(data.line);
01377 return true;
01378 } else return false;
01379 }
01380
01381
01382
01383 inline bool getPolygon(TPolygon2D &p) const {
01384 if (isPolygon()) {
01385 p=*(data.polygon);
01386 return true;
01387 } else return false;
01388 }
01389
01390
01391
01392 void operator=(const TObject2D &obj) {
01393 if (this==&obj) return;
01394 destroy();
01395 switch (type=obj.type) {
01396 case GEOMETRIC_TYPE_POINT:
01397 data.point=new TPoint2D(*(obj.data.point));
01398 break;
01399 case GEOMETRIC_TYPE_SEGMENT:
01400 data.segment=new TSegment2D(*(obj.data.segment));
01401 break;
01402 case GEOMETRIC_TYPE_LINE:
01403 data.line=new TLine2D(*(obj.data.line));
01404 break;
01405 case GEOMETRIC_TYPE_POLYGON:
01406 data.polygon=new TPolygon2D(*(obj.data.polygon));
01407 break;
01408 }
01409 }
01410
01411
01412
01413 inline void operator=(const TPoint2D &p) {
01414 destroy();
01415 type=GEOMETRIC_TYPE_POINT;
01416 data.point=new TPoint2D(p);
01417 }
01418
01419
01420
01421 inline void operator=(const TSegment2D &s) {
01422 destroy();
01423 type=GEOMETRIC_TYPE_SEGMENT;
01424 data.segment=new TSegment2D(s);
01425 }
01426
01427
01428
01429 inline void operator=(const TLine2D &l) {
01430 destroy();
01431 type=GEOMETRIC_TYPE_LINE;
01432 data.line=new TLine2D(l);
01433 }
01434
01435
01436
01437 inline void operator=(const TPolygon2D &p) {
01438 destroy();
01439 type=GEOMETRIC_TYPE_POLYGON;
01440 data.polygon=new TPolygon2D(p);
01441 }
01442
01443
01444
01445 void generate3DObject(TObject3D &obj) const;
01446
01447
01448
01449 TObject2D(const TObject2D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
01450 operator=(obj);
01451 }
01452
01453
01454
01455 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
01456
01457
01458
01459 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
01460
01461
01462
01463 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
01464
01465
01466
01467 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
01468
01469
01470
01471 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
01472
01473
01474
01475 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
01476
01477
01478
01479 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
01480
01481
01482
01483 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
01484 };
01485
01486
01487
01488
01489 struct BASE_IMPEXP TObject3D {
01490 private:
01491
01492
01493
01494 unsigned char type;
01495
01496
01497
01498 union {
01499 TPoint3D *point;
01500 TSegment3D *segment;
01501 TLine3D *line;
01502 TPolygon3D *polygon;
01503 TPlane *plane;
01504 } data;
01505
01506
01507
01508 void destroy() {
01509 switch (type) {
01510 case GEOMETRIC_TYPE_POINT:
01511 delete data.point;
01512 break;
01513 case GEOMETRIC_TYPE_SEGMENT:
01514 delete data.segment;
01515 break;
01516 case GEOMETRIC_TYPE_LINE:
01517 delete data.line;
01518 break;
01519 case GEOMETRIC_TYPE_POLYGON:
01520 delete data.polygon;
01521 break;
01522 case GEOMETRIC_TYPE_PLANE:
01523 delete data.plane;
01524 break;
01525 case GEOMETRIC_TYPE_UNDEFINED:
01526 break;
01527 default:
01528 THROW_EXCEPTION("Invalid TObject2D object");
01529 }
01530 type=GEOMETRIC_TYPE_UNDEFINED;
01531 }
01532 public:
01533
01534
01535
01536 TObject3D(const TPoint3D &p):type(GEOMETRIC_TYPE_POINT) {
01537 data.point=new TPoint3D(p);
01538 }
01539
01540
01541
01542 TObject3D(const TSegment3D &s):type(GEOMETRIC_TYPE_SEGMENT) {
01543 data.segment=new TSegment3D(s);
01544 }
01545
01546
01547
01548 TObject3D(const TLine3D &r):type(GEOMETRIC_TYPE_LINE) {
01549 data.line=new TLine3D(r);
01550 }
01551
01552
01553
01554 TObject3D(const TPolygon3D &p):type(GEOMETRIC_TYPE_POLYGON) {
01555 data.polygon=new TPolygon3D(p);
01556 }
01557
01558
01559
01560 TObject3D(const TPlane &p):type(GEOMETRIC_TYPE_PLANE) {
01561 data.plane=new TPlane(p);
01562 }
01563
01564
01565
01566 TObject3D():type(GEOMETRIC_TYPE_UNDEFINED) {}
01567
01568
01569
01570 ~TObject3D() {
01571 destroy();
01572 }
01573
01574
01575
01576 inline bool isPoint() const {
01577 return type==GEOMETRIC_TYPE_POINT;
01578 }
01579
01580
01581
01582 inline bool isSegment() const {
01583 return type==GEOMETRIC_TYPE_SEGMENT;
01584 }
01585
01586
01587
01588 inline bool isLine() const {
01589 return type==GEOMETRIC_TYPE_LINE;
01590 }
01591
01592
01593
01594 inline bool isPolygon() const {
01595 return type==GEOMETRIC_TYPE_POLYGON;
01596 }
01597
01598
01599
01600 inline bool isPlane() const {
01601 return type==GEOMETRIC_TYPE_PLANE;
01602 }
01603
01604
01605
01606 inline unsigned char getType() const {
01607 return type;
01608 }
01609
01610
01611
01612 inline bool getPoint(TPoint3D &p) const {
01613 if (isPoint()) {
01614 p=*(data.point);
01615 return true;
01616 } else return false;
01617 }
01618
01619
01620
01621 inline bool getSegment(TSegment3D &s) const {
01622 if (isSegment()) {
01623 s=*(data.segment);
01624 return true;
01625 } else return false;
01626 }
01627
01628
01629
01630 inline bool getLine(TLine3D &r) const {
01631 if (isLine()) {
01632 r=*(data.line);
01633 return true;
01634 } else return false;
01635 }
01636
01637
01638
01639 inline bool getPolygon(TPolygon3D &p) const {
01640 if (isPolygon()) {
01641 p=*(data.polygon);
01642 return true;
01643 } else return false;
01644 }
01645
01646
01647
01648 inline bool getPlane(TPlane &p) const {
01649 if (isPlane()) {
01650 p=*(data.plane);
01651 return true;
01652 } else return false;
01653 }
01654
01655
01656
01657 void operator=(const TObject3D &obj) {
01658 if (this==&obj) return;
01659 destroy();
01660 switch (type=obj.type) {
01661 case GEOMETRIC_TYPE_POINT:
01662 data.point=new TPoint3D(*(obj.data.point));
01663 break;
01664 case GEOMETRIC_TYPE_SEGMENT:
01665 data.segment=new TSegment3D(*(obj.data.segment));
01666 break;
01667 case GEOMETRIC_TYPE_LINE:
01668 data.line=new TLine3D(*(obj.data.line));
01669 break;
01670 case GEOMETRIC_TYPE_POLYGON:
01671 data.polygon=new TPolygon3D(*(obj.data.polygon));
01672 break;
01673 case GEOMETRIC_TYPE_PLANE:
01674 data.plane=new TPlane(*(obj.data.plane));
01675 break;
01676 case GEOMETRIC_TYPE_UNDEFINED:
01677 break;
01678 default:
01679 THROW_EXCEPTION("Invalid TObject3D object");
01680 }
01681 }
01682
01683
01684
01685 inline void operator=(const TPoint3D &p) {
01686 destroy();
01687 type=GEOMETRIC_TYPE_POINT;
01688 data.point=new TPoint3D(p);
01689 }
01690
01691
01692
01693 inline void operator=(const TSegment3D &s) {
01694 destroy();
01695 type=GEOMETRIC_TYPE_SEGMENT;
01696 data.segment=new TSegment3D(s);
01697 }
01698
01699
01700
01701 inline void operator=(const TLine3D &l) {
01702 destroy();
01703 type=GEOMETRIC_TYPE_LINE;
01704 data.line=new TLine3D(l);
01705 }
01706
01707
01708
01709 inline void operator=(const TPolygon3D &p) {
01710 destroy();
01711 type=GEOMETRIC_TYPE_POLYGON;
01712 data.polygon=new TPolygon3D(p);
01713 }
01714
01715
01716
01717 inline void operator=(const TPlane &p) {
01718 destroy();
01719 type=GEOMETRIC_TYPE_PLANE;
01720 data.plane=new TPlane(p);
01721 }
01722
01723
01724
01725
01726 inline void generate2DObject(TObject2D &obj) const {
01727 switch (type) {
01728 case GEOMETRIC_TYPE_POINT:
01729 obj=TPoint2D(*(data.point));
01730 break;
01731 case GEOMETRIC_TYPE_SEGMENT:
01732 obj=TSegment2D(*(data.segment));
01733 break;
01734 case GEOMETRIC_TYPE_LINE:
01735 obj=TLine2D(*(data.line));
01736 break;
01737 case GEOMETRIC_TYPE_POLYGON:
01738 obj=TPolygon2D(*(data.polygon));
01739 break;
01740 case GEOMETRIC_TYPE_PLANE:
01741 throw std::logic_error("Too many dimensions");
01742 default:
01743 obj=TObject2D();
01744 break;
01745 }
01746 }
01747
01748
01749
01750 TObject3D(const TObject3D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
01751 operator=(obj);
01752 }
01753
01754
01755
01756 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
01757
01758
01759
01760 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
01761
01762
01763
01764 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
01765
01766
01767
01768 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
01769
01770
01771
01772 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
01773
01774
01775
01776 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
01777
01778
01779
01780 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
01781
01782
01783
01784 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
01785
01786
01787
01788 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
01789
01790
01791
01792 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
01793 };
01794 #else
01795 struct BASE_IMPEXP TObject2D {
01796 private:
01797
01798
01799
01800 unsigned char type;
01801
01802
01803
01804 struct {
01805 TPoint2D point;
01806 TSegment2D segment;
01807 TLine2D line;
01808 TPolygon2D *polygon;
01809 } data;
01810
01811
01812
01813 inline void destroy() {
01814 if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
01815 type=GEOMETRIC_TYPE_UNDEFINED;
01816 }
01817 public:
01818
01819
01820
01821 inline TObject2D(const TPoint2D &p):type(GEOMETRIC_TYPE_POINT) {
01822 data.point=p;
01823 }
01824
01825
01826
01827 inline TObject2D(const TSegment2D &s):type(GEOMETRIC_TYPE_SEGMENT) {
01828 data.segment=s;
01829 }
01830
01831
01832
01833 inline TObject2D(const TLine2D &r):type(GEOMETRIC_TYPE_LINE) {
01834 data.line=r;
01835 }
01836
01837
01838
01839 inline TObject2D(const TPolygon2D &p):type(GEOMETRIC_TYPE_POLYGON) {
01840 data.polygon=new TPolygon2D(p);
01841 }
01842
01843
01844
01845 TObject2D():type(GEOMETRIC_TYPE_UNDEFINED) {}
01846
01847
01848
01849 ~TObject2D() {
01850 destroy();
01851 }
01852
01853
01854
01855 inline bool isPoint() const {
01856 return type==GEOMETRIC_TYPE_POINT;
01857 }
01858
01859
01860
01861 inline bool isSegment() const {
01862 return type==GEOMETRIC_TYPE_SEGMENT;
01863 }
01864
01865
01866
01867 inline bool isLine() const {
01868 return type==GEOMETRIC_TYPE_LINE;
01869 }
01870
01871
01872
01873 inline bool isPolygon() const {
01874 return type==GEOMETRIC_TYPE_POLYGON;
01875 }
01876
01877
01878
01879 inline unsigned char getType() const {
01880 return type;
01881 }
01882
01883
01884
01885 inline bool getPoint(TPoint2D &p) const {
01886 if (isPoint()) {
01887 p=data.point;
01888 return true;
01889 } else return false;
01890 }
01891
01892
01893
01894 inline bool getSegment(TSegment2D &s) const {
01895 if (isSegment()) {
01896 s=data.segment;
01897 return true;
01898 } else return false;
01899 }
01900
01901
01902
01903 inline bool getLine(TLine2D &r) const {
01904 if (isLine()) {
01905 r=data.line;
01906 return true;
01907 } else return false;
01908 }
01909
01910
01911
01912 inline bool getPolygon(TPolygon2D &p) const {
01913 if (isPolygon()) {
01914 p=*(data.polygon);
01915 return true;
01916 } else return false;
01917 }
01918
01919
01920
01921 void operator=(const TObject2D &obj) {
01922 if (this==&obj) return;
01923 destroy();
01924 switch (type=obj.type) {
01925 case GEOMETRIC_TYPE_POINT:
01926 data.point=obj.data.point;
01927 break;
01928 case GEOMETRIC_TYPE_SEGMENT:
01929 data.segment=obj.data.segment;
01930 break;
01931 case GEOMETRIC_TYPE_LINE:
01932 data.line=obj.data.line;
01933 break;
01934 case GEOMETRIC_TYPE_POLYGON:
01935 data.polygon=new TPolygon2D(*(obj.data.polygon));
01936 break;
01937 }
01938 }
01939
01940
01941
01942 inline void operator=(const TPoint2D &p) {
01943 destroy();
01944 type=GEOMETRIC_TYPE_POINT;
01945 data.point=p;
01946 }
01947
01948
01949
01950 inline void operator=(const TSegment2D &s) {
01951 destroy();
01952 type=GEOMETRIC_TYPE_SEGMENT;
01953 data.segment=s;
01954 }
01955
01956
01957
01958 inline void operator=(const TLine2D &l) {
01959 destroy();
01960 type=GEOMETRIC_TYPE_LINE;
01961 data.line=l;
01962 }
01963
01964
01965
01966 inline void operator=(const TPolygon2D &p) {
01967 destroy();
01968 type=GEOMETRIC_TYPE_POLYGON;
01969 data.polygon=new TPolygon2D(p);
01970 }
01971
01972
01973
01974 void generate3DObject(TObject3D &obj) const;
01975
01976
01977
01978 TObject2D(const TObject2D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
01979 operator=(obj);
01980 }
01981
01982
01983
01984 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts);
01985
01986
01987
01988 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms);
01989
01990
01991
01992 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins);
01993
01994
01995
01996 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys);
01997
01998
01999
02000 static void getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder);
02001
02002
02003
02004 static void getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder);
02005
02006
02007
02008 static void getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder);
02009
02010
02011
02012 static void getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,std::vector<TObject2D> &remainder);
02013 };
02014
02015
02016
02017
02018 struct BASE_IMPEXP TObject3D {
02019 private:
02020
02021
02022
02023 unsigned char type;
02024
02025
02026
02027 struct {
02028 TPoint3D point;
02029 TSegment3D segment;
02030 TLine3D line;
02031 TPolygon3D *polygon;
02032 TPlane plane;
02033 } data;
02034
02035
02036
02037 void destroy() {
02038 if (type==GEOMETRIC_TYPE_POLYGON) delete data.polygon;
02039 type=GEOMETRIC_TYPE_UNDEFINED;
02040 }
02041 public:
02042
02043
02044
02045 TObject3D(const TPoint3D &p):type(GEOMETRIC_TYPE_POINT) {
02046 data.point=p;
02047 }
02048
02049
02050
02051 TObject3D(const TSegment3D &s):type(GEOMETRIC_TYPE_SEGMENT) {
02052 data.segment=s;
02053 }
02054
02055
02056
02057 TObject3D(const TLine3D &r):type(GEOMETRIC_TYPE_LINE) {
02058 data.line=r;
02059 }
02060
02061
02062
02063 TObject3D(const TPolygon3D &p):type(GEOMETRIC_TYPE_POLYGON) {
02064 data.polygon=new TPolygon3D(p);
02065 }
02066
02067
02068
02069 TObject3D(const TPlane &p):type(GEOMETRIC_TYPE_PLANE) {
02070 data.plane=p;
02071 }
02072
02073
02074
02075 TObject3D():type(GEOMETRIC_TYPE_UNDEFINED) {}
02076
02077
02078
02079 ~TObject3D() {
02080 destroy();
02081 }
02082
02083
02084
02085 inline bool isPoint() const {
02086 return type==GEOMETRIC_TYPE_POINT;
02087 }
02088
02089
02090
02091 inline bool isSegment() const {
02092 return type==GEOMETRIC_TYPE_SEGMENT;
02093 }
02094
02095
02096
02097 inline bool isLine() const {
02098 return type==GEOMETRIC_TYPE_LINE;
02099 }
02100
02101
02102
02103 inline bool isPolygon() const {
02104 return type==GEOMETRIC_TYPE_POLYGON;
02105 }
02106
02107
02108
02109 inline bool isPlane() const {
02110 return type==GEOMETRIC_TYPE_PLANE;
02111 }
02112
02113
02114
02115 inline unsigned char getType() const {
02116 return type;
02117 }
02118
02119
02120
02121 inline bool getPoint(TPoint3D &p) const {
02122 if (isPoint()) {
02123 p=data.point;
02124 return true;
02125 } else return false;
02126 }
02127
02128
02129
02130 inline bool getSegment(TSegment3D &s) const {
02131 if (isSegment()) {
02132 s=data.segment;
02133 return true;
02134 } else return false;
02135 }
02136
02137
02138
02139 inline bool getLine(TLine3D &r) const {
02140 if (isLine()) {
02141 r=data.line;
02142 return true;
02143 } else return false;
02144 }
02145
02146
02147
02148 inline bool getPolygon(TPolygon3D &p) const {
02149 if (isPolygon()) {
02150 p=*(data.polygon);
02151 return true;
02152 } else return false;
02153 }
02154
02155
02156
02157 inline bool getPlane(TPlane &p) const {
02158 if (isPlane()) {
02159 p=data.plane;
02160 return true;
02161 } else return false;
02162 }
02163
02164
02165
02166 void operator=(const TObject3D &obj) {
02167 if (this==&obj) return;
02168 destroy();
02169 switch (type=obj.type) {
02170 case GEOMETRIC_TYPE_POINT:
02171 data.point=obj.data.point;
02172 break;
02173 case GEOMETRIC_TYPE_SEGMENT:
02174 data.segment=obj.data.segment;
02175 break;
02176 case GEOMETRIC_TYPE_LINE:
02177 data.line=obj.data.line;
02178 break;
02179 case GEOMETRIC_TYPE_POLYGON:
02180 data.polygon=new TPolygon3D(*(obj.data.polygon));
02181 break;
02182 case GEOMETRIC_TYPE_PLANE:
02183 data.plane=obj.data.plane;
02184 break;
02185 case GEOMETRIC_TYPE_UNDEFINED:
02186 break;
02187 default:
02188 THROW_EXCEPTION("Invalid TObject3D object");
02189 }
02190 }
02191
02192
02193
02194 inline void operator=(const TPoint3D &p) {
02195 destroy();
02196 type=GEOMETRIC_TYPE_POINT;
02197 data.point=p;
02198 }
02199
02200
02201
02202 inline void operator=(const TSegment3D &s) {
02203 destroy();
02204 type=GEOMETRIC_TYPE_SEGMENT;
02205 data.segment=s;
02206 }
02207
02208
02209
02210 inline void operator=(const TLine3D &l) {
02211 destroy();
02212 type=GEOMETRIC_TYPE_LINE;
02213 data.line=l;
02214 }
02215
02216
02217
02218 inline void operator=(const TPolygon3D &p) {
02219 destroy();
02220 type=GEOMETRIC_TYPE_POLYGON;
02221 data.polygon=new TPolygon3D(p);
02222 }
02223
02224
02225
02226 inline void operator=(const TPlane &p) {
02227 destroy();
02228 type=GEOMETRIC_TYPE_PLANE;
02229 data.plane=p;
02230 }
02231
02232
02233
02234
02235 inline void generate2DObject(TObject2D &obj) const {
02236 switch (type) {
02237 case GEOMETRIC_TYPE_POINT:
02238 obj=TPoint2D(data.point);
02239 break;
02240 case GEOMETRIC_TYPE_SEGMENT:
02241 obj=TSegment2D(data.segment);
02242 break;
02243 case GEOMETRIC_TYPE_LINE:
02244 obj=TLine2D(data.line);
02245 break;
02246 case GEOMETRIC_TYPE_POLYGON:
02247 obj=TPolygon2D(*(data.polygon));
02248 break;
02249 case GEOMETRIC_TYPE_PLANE:
02250 throw std::logic_error("Too many dimensions");
02251 default:
02252 obj=TObject2D();
02253 break;
02254 }
02255 }
02256
02257
02258
02259 TObject3D(const TObject3D &obj):type(GEOMETRIC_TYPE_UNDEFINED) {
02260 operator=(obj);
02261 }
02262
02263
02264
02265 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts);
02266
02267
02268
02269 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms);
02270
02271
02272
02273 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins);
02274
02275
02276
02277 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns);
02278
02279
02280
02281 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
02282
02283
02284
02285 static void getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder);
02286
02287
02288
02289 static void getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder);
02290
02291
02292
02293 static void getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder);
02294
02295
02296
02297 static void getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder);
02298
02299
02300
02301 static void getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
02302 };
02303
02304 #endif
02305
02306
02307
02308
02309
02310
02311 BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPoint2D &o);
02312
02313
02314
02315 BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPoint2D &o);
02316
02317
02318
02319
02320 BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPoint3D &o);
02321
02322
02323
02324 BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPoint3D &o);
02325
02326
02327
02328
02329 BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPose2D &o);
02330
02331
02332
02333 BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPose2D &o);
02334
02335
02336
02337
02338 BASE_IMPEXP mrpt::utils::CStream& operator>>(mrpt::utils::CStream& in,mrpt::math::TPose3D &o);
02339
02340
02341
02342 BASE_IMPEXP mrpt::utils::CStream& operator<<(mrpt::utils::CStream& out,const mrpt::math::TPose3D &o);
02343
02344
02345
02346
02347 inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TSegment2D &s) {
02348 return in>>s.point1>>s.point2;
02349 }
02350
02351
02352
02353 inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TSegment2D &s) {
02354 return out<<s.point1<<s.point2;
02355 }
02356
02357
02358
02359
02360 inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TLine2D &l) {
02361 return in>>l.coefs[0]>>l.coefs[1]>>l.coefs[2];
02362 }
02363
02364
02365
02366 inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TLine2D &l) {
02367 return out<<l.coefs[0]<<l.coefs[1]<<l.coefs[2];
02368 }
02369
02370
02371
02372
02373 BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TObject2D &o);
02374
02375
02376
02377 BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TObject2D &o);
02378
02379
02380
02381
02382 inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TSegment3D &s) {
02383 return in>>s.point1>>s.point2;
02384 }
02385
02386
02387
02388 inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TSegment3D &s) {
02389 return out<<s.point1<<s.point2;
02390 }
02391
02392
02393
02394
02395 inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TLine3D &l) {
02396 return in>>l.pBase>>l.director[0]>>l.director[1]>>l.director[2];
02397 }
02398
02399
02400
02401 inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TLine3D &l) {
02402 return out<<l.pBase<<l.director[0]<<l.director[1]<<l.director[2];
02403 }
02404
02405
02406
02407
02408 inline mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TPlane &p) {
02409 return in>>p.coefs[0]>>p.coefs[1]>>p.coefs[2]>>p.coefs[3];
02410 }
02411
02412
02413
02414 inline mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TPlane &p) {
02415 return out<<p.coefs[0]<<p.coefs[1]<<p.coefs[2]<<p.coefs[3];
02416 }
02417
02418
02419
02420
02421 BASE_IMPEXP mrpt::utils::CStream &operator>>(mrpt::utils::CStream &in,mrpt::math::TObject3D &o);
02422
02423
02424
02425 BASE_IMPEXP mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TObject3D &o);
02426
02427 }
02428
02429 namespace utils
02430 {
02431 using namespace ::mrpt::math;
02432
02433
02434 MRPT_DECLARE_TTYPENAME(TPoint2D)
02435 MRPT_DECLARE_TTYPENAME(TPoint3D)
02436 MRPT_DECLARE_TTYPENAME(TPose2D)
02437 MRPT_DECLARE_TTYPENAME(TPose3D)
02438 MRPT_DECLARE_TTYPENAME(TSegment2D)
02439 MRPT_DECLARE_TTYPENAME(TLine2D)
02440 MRPT_DECLARE_TTYPENAME(TPolygon2D)
02441 MRPT_DECLARE_TTYPENAME(TObject2D)
02442 MRPT_DECLARE_TTYPENAME(TSegment3D)
02443 MRPT_DECLARE_TTYPENAME(TLine3D)
02444 MRPT_DECLARE_TTYPENAME(TPlane)
02445 MRPT_DECLARE_TTYPENAME(TPolygon3D)
02446 MRPT_DECLARE_TTYPENAME(TObject3D)
02447
02448 }
02449
02450 }
02451 #endif