Main MRPT website > C++ reference for MRPT 1.5.8
geometry.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
8  +---------------------------------------------------------------------------+ */
9
10 #include "base-precomp.h" // Precompiled headers
11
12 #include <mrpt/math/geometry.h>
13 #include <mrpt/math/CPolygon.h>
16 #include <mrpt/math/data_utils.h>
18 #include <mrpt/poses/CPoint2D.h>
19 #include <mrpt/poses/CPose2D.h>
21
22 using namespace mrpt;
23 using namespace mrpt::utils;
24 using namespace std;
25 using namespace mrpt::poses;
26 using namespace mrpt::math;
27
29
30 /*---------------------------------------------------------------
31  Returns the closest point to a segment
32  ---------------------------------------------------------------*/
34  const double & Px,
35  const double & Py,
36  const double & x1,
37  const double & y1,
38  const double & x2,
39  const double & y2,
40  double &out_x,
41  double &out_y)
42 {
43  if (x1==x2 && y1==y2)
44  {
45  out_x = x1;
46  out_y = y1;
47  }
48  else
49  {
50  double Dx = x2 - x1;
51  double Dy = y2 - y1;
52  double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
53  if (Ratio<0)
54  {
55  out_x = x1;
56  out_y = y1;
57  }
58  else
59  {
60  if (Ratio > 1)
61  {
62  out_x = x2;
63  out_y = y2;
64  }
65  else
66  {
67  out_x = x1 + (Ratio * Dx);
68  out_y = y1 + (Ratio * Dy);
69  }
70  }
71  }
72 }
73
74 /*---------------------------------------------------------------
75  Returns the closest point to a line
76  ---------------------------------------------------------------*/
78  const double & Px,
79  const double & Py,
80  const double & x1,
81  const double & y1,
82  const double & x2,
83  const double & y2,
84  double &out_x,
85  double &out_y)
86 {
87  if (x1==x2 && y1==y2)
88  {
89  out_x = x1;
90  out_y = y1;
91  }
92  else
93  {
94  double Dx = x2 - x1;
95  double Dy = y2 - y1;
96  double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
97
98  out_x = x1 + (Ratio * Dx);
99  out_y = y1 + (Ratio * Dy);
100  }
101 }
102
103 /*---------------------------------------------------------------
104  Returns the sq. distance to closest point to a line
105  ---------------------------------------------------------------*/
107  const double & Px,
108  const double & Py,
109  const double & x1,
110  const double & y1,
111  const double & x2,
112  const double & y2 )
113 {
114  if (x1==x2 && y1==y2)
115  {
116  return square( Px-x1 ) + square( Py-y1 );
117  }
118  else
119  {
120  double Dx = x2 - x1;
121  double Dy = y2 - y1;
122  double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
123
124  return square( x1 + (Ratio * Dx) - Px ) + square( y1 + (Ratio * Dy) - Py );
125  }
126 }
127
128
129
130 /*---------------------------------------------------------------
131  Intersect
132  ---------------------------------------------------------------*/
134  const double x1,const double y1,
135  const double x2,const double y2,
136  const double x3,const double y3,
137  const double x4,const double y4,
138  double &ix,double &iy)
139 {
140  double UpperX,UpperY,LowerX,LowerY,Ax,Bx,Cx,Ay,By,Cy,d,f,e,Ratio;
141
142  Ax = x2 - x1;
143  Bx = x3 - x4;
144
145  if (Ax < 0)
146  {
147  LowerX = x2;
148  UpperX = x1;
149  }
150  else
151  {
152  UpperX = x2;
153  LowerX = x1;
154  }
155
156  if (Bx > 0)
157  {
158  if (UpperX < x4 || x3 < LowerX) return false;
159  }
160  else if (UpperX < x3 || x4 < LowerX) return false;
161
162  Ay = y2 - y1;
163  By = y3 - y4;
164
165  if (Ay < 0)
166  {
167  LowerY = y2;
168  UpperY = y1;
169  }
170  else
171  {
172  UpperY = y2;
173  LowerY = y1;
174  }
175
176  if (By > 0)
177  {
178  if (UpperY < y4 || y3 < LowerY) return false;
179  }
180  else if (UpperY < y3 || y4 < LowerY) return false;
181
182  Cx = x1 - x3;
183  Cy = y1 - y3;
184  d = (By * Cx) - (Bx * Cy);
185  f = (Ay * Bx) - (Ax * By);
186
187  if (f > 0)
188  {
189  if (d < 0 || d > f) return false;
190  }
191  else if (d > 0 || d < f) return false;
192
193  e = (Ax * Cy) - (Ay * Cx);
194
195  if (f > 0)
196  {
197  if (e < 0 || e > f) return false;
198  }
199  else if (e > 0 || e < f) return false;
200
201  Ratio = (Ax * -By) - (Ay * -Bx);
202
203  if (Ratio!=0)
204  {
205  Ratio = ((Cy * -Bx) - (Cx * -By)) / Ratio;
206  ix = x1 + (Ratio * Ax);
207  iy = y1 + (Ratio * Ay);
208  }
209  else
210  {
211  if ( (Ax * -Cy)==(-Cx * Ay) )
212  {
213  ix = x3;
214  iy = y3;
215  }
216  else
217  {
218  ix = x4;
219  iy = y4;
220  }
221  }
222  return true;
223 }
224
225 /*---------------------------------------------------------------
226  Intersect
227  ---------------------------------------------------------------*/
229  const double x1,const double y1,
230  const double x2,const double y2,
231  const double x3,const double y3,
232  const double x4,const double y4,
233  float &ix,float &iy)
234 {
235  double x,y;
236  bool b = SegmentsIntersection(x1,y1,x2,y2,x3,y3,x4,y4,x,y);
237  ix = static_cast<float>(x);
238  iy = static_cast<float>(y);
239  return b;
240 }
241
242
243 /*---------------------------------------------------------------
244  Intersect
245  ---------------------------------------------------------------*/
246 bool math::pointIntoPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys )
247 {
248  unsigned int i,j;
249  bool res = false;
250
251  if (polyEdges<3) return res;
252
253  j = polyEdges - 1;
254
255  for (i=0;i<polyEdges;i++)
256  {
257  if ((poly_ys[i] <= py && py < poly_ys[j]) || // an upward crossing
258  (poly_ys[j] <= py && py < poly_ys[i]) ) // a downward crossing
259  {
260  // compute the edge-ray intersect @ the x-coordinate
261  if (px - poly_xs[i]<((poly_xs[j] - poly_xs[i]) * (py - poly_ys[i]) / (poly_ys[j] - poly_ys[i]) ))
262  res =! res;
263  }
264  j = i;
265  }
266
267  return res;
268 }
269
270 /*---------------------------------------------------------------
271  Intersect
272  ---------------------------------------------------------------*/
273 double math::distancePointToPolygon2D(const double & px, const double & py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys )
274 {
275  unsigned int i,j;
276  double minDist = 1e20f;
277
278  // Is the point INTO?
279  if (pointIntoPolygon2D(px,py,polyEdges,poly_xs,poly_ys))
280  return 0;
281
282  // Compute the closest distance from the point to any segment:
283  j = polyEdges - 1;
284
285  for (i=0;i<polyEdges;i++)
286  {
287  // segment: [j]-[i]
288  // ----------------------
289  double closestX,closestY;
290  double d = minimumDistanceFromPointToSegment(px,py, poly_xs[j],poly_ys[j], poly_xs[i],poly_ys[i],closestX,closestY);
291
292  minDist = min(d,minDist);
293
294  // For next iter:
295  j = i;
296  }
297
298  return minDist;
299 }
300
301 /*---------------------------------------------------------------
302  minDistBetweenLines
303  --------------------------------------------------------------- */
305  const double & p1_x, const double & p1_y, const double & p1_z,
306  const double & p2_x, const double & p2_y, const double & p2_z,
307  const double & p3_x, const double & p3_y, const double & p3_z,
308  const double & p4_x, const double & p4_y, const double & p4_z,
309  double &x, double &y, double &z,
310  double &dist)
311 {
312  const double EPS = 1e-30f;
313
314  double p13_x,p13_y,p13_z;
315  double p43_x,p43_y,p43_z;
316  double p21_x,p21_y,p21_z;
317
318  double d1343,d4321,d1321,d4343,d2121;
319  double numer,denom;
320
321  p13_x = p1_x - p3_x;
322  p13_y = p1_y - p3_y;
323  p13_z = p1_z - p3_z;
324
325  p43_x = p4_x - p3_x;
326  p43_y = p4_y - p3_y;
327  p43_z = p4_z - p3_z;
328
329  if (fabs(p43_x) < EPS && fabs(p43_y) < EPS && fabs(p43_z) < EPS) return false;
330
331  p21_x = p2_x - p1_x;
332  p21_y = p2_y - p1_y;
333  p21_z = p2_z - p1_z;
334  if (fabs(p21_x) < EPS && fabs(p21_y) < EPS && fabs(p21_z) < EPS) return false;
335
336  d1343 = p13_x * p43_x + p13_y * p43_y + p13_z * p43_z;
337  d4321 = p43_x * p21_x + p43_y * p21_y + p43_z * p21_z;
338  d1321 = p13_x * p21_x + p13_y * p21_y + p13_z * p21_z;
339  d4343 = p43_x * p43_x + p43_y * p43_y + p43_z * p43_z;
340  d2121 = p21_x * p21_x + p21_y * p21_y + p21_z * p21_z;
341
342  denom = d2121 * d4343 - d4321 * d4321;
343  if (fabs(denom) < EPS) return false;
344
345  numer = d1343 * d4321 - d1321 * d4343;
346
347  double mua = numer / denom;
348  double mub = (d1343 + d4321 * mua) / d4343;
349  double pa_x, pa_y, pa_z;
350  double pb_x, pb_y, pb_z;
351
352  pa_x = p1_x + mua * p21_x;
353  pa_y = p1_y + mua * p21_y;
354  pa_z = p1_z + mua * p21_z;
355
356  pb_x = p3_x + mub * p43_x;
357  pb_y = p3_y + mub * p43_y;
358  pb_z = p3_z + mub * p43_z;
359
360  dist = (double) sqrt( square( pa_x - pb_x ) + square( pa_y - pb_y ) + square( pa_z - pb_z ) );
361
362  // the mid point:
363  x = 0.5*(pa_x + pb_x);
364  y = 0.5*(pa_y + pb_y);
365  z = 0.5*(pa_z + pb_z);
366
367  return true;
368 }
369
370
371 /*---------------------------------------------------------------
372  Rectangles Intersect
373  ---------------------------------------------------------------*/
375  const double & R1_x_min, const double & R1_x_max,
376  const double & R1_y_min, const double & R1_y_max,
377  const double & R2_x_min, const double & R2_x_max,
378  const double & R2_y_min, const double & R2_y_max,
379  const double & R2_pose_x,
380  const double & R2_pose_y,
381  const double & R2_pose_phi )
382 {
383  // Compute the rotated R2:
384  // ----------------------------------------
385  CVectorDouble xs(4),ys(4);
386  double ccos = cos(R2_pose_phi);
387  double ssin = sin(R2_pose_phi);
388
389  xs[0] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_min;
390  ys[0] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_min;
391
392  xs[1] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_min;
393  ys[1] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_min;
394
395  xs[2] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_max;
396  ys[2] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_max;
397
398  xs[3] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_max;
399  ys[3] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_max;
400
401  // Test for one vertice being inside the other rectangle:
402  // -------------------------------------------------------
403  if ( R1_x_min<=xs[0] && xs[0]<=R1_x_max && R1_y_min<=ys[0] && ys[0]<=R1_y_max) return true;
404  if ( R1_x_min<=xs[1] && xs[1]<=R1_x_max && R1_y_min<=ys[1] && ys[1]<=R1_y_max) return true;
405  if ( R1_x_min<=xs[2] && xs[2]<=R1_x_max && R1_y_min<=ys[2] && ys[2]<=R1_y_max) return true;
406  if ( R1_x_min<=xs[3] && xs[3]<=R1_x_max && R1_y_min<=ys[3] && ys[3]<=R1_y_max) return true;
407
408  CPolygon poly;
413
414  if (poly.PointIntoPolygon( R1_x_min, R1_y_min )) return true;
415  if (poly.PointIntoPolygon( R1_x_max, R1_y_min )) return true;
416  if (poly.PointIntoPolygon( R1_x_max, R1_y_max )) return true;
417  if (poly.PointIntoPolygon( R1_x_min, R1_y_max )) return true;
418
419
420  // Test for intersections:
421  // ----------------------------------------
422  double ix,iy;
423
424  for (int idx=0;idx<4;idx++)
425  {
426  if ( math::SegmentsIntersection( R1_x_min,R1_y_min, R1_x_max,R1_y_min, xs[idx],ys[idx], xs[(idx+1)%4],ys[(idx+1)%4], ix,iy) ) return true;
427  if ( math::SegmentsIntersection( R1_x_max,R1_y_min, R1_x_max,R1_y_max, xs[idx],ys[idx], xs[(idx+1)%4],ys[(idx+1)%4], ix,iy) ) return true;
428  if ( math::SegmentsIntersection( R1_x_max,R1_y_max, R1_x_min,R1_y_max, xs[idx],ys[idx], xs[(idx+1)%4],ys[(idx+1)%4], ix,iy) ) return true;
429  if ( math::SegmentsIntersection( R1_x_min,R1_y_max, R1_x_min,R1_y_min, xs[idx],ys[idx], xs[(idx+1)%4],ys[(idx+1)%4], ix,iy) ) return true;
430  }
431
432  // No intersections:
433  return false;
434 }
435
436 //Auxiliary functions needed to avoid code repetition and unnecesary recalculations
437 template<class T2D,class U2D,class T3D,class U3D> bool intersectInCommonPlane(const T3D &o1,const U3D &o2,const mrpt::math::TPlane &p,mrpt::math::TObject3D &obj) {
438  T3D proj1;
439  U3D proj2;
440  //Project into 3D plane, ignoring Z coordinate.
441  CPose3D pose;
442  TPlane(p).getAsPose3D(pose);
443  CPose3D poseNeg=CPose3D(0,0,0,0,0,0)-pose;
444  project3D(o1,poseNeg,proj1);
445  project3D(o2,poseNeg,proj2);
446  T2D proj1_2D;
447  U2D proj2_2D;
448  proj1.generate2DObject(proj1_2D);
449  proj2.generate2DObject(proj2_2D);
450  //Compute easier intersection in 2D space
451  TObject2D obj2D;
452  if (intersect(proj1_2D,proj2_2D,obj2D)) {
453  TObject3D tmp;
454  obj2D.generate3DObject(tmp);
455  //Undo projection
456  project3D(tmp,pose,obj);
457  return true;
458  } else return false;
459 }
461  //Move in a free coordinate, searching for minima and maxima.
462  size_t i1=0;
463  while (abs(lin.director[i1])<geometryEpsilon) i1++;
464  TSegment3D s11=(s1[0][i1]>s1[1][i1])?TSegment3D(s1[1],s1[0]):s1;
465  TSegment3D s21=(s2[0][i1]>s2[1][i1])?TSegment3D(s2[1],s2[0]):s2;
466  TPoint3D pMin=((s11[0][i1]<s21[0][i1])?s21:s11)[0];
467  TPoint3D pMax=((s11[1][i1]<s21[1][i1])?s11:s21)[1];
468  if (abs(pMax[i1]-pMin[i1])<geometryEpsilon) { //Intersection is a point
469  obj=pMax;
470  return true;
471  } else if (pMax[i1]<pMin[i1]) return false; //No intersection
472  else {
473  obj=TSegment3D(pMin,pMax); //Intersection is a segment
474  return true;
475  }
476 }
477 bool intersectInCommonLine(const TSegment2D &s1,const TSegment2D &s2,const TLine2D &lin,TObject2D &obj) {
478  //Move in a free coordinate, searching for minima and maxima
479  size_t i1=(abs(lin.coefs[0])>=geometryEpsilon)?1:0;
480  TSegment2D s11=(s1[0][i1]>s1[1][i1])?TSegment2D(s1[1],s1[0]):s1;
481  TSegment2D s21=(s2[0][i1]>s2[1][i1])?TSegment2D(s2[1],s2[0]):s2;
482  TPoint2D pMin=((s11[0][i1]<s21[0][i1])?s21:s11)[0];
483  TPoint2D pMax=((s11[1][i1]<s21[1][i1])?s11:s21)[1];
484  if (abs(pMax[i1]-pMin[i1])<geometryEpsilon) { //Intersection is a point
485  obj=pMax;
486  return true;
487  } else if (pMax[i1]<pMin[i1]) return false; //No intersection
488  else {
489  obj=TSegment2D(pMin,pMax); //Intersection is a segment
490  return true;
491  }
492 }
493 inline void unsafeProjectPoint(const TPoint3D &point,const CPose3D &pose,TPoint2D &newPoint) {
494  double dummy;
495  pose.composePoint(point.x,point.y,point.z,newPoint.x,newPoint.y,dummy);
496 }
497 void unsafeProjectPolygon(const TPolygon3D &poly,const CPose3D &pose,TPolygon2D &newPoly) {
498  size_t N=poly.size();
499  newPoly.resize(N);
500  for (size_t i=0;i<N;i++) unsafeProjectPoint(poly[i],pose,newPoly[i]);
501 }
502 bool intersect(const TPolygonWithPlane &p1,const TLine3D &l2,double &d,double bestKnown) {
503  //LINE MUST BE UNITARY
504  TObject3D obj;
505  TPoint3D p;
506  if (intersect(p1.plane,l2,obj)) if (obj.getPoint(p)) {
507  for (size_t i=0;i<3;i++) if (abs(l2.director[i])>geometryEpsilon) {
508  d=(p[i]-l2.pBase[i])/l2.director[i];
509  break;
510  }
511  if (d<0||d>bestKnown) return false;
512  TPolygon2D newPoly;
513  TPoint2D newP;
515  unsafeProjectPolygon(p1.poly,p1.inversePose,newPoly);
516  return newPoly.contains(newP);
517  }
518  return false;
519 }
521  if (!intersect(p1.plane,p2.plane,obj)) return false;
522  TLine3D lin3D;
523  TObject3D aux;
524  if (obj.getLine(lin3D)) {
525  TLine3D lin3D1,lin3D2;
526  TLine2D lin2D1,lin2D2;
527  TObject2D obj2D1,obj2D2;
528  project3D(lin3D,p1.inversePose,lin3D1);
529  project3D(lin3D,p2.inversePose,lin3D2);
530  lin3D1.generate2DObject(lin2D1);
531  lin3D2.generate2DObject(lin2D2);
532  if (intersect(p1.poly2D,lin2D1,obj2D1)&&intersect(p2.poly2D,lin2D2,obj2D2)) {
533  TObject3D obj3D1,obj3D2,obj3Dp1,obj3Dp2;
534  obj2D1.generate3DObject(obj3D1);
535  obj2D2.generate3DObject(obj3D2);
536  project3D(obj3D1,p1.pose,obj3Dp1);
537  project3D(obj3D2,p2.pose,obj3Dp2);
538  TPoint3D po1,po2;
539  TSegment3D s1,s2;
540  if (obj3D1.getPoint(po1)) s1=TSegment3D(po1,po1);
541  else obj3D1.getSegment(s1);
542  if (obj3D2.getPoint(po2)) s2=TSegment3D(po2,po2);
543  else obj3D2.getSegment(s2);
544  return intersectInCommonLine(s1,s2,lin3D,obj);
545  } else return false;
546  } else {
547  TObject2D obj2D;
548  if (intersect(p1.poly2D,p2.poly2D,obj2D)) {
549  obj2D.generate3DObject(aux);
550  project3D(aux,p1.pose,obj);
551  return true;
552  } else return false;
553  }
554 }
555 //End of auxiliary methods
559  inversePose=-pose;
561 }
562 void math::TPolygonWithPlane::getPlanes(const vector<TPolygon3D> &oldPolys,vector<TPolygonWithPlane> &newPolys) {
563  size_t N=oldPolys.size();
564  newPolys.resize(N);
565  for (size_t i=0;i<N;i++) newPolys[i]=oldPolys[i];
566 }
567
568 bool math::intersect(const TSegment3D &s1,const TSegment3D &s2,TObject3D &obj) {
569  TObject3D irr;
570  TLine3D l=TLine3D(s1);
571  if (!intersect(l,TLine3D(s2),irr)) return false;
572  if (irr.isPoint()) {
573  //Both lines cross in a point.
574  TPoint3D p;
575  irr.getPoint(p);
576  if (s1.contains(p)&&s2.contains(p)) {
577  obj=p;
578  return true;
579  } else return false;
580  } else return intersectInCommonLine(s1,s2,l,obj);
581 }
582
583 bool math::intersect(const TSegment3D &s1,const TPlane &p1,TObject3D &obj) {
584  if (!intersect(TLine3D(s1),p1,obj)) return false;
585  if (obj.isLine()) {
586  //Segment is fully inside the plane, so it is the return value.
587  obj=s1;
588  return true;
589  } else {
590  //Segment's line intersects the plane in a point. This may be or not be part of the segment.
591  TPoint3D p;
592  if (!obj.getPoint(p)) return false;
593  return s1.contains(p);
594  }
595 }
596
597 bool math::intersect(const TSegment3D &s1,const TLine3D &r1,TObject3D &obj) {
598  if (!intersect(TLine3D(s1),r1,obj)) return false;
599  if (obj.isLine()) {
600  //Segment's line is the other line.
601  obj=s1;
602  return true;
603  } else {
604  //Segment's line and the other line cross in a point, which may be or not be inside the segment.
605  TPoint3D p;
606  if (!obj.getPoint(p)) return false;
607  return s1.contains(p);
608  }
609 }
610
611 bool math::intersect(const TPlane &p1,const TPlane &p2,TObject3D &obj) {
612  TLine3D lin;
613  crossProduct3D(p1.coefs,p2.coefs,lin.director);
614  if ((abs(lin.director[0])<geometryEpsilon)&&(abs(lin.director[1])<geometryEpsilon)&&(abs(lin.director[2])<geometryEpsilon)) {
615  //Planes are parallel
616  for (size_t i=0;i<3;i++) if (abs(p1.coefs[i]*p2.coefs[3]-p1.coefs[3]*p2.coefs[i])>=geometryEpsilon) return false;
617  //Planes are the same
618  obj=p1;
619  return true;
620  } else {
621  //Planes cross in a line whose director vector is already calculated (normal to both planes' normal).
622  //The following process manages to create a random point in the line without loss of generality and almost without conditional sentences.
623  size_t i1=0;
624  while (abs(lin.director[i1])<geometryEpsilon) i1++;
625  //At this point, i1 points to a coordinate (0->x, 1->y, 2->z) in which we can move freely.
626  //If we arbitrarily assign this coordinate to 0, we'll find a suitable base point by solving both planes' equations.
627  size_t c1=(i1+1)%3,c2=(i1+2)%3;
628  lin.pBase[i1]=0.0;
629  lin.pBase[c1]=(p2.coefs[3]*p1.coefs[c2]-p1.coefs[3]*p2.coefs[c2])/lin.director[i1];
630  lin.pBase[c2]=(p2.coefs[c1]*p1.coefs[3]-p1.coefs[c1]*p2.coefs[3])/lin.director[i1];
631  lin.unitarize();
632  obj=lin;
633  return true;
634  }
635 }
636
637 bool math::intersect(const TPlane &p1,const TLine3D &r2,TObject3D &obj) {
638  //double n=p1.coefs[0]*r2.director[0]+p1.coefs[1]*r2.director[1]+p1.coefs[2]*r2.director[2];
639  double n=dotProduct<3,double>(p1.coefs,r2.director);
640  double e=p1.evaluatePoint(r2.pBase);
641  if (abs(n)<geometryEpsilon) {
642  //Plane's normal and line's director are orthogonal, so both are parallel
643  if (abs(e)<geometryEpsilon) {
644  //Line is contained in plane.
645  obj=r2;
646  return true;
647  } else return false;
648  } else {
649  //Plane and line cross in a point.
650  double t=e/n;
651  TPoint3D p;
652  p.x=r2.pBase.x-t*r2.director[0];
653  p.y=r2.pBase.y-t*r2.director[1];
654  p.z=r2.pBase.z-t*r2.director[2];
655  obj=p;
656  return true;
657  }
658 }
659
660 bool math::intersect(const TLine3D &r1,const TLine3D &r2,TObject3D &obj) {
661  double u,d[3];
662  TPoint3D p;
663  const static size_t c1[]={1,2,0};
664  const static size_t c2[]={2,0,1};
665  //With indirect memory accesses, almost all the code goes in a single loop.
666  for (size_t i=0;i<3;i++) {
667  double sysDet=-r1.director[c1[i]]*r2.director[c2[i]]+r2.director[c1[i]]*r1.director[c2[i]];
668  if (abs(sysDet)<geometryEpsilon) continue;
669  //We've found a coordinate in which we can solve the associated system
670  d[c1[i]]=r2.pBase[c1[i]]-r1.pBase[c1[i]];
671  d[c2[i]]=r2.pBase[c2[i]]-r1.pBase[c2[i]];
672  u=(r1.director[c1[i]]*d[c2[i]]-r1.director[c2[i]]*d[c1[i]])/sysDet;
673  for (size_t k=0;k<3;k++) p[k]=r2.pBase[k]+u*r2.director[k];
674  if (r1.contains(p)) {
675  obj=p;
676  return true;
677  } else return false;
678  }
679  //Lines are parallel
680  if (r1.contains(r2.pBase)) {
681  //Lines are the same
682  obj=r1;
683  return true;
684  } else return false;
685 }
686
687 bool math::intersect(const TLine2D &r1,const TLine2D &r2,TObject2D &obj) {
688  double sysDet=r1.coefs[0]*r2.coefs[1]-r1.coefs[1]*r2.coefs[0];
689  if (abs(sysDet)>=geometryEpsilon) {
690  //Resulting point comes simply from solving an equation.
691  TPoint2D p;
692  p.x=(r1.coefs[1]*r2.coefs[2]-r1.coefs[2]*r2.coefs[1])/sysDet;
693  p.y=(r1.coefs[2]*r2.coefs[0]-r1.coefs[0]*r2.coefs[2])/sysDet;
694  obj=p;
695  return true;
696  } else {
697  //Lines are parallel
698  if (abs(r1.coefs[0]*r2.coefs[2]-r1.coefs[2]*r2.coefs[0])>=geometryEpsilon) return false;
699  if (abs(r1.coefs[1]*r2.coefs[2]-r1.coefs[2]*r2.coefs[1])>=geometryEpsilon) return false;
700  //Lines are the same
701  obj=r1;
702  return true;
703  }
704 }
705
706 bool math::intersect(const TLine2D &r1,const TSegment2D &s2,TObject2D &obj) {
707  if (!intersect(r1,TLine2D(s2),obj)) return false;
708  TPoint2D p;
709  if (obj.isLine()) {
710  //Segment is inside the line
711  obj=s2;
712  return true;
713  } else if (obj.getPoint(p)) return s2.contains(p); //Both lines cross in a point.
714  return false;
715 }
716
717 bool math::intersect(const TSegment2D &s1,const TSegment2D &s2,TObject2D &obj) {
718  TLine2D lin=TLine2D(s1);
719  if (!intersect(lin,TLine2D(s2),obj)) return false;
720  TPoint2D p;
721  if (obj.isLine()) return intersectInCommonLine(s1,s2,lin,obj); //Segments' lines are parallel
722  else if (obj.getPoint(p)) return s1.contains(p)&&s2.contains(p); //Segments' lines cross in a point
723  return false;
724 }
725
726 double math::getAngle(const TPlane &s1,const TPlane &s2) {
727  double c=0,n1=0,n2=0;
728  for (size_t i=0;i<3;i++) {
729  c+=s1.coefs[i]*s2.coefs[i];
730  n1+=s1.coefs[i]*s1.coefs[i];
731  n2+=s2.coefs[i]+s2.coefs[i];
732  }
733  double s=sqrt(n1*n2);
734  if (s<geometryEpsilon) throw std::logic_error("Invalid plane(s)");
735  if (abs(s)<abs(c)) return (c/s<0)?M_PI:0;
736  else return acos(c/s);
737 }
738
739 double math::getAngle(const TPlane &s1,const TLine3D &r2) {
740  double c=0,n1=0,n2=0;
741  for (size_t i=0;i<3;i++) {
742  c+=s1.coefs[i]*r2.director[i];
743  n1+=s1.coefs[i]*s1.coefs[i];
744  n2+=r2.director[i]*r2.director[i];
745  }
746  double s=sqrt(n1*n2);
747  if (s<geometryEpsilon) throw std::logic_error("Invalid plane or line");
748  if (abs(s)<abs(c)) return M_PI*sign(c/s)/2;
749  else return asin(c/s);
750 }
751
752 double math::getAngle(const TLine3D &r1,const TLine3D &r2) {
753  double c=0,n1=0,n2=0;
754  for (size_t i=0;i<3;i++) {
755  c+=r1.director[i]*r2.director[i];
756  n1+=r1.director[i]*r1.director[i];
757  n2+=r2.director[i]*r2.director[i];
758  }
759  double s=sqrt(n1*n2);
760  if (s<geometryEpsilon) throw std::logic_error("Invalid line(s)");
761  if (abs(s)<abs(c)) return (c/s<0)?M_PI:0;
762  else return acos(c/s);
763 }
764
765 double math::getAngle(const TLine2D &r1,const TLine2D &r2) {
766  const double ang1 = std::atan2(-r1.coefs[0], r1.coefs[1]);
767  const double ang2 = std::atan2(-r2.coefs[0], r2.coefs[1]);
768  return mrpt::math::wrapToPi(ang2 - ang1);
769 }
770
771 //Auxiliary method
772 void createFromPoseAndAxis(const CPose3D &p,TLine3D &r,size_t axis) {
773  CMatrixDouble44 m = p.getHomogeneousMatrixVal();
774  for (size_t i=0;i<3;i++) {
775  r.pBase[i]=m.get_unsafe(i,3);
776  r.director[i]=m.get_unsafe(i,axis);
777  }
778 }
779 //End of auxiliary method
780
783 }
784
787 }
788
791 }
792
793 void math::createFromPoseAndVector(const CPose3D &p,const double (&vector)[3],TLine3D &r) {
794  CMatrixDouble44 m=p.getHomogeneousMatrixVal();
795  for (size_t i=0;i<3;i++) {
796  r.pBase[i]=m.get_unsafe(i,3);
797  r.director[i]=0;
798  for (size_t j=0;j<3;j++) r.director[i]+=m.get_unsafe(i,j)*vector[j];
799  }
800 }
801
803  r.coefs[0]=cos(p.phi);
804  r.coefs[1]=-sin(p.phi);
805  r.coefs[2]=-r.coefs[0]*p.x-r.coefs[1]*p.y;
806 }
807
809  r.coefs[0]=sin(p.phi);
810  r.coefs[1]=cos(p.phi);
811  r.coefs[2]=-r.coefs[0]*p.x-r.coefs[1]*p.y;
812 }
813
814 void math::createFromPoseAndVector(const TPose2D &p,const double (&vector)[2],TLine2D &r) {
815  double c=cos(p.phi);
816  double s=sin(p.phi);
817  r.coefs[0]=vector[0]*c+vector[1]*s;
818  r.coefs[1]=-vector[0]*s+vector[1]*c;
819  r.coefs[2]=-r.coefs[0]*p.x-r.coefs[1]*p.y;
820 }
821
822 bool math::conformAPlane(const std::vector<TPoint3D> &points) {
823  size_t N=points.size();
824  if (N<3) return false;
826  const TPoint3D &orig=points[N-1];
827  for (size_t i=0;i<N-1;i++) {
828  const TPoint3D &p=points[i];
829  mat(i,0)=p.x-orig.x;
830  mat(i,1)=p.y-orig.y;
831  mat(i,2)=p.z-orig.z;
832  }
833  return mat.rank(geometryEpsilon)==2;
834 }
835
836 bool math::conformAPlane(const std::vector<TPoint3D> &points,TPlane &p) {
838 }
839
840 bool math::areAligned(const std::vector<TPoint2D> &points) {
841  size_t N=points.size();
842  if (N<2) return false;
844  const TPoint2D &orig=points[N-1];
845  for (size_t i=0;i<N-1;i++) {
846  const TPoint2D &p=points[i];
847  mat(i,0)=p.x-orig.x;
848  mat(i,1)=p.y-orig.y;
849  }
850  return mat.rank(geometryEpsilon)==1;
851 }
852
853 bool math::areAligned(const std::vector<TPoint2D> &points,TLine2D &r) {
854  if (!areAligned(points)) return false;
855  const TPoint2D &p0=points[0];
856  for (size_t i=1;;i++) try {
857  r=TLine2D(p0,points[i]);
858  return true;
859  } catch (logic_error &) {}
860 }
861
862 bool math::areAligned(const std::vector<TPoint3D> &points) {
863  size_t N=points.size();
864  if (N<2) return false;
866  const TPoint3D &orig=points[N-1];
867  for (size_t i=0;i<N-1;i++) {
868  const TPoint3D &p=points[i];
869  mat(i,0)=p.x-orig.x;
870  mat(i,1)=p.y-orig.y;
871  mat(i,2)=p.z-orig.z;
872  }
873  return mat.rank(geometryEpsilon)==1;
874 }
875
876 bool math::areAligned(const std::vector<TPoint3D> &points,TLine3D &r) {
877  if (!areAligned(points)) return false;
878  const TPoint3D &p0=points[0];
879  for (size_t i=1;;i++) try {
880  r=TLine3D(p0,points[i]);
881  return true;
882  } catch (logic_error &) {}
883 }
884
885 void math::project3D(const TLine3D &line,const CPose3D &newXYpose,TLine3D &newLine) {
886  newXYpose.composePoint(line.pBase.x,line.pBase.y,line.pBase.z,newLine.pBase.x,newLine.pBase.y,newLine.pBase.z);
888  for (size_t i=0;i<3;i++) {
889  newLine.director[i]=0;
890  for (size_t j=0;j<3;j++) newLine.director[i]+=mat.get_unsafe(i,j)*line.director[j];
891  }
892  newLine.unitarize();
893 }
894
895 void math::project3D(const TPlane &plane,const CPose3D &newXYpose,TPlane &newPlane) {
897  for (size_t i=0;i<3;i++) {
898  newPlane.coefs[i]=0;
899  for (size_t j=0;j<3;j++) newPlane.coefs[i]+=mat.get_unsafe(i,j)*plane.coefs[j];
900  }
901  //VORSICHT! NO INTENTEN HACER ESTO EN SUS CASAS (nota: comentar sÃ­ o sÃ­, mÃ¡s tarde)
902  //La idea es mantener la distancia al nuevo origen igual a la distancia del punto original antes de proyectar.
903  //newPlane.coefs[3]=plane.evaluatePoint(TPoint3D(CPose3D(0,0,0,0,0,0)-newXYpose))*sqrt((newPlane.coefs[0]*newPlane.coefs[0]+newPlane.coefs[1]*newPlane.coefs[1]+newPlane.coefs[2]*newPlane.coefs[2])/(plane.coefs[0]*plane.coefs[0]+plane.coefs[1]*plane.coefs[1]+plane.coefs[2]*plane.coefs[2]));
904  newPlane.coefs[3]=plane.evaluatePoint(TPoint3D(-newXYpose))*sqrt(squareNorm<3,double>(newPlane.coefs)/squareNorm<3,double>(plane.coefs));
905  newPlane.unitarize();
906 }
907
908 void math::project3D(const TPolygon3D &polygon,const CPose3D &newXYpose,TPolygon3D &newPolygon) {
909  size_t N=polygon.size();
910  newPolygon.resize(N);
911  for (size_t i=0;i<N;i++) project3D(polygon[i],newXYpose,newPolygon[i]);
912 }
913
914 void math::project3D(const TObject3D &object,const CPose3D &newXYpose,TObject3D &newObject) {
915  switch (object.getType()) {
917  {
918  TPoint3D p,p2;
919  object.getPoint(p);
920  project3D(p,newXYpose,p2);
921  newObject=p2;
922  break;
923  }
925  {
926  TSegment3D p,p2;
927  object.getSegment(p);
928  project3D(p,newXYpose,p2);
929  newObject=p2;
930  break;
931  }
932  case GEOMETRIC_TYPE_LINE:
933  {
934  TLine3D p,p2;
935  object.getLine(p);
936  project3D(p,newXYpose,p2);
937  newObject=p2;
938  break;
939  }
941  {
942  TPlane p,p2;
943  object.getPlane(p);
944  project3D(p,newXYpose,p2);
945  newObject=p2;
946  break;
947  }
949  {
950  TPolygon3D p,p2;
951  object.getPolygon(p);
952  project3D(p,newXYpose,p2);
953  newObject=p2;
954  break;
955  }
956  default:
957  newObject=TObject3D();
958  }
959 }
960
961 void math::project2D(const TPoint2D &point,const mrpt::poses::CPose2D &newXpose,TPoint2D &newPoint) {
962  newPoint=newXpose+mrpt::poses::CPoint2D(point);
963 }
964
965 void math::project2D(const TLine2D &line,const CPose2D &newXpose,TLine2D &newLine) {
966  double c=cos(newXpose.phi());
967  double s=sin(newXpose.phi());
968  newLine.coefs[0]=line.coefs[0]*c-line.coefs[1]*s;
969  newLine.coefs[1]=line.coefs[1]*c+line.coefs[0]*s;
970  newLine.coefs[2]=line.coefs[2]-(newLine.coefs[0]*newXpose.x()+newLine.coefs[1]*newXpose.y());
971  return;
972 }
973
974 void math::project2D(const TPolygon2D &line,const CPose2D &newXpose,TPolygon2D &newLine) {
975  size_t N=line.size();
976  newLine.resize(N);
977  for (size_t i=0;i<N;i++) newLine[i]=newXpose+line[i];
978  return;
979 }
980
981 void math::project2D(const TObject2D &obj,const CPose2D &newXpose,TObject2D &newObject) {
982  switch (obj.getType()) {
984  {
985  TPoint2D p,p2;
986  obj.getPoint(p);
987  project2D(p,newXpose,p2);
988  newObject=p2;
989  break;
990  }
992  {
993  TSegment2D p,p2;
994  obj.getSegment(p);
995  project2D(p,newXpose,p2);
996  newObject=p2;
997  break;
998  }
999  case GEOMETRIC_TYPE_LINE:
1000  {
1001  TLine2D p,p2;
1002  obj.getLine(p);
1003  project2D(p,newXpose,p2);
1004  newObject=p2;
1005  break;
1006  }
1008  {
1009  TPolygon2D p,p2;
1010  obj.getPolygon(p);
1011  project2D(p,newXpose,p2);
1012  newObject=p2;
1013  break;
1014  }
1015  default:
1016  newObject=TObject2D();
1017  }
1018 }
1019
1020 bool math::intersect(const TPolygon2D &p1,const TSegment2D &s2,TObject2D &obj) {
1021  TLine2D l2=TLine2D(s2);
1022  if (!intersect(p1,l2,obj)) return false;
1023  TPoint2D p;
1024  TSegment2D s;
1025  if (obj.getPoint(p)) return s2.contains(p);
1026  else if (obj.getSegment(s)) return intersectInCommonLine(s,s2,l2,obj);
1027  return false;
1028 }
1029
1030 bool math::intersect(const TPolygon2D &p1,const TLine2D &r2,TObject2D &obj) {
1031  //Proceeding: project polygon so that the line happens to be y=0 (phi=0).
1032  //Then, search this new polygons for intersections with the X axis (very simple).
1033  if (p1.size()<3) return false;
1034  CPose2D pose,poseNeg;
1035  r2.getAsPose2D(pose);
1036  poseNeg=CPose2D(0,0,0)-pose;
1037  TPolygon2D projPoly;
1038  project2D(p1,poseNeg,projPoly);
1039  size_t N=projPoly.size();
1040  projPoly.push_back(projPoly[0]);
1041  double pre=projPoly[0].y;
1042  vector<TPoint2D> pnts;
1043  pnts.reserve(2);
1044  for (size_t i=1;i<=N;i++) {
1045  double cur=projPoly[i].y;
1046  if (abs(cur)<geometryEpsilon) {
1047  if (abs(pre)<geometryEpsilon) {
1048  pnts.resize(2);
1049  pnts[0]=projPoly[i-1];
1050  pnts[1]=projPoly[i];
1051  break;
1052  } else pnts.push_back(projPoly[i]);
1053  } else if ((abs(pre)>=geometryEpsilon)&&(sign(cur)!=sign(pre))) {
1054  double a=projPoly[i-1].x;
1055  double c=projPoly[i].x;
1056  double x=a-pre*(c-a)/(cur-pre);
1057  pnts.push_back(TPoint2D(x,0));
1058  }
1059  pre=cur;
1060  }
1061  //All results must undo the initial projection
1062  switch (pnts.size()) {
1063  case 0:
1064  return false;
1065  case 1:
1066  {
1067  TPoint2D p;
1068  project2D(pnts[0],pose,p);
1069  obj=p;
1070  return true;
1071  }
1072  case 2:
1073  {
1074  TSegment2D s;
1075  project2D(TSegment2D(pnts[0],pnts[1]),pose,s);
1076  obj=s;
1077  return true;
1078  }
1079  default:
1080  throw std::logic_error("Polygon is not convex");
1081  }
1082 }
1083
1084 //Auxiliary structs and code for 2D polygon intersection
1086  vector<TSegment2D> l1;
1087  vector<TSegment2D> l2;
1088 };
1090  unsigned char type; //0 -> point, 1-> segment, any other-> empty
1091  union {
1094  } data;
1095  void destroy() {
1096  switch (type) {
1097  case 0:
1098  delete data.point;
1099  break;
1100  case 1:
1101  delete data.segment;
1102  break;
1103  }
1104  type=255;
1105  }
1107  data.point=new TPoint2D(p);
1108  }
1110  data.segment=new TSegment2D(s);
1111  }
1113  destroy();
1114  }
1116  if (&r==this) return *this;
1117  destroy();
1118  switch (type=r.type) {
1119  case 0:
1120  data.point=new TPoint2D(*(r.data.point));
1121  break;
1122  case 1:
1123  data.segment=new TSegment2D(*(r.data.segment));
1124  break;
1125  }
1126  return *this;
1127  }
1129  operator=(r);
1130  }
1131 };
1133  unsigned char type; //0->two lists of segments, 1-> common region
1134  union {
1137  } data;
1138  void destroy() {
1139  switch (type) {
1140  case 0:
1141  delete data.segms;
1142  break;
1143  case 1:
1144  delete data.common;
1145  break;
1146  }
1147  type=255;
1148  };
1150  data.segms=new T2ListsOfSegments(segms);
1151  }
1153  data.common=new TCommonRegion(common);
1154  }
1156  destroy();
1157  }
1159  if (&t==this) return *this;
1160  destroy();
1161  switch (type=t.type) {
1162  case 0:
1163  data.segms=new T2ListsOfSegments(*(t.data.segms));
1164  break;
1165  case 1:
1166  data.common=new TCommonRegion(*(t.data.common));
1167  break;
1168  }
1169  return *this;
1170  }
1172  operator=(t);
1173  }
1174 };
1178  explicit TSegmentWithLine(const TSegment2D &s):segment(s) {
1179  line=TLine2D(s[0],s[1]);
1180  }
1181  TSegmentWithLine(const TPoint2D &p1,const TPoint2D &p2):segment(p1,p2) {
1182  line=TLine2D(p1,p2);
1183  }
1185 };
1187  if (!intersect(s1.line,s2.line,obj)) return false;
1188  if (obj.isLine()) return intersectInCommonLine(s1.segment,s2.segment,s1.line,obj);
1189  TPoint2D p;
1190  obj.getPoint(p);
1191  return s1.segment.contains(p)&&s2.segment.contains(p);
1192 }
1193 void getSegmentsWithLine(const TPolygon2D &poly,vector<TSegmentWithLine> &segs) {
1194  size_t N=poly.size();
1195  segs.resize(N);
1196  for (size_t i=0;i<N-1;i++) segs[i]=TSegmentWithLine(poly[i],poly[i+1]);
1197  segs[N-1]=TSegmentWithLine(poly[N-1],poly[0]);
1198 }
1199
1200 inline char fromObject(const TObject2D &obj) {
1201  switch (obj.getType()) {
1202  case GEOMETRIC_TYPE_POINT:return 'P';
1203  case GEOMETRIC_TYPE_SEGMENT:return 'S';
1204  case GEOMETRIC_TYPE_LINE:return 'L';
1205  case GEOMETRIC_TYPE_POLYGON:return 'O';
1206  default:return 'U';
1207  }
1208 }
1209
1210 bool math::intersect(const TPolygon2D &/*p1*/,const TPolygon2D &/*p2*/,TObject2D &/*obj*/) {
1211  THROW_EXCEPTION("TODO");
1212 #if 0
1213  return false; //TODO
1214
1215  CSparseMatrixTemplate<TObject2D> intersections=CSparseMatrixTemplate<TObject2D>(p1.size(),p2.size());
1216  std::vector<TSegmentWithLine> segs1,segs2;
1217  getSegmentsWithLine(p1,segs1);
1218  getSegmentsWithLine(p2,segs2);
1219  unsigned int hmInters=0;
1220  for (size_t i=0;i<segs1.size();i++) {
1221  const TSegmentWithLine &s1=segs1[i];
1222  for (size_t j=0;j<segs2.size();j++) if (intersect(s1,segs2[j],obj)) {
1223  intersections(i,j)=obj;
1224  hmInters++;
1225  }
1226  }
1227  for (size_t i=0;i<intersections.getRowCount();i++) {
1228  for (size_t j=0;j<intersections.getColCount();j++) cout<<fromObject(intersections(i,j));
1229  cout<<'\n';
1230  }
1231  if (hmInters==0) {
1232  if (p1.contains(p2[0])) {
1233  obj=p2;
1234  return true;
1235  } else if (p2.contains(p1[0])) {
1236  obj=p1;
1237  return true;
1238  } else return false;
1239  }
1240  //ESTO ES UNA PESADILLA, HAY CIEN MILLONES DE CASOS DISTINTOS A LA HORA DE RECORRER LAS POSIBILIDADES...
1241  /*
1242  Dividir cada segmento en sus distintas partes segÃºn sus intersecciones, y generar un nuevo polÃ­gono.
1243  Recorrer de segmento en segmento, por cada uno de los dos lados (recorriendo desde un punto comÃºn a otro;
1244  en un polÃ­gono se escoge el camino secuencial directo, mientras que del otro se escoge, de los dos posibles,
1245  el que no se corta con ningÃºn elemento del primero).
1246  Seleccionar, para cada segmento, si estÃ¡ dentro o fuera.
1247  Parece fÃ¡cil, pero es una puta mierda.
1248  TODO: hacer en algÃºn momento de mucho tiempo libre...
1249  */
1250
1251  /* Â¿Seguir? */
1252  return false;
1253 #endif
1254 }
1255
1256 bool math::intersect(const TPolygon3D &p1,const TSegment3D &s2,TObject3D &obj) {
1257  TPlane p;
1258  if (!p1.getPlane(p)) return false;
1259  if (!intersect(p,s2,obj)) return false;
1260  TPoint3D pnt;
1261  TSegment3D sgm;
1262  if (obj.getPoint(pnt)) {
1263  CPose3D pose;
1264  p.getAsPose3DForcingOrigin(p1[0],pose);
1265  CPose3D poseNeg=CPose3D(0,0,0,0,0,0)-pose;
1266  TPolygon3D projPoly;
1267  TPoint3D projPnt;
1268  project3D(p1,poseNeg,projPoly);
1269  project3D(pnt,poseNeg,projPnt);
1270  return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
1271  } else if (obj.getSegment(sgm)) return intersectInCommonPlane<TPolygon2D,TSegment2D>(p1,s2,p,obj);
1272  return false;
1273 }
1274
1275 bool math::intersect(const TPolygon3D &p1,const TLine3D &r2,TObject3D &obj) {
1276  TPlane p;
1277  if (!p1.getPlane(p)) return false;
1278  if (!intersect(p,r2,obj)) return false;
1279  TPoint3D pnt;
1280  if (obj.getPoint(pnt)) {
1281  CPose3D pose;
1282  p.getAsPose3DForcingOrigin(p1[0],pose);
1283  CPose3D poseNeg=CPose3D(0,0,0,0,0,0)-pose;
1284  TPolygon3D projPoly;
1285  TPoint3D projPnt;
1286  project3D(p1,poseNeg,projPoly);
1287  project3D(pnt,poseNeg,projPnt);
1288  return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
1289  } else if (obj.isLine()) return intersectInCommonPlane<TPolygon2D,TLine2D>(p1,r2,p,obj);
1290  return false;
1291 }
1292
1293 bool math::intersect(const TPolygon3D &p1,const TPlane &p2,TObject3D &obj) {
1294  TPlane p;
1295  if (!p1.getPlane(p)) return false;
1296  if (!intersect(p,p2,obj)) return false;
1297  TLine3D ln;
1298  if (obj.isPlane()) {
1299  //Polygon is inside the plane
1300  obj=p1;
1301  return true;
1302  } else if (obj.getLine(ln)) return intersectInCommonPlane<TPolygon2D,TLine2D>(p1,ln,p,obj);
1303  return false;
1304 }
1305
1306 //Auxiliary function2
1307 bool intersectAux(const TPolygon3D &p1,const TPlane &pl1,const TPolygon3D &p2,const TPlane &pl2,TObject3D &obj) {
1308  if (!intersect(pl1,pl2,obj)) return false;
1309  TLine3D ln;
1310  if (obj.isPlane()) return intersectInCommonPlane<TPolygon2D,TPolygon2D>(p1,p2,pl1,obj); //Polygons are on the same plane
1311  else if (obj.getLine(ln)) {
1312  TObject3D obj1,obj2;
1313  if (!intersectInCommonPlane<TPolygon2D,TLine2D>(p1,ln,pl1,obj1)) return false;
1314  if (!intersectInCommonPlane<TPolygon2D,TLine2D>(p2,ln,pl2,obj2)) return false;
1315  TPoint3D po1,po2;
1316  TSegment3D s1,s2;
1317  if (obj1.getPoint(po1)) s1=TSegment3D(po1,po1);
1318  else obj1.getSegment(s1);
1319  if (obj2.getPoint(po2)) s2=TSegment3D(po2,po2);
1320  else obj2.getSegment(s2);
1321  return intersectInCommonLine(s1,s2,ln,obj);
1322  }
1323  return false;
1324 }
1325
1326 bool compatibleBounds(const TPoint3D &min1,const TPoint3D &max1,const TPoint3D &min2,const TPoint3D &max2) {
1327  for (size_t i=0;i<3;i++) if ((min1[i]>max2[i])||(min2[i]>max1[i])) return false;
1328  return true;
1329 }
1330 //End of auxiliary functions
1331
1332 bool math::intersect(const TPolygon3D &p1,const TPolygon3D &p2,TObject3D &obj) {
1333  TPoint3D min1,max1,min2,max2;
1334  getPrismBounds(p1,min1,max1);
1335  getPrismBounds(p2,min2,max2);
1336  if (!compatibleBounds(min1,max1,min2,max2)) return false;
1337  TPlane pl1,pl2;
1338  if (!p1.getPlane(pl1)) return false;
1339  if (!p2.getPlane(pl2)) return false;
1340  return intersectAux(p1,pl1,p2,pl2,obj);
1341 }
1342
1343 //Auxiliary function
1344 inline void getPlanes(const std::vector<TPolygon3D> &polys,std::vector<TPlane> &planes) {
1345  size_t N=polys.size();
1346  planes.resize(N);
1347  for (size_t i=0;i<N;i++) getRegressionPlane(polys[i],planes[i]);
1348 }
1349
1350 //Auxiliary functions
1351 void getMinAndMaxBounds(const std::vector<TPolygon3D> &v1,std::vector<TPoint3D> &minP,std::vector<TPoint3D> &maxP) {
1352  minP.resize(0);
1353  maxP.resize(0);
1354  size_t N=v1.size();
1355  minP.reserve(N);
1356  maxP.reserve(N);
1357  TPoint3D p1,p2;
1358  for (std::vector<TPolygon3D>::const_iterator it=v1.begin();it!=v1.end();++it) {
1359  getPrismBounds(*it,p1,p2);
1360  minP.push_back(p1);
1361  maxP.push_back(p2);
1362  }
1363 }
1364
1365 size_t math::intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,CSparseMatrixTemplate<TObject3D> &objs) {
1366  std::vector<TPlane> w1,w2;
1367  getPlanes(v1,w1);
1368  getPlanes(v2,w2);
1369  std::vector<TPoint3D> minBounds1,maxBounds1,minBounds2,maxBounds2;
1370  getMinAndMaxBounds(v1,minBounds1,maxBounds1);
1371  getMinAndMaxBounds(v2,minBounds2,maxBounds2);
1372  size_t M=v1.size(),N=v2.size();
1373  objs.clear();
1374  objs.resize(M,N);
1375  TObject3D obj;
1376  for (size_t i=0;i<M;i++) for (size_t j=0;j<N;j++) if (!compatibleBounds(minBounds1[i],maxBounds1[i],minBounds2[j],maxBounds2[j])) continue;
1377  else if (intersectAux(v1[i],w1[i],v2[j],w2[j],obj)) objs(i,j)=obj;
1378  return objs.getNonNullElements();
1379 }
1380
1381 size_t math::intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,std::vector<TObject3D> &objs) {
1382  objs.resize(0);
1383  std::vector<TPlane> w1,w2;
1384  getPlanes(v1,w1);
1385  getPlanes(v2,w2);
1386  std::vector<TPoint3D> minBounds1,maxBounds1,minBounds2,maxBounds2;
1387  getMinAndMaxBounds(v1,minBounds1,maxBounds1);
1388  getMinAndMaxBounds(v2,minBounds2,maxBounds2);
1389  TObject3D obj;
1391  std::vector<TPoint3D>::const_iterator itMin1=minBounds1.begin();
1392  std::vector<TPoint3D>::const_iterator itMax1=maxBounds1.begin();
1393  for (std::vector<TPolygon3D>::const_iterator it1=v1.begin();it1!=v1.end();++it1,++itP1,++itMin1,++itMax1) {
1394  const TPolygon3D &poly1=*it1;
1395  const TPlane &plane1=*itP1;
1397  const TPoint3D &min1=*itMin1,max1=*itMax1;
1398  std::vector<TPoint3D>::const_iterator itMin2=minBounds2.begin();
1399  std::vector<TPoint3D>::const_iterator itMax2=maxBounds2.begin();
1400  for (std::vector<TPolygon3D>::const_iterator it2=v2.begin();it2!=v2.end();++it2,++itP2,++itMin2,++itMax2) if (!compatibleBounds(min1,max1,*itMin2,*itMax2)) continue;
1401  else if (intersectAux(poly1,plane1,*it2,*itP2,obj)) objs.push_back(obj);
1402  }
1403  return objs.size();
1404 }
1405
1406 bool math::intersect(const TObject2D &o1,const TObject2D &o2,TObject2D &obj) {
1407  TPoint2D p1,p2;
1408  TSegment2D s1,s2;
1409  TLine2D l1,l2;
1410  TPolygon2D po1,po2;
1411  if (o1.getPoint(p1)) {
1412  obj=p1;
1413  if (o2.getPoint(p2)) return distance(p1,p2)<geometryEpsilon;
1414  else if (o2.getSegment(s2)) return s2.contains(p1);
1415  else if (o2.getLine(l2)) return l2.contains(p1);
1416  else if (o2.getPolygon(po2)) return po2.contains(p1); //else return false;
1417  } else if (o1.getSegment(s1)) {
1418  if (o2.getPoint(p2)) {
1419  if (s1.contains(p2)) {
1420  obj=p2;
1421  return true;
1422  } //else return false;
1423  } else if (o2.getSegment(s2)) return intersect(s1,s2,obj);
1424  else if (o2.getLine(l2)) return intersect(s1,l2,obj);
1425  else if (o2.getPolygon(po2)) return intersect(s1,po2,obj); //else return false;
1426  } else if (o1.getLine(l1)) {
1427  if (o2.getPoint(p2)) {
1428  if (l1.contains(p2)) {
1429  obj=p2;
1430  return true;
1431  } //else return false;
1432  } else if (o2.getSegment(s2)) return intersect(l1,s2,obj);
1433  else if (o2.getLine(l2)) return intersect(l1,l2,obj);
1434  else if (o2.getPolygon(po2)) return intersect(l1,po2,obj); //else return false;
1435  } else if (o1.getPolygon(po1)) {
1436  if (o2.getPoint(p2)) {
1437  if (po1.contains(p2)) {
1438  obj=p2;
1439  return true;
1440  } //else return false;
1441  } else if (o2.getSegment(s2)) return intersect(po1,s2,obj);
1442  else if (o2.getLine(l2)) return intersect(po1,l2,obj);
1443  else if (o2.getPolygon(po2)) return intersect(po1,po2,obj); //else return false;
1444  } //else return false;
1445  return false;
1446 }
1447
1448 bool math::intersect(const TObject3D &o1,const TObject3D &o2,TObject3D &obj) {
1449  TPoint3D p1,p2;
1450  TSegment3D s1,s2;
1451  TLine3D l1,l2;
1452  TPolygon3D po1,po2;
1453  TPlane pl1,pl2;
1454  if (o1.getPoint(p1)) {
1455  obj=p1;
1456  if (o2.getPoint(p2)) return distance(p1,p2)<geometryEpsilon;
1457  else if (o2.getSegment(s2)) return s2.contains(p1);
1458  else if (o2.getLine(l2)) return l2.contains(p1);
1459  else if (o2.getPolygon(po2)) return po2.contains(p1);
1460  else if (o2.getPlane(pl2)) return pl2.contains(p1); //else return false;
1461  } else if (o1.getSegment(s1)) {
1462  if (o2.getPoint(p2)) {
1463  if (s1.contains(p2)) {
1464  obj=p2;
1465  return true;
1466  } //else return false;
1467  } else if (o2.getSegment(s2)) return intersect(s1,s2,obj);
1468  else if (o2.getLine(l2)) return intersect(s1,l2,obj);
1469  else if (o2.getPolygon(po2)) return intersect(s1,po2,obj);
1470  else if (o2.getPlane(pl2)) return intersect(s1,pl2,obj); //else return false;
1471  } else if (o1.getLine(l1)) {
1472  if (o2.getPoint(p2)) {
1473  if (l1.contains(p2)) {
1474  obj=p2;
1475  return true;
1476  } //else return false;
1477  } else if (o2.getSegment(s2)) return intersect(l1,s2,obj);
1478  else if (o2.getLine(l2)) return intersect(l1,l2,obj);
1479  else if (o2.getPolygon(po2)) return intersect(l1,po2,obj);
1480  else if (o2.getPlane(pl2)) return intersect(l2,pl2,obj); //else return false;
1481  } else if (o1.getPolygon(po1)) {
1482  if (o2.getPoint(p2)) {
1483  if (po1.contains(p2)) {
1484  obj=p2;
1485  return true;
1486  } //else return false;
1487  } else if (o2.getSegment(s2)) return intersect(po1,s2,obj);
1488  else if (o2.getLine(l2)) return intersect(po1,l2,obj);
1489  else if (o2.getPolygon(po2)) return intersect(po1,po2,obj);
1490  else if (o2.getPlane(pl2)) return intersect(po1,pl2,obj); //else return false;
1491  } else if (o1.getPlane(pl1)) {
1492  if (o2.getPoint(p2)) {
1493  if (pl1.contains(p2)) {
1494  obj=p2;
1495  return true;
1496  } //else return false;
1497  } else if (o2.getSegment(s2)) return intersect(pl1,s2,obj);
1498  else if (o2.getLine(l2)) return intersect(pl1,l2,obj);
1499  else if (o2.getPlane(pl2)) return intersect(pl1,pl2,obj); //else return false;
1500  } //else return false;
1501  return false;
1502 }
1503
1504 double math::distance(const TPoint2D &p1,const TPoint2D &p2) {
1505  double dx=p2.x-p1.x;
1506  double dy=p2.y-p1.y;
1507  return sqrt(dx*dx+dy*dy);
1508 }
1509
1510 double math::distance(const TPoint3D &p1,const TPoint3D &p2) {
1511  double dx=p2.x-p1.x;
1512  double dy=p2.y-p1.y;
1513  double dz=p2.z-p1.z;
1514  return sqrt(dx*dx+dy*dy+dz*dz);
1515 }
1516
1517 void math::getRectangleBounds(const std::vector<TPoint2D> &poly,TPoint2D &pMin,TPoint2D &pMax) {
1518  size_t N=poly.size();
1519  if (N<1) throw logic_error("Empty polygon");
1520  pMin=poly[0];
1521  pMax=poly[0];
1522  for (size_t i=1;i<N;i++) {
1523  pMin.x=min(pMin.x,poly[i].x);
1524  pMin.y=min(pMin.y,poly[i].y);
1525  pMax.x=max(pMax.x,poly[i].x);
1526  pMax.y=max(pMax.y,poly[i].y);
1527  }
1528 }
1529
1530 double math::distance(const TLine2D &r1,const TLine2D &r2) {
1531  if (abs(r1.coefs[0]*r2.coefs[1]-r2.coefs[0]*r1.coefs[1])<geometryEpsilon) {
1532  //Lines are parallel
1533  size_t i1=(abs(r1.coefs[0])<geometryEpsilon)?0:1;
1534  TPoint2D p;
1535  p[i1]=0.0;
1536  p[1-i1]=-r1.coefs[2]/r1.coefs[1-i1];
1537  return r2.distance(p);
1538  } else return 0; //Lines cross in some point
1539 }
1540
1541 double math::distance(const TLine3D &r1,const TLine3D &r2) {
1542  if (abs(getAngle(r1,r2))<geometryEpsilon) return r1.distance(r2.pBase); //Lines are parallel
1543  else {
1544  //We build a plane parallel to r2 which contains r1
1545  TPlane p;
1546  crossProduct3D(r1.director,r2.director,p.coefs);
1547  p.coefs[3]=-(p.coefs[0]*r1.pBase[0]+p.coefs[1]*r1.pBase[1]+p.coefs[2]*r1.pBase[2]);
1548  return p.distance(r2.pBase);
1549  }
1550 }
1551
1552 double math::distance(const TPlane &p1,const TPlane &p2) {
1553  if (abs(getAngle(p1,p2))<geometryEpsilon) {
1554  //Planes are parallel
1555  TPoint3D p(0,0,0);
1556  for (size_t i=0;i<3;i++) if (abs(p1.coefs[i])>=geometryEpsilon) {
1557  p[i]=-p1.coefs[3]/p1.coefs[i];
1558  break;
1559  }
1560  return p2.distance(p);
1561  } else return 0; //Planes cross in a line
1562 }
1563
1564 double math::distance(const TPolygon2D &p1,const TPolygon2D &p2) {
1566  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TPolygon2D)");
1567 }
1568
1569 double math::distance(const TPolygon2D &p1,const TSegment2D &s2) {
1571  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TSegment)");
1572 }
1573
1574 double math::distance(const TPolygon2D &p1,const TLine2D &l2) {
1576  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TLine2D)");
1577 }
1578
1579 double math::distance(const TPolygon3D &p1,const TPolygon3D &p2) {
1581  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPolygon3D");
1582 }
1583
1584 double math::distance(const TPolygon3D &p1,const TSegment3D &s2) {
1586  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TSegment3D");
1587 }
1588
1589 double math::distance(const TPolygon3D &p1,const TLine3D &l2) {
1591  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TLine3D");
1592 }
1593
1594 double math::distance(const TPolygon3D &po,const TPlane &pl) {
1596  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPlane");
1597 }
1598
1599 void math::getPrismBounds(const std::vector<TPoint3D> &poly,TPoint3D &pMin,TPoint3D &pMax) {
1600  size_t N=poly.size();
1601  if (N<1) throw logic_error("Empty polygon");
1602  pMin=poly[0];
1603  pMax=poly[0];
1604  for (size_t i=1;i<N;i++) {
1605  pMin.x=min(pMin.x,poly[i].x);
1606  pMin.y=min(pMin.y,poly[i].y);
1607  pMin.z=min(pMin.z,poly[i].z);
1608  pMax.x=max(pMax.x,poly[i].x);
1609  pMax.y=max(pMax.y,poly[i].y);
1610  pMax.z=max(pMax.z,poly[i].z);
1611  }
1612 }
1613
1614 void createPlaneFromPoseAndAxis(const CPose3D &pose,TPlane &plane,size_t axis) {
1615  plane.coefs[3]=0;
1617  for (size_t i=0;i<3;i++) {
1618  plane.coefs[i]=m.get_unsafe(i,axis);
1619  plane.coefs[3]-=plane.coefs[i]*m.get_unsafe(i,3);
1620  }
1621 }
1622
1623 void math::createPlaneFromPoseXY(const CPose3D &pose,TPlane &plane) {
1624  createPlaneFromPoseAndAxis(pose,plane,2);
1625 }
1626
1627 void math::createPlaneFromPoseXZ(const CPose3D &pose,TPlane &plane) {
1628  createPlaneFromPoseAndAxis(pose,plane,1);
1629 }
1630
1631 void math::createPlaneFromPoseYZ(const CPose3D &pose,TPlane &plane) {
1632  createPlaneFromPoseAndAxis(pose,plane,0);
1633 }
1634
1635 void math::createPlaneFromPoseAndNormal(const CPose3D &pose,const double (&normal)[3],TPlane &plane) {
1636  plane.coefs[3]=0;
1638  for (size_t i=0;i<3;i++) {
1639  plane.coefs[i]=0;
1640  for (size_t j=0;j<3;j++) plane.coefs[i]+=normal[j]*m.get_unsafe(i,j);
1641  plane.coefs[3]-=plane.coefs[i]*m.get_unsafe(i,3);
1642  }
1643 }
1644
1646  //Assumes vector is unitary.
1647  //coord: 0=x, 1=y, 2=z.
1648  char coord1=(coord+1)%3;
1649  char coord2=(coord+2)%3;
1650  matrix.setSize(4,4);
1651  matrix(3,3)=1.0;
1652  for (size_t i=0;i<3;i++) matrix.set_unsafe(i,coord,vec[i]);
1653  matrix.set_unsafe(0,coord1,0);
1654  double h=hypot(vec[1],vec[2]);
1655  if (h<geometryEpsilon) {
1656  matrix.set_unsafe(1,coord1,1);
1657  matrix.set_unsafe(2,coord1,0);
1658  } else {
1659  matrix.set_unsafe(1,coord1,-vec[2]/h);
1660  matrix.set_unsafe(2,coord1,vec[1]/h);
1661  }
1662  matrix.set_unsafe(0,coord2,matrix.get_unsafe(1,coord)*matrix.get_unsafe(2,coord1)-matrix.get_unsafe(2,coord)*matrix.get_unsafe(1,coord1));
1663  matrix.set_unsafe(1,coord2,matrix.get_unsafe(2,coord)*matrix.get_unsafe(0,coord1)-matrix.get_unsafe(0,coord)*matrix.get_unsafe(2,coord1));
1664  matrix.set_unsafe(2,coord2,matrix.get_unsafe(0,coord)*matrix.get_unsafe(1,coord1)-matrix.get_unsafe(1,coord)*matrix.get_unsafe(0,coord1));
1665 }
1666
1667 double math::getRegressionLine(const vector<TPoint2D> &points,TLine2D &line) {
1668  CArrayDouble<2> means;
1669  CMatrixTemplateNumeric<double> covars(2,2),eigenVal(2,2),eigenVec(2,2);
1670  covariancesAndMean(points,covars,means);
1671  covars.eigenVectors(eigenVec,eigenVal);
1672  size_t selected=(eigenVal.get_unsafe(0,0)>=eigenVal.get_unsafe(1,1))?0:1;
1673  line.coefs[0]=-eigenVec.get_unsafe(1,selected);
1674  line.coefs[1]=eigenVec.get_unsafe(0,selected);
1675  line.coefs[2]=-line.coefs[0]*means[0]-line.coefs[1]*means[1];
1676  return sqrt(eigenVal.get_unsafe(1-selected,1-selected)/eigenVal.get_unsafe(selected,selected));
1677 }
1678
1679 template<class T>
1680 inline size_t getIndexOfMin(const T &e1,const T &e2,const T &e3) {
1681  return (e1<e2)?((e1<e3)?0:2):((e2<e3)?1:2);
1682 }
1683
1684 template<class T>
1685 inline size_t getIndexOfMax(const T &e1,const T &e2,const T &e3) {
1686  return (e1>e2)?((e1>e3)?0:2):((e2>e3)?1:2);
1687 }
1688
1689 double math::getRegressionLine(const vector<TPoint3D> &points,TLine3D &line) {
1690  CArrayDouble<3> means;
1691  CMatrixTemplateNumeric<double> covars(3,3),eigenVal(3,3),eigenVec(3,3);
1692  covariancesAndMean(points,covars,means);
1693  covars.eigenVectors(eigenVec,eigenVal);
1694  size_t selected=getIndexOfMax(eigenVal.get_unsafe(0,0),eigenVal.get_unsafe(1,1),eigenVal.get_unsafe(2,2));
1695  for (size_t i=0;i<3;i++) {
1696  line.pBase[i]=means[i];
1697  line.director[i]=eigenVec.get_unsafe(i,selected);
1698  }
1699  size_t i1=(selected+1)%3,i2=(selected+2)%3;
1700  return sqrt((eigenVal.get_unsafe(i1,i1)+eigenVal.get_unsafe(i2,i2))/eigenVal.get_unsafe(selected,selected));
1701 }
1702
1703 double math::getRegressionPlane(const vector<TPoint3D> &points,TPlane &plane) {
1704  vector<double> means;
1705  CMatrixDouble33 covars,eigenVal,eigenVec;
1706  covariancesAndMean(points,covars,means);
1707
1708  covars.eigenVectors(eigenVec,eigenVal);
1709  for (size_t i=0;i<3;++i) if (eigenVal.get_unsafe(i,i)<0&&fabs(eigenVal.get_unsafe(i,i))<geometryEpsilon) eigenVal.set_unsafe(i,i,0);
1710  size_t selected=getIndexOfMin(eigenVal.get_unsafe(0,0),eigenVal.get_unsafe(1,1),eigenVal.get_unsafe(2,2));
1711  plane.coefs[3]=0;
1712  for (size_t i=0;i<3;i++) {
1713  plane.coefs[i]=eigenVec.get_unsafe(i,selected);
1714  plane.coefs[3]-=plane.coefs[i]*means[i];
1715  }
1716  size_t i1=(selected+1)%3,i2=(selected+2)%3;
1717  return sqrt(eigenVal.get_unsafe(selected,selected)/(eigenVal.get_unsafe(i1,i1)+eigenVal.get_unsafe(i2,i2)));
1718 }
1719
1720 void math::assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys) {
1721  std::vector<TSegment3D> tmp;
1722  assemblePolygons(segms,polys,tmp);
1723 }
1724
1726  size_t seg1;
1727  size_t seg2;
1728  bool seg1Point; //true for point2, false for point1
1729  bool seg2Point; //same
1731  MatchingVertex(size_t s1,size_t s2,bool s1p,bool s2p):seg1(s1),seg2(s2),seg1Point(s1p),seg2Point(s2p) {}
1732 };
1734 public:
1735  const std::vector<TSegment3D> &segs;
1736  FCreatePolygon(const std::vector<TSegment3D> &s):segs(s) {}
1737  TPolygon3D operator()(const std::vector<MatchingVertex> &vertices) {
1738  TPolygon3D res;
1739  size_t N=vertices.size();
1740  res.reserve(N);
1741  for (std::vector<MatchingVertex>::const_iterator it=vertices.begin();it!=vertices.end();++it) res.push_back(segs[it->seg2][it->seg2Point?1:0]);
1742  return res;
1743  }
1744 };
1745 inline bool firstOrNonPresent(size_t i,const std::vector<MatchingVertex> &v) {
1746  if (v.size()>0&&v[0].seg1==i) return true;
1747  for (std::vector<MatchingVertex>::const_iterator it=v.begin();it!=v.end();++it) if (it->seg1==i||it->seg2==i) return false;
1748  return true;
1749 }
1750 bool depthFirstSearch(const CSparseMatrixTemplate<unsigned char> &mat,std::vector<std::vector<MatchingVertex> > &res,std::vector<bool> &used,size_t searching,unsigned char mask,std::vector<MatchingVertex> &current) {
1751  for (size_t i=0;i<mat.getColCount();i++) if (!used[i]&&mat.isNotNull(searching,i)) {
1753  if (!match) continue;
1754  else if (firstOrNonPresent(i,current)) {
1755  bool s1p,s2p;
1756  if (true==(s1p=(!(match&3)))) match>>=2;
1757  s2p=!(match&1);
1758  if (current.size()>=2&&current[0].seg1==i) {
1759  if (s2p!=current[0].seg1Point) {
1760  current.push_back(MatchingVertex(searching,i,s1p,s2p));
1761  for (std::vector<MatchingVertex>::const_iterator it=current.begin();it!=current.end();++it) used[it->seg2]=true;
1762  res.push_back(current);
1763  return true;
1764  } else continue; //Strange shape... not a polygon, although it'll be without the first segment
1765  } else {
1766  current.push_back(MatchingVertex(searching,i,s1p,s2p));
1767  if (depthFirstSearch(mat,res,used,i,s2p?0x3:0xC,current)) return true;
1768  current.pop_back();
1769  }
1770  }
1771  }
1772  //No match has been found. Backtrack
1773  return false;
1774 }
1775 void depthFirstSearch(const CSparseMatrixTemplate<unsigned char> &mat,std::vector<std::vector<MatchingVertex> >&res,std::vector<bool> &used) {
1776  vector<MatchingVertex> cur;
1777  for (size_t i=0;i<used.size();i++) if (!used[i]) if (depthFirstSearch(mat,res,used,i,0xf,cur)) cur.clear();
1778 }
1779 void math::assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder) {
1780  std::vector<TSegment3D> tmp;
1781  tmp.reserve(segms.size());
1782  for (std::vector<TSegment3D>::const_iterator it=segms.begin();it!=segms.end();++it) if (it->length()>=geometryEpsilon) tmp.push_back(*it);
1783  else remainder.push_back(*it);
1784  size_t N=tmp.size();
1786  for (size_t i=0;i<N-1;i++) for (size_t j=i+1;j<N;j++) {
1787  if (distance(tmp[i].point1,tmp[j].point1)<geometryEpsilon) {
1788  matches(i,j)|=1;
1789  matches(j,i)|=1;
1790  }
1791  if (distance(tmp[i].point1,tmp[j].point2)<geometryEpsilon) {
1792  matches(i,j)|=2;
1793  matches(j,i)|=4;
1794  }
1795  if (distance(tmp[i].point2,tmp[j].point1)<geometryEpsilon) {
1796  matches(i,j)|=4;
1797  matches(j,i)|=2;
1798  }
1799  if (distance(tmp[i].point2,tmp[j].point2)<geometryEpsilon) {
1800  matches(i,j)|=8;
1801  matches(j,i)|=8;
1802  }
1803  }
1804  std::vector<std::vector<MatchingVertex> > results;
1805  std::vector<bool> usedSegments(N,false);
1806  depthFirstSearch(matches,results,usedSegments);
1807  polys.resize(results.size());
1808  transform(results.begin(),results.end(),polys.begin(),FCreatePolygon(segms));
1809  for (size_t i=0;i<N;i++) if (!usedSegments[i]) remainder.push_back(tmp[i]);
1810 }
1811
1812 void math::assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys) {
1813  std::vector<TObject3D> tmp;
1814  std::vector<TSegment3D> sgms;
1815  TObject3D::getPolygons(objs,polys,tmp);
1816  TObject3D::getSegments(tmp,sgms);
1817  assemblePolygons(sgms,polys);
1818 }
1819
1820 void math::assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder) {
1821  std::vector<TObject3D> tmp;
1822  std::vector<TSegment3D> sgms,remainderSgms;
1823  TObject3D::getPolygons(objs,polys,tmp);
1824  TObject3D::getSegments(tmp,sgms,remainder);
1825  assemblePolygons(sgms,polys,remainderSgms);
1826  remainder.insert(remainder.end(),remainderSgms.begin(),remainderSgms.end());
1827 }
1828
1829 void math::assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder1,std::vector<TObject3D> &remainder2) {
1830  std::vector<TObject3D> tmp;
1831  std::vector<TSegment3D> sgms;
1832  TObject3D::getPolygons(objs,polys,tmp);
1833  TObject3D::getSegments(tmp,sgms,remainder2);
1834  assemblePolygons(sgms,polys,remainder1);
1835 }
1836
1837 bool intersect(const TLine2D &l1,const TSegmentWithLine &s2,TObject2D &obj) {
1838  if (intersect(l1,s2.line,obj)) {
1839  if (obj.isLine()) {
1840  obj=s2.segment;
1841  return true;
1842  } else {
1843  TPoint2D p;
1844  obj.getPoint(p);
1845  return s2.segment.contains(p);
1846  }
1847  } else return false;
1848 }
1849
1850 inline bool intersect(const TSegmentWithLine &s1,const TLine2D &l2,TObject2D &obj) {
1851  return intersect(l2,s1,obj);
1852 }
1853
1854 bool math::splitInConvexComponents(const TPolygon2D &poly,vector<TPolygon2D> &components) {
1855  components.clear();
1856  size_t N=poly.size();
1857  if (N<=3) return false;
1858  vector<TSegmentWithLine> segms(N);
1859  for (size_t i=0;i<N-1;i++) segms[i]=TSegmentWithLine(poly[i],poly[i+1]);
1860  segms[N-1]=TSegmentWithLine(poly[N-1],poly[0]);
1861  TObject2D obj;
1862  TPoint2D pnt;
1863  for (size_t i=0;i<N;i++) {
1864  size_t ii=(i+2)%N,i_=(i+N-1)%N;
1865  for (size_t j=ii;j!=i_;j=(j+1)%N) if (intersect(segms[i].line,segms[j],obj)&&obj.getPoint(pnt)) {
1866  TSegmentWithLine sTmp=TSegmentWithLine(pnt,segms[i].segment[(distance(pnt,segms[i].segment.point1)<distance(pnt,segms[i].segment.point2))?0:1]);
1867  bool cross=false;
1868  TPoint2D pTmp;
1869  for (size_t k=0;(k<N)&&!cross;k++) if (intersect(sTmp,segms[k],obj)) {
1870  if (obj.getPoint(pTmp)&&(distance(pTmp,sTmp.segment[0])>=geometryEpsilon)&&(distance(pTmp,sTmp.segment[1])>=geometryEpsilon)) cross=true;
1871  }
1872  if (cross) continue;
1873  //At this point, we have a suitable point, although we must check if the division is right.
1874  //We do this by evaluating, in the expanded segment's line, the next and previous points. If both signs differ, proceed.
1875  if (sign(segms[i].line.evaluatePoint(poly[(i+N-1)%N]))==sign(segms[i].line.evaluatePoint(poly[(i+2)%N]))) continue;
1876  TPolygon2D p1,p2;
1877  if (i>j) {
1878  p1.insert(p1.end(),poly.begin()+i+1,poly.end());
1879  p1.insert(p1.end(),poly.begin(),poly.begin()+j+1);
1880  p2.insert(p2.end(),poly.begin()+j+1,poly.begin()+i+1);
1881  } else {
1882  p1.insert(p1.end(),poly.begin()+i+1,poly.begin()+j+1);
1883  p2.insert(p2.end(),poly.begin()+j+1,poly.end());
1884  p2.insert(p2.end(),poly.begin(),poly.begin()+i+1);
1885  }
1886  if (distance(*(p1.rbegin()),pnt)>=geometryEpsilon) p1.push_back(pnt);
1887  if (distance(*(p2.rbegin()),pnt)>=geometryEpsilon) p2.push_back(pnt);
1890  vector<TPolygon2D> tempComps;
1891  if (splitInConvexComponents(p1,tempComps)) components.insert(components.end(),tempComps.begin(),tempComps.end());
1892  else components.push_back(p1);
1893  if (splitInConvexComponents(p2,tempComps)) components.insert(components.end(),tempComps.begin(),tempComps.end());
1894  else components.push_back(p2);
1895  return true;
1896  }
1897  }
1898  return false;
1899 }
1900
1902 public:
1903  const CPose3D &pose;
1905  FUnprojectPolygon2D(const CPose3D &p):pose(p),tmp1(0),tmp2(0) {}
1907  tmp1=TPolygon3D(poly2D);
1908  project3D(tmp1,pose,tmp2);
1909  return tmp2;
1910  }
1911 };
1912 bool math::splitInConvexComponents(const TPolygon3D &poly,vector<TPolygon3D> &components) {
1913  TPlane p;
1914  if (!poly.getPlane(p)) throw std::logic_error("Polygon is skew");
1915  CPose3D pose1,pose2;
1916  p.getAsPose3DForcingOrigin(poly[0],pose1);
1917  pose2=-pose1;
1918  TPolygon3D polyTmp;
1919  project3D(poly,pose2,polyTmp);
1920  TPolygon2D poly2D=TPolygon2D(polyTmp);
1921  vector<TPolygon2D> components2D;
1922  if (splitInConvexComponents(poly2D,components2D)) {
1923  components.resize(components2D.size());
1924  transform(components2D.begin(),components2D.end(),components.begin(),FUnprojectPolygon2D(pose1));
1925  return true;
1926  } else return false;
1927 }
1928
1930  TPoint2D p;
1931  sgm.getCenter(p);
1932  bis.coefs[0]=sgm.point2.x-sgm.point1.x;
1933  bis.coefs[1]=sgm.point2.y-sgm.point1.y;
1934  bis.coefs[2]=-bis.coefs[0]*p.x-bis.coefs[1]*p.y;
1935  bis.unitarize();
1936 }
1937
1939  TPoint3D p;
1940  sgm.getCenter(p);
1941  bis.coefs[0]=sgm.point2.x-sgm.point1.x;
1942  bis.coefs[1]=sgm.point2.y-sgm.point1.y;
1943  bis.coefs[2]=sgm.point2.z-sgm.point1.z;
1944  bis.coefs[2]=-bis.coefs[0]*p.x-bis.coefs[1]*p.y-bis.coefs[2]*p.z;
1945  bis.unitarize();
1946 }
1947
1948 void math::getAngleBisector(const TLine2D &l1,const TLine2D &l2,TLine2D &bis) {
1949  TPoint2D p;
1950  TObject2D obj;
1951  if (!intersect(l1,l2,obj)) {
1952  //Both lines are parallel
1953  double mod1=sqrt(square(l1.coefs[0])+square(l1.coefs[1]));
1954  double mod2=sqrt(square(l2.coefs[0])+square(l2.coefs[2]));
1955  bis.coefs[0]=l1.coefs[0]/mod1;
1956  bis.coefs[1]=l1.coefs[1]/mod1;
1957  bool sameSign;
1958  if (abs(bis.coefs[0])<geometryEpsilon) sameSign=(l1.coefs[1]*l2.coefs[1])>0;
1959  else sameSign=(l1.coefs[0]*l2.coefs[0])>0;
1960  if (sameSign) bis.coefs[2]=(l1.coefs[2]/mod1)+(l2.coefs[2]/mod2);
1961  else bis.coefs[2]=(l1.coefs[2]/mod1)-(l2.coefs[2]/mod2);
1962  } else if (obj.getPoint(p)) {
1963  //Both lines cross
1964  double ang1=atan2(-l1.coefs[0],l1.coefs[1]);
1965  double ang2=atan2(-l2.coefs[0],l2.coefs[1]);
1966  double ang=(ang1+ang2)/2;
1967  bis.coefs[0]=-sin(ang);
1968  bis.coefs[1]=cos(ang);
1969  bis.coefs[2]=-bis.coefs[0]*p.x-bis.coefs[1]*p.y;
1970  } else {
1971  bis=l1;
1972  bis.unitarize();
1973  }
1974 }
1975
1976 void math::getAngleBisector(const TLine3D &l1,const TLine3D &l2,TLine3D &bis) {
1977  TPlane p=TPlane(l1,l2); //May throw an exception
1978  TLine3D l1P,l2P;
1979  TLine2D bis2D;
1980  CPose3D pose,pose2;
1981  p.getAsPose3D(pose);
1982  pose2=-pose;
1983  project3D(l1,pose2,l1P);
1984  project3D(l2,pose2,l2P);
1985  getAngleBisector(TLine2D(l1P),TLine2D(l2P),bis2D);
1986  project3D(TLine3D(bis2D),pose,bis);
1987 }
1988
1989 bool math::traceRay(const vector<TPolygonWithPlane> &vec,const CPose3D &pose,double &dist) {
1990  dist=HUGE_VAL;
1991  double nDist=0;
1992  TLine3D lin;
1993  createFromPoseX(pose,lin);
1994  lin.unitarize();
1995  bool res=false;
1996  for (vector<TPolygonWithPlane>::const_iterator it=vec.begin();it!=vec.end();++it) if (::intersect(*it,lin,nDist,dist)) {
1997  res=true;
1998  dist=nDist;
1999  }
2000  return res;
2001 }
void getCenter(TPoint2D &p) const
Segment&#39;s central point.
void BASE_IMPEXP createPlaneFromPoseAndNormal(const mrpt::poses::CPose3D &pose, const double(&normal)[3], TPlane &plane)
Given a pose and any vector, creates a plane orthogonal to that vector in the pose&#39;s coordinates...
Definition: geometry.cpp:1635
bool intersectInCommonLine(const mrpt::math::TSegment3D &s1, const mrpt::math::TSegment3D &s2, const mrpt::math::TLine3D &lin, mrpt::math::TObject3D &obj)
Definition: geometry.cpp:460
const unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
bool isNotNull(size_t nRow, size_t nCol) const
Checks whether an element of the matrix is not the default object.
TSegmentWithLine(const TPoint2D &p1, const TPoint2D &p2)
Definition: geometry.cpp:1181
TPolygon3D operator()(const TPolygon2D &poly2D)
Definition: geometry.cpp:1906
Definition: glext.h:3888
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
bool BASE_IMPEXP RectanglesIntersection(const double &R1_x_min, const double &R1_x_max, const double &R1_y_min, const double &R1_y_max, const double &R2_x_min, const double &R2_x_max, const double &R2_y_min, const double &R2_y_max, const double &R2_pose_x, const double &R2_pose_y, const double &R2_pose_phi)
Returns whether two rotated rectangles intersect.
Definition: geometry.cpp:374
double y
X,Y coordinates.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble t
Definition: glext.h:3610
const unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
bool BASE_IMPEXP splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
Definition: geometry.cpp:1854
GLdouble GLdouble z
Definition: glext.h:3734
#define min(a, b)
void BASE_IMPEXP generateAxisBaseFromDirectionAndAxis(const double(&vec)[3], char coord, CMatrixDouble &matrix)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
Definition: geometry.cpp:1645
const unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
bool intersect(const TPolygonWithPlane &p1, const TLine3D &l2, double &d, double bestKnown)
Definition: geometry.cpp:502
void getPlanes(const std::vector< TPolygon3D > &polys, std::vector< TPlane > &planes)
Definition: geometry.cpp:1344
bool firstOrNonPresent(size_t i, const std::vector< MatchingVertex > &v)
Definition: geometry.cpp:1745
double distance(const TPoint3D &point) const
Distance between the line and a point.
void BASE_IMPEXP closestFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a (infinite) line.
Definition: geometry.cpp:77
void BASE_IMPEXP closestFromPointToSegment(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2, double &out_x, double &out_y)
Computes the closest point from a given point to a segment.
Definition: geometry.cpp:33
bool BASE_IMPEXP minDistBetweenLines(const double &p1_x, const double &p1_y, const double &p1_z, const double &p2_x, const double &p2_y, const double &p2_z, const double &p3_x, const double &p3_y, const double &p3_z, const double &p4_x, const double &p4_y, const double &p4_z, double &x, double &y, double &z, double &dist)
Calculates the minimum distance between a pair of lines.
Definition: geometry.cpp:304
GLuint GLenum matrix
Definition: glext.h:6092
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
double BASE_IMPEXP closestSquareDistanceFromPointToLine(const double &Px, const double &Py, const double &x1, const double &y1, const double &x2, const double &y2)
Returns the square distance from a point to a line.
Definition: geometry.cpp:106
bool contains(const TPoint2D &point) const
Check whether a point is inside a segment.
void destroy()
Definition: geometry.cpp:1095
bool BASE_IMPEXP pointIntoPolygon2D(const double &px, const double &py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns true if the 2D point (px,py) falls INTO the given polygon.
Definition: geometry.cpp:246
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
bool BASE_IMPEXP traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:1989
void BASE_IMPEXP assemblePolygons(const std::vector< TSegment3D > &segms, std::vector< TPolygon3D > &polys)
Tries to assemble a set of segments into a set of closed polygons.
Definition: geometry.cpp:1720
void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
Definition: geometry.cpp:1929
char fromObject(const TObject2D &obj)
Definition: geometry.cpp:1200
void createPlaneFromPoseAndAxis(const CPose3D &pose, TPlane &plane, size_t axis)
Definition: geometry.cpp:1614
#define THROW_EXCEPTION(msg)
void unsafeProjectPolygon(const TPolygon3D &poly, const CPose3D &pose, TPolygon2D &newPoly)
Definition: geometry.cpp:497
GLenum GLsizei n
Definition: glext.h:4618
Slightly heavyweight type to speed-up calculations with polygons in 3D.
Definition: geometry.h:33
This file implements several operations that operate element-wise on individual or pairs of container...
vector< TSegment2D > l2
Definition: geometry.cpp:1087
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
size_t getColCount() const
Returns the amount of columns in this matrix.
Standard type for storing any lightweight 2D type.
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:25
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
void BASE_IMPEXP createFromPoseAndVector(const mrpt::poses::CPose3D &p, const double(&vector)[3], TLine3D &r)
Gets a 3D line corresponding to any arbitrary vector, in the base given by the pose.
Definition: geometry.cpp:793
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
bool getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
TPoint3D pBase
Base point.
Standard object for storing any 3D lightweight object.
STL namespace.
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
bool contains(const TPoint2D &point) const
Check whether a point is inside the line.
#define M_PI
Definition: bits.h:78
TSegmentWithLine(const TSegment2D &s)
Definition: geometry.cpp:1178
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void getSegmentsWithLine(const TPolygon2D &poly, vector< TSegmentWithLine > &segs)
Definition: geometry.cpp:1193
bool intersectInCommonPlane(const T3D &o1, const U3D &o2, const mrpt::math::TPlane &p, mrpt::math::TObject3D &obj)
Definition: geometry.cpp:437
double z
X,Y,Z coordinates.
GLenum GLenum GLuint components
Definition: glext.h:6305
double distance(const TPoint3D &point) const
Distance to 3D point.
struct BASE_IMPEXP TObject3D
size_t getIndexOfMin(const T &e1, const T &e2, const T &e3)
Definition: geometry.cpp:1680
GLdouble s
Definition: glext.h:3602
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
TSegment2D segment
Definition: geometry.cpp:1176
void BASE_IMPEXP getAngleBisector(const TLine2D &l1, const TLine2D &l2, TLine2D &bis)
Gets the bisector of two lines or segments (implicit constructor will be used if necessary) ...
Definition: geometry.cpp:1948
GLuint coord
Definition: glext.h:6196
void BASE_IMPEXP project2D(const TPoint2D &point, const mrpt::poses::CPose2D &newXpose, TPoint2D &newPoint)
Uses the given pose 2D to project a point into a new base.
Definition: geometry.cpp:961
struct BASE_IMPEXP TSegment3D
A sparse matrix container (with cells of any type), with iterators.
TTempIntersection(const TCommonRegion &common)
Definition: geometry.cpp:1152
void BASE_IMPEXP createFromPoseY(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Y axis in a given pose.
Definition: geometry.cpp:785
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
Definition: geometry.h:616
TPoint3D point1
Origin point.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
bool BASE_IMPEXP conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
Definition: geometry.cpp:822
GLsizei const GLfloat * points
Definition: glext.h:4797
static void getSegments(const std::vector< TObject3D > &objs, std::vector< TSegment3D > &sgms)
Static method to retrieve every segment included in a vector of objects.
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
void BASE_IMPEXP createFromPoseX(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
Definition: geometry.cpp:781
const std::vector< TSegment3D > & segs
Definition: geometry.cpp:1735
TTempIntersection & operator=(const TTempIntersection &t)
Definition: geometry.cpp:1158
TPolygonWithPlane()
Basic constructor.
Definition: geometry.h:42
unsigned char type
Definition: geometry.cpp:1090
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
size_t getRowCount() const
Returns the amount of rows in this matrix.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:427
void unitarize()
Unitarize line&#39;s normal vector.
A numeric matrix of compile-time fixed size.
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
vector< TSegment2D > l1
Definition: geometry.cpp:1086
2D segment, consisting of two points.
bool isPoint() const
Checks whether content is a point.
TCommonRegion & operator=(const TCommonRegion &r)
Definition: geometry.cpp:1115
3D segment, consisting of two points.
void BASE_IMPEXP createPlaneFromPoseXY(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Z vector.
Definition: geometry.cpp:1623
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
TPoint3D point2
Destiny point.
double BASE_IMPEXP distancePointToPolygon2D(const double &px, const double &py, unsigned int polyEdges, const double *poly_xs, const double *poly_ys)
Returns the closest distance of a given 2D point to a polygon, or "0" if the point is INTO the polygo...
Definition: geometry.cpp:273
const GLubyte * c
Definition: glext.h:5590
void BASE_IMPEXP createPlaneFromPoseXZ(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Y vector.
Definition: geometry.cpp:1627
bool PointIntoPolygon(double x, double y) const
Check if a point is inside the polygon.
Definition: CPolygon.h:61
void unitarize()
Unitarize normal vector.
void BASE_IMPEXP getRectangleBounds(const std::vector< TPoint2D > &poly, TPoint2D &pMin, TPoint2D &pMax)
Gets the rectangular bounds of a 2D polygon or set of 2D points.
Definition: geometry.cpp:1517
const unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
TPolygon3D poly
Actual polygon.
Definition: geometry.h:35
static void getPolygons(const std::vector< TObject3D > &objs, std::vector< TPolygon3D > &polys)
Static method to retrieve every polygon included in a vector of objects.
3D Plane, represented by its equation
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit insid...
GLubyte GLubyte b
Definition: glext.h:5575
double coefs[3]
Line coefficients, stored as an array: .
class BASE_IMPEXP TPolygon3D
double BASE_IMPEXP getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
Definition: geometry.cpp:1703
void getCenter(TPoint3D &p) const
Segment&#39;s central point.
FCreatePolygon(const std::vector< TSegment3D > &s)
Definition: geometry.cpp:1736
TPoint2D point2
Destiny point.
TPolygon2D poly2D
Polygon, after being projected to the plane using inversePose.
Definition: geometry.h:39
TCommonRegion(const TPoint2D &p)
Definition: geometry.cpp:1106
double BASE_IMPEXP getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:726
static void getPlanes(const std::vector< TPolygon3D > &oldPolys, std::vector< TPolygonWithPlane > &newPolys)
Static method for vectors.
Definition: geometry.cpp:562
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
A class used to store a 2D point.
Definition: CPoint2D.h:36
void unitarize()
Unitarize director vector.
int sign(T x)
Returns the sign of X as "1" or "-1".
Definition: bits.h:104
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
bool contains(const TPoint3D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave...
float cross(const mPointHull &O, const mPointHull &A, const mPointHull &B)
Definition: Plane.cpp:709
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
mrpt::poses::CPose3D pose
Plane&#39;s pose.
Definition: geometry.h:37
double director[3]
Director vector.
TPoint2D point1
Origin point.
bool BASE_IMPEXP SegmentsIntersection(const double x1, const double y1, const double x2, const double y2, const double x3, const double y3, const double x4, const double y4, double &ix, double &iy)
Returns the intersection point, and if it exists, between two segments.
Definition: geometry.cpp:133
void clear()
Completely removes all elements, although maintaining the matrix&#39;s size.
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave...
void getAsPose2D(mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line.
const CPose3D & pose
Definition: geometry.cpp:1903
const GLdouble * v
Definition: glext.h:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
FUnprojectPolygon2D(const CPose3D &p)
Definition: geometry.cpp:1905
TTempIntersection(const T2ListsOfSegments &segms)
Definition: geometry.cpp:1149
TPolygon3D operator()(const std::vector< MatchingVertex > &vertices)
Definition: geometry.cpp:1737
void createFromPoseAndAxis(const CPose3D &p, TLine3D &r, size_t axis)
Definition: geometry.cpp:772
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
Definition: glext.h:4993
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
GLfloat GLfloat v1
Definition: glext.h:3922
double BASE_IMPEXP getRegressionLine(const std::vector< TPoint2D > &points, TLine2D &line)
Using eigenvalues, gets the best fitting line for a set of 2D points.
Definition: geometry.cpp:1667
void generate3DObject(TObject3D &obj) const
Project into 3D space.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
struct BASE_IMPEXP TLine3D
T2ListsOfSegments * segms
Definition: geometry.cpp:1135
void BASE_IMPEXP getPrismBounds(const std::vector< TPoint3D > &poly, TPoint3D &pMin, TPoint3D &pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
Definition: geometry.cpp:1599
TPoint2D * point
Definition: geometry.cpp:1092
double coefs[4]
Plane coefficients, stored as an array: .
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
void resize(size_t nRows, size_t nCols)
Changes the size of the matrix.
void project3D(const TPoint3D &point, const mrpt::poses::CPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
Definition: geometry.h:272
Lightweight 2D pose.
void removeRedundantVertices()
Erase every redundant vertex from the polygon, saving space.
MatchingVertex(size_t s1, size_t s2, bool s1p, bool s2p)
Definition: geometry.cpp:1731
bool contains(const TPoint3D &point) const
Check whether a point is inside the segment.
bool getLine(TLine3D &r) const
Gets the content as a line, returning false if the type is not adequate.
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane&#39;s equation.
bool depthFirstSearch(const CSparseMatrixTemplate< unsigned char > &mat, std::vector< std::vector< MatchingVertex > > &res, std::vector< bool > &used, size_t searching, unsigned char mask, std::vector< MatchingVertex > &current)
Definition: geometry.cpp:1750
mrpt::poses::CPose3D inversePose
Plane&#39;s inverse pose.
Definition: geometry.h:38
GLenum GLint GLint y
Definition: glext.h:3516
void unsafeProjectPoint(const TPoint3D &point, const CPose3D &pose, TPoint2D &newPoint)
Definition: geometry.cpp:493
TCommonRegion * common
Definition: geometry.cpp:1136
void BASE_IMPEXP createPlaneFromPoseYZ(const mrpt::poses::CPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its X vector.
Definition: geometry.cpp:1631
bool BASE_IMPEXP areAligned(const std::vector< TPoint2D > &points)
Checks whether this set of points acceptably fits a 2D line.
Definition: geometry.cpp:840
bool compatibleBounds(const TPoint3D &min1, const TPoint3D &max1, const TPoint3D &min2, const TPoint3D &max2)
Definition: geometry.cpp:1326
size_t getIndexOfMax(const T &e1, const T &e2, const T &e3)
Definition: geometry.cpp:1685
GLfloat GLfloat GLfloat v2
Definition: glext.h:3923
void BASE_IMPEXP createFromPoseZ(const mrpt::poses::CPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Z axis in a given pose.
Definition: geometry.cpp:789
TCommonRegion(const TCommonRegion &r)
Definition: geometry.cpp:1128
bool intersectAux(const TPolygon3D &p1, const TPlane &pl1, const TPolygon3D &p2, const TPlane &pl2, TObject3D &obj)
Definition: geometry.cpp:1307
GLuint res
Definition: glext.h:6298
TTempIntersection(const TTempIntersection &t)
Definition: geometry.cpp:1171
GLenum GLint x
Definition: glext.h:3516
void covariancesAndMean(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const bool *elem_do_wrap2pi=NULL)
Computes covariances and mean of any vector of containers.
Definition: data_utils.h:354
Lightweight 3D point.
double distance(const TPoint2D &point) const
Distance from a given point.
GLuint GLenum GLenum transform
Definition: glext.h:6092
void getBestFittingPlane(TPlane &p) const
Gets the best fitting plane, disregarding whether the polygon actually fits inside or not...
TPlane plane
Plane containing the polygon.
Definition: geometry.h:36
Lightweight 2D point.
const unsigned char GEOMETRIC_TYPE_POLYGON
Object type identifier for TPolygon2D or TPolygon3D.
Add a new vertex to polygon.
Definition: CPolygon.h:36
double minimumDistanceFromPointToSegment(const double Px, const double Py, const double x1, const double y1, const double x2, const double y2, T &out_x, T &out_y)
Computes the closest point from a given point to a segment, and returns that minimum distance...
Definition: geometry.h:779
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
GLfloat GLfloat p
Definition: glext.h:5587
bool BASE_IMPEXP intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:568
unsigned char type
Definition: geometry.cpp:1133
TCommonRegion(const TSegment2D &s)
Definition: geometry.cpp:1109
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3512
2D polygon, inheriting from std::vector<TPoint2D>.
3D polygon, inheriting from std::vector<TPoint3D>
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504
void getMinAndMaxBounds(const std::vector< TPolygon3D > &v1, std::vector< TPoint3D > &minP, std::vector< TPoint3D > &maxP)
Definition: geometry.cpp:1351
TSegment2D * segment
Definition: geometry.cpp:1093
bool contains(const TPoint3D &point) const
Check whether a point is contained into the plane.
double BASE_IMPEXP geometryEpsilon
Global epsilon to overcome small precision errors (Default=1e-5)
Definition: geometry.cpp:28
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:4993
3D line, represented by a base point and a director vector.
2D line without bounds, represented by its equation .

 Page generated by Doxygen 1.8.14 for MRPT 1.5.8 Git: f67d0f871 Wed Sep 25 18:32:17 2019 +0200 at lun oct 28 01:58:29 CET 2019