MRPT  1.9.9
geometry.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
13 #include <mrpt/math/CMatrixFixed.h>
14 #include <mrpt/math/CPolygon.h>
16 #include <mrpt/math/CVectorFixed.h>
17 #include <mrpt/math/TLine2D.h>
18 #include <mrpt/math/TLine3D.h>
19 #include <mrpt/math/TObject2D.h>
20 #include <mrpt/math/TObject3D.h>
21 #include <mrpt/math/TPose2D.h>
22 #include <mrpt/math/TPose3D.h>
23 #include <mrpt/math/data_utils.h>
24 #include <mrpt/math/geometry.h>
26 #include <Eigen/Dense>
27 
28 using namespace mrpt;
29 using namespace std;
30 using namespace mrpt::math;
31 
32 static double geometryEpsilon = 1e-5;
33 
36 /*---------------------------------------------------------------
37  Returns the closest point to a segment
38  ---------------------------------------------------------------*/
40  double Px, double Py, double x1, double y1, double x2, double y2,
41  double& out_x, 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  double Px, double Py, double x1, double y1, double x2, double y2,
79  double& out_x, double& out_y)
80 {
81  if (x1 == x2 && y1 == y2)
82  {
83  out_x = x1;
84  out_y = y1;
85  }
86  else
87  {
88  double Dx = x2 - x1;
89  double Dy = y2 - y1;
90  double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
91 
92  out_x = x1 + (Ratio * Dx);
93  out_y = y1 + (Ratio * Dy);
94  }
95 }
96 
97 /*---------------------------------------------------------------
98  Returns the sq. distance to closest point to a line
99  ---------------------------------------------------------------*/
101  double Px, double Py, double x1, double y1, double x2, double y2)
102 {
103  if (x1 == x2 && y1 == y2)
104  {
105  return square(Px - x1) + square(Py - y1);
106  }
107  else
108  {
109  double Dx = x2 - x1;
110  double Dy = y2 - y1;
111  double Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy);
112 
113  return square(x1 + (Ratio * Dx) - Px) + square(y1 + (Ratio * Dy) - Py);
114  }
115 }
116 
117 /*---------------------------------------------------------------
118  Intersect
119  ---------------------------------------------------------------*/
121  const double x1, const double y1, const double x2, const double y2,
122  const double x3, const double y3, const double x4, const double y4,
123  double& ix, double& iy)
124 {
125  double UpperX, UpperY, LowerX, LowerY, Ax, Bx, Cx, Ay, By, Cy, d, f, e,
126  Ratio;
127 
128  Ax = x2 - x1;
129  Bx = x3 - x4;
130 
131  if (Ax < 0)
132  {
133  LowerX = x2;
134  UpperX = x1;
135  }
136  else
137  {
138  UpperX = x2;
139  LowerX = x1;
140  }
141 
142  if (Bx > 0)
143  {
144  if (UpperX < x4 || x3 < LowerX) return false;
145  }
146  else if (UpperX < x3 || x4 < LowerX)
147  return false;
148 
149  Ay = y2 - y1;
150  By = y3 - y4;
151 
152  if (Ay < 0)
153  {
154  LowerY = y2;
155  UpperY = y1;
156  }
157  else
158  {
159  UpperY = y2;
160  LowerY = y1;
161  }
162 
163  if (By > 0)
164  {
165  if (UpperY < y4 || y3 < LowerY) return false;
166  }
167  else if (UpperY < y3 || y4 < LowerY)
168  return false;
169 
170  Cx = x1 - x3;
171  Cy = y1 - y3;
172  d = (By * Cx) - (Bx * Cy);
173  f = (Ay * Bx) - (Ax * By);
174 
175  if (f > 0)
176  {
177  if (d < 0 || d > f) return false;
178  }
179  else if (d > 0 || d < f)
180  return false;
181 
182  e = (Ax * Cy) - (Ay * Cx);
183 
184  if (f > 0)
185  {
186  if (e < 0 || e > f) return false;
187  }
188  else if (e > 0 || e < f)
189  return false;
190 
191  Ratio = (Ax * -By) - (Ay * -Bx);
192 
193  if (Ratio != 0)
194  {
195  Ratio = ((Cy * -Bx) - (Cx * -By)) / Ratio;
196  ix = x1 + (Ratio * Ax);
197  iy = y1 + (Ratio * Ay);
198  }
199  else
200  {
201  if ((Ax * -Cy) == (-Cx * Ay))
202  {
203  ix = x3;
204  iy = y3;
205  }
206  else
207  {
208  ix = x4;
209  iy = y4;
210  }
211  }
212  return true;
213 }
214 
215 /*---------------------------------------------------------------
216  Intersect
217  ---------------------------------------------------------------*/
219  const double x1, const double y1, const double x2, const double y2,
220  const double x3, const double y3, const double x4, const double y4,
221  float& ix, float& iy)
222 {
223  double x, y;
224  bool b = SegmentsIntersection(x1, y1, x2, y2, x3, y3, x4, y4, x, y);
225  ix = static_cast<float>(x);
226  iy = static_cast<float>(y);
227  return b;
228 }
229 
230 /*---------------------------------------------------------------
231  Intersect
232  ---------------------------------------------------------------*/
234  double px, double py, unsigned int polyEdges, const double* poly_xs,
235  const double* poly_ys)
236 {
237  unsigned int i, j;
238  bool res = false;
239 
240  if (polyEdges < 3) return res;
241 
242  j = polyEdges - 1;
243 
244  for (i = 0; i < polyEdges; i++)
245  {
246  if ((poly_ys[i] <= py && py < poly_ys[j]) || // an upward crossing
247  (poly_ys[j] <= py && py < poly_ys[i])) // a downward crossing
248  {
249  // compute the edge-ray intersect @ the x-coordinate
250  if (px - poly_xs[i] <
251  ((poly_xs[j] - poly_xs[i]) * (py - poly_ys[i]) /
252  (poly_ys[j] - poly_ys[i])))
253  res = !res;
254  }
255  j = i;
256  }
257 
258  return res;
259 }
260 
261 /*---------------------------------------------------------------
262  Intersect
263  ---------------------------------------------------------------*/
265  double px, double py, unsigned int polyEdges, const double* poly_xs,
266  const double* poly_ys)
267 {
268  unsigned int i, j;
269  double minDist = 1e20f;
270 
271  // Is the point INTO?
272  if (pointIntoPolygon2D(px, py, polyEdges, poly_xs, poly_ys)) return 0;
273 
274  // Compute the closest distance from the point to any segment:
275  j = polyEdges - 1;
276 
277  for (i = 0; i < polyEdges; i++)
278  {
279  // segment: [j]-[i]
280  // ----------------------
281  double closestX, closestY;
283  px, py, poly_xs[j], poly_ys[j], poly_xs[i], poly_ys[i], closestX,
284  closestY);
285 
286  minDist = min(d, minDist);
287 
288  // For next iter:
289  j = i;
290  }
291 
292  return minDist;
293 }
294 
295 /*---------------------------------------------------------------
296  minDistBetweenLines
297  --------------------------------------------------------------- */
299  double p1_x, double p1_y, double p1_z, double p2_x, double p2_y,
300  double p2_z, double p3_x, double p3_y, double p3_z, double p4_x,
301  double p4_y, double p4_z, double& x, double& y, double& z, double& dist)
302 {
303  const double EPS = 1e-30f;
304 
305  double p13_x, p13_y, p13_z;
306  double p43_x, p43_y, p43_z;
307  double p21_x, p21_y, p21_z;
308 
309  double d1343, d4321, d1321, d4343, d2121;
310  double numer, denom;
311 
312  p13_x = p1_x - p3_x;
313  p13_y = p1_y - p3_y;
314  p13_z = p1_z - p3_z;
315 
316  p43_x = p4_x - p3_x;
317  p43_y = p4_y - p3_y;
318  p43_z = p4_z - p3_z;
319 
320  if (fabs(p43_x) < EPS && fabs(p43_y) < EPS && fabs(p43_z) < EPS)
321  return false;
322 
323  p21_x = p2_x - p1_x;
324  p21_y = p2_y - p1_y;
325  p21_z = p2_z - p1_z;
326  if (fabs(p21_x) < EPS && fabs(p21_y) < EPS && fabs(p21_z) < EPS)
327  return false;
328 
329  d1343 = p13_x * p43_x + p13_y * p43_y + p13_z * p43_z;
330  d4321 = p43_x * p21_x + p43_y * p21_y + p43_z * p21_z;
331  d1321 = p13_x * p21_x + p13_y * p21_y + p13_z * p21_z;
332  d4343 = p43_x * p43_x + p43_y * p43_y + p43_z * p43_z;
333  d2121 = p21_x * p21_x + p21_y * p21_y + p21_z * p21_z;
334 
335  denom = d2121 * d4343 - d4321 * d4321;
336  if (fabs(denom) < EPS) return false;
337 
338  numer = d1343 * d4321 - d1321 * d4343;
339 
340  double mua = numer / denom;
341  double mub = (d1343 + d4321 * mua) / d4343;
342  double pa_x, pa_y, pa_z;
343  double pb_x, pb_y, pb_z;
344 
345  pa_x = p1_x + mua * p21_x;
346  pa_y = p1_y + mua * p21_y;
347  pa_z = p1_z + mua * p21_z;
348 
349  pb_x = p3_x + mub * p43_x;
350  pb_y = p3_y + mub * p43_y;
351  pb_z = p3_z + mub * p43_z;
352 
353  dist = (double)sqrt(
354  square(pa_x - pb_x) + square(pa_y - pb_y) + square(pa_z - pb_z));
355 
356  // the mid point:
357  x = 0.5 * (pa_x + pb_x);
358  y = 0.5 * (pa_y + pb_y);
359  z = 0.5 * (pa_z + pb_z);
360 
361  return true;
362 }
363 
364 /*---------------------------------------------------------------
365  Rectangles Intersect
366  ---------------------------------------------------------------*/
368  double R1_x_min, double R1_x_max, double R1_y_min, double R1_y_max,
369  double R2_x_min, double R2_x_max, double R2_y_min, double R2_y_max,
370  double R2_pose_x, double R2_pose_y, double R2_pose_phi)
371 {
372  // Compute the rotated R2:
373  // ----------------------------------------
374  CVectorDouble xs(4), ys(4);
375  double ccos = cos(R2_pose_phi);
376  double ssin = sin(R2_pose_phi);
377 
378  xs[0] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_min;
379  ys[0] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_min;
380 
381  xs[1] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_min;
382  ys[1] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_min;
383 
384  xs[2] = R2_pose_x + ccos * R2_x_max - ssin * R2_y_max;
385  ys[2] = R2_pose_y + ssin * R2_x_max + ccos * R2_y_max;
386 
387  xs[3] = R2_pose_x + ccos * R2_x_min - ssin * R2_y_max;
388  ys[3] = R2_pose_y + ssin * R2_x_min + ccos * R2_y_max;
389 
390  // Test for one vertice being inside the other rectangle:
391  // -------------------------------------------------------
392  if (R1_x_min <= xs[0] && xs[0] <= R1_x_max && R1_y_min <= ys[0] &&
393  ys[0] <= R1_y_max)
394  return true;
395  if (R1_x_min <= xs[1] && xs[1] <= R1_x_max && R1_y_min <= ys[1] &&
396  ys[1] <= R1_y_max)
397  return true;
398  if (R1_x_min <= xs[2] && xs[2] <= R1_x_max && R1_y_min <= ys[2] &&
399  ys[2] <= R1_y_max)
400  return true;
401  if (R1_x_min <= xs[3] && xs[3] <= R1_x_max && R1_y_min <= ys[3] &&
402  ys[3] <= R1_y_max)
403  return true;
404 
405  CPolygon poly;
406  poly.AddVertex(xs[0], ys[0]);
407  poly.AddVertex(xs[1], ys[1]);
408  poly.AddVertex(xs[2], ys[2]);
409  poly.AddVertex(xs[3], ys[3]);
410 
411  if (poly.PointIntoPolygon(R1_x_min, R1_y_min)) return true;
412  if (poly.PointIntoPolygon(R1_x_max, R1_y_min)) return true;
413  if (poly.PointIntoPolygon(R1_x_max, R1_y_max)) return true;
414  if (poly.PointIntoPolygon(R1_x_min, R1_y_max)) return true;
415 
416  // Test for intersections:
417  // ----------------------------------------
418  double ix, iy;
419 
420  for (int idx = 0; idx < 4; idx++)
421  {
423  R1_x_min, R1_y_min, R1_x_max, R1_y_min, xs[idx], ys[idx],
424  xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
425  return true;
427  R1_x_max, R1_y_min, R1_x_max, R1_y_max, xs[idx], ys[idx],
428  xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
429  return true;
431  R1_x_max, R1_y_max, R1_x_min, R1_y_max, xs[idx], ys[idx],
432  xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
433  return true;
435  R1_x_min, R1_y_max, R1_x_min, R1_y_min, xs[idx], ys[idx],
436  xs[(idx + 1) % 4], ys[(idx + 1) % 4], ix, iy))
437  return true;
438  }
439 
440  // No intersections:
441  return false;
442 }
443 
444 // Auxiliary functions needed to avoid code repetition and unnecesary
445 // recalculations
446 template <class T2D, class U2D, class T3D, class U3D>
448  const T3D& o1, const U3D& o2, const mrpt::math::TPlane& p,
450 {
451  T3D proj1;
452  U3D proj2;
453  // Project into 3D plane, ignoring Z coordinate.
454  TPose3D pose;
455  TPlane(p).getAsPose3D(pose);
456  TPose3D poseNeg = -pose;
457  project3D(o1, poseNeg, proj1);
458  project3D(o2, poseNeg, proj2);
459  T2D proj1_2D;
460  U2D proj2_2D;
461  proj1.generate2DObject(proj1_2D);
462  proj2.generate2DObject(proj2_2D);
463  // Compute easier intersection in 2D space
464  TObject2D obj2D;
465  if (intersect(proj1_2D, proj2_2D, obj2D))
466  {
467  TObject3D tmp;
468  obj2D.generate3DObject(tmp);
469  // Undo projection
470  project3D(tmp, pose, obj);
471  return true;
472  }
473  else
474  return false;
475 }
477  const mrpt::math::TSegment3D& s1, const mrpt::math::TSegment3D& s2,
479 {
480  // Move in a free coordinate, searching for minima and maxima.
481  size_t i1 = 0;
482  while (abs(lin.director[i1]) < geometryEpsilon) i1++;
483  TSegment3D s11 = (s1[0][i1] > s1[1][i1]) ? TSegment3D(s1[1], s1[0]) : s1;
484  TSegment3D s21 = (s2[0][i1] > s2[1][i1]) ? TSegment3D(s2[1], s2[0]) : s2;
485  TPoint3D pMin = ((s11[0][i1] < s21[0][i1]) ? s21 : s11)[0];
486  TPoint3D pMax = ((s11[1][i1] < s21[1][i1]) ? s11 : s21)[1];
487  if (abs(pMax[i1] - pMin[i1]) < geometryEpsilon)
488  { // Intersection is a point
489  obj = pMax;
490  return true;
491  }
492  else if (pMax[i1] < pMin[i1])
493  return false; // No intersection
494  else
495  {
496  obj = TSegment3D(pMin, pMax); // Intersection is a segment
497  return true;
498  }
499 }
501  const TSegment2D& s1, const TSegment2D& s2, const TLine2D& lin,
502  TObject2D& obj)
503 {
504  // Move in a free coordinate, searching for minima and maxima
505  size_t i1 = (abs(lin.coefs[0]) >= geometryEpsilon) ? 1 : 0;
506  TSegment2D s11 = (s1[0][i1] > s1[1][i1]) ? TSegment2D(s1[1], s1[0]) : s1;
507  TSegment2D s21 = (s2[0][i1] > s2[1][i1]) ? TSegment2D(s2[1], s2[0]) : s2;
508  TPoint2D pMin = ((s11[0][i1] < s21[0][i1]) ? s21 : s11)[0];
509  TPoint2D pMax = ((s11[1][i1] < s21[1][i1]) ? s11 : s21)[1];
510  if (abs(pMax[i1] - pMin[i1]) < geometryEpsilon)
511  { // Intersection is a point
512  obj = pMax;
513  return true;
514  }
515  else if (pMax[i1] < pMin[i1])
516  return false; // No intersection
517  else
518  {
519  obj = TSegment2D(pMin, pMax); // Intersection is a segment
520  return true;
521  }
522 }
523 inline void unsafeProjectPoint(
524  const TPoint3D& point, const TPose3D& pose, TPoint2D& newPoint)
525 {
526  TPoint3D dummy;
527  pose.composePoint(point, dummy);
528  newPoint.x = dummy.x;
529  newPoint.y = dummy.y;
530 }
532  const TPolygon3D& poly, const TPose3D& pose, TPolygon2D& newPoly)
533 {
534  size_t N = poly.size();
535  newPoly.resize(N);
536  for (size_t i = 0; i < N; i++)
537  unsafeProjectPoint(poly[i], pose, newPoly[i]);
538 }
540  const TPolygonWithPlane& p1, const TLine3D& l2, double& d, double bestKnown)
541 {
542  // LINE MUST BE UNITARY
543  TObject3D obj;
544  TPoint3D p;
545  if (intersect(p1.plane, l2, obj))
546  if (obj.getPoint(p))
547  {
548  for (size_t i = 0; i < 3; i++)
549  if (abs(l2.director[i]) > geometryEpsilon)
550  {
551  d = (p[i] - l2.pBase[i]) / l2.director[i];
552  break;
553  }
554  if (d < 0 || d > bestKnown) return false;
555  TPolygon2D newPoly;
556  TPoint2D newP;
557  unsafeProjectPoint(p, p1.inversePose, newP);
558  unsafeProjectPolygon(p1.poly, p1.inversePose, newPoly);
559  return newPoly.contains(newP);
560  }
561  return false;
562 }
564  const TPolygonWithPlane& p1, const TPolygonWithPlane& p2, TObject3D& obj)
565 {
566  if (!intersect(p1.plane, p2.plane, obj)) return false;
567  TLine3D lin3D;
568  TObject3D aux;
569  if (obj.getLine(lin3D))
570  {
571  TLine3D lin3D1, lin3D2;
572  TLine2D lin2D1, lin2D2;
573  TObject2D obj2D1, obj2D2;
574  project3D(lin3D, p1.inversePose, lin3D1);
575  project3D(lin3D, p2.inversePose, lin3D2);
576  lin3D1.generate2DObject(lin2D1);
577  lin3D2.generate2DObject(lin2D2);
578  if (intersect(p1.poly2D, lin2D1, obj2D1) &&
579  intersect(p2.poly2D, lin2D2, obj2D2))
580  {
581  TObject3D obj3D1, obj3D2, obj3Dp1, obj3Dp2;
582  obj2D1.generate3DObject(obj3D1);
583  obj2D2.generate3DObject(obj3D2);
584  project3D(obj3D1, p1.pose, obj3Dp1);
585  project3D(obj3D2, p2.pose, obj3Dp2);
586  TPoint3D po1, po2;
587  TSegment3D s1, s2;
588  if (obj3D1.getPoint(po1))
589  s1 = TSegment3D(po1, po1);
590  else
591  obj3D1.getSegment(s1);
592  if (obj3D2.getPoint(po2))
593  s2 = TSegment3D(po2, po2);
594  else
595  obj3D2.getSegment(s2);
596  return intersectInCommonLine(s1, s2, lin3D, obj);
597  }
598  else
599  return false;
600  }
601  else
602  {
603  TObject2D obj2D;
604  if (intersect(p1.poly2D, p2.poly2D, obj2D))
605  {
606  obj2D.generate3DObject(aux);
607  project3D(aux, p1.pose, obj);
608  return true;
609  }
610  else
611  return false;
612  }
613 }
614 // End of auxiliary methods
616 {
619  // inversePose = -pose;
620  CMatrixDouble44 P_inv;
623 
625 }
627  const vector<TPolygon3D>& oldPolys, vector<TPolygonWithPlane>& newPolys)
628 {
629  size_t N = oldPolys.size();
630  newPolys.resize(N);
631  for (size_t i = 0; i < N; i++) newPolys[i] = oldPolys[i];
632 }
633 
634 bool math::intersect(const TSegment3D& s1, const TSegment3D& s2, TObject3D& obj)
635 {
636  TObject3D irr;
637  auto l = TLine3D(s1);
638  if (!intersect(l, TLine3D(s2), irr)) return false;
639  if (irr.isPoint())
640  {
641  // Both lines cross in a point.
642  TPoint3D p;
643  irr.getPoint(p);
644  if (s1.contains(p) && s2.contains(p))
645  {
646  obj = p;
647  return true;
648  }
649  else
650  return false;
651  }
652  else
653  return intersectInCommonLine(s1, s2, l, obj);
654 }
655 
656 bool math::intersect(const TSegment3D& s1, const TPlane& p1, TObject3D& obj)
657 {
658  if (!intersect(TLine3D(s1), p1, obj)) return false;
659  if (obj.isLine())
660  {
661  // Segment is fully inside the plane, so it is the return value.
662  obj = s1;
663  return true;
664  }
665  else
666  {
667  // Segment's line intersects the plane in a point. This may be or not be
668  // part of the segment.
669  TPoint3D p;
670  if (!obj.getPoint(p)) return false;
671  return s1.contains(p);
672  }
673 }
674 
675 bool math::intersect(const TSegment3D& s1, const TLine3D& r1, TObject3D& obj)
676 {
677  if (!intersect(TLine3D(s1), r1, obj)) return false;
678  if (obj.isLine())
679  {
680  // Segment's line is the other line.
681  obj = s1;
682  return true;
683  }
684  else
685  {
686  // Segment's line and the other line cross in a point, which may be or
687  // not be inside the segment.
688  TPoint3D p;
689  if (!obj.getPoint(p)) return false;
690  return s1.contains(p);
691  }
692 }
693 
694 bool math::intersect(const TPlane& p1, const TPlane& p2, TObject3D& obj)
695 {
696  TLine3D lin;
697  crossProduct3D(p1.coefs, p2.coefs, lin.director);
698  if ((abs(lin.director[0]) < geometryEpsilon) &&
699  (abs(lin.director[1]) < geometryEpsilon) &&
700  (abs(lin.director[2]) < geometryEpsilon))
701  {
702  // Planes are parallel
703  for (size_t i = 0; i < 3; i++)
704  if (abs(p1.coefs[i] * p2.coefs[3] - p1.coefs[3] * p2.coefs[i]) >=
706  return false;
707  // Planes are the same
708  obj = p1;
709  return true;
710  }
711  else
712  {
713  // Planes cross in a line whose director vector is already calculated
714  // (normal to both planes' normal).
715  // The following process manages to create a random point in the line
716  // without loss of generality and almost without conditional sentences.
717  size_t i1 = 0;
718  while (abs(lin.director[i1]) < geometryEpsilon) i1++;
719  // At this point, i1 points to a coordinate (0->x, 1->y, 2->z) in which
720  // we can move freely.
721  // If we arbitrarily assign this coordinate to 0, we'll find a suitable
722  // base point by solving both planes' equations.
723  size_t c1 = (i1 + 1) % 3, c2 = (i1 + 2) % 3;
724  lin.pBase[i1] = 0.0;
725  lin.pBase[c1] =
726  (p2.coefs[3] * p1.coefs[c2] - p1.coefs[3] * p2.coefs[c2]) /
727  lin.director[i1];
728  lin.pBase[c2] =
729  (p2.coefs[c1] * p1.coefs[3] - p1.coefs[c1] * p2.coefs[3]) /
730  lin.director[i1];
731  lin.unitarize();
732  obj = lin;
733  return true;
734  }
735 }
736 
737 bool math::intersect(const TPlane& p1, const TLine3D& r2, TObject3D& obj)
738 {
739  // double
740  // n=p1.coefs[0]*r2.director[0]+p1.coefs[1]*r2.director[1]+p1.coefs[2]*r2.director[2];
741  double n = dotProduct<3, double>(p1.coefs, r2.director);
742  double e = p1.evaluatePoint(r2.pBase);
743  if (abs(n) < geometryEpsilon)
744  {
745  // Plane's normal and line's director are orthogonal, so both are
746  // parallel
747  if (abs(e) < geometryEpsilon)
748  {
749  // Line is contained in plane.
750  obj = r2;
751  return true;
752  }
753  else
754  return false;
755  }
756  else
757  {
758  // Plane and line cross in a point.
759  double t = e / n;
760  TPoint3D p;
761  p.x = r2.pBase.x - t * r2.director[0];
762  p.y = r2.pBase.y - t * r2.director[1];
763  p.z = r2.pBase.z - t * r2.director[2];
764  obj = p;
765  return true;
766  }
767 }
768 
769 bool math::intersect(const TLine3D& r1, const TLine3D& r2, TObject3D& obj)
770 {
771  double u, d[3];
772  TPoint3D p;
773  const static size_t c1[] = {1, 2, 0};
774  const static size_t c2[] = {2, 0, 1};
775  // With indirect memory accesses, almost all the code goes in a single loop.
776  for (size_t i = 0; i < 3; i++)
777  {
778  double sysDet = -r1.director[c1[i]] * r2.director[c2[i]] +
779  r2.director[c1[i]] * r1.director[c2[i]];
780  if (abs(sysDet) < geometryEpsilon) continue;
781  // We've found a coordinate in which we can solve the associated system
782  d[c1[i]] = r2.pBase[c1[i]] - r1.pBase[c1[i]];
783  d[c2[i]] = r2.pBase[c2[i]] - r1.pBase[c2[i]];
784  u = (r1.director[c1[i]] * d[c2[i]] - r1.director[c2[i]] * d[c1[i]]) /
785  sysDet;
786  for (size_t k = 0; k < 3; k++) p[k] = r2.pBase[k] + u * r2.director[k];
787  if (r1.contains(p))
788  {
789  obj = p;
790  return true;
791  }
792  else
793  return false;
794  }
795  // Lines are parallel
796  if (r1.contains(r2.pBase))
797  {
798  // Lines are the same
799  obj = r1;
800  return true;
801  }
802  else
803  return false;
804 }
805 
806 bool math::intersect(const TLine2D& r1, const TLine2D& r2, TObject2D& obj)
807 {
808  double sysDet = r1.coefs[0] * r2.coefs[1] - r1.coefs[1] * r2.coefs[0];
809  if (abs(sysDet) >= geometryEpsilon)
810  {
811  // Resulting point comes simply from solving an equation.
812  TPoint2D p;
813  p.x = (r1.coefs[1] * r2.coefs[2] - r1.coefs[2] * r2.coefs[1]) / sysDet;
814  p.y = (r1.coefs[2] * r2.coefs[0] - r1.coefs[0] * r2.coefs[2]) / sysDet;
815  obj = p;
816  return true;
817  }
818  else
819  {
820  // Lines are parallel
821  if (abs(r1.coefs[0] * r2.coefs[2] - r1.coefs[2] * r2.coefs[0]) >=
822  geometryEpsilon ||
823  abs(r1.coefs[1] * r2.coefs[2] - r1.coefs[2] * r2.coefs[1]) >=
825  return false;
826 
827  // Lines are the same
828  obj = r1;
829  return true;
830  }
831 }
832 
833 bool math::intersect(const TLine2D& r1, const TSegment2D& s2, TObject2D& obj)
834 {
835  if (!intersect(r1, TLine2D(s2), obj)) return false;
836  TPoint2D p;
837  if (obj.isLine())
838  {
839  // Segment is inside the line
840  obj = s2;
841  return true;
842  }
843  else if (obj.getPoint(p))
844  return s2.contains(p); // Both lines cross in a point.
845  return false;
846 }
847 
848 bool math::intersect(const TSegment2D& s1, const TSegment2D& s2, TObject2D& obj)
849 {
850  auto lin = TLine2D(s1);
851  if (!intersect(lin, TLine2D(s2), obj)) return false;
852  TPoint2D p;
853  if (obj.isLine())
854  return intersectInCommonLine(
855  s1, s2, lin, obj); // Segments' lines are parallel
856  else if (obj.getPoint(p))
857  return s1.contains(p) &&
858  s2.contains(p); // Segments' lines cross in a point
859  return false;
860 }
861 
862 double math::getAngle(const TPlane& s1, const TPlane& s2)
863 {
864  double c = 0, n1 = 0, n2 = 0;
865  for (size_t i = 0; i < 3; i++)
866  {
867  c += s1.coefs[i] * s2.coefs[i];
868  n1 += s1.coefs[i] * s1.coefs[i];
869  n2 += s2.coefs[i] + s2.coefs[i];
870  }
871  double s = sqrt(n1 * n2);
872  if (s < geometryEpsilon) throw std::logic_error("Invalid plane(s)");
873  if (abs(s) < abs(c))
874  return (c / s < 0) ? M_PI : 0;
875  else
876  return acos(c / s);
877 }
878 
879 double math::getAngle(const TPlane& s1, const TLine3D& r2)
880 {
881  double c = 0, n1 = 0, n2 = 0;
882  for (size_t i = 0; i < 3; i++)
883  {
884  c += s1.coefs[i] * r2.director[i];
885  n1 += s1.coefs[i] * s1.coefs[i];
886  n2 += r2.director[i] * r2.director[i];
887  }
888  double s = sqrt(n1 * n2);
889  if (s < geometryEpsilon) throw std::logic_error("Invalid plane or line");
890  if (abs(s) < abs(c))
891  return M_PI * sign(c / s) / 2;
892  else
893  return asin(c / s);
894 }
895 
896 double math::getAngle(const TLine3D& r1, const TLine3D& r2)
897 {
898  double c = 0, n1 = 0, n2 = 0;
899  for (size_t i = 0; i < 3; i++)
900  {
901  c += r1.director[i] * r2.director[i];
902  n1 += r1.director[i] * r1.director[i];
903  n2 += r2.director[i] * r2.director[i];
904  }
905  double s = sqrt(n1 * n2);
906  if (s < geometryEpsilon) throw std::logic_error("Invalid line(s)");
907  if (abs(s) < abs(c))
908  return (c / s < 0) ? M_PI : 0;
909  else
910  return acos(c / s);
911 }
912 
913 double math::getAngle(const TLine2D& r1, const TLine2D& r2)
914 {
915  const double ang1 = std::atan2(-r1.coefs[0], r1.coefs[1]);
916  const double ang2 = std::atan2(-r2.coefs[0], r2.coefs[1]);
917  return mrpt::math::wrapToPi(ang2 - ang1);
918 }
919 
920 // Auxiliary method
921 void createFromPoseAndAxis(const TPose3D& p, TLine3D& r, size_t axis)
922 {
923  CMatrixDouble44 m;
924  p.getHomogeneousMatrix(m);
925  for (size_t i = 0; i < 3; i++)
926  {
927  r.pBase[i] = m(i, 3);
928  r.director[i] = m(i, axis);
929  }
930 }
931 // End of auxiliary method
932 
934 {
936 }
937 
939 {
941 }
942 
944 {
946 }
947 
949  const TPose3D& p, const double (&vector)[3], TLine3D& r)
950 {
951  CMatrixDouble44 m;
952  p.getHomogeneousMatrix(m);
953  for (size_t i = 0; i < 3; i++)
954  {
955  r.pBase[i] = m(i, 3);
956  r.director[i] = 0;
957  for (size_t j = 0; j < 3; j++) r.director[i] += m(i, j) * vector[j];
958  }
959 }
960 
962 {
963  r.coefs[0] = cos(p.phi);
964  r.coefs[1] = -sin(p.phi);
965  r.coefs[2] = -r.coefs[0] * p.x - r.coefs[1] * p.y;
966 }
967 
969 {
970  r.coefs[0] = sin(p.phi);
971  r.coefs[1] = cos(p.phi);
972  r.coefs[2] = -r.coefs[0] * p.x - r.coefs[1] * p.y;
973 }
974 
976  const TPose2D& p, const double (&vector)[2], TLine2D& r)
977 {
978  double c = cos(p.phi);
979  double s = sin(p.phi);
980  r.coefs[0] = vector[0] * c + vector[1] * s;
981  r.coefs[1] = -vector[0] * s + vector[1] * c;
982  r.coefs[2] = -r.coefs[0] * p.x - r.coefs[1] * p.y;
983 }
984 
985 bool math::conformAPlane(const std::vector<TPoint3D>& points)
986 {
987  size_t N = points.size();
988  if (N < 3) return false;
989  CMatrixDouble mat(N - 1, 3);
990  const TPoint3D& orig = points[N - 1];
991  for (size_t i = 0; i < N - 1; i++)
992  {
993  const TPoint3D& p = points[i];
994  mat(i, 0) = p.x - orig.x;
995  mat(i, 1) = p.y - orig.y;
996  mat(i, 2) = p.z - orig.z;
997  }
998  return mat.rank(geometryEpsilon) == 2;
999 }
1000 
1001 bool math::conformAPlane(const std::vector<TPoint3D>& points, TPlane& p)
1002 {
1003  return abs(getRegressionPlane(points, p)) < geometryEpsilon;
1004 }
1005 
1006 bool math::areAligned(const std::vector<TPoint2D>& points)
1007 {
1008  size_t N = points.size();
1009  if (N < 2) return false;
1010  CMatrixDouble mat(N - 1, 2);
1011  const TPoint2D& orig = points[N - 1];
1012  for (size_t i = 0; i < N - 1; i++)
1013  {
1014  const TPoint2D& p = points[i];
1015  mat(i, 0) = p.x - orig.x;
1016  mat(i, 1) = p.y - orig.y;
1017  }
1018  return mat.rank(geometryEpsilon) == 1;
1019 }
1020 
1021 bool math::areAligned(const std::vector<TPoint2D>& points, TLine2D& r)
1022 {
1023  if (!areAligned(points)) return false;
1024  const TPoint2D& p0 = points[0];
1025  for (size_t i = 1;; i++)
1026  {
1027  try
1028  {
1029  r = TLine2D(p0, points[i]);
1030  return true;
1031  }
1032  catch (logic_error&)
1033  {
1034  }
1035  }
1036 }
1037 
1038 bool math::areAligned(const std::vector<TPoint3D>& points)
1039 {
1040  size_t N = points.size();
1041  if (N < 2) return false;
1042  CMatrixDouble mat(N - 1, 3);
1043  const TPoint3D& orig = points[N - 1];
1044  for (size_t i = 0; i < N - 1; i++)
1045  {
1046  const TPoint3D& p = points[i];
1047  mat(i, 0) = p.x - orig.x;
1048  mat(i, 1) = p.y - orig.y;
1049  mat(i, 2) = p.z - orig.z;
1050  }
1051  return mat.rank(geometryEpsilon) == 1;
1052 }
1053 
1054 bool math::areAligned(const std::vector<TPoint3D>& points, TLine3D& r)
1055 {
1056  if (!areAligned(points)) return false;
1057  const TPoint3D& p0 = points[0];
1058  for (size_t i = 1;; i++) try
1059  {
1060  r = TLine3D(p0, points[i]);
1061  return true;
1062  }
1063  catch (logic_error&)
1064  {
1065  }
1066 }
1067 
1069  const TLine3D& line, const TPose3D& newXYpose, TLine3D& newLine)
1070 {
1071  newXYpose.composePoint(line.pBase, newLine.pBase);
1072  CMatrixDouble44 mat;
1073  newXYpose.getHomogeneousMatrix(mat);
1074  for (size_t i = 0; i < 3; i++)
1075  {
1076  newLine.director[i] = 0;
1077  for (size_t j = 0; j < 3; j++)
1078  newLine.director[i] += mat(i, j) * line.director[j];
1079  }
1080  newLine.unitarize();
1081 }
1082 
1084  const TPlane& plane, const TPose3D& newXYpose, TPlane& newPlane)
1085 {
1086  CMatrixDouble44 mat;
1087  newXYpose.getHomogeneousMatrix(mat);
1088  for (size_t i = 0; i < 3; i++)
1089  {
1090  newPlane.coefs[i] = 0;
1091  for (size_t j = 0; j < 3; j++)
1092  newPlane.coefs[i] += mat(i, j) * plane.coefs[j];
1093  }
1094  // VORSICHT! NO INTENTEN HACER ESTO EN SUS CASAS (nota: comentar sí o sí,
1095  // más tarde)
1096  // La idea es mantener la distancia al nuevo origen igual a la distancia del
1097  // punto original antes de proyectar.
1098  // newPlane.coefs[3]=plane.evaluatePoint(TPoint3D(TPose3D(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]));
1099  CMatrixDouble44 HMinv;
1100  newXYpose.getInverseHomogeneousMatrix(HMinv);
1101  newPlane.coefs[3] =
1102  plane.evaluatePoint(TPoint3D(HMinv(0, 3), HMinv(1, 3), HMinv(2, 3))) *
1103  sqrt(
1104  squareNorm<3, double>(newPlane.coefs) /
1105  squareNorm<3, double>(plane.coefs));
1106  newPlane.unitarize();
1107 }
1108 
1110  const TPolygon3D& polygon, const TPose3D& newXYpose, TPolygon3D& newPolygon)
1111 {
1112  size_t N = polygon.size();
1113  newPolygon.resize(N);
1114  for (size_t i = 0; i < N; i++)
1115  project3D(polygon[i], newXYpose, newPolygon[i]);
1116 }
1117 
1119  const TObject3D& object, const TPose3D& newXYpose, TObject3D& newObject)
1120 {
1121  switch (object.getType())
1122  {
1123  case GEOMETRIC_TYPE_POINT:
1124  {
1125  TPoint3D p, p2;
1126  object.getPoint(p);
1127  project3D(p, newXYpose, p2);
1128  newObject = p2;
1129  break;
1130  }
1132  {
1133  TSegment3D p, p2;
1134  object.getSegment(p);
1135  project3D(p, newXYpose, p2);
1136  newObject = p2;
1137  break;
1138  }
1139  case GEOMETRIC_TYPE_LINE:
1140  {
1141  TLine3D p, p2;
1142  object.getLine(p);
1143  project3D(p, newXYpose, p2);
1144  newObject = p2;
1145  break;
1146  }
1147  case GEOMETRIC_TYPE_PLANE:
1148  {
1149  TPlane p, p2;
1150  object.getPlane(p);
1151  project3D(p, newXYpose, p2);
1152  newObject = p2;
1153  break;
1154  }
1156  {
1157  TPolygon3D p, p2;
1158  object.getPolygon(p);
1159  project3D(p, newXYpose, p2);
1160  newObject = p2;
1161  break;
1162  }
1163  default:
1164  newObject = TObject3D();
1165  }
1166 }
1167 
1169  const TPoint2D& point, const TPose2D& newXpose, TPoint2D& newPoint)
1170 {
1171  newPoint = newXpose.composePoint(point);
1172 }
1173 
1175  const TLine2D& line, const TPose2D& newXpose, TLine2D& newLine)
1176 {
1177  double c = cos(newXpose.phi);
1178  double s = sin(newXpose.phi);
1179  newLine.coefs[0] = line.coefs[0] * c - line.coefs[1] * s;
1180  newLine.coefs[1] = line.coefs[1] * c + line.coefs[0] * s;
1181  newLine.coefs[2] = line.coefs[2] - (newLine.coefs[0] * newXpose.x +
1182  newLine.coefs[1] * newXpose.y);
1183  return;
1184 }
1185 
1187  const TPolygon2D& line, const TPose2D& newXpose, TPolygon2D& newLine)
1188 {
1189  size_t N = line.size();
1190  newLine.resize(N);
1191  for (size_t i = 0; i < N; i++) newLine[i] = newXpose + line[i];
1192  return;
1193 }
1194 
1196  const TObject2D& obj, const TPose2D& newXpose, TObject2D& newObject)
1197 {
1198  switch (obj.getType())
1199  {
1200  case GEOMETRIC_TYPE_POINT:
1201  {
1202  TPoint2D p, p2;
1203  obj.getPoint(p);
1204  project2D(p, newXpose, p2);
1205  newObject = p2;
1206  break;
1207  }
1209  {
1210  TSegment2D p, p2;
1211  obj.getSegment(p);
1212  project2D(p, newXpose, p2);
1213  newObject = p2;
1214  break;
1215  }
1216  case GEOMETRIC_TYPE_LINE:
1217  {
1218  TLine2D p, p2;
1219  obj.getLine(p);
1220  project2D(p, newXpose, p2);
1221  newObject = p2;
1222  break;
1223  }
1225  {
1226  TPolygon2D p, p2;
1227  obj.getPolygon(p);
1228  project2D(p, newXpose, p2);
1229  newObject = p2;
1230  break;
1231  }
1232  default:
1233  newObject = TObject2D();
1234  }
1235 }
1236 
1237 bool math::intersect(const TPolygon2D& p1, const TSegment2D& s2, TObject2D& obj)
1238 {
1239  auto l2 = TLine2D(s2);
1240  if (!intersect(p1, l2, obj)) return false;
1241  TPoint2D p;
1242  TSegment2D s;
1243  if (obj.getPoint(p))
1244  return s2.contains(p);
1245  else if (obj.getSegment(s))
1246  return intersectInCommonLine(s, s2, l2, obj);
1247  return false;
1248 }
1249 
1250 bool math::intersect(const TPolygon2D& p1, const TLine2D& r2, TObject2D& obj)
1251 {
1252  // Proceeding: project polygon so that the line happens to be y=0 (phi=0).
1253  // Then, search this new polygons for intersections with the X axis (very
1254  // simple).
1255  if (p1.size() < 3) return false;
1256  TPose2D pose, poseNeg;
1257  r2.getAsPose2D(pose);
1258  poseNeg = TPose2D(0, 0, 0) - pose;
1259  TPolygon2D projPoly;
1260  project2D(p1, poseNeg, projPoly);
1261  size_t N = projPoly.size();
1262  projPoly.push_back(projPoly[0]);
1263  double pre = projPoly[0].y;
1264  vector<TPoint2D> pnts;
1265  pnts.reserve(2);
1266  for (size_t i = 1; i <= N; i++)
1267  {
1268  double cur = projPoly[i].y;
1269  if (abs(cur) < geometryEpsilon)
1270  {
1271  if (abs(pre) < geometryEpsilon)
1272  {
1273  pnts.resize(2);
1274  pnts[0] = projPoly[i - 1];
1275  pnts[1] = projPoly[i];
1276  break;
1277  }
1278  else
1279  pnts.push_back(projPoly[i]);
1280  }
1281  else if ((abs(pre) >= geometryEpsilon) && (sign(cur) != sign(pre)))
1282  {
1283  double a = projPoly[i - 1].x;
1284  double c = projPoly[i].x;
1285  double x = a - pre * (c - a) / (cur - pre);
1286  pnts.emplace_back(x, 0);
1287  }
1288  pre = cur;
1289  }
1290  // All results must undo the initial projection
1291  switch (pnts.size())
1292  {
1293  case 0:
1294  return false;
1295  case 1:
1296  {
1297  TPoint2D p;
1298  project2D(pnts[0], pose, p);
1299  obj = p;
1300  return true;
1301  }
1302  case 2:
1303  {
1304  TSegment2D s;
1305  project2D(TSegment2D(pnts[0], pnts[1]), pose, s);
1306  obj = s;
1307  return true;
1308  }
1309  default:
1310  throw std::logic_error("Polygon is not convex");
1311  }
1312 }
1313 
1314 // Auxiliary structs and code for 2D polygon intersection
1316 {
1317  vector<TSegment2D> l1;
1318  vector<TSegment2D> l2;
1319 };
1320 // TODO: Replace by std::variant
1322 {
1323  unsigned char type; // 0 -> point, 1-> segment, any other-> empty
1324  union {
1325  TPoint2D* point = nullptr;
1327  } data;
1328  void destroy()
1329  {
1330  switch (type)
1331  {
1332  case 0:
1333  delete data.point;
1334  break;
1335  case 1:
1336  delete data.segment;
1337  break;
1338  }
1339  type = 255;
1340  }
1341  TCommonRegion(const TPoint2D& p) : type(0) { data.point = new TPoint2D(p); }
1343  {
1344  data.segment = new TSegment2D(s);
1345  }
1346  ~TCommonRegion() { destroy(); }
1348  {
1349  if (&r == this) return *this;
1350  destroy();
1351  switch (type = r.type)
1352  {
1353  case 0:
1354  data.point = new TPoint2D(*(r.data.point));
1355  break;
1356  case 1:
1357  data.segment = new TSegment2D(*(r.data.segment));
1358  break;
1359  }
1360  return *this;
1361  }
1362  TCommonRegion(const TCommonRegion& r) : type(0) { operator=(r); }
1363 };
1364 // TODO: Replace by std::variant
1366 {
1367  unsigned char type; // 0->two lists of segments, 1-> common region
1368  union {
1369  T2ListsOfSegments* segms = nullptr;
1371  } data;
1372  void destroy()
1373  {
1374  switch (type)
1375  {
1376  case 0:
1377  delete data.segms;
1378  break;
1379  case 1:
1380  delete data.common;
1381  break;
1382  }
1383  type = 255;
1384  };
1386  {
1387  data.segms = new T2ListsOfSegments(segms);
1388  }
1390  {
1391  data.common = new TCommonRegion(common);
1392  }
1393  ~TTempIntersection() { destroy(); }
1395  {
1396  if (&t == this) return *this;
1397  destroy();
1398  switch (type = t.type)
1399  {
1400  case 0:
1401  data.segms = new T2ListsOfSegments(*(t.data.segms));
1402  break;
1403  case 1:
1404  data.common = new TCommonRegion(*(t.data.common));
1405  break;
1406  }
1407  return *this;
1408  }
1409  TTempIntersection(const TTempIntersection& t) : type(0) { operator=(t); }
1410 };
1412 {
1415  explicit TSegmentWithLine(const TSegment2D& s) : segment(s)
1416  {
1417  line = TLine2D(s[0], s[1]);
1418  }
1419  TSegmentWithLine(const TPoint2D& p1, const TPoint2D& p2) : segment(p1, p2)
1420  {
1421  line = TLine2D(p1, p2);
1422  }
1423  TSegmentWithLine() = default;
1424 };
1426  const TSegmentWithLine& s1, const TSegmentWithLine& s2, TObject2D& obj)
1427 {
1428  if (!intersect(s1.line, s2.line, obj)) return false;
1429  if (obj.isLine())
1430  return intersectInCommonLine(s1.segment, s2.segment, s1.line, obj);
1431  TPoint2D p;
1432  obj.getPoint(p);
1433  return s1.segment.contains(p) && s2.segment.contains(p);
1434 }
1435 void getSegmentsWithLine(const TPolygon2D& poly, vector<TSegmentWithLine>& segs)
1436 {
1437  size_t N = poly.size();
1438  segs.resize(N);
1439  for (size_t i = 0; i < N - 1; i++)
1440  segs[i] = TSegmentWithLine(poly[i], poly[i + 1]);
1441  segs[N - 1] = TSegmentWithLine(poly[N - 1], poly[0]);
1442 }
1443 
1444 inline char fromObject(const TObject2D& obj)
1445 {
1446  switch (obj.getType())
1447  {
1448  case GEOMETRIC_TYPE_POINT:
1449  return 'P';
1451  return 'S';
1452  case GEOMETRIC_TYPE_LINE:
1453  return 'L';
1455  return 'O';
1456  default:
1457  return 'U';
1458  }
1459 }
1460 
1462  const TPolygon2D& /*p1*/, const TPolygon2D& /*p2*/, TObject2D& /*obj*/)
1463 {
1464  THROW_EXCEPTION("TODO");
1465 #if 0
1466  return false; //TODO
1467 
1468  CSparseMatrixTemplate<TObject2D> intersections=CSparseMatrixTemplate<TObject2D>(p1.size(),p2.size());
1469  std::vector<TSegmentWithLine> segs1,segs2;
1470  getSegmentsWithLine(p1,segs1);
1471  getSegmentsWithLine(p2,segs2);
1472  unsigned int hmInters=0;
1473  for (size_t i=0;i<segs1.size();i++) {
1474  const TSegmentWithLine &s1=segs1[i];
1475  for (size_t j=0;j<segs2.size();j++) if (intersect(s1,segs2[j],obj)) {
1476  intersections(i,j)=obj;
1477  hmInters++;
1478  }
1479  }
1480  for (size_t i=0;i<intersections.rows();i++) {
1481  for (size_t j=0;j<intersections.cols();j++) cout<<fromObject(intersections(i,j));
1482  cout<<'\n';
1483  }
1484  if (hmInters==0) {
1485  if (p1.contains(p2[0])) {
1486  obj=p2;
1487  return true;
1488  } else if (p2.contains(p1[0])) {
1489  obj=p1;
1490  return true;
1491  } else return false;
1492  }
1493  //ESTO ES UNA PESADILLA, HAY CIEN MILLONES DE CASOS DISTINTOS A LA HORA DE RECORRER LAS POSIBILIDADES...
1494  /*
1495  Dividir cada segmento en sus distintas partes según sus intersecciones, y generar un nuevo polígono.
1496  Recorrer de segmento en segmento, por cada uno de los dos lados (recorriendo desde un punto común a otro;
1497  en un polígono se escoge el camino secuencial directo, mientras que del otro se escoge, de los dos posibles,
1498  el que no se corta con ningún elemento del primero).
1499  Seleccionar, para cada segmento, si está dentro o fuera.
1500  Parece fácil, pero es una puta mierda.
1501  TODO: hacer en algún momento de mucho tiempo libre...
1502  */
1503 
1504  /* ¿Seguir? */
1505  return false;
1506 #endif
1507 }
1508 
1509 bool math::intersect(const TPolygon3D& p1, const TSegment3D& s2, TObject3D& obj)
1510 {
1511  TPlane p;
1512  if (!p1.getPlane(p)) return false;
1513  if (!intersect(p, s2, obj)) return false;
1514  TPoint3D pnt;
1515  TSegment3D sgm;
1516  if (obj.getPoint(pnt))
1517  {
1518  TPose3D pose;
1519  p.getAsPose3DForcingOrigin(p1[0], pose);
1520  CMatrixDouble44 HMinv;
1521  pose.getInverseHomogeneousMatrix(HMinv);
1522  TPose3D poseNeg;
1523  poseNeg.fromHomogeneousMatrix(HMinv);
1524  TPolygon3D projPoly;
1525  TPoint3D projPnt;
1526  project3D(p1, poseNeg, projPoly);
1527  project3D(pnt, poseNeg, projPnt);
1528  return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
1529  }
1530  else if (obj.getSegment(sgm))
1531  return intersectInCommonPlane<TPolygon2D, TSegment2D>(p1, s2, p, obj);
1532  return false;
1533 }
1534 
1535 bool math::intersect(const TPolygon3D& p1, const TLine3D& r2, TObject3D& obj)
1536 {
1537  TPlane p;
1538  if (!p1.getPlane(p)) return false;
1539  if (!intersect(p, r2, obj)) return false;
1540  TPoint3D pnt;
1541  if (obj.getPoint(pnt))
1542  {
1543  TPose3D pose;
1544  p.getAsPose3DForcingOrigin(p1[0], pose);
1545  CMatrixDouble44 HMinv;
1546  pose.getInverseHomogeneousMatrix(HMinv);
1547  TPose3D poseNeg;
1548  poseNeg.fromHomogeneousMatrix(HMinv);
1549  TPolygon3D projPoly;
1550  TPoint3D projPnt;
1551  project3D(p1, poseNeg, projPoly);
1552  project3D(pnt, poseNeg, projPnt);
1553  return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
1554  }
1555  else if (obj.isLine())
1556  return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, r2, p, obj);
1557  return false;
1558 }
1559 
1560 bool math::intersect(const TPolygon3D& p1, const TPlane& p2, TObject3D& obj)
1561 {
1562  TPlane p;
1563  if (!p1.getPlane(p)) return false;
1564  if (!intersect(p, p2, obj)) return false;
1565  TLine3D ln;
1566  if (obj.isPlane())
1567  {
1568  // Polygon is inside the plane
1569  obj = p1;
1570  return true;
1571  }
1572  else if (obj.getLine(ln))
1573  return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln, p, obj);
1574  return false;
1575 }
1576 
1577 // Auxiliary function2
1579  const TPolygon3D& p1, const TPlane& pl1, const TPolygon3D& p2,
1580  const TPlane& pl2, TObject3D& obj)
1581 {
1582  if (!intersect(pl1, pl2, obj)) return false;
1583  TLine3D ln;
1584  if (obj.isPlane())
1585  return intersectInCommonPlane<TPolygon2D, TPolygon2D>(
1586  p1, p2, pl1, obj); // Polygons are on the same plane
1587  else if (obj.getLine(ln))
1588  {
1589  TObject3D obj1, obj2;
1590  if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln, pl1, obj1))
1591  return false;
1592  if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p2, ln, pl2, obj2))
1593  return false;
1594  TPoint3D po1, po2;
1595  TSegment3D s1, s2;
1596  if (obj1.getPoint(po1))
1597  s1 = TSegment3D(po1, po1);
1598  else
1599  obj1.getSegment(s1);
1600  if (obj2.getPoint(po2))
1601  s2 = TSegment3D(po2, po2);
1602  else
1603  obj2.getSegment(s2);
1604  return intersectInCommonLine(s1, s2, ln, obj);
1605  }
1606  return false;
1607 }
1608 
1610  const TPoint3D& min1, const TPoint3D& max1, const TPoint3D& min2,
1611  const TPoint3D& max2)
1612 {
1613  for (size_t i = 0; i < 3; i++)
1614  if ((min1[i] > max2[i]) || (min2[i] > max1[i])) return false;
1615  return true;
1616 }
1617 // End of auxiliary functions
1618 
1619 bool math::intersect(const TPolygon3D& p1, const TPolygon3D& p2, TObject3D& obj)
1620 {
1621  TPoint3D min1, max1, min2, max2;
1622  getPrismBounds(p1, min1, max1);
1623  getPrismBounds(p2, min2, max2);
1624  if (!compatibleBounds(min1, max1, min2, max2)) return false;
1625  TPlane pl1, pl2;
1626  if (!p1.getPlane(pl1)) return false;
1627  if (!p2.getPlane(pl2)) return false;
1628  return intersectAux(p1, pl1, p2, pl2, obj);
1629 }
1630 
1631 // Auxiliary function
1632 inline void getPlanes(
1633  const std::vector<TPolygon3D>& polys, std::vector<TPlane>& planes)
1634 {
1635  size_t N = polys.size();
1636  planes.resize(N);
1637  for (size_t i = 0; i < N; i++) getRegressionPlane(polys[i], planes[i]);
1638 }
1639 
1640 // Auxiliary functions
1642  const std::vector<TPolygon3D>& v1, std::vector<TPoint3D>& minP,
1643  std::vector<TPoint3D>& maxP)
1644 {
1645  minP.resize(0);
1646  maxP.resize(0);
1647  size_t N = v1.size();
1648  minP.reserve(N);
1649  maxP.reserve(N);
1650  TPoint3D p1, p2;
1651  for (const auto& it : v1)
1652  {
1653  getPrismBounds(it, p1, p2);
1654  minP.push_back(p1);
1655  maxP.push_back(p2);
1656  }
1657 }
1658 
1660  const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2,
1662 {
1663  std::vector<TPlane> w1, w2;
1664  getPlanes(v1, w1);
1665  getPlanes(v2, w2);
1666  std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
1667  getMinAndMaxBounds(v1, minBounds1, maxBounds1);
1668  getMinAndMaxBounds(v2, minBounds2, maxBounds2);
1669  size_t M = v1.size(), N = v2.size();
1670  objs.clear();
1671  objs.resize(M, N);
1672  TObject3D obj;
1673  for (size_t i = 0; i < M; i++)
1674  for (size_t j = 0; j < N; j++)
1675  if (!compatibleBounds(
1676  minBounds1[i], maxBounds1[i], minBounds2[j], maxBounds2[j]))
1677  continue;
1678  else if (intersectAux(v1[i], w1[i], v2[j], w2[j], obj))
1679  objs(i, j) = obj;
1680  return objs.getNonNullElements();
1681 }
1682 
1684  const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2,
1685  std::vector<TObject3D>& objs)
1686 {
1687  objs.resize(0);
1688  std::vector<TPlane> w1, w2;
1689  getPlanes(v1, w1);
1690  getPlanes(v2, w2);
1691  std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
1692  getMinAndMaxBounds(v1, minBounds1, maxBounds1);
1693  getMinAndMaxBounds(v2, minBounds2, maxBounds2);
1694  TObject3D obj;
1695  auto itP1 = w1.begin();
1696  auto itMin1 = minBounds1.begin();
1697  auto itMax1 = maxBounds1.begin();
1698  for (auto it1 = v1.begin(); it1 != v1.end();
1699  ++it1, ++itP1, ++itMin1, ++itMax1)
1700  {
1701  const TPolygon3D& poly1 = *it1;
1702  const TPlane& plane1 = *itP1;
1703  auto itP2 = w2.begin();
1704  const TPoint3D &min1 = *itMin1, max1 = *itMax1;
1705  auto itMin2 = minBounds2.begin();
1706  auto itMax2 = maxBounds2.begin();
1707  for (auto it2 = v2.begin(); it2 != v2.end();
1708  ++it2, ++itP2, ++itMin2, ++itMax2)
1709  if (!compatibleBounds(min1, max1, *itMin2, *itMax2))
1710  continue;
1711  else if (intersectAux(poly1, plane1, *it2, *itP2, obj))
1712  objs.push_back(obj);
1713  }
1714  return objs.size();
1715 }
1716 
1717 bool math::intersect(const TObject2D& o1, const TObject2D& o2, TObject2D& obj)
1718 {
1719  TPoint2D p1, p2;
1720  TSegment2D s1, s2;
1721  TLine2D l1, l2;
1722  TPolygon2D po1, po2;
1723  if (o1.getPoint(p1))
1724  {
1725  obj = p1;
1726  if (o2.getPoint(p2))
1727  return distance(p1, p2) < geometryEpsilon;
1728  else if (o2.getSegment(s2))
1729  return s2.contains(p1);
1730  else if (o2.getLine(l2))
1731  return l2.contains(p1);
1732  else if (o2.getPolygon(po2))
1733  return po2.contains(p1); // else return false;
1734  }
1735  else if (o1.getSegment(s1))
1736  {
1737  if (o2.getPoint(p2))
1738  {
1739  if (s1.contains(p2))
1740  {
1741  obj = p2;
1742  return true;
1743  } // else return false;
1744  }
1745  else if (o2.getSegment(s2))
1746  return intersect(s1, s2, obj);
1747  else if (o2.getLine(l2))
1748  return intersect(s1, l2, obj);
1749  else if (o2.getPolygon(po2))
1750  return intersect(s1, po2, obj); // else return false;
1751  }
1752  else if (o1.getLine(l1))
1753  {
1754  if (o2.getPoint(p2))
1755  {
1756  if (l1.contains(p2))
1757  {
1758  obj = p2;
1759  return true;
1760  } // else return false;
1761  }
1762  else if (o2.getSegment(s2))
1763  return intersect(l1, s2, obj);
1764  else if (o2.getLine(l2))
1765  return intersect(l1, l2, obj);
1766  else if (o2.getPolygon(po2))
1767  return intersect(l1, po2, obj); // else return false;
1768  }
1769  else if (o1.getPolygon(po1))
1770  {
1771  if (o2.getPoint(p2))
1772  {
1773  if (po1.contains(p2))
1774  {
1775  obj = p2;
1776  return true;
1777  } // else return false;
1778  }
1779  else if (o2.getSegment(s2))
1780  return intersect(po1, s2, obj);
1781  else if (o2.getLine(l2))
1782  return intersect(po1, l2, obj);
1783  else if (o2.getPolygon(po2))
1784  return intersect(po1, po2, obj); // else return false;
1785  } // else return false;
1786  return false;
1787 }
1788 
1789 bool math::intersect(const TObject3D& o1, const TObject3D& o2, TObject3D& obj)
1790 {
1791  TPoint3D p1, p2;
1792  TSegment3D s1, s2;
1793  TLine3D l1, l2;
1794  TPolygon3D po1, po2;
1795  TPlane pl1, pl2;
1796  if (o1.getPoint(p1))
1797  {
1798  obj = p1;
1799  if (o2.getPoint(p2))
1800  return distance(p1, p2) < geometryEpsilon;
1801  else if (o2.getSegment(s2))
1802  return s2.contains(p1);
1803  else if (o2.getLine(l2))
1804  return l2.contains(p1);
1805  else if (o2.getPolygon(po2))
1806  return po2.contains(p1);
1807  else if (o2.getPlane(pl2))
1808  return pl2.contains(p1); // else return false;
1809  }
1810  else if (o1.getSegment(s1))
1811  {
1812  if (o2.getPoint(p2))
1813  {
1814  if (s1.contains(p2))
1815  {
1816  obj = p2;
1817  return true;
1818  } // else return false;
1819  }
1820  else if (o2.getSegment(s2))
1821  return intersect(s1, s2, obj);
1822  else if (o2.getLine(l2))
1823  return intersect(s1, l2, obj);
1824  else if (o2.getPolygon(po2))
1825  return intersect(s1, po2, obj);
1826  else if (o2.getPlane(pl2))
1827  return intersect(s1, pl2, obj); // else return false;
1828  }
1829  else if (o1.getLine(l1))
1830  {
1831  if (o2.getPoint(p2))
1832  {
1833  if (l1.contains(p2))
1834  {
1835  obj = p2;
1836  return true;
1837  } // else return false;
1838  }
1839  else if (o2.getSegment(s2))
1840  return intersect(l1, s2, obj);
1841  else if (o2.getLine(l2))
1842  return intersect(l1, l2, obj);
1843  else if (o2.getPolygon(po2))
1844  return intersect(l1, po2, obj);
1845  else if (o2.getPlane(pl2))
1846  return intersect(l2, pl2, obj); // else return false;
1847  }
1848  else if (o1.getPolygon(po1))
1849  {
1850  if (o2.getPoint(p2))
1851  {
1852  if (po1.contains(p2))
1853  {
1854  obj = p2;
1855  return true;
1856  } // else return false;
1857  }
1858  else if (o2.getSegment(s2))
1859  return intersect(po1, s2, obj);
1860  else if (o2.getLine(l2))
1861  return intersect(po1, l2, obj);
1862  else if (o2.getPolygon(po2))
1863  return intersect(po1, po2, obj);
1864  else if (o2.getPlane(pl2))
1865  return intersect(po1, pl2, obj); // else return false;
1866  }
1867  else if (o1.getPlane(pl1))
1868  {
1869  if (o2.getPoint(p2))
1870  {
1871  if (pl1.contains(p2))
1872  {
1873  obj = p2;
1874  return true;
1875  } // else return false;
1876  }
1877  else if (o2.getSegment(s2))
1878  return intersect(pl1, s2, obj);
1879  else if (o2.getLine(l2))
1880  return intersect(pl1, l2, obj);
1881  else if (o2.getPlane(pl2))
1882  return intersect(pl1, pl2, obj); // else return false;
1883  } // else return false;
1884  return false;
1885 }
1886 
1887 double math::distance(const TPoint2D& p1, const TPoint2D& p2)
1888 {
1889  double dx = p2.x - p1.x;
1890  double dy = p2.y - p1.y;
1891  return sqrt(dx * dx + dy * dy);
1892 }
1893 
1894 double math::distance(const TPoint3D& p1, const TPoint3D& p2)
1895 {
1896  double dx = p2.x - p1.x;
1897  double dy = p2.y - p1.y;
1898  double dz = p2.z - p1.z;
1899  return sqrt(dx * dx + dy * dy + dz * dz);
1900 }
1901 
1903  const std::vector<TPoint2D>& poly, TPoint2D& pMin, TPoint2D& pMax)
1904 {
1905  size_t N = poly.size();
1906  if (N < 1) throw logic_error("Empty polygon");
1907  pMin = poly[0];
1908  pMax = poly[0];
1909  for (size_t i = 1; i < N; i++)
1910  {
1911  pMin.x = min(pMin.x, poly[i].x);
1912  pMin.y = min(pMin.y, poly[i].y);
1913  pMax.x = max(pMax.x, poly[i].x);
1914  pMax.y = max(pMax.y, poly[i].y);
1915  }
1916 }
1917 
1918 double math::distance(const TLine2D& r1, const TLine2D& r2)
1919 {
1920  if (abs(r1.coefs[0] * r2.coefs[1] - r2.coefs[0] * r1.coefs[1]) <
1922  {
1923  // Lines are parallel
1924  size_t i1 = (abs(r1.coefs[0]) < geometryEpsilon) ? 0 : 1;
1925  TPoint2D p;
1926  p[i1] = 0.0;
1927  p[1 - i1] = -r1.coefs[2] / r1.coefs[1 - i1];
1928  return r2.distance(p);
1929  }
1930  else
1931  return 0; // Lines cross in some point
1932 }
1933 
1934 double math::distance(const TLine3D& r1, const TLine3D& r2)
1935 {
1936  if (abs(getAngle(r1, r2)) < geometryEpsilon)
1937  return r1.distance(r2.pBase); // Lines are parallel
1938  else
1939  {
1940  // We build a plane parallel to r2 which contains r1
1941  TPlane p;
1942  crossProduct3D(r1.director, r2.director, p.coefs);
1943  p.coefs[3] =
1944  -(p.coefs[0] * r1.pBase[0] + p.coefs[1] * r1.pBase[1] +
1945  p.coefs[2] * r1.pBase[2]);
1946  return p.distance(r2.pBase);
1947  }
1948 }
1949 
1950 double math::distance(const TPlane& p1, const TPlane& p2)
1951 {
1952  if (abs(getAngle(p1, p2)) < geometryEpsilon)
1953  {
1954  // Planes are parallel
1955  TPoint3D p(0, 0, 0);
1956  for (size_t i = 0; i < 3; i++)
1957  if (abs(p1.coefs[i]) >= geometryEpsilon)
1958  {
1959  p[i] = -p1.coefs[3] / p1.coefs[i];
1960  break;
1961  }
1962  return p2.distance(p);
1963  }
1964  else
1965  return 0; // Planes cross in a line
1966 }
1967 
1968 double math::distance(const TPolygon2D& p1, const TPolygon2D& p2)
1969 {
1970  MRPT_UNUSED_PARAM(p1);
1971  MRPT_UNUSED_PARAM(p2);
1972  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TPolygon2D)");
1973 }
1974 
1975 double math::distance(const TPolygon2D& p1, const TSegment2D& s2)
1976 {
1977  MRPT_UNUSED_PARAM(p1);
1978  MRPT_UNUSED_PARAM(s2);
1979  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TSegment)");
1980 }
1981 
1982 double math::distance(const TPolygon2D& p1, const TLine2D& l2)
1983 {
1984  MRPT_UNUSED_PARAM(p1);
1985  MRPT_UNUSED_PARAM(l2);
1986  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TLine2D)");
1987 }
1988 
1989 double math::distance(const TPolygon3D& p1, const TPolygon3D& p2)
1990 {
1991  MRPT_UNUSED_PARAM(p1);
1992  MRPT_UNUSED_PARAM(p2);
1993  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPolygon3D");
1994 }
1995 
1996 double math::distance(const TPolygon3D& p1, const TSegment3D& s2)
1997 {
1998  MRPT_UNUSED_PARAM(p1);
1999  MRPT_UNUSED_PARAM(s2);
2000  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TSegment3D");
2001 }
2002 
2003 double math::distance(const TPolygon3D& p1, const TLine3D& l2)
2004 {
2005  MRPT_UNUSED_PARAM(p1);
2006  MRPT_UNUSED_PARAM(l2);
2007  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TLine3D");
2008 }
2009 
2010 double math::distance(const TPolygon3D& po, const TPlane& pl)
2011 {
2012  MRPT_UNUSED_PARAM(po);
2013  MRPT_UNUSED_PARAM(pl);
2014  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPlane");
2015 }
2016 
2018  const std::vector<TPoint3D>& poly, TPoint3D& pMin, TPoint3D& pMax)
2019 {
2020  size_t N = poly.size();
2021  if (N < 1) throw logic_error("Empty polygon");
2022  pMin = poly[0];
2023  pMax = poly[0];
2024  for (size_t i = 1; i < N; i++)
2025  {
2026  pMin.x = min(pMin.x, poly[i].x);
2027  pMin.y = min(pMin.y, poly[i].y);
2028  pMin.z = min(pMin.z, poly[i].z);
2029  pMax.x = max(pMax.x, poly[i].x);
2030  pMax.y = max(pMax.y, poly[i].y);
2031  pMax.z = max(pMax.z, poly[i].z);
2032  }
2033 }
2034 
2035 void createPlaneFromPoseAndAxis(const TPose3D& pose, TPlane& plane, size_t axis)
2036 {
2037  plane.coefs[3] = 0;
2038  CMatrixDouble44 m;
2039  pose.getHomogeneousMatrix(m);
2040  for (size_t i = 0; i < 3; i++)
2041  {
2042  plane.coefs[i] = m(i, axis);
2043  plane.coefs[3] -= plane.coefs[i] * m(i, 3);
2044  }
2045 }
2046 
2047 void math::createPlaneFromPoseXY(const TPose3D& pose, TPlane& plane)
2048 {
2049  createPlaneFromPoseAndAxis(pose, plane, 2);
2050 }
2051 
2052 void math::createPlaneFromPoseXZ(const TPose3D& pose, TPlane& plane)
2053 {
2054  createPlaneFromPoseAndAxis(pose, plane, 1);
2055 }
2056 
2057 void math::createPlaneFromPoseYZ(const TPose3D& pose, TPlane& plane)
2058 {
2059  createPlaneFromPoseAndAxis(pose, plane, 0);
2060 }
2061 
2063  const TPose3D& pose, const double (&normal)[3], TPlane& plane)
2064 {
2065  plane.coefs[3] = 0;
2066  CMatrixDouble44 m;
2067  pose.getHomogeneousMatrix(m);
2068  for (size_t i = 0; i < 3; i++)
2069  {
2070  plane.coefs[i] = 0;
2071  for (size_t j = 0; j < 3; j++) plane.coefs[i] += normal[j] * m(i, j);
2072  plane.coefs[3] -= plane.coefs[i] * m(i, 3);
2073  }
2074 }
2075 
2077  const double (&vec)[3], uint8_t coord, CMatrixDouble44& m)
2078 {
2079  // Assumes vector is unitary.
2080  // coord: 0=x, 1=y, 2=z.
2081  const uint8_t coord1 = (coord + 1) % 3;
2082  const uint8_t coord2 = (coord + 2) % 3;
2083  m.setZero();
2084  m(3, 3) = 1.0;
2085  for (size_t i = 0; i < 3; i++) m(i, coord) = vec[i];
2086  m(0, coord1) = 0;
2087  double h = hypot(vec[1], vec[2]);
2088  if (h < geometryEpsilon)
2089  {
2090  m(1, coord1) = 1;
2091  m(2, coord1) = 0;
2092  }
2093  else
2094  {
2095  m(1, coord1) = -vec[2] / h;
2096  m(2, coord1) = vec[1] / h;
2097  }
2098  m(0, coord2) = m(1, coord) * m(2, coord1) - m(2, coord) * m(1, coord1);
2099  m(1, coord2) = m(2, coord) * m(0, coord1) - m(0, coord) * m(2, coord1);
2100  m(2, coord2) = m(0, coord) * m(1, coord1) - m(1, coord) * m(0, coord1);
2101 }
2102 
2103 double math::getRegressionLine(const vector<TPoint2D>& points, TLine2D& line)
2104 {
2105  CVectorFixedDouble<2> means;
2106  CMatrixDouble22 covars;
2107  covariancesAndMean(points, covars, means);
2108 
2109  std::vector<double> eigenVals;
2110  CMatrixDouble22 eigenVecs;
2111  covars.eig_symmetric(eigenVecs, eigenVals);
2112 
2113  // size_t selected = (eigenVal[0] >= eigenVal[1]) ? 0 : 1;
2114  line.coefs[0] = -eigenVecs(1, 1); // 1: largest eigenVal
2115  line.coefs[1] = eigenVecs(0, 1);
2116  line.coefs[2] = -line.coefs[0] * means[0] - line.coefs[1] * means[1];
2117  return std::sqrt(eigenVals[0] / eigenVals[1]);
2118 }
2119 
2120 template <class T>
2121 inline size_t getIndexOfMin(const T& e1, const T& e2, const T& e3)
2122 {
2123  return (e1 < e2) ? ((e1 < e3) ? 0 : 2) : ((e2 < e3) ? 1 : 2);
2124 }
2125 
2126 double math::getRegressionLine(const vector<TPoint3D>& points, TLine3D& line)
2127 {
2128  CVectorFixedDouble<3> means;
2129  CMatrixDouble33 covars;
2130  covariancesAndMean(points, covars, means);
2131 
2132  std::vector<double> eigenVal;
2133  CMatrixDouble33 eigenVec;
2134  covars.eig_symmetric(eigenVec, eigenVal);
2135 
2136  const size_t selected = 2; // sorted: max eigenvalue
2137  for (size_t i = 0; i < 3; i++)
2138  {
2139  line.pBase[i] = means[i];
2140  line.director[i] = eigenVec(i, selected);
2141  }
2142  size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
2143  return std::sqrt((eigenVal[i1] + eigenVal[i2]) / eigenVal[selected]);
2144 }
2145 
2146 double math::getRegressionPlane(const vector<TPoint3D>& points, TPlane& plane)
2147 {
2148  vector<double> means;
2149  CMatrixDouble33 covars;
2150  covariancesAndMean(points, covars, means);
2151 
2152  std::vector<double> eigenVal;
2153  CMatrixDouble33 eigenVec;
2154  covars.eig_symmetric(eigenVec, eigenVal);
2155 
2156  for (size_t i = 0; i < 3; ++i)
2157  if (eigenVal[i] < 0 && std::abs(eigenVal[i]) < geometryEpsilon)
2158  eigenVal[i] = 0;
2159 
2160  const size_t selected = 0; // sorted: minimum eigenVal
2161  plane.coefs[3] = 0;
2162  for (size_t i = 0; i < 3; i++)
2163  {
2164  plane.coefs[i] = eigenVec(i, selected);
2165  plane.coefs[3] -= plane.coefs[i] * means[i];
2166  }
2167  size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
2168  return std::sqrt(eigenVal[selected] / (eigenVal[i1] + eigenVal[i2]));
2169 }
2170 
2172  const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys)
2173 {
2174  std::vector<TSegment3D> tmp;
2175  assemblePolygons(segms, polys, tmp);
2176 }
2177 
2179 {
2180  size_t seg1;
2181  size_t seg2;
2182  bool seg1Point; // true for point2, false for point1
2183  bool seg2Point; // same
2184  MatchingVertex() = default;
2185  MatchingVertex(size_t s1, size_t s2, bool s1p, bool s2p)
2186  : seg1(s1), seg2(s2), seg1Point(s1p), seg2Point(s2p)
2187  {
2188  }
2189 };
2191 {
2192  public:
2193  const std::vector<TSegment3D>& segs;
2194  FCreatePolygon(const std::vector<TSegment3D>& s) : segs(s) {}
2195  TPolygon3D operator()(const std::vector<MatchingVertex>& vertices)
2196  {
2197  TPolygon3D res;
2198  size_t N = vertices.size();
2199  res.reserve(N);
2200  for (const auto& vertice : vertices)
2201  res.push_back(segs[vertice.seg2][vertice.seg2Point ? 1 : 0]);
2202  return res;
2203  }
2204 };
2205 inline bool firstOrNonPresent(size_t i, const std::vector<MatchingVertex>& v)
2206 {
2207  if (v.size() > 0 && v[0].seg1 == i) return true;
2208  for (const auto& it : v)
2209  if (it.seg1 == i || it.seg2 == i) return false;
2210  return true;
2211 }
2214  std::vector<std::vector<MatchingVertex>>& res, std::vector<bool>& used,
2215  size_t searching, unsigned char mask, std::vector<MatchingVertex>& current)
2216 {
2217  for (size_t i = 0; i < mat.cols(); i++)
2218  if (!used[i] && mat.isNotNull(searching, i))
2219  {
2220  unsigned char match = mat(searching, i) & mask;
2221  if (!match)
2222  continue;
2223  else if (firstOrNonPresent(i, current))
2224  {
2225  bool s1p, s2p;
2226  if (true == (s1p = (!(match & 3)))) match >>= 2;
2227  s2p = !(match & 1);
2228  if (current.size() >= 2 && current[0].seg1 == i)
2229  {
2230  if (s2p != current[0].seg1Point)
2231  {
2232  current.emplace_back(searching, i, s1p, s2p);
2233  for (auto it = current.begin(); it != current.end();
2234  ++it)
2235  used[it->seg2] = true;
2236  res.push_back(current);
2237  return true;
2238  }
2239  else
2240  continue; // Strange shape... not a polygon, although
2241  // it'll be without the first segment
2242  }
2243  else
2244  {
2245  current.emplace_back(searching, i, s1p, s2p);
2246  if (depthFirstSearch(
2247  mat, res, used, i, s2p ? 0x3 : 0xC, current))
2248  return true;
2249  current.pop_back();
2250  }
2251  }
2252  }
2253  // No match has been found. Backtrack
2254  return false;
2255 }
2258  std::vector<std::vector<MatchingVertex>>& res, std::vector<bool>& used)
2259 {
2260  vector<MatchingVertex> cur;
2261  for (size_t i = 0; i < used.size(); i++)
2262  if (!used[i])
2263  if (depthFirstSearch(mat, res, used, i, 0xf, cur)) cur.clear();
2264 }
2266  const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys,
2267  std::vector<TSegment3D>& remainder)
2268 {
2269  std::vector<TSegment3D> tmp;
2270  tmp.reserve(segms.size());
2271  for (const auto& segm : segms)
2272  if (segm.length() >= geometryEpsilon)
2273  tmp.push_back(segm);
2274  else
2275  remainder.push_back(segm);
2276  size_t N = tmp.size();
2278  for (size_t i = 0; i < N - 1; i++)
2279  for (size_t j = i + 1; j < N; j++)
2280  {
2281  if (distance(tmp[i].point1, tmp[j].point1) < geometryEpsilon)
2282  {
2283  matches(i, j) |= 1;
2284  matches(j, i) |= 1;
2285  }
2286  if (distance(tmp[i].point1, tmp[j].point2) < geometryEpsilon)
2287  {
2288  matches(i, j) |= 2;
2289  matches(j, i) |= 4;
2290  }
2291  if (distance(tmp[i].point2, tmp[j].point1) < geometryEpsilon)
2292  {
2293  matches(i, j) |= 4;
2294  matches(j, i) |= 2;
2295  }
2296  if (distance(tmp[i].point2, tmp[j].point2) < geometryEpsilon)
2297  {
2298  matches(i, j) |= 8;
2299  matches(j, i) |= 8;
2300  }
2301  }
2302  std::vector<std::vector<MatchingVertex>> results;
2303  std::vector<bool> usedSegments(N, false);
2304  depthFirstSearch(matches, results, usedSegments);
2305  polys.resize(results.size());
2306  transform(
2307  results.begin(), results.end(), polys.begin(), FCreatePolygon(segms));
2308  for (size_t i = 0; i < N; i++)
2309  if (!usedSegments[i]) remainder.push_back(tmp[i]);
2310 }
2311 
2313  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
2314 {
2315  std::vector<TObject3D> tmp;
2316  std::vector<TSegment3D> sgms;
2317  TObject3D::getPolygons(objs, polys, tmp);
2318  TObject3D::getSegments(tmp, sgms);
2319  assemblePolygons(sgms, polys);
2320 }
2321 
2323  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
2324  std::vector<TObject3D>& remainder)
2325 {
2326  std::vector<TObject3D> tmp;
2327  std::vector<TSegment3D> sgms, remainderSgms;
2328  TObject3D::getPolygons(objs, polys, tmp);
2329  TObject3D::getSegments(tmp, sgms, remainder);
2330  assemblePolygons(sgms, polys, remainderSgms);
2331  remainder.insert(
2332  remainder.end(), remainderSgms.begin(), remainderSgms.end());
2333 }
2334 
2336  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
2337  std::vector<TSegment3D>& remainder1, std::vector<TObject3D>& remainder2)
2338 {
2339  std::vector<TObject3D> tmp;
2340  std::vector<TSegment3D> sgms;
2341  TObject3D::getPolygons(objs, polys, tmp);
2342  TObject3D::getSegments(tmp, sgms, remainder2);
2343  assemblePolygons(sgms, polys, remainder1);
2344 }
2345 
2346 bool intersect(const TLine2D& l1, const TSegmentWithLine& s2, TObject2D& obj)
2347 {
2348  if (intersect(l1, s2.line, obj))
2349  {
2350  if (obj.isLine())
2351  {
2352  obj = s2.segment;
2353  return true;
2354  }
2355  else
2356  {
2357  TPoint2D p;
2358  obj.getPoint(p);
2359  return s2.segment.contains(p);
2360  }
2361  }
2362  else
2363  return false;
2364 }
2365 
2366 inline bool intersect(
2367  const TSegmentWithLine& s1, const TLine2D& l2, TObject2D& obj)
2368 {
2369  return intersect(l2, s1, obj);
2370 }
2371 
2373  const TPolygon2D& poly, vector<TPolygon2D>& components)
2374 {
2375  components.clear();
2376  size_t N = poly.size();
2377  if (N <= 3) return false;
2378  vector<TSegmentWithLine> segms(N);
2379  for (size_t i = 0; i < N - 1; i++)
2380  segms[i] = TSegmentWithLine(poly[i], poly[i + 1]);
2381  segms[N - 1] = TSegmentWithLine(poly[N - 1], poly[0]);
2382  TObject2D obj;
2383  TPoint2D pnt;
2384  for (size_t i = 0; i < N; i++)
2385  {
2386  size_t ii = (i + 2) % N, i_ = (i + N - 1) % N;
2387  for (size_t j = ii; j != i_; j = (j + 1) % N)
2388  if (intersect(segms[i].line, segms[j], obj) && obj.getPoint(pnt))
2389  {
2391  pnt, segms[i].segment
2392  [(distance(pnt, segms[i].segment.point1) <
2393  distance(pnt, segms[i].segment.point2))
2394  ? 0
2395  : 1]);
2396  bool cross = false;
2397  TPoint2D pTmp;
2398  for (size_t k = 0; (k < N) && !cross; k++)
2399  if (intersect(sTmp, segms[k], obj))
2400  {
2401  if (obj.getPoint(pTmp) &&
2402  (distance(pTmp, sTmp.segment[0]) >=
2403  geometryEpsilon) &&
2404  (distance(pTmp, sTmp.segment[1]) >=
2405  geometryEpsilon))
2406  cross = true;
2407  }
2408  if (cross) continue;
2409  // At this point, we have a suitable point, although we must
2410  // check if the division is right.
2411  // We do this by evaluating, in the expanded segment's line, the
2412  // next and previous points. If both signs differ, proceed.
2413  if (sign(segms[i].line.evaluatePoint(poly[(i + N - 1) % N])) ==
2414  sign(segms[i].line.evaluatePoint(poly[(i + 2) % N])))
2415  continue;
2416  TPolygon2D p1, p2;
2417  if (i > j)
2418  {
2419  p1.insert(p1.end(), poly.begin() + i + 1, poly.end());
2420  p1.insert(p1.end(), poly.begin(), poly.begin() + j + 1);
2421  p2.insert(
2422  p2.end(), poly.begin() + j + 1, poly.begin() + i + 1);
2423  }
2424  else
2425  {
2426  p1.insert(
2427  p1.end(), poly.begin() + i + 1, poly.begin() + j + 1);
2428  p2.insert(p2.end(), poly.begin() + j + 1, poly.end());
2429  p2.insert(p2.end(), poly.begin(), poly.begin() + i + 1);
2430  }
2431  if (distance(*(p1.rbegin()), pnt) >= geometryEpsilon)
2432  p1.push_back(pnt);
2433  if (distance(*(p2.rbegin()), pnt) >= geometryEpsilon)
2434  p2.push_back(pnt);
2437  vector<TPolygon2D> tempComps;
2438  if (splitInConvexComponents(p1, tempComps))
2439  components.insert(
2440  components.end(), tempComps.begin(), tempComps.end());
2441  else
2442  components.push_back(p1);
2443  if (splitInConvexComponents(p2, tempComps))
2444  components.insert(
2445  components.end(), tempComps.begin(), tempComps.end());
2446  else
2447  components.push_back(p2);
2448  return true;
2449  }
2450  }
2451  return false;
2452 }
2453 
2455 {
2456  public:
2457  const TPose3D& pose;
2459  FUnprojectPolygon2D(const TPose3D& p) : pose(p), tmp1(0), tmp2(0) {}
2461  {
2462  tmp1 = TPolygon3D(poly2D);
2463  project3D(tmp1, pose, tmp2);
2464  return tmp2;
2465  }
2466 };
2468  const TPolygon3D& poly, vector<TPolygon3D>& components)
2469 {
2470  TPlane p;
2471  if (!poly.getPlane(p)) throw std::logic_error("Polygon is skew");
2472  TPose3D pose1;
2473  p.getAsPose3DForcingOrigin(poly[0], pose1);
2474  const TPose3D pose2 = -pose1;
2475  TPolygon3D polyTmp;
2476  project3D(poly, pose2, polyTmp);
2477  TPolygon2D poly2D = TPolygon2D(polyTmp);
2478  vector<TPolygon2D> components2D;
2479  if (splitInConvexComponents(poly2D, components2D))
2480  {
2481  components.resize(components2D.size());
2482  transform(
2483  components2D.begin(), components2D.end(), components.begin(),
2484  FUnprojectPolygon2D(pose1));
2485  return true;
2486  }
2487  else
2488  return false;
2489 }
2490 
2492 {
2493  TPoint2D p;
2494  sgm.getCenter(p);
2495  bis.coefs[0] = sgm.point2.x - sgm.point1.x;
2496  bis.coefs[1] = sgm.point2.y - sgm.point1.y;
2497  bis.coefs[2] = -bis.coefs[0] * p.x - bis.coefs[1] * p.y;
2498  bis.unitarize();
2499 }
2500 
2502 {
2503  TPoint3D p;
2504  sgm.getCenter(p);
2505  bis.coefs[0] = sgm.point2.x - sgm.point1.x;
2506  bis.coefs[1] = sgm.point2.y - sgm.point1.y;
2507  bis.coefs[2] = sgm.point2.z - sgm.point1.z;
2508  bis.coefs[2] =
2509  -bis.coefs[0] * p.x - bis.coefs[1] * p.y - bis.coefs[2] * p.z;
2510  bis.unitarize();
2511 }
2512 
2513 void math::getAngleBisector(const TLine2D& l1, const TLine2D& l2, TLine2D& bis)
2514 {
2515  TPoint2D p;
2516  TObject2D obj;
2517  if (!intersect(l1, l2, obj))
2518  {
2519  // Both lines are parallel
2520  double mod1 = sqrt(square(l1.coefs[0]) + square(l1.coefs[1]));
2521  double mod2 = sqrt(square(l2.coefs[0]) + square(l2.coefs[2]));
2522  bis.coefs[0] = l1.coefs[0] / mod1;
2523  bis.coefs[1] = l1.coefs[1] / mod1;
2524  bool sameSign;
2525  if (abs(bis.coefs[0]) < geometryEpsilon)
2526  sameSign = (l1.coefs[1] * l2.coefs[1]) > 0;
2527  else
2528  sameSign = (l1.coefs[0] * l2.coefs[0]) > 0;
2529  if (sameSign)
2530  bis.coefs[2] = (l1.coefs[2] / mod1) + (l2.coefs[2] / mod2);
2531  else
2532  bis.coefs[2] = (l1.coefs[2] / mod1) - (l2.coefs[2] / mod2);
2533  }
2534  else if (obj.getPoint(p))
2535  {
2536  // Both lines cross
2537  double ang1 = atan2(-l1.coefs[0], l1.coefs[1]);
2538  double ang2 = atan2(-l2.coefs[0], l2.coefs[1]);
2539  double ang = (ang1 + ang2) / 2;
2540  bis.coefs[0] = -sin(ang);
2541  bis.coefs[1] = cos(ang);
2542  bis.coefs[2] = -bis.coefs[0] * p.x - bis.coefs[1] * p.y;
2543  }
2544  else
2545  {
2546  bis = l1;
2547  bis.unitarize();
2548  }
2549 }
2550 
2551 void math::getAngleBisector(const TLine3D& l1, const TLine3D& l2, TLine3D& bis)
2552 {
2553  TPlane p = TPlane(l1, l2); // May throw an exception
2554  TLine3D l1P, l2P;
2555  TLine2D bis2D;
2556  TPose3D pose, pose2;
2557  p.getAsPose3D(pose);
2558  pose2 = -pose;
2559  project3D(l1, pose2, l1P);
2560  project3D(l2, pose2, l2P);
2561  getAngleBisector(TLine2D(l1P), TLine2D(l2P), bis2D);
2562  project3D(TLine3D(bis2D), pose, bis);
2563 }
2564 
2566  const vector<TPolygonWithPlane>& vec, const TPose3D& pose, double& dist)
2567 {
2568  dist = HUGE_VAL;
2569  double nDist = 0;
2570  TLine3D lin;
2571  createFromPoseX(pose, lin);
2572  lin.unitarize();
2573  bool res = false;
2574  for (const auto& it : vec)
2575  if (::intersect(it, lin, nDist, dist))
2576  {
2577  res = true;
2578  dist = nDist;
2579  }
2580  return res;
2581 }
2582 
2584  double dx, double dy, double dz)
2585 {
2586  MRPT_START
2587 
2588  if (std::abs(dx) < 1e-10 && std::abs(dy) < 1e-10 && std::abs(dz) < 1e-10)
2589  THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0);");
2590 
2591  CMatrixDouble33 P;
2592 
2593  // 1st vector:
2594  double n_xy = square(dx) + square(dy);
2595  double n = sqrt(n_xy + square(dz));
2596  n_xy = sqrt(n_xy);
2597  P(0, 0) = dx / n;
2598  P(1, 0) = dy / n;
2599  P(2, 0) = dz / n;
2600 
2601  // 2nd perpendicular vector:
2602  if (fabs(dx) > 1e-4 || fabs(dy) > 1e-4)
2603  {
2604  P(0, 1) = -dy / n_xy;
2605  P(1, 1) = dx / n_xy;
2606  P(2, 1) = 0;
2607  }
2608  else
2609  {
2610  // Any vector in the XY plane will work:
2611  P(0, 1) = 1;
2612  P(1, 1) = 0;
2613  P(2, 1) = 0;
2614  }
2615 
2616  // 3rd perpendicular vector: cross product of the two last vectors:
2617  Eigen::Vector3d c2;
2618  crossProduct3D(P.col(0), P.col(1), c2);
2619  P.col(2) = c2;
2620 
2621  return P;
2622  MRPT_END
2623 }
bool RectanglesIntersection(double R1_x_min, double R1_x_max, double R1_y_min, double R1_y_max, double R2_x_min, double R2_x_max, double R2_y_min, double R2_y_max, double R2_pose_x, double R2_pose_y, double R2_pose_phi)
Returns whether two rotated rectangles intersect.
Definition: geometry.cpp:367
void getCenter(TPoint2D &p) const
Segment&#39;s central point.
Definition: TSegment2D.h:80
void project3D(const TPoint3D &point, const mrpt::math::TPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
Definition: geometry.h:329
bool intersectInCommonLine(const mrpt::math::TSegment3D &s1, const mrpt::math::TSegment3D &s2, const mrpt::math::TLine3D &lin, mrpt::math::TObject3D &obj)
Definition: geometry.cpp:476
void closestFromPointToLine(double Px, double Py, double x1, double y1, double x2, 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 getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
Definition: TObject2D.h:109
void generateAxisBaseFromDirectionAndAxis(const double(&vec)[3], uint8_t coord, CMatrixDouble44 &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:2076
double x
X,Y coordinates.
Definition: TPoint2D.h:23
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:1419
TPolygon3D operator()(const TPolygon2D &poly2D)
Definition: geometry.cpp:2460
GLenum GLint GLuint mask
Definition: glext.h:4062
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
Definition: TObject2D.h:150
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble t
Definition: glext.h:3695
bool splitInConvexComponents(const TPolygon2D &poly, std::vector< TPolygon2D > &components)
Splits a 2D polygon into convex components.
Definition: geometry.cpp:2372
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
auto col(int colIdx)
Definition: MatrixBase.h:89
void unsafeProjectPoint(const TPoint3D &point, const TPose3D &pose, TPoint2D &newPoint)
Definition: geometry.cpp:523
bool intersect(const TPolygonWithPlane &p1, const TLine3D &l2, double &d, double bestKnown)
Definition: geometry.cpp:539
void getPlanes(const std::vector< TPolygon3D > &polys, std::vector< TPlane > &planes)
Definition: geometry.cpp:1632
double x
X,Y coordinates.
Definition: TPose2D.h:30
bool firstOrNonPresent(size_t i, const std::vector< MatchingVertex > &v)
Definition: geometry.cpp:2205
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double distance(const TPoint3D &point) const
Distance between the line and a point.
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:2212
static constexpr unsigned char GEOMETRIC_TYPE_POLYGON
Object type identifier for TPolygon2D or TPolygon3D.
Definition: TPoseOrPoint.h:116
bool traceRay(const std::vector< TPolygonWithPlane > &vec, const mrpt::math::TPose3D &pose, double &dist)
Fast ray tracing method using polygons&#39; properties.
Definition: geometry.cpp:2565
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
Definition: TObject3D.h:134
bool contains(const TPoint2D &point) const
Check whether a point is inside a segment.
FUnprojectPolygon2D(const TPose3D &p)
Definition: geometry.cpp:2459
void destroy()
Definition: geometry.cpp:1328
size_t getNonNullElements() const
Gets the amount of non-null elements inside the matrix.
void 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:2171
void getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
Definition: geometry.cpp:2491
mrpt::math::TPoint2D composePoint(const TPoint2D l) const
char fromObject(const TObject2D &obj)
Definition: geometry.cpp:1444
mrpt::math::TPose3D inversePose
Plane&#39;s inverse pose.
Definition: geometry.h:42
GLenum GLsizei n
Definition: glext.h:5136
void createFromPoseAndVector(const mrpt::math::TPose3D &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:948
Slightly heavyweight type to speed-up calculations with polygons in 3D.
Definition: geometry.h:32
This file implements several operations that operate element-wise on individual or pairs of container...
vector< TSegment2D > l2
Definition: geometry.cpp:1318
bool minDistBetweenLines(double p1_x, double p1_y, double p1_z, double p2_x, double p2_y, double p2_z, double p3_x, double p3_y, double p3_z, double p4_x, double p4_y, double p4_z, double &x, double &y, double &z, double &dist)
Calculates the minimum distance between a pair of lines.
Definition: geometry.cpp:298
void createPlaneFromPoseXY(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Z vector.
Definition: geometry.cpp:2047
double closestSquareDistanceFromPointToLine(double Px, double Py, double x1, double y1, double x2, double y2)
Returns the square distance from a point to a line.
Definition: geometry.cpp:100
Standard type for storing any lightweight 2D type.
Definition: TObject2D.h:24
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
Definition: TObject2D.h:136
static constexpr unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
Definition: TPoseOrPoint.h:101
bool getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
Definition: TObject3D.h:175
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
Standard object for storing any 3D lightweight object.
Definition: TObject3D.h:25
STL namespace.
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
Definition: TObject2D.h:123
bool contains(const TPoint2D &point) const
Check whether a point is inside the line.
TSegmentWithLine(const TSegment2D &s)
Definition: geometry.cpp:1415
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
void getSegmentsWithLine(const TPolygon2D &poly, vector< TSegmentWithLine > &segs)
Definition: geometry.cpp:1435
bool intersectInCommonPlane(const T3D &o1, const U3D &o2, const mrpt::math::TPlane &p, mrpt::math::TObject3D &obj)
Definition: geometry.cpp:447
void getAsPose2D(TPose2D &outPose) const
GLenum GLenum GLuint components
Definition: glext.h:7401
double distance(const TPoint3D &point) const
Distance to 3D point.
void createPlaneFromPoseAndNormal(const mrpt::math::TPose3D &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:2062
size_t getIndexOfMin(const T &e1, const T &e2, const T &e3)
Definition: geometry.cpp:2121
int rank(Scalar threshold=0) const
Finds the rank of the matrix via LU decomposition.
GLdouble s
Definition: glext.h:3682
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
TSegment2D segment
Definition: geometry.cpp:1413
void 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:2513
GLuint coord
Definition: glext.h:7245
A sparse matrix container (with cells of any type), with iterators.
TTempIntersection(const TCommonRegion &common)
Definition: geometry.cpp:1389
void createFromPoseY(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Y axis in a given pose.
Definition: geometry.cpp:938
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:804
TPoint3D point1
Origin point.
Definition: TSegment3D.h:27
bool conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
Definition: geometry.cpp:985
GLsizei const GLfloat * points
Definition: glext.h:5414
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.
const std::vector< TSegment3D > & segs
Definition: geometry.cpp:2193
TTempIntersection & operator=(const TTempIntersection &t)
Definition: geometry.cpp:1394
unsigned char type
Definition: geometry.cpp:1323
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
static constexpr unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
Definition: TPoseOrPoint.h:121
void unitarize()
Unitarize line&#39;s normal vector.
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
Definition: TObject3D.h:161
This base provides a set of functions for maths stuff.
vector< TSegment2D > l1
Definition: geometry.cpp:1317
2D segment, consisting of two points.
Definition: TSegment2D.h:20
bool isPoint() const
Checks whether content is a point.
Definition: TObject3D.h:95
TCommonRegion & operator=(const TCommonRegion &r)
Definition: geometry.cpp:1347
map< string, CVectorDouble > results
3D segment, consisting of two points.
Definition: TSegment3D.h:21
size_t cols() const
Returns the amount of columns in this matrix.
static double geometryEpsilon
Definition: geometry.cpp:32
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44 &HG)
TPoint3D point2
Destiny point.
Definition: TSegment3D.h:31
void covariancesAndMean(const VECTOR_OF_VECTORS &elements, MATRIXLIKE &covariances, VECTORLIKE &means, const bool *elem_do_wrap2pi=nullptr)
Computes covariances and mean of any vector of containers.
Definition: data_utils.h:381
T square(const T x)
Inline function for the square of a number.
const GLubyte * c
Definition: glext.h:6406
bool PointIntoPolygon(double x, double y) const
Check if a point is inside the polygon.
Definition: CPolygon.h:63
void unitarize()
Unitarize normal vector.
void 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:1902
void createPlaneFromPoseXZ(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Y vector.
Definition: geometry.cpp:2052
size_t rows() const
Returns the amount of rows in this matrix.
TPolygon3D poly
Actual polygon.
Definition: geometry.h:36
static void getPolygons(const std::vector< TObject3D > &objs, std::vector< TPolygon3D > &polys)
Static method to retrieve every polygon included in a vector of objects.
void setEpsilon(double nE)
Changes the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:35
3D Plane, represented by its equation
Definition: TPlane.h:22
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon.
GLubyte GLubyte b
Definition: glext.h:6372
const double eps
int sign(T x)
Returns the sign of X as "1" or "-1".
double getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
Definition: geometry.cpp:2146
void getCenter(TPoint3D &p) const
Segment&#39;s central point.
Definition: TSegment3D.h:81
FCreatePolygon(const std::vector< TSegment3D > &s)
Definition: geometry.cpp:2194
TPoint2D point2
Destiny point.
Definition: TSegment2D.h:30
TPolygon2D poly2D
Polygon, after being projected to the plane using inversePose.
Definition: geometry.h:45
TCommonRegion(const TPoint2D &p)
Definition: geometry.cpp:1341
bool pointIntoPolygon2D(double px, 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:233
double getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:862
static void getPlanes(const std::vector< TPolygon3D > &oldPolys, std::vector< TPolygonWithPlane > &newPolys)
Static method for vectors.
Definition: geometry.cpp:626
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
void unitarize()
Unitarize director vector.
bool contains(const TPoint3D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
Definition: TObject3D.h:120
TPoint2D point1
Origin point.
Definition: TSegment2D.h:26
bool 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:120
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
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).
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TPolygonWithPlane()=default
Basic constructor.
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:23
TTempIntersection(const T2ListsOfSegments &segms)
Definition: geometry.cpp:1385
TPolygon3D operator()(const std::vector< MatchingVertex > &vertices)
Definition: geometry.cpp:2195
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
Definition: glext.h:5645
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
GLfloat GLfloat v1
Definition: glext.h:4121
double getRegressionLine(const std::vector< TPoint2D > &points, TLine2D &line)
Using eigenvalues, gets the best fitting line for a set of 2D points.
Definition: geometry.cpp:2103
void generate3DObject(TObject3D &obj) const
Project into 3D space.
void createFromPoseX(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the X axis in a given pose.
Definition: geometry.cpp:933
const TPose3D & pose
Definition: geometry.cpp:2457
mrpt::math::TPose3D pose
Plane&#39;s pose.
Definition: geometry.h:40
void 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:2017
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
void resize(size_t nRows, size_t nCols)
Changes the size of the matrix.
CMatrixDouble33 generateAxisBaseFromDirection(double dx, double dy, double dz)
Computes an axis base (a set of three 3D normal vectors) with the given vector being the first of the...
Definition: geometry.cpp:2583
#define MRPT_END
Definition: exceptions.h:245
void createPlaneFromPoseAndAxis(const TPose3D &pose, TPlane &plane, size_t axis)
Definition: geometry.cpp:2035
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
void getAsPose3D(mrpt::math::TPose3D &outPose) const
Lightweight 2D pose.
Definition: TPose2D.h:22
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:2185
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.
Definition: TObject3D.h:147
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane&#39;s equation.
void project2D(const TPoint2D &point, const TPose2D &newXpose, TPoint2D &newPoint)
Uses the given pose 2D to project a point into a new base.
Definition: geometry.cpp:1168
void createFromPoseZ(const mrpt::math::TPose3D &p, TLine3D &r)
Gets a 3D line corresponding to the Z axis in a given pose.
Definition: geometry.cpp:943
void createPlaneFromPoseYZ(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its X vector.
Definition: geometry.cpp:2057
GLenum GLint GLint y
Definition: glext.h:3542
TCommonRegion * common
Definition: geometry.cpp:1370
void createFromPoseAndAxis(const TPose3D &p, TLine3D &r, size_t axis)
Definition: geometry.cpp:921
bool areAligned(const std::vector< TPoint2D > &points)
Checks whether this set of points acceptably fits a 2D line.
Definition: geometry.cpp:1006
bool compatibleBounds(const TPoint3D &min1, const TPoint3D &max1, const TPoint3D &min2, const TPoint3D &max2)
Definition: geometry.cpp:1609
GLfloat GLfloat GLfloat v2
Definition: glext.h:4123
TCommonRegion(const TCommonRegion &r)
Definition: geometry.cpp:1362
bool intersectAux(const TPolygon3D &p1, const TPlane &pl1, const TPolygon3D &p2, const TPlane &pl2, TObject3D &obj)
Definition: geometry.cpp:1578
void closestFromPointToSegment(double Px, double Py, double x1, double y1, double x2, double y2, double &out_x, double &out_y)
Computes the closest point from a given point to a segment.
Definition: geometry.cpp:39
GLuint res
Definition: glext.h:7385
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
Definition: TPoseOrPoint.h:106
TTempIntersection(const TTempIntersection &t)
Definition: geometry.cpp:1409
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
double distance(const TPoint2D &point) const
Distance from a given point.
GLuint GLenum GLenum transform
Definition: glext.h:7089
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:38
Lightweight 2D point.
Definition: TPoint2D.h:31
void AddVertex(double x, double y)
Add a new vertex to polygon.
Definition: CPolygon.h:28
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:973
std::array< double, 4 > coefs
Plane coefficients, stored as an array: .
Definition: TPlane.h:26
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:634
unsigned char type
Definition: geometry.cpp:1367
double phi
Orientation (rads)
Definition: TPose2D.h:32
TCommonRegion(const TSegment2D &s)
Definition: geometry.cpp:1342
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3532
static constexpr unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
Definition: TPoseOrPoint.h:111
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:18
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1887
void getMinAndMaxBounds(const std::vector< TPolygon3D > &v1, std::vector< TPoint3D > &minP, std::vector< TPoint3D > &maxP)
Definition: geometry.cpp:1641
TSegment2D * segment
Definition: geometry.cpp:1326
bool contains(const TPoint3D &point) const
Check whether a point is contained into the plane.
void unsafeProjectPolygon(const TPolygon3D &poly, const TPose3D &pose, TPolygon2D &newPoly)
Definition: geometry.cpp:531
double distancePointToPolygon2D(double px, 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:264
void composePoint(const TPoint3D &l, TPoint3D &g) const
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:5645
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019