Main MRPT website > C++ reference for MRPT 1.5.6
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;
514  unsafeProjectPoint(p,p1.inversePose,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  double c=0,n1=0,n2=0;
767  for (size_t i=0;i<2;i++) {
768  c+=r1.coefs[i]*r2.coefs[i];
769  n1+=r1.coefs[i]*r1.coefs[i];
770  n2+=r2.coefs[i]*r2.coefs[i];
771  }
772  double s=sqrt(n1*n2);
773  if (s<geometryEpsilon) throw std::logic_error("Invalid line(s)");
774  if (abs(s)<abs(c)) return (c/s<0)?M_PI:0;
775  else return acos(c/sqrt(n1*n2));
776 }
777
778 //Auxiliary method
779 void createFromPoseAndAxis(const CPose3D &p,TLine3D &r,size_t axis) {
781  for (size_t i=0;i<3;i++) {
782  r.pBase[i]=m.get_unsafe(i,3);
783  r.director[i]=m.get_unsafe(i,axis);
784  }
785 }
786 //End of auxiliary method
787
789  createFromPoseAndAxis(p,r,0);
790 }
791
793  createFromPoseAndAxis(p,r,1);
794 }
795
797  createFromPoseAndAxis(p,r,2);
798 }
799
800 void math::createFromPoseAndVector(const CPose3D &p,const double (&vector)[3],TLine3D &r) {
802  for (size_t i=0;i<3;i++) {
803  r.pBase[i]=m.get_unsafe(i,3);
804  r.director[i]=0;
805  for (size_t j=0;j<3;j++) r.director[i]+=m.get_unsafe(i,j)*vector[j];
806  }
807 }
808
810  r.coefs[0]=cos(p.phi);
811  r.coefs[1]=-sin(p.phi);
812  r.coefs[2]=-r.coefs[0]*p.x-r.coefs[1]*p.y;
813 }
814
816  r.coefs[0]=sin(p.phi);
817  r.coefs[1]=cos(p.phi);
818  r.coefs[2]=-r.coefs[0]*p.x-r.coefs[1]*p.y;
819 }
820
821 void math::createFromPoseAndVector(const TPose2D &p,const double (&vector)[2],TLine2D &r) {
822  double c=cos(p.phi);
823  double s=sin(p.phi);
824  r.coefs[0]=vector[0]*c+vector[1]*s;
825  r.coefs[1]=-vector[0]*s+vector[1]*c;
826  r.coefs[2]=-r.coefs[0]*p.x-r.coefs[1]*p.y;
827 }
828
829 bool math::conformAPlane(const std::vector<TPoint3D> &points) {
830  size_t N=points.size();
831  if (N<3) return false;
833  const TPoint3D &orig=points[N-1];
834  for (size_t i=0;i<N-1;i++) {
835  const TPoint3D &p=points[i];
836  mat(i,0)=p.x-orig.x;
837  mat(i,1)=p.y-orig.y;
838  mat(i,2)=p.z-orig.z;
839  }
840  return mat.rank(geometryEpsilon)==2;
841 }
842
843 bool math::conformAPlane(const std::vector<TPoint3D> &points,TPlane &p) {
844  return abs(getRegressionPlane(points,p))<geometryEpsilon;
845 }
846
847 bool math::areAligned(const std::vector<TPoint2D> &points) {
848  size_t N=points.size();
849  if (N<2) return false;
851  const TPoint2D &orig=points[N-1];
852  for (size_t i=0;i<N-1;i++) {
853  const TPoint2D &p=points[i];
854  mat(i,0)=p.x-orig.x;
855  mat(i,1)=p.y-orig.y;
856  }
857  return mat.rank(geometryEpsilon)==1;
858 }
859
860 bool math::areAligned(const std::vector<TPoint2D> &points,TLine2D &r) {
861  if (!areAligned(points)) return false;
862  const TPoint2D &p0=points[0];
863  for (size_t i=1;;i++) try {
864  r=TLine2D(p0,points[i]);
865  return true;
866  } catch (logic_error &) {}
867 }
868
869 bool math::areAligned(const std::vector<TPoint3D> &points) {
870  size_t N=points.size();
871  if (N<2) return false;
873  const TPoint3D &orig=points[N-1];
874  for (size_t i=0;i<N-1;i++) {
875  const TPoint3D &p=points[i];
876  mat(i,0)=p.x-orig.x;
877  mat(i,1)=p.y-orig.y;
878  mat(i,2)=p.z-orig.z;
879  }
880  return mat.rank(geometryEpsilon)==1;
881 }
882
883 bool math::areAligned(const std::vector<TPoint3D> &points,TLine3D &r) {
884  if (!areAligned(points)) return false;
885  const TPoint3D &p0=points[0];
886  for (size_t i=1;;i++) try {
887  r=TLine3D(p0,points[i]);
888  return true;
889  } catch (logic_error &) {}
890 }
891
892 void math::project3D(const TLine3D &line,const CPose3D &newXYpose,TLine3D &newLine) {
893  newXYpose.composePoint(line.pBase.x,line.pBase.y,line.pBase.z,newLine.pBase.x,newLine.pBase.y,newLine.pBase.z);
895  for (size_t i=0;i<3;i++) {
896  newLine.director[i]=0;
897  for (size_t j=0;j<3;j++) newLine.director[i]+=mat.get_unsafe(i,j)*line.director[j];
898  }
899  newLine.unitarize();
900 }
901
902 void math::project3D(const TPlane &plane,const CPose3D &newXYpose,TPlane &newPlane) {
904  for (size_t i=0;i<3;i++) {
905  newPlane.coefs[i]=0;
906  for (size_t j=0;j<3;j++) newPlane.coefs[i]+=mat.get_unsafe(i,j)*plane.coefs[j];
907  }
908  //VORSICHT! NO INTENTEN HACER ESTO EN SUS CASAS (nota: comentar sÃ­ o sÃ­, mÃ¡s tarde)
909  //La idea es mantener la distancia al nuevo origen igual a la distancia del punto original antes de proyectar.
910  //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]));
911  newPlane.coefs[3]=plane.evaluatePoint(TPoint3D(-newXYpose))*sqrt(squareNorm<3,double>(newPlane.coefs)/squareNorm<3,double>(plane.coefs));
912  newPlane.unitarize();
913 }
914
915 void math::project3D(const TPolygon3D &polygon,const CPose3D &newXYpose,TPolygon3D &newPolygon) {
916  size_t N=polygon.size();
917  newPolygon.resize(N);
918  for (size_t i=0;i<N;i++) project3D(polygon[i],newXYpose,newPolygon[i]);
919 }
920
921 void math::project3D(const TObject3D &object,const CPose3D &newXYpose,TObject3D &newObject) {
922  switch (object.getType()) {
924  {
925  TPoint3D p,p2;
926  object.getPoint(p);
927  project3D(p,newXYpose,p2);
928  newObject=p2;
929  break;
930  }
932  {
933  TSegment3D p,p2;
934  object.getSegment(p);
935  project3D(p,newXYpose,p2);
936  newObject=p2;
937  break;
938  }
939  case GEOMETRIC_TYPE_LINE:
940  {
941  TLine3D p,p2;
942  object.getLine(p);
943  project3D(p,newXYpose,p2);
944  newObject=p2;
945  break;
946  }
948  {
949  TPlane p,p2;
950  object.getPlane(p);
951  project3D(p,newXYpose,p2);
952  newObject=p2;
953  break;
954  }
956  {
957  TPolygon3D p,p2;
958  object.getPolygon(p);
959  project3D(p,newXYpose,p2);
960  newObject=p2;
961  break;
962  }
963  default:
964  newObject=TObject3D();
965  }
966 }
967
968 void math::project2D(const TPoint2D &point,const mrpt::poses::CPose2D &newXpose,TPoint2D &newPoint) {
969  newPoint=newXpose+mrpt::poses::CPoint2D(point);
970 }
971
972 void math::project2D(const TLine2D &line,const CPose2D &newXpose,TLine2D &newLine) {
973  double c=cos(newXpose.phi());
974  double s=sin(newXpose.phi());
975  newLine.coefs[0]=line.coefs[0]*c-line.coefs[1]*s;
976  newLine.coefs[1]=line.coefs[1]*c+line.coefs[0]*s;
977  newLine.coefs[2]=line.coefs[2]-(newLine.coefs[0]*newXpose.x()+newLine.coefs[1]*newXpose.y());
978  return;
979 }
980
981 void math::project2D(const TPolygon2D &line,const CPose2D &newXpose,TPolygon2D &newLine) {
982  size_t N=line.size();
983  newLine.resize(N);
984  for (size_t i=0;i<N;i++) newLine[i]=newXpose+line[i];
985  return;
986 }
987
988 void math::project2D(const TObject2D &obj,const CPose2D &newXpose,TObject2D &newObject) {
989  switch (obj.getType()) {
991  {
992  TPoint2D p,p2;
993  obj.getPoint(p);
994  project2D(p,newXpose,p2);
995  newObject=p2;
996  break;
997  }
999  {
1000  TSegment2D p,p2;
1001  obj.getSegment(p);
1002  project2D(p,newXpose,p2);
1003  newObject=p2;
1004  break;
1005  }
1006  case GEOMETRIC_TYPE_LINE:
1007  {
1008  TLine2D p,p2;
1009  obj.getLine(p);
1010  project2D(p,newXpose,p2);
1011  newObject=p2;
1012  break;
1013  }
1015  {
1016  TPolygon2D p,p2;
1017  obj.getPolygon(p);
1018  project2D(p,newXpose,p2);
1019  newObject=p2;
1020  break;
1021  }
1022  default:
1023  newObject=TObject2D();
1024  }
1025 }
1026
1027 bool math::intersect(const TPolygon2D &p1,const TSegment2D &s2,TObject2D &obj) {
1028  TLine2D l2=TLine2D(s2);
1029  if (!intersect(p1,l2,obj)) return false;
1030  TPoint2D p;
1031  TSegment2D s;
1032  if (obj.getPoint(p)) return s2.contains(p);
1033  else if (obj.getSegment(s)) return intersectInCommonLine(s,s2,l2,obj);
1034  return false;
1035 }
1036
1037 bool math::intersect(const TPolygon2D &p1,const TLine2D &r2,TObject2D &obj) {
1038  //Proceeding: project polygon so that the line happens to be y=0 (phi=0).
1039  //Then, search this new polygons for intersections with the X axis (very simple).
1040  if (p1.size()<3) return false;
1041  CPose2D pose,poseNeg;
1042  r2.getAsPose2D(pose);
1043  poseNeg=CPose2D(0,0,0)-pose;
1044  TPolygon2D projPoly;
1045  project2D(p1,poseNeg,projPoly);
1046  size_t N=projPoly.size();
1047  projPoly.push_back(projPoly[0]);
1048  double pre=projPoly[0].y;
1049  vector<TPoint2D> pnts;
1050  pnts.reserve(2);
1051  for (size_t i=1;i<=N;i++) {
1052  double cur=projPoly[i].y;
1053  if (abs(cur)<geometryEpsilon) {
1054  if (abs(pre)<geometryEpsilon) {
1055  pnts.resize(2);
1056  pnts[0]=projPoly[i-1];
1057  pnts[1]=projPoly[i];
1058  break;
1059  } else pnts.push_back(projPoly[i]);
1060  } else if ((abs(pre)>=geometryEpsilon)&&(sign(cur)!=sign(pre))) {
1061  double a=projPoly[i-1].x;
1062  double c=projPoly[i].x;
1063  double x=a-pre*(c-a)/(cur-pre);
1064  pnts.push_back(TPoint2D(x,0));
1065  }
1066  pre=cur;
1067  }
1068  //All results must undo the initial projection
1069  switch (pnts.size()) {
1070  case 0:
1071  return false;
1072  case 1:
1073  {
1074  TPoint2D p;
1075  project2D(pnts[0],pose,p);
1076  obj=p;
1077  return true;
1078  }
1079  case 2:
1080  {
1081  TSegment2D s;
1082  project2D(TSegment2D(pnts[0],pnts[1]),pose,s);
1083  obj=s;
1084  return true;
1085  }
1086  default:
1087  throw std::logic_error("Polygon is not convex");
1088  }
1089 }
1090
1091 //Auxiliary structs and code for 2D polygon intersection
1093  vector<TSegment2D> l1;
1094  vector<TSegment2D> l2;
1095 };
1097  unsigned char type; //0 -> point, 1-> segment, any other-> empty
1098  union {
1101  } data;
1102  void destroy() {
1103  switch (type) {
1104  case 0:
1105  delete data.point;
1106  break;
1107  case 1:
1108  delete data.segment;
1109  break;
1110  }
1111  type=255;
1112  }
1113  TCommonRegion(const TPoint2D &p):type(0) {
1114  data.point=new TPoint2D(p);
1115  }
1117  data.segment=new TSegment2D(s);
1118  }
1120  destroy();
1121  }
1123  if (&r==this) return *this;
1124  destroy();
1125  switch (type=r.type) {
1126  case 0:
1127  data.point=new TPoint2D(*(r.data.point));
1128  break;
1129  case 1:
1130  data.segment=new TSegment2D(*(r.data.segment));
1131  break;
1132  }
1133  return *this;
1134  }
1136  operator=(r);
1137  }
1138 };
1140  unsigned char type; //0->two lists of segments, 1-> common region
1141  union {
1144  } data;
1145  void destroy() {
1146  switch (type) {
1147  case 0:
1148  delete data.segms;
1149  break;
1150  case 1:
1151  delete data.common;
1152  break;
1153  }
1154  type=255;
1155  };
1157  data.segms=new T2ListsOfSegments(segms);
1158  }
1160  data.common=new TCommonRegion(common);
1161  }
1163  destroy();
1164  }
1166  if (&t==this) return *this;
1167  destroy();
1168  switch (type=t.type) {
1169  case 0:
1170  data.segms=new T2ListsOfSegments(*(t.data.segms));
1171  break;
1172  case 1:
1173  data.common=new TCommonRegion(*(t.data.common));
1174  break;
1175  }
1176  return *this;
1177  }
1179  operator=(t);
1180  }
1181 };
1185  explicit TSegmentWithLine(const TSegment2D &s):segment(s) {
1186  line=TLine2D(s[0],s[1]);
1187  }
1188  TSegmentWithLine(const TPoint2D &p1,const TPoint2D &p2):segment(p1,p2) {
1189  line=TLine2D(p1,p2);
1190  }
1192 };
1194  if (!intersect(s1.line,s2.line,obj)) return false;
1195  if (obj.isLine()) return intersectInCommonLine(s1.segment,s2.segment,s1.line,obj);
1196  TPoint2D p;
1197  obj.getPoint(p);
1198  return s1.segment.contains(p)&&s2.segment.contains(p);
1199 }
1200 void getSegmentsWithLine(const TPolygon2D &poly,vector<TSegmentWithLine> &segs) {
1201  size_t N=poly.size();
1202  segs.resize(N);
1203  for (size_t i=0;i<N-1;i++) segs[i]=TSegmentWithLine(poly[i],poly[i+1]);
1204  segs[N-1]=TSegmentWithLine(poly[N-1],poly[0]);
1205 }
1206
1207 inline char fromObject(const TObject2D &obj) {
1208  switch (obj.getType()) {
1209  case GEOMETRIC_TYPE_POINT:return 'P';
1210  case GEOMETRIC_TYPE_SEGMENT:return 'S';
1211  case GEOMETRIC_TYPE_LINE:return 'L';
1212  case GEOMETRIC_TYPE_POLYGON:return 'O';
1213  default:return 'U';
1214  }
1215 }
1216
1217 bool math::intersect(const TPolygon2D &/*p1*/,const TPolygon2D &/*p2*/,TObject2D &/*obj*/) {
1218  THROW_EXCEPTION("TODO");
1219 #if 0
1220  return false; //TODO
1221
1222  CSparseMatrixTemplate<TObject2D> intersections=CSparseMatrixTemplate<TObject2D>(p1.size(),p2.size());
1223  std::vector<TSegmentWithLine> segs1,segs2;
1224  getSegmentsWithLine(p1,segs1);
1225  getSegmentsWithLine(p2,segs2);
1226  unsigned int hmInters=0;
1227  for (size_t i=0;i<segs1.size();i++) {
1228  const TSegmentWithLine &s1=segs1[i];
1229  for (size_t j=0;j<segs2.size();j++) if (intersect(s1,segs2[j],obj)) {
1230  intersections(i,j)=obj;
1231  hmInters++;
1232  }
1233  }
1234  for (size_t i=0;i<intersections.getRowCount();i++) {
1235  for (size_t j=0;j<intersections.getColCount();j++) cout<<fromObject(intersections(i,j));
1236  cout<<'\n';
1237  }
1238  if (hmInters==0) {
1239  if (p1.contains(p2[0])) {
1240  obj=p2;
1241  return true;
1242  } else if (p2.contains(p1[0])) {
1243  obj=p1;
1244  return true;
1245  } else return false;
1246  }
1247  //ESTO ES UNA PESADILLA, HAY CIEN MILLONES DE CASOS DISTINTOS A LA HORA DE RECORRER LAS POSIBILIDADES...
1248  /*
1249  Dividir cada segmento en sus distintas partes segÃºn sus intersecciones, y generar un nuevo polÃ­gono.
1250  Recorrer de segmento en segmento, por cada uno de los dos lados (recorriendo desde un punto comÃºn a otro;
1251  en un polÃ­gono se escoge el camino secuencial directo, mientras que del otro se escoge, de los dos posibles,
1252  el que no se corta con ningÃºn elemento del primero).
1253  Seleccionar, para cada segmento, si estÃ¡ dentro o fuera.
1254  Parece fÃ¡cil, pero es una puta mierda.
1255  TODO: hacer en algÃºn momento de mucho tiempo libre...
1256  */
1257
1258  /* Â¿Seguir? */
1259  return false;
1260 #endif
1261 }
1262
1263 bool math::intersect(const TPolygon3D &p1,const TSegment3D &s2,TObject3D &obj) {
1264  TPlane p;
1265  if (!p1.getPlane(p)) return false;
1266  if (!intersect(p,s2,obj)) return false;
1267  TPoint3D pnt;
1268  TSegment3D sgm;
1269  if (obj.getPoint(pnt)) {
1270  CPose3D pose;
1271  p.getAsPose3DForcingOrigin(p1[0],pose);
1272  CPose3D poseNeg=CPose3D(0,0,0,0,0,0)-pose;
1273  TPolygon3D projPoly;
1274  TPoint3D projPnt;
1275  project3D(p1,poseNeg,projPoly);
1276  project3D(pnt,poseNeg,projPnt);
1277  return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
1278  } else if (obj.getSegment(sgm)) return intersectInCommonPlane<TPolygon2D,TSegment2D>(p1,s2,p,obj);
1279  return false;
1280 }
1281
1282 bool math::intersect(const TPolygon3D &p1,const TLine3D &r2,TObject3D &obj) {
1283  TPlane p;
1284  if (!p1.getPlane(p)) return false;
1285  if (!intersect(p,r2,obj)) return false;
1286  TPoint3D pnt;
1287  if (obj.getPoint(pnt)) {
1288  CPose3D pose;
1289  p.getAsPose3DForcingOrigin(p1[0],pose);
1290  CPose3D poseNeg=CPose3D(0,0,0,0,0,0)-pose;
1291  TPolygon3D projPoly;
1292  TPoint3D projPnt;
1293  project3D(p1,poseNeg,projPoly);
1294  project3D(pnt,poseNeg,projPnt);
1295  return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
1296  } else if (obj.isLine()) return intersectInCommonPlane<TPolygon2D,TLine2D>(p1,r2,p,obj);
1297  return false;
1298 }
1299
1300 bool math::intersect(const TPolygon3D &p1,const TPlane &p2,TObject3D &obj) {
1301  TPlane p;
1302  if (!p1.getPlane(p)) return false;
1303  if (!intersect(p,p2,obj)) return false;
1304  TLine3D ln;
1305  if (obj.isPlane()) {
1306  //Polygon is inside the plane
1307  obj=p1;
1308  return true;
1309  } else if (obj.getLine(ln)) return intersectInCommonPlane<TPolygon2D,TLine2D>(p1,ln,p,obj);
1310  return false;
1311 }
1312
1313 //Auxiliary function2
1314 bool intersectAux(const TPolygon3D &p1,const TPlane &pl1,const TPolygon3D &p2,const TPlane &pl2,TObject3D &obj) {
1315  if (!intersect(pl1,pl2,obj)) return false;
1316  TLine3D ln;
1317  if (obj.isPlane()) return intersectInCommonPlane<TPolygon2D,TPolygon2D>(p1,p2,pl1,obj); //Polygons are on the same plane
1318  else if (obj.getLine(ln)) {
1319  TObject3D obj1,obj2;
1320  if (!intersectInCommonPlane<TPolygon2D,TLine2D>(p1,ln,pl1,obj1)) return false;
1321  if (!intersectInCommonPlane<TPolygon2D,TLine2D>(p2,ln,pl2,obj2)) return false;
1322  TPoint3D po1,po2;
1323  TSegment3D s1,s2;
1324  if (obj1.getPoint(po1)) s1=TSegment3D(po1,po1);
1325  else obj1.getSegment(s1);
1326  if (obj2.getPoint(po2)) s2=TSegment3D(po2,po2);
1327  else obj2.getSegment(s2);
1328  return intersectInCommonLine(s1,s2,ln,obj);
1329  }
1330  return false;
1331 }
1332
1333 bool compatibleBounds(const TPoint3D &min1,const TPoint3D &max1,const TPoint3D &min2,const TPoint3D &max2) {
1334  for (size_t i=0;i<3;i++) if ((min1[i]>max2[i])||(min2[i]>max1[i])) return false;
1335  return true;
1336 }
1337 //End of auxiliary functions
1338
1339 bool math::intersect(const TPolygon3D &p1,const TPolygon3D &p2,TObject3D &obj) {
1340  TPoint3D min1,max1,min2,max2;
1341  getPrismBounds(p1,min1,max1);
1342  getPrismBounds(p2,min2,max2);
1343  if (!compatibleBounds(min1,max1,min2,max2)) return false;
1344  TPlane pl1,pl2;
1345  if (!p1.getPlane(pl1)) return false;
1346  if (!p2.getPlane(pl2)) return false;
1347  return intersectAux(p1,pl1,p2,pl2,obj);
1348 }
1349
1350 //Auxiliary function
1351 inline void getPlanes(const std::vector<TPolygon3D> &polys,std::vector<TPlane> &planes) {
1352  size_t N=polys.size();
1353  planes.resize(N);
1354  for (size_t i=0;i<N;i++) getRegressionPlane(polys[i],planes[i]);
1355 }
1356
1357 //Auxiliary functions
1358 void getMinAndMaxBounds(const std::vector<TPolygon3D> &v1,std::vector<TPoint3D> &minP,std::vector<TPoint3D> &maxP) {
1359  minP.resize(0);
1360  maxP.resize(0);
1361  size_t N=v1.size();
1362  minP.reserve(N);
1363  maxP.reserve(N);
1364  TPoint3D p1,p2;
1365  for (std::vector<TPolygon3D>::const_iterator it=v1.begin();it!=v1.end();++it) {
1366  getPrismBounds(*it,p1,p2);
1367  minP.push_back(p1);
1368  maxP.push_back(p2);
1369  }
1370 }
1371
1372 size_t math::intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,CSparseMatrixTemplate<TObject3D> &objs) {
1373  std::vector<TPlane> w1,w2;
1374  getPlanes(v1,w1);
1375  getPlanes(v2,w2);
1376  std::vector<TPoint3D> minBounds1,maxBounds1,minBounds2,maxBounds2;
1377  getMinAndMaxBounds(v1,minBounds1,maxBounds1);
1378  getMinAndMaxBounds(v2,minBounds2,maxBounds2);
1379  size_t M=v1.size(),N=v2.size();
1380  objs.clear();
1381  objs.resize(M,N);
1382  TObject3D obj;
1383  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;
1384  else if (intersectAux(v1[i],w1[i],v2[j],w2[j],obj)) objs(i,j)=obj;
1385  return objs.getNonNullElements();
1386 }
1387
1388 size_t math::intersect(const std::vector<TPolygon3D> &v1,const std::vector<TPolygon3D> &v2,std::vector<TObject3D> &objs) {
1389  objs.resize(0);
1390  std::vector<TPlane> w1,w2;
1391  getPlanes(v1,w1);
1392  getPlanes(v2,w2);
1393  std::vector<TPoint3D> minBounds1,maxBounds1,minBounds2,maxBounds2;
1394  getMinAndMaxBounds(v1,minBounds1,maxBounds1);
1395  getMinAndMaxBounds(v2,minBounds2,maxBounds2);
1396  TObject3D obj;
1397  std::vector<TPlane>::const_iterator itP1=w1.begin();
1398  std::vector<TPoint3D>::const_iterator itMin1=minBounds1.begin();
1399  std::vector<TPoint3D>::const_iterator itMax1=maxBounds1.begin();
1400  for (std::vector<TPolygon3D>::const_iterator it1=v1.begin();it1!=v1.end();++it1,++itP1,++itMin1,++itMax1) {
1401  const TPolygon3D &poly1=*it1;
1402  const TPlane &plane1=*itP1;
1403  std::vector<TPlane>::const_iterator itP2=w2.begin();
1404  const TPoint3D &min1=*itMin1,max1=*itMax1;
1405  std::vector<TPoint3D>::const_iterator itMin2=minBounds2.begin();
1406  std::vector<TPoint3D>::const_iterator itMax2=maxBounds2.begin();
1407  for (std::vector<TPolygon3D>::const_iterator it2=v2.begin();it2!=v2.end();++it2,++itP2,++itMin2,++itMax2) if (!compatibleBounds(min1,max1,*itMin2,*itMax2)) continue;
1408  else if (intersectAux(poly1,plane1,*it2,*itP2,obj)) objs.push_back(obj);
1409  }
1410  return objs.size();
1411 }
1412
1413 bool math::intersect(const TObject2D &o1,const TObject2D &o2,TObject2D &obj) {
1414  TPoint2D p1,p2;
1415  TSegment2D s1,s2;
1416  TLine2D l1,l2;
1417  TPolygon2D po1,po2;
1418  if (o1.getPoint(p1)) {
1419  obj=p1;
1420  if (o2.getPoint(p2)) return distance(p1,p2)<geometryEpsilon;
1421  else if (o2.getSegment(s2)) return s2.contains(p1);
1422  else if (o2.getLine(l2)) return l2.contains(p1);
1423  else if (o2.getPolygon(po2)) return po2.contains(p1); //else return false;
1424  } else if (o1.getSegment(s1)) {
1425  if (o2.getPoint(p2)) {
1426  if (s1.contains(p2)) {
1427  obj=p2;
1428  return true;
1429  } //else return false;
1430  } else if (o2.getSegment(s2)) return intersect(s1,s2,obj);
1431  else if (o2.getLine(l2)) return intersect(s1,l2,obj);
1432  else if (o2.getPolygon(po2)) return intersect(s1,po2,obj); //else return false;
1433  } else if (o1.getLine(l1)) {
1434  if (o2.getPoint(p2)) {
1435  if (l1.contains(p2)) {
1436  obj=p2;
1437  return true;
1438  } //else return false;
1439  } else if (o2.getSegment(s2)) return intersect(l1,s2,obj);
1440  else if (o2.getLine(l2)) return intersect(l1,l2,obj);
1441  else if (o2.getPolygon(po2)) return intersect(l1,po2,obj); //else return false;
1442  } else if (o1.getPolygon(po1)) {
1443  if (o2.getPoint(p2)) {
1444  if (po1.contains(p2)) {
1445  obj=p2;
1446  return true;
1447  } //else return false;
1448  } else if (o2.getSegment(s2)) return intersect(po1,s2,obj);
1449  else if (o2.getLine(l2)) return intersect(po1,l2,obj);
1450  else if (o2.getPolygon(po2)) return intersect(po1,po2,obj); //else return false;
1451  } //else return false;
1452  return false;
1453 }
1454
1455 bool math::intersect(const TObject3D &o1,const TObject3D &o2,TObject3D &obj) {
1456  TPoint3D p1,p2;
1457  TSegment3D s1,s2;
1458  TLine3D l1,l2;
1459  TPolygon3D po1,po2;
1460  TPlane pl1,pl2;
1461  if (o1.getPoint(p1)) {
1462  obj=p1;
1463  if (o2.getPoint(p2)) return distance(p1,p2)<geometryEpsilon;
1464  else if (o2.getSegment(s2)) return s2.contains(p1);
1465  else if (o2.getLine(l2)) return l2.contains(p1);
1466  else if (o2.getPolygon(po2)) return po2.contains(p1);
1467  else if (o2.getPlane(pl2)) return pl2.contains(p1); //else return false;
1468  } else if (o1.getSegment(s1)) {
1469  if (o2.getPoint(p2)) {
1470  if (s1.contains(p2)) {
1471  obj=p2;
1472  return true;
1473  } //else return false;
1474  } else if (o2.getSegment(s2)) return intersect(s1,s2,obj);
1475  else if (o2.getLine(l2)) return intersect(s1,l2,obj);
1476  else if (o2.getPolygon(po2)) return intersect(s1,po2,obj);
1477  else if (o2.getPlane(pl2)) return intersect(s1,pl2,obj); //else return false;
1478  } else if (o1.getLine(l1)) {
1479  if (o2.getPoint(p2)) {
1480  if (l1.contains(p2)) {
1481  obj=p2;
1482  return true;
1483  } //else return false;
1484  } else if (o2.getSegment(s2)) return intersect(l1,s2,obj);
1485  else if (o2.getLine(l2)) return intersect(l1,l2,obj);
1486  else if (o2.getPolygon(po2)) return intersect(l1,po2,obj);
1487  else if (o2.getPlane(pl2)) return intersect(l2,pl2,obj); //else return false;
1488  } else if (o1.getPolygon(po1)) {
1489  if (o2.getPoint(p2)) {
1490  if (po1.contains(p2)) {
1491  obj=p2;
1492  return true;
1493  } //else return false;
1494  } else if (o2.getSegment(s2)) return intersect(po1,s2,obj);
1495  else if (o2.getLine(l2)) return intersect(po1,l2,obj);
1496  else if (o2.getPolygon(po2)) return intersect(po1,po2,obj);
1497  else if (o2.getPlane(pl2)) return intersect(po1,pl2,obj); //else return false;
1498  } else if (o1.getPlane(pl1)) {
1499  if (o2.getPoint(p2)) {
1500  if (pl1.contains(p2)) {
1501  obj=p2;
1502  return true;
1503  } //else return false;
1504  } else if (o2.getSegment(s2)) return intersect(pl1,s2,obj);
1505  else if (o2.getLine(l2)) return intersect(pl1,l2,obj);
1506  else if (o2.getPlane(pl2)) return intersect(pl1,pl2,obj); //else return false;
1507  } //else return false;
1508  return false;
1509 }
1510
1511 double math::distance(const TPoint2D &p1,const TPoint2D &p2) {
1512  double dx=p2.x-p1.x;
1513  double dy=p2.y-p1.y;
1514  return sqrt(dx*dx+dy*dy);
1515 }
1516
1517 double math::distance(const TPoint3D &p1,const TPoint3D &p2) {
1518  double dx=p2.x-p1.x;
1519  double dy=p2.y-p1.y;
1520  double dz=p2.z-p1.z;
1521  return sqrt(dx*dx+dy*dy+dz*dz);
1522 }
1523
1524 void math::getRectangleBounds(const std::vector<TPoint2D> &poly,TPoint2D &pMin,TPoint2D &pMax) {
1525  size_t N=poly.size();
1526  if (N<1) throw logic_error("Empty polygon");
1527  pMin=poly[0];
1528  pMax=poly[0];
1529  for (size_t i=1;i<N;i++) {
1530  pMin.x=min(pMin.x,poly[i].x);
1531  pMin.y=min(pMin.y,poly[i].y);
1532  pMax.x=max(pMax.x,poly[i].x);
1533  pMax.y=max(pMax.y,poly[i].y);
1534  }
1535 }
1536
1537 double math::distance(const TLine2D &r1,const TLine2D &r2) {
1538  if (abs(r1.coefs[0]*r2.coefs[1]-r2.coefs[0]*r1.coefs[1])<geometryEpsilon) {
1539  //Lines are parallel
1540  size_t i1=(abs(r1.coefs[0])<geometryEpsilon)?0:1;
1541  TPoint2D p;
1542  p[i1]=0.0;
1543  p[1-i1]=-r1.coefs[2]/r1.coefs[1-i1];
1544  return r2.distance(p);
1545  } else return 0; //Lines cross in some point
1546 }
1547
1548 double math::distance(const TLine3D &r1,const TLine3D &r2) {
1549  if (abs(getAngle(r1,r2))<geometryEpsilon) return r1.distance(r2.pBase); //Lines are parallel
1550  else {
1551  //We build a plane parallel to r2 which contains r1
1552  TPlane p;
1554  p.coefs[3]=-(p.coefs[0]*r1.pBase[0]+p.coefs[1]*r1.pBase[1]+p.coefs[2]*r1.pBase[2]);
1555  return p.distance(r2.pBase);
1556  }
1557 }
1558
1559 double math::distance(const TPlane &p1,const TPlane &p2) {
1560  if (abs(getAngle(p1,p2))<geometryEpsilon) {
1561  //Planes are parallel
1562  TPoint3D p(0,0,0);
1563  for (size_t i=0;i<3;i++) if (abs(p1.coefs[i])>=geometryEpsilon) {
1564  p[i]=-p1.coefs[3]/p1.coefs[i];
1565  break;
1566  }
1567  return p2.distance(p);
1568  } else return 0; //Planes cross in a line
1569 }
1570
1571 double math::distance(const TPolygon2D &p1,const TPolygon2D &p2) {
1573  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TPolygon2D)");
1574 }
1575
1576 double math::distance(const TPolygon2D &p1,const TSegment2D &s2) {
1578  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TSegment)");
1579 }
1580
1581 double math::distance(const TPolygon2D &p1,const TLine2D &l2) {
1583  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TLine2D)");
1584 }
1585
1586 double math::distance(const TPolygon3D &p1,const TPolygon3D &p2) {
1588  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPolygon3D");
1589 }
1590
1591 double math::distance(const TPolygon3D &p1,const TSegment3D &s2) {
1593  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TSegment3D");
1594 }
1595
1596 double math::distance(const TPolygon3D &p1,const TLine3D &l2) {
1598  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TLine3D");
1599 }
1600
1601 double math::distance(const TPolygon3D &po,const TPlane &pl) {
1603  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPlane");
1604 }
1605
1606 void math::getPrismBounds(const std::vector<TPoint3D> &poly,TPoint3D &pMin,TPoint3D &pMax) {
1607  size_t N=poly.size();
1608  if (N<1) throw logic_error("Empty polygon");
1609  pMin=poly[0];
1610  pMax=poly[0];
1611  for (size_t i=1;i<N;i++) {
1612  pMin.x=min(pMin.x,poly[i].x);
1613  pMin.y=min(pMin.y,poly[i].y);
1614  pMin.z=min(pMin.z,poly[i].z);
1615  pMax.x=max(pMax.x,poly[i].x);
1616  pMax.y=max(pMax.y,poly[i].y);
1617  pMax.z=max(pMax.z,poly[i].z);
1618  }
1619 }
1620
1621 void createPlaneFromPoseAndAxis(const CPose3D &pose,TPlane &plane,size_t axis) {
1622  plane.coefs[3]=0;
1624  for (size_t i=0;i<3;i++) {
1625  plane.coefs[i]=m.get_unsafe(i,axis);
1626  plane.coefs[3]-=plane.coefs[i]*m.get_unsafe(i,3);
1627  }
1628 }
1629
1630 void math::createPlaneFromPoseXY(const CPose3D &pose,TPlane &plane) {
1631  createPlaneFromPoseAndAxis(pose,plane,2);
1632 }
1633
1634 void math::createPlaneFromPoseXZ(const CPose3D &pose,TPlane &plane) {
1635  createPlaneFromPoseAndAxis(pose,plane,1);
1636 }
1637
1638 void math::createPlaneFromPoseYZ(const CPose3D &pose,TPlane &plane) {
1639  createPlaneFromPoseAndAxis(pose,plane,0);
1640 }
1641
1642 void math::createPlaneFromPoseAndNormal(const CPose3D &pose,const double (&normal)[3],TPlane &plane) {
1643  plane.coefs[3]=0;
1645  for (size_t i=0;i<3;i++) {
1646  plane.coefs[i]=0;
1647  for (size_t j=0;j<3;j++) plane.coefs[i]+=normal[j]*m.get_unsafe(i,j);
1648  plane.coefs[3]-=plane.coefs[i]*m.get_unsafe(i,3);
1649  }
1650 }
1651
1653  //Assumes vector is unitary.
1654  //coord: 0=x, 1=y, 2=z.
1655  char coord1=(coord+1)%3;
1656  char coord2=(coord+2)%3;
1657  matrix.setSize(4,4);
1658  matrix(3,3)=1.0;
1659  for (size_t i=0;i<3;i++) matrix.set_unsafe(i,coord,vec[i]);
1660  matrix.set_unsafe(0,coord1,0);
1661  double h=hypot(vec[1],vec[2]);
1662  if (h<geometryEpsilon) {
1663  matrix.set_unsafe(1,coord1,1);
1664  matrix.set_unsafe(2,coord1,0);
1665  } else {
1666  matrix.set_unsafe(1,coord1,-vec[2]/h);
1667  matrix.set_unsafe(2,coord1,vec[1]/h);
1668  }
1669  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));
1670  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));
1671  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));
1672 }
1673
1674 double math::getRegressionLine(const vector<TPoint2D> &points,TLine2D &line) {
1675  CArrayDouble<2> means;
1676  CMatrixTemplateNumeric<double> covars(2,2),eigenVal(2,2),eigenVec(2,2);
1677  covariancesAndMean(points,covars,means);
1678  covars.eigenVectors(eigenVec,eigenVal);
1679  size_t selected=(eigenVal.get_unsafe(0,0)>=eigenVal.get_unsafe(1,1))?0:1;
1680  line.coefs[0]=-eigenVec.get_unsafe(1,selected);
1681  line.coefs[1]=eigenVec.get_unsafe(0,selected);
1682  line.coefs[2]=-line.coefs[0]*means[0]-line.coefs[1]*means[1];
1683  return sqrt(eigenVal.get_unsafe(1-selected,1-selected)/eigenVal.get_unsafe(selected,selected));
1684 }
1685
1686 template<class T>
1687 inline size_t getIndexOfMin(const T &e1,const T &e2,const T &e3) {
1688  return (e1<e2)?((e1<e3)?0:2):((e2<e3)?1:2);
1689 }
1690
1691 template<class T>
1692 inline size_t getIndexOfMax(const T &e1,const T &e2,const T &e3) {
1693  return (e1>e2)?((e1>e3)?0:2):((e2>e3)?1:2);
1694 }
1695
1696 double math::getRegressionLine(const vector<TPoint3D> &points,TLine3D &line) {
1697  CArrayDouble<3> means;
1698  CMatrixTemplateNumeric<double> covars(3,3),eigenVal(3,3),eigenVec(3,3);
1699  covariancesAndMean(points,covars,means);
1700  covars.eigenVectors(eigenVec,eigenVal);
1701  size_t selected=getIndexOfMax(eigenVal.get_unsafe(0,0),eigenVal.get_unsafe(1,1),eigenVal.get_unsafe(2,2));
1702  for (size_t i=0;i<3;i++) {
1703  line.pBase[i]=means[i];
1704  line.director[i]=eigenVec.get_unsafe(i,selected);
1705  }
1706  size_t i1=(selected+1)%3,i2=(selected+2)%3;
1707  return sqrt((eigenVal.get_unsafe(i1,i1)+eigenVal.get_unsafe(i2,i2))/eigenVal.get_unsafe(selected,selected));
1708 }
1709
1710 double math::getRegressionPlane(const vector<TPoint3D> &points,TPlane &plane) {
1711  vector<double> means;
1712  CMatrixDouble33 covars,eigenVal,eigenVec;
1713  covariancesAndMean(points,covars,means);
1714
1715  covars.eigenVectors(eigenVec,eigenVal);
1716  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);
1717  size_t selected=getIndexOfMin(eigenVal.get_unsafe(0,0),eigenVal.get_unsafe(1,1),eigenVal.get_unsafe(2,2));
1718  plane.coefs[3]=0;
1719  for (size_t i=0;i<3;i++) {
1720  plane.coefs[i]=eigenVec.get_unsafe(i,selected);
1721  plane.coefs[3]-=plane.coefs[i]*means[i];
1722  }
1723  size_t i1=(selected+1)%3,i2=(selected+2)%3;
1724  return sqrt(eigenVal.get_unsafe(selected,selected)/(eigenVal.get_unsafe(i1,i1)+eigenVal.get_unsafe(i2,i2)));
1725 }
1726
1727 void math::assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys) {
1728  std::vector<TSegment3D> tmp;
1729  assemblePolygons(segms,polys,tmp);
1730 }
1731
1733  size_t seg1;
1734  size_t seg2;
1735  bool seg1Point; //true for point2, false for point1
1736  bool seg2Point; //same
1738  MatchingVertex(size_t s1,size_t s2,bool s1p,bool s2p):seg1(s1),seg2(s2),seg1Point(s1p),seg2Point(s2p) {}
1739 };
1741 public:
1742  const std::vector<TSegment3D> &segs;
1743  FCreatePolygon(const std::vector<TSegment3D> &s):segs(s) {}
1744  TPolygon3D operator()(const std::vector<MatchingVertex> &vertices) {
1745  TPolygon3D res;
1746  size_t N=vertices.size();
1747  res.reserve(N);
1748  for (std::vector<MatchingVertex>::const_iterator it=vertices.begin();it!=vertices.end();++it) res.push_back(segs[it->seg2][it->seg2Point?1:0]);
1749  return res;
1750  }
1751 };
1752 inline bool firstOrNonPresent(size_t i,const std::vector<MatchingVertex> &v) {
1753  if (v.size()>0&&v[0].seg1==i) return true;
1754  for (std::vector<MatchingVertex>::const_iterator it=v.begin();it!=v.end();++it) if (it->seg1==i||it->seg2==i) return false;
1755  return true;
1756 }
1757 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) {
1758  for (size_t i=0;i<mat.getColCount();i++) if (!used[i]&&mat.isNotNull(searching,i)) {
1760  if (!match) continue;
1761  else if (firstOrNonPresent(i,current)) {
1762  bool s1p,s2p;
1763  if (true==(s1p=(!(match&3)))) match>>=2;
1764  s2p=!(match&1);
1765  if (current.size()>=2&&current[0].seg1==i) {
1766  if (s2p!=current[0].seg1Point) {
1767  current.push_back(MatchingVertex(searching,i,s1p,s2p));
1768  for (std::vector<MatchingVertex>::const_iterator it=current.begin();it!=current.end();++it) used[it->seg2]=true;
1769  res.push_back(current);
1770  return true;
1771  } else continue; //Strange shape... not a polygon, although it'll be without the first segment
1772  } else {
1773  current.push_back(MatchingVertex(searching,i,s1p,s2p));
1774  if (depthFirstSearch(mat,res,used,i,s2p?0x3:0xC,current)) return true;
1775  current.pop_back();
1776  }
1777  }
1778  }
1779  //No match has been found. Backtrack
1780  return false;
1781 }
1782 void depthFirstSearch(const CSparseMatrixTemplate<unsigned char> &mat,std::vector<std::vector<MatchingVertex> >&res,std::vector<bool> &used) {
1783  vector<MatchingVertex> cur;
1784  for (size_t i=0;i<used.size();i++) if (!used[i]) if (depthFirstSearch(mat,res,used,i,0xf,cur)) cur.clear();
1785 }
1786 void math::assemblePolygons(const std::vector<TSegment3D> &segms,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder) {
1787  std::vector<TSegment3D> tmp;
1788  tmp.reserve(segms.size());
1789  for (std::vector<TSegment3D>::const_iterator it=segms.begin();it!=segms.end();++it) if (it->length()>=geometryEpsilon) tmp.push_back(*it);
1790  else remainder.push_back(*it);
1791  size_t N=tmp.size();
1793  for (size_t i=0;i<N-1;i++) for (size_t j=i+1;j<N;j++) {
1794  if (distance(tmp[i].point1,tmp[j].point1)<geometryEpsilon) {
1795  matches(i,j)|=1;
1796  matches(j,i)|=1;
1797  }
1798  if (distance(tmp[i].point1,tmp[j].point2)<geometryEpsilon) {
1799  matches(i,j)|=2;
1800  matches(j,i)|=4;
1801  }
1802  if (distance(tmp[i].point2,tmp[j].point1)<geometryEpsilon) {
1803  matches(i,j)|=4;
1804  matches(j,i)|=2;
1805  }
1806  if (distance(tmp[i].point2,tmp[j].point2)<geometryEpsilon) {
1807  matches(i,j)|=8;
1808  matches(j,i)|=8;
1809  }
1810  }
1811  std::vector<std::vector<MatchingVertex> > results;
1812  std::vector<bool> usedSegments(N,false);
1813  depthFirstSearch(matches,results,usedSegments);
1814  polys.resize(results.size());
1815  transform(results.begin(),results.end(),polys.begin(),FCreatePolygon(segms));
1816  for (size_t i=0;i<N;i++) if (!usedSegments[i]) remainder.push_back(tmp[i]);
1817 }
1818
1819 void math::assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys) {
1820  std::vector<TObject3D> tmp;
1821  std::vector<TSegment3D> sgms;
1822  TObject3D::getPolygons(objs,polys,tmp);
1823  TObject3D::getSegments(tmp,sgms);
1824  assemblePolygons(sgms,polys);
1825 }
1826
1827 void math::assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TObject3D> &remainder) {
1828  std::vector<TObject3D> tmp;
1829  std::vector<TSegment3D> sgms,remainderSgms;
1830  TObject3D::getPolygons(objs,polys,tmp);
1831  TObject3D::getSegments(tmp,sgms,remainder);
1832  assemblePolygons(sgms,polys,remainderSgms);
1833  remainder.insert(remainder.end(),remainderSgms.begin(),remainderSgms.end());
1834 }
1835
1836 void math::assemblePolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,std::vector<TSegment3D> &remainder1,std::vector<TObject3D> &remainder2) {
1837  std::vector<TObject3D> tmp;
1838  std::vector<TSegment3D> sgms;
1839  TObject3D::getPolygons(objs,polys,tmp);
1840  TObject3D::getSegments(tmp,sgms,remainder2);
1841  assemblePolygons(sgms,polys,remainder1);
1842 }
1843
1844 bool intersect(const TLine2D &l1,const TSegmentWithLine &s2,TObject2D &obj) {
1845  if (intersect(l1,s2.line,obj)) {
1846  if (obj.isLine()) {
1847  obj=s2.segment;
1848  return true;
1849  } else {
1850  TPoint2D p;
1851  obj.getPoint(p);
1852  return s2.segment.contains(p);
1853  }
1854  } else return false;
1855 }
1856
1857 inline bool intersect(const TSegmentWithLine &s1,const TLine2D &l2,TObject2D &obj) {
1858  return intersect(l2,s1,obj);
1859 }
1860
1861 bool math::splitInConvexComponents(const TPolygon2D &poly,vector<TPolygon2D> &components) {
1862  components.clear();
1863  size_t N=poly.size();
1864  if (N<=3) return false;
1865  vector<TSegmentWithLine> segms(N);
1866  for (size_t i=0;i<N-1;i++) segms[i]=TSegmentWithLine(poly[i],poly[i+1]);
1867  segms[N-1]=TSegmentWithLine(poly[N-1],poly[0]);
1868  TObject2D obj;
1869  TPoint2D pnt;
1870  for (size_t i=0;i<N;i++) {
1871  size_t ii=(i+2)%N,i_=(i+N-1)%N;
1872  for (size_t j=ii;j!=i_;j=(j+1)%N) if (intersect(segms[i].line,segms[j],obj)&&obj.getPoint(pnt)) {
1873  TSegmentWithLine sTmp=TSegmentWithLine(pnt,segms[i].segment[(distance(pnt,segms[i].segment.point1)<distance(pnt,segms[i].segment.point2))?0:1]);
1874  bool cross=false;
1875  TPoint2D pTmp;
1876  for (size_t k=0;(k<N)&&!cross;k++) if (intersect(sTmp,segms[k],obj)) {
1877  if (obj.getPoint(pTmp)&&(distance(pTmp,sTmp.segment[0])>=geometryEpsilon)&&(distance(pTmp,sTmp.segment[1])>=geometryEpsilon)) cross=true;
1878  }
1879  if (cross) continue;
1880  //At this point, we have a suitable point, although we must check if the division is right.
1881  //We do this by evaluating, in the expanded segment's line, the next and previous points. If both signs differ, proceed.
1882  if (sign(segms[i].line.evaluatePoint(poly[(i+N-1)%N]))==sign(segms[i].line.evaluatePoint(poly[(i+2)%N]))) continue;
1883  TPolygon2D p1,p2;
1884  if (i>j) {
1885  p1.insert(p1.end(),poly.begin()+i+1,poly.end());
1886  p1.insert(p1.end(),poly.begin(),poly.begin()+j+1);
1887  p2.insert(p2.end(),poly.begin()+j+1,poly.begin()+i+1);
1888  } else {
1889  p1.insert(p1.end(),poly.begin()+i+1,poly.begin()+j+1);
1890  p2.insert(p2.end(),poly.begin()+j+1,poly.end());
1891  p2.insert(p2.end(),poly.begin(),poly.begin()+i+1);
1892  }
1893  if (distance(*(p1.rbegin()),pnt)>=geometryEpsilon) p1.push_back(pnt);
1894  if (distance(*(p2.rbegin()),pnt)>=geometryEpsilon) p2.push_back(pnt);
1897  vector<TPolygon2D> tempComps;
1898  if (splitInConvexComponents(p1,tempComps)) components.insert(components.end(),tempComps.begin(),tempComps.end());
1899  else components.push_back(p1);
1900  if (splitInConvexComponents(p2,tempComps)) components.insert(components.end(),tempComps.begin(),tempComps.end());
1901  else components.push_back(p2);
1902  return true;
1903  }
1904  }
1905  return false;
1906 }
1907
1909 public:
1910  const CPose3D &pose;
1912  FUnprojectPolygon2D(const CPose3D &p):pose(p),tmp1(0),tmp2(0) {}
1914  tmp1=TPolygon3D(poly2D);
1915  project3D(tmp1,pose,tmp2);
1916  return tmp2;
1917  }
1918 };
1919 bool math::splitInConvexComponents(const TPolygon3D &poly,vector<TPolygon3D> &components) {
1920  TPlane p;
1921  if (!poly.getPlane(p)) throw std::logic_error("Polygon is skew");
1922  CPose3D pose1,pose2;
1923  p.getAsPose3DForcingOrigin(poly[0],pose1);
1924  pose2=-pose1;
1925  TPolygon3D polyTmp;
1926  project3D(poly,pose2,polyTmp);
1927  TPolygon2D poly2D=TPolygon2D(polyTmp);
1928  vector<TPolygon2D> components2D;
1929  if (splitInConvexComponents(poly2D,components2D)) {
1930  components.resize(components2D.size());
1931  transform(components2D.begin(),components2D.end(),components.begin(),FUnprojectPolygon2D(pose1));
1932  return true;
1933  } else return false;
1934 }
1935
1937  TPoint2D p;
1938  sgm.getCenter(p);
1939  bis.coefs[0]=sgm.point2.x-sgm.point1.x;
1940  bis.coefs[1]=sgm.point2.y-sgm.point1.y;
1941  bis.coefs[2]=-bis.coefs[0]*p.x-bis.coefs[1]*p.y;
1942  bis.unitarize();
1943 }
1944
1946  TPoint3D p;
1947  sgm.getCenter(p);
1948  bis.coefs[0]=sgm.point2.x-sgm.point1.x;
1949  bis.coefs[1]=sgm.point2.y-sgm.point1.y;
1950  bis.coefs[2]=sgm.point2.z-sgm.point1.z;
1951  bis.coefs[2]=-bis.coefs[0]*p.x-bis.coefs[1]*p.y-bis.coefs[2]*p.z;
1952  bis.unitarize();
1953 }
1954
1955 void math::getAngleBisector(const TLine2D &l1,const TLine2D &l2,TLine2D &bis) {
1956  TPoint2D p;
1957  TObject2D obj;
1958  if (!intersect(l1,l2,obj)) {
1959  //Both lines are parallel
1960  double mod1=sqrt(square(l1.coefs[0])+square(l1.coefs[1]));
1961  double mod2=sqrt(square(l2.coefs[0])+square(l2.coefs[2]));
1962  bis.coefs[0]=l1.coefs[0]/mod1;
1963  bis.coefs[1]=l1.coefs[1]/mod1;
1964  bool sameSign;
1965  if (abs(bis.coefs[0])<geometryEpsilon) sameSign=(l1.coefs[1]*l2.coefs[1])>0;
1966  else sameSign=(l1.coefs[0]*l2.coefs[0])>0;
1967  if (sameSign) bis.coefs[2]=(l1.coefs[2]/mod1)+(l2.coefs[2]/mod2);
1968  else bis.coefs[2]=(l1.coefs[2]/mod1)-(l2.coefs[2]/mod2);
1969  } else if (obj.getPoint(p)) {
1970  //Both lines cross
1971  double ang1=atan2(-l1.coefs[0],l1.coefs[1]);
1972  double ang2=atan2(-l2.coefs[0],l2.coefs[1]);
1973  double ang=(ang1+ang2)/2;
1974  bis.coefs[0]=-sin(ang);
1975  bis.coefs[1]=cos(ang);
1976  bis.coefs[2]=-bis.coefs[0]*p.x-bis.coefs[1]*p.y;
1977  } else {
1978  bis=l1;
1979  bis.unitarize();
1980  }
1981 }
1982
1983 void math::getAngleBisector(const TLine3D &l1,const TLine3D &l2,TLine3D &bis) {
1984  TPlane p=TPlane(l1,l2); //May throw an exception
1985  TLine3D l1P,l2P;
1986  TLine2D bis2D;
1987  CPose3D pose,pose2;
1988  p.getAsPose3D(pose);
1989  pose2=-pose;
1990  project3D(l1,pose2,l1P);
1991  project3D(l2,pose2,l2P);
1992  getAngleBisector(TLine2D(l1P),TLine2D(l2P),bis2D);
1993  project3D(TLine3D(bis2D),pose,bis);
1994 }
1995
1996 bool math::traceRay(const vector<TPolygonWithPlane> &vec,const CPose3D &pose,double &dist) {
1997  dist=HUGE_VAL;
1998  double nDist=0;
1999  TLine3D lin;
2000  createFromPoseX(pose,lin);
2001  lin.unitarize();
2002  bool res=false;
2003  for (vector<TPolygonWithPlane>::const_iterator it=vec.begin();it!=vec.end();++it) if (::intersect(*it,lin,nDist,dist)) {
2004  res=true;
2005  dist=nDist;
2006  }
2007  return res;
2008 }
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's coordinates...
Definition: geometry.cpp:1642
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
bool contains(const TPoint2D &point) const
Check whether a point is inside the line.
const GLdouble * v
Definition: glew.h:1296
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 getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
void generate3DObject(TObject3D &obj) const
Project into 3D space.
TSegmentWithLine(const TPoint2D &p1, const TPoint2D &p2)
Definition: geometry.cpp:1188
TPolygon3D operator()(const TPolygon2D &poly2D)
Definition: geometry.cpp:1913
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.
void getBestFittingPlane(TPlane &p) const
Gets the best fitting plane, disregarding whether the polygon actually fits inside or not...
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:1861
#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:1652
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
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
void getPlanes(const std::vector< TPolygon3D > &polys, std::vector< TPlane > &planes)
Definition: geometry.cpp:1351
bool firstOrNonPresent(size_t i, const std::vector< MatchingVertex > &v)
Definition: geometry.cpp:1752
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
bool getLine(TLine3D &r) const
Gets the content as a line, returning false if the type is not adequate.
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
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
void destroy()
Definition: geometry.cpp:1102
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
GLuint coord
Definition: glew.h:4333
bool BASE_IMPEXP traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::poses::CPose3D &pose, double &dist)
Fast ray tracing method using polygons' properties.
Definition: geometry.cpp:1996
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:1727
void BASE_IMPEXP getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
Definition: geometry.cpp:1936
char fromObject(const TObject2D &obj)
Definition: geometry.cpp:1207
const GLfloat * c
Definition: glew.h:10088
void createPlaneFromPoseAndAxis(const CPose3D &pose, TPlane &plane, size_t axis)
Definition: geometry.cpp:1621
#define THROW_EXCEPTION(msg)
bool contains(const TPoint2D &point) const
Check whether a point is inside a segment.
void unsafeProjectPolygon(const TPolygon3D &poly, const CPose3D &pose, TPolygon2D &newPoly)
Definition: geometry.cpp:497
double distance(const TPoint2D &point) const
Distance from a given point.
GLfloat GLfloat v1
Definition: glew.h:1759
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...
GLdouble GLdouble t
Definition: glew.h:1303
vector< TSegment2D > l2
Definition: geometry.cpp:1094
void getAsPose2D(mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line.
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
size_t getRowCount() const
Returns the amount of rows in this matrix.
GLfloat GLfloat GLfloat v2
Definition: glew.h:1763
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
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:800
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
TPoint3D pBase
Base point.
union TTempIntersection::@76 data
Standard object for storing any 3D lightweight object.
#define M_PI
Definition: bits.h:78
TSegmentWithLine(const TSegment2D &s)
Definition: geometry.cpp:1185
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void getSegmentsWithLine(const TPolygon2D &poly, vector< TSegmentWithLine > &segs)
Definition: geometry.cpp:1200
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.
void getCenter(TPoint2D &p) const
Segment's central point.
struct BASE_IMPEXP TObject3D
bool contains(const TPoint3D &point) const
Check whether a point is inside the segment.
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
size_t getIndexOfMin(const T &e1, const T &e2, const T &e3)
Definition: geometry.cpp:1687
TSegment2D segment
Definition: geometry.cpp:1183
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:1955
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:968
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit insid...
struct BASE_IMPEXP TSegment3D
A sparse matrix container (with cells of any type), with iterators.
TTempIntersection(const TCommonRegion &common)
Definition: geometry.cpp:1159
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:792
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:614
GLdouble l
Definition: glew.h:5092
void getCenter(TPoint3D &p) const
Segment's central point.
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:829
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 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:788
GLuint GLenum GLenum transform
Definition: glew.h:8832
const std::vector< TSegment3D > & segs
Definition: geometry.cpp:1742
TTempIntersection & operator=(const TTempIntersection &t)
Definition: geometry.cpp:1165
TPolygonWithPlane()
Basic constructor.
Definition: geometry.h:42
unsigned char type
Definition: geometry.cpp:1097
GLdouble s
Definition: glew.h:1295
GLuint GLenum GLsizei GLsizei GLboolean void * points
Definition: glew.h:7748
bool PointIntoPolygon(double x, double y) const
Check if a point is inside the polygon.
Definition: CPolygon.h:61
void unitarize()
Unitarize line's normal vector.
A numeric matrix of compile-time fixed size.
vector< TSegment2D > l1
Definition: geometry.cpp:1093
2D segment, consisting of two points.
TCommonRegion & operator=(const TCommonRegion &r)
Definition: geometry.cpp:1122
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
GLsizei n
Definition: glew.h:5051
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:1630
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
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:1634
void unitarize()
Unitarize normal vector.
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
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:1524
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.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
3D Plane, represented by its equation
GLhandleARB obj
Definition: glew.h:3276
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
bool isNotNull(size_t nRow, size_t nCol) const
Checks whether an element of the matrix is not the default object.
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:1710
FCreatePolygon(const std::vector< TSegment3D > &s)
Definition: geometry.cpp:1743
TPoint2D point2
Destiny point.
TPolygon2D poly2D
Polygon, after being projected to the plane using inversePose.
Definition: geometry.h:39
GLint GLenum GLsizei GLint GLsizei const GLvoid * data
Definition: glew.h:1284
GLfloat GLfloat p
Definition: glew.h:10113
TCommonRegion(const TPoint2D &p)
Definition: geometry.cpp:1113
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
GLenum GLenum GLuint components
Definition: glew.h:7127
A class used to store a 2D point.
Definition: CPoint2D.h:36
bool isLine() const
Checks whether content is a line.
void unitarize()
Unitarize director vector.
int sign(T x)
Returns the sign of X as "1" or "-1".
Definition: bits.h:104
double distance(const TPoint3D &point) const
Distance to 3D point.
float cross(const mPointHull &O, const mPointHull &A, const mPointHull &B)
Definition: Plane.cpp:709
mrpt::poses::CPose3D pose
Plane's pose.
Definition: geometry.h:37
double director[3]
Director vector.
TPoint2D point1
Origin point.
unsigned char getType() const
Gets content type.
GLuint res
Definition: glew.h:7143
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
double y
X,Y coordinates.
void clear()
Completely removes all elements, although maintaining the matrix's size.
GLuint GLenum matrix
Definition: glew.h:8832
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
const CPose3D & pose
Definition: geometry.cpp:1910
FUnprojectPolygon2D(const CPose3D &p)
Definition: geometry.cpp:1912
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave...
GLdouble GLdouble z
Definition: glew.h:1464
TTempIntersection(const T2ListsOfSegments &segms)
Definition: geometry.cpp:1156
TPolygon3D operator()(const std::vector< MatchingVertex > &vertices)
Definition: geometry.cpp:1744
void createFromPoseAndAxis(const CPose3D &p, TLine3D &r, size_t axis)
Definition: geometry.cpp:779
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
Definition: glext.h:4993
bool isPoint() const
Checks whether content is a point.
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:1674
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane's equation.
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:1142
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:1606
TPoint2D * point
Definition: geometry.cpp:1099
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
double coefs[4]
Plane coefficients, stored as an array: .
bool contains(const TPoint3D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave...
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:270
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:1738
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
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:1757
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
mrpt::poses::CPose3D inversePose
Plane's inverse pose.
Definition: geometry.h:38
void unsafeProjectPoint(const TPoint3D &point, const CPose3D &pose, TPoint2D &newPoint)
Definition: geometry.cpp:493
TCommonRegion * common
Definition: geometry.cpp:1143
double distance(const TPoint3D &point) const
Distance between the line and a point.
const GLdouble * m
Definition: glew.h:5094
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:1638
bool BASE_IMPEXP areAligned(const std::vector< TPoint2D > &points)
Checks whether this set of points acceptably fits a 2D line.
Definition: geometry.cpp:847
bool compatibleBounds(const TPoint3D &min1, const TPoint3D &max1, const TPoint3D &min2, const TPoint3D &max2)
Definition: geometry.cpp:1333
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
size_t getIndexOfMax(const T &e1, const T &e2, const T &e3)
Definition: geometry.cpp:1692
INT64 INT64 INT64 remainder
Definition: wglew.h:919
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
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:796
TCommonRegion(const TCommonRegion &r)
Definition: geometry.cpp:1135
bool intersectAux(const TPolygon3D &p1, const TPlane &pl1, const TPolygon3D &p2, const TPlane &pl2, TObject3D &obj)
Definition: geometry.cpp:1314
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
TTempIntersection(const TTempIntersection &t)
Definition: geometry.cpp:1178
GLuint GLuint GLsizei GLenum type
Definition: glew.h:1167
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.
TPlane plane
Plane containing the polygon.
Definition: geometry.h:36
bool isPlane() const
Checks whether content is a plane.
Lightweight 2D point.
union TCommonRegion::@75 data
void getAsPose3DForcingOrigin(const TPoint3D &newOrigin, mrpt::poses::CPose3D &pose)
Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates...
const unsigned char GEOMETRIC_TYPE_POLYGON
Object type identifier for TPolygon2D or TPolygon3D.
Definition: glew.h:1752
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:777
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:1140
double phi
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
TCommonRegion(const TSegment2D &s)
Definition: geometry.cpp:1116
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:1511
void getMinAndMaxBounds(const std::vector< TPolygon3D > &v1, std::vector< TPoint3D > &minP, std::vector< TPoint3D > &maxP)
Definition: geometry.cpp:1358
TSegment2D * segment
Definition: geometry.cpp:1100
bool contains(const TPoint3D &point) const
Check whether a point is contained into the plane.
bool isLine() const
Checks whether content is a line.
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.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
2D line without bounds, represented by its equation .

 Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018