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 {
00043 double MRPTDLLIMPEXP minimumDistanceFromPointToSegment(
00044 const double & Px,
00045 const double & Py,
00046 const double & x1,
00047 const double & y1,
00048 const double & x2,
00049 const double & y2,
00050 double & out_x,
00051 double & out_y);
00052
00055 double MRPTDLLIMPEXP minimumDistanceFromPointToSegment(
00056 const double & Px,
00057 const double & Py,
00058 const double & x1,
00059 const double & y1,
00060 const double & x2,
00061 const double & y2,
00062 float & out_x,
00063 float & out_y);
00064
00065
00069 void MRPTDLLIMPEXP closestFromPointToSegment(
00070 const double & Px,
00071 const double & Py,
00072 const double & x1,
00073 const double & y1,
00074 const double & x2,
00075 const double & y2,
00076 double &out_x,
00077 double &out_y);
00078
00082 void MRPTDLLIMPEXP closestFromPointToLine(
00083 const double & Px,
00084 const double & Py,
00085 const double & x1,
00086 const double & y1,
00087 const double & x2,
00088 const double & y2,
00089 double &out_x,
00090 double &out_y);
00091
00094 double MRPTDLLIMPEXP closestSquareDistanceFromPointToLine(
00095 const double & Px,
00096 const double & Py,
00097 const double & x1,
00098 const double & y1,
00099 const double & x2,
00100 const double & y2 );
00101
00102
00105 double MRPTDLLIMPEXP distanceBetweenPoints(const double & x1,const double & y1,const double & x2,const double & y2);
00106
00109 bool MRPTDLLIMPEXP SegmentsIntersection(
00110 const double & x1,const double & y1,
00111 const double & x2,const double & y2,
00112 const double & x3,const double & y3,
00113 const double & x4,const double & y4,
00114 double &ix,double &iy);
00115
00118 bool MRPTDLLIMPEXP SegmentsIntersection(
00119 const double & x1,const double & y1,
00120 const double & x2,const double & y2,
00121 const double & x3,const double & y3,
00122 const double & x4,const double & y4,
00123 float &ix,float &iy);
00124
00127 bool MRPTDLLIMPEXP pointIntoPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
00128
00131 double MRPTDLLIMPEXP distancePointToPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys );
00132
00140 bool MRPTDLLIMPEXP minDistBetweenLines(
00141 const double & p1_x, const double & p1_y, const double & p1_z,
00142 const double & p2_x, const double & p2_y, const double & p2_z,
00143 const double & p3_x, const double & p3_y, const double & p3_z,
00144 const double & p4_x, const double & p4_y, const double & p4_z,
00145 double &x, double &y, double &z,
00146 double &dist);
00147
00154 bool MRPTDLLIMPEXP RectanglesIntersection(
00155 const double & R1_x_min, const double & R1_x_max,
00156 const double & R1_y_min, const double & R1_y_max,
00157 const double & R2_x_min, const double & R2_x_max,
00158 const double & R2_y_min, const double & R2_y_max,
00159 const double & R2_pose_x,
00160 const double & R2_pose_y,
00161 const double & R2_pose_phi );
00162
00194 template<class T>
00195 CMatrixTemplateNumeric<T> generateAxisBaseFromDirection( T dx, T dy, T dz )
00196 {
00197 MRPT_TRY_START;
00198
00199 if (dx==0 && dy==0 && dz==0)
00200 THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0)!");
00201
00202 CMatrixTemplateNumeric<T> P(3,3);
00203
00204
00205 T n_xy = square(dx)+square(dy);
00206 T n = sqrt(n_xy+square(dz));
00207 n_xy = sqrt(n_xy);
00208 P(0,0) = dx / n;
00209 P(1,0) = dy / n;
00210 P(2,0) = dz / n;
00211
00212
00213 if (fabs(dx)>1e-4 || fabs(dy)>1e-4)
00214 {
00215 P(0,1) = -dy / n_xy;
00216 P(1,1) = dx / n_xy;
00217 P(2,1) = 0;
00218 }
00219 else
00220 {
00221
00222 P(0,1) = 1;
00223 P(1,1) = 0;
00224 P(2,1) = 0;
00225 }
00226
00227
00228 crossProduct3D(
00229 P(0,0),P(1,0),P(2,0),
00230 P(0,1),P(1,1),P(2,1),
00231 P(0,2),P(1,2),P(2,2) );
00232
00233 return P;
00234 MRPT_TRY_END;
00235 }
00236
00237
00249 template<class T>
00250 void crossProduct3D(
00251 T x0, T y0, T z0,
00252 T x1, T y1, T z1,
00253 T &x_out, T &y_out, T &z_out )
00254 {
00255 x_out = y0*z1 - z0*y1;
00256 y_out = -x0*z1 + z0*x1;
00257 z_out = x0*y1 - y0*x1;
00258 }
00259
00271 template<class T>
00272 void crossProduct3D(
00273 const std::vector<T> &v0,
00274 const std::vector<T> &v1,
00275 std::vector<T> &v_out )
00276 {
00277 ASSERT_(v0.size()==3)
00278 ASSERT_(v1.size()==3);
00279 v_out.resize(3);
00280 v_out[0] = v0[1]*v1[2] - v0[2]*v1[1];
00281 v_out[1] = -v0[0]*v1[2] + v0[2]*v1[0];
00282 v_out[2] = v0[0]*v1[1] - v0[1]*v1[0];
00283 }
00284
00285
00286 }
00287
00288 }
00289 #endif