Main MRPT website > C++ reference for MRPT 1.5.5
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 |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
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;
409  poly.AddVertex( xs[0],ys[0] );
410  poly.AddVertex( xs[1],ys[1] );
411  poly.AddVertex( xs[2],ys[2] );
412  poly.AddVertex( xs[3],ys[3] );
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  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) {
780  CMatrixDouble44 m = p.getHomogeneousMatrixVal();
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 
790 }
791 
794 }
795 
798 }
799 
800 void math::createFromPoseAndVector(const CPose3D &p,const double (&vector)[3],TLine3D &r) {
801  CMatrixDouble44 m=p.getHomogeneousMatrixVal();
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) {
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  }
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;
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;
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;
1553  crossProduct3D(r1.director,r2.director,p.coefs);
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)) {
1759  unsigned char match=mat(searching,i)&mask;
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 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:1642
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:1188
TPolygon3D operator()(const TPolygon2D &poly2D)
Definition: geometry.cpp:1913
GLenum GLint GLuint mask
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:1861
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: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
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
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: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
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: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
void createPlaneFromPoseAndAxis(const CPose3D &pose, TPlane &plane, size_t axis)
Definition: geometry.cpp:1621
#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:1094
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:800
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: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.
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:1687
GLdouble s
Definition: glext.h:3602
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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
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:968
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
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
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:788
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
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:1093
2D segment, consisting of two points.
bool isPoint() const
Checks whether content is a point.
TCommonRegion & operator=(const TCommonRegion &r)
Definition: geometry.cpp:1122
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.
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:1634
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: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.
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:1710
void getCenter(TPoint3D &p) const
Segment&#39;s central point.
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
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
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:1910
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:1912
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
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:1674
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: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
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: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
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:1757
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:1143
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
size_t getIndexOfMax(const T &e1, const T &e2, const T &e3)
Definition: geometry.cpp:1692
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: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
GLuint res
Definition: glext.h:6298
TTempIntersection(const TTempIntersection &t)
Definition: geometry.cpp:1178
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.
void AddVertex(double x, double y)
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
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:1140
TCommonRegion(const TSegment2D &s)
Definition: geometry.cpp:1116
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: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.
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.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019