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
00034
00035
00036
00037 namespace mrpt
00038 {
00039 namespace math
00040 {
00041 using namespace mrpt::utils;
00042
00045 double MRPTDLLIMPEXP minimumDistanceFromPointToSegment(
00046 const double & Px,
00047 const double & Py,
00048 const double & x1,
00049 const double & y1,
00050 const double & x2,
00051 const double & y2,
00052 double & out_x,
00053 double & out_y);
00054
00057 double MRPTDLLIMPEXP minimumDistanceFromPointToSegment(
00058 const double & Px,
00059 const double & Py,
00060 const double & x1,
00061 const double & y1,
00062 const double & x2,
00063 const double & y2,
00064 float & out_x,
00065 float & out_y);
00066
00067
00071 void MRPTDLLIMPEXP closestFromPointToSegment(
00072 const double & Px,
00073 const double & Py,
00074 const double & x1,
00075 const double & y1,
00076 const double & x2,
00077 const double & y2,
00078 double &out_x,
00079 double &out_y);
00080
00084 void MRPTDLLIMPEXP closestFromPointToLine(
00085 const double & Px,
00086 const double & Py,
00087 const double & x1,
00088 const double & y1,
00089 const double & x2,
00090 const double & y2,
00091 double &out_x,
00092 double &out_y);
00093
00096 double MRPTDLLIMPEXP closestSquareDistanceFromPointToLine(
00097 const double & Px,
00098 const double & Py,
00099 const double & x1,
00100 const double & y1,
00101 const double & x2,
00102 const double & y2 );
00103
00104
00106 double MRPTDLLIMPEXP distanceBetweenPoints(const double x1,const double y1,const double x2,const double y2);
00107
00109 double MRPTDLLIMPEXP distanceBetweenPoints(const double x1,const double y1,const double z1, const double x2,const double y2, const double z2);
00110
00112 double MRPTDLLIMPEXP distanceSqrBetweenPoints(const double x1,const double y1,const double x2,const double y2);
00113
00115 double MRPTDLLIMPEXP distanceSqrBetweenPoints(const double x1,const double y1,const double z1, const double x2,const double y2, const double z2);
00116
00119 bool MRPTDLLIMPEXP SegmentsIntersection(
00120 const double & x1,const double & y1,
00121 const double & x2,const double & y2,
00122 const double & x3,const double & y3,
00123 const double & x4,const double & y4,
00124 double &ix,double &iy);
00125
00128 bool MRPTDLLIMPEXP SegmentsIntersection(
00129 const double & x1,const double & y1,
00130 const double & x2,const double & y2,
00131 const double & x3,const double & y3,
00132 const double & x4,const double & y4,
00133 float &ix,float &iy);
00134
00137 bool MRPTDLLIMPEXP pointIntoPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
00138
00141 double MRPTDLLIMPEXP distancePointToPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
00142
00150 bool MRPTDLLIMPEXP minDistBetweenLines(
00151 const double & p1_x, const double & p1_y, const double & p1_z,
00152 const double & p2_x, const double & p2_y, const double & p2_z,
00153 const double & p3_x, const double & p3_y, const double & p3_z,
00154 const double & p4_x, const double & p4_y, const double & p4_z,
00155 double &x, double &y, double &z,
00156 double &dist);
00157
00164 bool MRPTDLLIMPEXP RectanglesIntersection(
00165 const double & R1_x_min, const double & R1_x_max,
00166 const double & R1_y_min, const double & R1_y_max,
00167 const double & R2_x_min, const double & R2_x_max,
00168 const double & R2_y_min, const double & R2_y_max,
00169 const double & R2_pose_x,
00170 const double & R2_pose_y,
00171 const double & R2_pose_phi );
00172
00204 template<class T>
00205 CMatrixTemplateNumeric<T> generateAxisBaseFromDirection( T dx, T dy, T dz )
00206 {
00207 MRPT_TRY_START;
00208
00209 if (dx==0 && dy==0 && dz==0)
00210 THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0)!");
00211
00212 CMatrixTemplateNumeric<T> P(3,3);
00213
00214
00215 T n_xy = square(dx)+square(dy);
00216 T n = sqrt(n_xy+square(dz));
00217 n_xy = sqrt(n_xy);
00218 P(0,0) = dx / n;
00219 P(1,0) = dy / n;
00220 P(2,0) = dz / n;
00221
00222
00223 if (fabs(dx)>1e-4 || fabs(dy)>1e-4)
00224 {
00225 P(0,1) = -dy / n_xy;
00226 P(1,1) = dx / n_xy;
00227 P(2,1) = 0;
00228 }
00229 else
00230 {
00231
00232 P(0,1) = 1;
00233 P(1,1) = 0;
00234 P(2,1) = 0;
00235 }
00236
00237
00238 crossProduct3D(
00239 P(0,0),P(1,0),P(2,0),
00240 P(0,1),P(1,1),P(2,1),
00241 P(0,2),P(1,2),P(2,2) );
00242
00243 return P;
00244 MRPT_TRY_END;
00245 }
00246
00247
00259 template<class T>
00260 void crossProduct3D(
00261 T x0, T y0, T z0,
00262 T x1, T y1, T z1,
00263 T &x_out, T &y_out, T &z_out )
00264 {
00265 x_out = y0*z1 - z0*y1;
00266 y_out = -x0*z1 + z0*x1;
00267 z_out = x0*y1 - y0*x1;
00268 }
00269
00281 template<class T>
00282 void crossProduct3D(
00283 const std::vector<T> &v0,
00284 const std::vector<T> &v1,
00285 std::vector<T> &v_out )
00286 {
00287 ASSERT_(v0.size()==3)
00288 ASSERT_(v1.size()==3);
00289 v_out.resize(3);
00290 v_out[0] = v0[1]*v1[2] - v0[2]*v1[1];
00291 v_out[1] = -v0[0]*v1[2] + v0[2]*v1[0];
00292 v_out[2] = v0[0]*v1[1] - v0[1]*v1[0];
00293 }
00294
00295
00296 }
00297
00298 }
00299 #endif