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