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 GEO_H
00029 #define GEO_H
00030
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/math/CMatrixTemplateNumeric.h>
00033 #include <mrpt/poses/CPoint2D.h>
00034 #include <mrpt/poses/CPoint3D.h>
00035 #include <mrpt/poses/CPose2D.h>
00036 #include <mrpt/poses/CPose3D.h>
00037 #include <mrpt/math/lightweight_geom_data.h>
00038 #include <mrpt/math/CSparseMatrixTemplate.h>
00039 #include <mrpt/math/utils.h>
00040
00041
00042
00043
00044 namespace mrpt
00045 {
00046 namespace math
00047 {
00048 using std::vector;
00049 using namespace mrpt::utils;
00050 using namespace mrpt::poses;
00051
00052
00053
00054
00055 extern double BASE_IMPEXP geometryEpsilon;
00056
00057
00058
00059
00060
00061 class BASE_IMPEXP TPolygonWithPlane {
00062 public:
00063
00064
00065
00066 TPolygon3D poly;
00067
00068
00069
00070 TPlane plane;
00071
00072
00073
00074
00075 mrpt::poses::CPose3D pose;
00076
00077
00078
00079
00080 mrpt::poses::CPose3D inversePose;
00081
00082
00083
00084
00085 TPolygon2D poly2D;
00086
00087
00088
00089 TPolygonWithPlane(const TPolygon3D &p);
00090
00091
00092
00093
00094 TPolygonWithPlane() {}
00095
00096
00097
00098 static void getPlanes(const vector<TPolygon3D> &oldPolys,vector<TPolygonWithPlane> &newPolys);
00099 };
00100
00101
00102
00103
00104
00105
00106
00107
00108 bool BASE_IMPEXP intersect(const TSegment3D &s1,const TSegment3D &s2,TObject3D &obj);
00109
00110
00111
00112
00113 bool BASE_IMPEXP intersect(const TSegment3D &s1,const TPlane &p2,TObject3D &obj);
00114
00115
00116
00117
00118 bool BASE_IMPEXP intersect(const TSegment3D &s1,const TLine3D &r2,TObject3D &obj);
00119
00120
00121
00122
00123 inline bool intersect(const TPlane &p1,const TSegment3D &s2,TObject3D &obj) {
00124 return intersect(s2,p1,obj);
00125 }
00126
00127
00128
00129
00130 bool BASE_IMPEXP intersect(const TPlane &p1,const TPlane &p2,TObject3D &obj);
00131
00132
00133
00134
00135 bool BASE_IMPEXP intersect(const TPlane &p1,const TLine3D &p2,TObject3D &obj);
00136
00137
00138
00139
00140 inline bool intersect(const TLine3D &r1,const TSegment3D &s2,TObject3D &obj) {
00141 return intersect(s2,r1,obj);
00142 }
00143
00144
00145
00146
00147 inline bool intersect(const TLine3D &r1,const TPlane &p2,TObject3D &obj) {
00148 return intersect(p2,r1,obj);
00149 }
00150
00151
00152
00153
00154 bool BASE_IMPEXP intersect(const TLine3D &r1,const TLine3D &r2,TObject3D &obj);
00155
00156
00157
00158
00159 bool BASE_IMPEXP intersect(const TLine2D &r1,const TLine2D &r2,TObject2D &obj);
00160
00161
00162
00163
00164 bool BASE_IMPEXP intersect(const TLine2D &r1,const TSegment2D &s2,TObject2D &obj);
00165
00166
00167
00168
00169 inline bool intersect(const TSegment2D &s1,const TLine2D &r2,TObject2D &obj) {
00170 return intersect(r2,s1,obj);
00171 }
00172
00173
00174
00175
00176 bool BASE_IMPEXP intersect(const TSegment2D &s1,const TSegment2D &s2,TObject2D &obj);
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186 double BASE_IMPEXP getAngle(const TPlane &p1,const TPlane &p2);
00187
00188
00189
00190 double BASE_IMPEXP getAngle(const TPlane &p1,const TLine3D &r2);
00191
00192
00193
00194 inline double getAngle(const TLine3D &r1,const TPlane &p2) {
00195 return getAngle(p2,r1);
00196 }
00197
00198
00199
00200 double BASE_IMPEXP getAngle(const TLine3D &r1,const TLine3D &r2);
00201
00202
00203
00204 double BASE_IMPEXP getAngle(const TLine2D &r1,const TLine2D &r2);
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 void BASE_IMPEXP createFromPoseX(const CPose3D &p,TLine3D &r);
00216
00217
00218
00219
00220 void BASE_IMPEXP createFromPoseY(const CPose3D &p,TLine3D &r);
00221
00222
00223
00224
00225 void BASE_IMPEXP createFromPoseZ(const CPose3D &p,TLine3D &r);
00226
00227
00228
00229
00230 void BASE_IMPEXP createFromPoseAndVector(const CPose3D &p,const double (&vector)[3],TLine3D &r);
00231
00232
00233
00234
00235 void BASE_IMPEXP createFromPoseX(const TPose2D &p,TLine2D &r);
00236
00237
00238
00239
00240 void BASE_IMPEXP createFromPoseY(const TPose2D &p,TLine2D &r);
00241
00242
00243
00244
00245 void BASE_IMPEXP createFromPoseAndVector(const TPose2D &p,const double (&vector)[2],TLine2D &r);
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256 bool BASE_IMPEXP conformAPlane(const std::vector<TPoint3D> &points);
00257
00258
00259
00260
00261 bool BASE_IMPEXP conformAPlane(const std::vector<TPoint3D> &points,TPlane &p);
00262
00263
00264
00265
00266 bool BASE_IMPEXP areAligned(const std::vector<TPoint2D> &points);
00267
00268
00269
00270
00271 bool BASE_IMPEXP areAligned(const std::vector<TPoint2D> &points,TLine2D &r);
00272
00273
00274
00275
00276 bool BASE_IMPEXP areAligned(const std::vector<TPoint3D> &points);
00277
00278
00279
00280 bool BASE_IMPEXP areAligned(const std::vector<TPoint3D> &points,TLine3D &r);
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290 inline void project3D(const TPoint3D &point, const CPose3D &newXYpose,TPoint3D &newPoint) {
00291 newXYpose.composePoint(point.x,point.y,point.z,newPoint.x,newPoint.y,newPoint.z);
00292 }
00293
00294
00295
00296 inline void project3D(const TSegment3D &segment,const CPose3D &newXYpose,TSegment3D &newSegment) {
00297 project3D(segment.point1,newXYpose,newSegment.point1);
00298 project3D(segment.point2,newXYpose,newSegment.point2);
00299 }
00300
00301
00302
00303 void BASE_IMPEXP project3D(const TLine3D &line,const CPose3D &newXYpose,TLine3D &newLine);
00304
00305
00306
00307 void BASE_IMPEXP project3D(const TPlane &plane,const CPose3D &newXYpose,TPlane &newPlane);
00308
00309
00310
00311 void BASE_IMPEXP project3D(const TPolygon3D &polygon,const CPose3D &newXYpose,TPolygon3D &newPolygon);
00312
00313
00314
00315 void BASE_IMPEXP project3D(const TObject3D &object,const CPose3D &newXYPose,TObject3D &newObject);
00316
00317
00318
00319
00320 template<class T> void project3D(const T &obj,const TPlane &newXYPlane,T &newObj) {
00321 CPose3D pose;
00322 TPlane(newXYPlane).getAsPose3D(pose);
00323 project3D(obj,-pose,newObj);
00324 }
00325
00326
00327
00328
00329 template<class T> void project3D(const T &obj,const TPlane &newXYPlane,const TPoint3D &newOrigin,T &newObj) {
00330 CPose3D pose;
00331
00332 TPlane(newXYPlane).getAsPose3D(pose);
00333 project3D(obj,-pose,newObj);
00334 }
00335
00336
00337
00338
00339 template<class T> void project3D(const std::vector<T> &objs,const CPose3D &newXYpose,std::vector<T> &newObjs) {
00340 size_t N=objs.size();
00341 newObjs.resize(N);
00342 for (size_t i=0;i<N;i++) project3D(objs[i],newXYpose,newObjs[i]);
00343 }
00344
00345
00346
00347
00348 inline void project2D(const TPoint2D &point,const CPose2D &newXpose,TPoint2D &newPoint) {
00349 newPoint=newXpose+CPoint2D(point);
00350 }
00351
00352
00353
00354 inline void project2D(const TSegment2D &segment,const CPose2D &newXpose,TSegment2D &newSegment) {
00355 project2D(segment.point1,newXpose,newSegment.point1);
00356 project2D(segment.point2,newXpose,newSegment.point2);
00357 }
00358
00359
00360
00361
00362 void BASE_IMPEXP project2D(const TLine2D &line,const CPose2D &newXpose,TLine2D &newLine);
00363
00364
00365
00366 void BASE_IMPEXP project2D(const TPolygon2D &polygon,const CPose2D &newXpose,TPolygon2D &newPolygon);
00367
00368
00369
00370 void BASE_IMPEXP project2D(const TObject2D &object,const CPose2D &newXpose,TObject2D &newObject);
00371
00372
00373
00374
00375 template<class T> void project2D(const T &obj,const TLine2D &newXLine,T &newObj) {
00376 CPose2D pose;
00377 newXLine.getAsPose2D(pose);
00378 project2D(obj,CPose2D(0,0,0)-pose,newObj);
00379 }
00380
00381
00382
00383
00384 template<class T> void project2D(const T &obj,const TLine2D &newXLine,const TPoint2D &newOrigin,T &newObj) {
00385 CPose2D pose;
00386 newXLine.getAsPose2DForcingOrigin(newOrigin,pose);
00387 project2D(obj,CPose2D(0,0,0)-pose,newObj);
00388 }
00389
00390
00391
00392
00393 template<class T> void project2D(const std::vector<T> &objs,const CPose2D &newXpose,std::vector<T> &newObjs) {
00394 size_t N=objs.size();
00395 newObjs.resize(N);
00396 for (size_t i=0;i<N;i++) project2D(objs[i],newXpose,newObjs[i]);
00397 }
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408 bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TSegment2D &s2,TObject2D &obj);
00409
00410
00411
00412
00413 bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TLine2D &r2,TObject2D &obj);
00414
00415
00416
00417
00418 bool BASE_IMPEXP intersect(const TPolygon2D &p1,const TPolygon2D &p2,TObject2D &obj);
00419
00420
00421
00422
00423 inline bool intersect(const TSegment2D &s1,const TPolygon2D &p2,TObject2D &obj) {
00424 return intersect(p2,s1,obj);
00425 }
00426
00427
00428
00429
00430 inline bool intersect(const TLine2D &r1,const TPolygon2D &p2,TObject2D &obj) {
00431 return intersect(p2,r1,obj);
00432 }
00433
00434
00435
00436
00437 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TSegment3D &s2,TObject3D &obj);
00438
00439
00440
00441
00442 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TLine3D &r2,TObject3D &obj);
00443
00444
00445
00446
00447 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TPlane &p2,TObject3D &obj);
00448
00449
00450
00451
00452 bool BASE_IMPEXP intersect(const TPolygon3D &p1,const TPolygon3D &p2,TObject3D &obj);
00453
00454
00455
00456
00457 inline bool intersect(const TSegment3D &s1,const TPolygon3D &p2,TObject3D &obj) {
00458 return intersect(p2,s1,obj);
00459 }
00460
00461
00462
00463
00464 inline bool intersect(const TLine3D &r1,const TPolygon3D &p2,TObject3D &obj) {
00465 return intersect(p2,r1,obj);
00466 }
00467
00468
00469
00470
00471 inline bool intersect(const TPlane &p1,const TPolygon3D &p2,TObject3D &obj) {
00472 return intersect(p2,p1,obj);
00473 }
00474
00475
00476
00477
00478
00479 size_t BASE_IMPEXP intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,CSparseMatrixTemplate<TObject3D> &objs);
00480
00481
00482
00483
00484 size_t BASE_IMPEXP intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,std::vector<TObject3D> &objs);
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495 template<class T,class U,class O> size_t intersect(const std::vector<T> &v1,const std::vector<U> &v2,CSparseMatrixTemplate<O> &objs) {
00496 size_t M=v1.size(),N=v2.size();
00497 O obj;
00498 objs.clear();
00499 objs.resize(M,N);
00500 for (size_t i=0;i<M;i++) for (size_t j=0;j<M;j++) if (intersect(v1[i],v2[j],obj)) objs(i,j)=obj;
00501 return objs.getNonNullElements();
00502 }
00503
00504
00505
00506
00507
00508 template<class T,class U,class O> size_t intersect(const std::vector<T> &v1,const std::vector<U> &v2,std::vector<O> objs) {
00509 objs.resize(0);
00510 O obj;
00511 for (typename std::vector<T>::const_iterator it1=v1.begin();it1!=v1.end();it1++) {
00512 const T &elem1=*it1;
00513 for (typename std::vector<U>::const_iterator it2=v2.begin();it2!=v2.end();it2++) if (intersect(elem1,*it2,obj)) objs.push_back(obj);
00514 }
00515 return objs.size();
00516 }
00517
00518
00519
00520
00521 bool BASE_IMPEXP intersect(const TObject2D &o1,const TObject2D &o2,TObject2D &obj);
00522
00523
00524
00525 bool BASE_IMPEXP intersect(const TObject3D &o1,const TObject3D &o2,TObject3D &obj);
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535 double BASE_IMPEXP distance(const TPoint2D &p1,const TPoint2D &p2);
00536
00537
00538
00539 double BASE_IMPEXP distance(const TPoint3D &p1,const TPoint3D &p2);
00540
00541
00542
00543 double BASE_IMPEXP distance(const TLine2D &r1,const TLine2D &r2);
00544
00545
00546
00547 double BASE_IMPEXP distance(const TLine3D &r1,const TLine3D &r2);
00548
00549
00550
00551 double BASE_IMPEXP distance(const TPlane &p1,const TPlane &p2);
00552
00553
00554
00555
00556 double BASE_IMPEXP distance(const TPolygon2D &p1,const TPolygon2D &p2);
00557
00558
00559
00560 double BASE_IMPEXP distance(const TPolygon2D &p1,const TSegment2D &s2);
00561
00562
00563
00564 inline double distance(const TSegment2D &s1,const TPolygon2D &p2) {
00565 return distance(p2,s1);
00566 }
00567
00568
00569
00570 double BASE_IMPEXP distance(const TPolygon2D &p1,const TLine2D &l2);
00571 inline double distance(const TLine2D &l1,const TPolygon2D &p2) {
00572 return distance(p2,l1);
00573 }
00574
00575
00576
00577 double BASE_IMPEXP distance(const TPolygon3D &p1,const TPolygon3D &p2);
00578
00579
00580
00581 double BASE_IMPEXP distance(const TPolygon3D &p1,const TSegment3D &s2);
00582
00583
00584
00585 inline double distance(const TSegment3D &s1,const TPolygon3D &p2) {
00586 return distance(p2,s1);
00587 }
00588
00589
00590
00591 double BASE_IMPEXP distance(const TPolygon3D &p1,const TLine3D &l2);
00592
00593
00594
00595 inline double distance(const TLine3D &l1,const TPolygon3D &p2) {
00596 return distance(p2,l1);
00597 }
00598
00599
00600
00601 double BASE_IMPEXP distance(const TPolygon3D &po,const TPlane &pl);
00602
00603
00604
00605 inline double distance(const TPlane &pl,const TPolygon3D &po) {
00606 return distance(po,pl);
00607 }
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617 void BASE_IMPEXP getRectangleBounds(const std::vector<TPoint2D> &poly,TPoint2D &pMin,TPoint2D &pMax);
00618
00619
00620
00621 void BASE_IMPEXP getPrismBounds(const std::vector<TPoint3D> &poly,TPoint3D &pMin,TPoint3D &pMax);
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632 void BASE_IMPEXP createPlaneFromPoseXY(const CPose3D &pose,TPlane &plane);
00633
00634
00635
00636
00637 void BASE_IMPEXP createPlaneFromPoseXZ(const CPose3D &pose,TPlane &plane);
00638
00639
00640
00641
00642 void BASE_IMPEXP createPlaneFromPoseYZ(const CPose3D &pose,TPlane &plane);
00643
00644
00645
00646
00647 void BASE_IMPEXP createPlaneFromPoseAndNormal(const CPose3D &pose,const double (&normal)[3],TPlane &plane);
00648
00649
00650
00651 void BASE_IMPEXP generateAxisBaseFromDirectionAndAxis(const double (&vec)[3],char coord,CMatrixDouble &matrix);
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662 double BASE_IMPEXP getRegressionLine(const std::vector<TPoint2D> &points,TLine2D &line);
00663
00664
00665
00666
00667 double BASE_IMPEXP getRegressionLine(const std::vector<TPoint3D> &points,TLine3D &line);
00668
00669
00670
00671
00672 double BASE_IMPEXP getRegressionPlane(const std::vector<TPoint3D> &points,TPlane &plane);
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682 void BASE_IMPEXP assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys);
00683
00684
00685
00686 void BASE_IMPEXP assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder);
00687
00688
00689
00690 void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys);
00691
00692
00693
00694 void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder);
00695
00696
00697
00698 void BASE_IMPEXP assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder1,std::vector<TObject3D> &remainder2);
00699
00700
00701
00702
00703
00704 inline void setEpsilon(double nE) {
00705 geometryEpsilon=nE;
00706 }
00707
00708
00709
00710
00711 inline double getEpsilon() {
00712 return geometryEpsilon;
00713 }
00714
00715
00716
00717
00718 bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly,vector<TPolygon2D> &components);
00719
00720
00721
00722
00723 bool BASE_IMPEXP splitInConvexComponents(const TPolygon3D &poly,vector<TPolygon3D> &components);
00724
00725
00726
00727
00728 void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm,TLine2D &bis);
00729
00730
00731
00732 void BASE_IMPEXP getSegmentBisector(const TSegment3D &sgm,TPlane &bis);
00733
00734
00735
00736 void BASE_IMPEXP getAngleBisector(const TLine2D &l1,const TLine2D &l2,TLine2D &bis);
00737
00738
00739
00740
00741 void BASE_IMPEXP getAngleBisector(const TLine3D &l1,const TLine3D &l2,TLine3D &bis);
00742
00743
00744
00745
00746
00747 bool BASE_IMPEXP traceRay(const vector<TPolygonWithPlane> &vec,const mrpt::poses::CPose3D &pose,double &dist);
00748
00749
00750
00751
00752 inline bool traceRay(const vector<TPolygon3D> &vec,const mrpt::poses::CPose3D &pose,double &dist) {
00753 vector<TPolygonWithPlane> pwp;
00754 TPolygonWithPlane::getPlanes(vec,pwp);
00755 return traceRay(pwp,pose,dist);
00756 }
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769 template<class T,class U,class V>
00770 inline void crossProduct3D(const T &v0,const U &v1,V& vOut) {
00771 vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
00772 vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
00773 vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
00774 }
00775
00776
00777 template<class T>
00778 inline void crossProduct3D(
00779 const std::vector<T> &v0,
00780 const std::vector<T> &v1,
00781 std::vector<T> &v_out )
00782 {
00783 ASSERT_(v0.size()==3)
00784 ASSERT_(v1.size()==3);
00785 v_out.resize(3);
00786 v_out[0] = v0[1]*v1[2] - v0[2]*v1[1];
00787 v_out[1] = -v0[0]*v1[2] + v0[2]*v1[0];
00788 v_out[2] = v0[0]*v1[1] - v0[1]*v1[0];
00789 }
00790
00791
00792 template<class VEC1,class VEC2>
00793 inline Eigen::Matrix<double,3,1> crossProduct3D(const VEC1 &v0,const VEC2 &v1) {
00794 Eigen::Matrix<double,3,1> vOut;
00795 vOut[0]=v0[1]*v1[2]-v0[2]*v1[1];
00796 vOut[1]=v0[2]*v1[0]-v0[0]*v1[2];
00797 vOut[2]=v0[0]*v1[1]-v0[1]*v1[0];
00798 return vOut;
00799 }
00800
00801
00802
00803
00804
00805
00806
00807
00808
00809
00810 template<class VECTOR,class MATRIX>
00811 inline void skew_symmetric3(const VECTOR &v,MATRIX& M) {
00812 ASSERT_(v.size()==3)
00813 M.setSize(3,3);
00814 M.set_unsafe(0,0, 0); M.set_unsafe(0,1, -v[2]); M.set_unsafe(0,2, v[1]);
00815 M.set_unsafe(1,0, v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, -v[0]);
00816 M.set_unsafe(2,0, -v[1]); M.set_unsafe(2,1, v[0]); M.set_unsafe(2,2, 0);
00817 }
00818
00819 template<class VECTOR>
00820 inline mrpt::math::CMatrixDouble33 skew_symmetric3(const VECTOR &v) {
00821 mrpt::math::CMatrixDouble33 M(UNINITIALIZED_MATRIX);
00822 skew_symmetric3(v,M);
00823 return M;
00824 }
00825
00826
00827
00828
00829
00830
00831
00832
00833
00834
00835 template<class VECTOR,class MATRIX>
00836 inline void skew_symmetric3_neg(const VECTOR &v,MATRIX& M) {
00837 ASSERT_(v.size()==3)
00838 M.setSize(3,3);
00839 M.set_unsafe(0,0, 0); M.set_unsafe(0,1, v[2]); M.set_unsafe(0,2, -v[1]);
00840 M.set_unsafe(1,0, -v[2]); M.set_unsafe(1,1, 0); M.set_unsafe(1,2, v[0]);
00841 M.set_unsafe(2,0, v[1]); M.set_unsafe(2,1, -v[0]); M.set_unsafe(2,2, 0);
00842 }
00843
00844 template<class VECTOR>
00845 inline mrpt::math::CMatrixDouble33 skew_symmetric3_neg(const VECTOR &v) {
00846 mrpt::math::CMatrixDouble33 M(UNINITIALIZED_MATRIX);
00847 skew_symmetric3_neg(v,M);
00848 return M;
00849 }
00850
00851
00852
00853
00854
00855 template<class T,class U> inline bool vectorsAreParallel2D(const T &v1,const U &v2) {
00856 return abs(v1[0]*v2[1]-v2[0]*v1[1])<geometryEpsilon;
00857 }
00858
00859
00860
00861
00862 template<class T,class U>
00863 inline bool vectorsAreParallel3D(const T &v1,const U &v2) {
00864 if (abs(v1[0]*v2[1]-v2[0]*v1[1])>=geometryEpsilon) return false;
00865 if (abs(v1[1]*v2[2]-v2[1]*v1[2])>=geometryEpsilon) return false;
00866 return abs(v1[2]*v2[0]-v2[2]*v1[0])<geometryEpsilon;
00867 }
00868
00869
00870
00871 double BASE_IMPEXP minimumDistanceFromPointToSegment(
00872 const double & Px,
00873 const double & Py,
00874 const double & x1,
00875 const double & y1,
00876 const double & x2,
00877 const double & y2,
00878 double & out_x,
00879 double & out_y);
00880
00881
00882
00883 double BASE_IMPEXP minimumDistanceFromPointToSegment(
00884 const double & Px,
00885 const double & Py,
00886 const double & x1,
00887 const double & y1,
00888 const double & x2,
00889 const double & y2,
00890 float & out_x,
00891 float & out_y);
00892
00893
00894
00895
00896
00897 void BASE_IMPEXP closestFromPointToSegment(
00898 const double & Px,
00899 const double & Py,
00900 const double & x1,
00901 const double & y1,
00902 const double & x2,
00903 const double & y2,
00904 double &out_x,
00905 double &out_y);
00906
00907
00908
00909
00910 void BASE_IMPEXP closestFromPointToLine(
00911 const double & Px,
00912 const double & Py,
00913 const double & x1,
00914 const double & y1,
00915 const double & x2,
00916 const double & y2,
00917 double &out_x,
00918 double &out_y);
00919
00920
00921
00922 double BASE_IMPEXP closestSquareDistanceFromPointToLine(
00923 const double & Px,
00924 const double & Py,
00925 const double & x1,
00926 const double & y1,
00927 const double & x2,
00928 const double & y2 );
00929
00930
00931
00932 template <typename T>
00933 T distanceBetweenPoints(const T x1,const T y1,const T x2,const T y2) {
00934 return std::sqrt( square(x1-x2)+square(y1-y2) );
00935 }
00936
00937
00938 template <typename T>
00939 T distanceBetweenPoints(const T x1,const T y1,const T z1, const T x2,const T y2, const T z2) {
00940 return std::sqrt( square(x1-x2)+square(y1-y2)+square(z1-z2) );
00941 }
00942
00943
00944 template <typename T>
00945 T distanceSqrBetweenPoints(const T x1,const T y1,const T x2,const T y2) {
00946 return square(x1-x2)+square(y1-y2);
00947 }
00948
00949
00950 template <typename T>
00951 T distanceSqrBetweenPoints(const T x1,const T y1,const T z1, const T x2,const T y2, const T z2) {
00952 return square(x1-x2)+square(y1-y2)+square(z1-z2);
00953 }
00954
00955
00956
00957 bool BASE_IMPEXP SegmentsIntersection(
00958 const double & x1,const double & y1,
00959 const double & x2,const double & y2,
00960 const double & x3,const double & y3,
00961 const double & x4,const double & y4,
00962 double &ix,double &iy);
00963
00964
00965
00966 bool BASE_IMPEXP SegmentsIntersection(
00967 const double & x1,const double & y1,
00968 const double & x2,const double & y2,
00969 const double & x3,const double & y3,
00970 const double & x4,const double & y4,
00971 float &ix,float &iy);
00972
00973
00974
00975
00976 bool BASE_IMPEXP pointIntoPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
00977
00978
00979
00980
00981 template <typename T>
00982 bool pointIntoQuadrangle(
00983 T x, T y,
00984 T v1x, T v1y,
00985 T v2x, T v2y,
00986 T v3x, T v3y,
00987 T v4x, T v4y )
00988 {
00989 using mrpt::utils::sign;
00990
00991 const T a1 = atan2( v1y - y , v1x - x );
00992 const T a2 = atan2( v2y - y , v2x - x );
00993 const T a3 = atan2( v3y - y , v3x - x );
00994 const T a4 = atan2( v4y - y , v4x - x );
00995
00996
00997
00998 const T da1 = mrpt::math::wrapToPi( a2-a1 );
00999 const T da2 = mrpt::math::wrapToPi( a3-a2 );
01000 if (sign(da1)!=sign(da2)) return false;
01001
01002 const T da3 = mrpt::math::wrapToPi( a4-a3 );
01003 if (sign(da2)!=sign(da3)) return false;
01004
01005 const T da4 = mrpt::math::wrapToPi( a1-a4 );
01006 return (sign(da3)==sign(da4) && (sign(da4)==sign(da1)));
01007 }
01008
01009
01010
01011 double BASE_IMPEXP distancePointToPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
01012
01013
01014
01015
01016
01017
01018
01019
01020 bool BASE_IMPEXP minDistBetweenLines(
01021 const double & p1_x, const double & p1_y, const double & p1_z,
01022 const double & p2_x, const double & p2_y, const double & p2_z,
01023 const double & p3_x, const double & p3_y, const double & p3_z,
01024 const double & p4_x, const double & p4_y, const double & p4_z,
01025 double &x, double &y, double &z,
01026 double &dist);
01027
01028
01029
01030
01031
01032
01033
01034 bool BASE_IMPEXP RectanglesIntersection(
01035 const double & R1_x_min, const double & R1_x_max,
01036 const double & R1_y_min, const double & R1_y_max,
01037 const double & R2_x_min, const double & R2_x_max,
01038 const double & R2_y_min, const double & R2_y_max,
01039 const double & R2_pose_x,
01040 const double & R2_pose_y,
01041 const double & R2_pose_phi );
01042
01043
01044
01045
01046
01047
01048
01049
01050
01051
01052
01053
01054
01055
01056
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066
01067
01068
01069
01070
01071
01072
01073
01074 template<class T>
01075 CMatrixTemplateNumeric<T> generateAxisBaseFromDirection( T dx, T dy, T dz )
01076 {
01077 MRPT_START;
01078
01079 if (dx==0 && dy==0 && dz==0)
01080 THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0)!");
01081
01082 CMatrixTemplateNumeric<T> P(3,3);
01083
01084
01085 T n_xy = square(dx)+square(dy);
01086 T n = sqrt(n_xy+square(dz));
01087 n_xy = sqrt(n_xy);
01088 P(0,0) = dx / n;
01089 P(1,0) = dy / n;
01090 P(2,0) = dz / n;
01091
01092
01093 if (fabs(dx)>1e-4 || fabs(dy)>1e-4)
01094 {
01095 P(0,1) = -dy / n_xy;
01096 P(1,1) = dx / n_xy;
01097 P(2,1) = 0;
01098 }
01099 else
01100 {
01101
01102 P(0,1) = 1;
01103 P(1,1) = 0;
01104 P(2,1) = 0;
01105 }
01106
01107
01108 P.col(2) = crossProduct3D(P.col(0),P.col(1));
01109
01110 return P;
01111 MRPT_END;
01112 }
01113
01114
01115
01116
01117
01118
01119
01120
01121
01122
01123
01124
01125
01126
01127
01128 template <typename VECTOR_LIKE, typename Precision, typename MATRIX_LIKE>
01129 inline void rodrigues_so3_exp(const VECTOR_LIKE& w, const Precision A,const Precision B,MATRIX_LIKE & R)
01130 {
01131 ASSERT_EQUAL_(w.size(),3)
01132 ASSERT_EQUAL_(R.getColCount(),3)
01133 ASSERT_EQUAL_(R.getRowCount(),3)
01134 {
01135 const Precision wx2 = (Precision)w[0]*w[0];
01136 const Precision wy2 = (Precision)w[1]*w[1];
01137 const Precision wz2 = (Precision)w[2]*w[2];
01138 R(0,0) = 1.0 - B*(wy2 + wz2);
01139 R(1,1) = 1.0 - B*(wx2 + wz2);
01140 R(2,2) = 1.0 - B*(wx2 + wy2);
01141 }
01142 {
01143 const Precision a = A*w[2];
01144 const Precision b = B*(w[0]*w[1]);
01145 R(0,1) = b - a;
01146 R(1,0) = b + a;
01147 }
01148 {
01149 const Precision a = A*w[1];
01150 const Precision b = B*(w[0]*w[2]);
01151 R(0,2) = b + a;
01152 R(2,0) = b - a;
01153 }
01154 {
01155 const Precision a = A*w[0];
01156 const Precision b = B*(w[1]*w[2]);
01157 R(1,2) = b - a;
01158 R(2,1) = b + a;
01159 }
01160 }
01161
01162
01163
01164 }
01165
01166 }
01167 #endif