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