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]) >=
827  return false;
828  if (abs(r1.coefs[1] * r2.coefs[2] - r1.coefs[2] * r2.coefs[1]) >=
830  return false;
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() == 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() == 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() == 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 };
1325 {
1326  unsigned char type; // 0 -> point, 1-> segment, any other-> empty
1327  union {
1330  } data;
1331  void destroy()
1332  {
1333  switch (type)
1334  {
1335  case 0:
1336  delete data.point;
1337  break;
1338  case 1:
1339  delete data.segment;
1340  break;
1341  }
1342  type = 255;
1343  }
1344  TCommonRegion(const TPoint2D& p) : type(0) { data.point = new TPoint2D(p); }
1346  {
1347  data.segment = new TSegment2D(s);
1348  }
1349  ~TCommonRegion() { destroy(); }
1351  {
1352  if (&r == this) return *this;
1353  destroy();
1354  switch (type = r.type)
1355  {
1356  case 0:
1357  data.point = new TPoint2D(*(r.data.point));
1358  break;
1359  case 1:
1360  data.segment = new TSegment2D(*(r.data.segment));
1361  break;
1362  }
1363  return *this;
1364  }
1365  TCommonRegion(const TCommonRegion& r) : type(0) { operator=(r); }
1366 };
1368 {
1369  unsigned char type; // 0->two lists of segments, 1-> common region
1370  union {
1373  } data;
1374  void destroy()
1375  {
1376  switch (type)
1377  {
1378  case 0:
1379  delete data.segms;
1380  break;
1381  case 1:
1382  delete data.common;
1383  break;
1384  }
1385  type = 255;
1386  };
1388  {
1389  data.segms = new T2ListsOfSegments(segms);
1390  }
1392  {
1393  data.common = new TCommonRegion(common);
1394  }
1395  ~TTempIntersection() { destroy(); }
1397  {
1398  if (&t == this) return *this;
1399  destroy();
1400  switch (type = t.type)
1401  {
1402  case 0:
1403  data.segms = new T2ListsOfSegments(*(t.data.segms));
1404  break;
1405  case 1:
1406  data.common = new TCommonRegion(*(t.data.common));
1407  break;
1408  }
1409  return *this;
1410  }
1411  TTempIntersection(const TTempIntersection& t) : type(0) { operator=(t); }
1412 };
1414 {
1417  explicit TSegmentWithLine(const TSegment2D& s) : segment(s)
1418  {
1419  line = TLine2D(s[0], s[1]);
1420  }
1421  TSegmentWithLine(const TPoint2D& p1, const TPoint2D& p2) : segment(p1, p2)
1422  {
1423  line = TLine2D(p1, p2);
1424  }
1425  TSegmentWithLine() = default;
1426 };
1428  const TSegmentWithLine& s1, const TSegmentWithLine& s2, TObject2D& obj)
1429 {
1430  if (!intersect(s1.line, s2.line, obj)) return false;
1431  if (obj.isLine())
1432  return intersectInCommonLine(s1.segment, s2.segment, s1.line, obj);
1433  TPoint2D p;
1434  obj.getPoint(p);
1435  return s1.segment.contains(p) && s2.segment.contains(p);
1436 }
1437 void getSegmentsWithLine(const TPolygon2D& poly, vector<TSegmentWithLine>& segs)
1438 {
1439  size_t N = poly.size();
1440  segs.resize(N);
1441  for (size_t i = 0; i < N - 1; i++)
1442  segs[i] = TSegmentWithLine(poly[i], poly[i + 1]);
1443  segs[N - 1] = TSegmentWithLine(poly[N - 1], poly[0]);
1444 }
1445 
1446 inline char fromObject(const TObject2D& obj)
1447 {
1448  switch (obj.getType())
1449  {
1450  case GEOMETRIC_TYPE_POINT:
1451  return 'P';
1453  return 'S';
1454  case GEOMETRIC_TYPE_LINE:
1455  return 'L';
1457  return 'O';
1458  default:
1459  return 'U';
1460  }
1461 }
1462 
1464  const TPolygon2D& /*p1*/, const TPolygon2D& /*p2*/, TObject2D& /*obj*/)
1465 {
1466  THROW_EXCEPTION("TODO");
1467 #if 0
1468  return false; //TODO
1469 
1470  CSparseMatrixTemplate<TObject2D> intersections=CSparseMatrixTemplate<TObject2D>(p1.size(),p2.size());
1471  std::vector<TSegmentWithLine> segs1,segs2;
1472  getSegmentsWithLine(p1,segs1);
1473  getSegmentsWithLine(p2,segs2);
1474  unsigned int hmInters=0;
1475  for (size_t i=0;i<segs1.size();i++) {
1476  const TSegmentWithLine &s1=segs1[i];
1477  for (size_t j=0;j<segs2.size();j++) if (intersect(s1,segs2[j],obj)) {
1478  intersections(i,j)=obj;
1479  hmInters++;
1480  }
1481  }
1482  for (size_t i=0;i<intersections.rows();i++) {
1483  for (size_t j=0;j<intersections.cols();j++) cout<<fromObject(intersections(i,j));
1484  cout<<'\n';
1485  }
1486  if (hmInters==0) {
1487  if (p1.contains(p2[0])) {
1488  obj=p2;
1489  return true;
1490  } else if (p2.contains(p1[0])) {
1491  obj=p1;
1492  return true;
1493  } else return false;
1494  }
1495  //ESTO ES UNA PESADILLA, HAY CIEN MILLONES DE CASOS DISTINTOS A LA HORA DE RECORRER LAS POSIBILIDADES...
1496  /*
1497  Dividir cada segmento en sus distintas partes según sus intersecciones, y generar un nuevo polígono.
1498  Recorrer de segmento en segmento, por cada uno de los dos lados (recorriendo desde un punto común a otro;
1499  en un polígono se escoge el camino secuencial directo, mientras que del otro se escoge, de los dos posibles,
1500  el que no se corta con ningún elemento del primero).
1501  Seleccionar, para cada segmento, si está dentro o fuera.
1502  Parece fácil, pero es una puta mierda.
1503  TODO: hacer en algún momento de mucho tiempo libre...
1504  */
1505 
1506  /* ¿Seguir? */
1507  return false;
1508 #endif
1509 }
1510 
1511 bool math::intersect(const TPolygon3D& p1, const TSegment3D& s2, TObject3D& obj)
1512 {
1513  TPlane p;
1514  if (!p1.getPlane(p)) return false;
1515  if (!intersect(p, s2, obj)) return false;
1516  TPoint3D pnt;
1517  TSegment3D sgm;
1518  if (obj.getPoint(pnt))
1519  {
1520  TPose3D pose;
1521  p.getAsPose3DForcingOrigin(p1[0], pose);
1522  CMatrixDouble44 HMinv;
1523  pose.getInverseHomogeneousMatrix(HMinv);
1524  TPose3D poseNeg;
1525  poseNeg.fromHomogeneousMatrix(HMinv);
1526  TPolygon3D projPoly;
1527  TPoint3D projPnt;
1528  project3D(p1, poseNeg, projPoly);
1529  project3D(pnt, poseNeg, projPnt);
1530  return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
1531  }
1532  else if (obj.getSegment(sgm))
1533  return intersectInCommonPlane<TPolygon2D, TSegment2D>(p1, s2, p, obj);
1534  return false;
1535 }
1536 
1537 bool math::intersect(const TPolygon3D& p1, const TLine3D& r2, TObject3D& obj)
1538 {
1539  TPlane p;
1540  if (!p1.getPlane(p)) return false;
1541  if (!intersect(p, r2, obj)) return false;
1542  TPoint3D pnt;
1543  if (obj.getPoint(pnt))
1544  {
1545  TPose3D pose;
1546  p.getAsPose3DForcingOrigin(p1[0], pose);
1547  CMatrixDouble44 HMinv;
1548  pose.getInverseHomogeneousMatrix(HMinv);
1549  TPose3D poseNeg;
1550  poseNeg.fromHomogeneousMatrix(HMinv);
1551  TPolygon3D projPoly;
1552  TPoint3D projPnt;
1553  project3D(p1, poseNeg, projPoly);
1554  project3D(pnt, poseNeg, projPnt);
1555  return TPolygon2D(projPoly).contains(TPoint2D(projPnt));
1556  }
1557  else if (obj.isLine())
1558  return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, r2, p, obj);
1559  return false;
1560 }
1561 
1562 bool math::intersect(const TPolygon3D& p1, const TPlane& p2, TObject3D& obj)
1563 {
1564  TPlane p;
1565  if (!p1.getPlane(p)) return false;
1566  if (!intersect(p, p2, obj)) return false;
1567  TLine3D ln;
1568  if (obj.isPlane())
1569  {
1570  // Polygon is inside the plane
1571  obj = p1;
1572  return true;
1573  }
1574  else if (obj.getLine(ln))
1575  return intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln, p, obj);
1576  return false;
1577 }
1578 
1579 // Auxiliary function2
1581  const TPolygon3D& p1, const TPlane& pl1, const TPolygon3D& p2,
1582  const TPlane& pl2, TObject3D& obj)
1583 {
1584  if (!intersect(pl1, pl2, obj)) return false;
1585  TLine3D ln;
1586  if (obj.isPlane())
1587  return intersectInCommonPlane<TPolygon2D, TPolygon2D>(
1588  p1, p2, pl1, obj); // Polygons are on the same plane
1589  else if (obj.getLine(ln))
1590  {
1591  TObject3D obj1, obj2;
1592  if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p1, ln, pl1, obj1))
1593  return false;
1594  if (!intersectInCommonPlane<TPolygon2D, TLine2D>(p2, ln, pl2, obj2))
1595  return false;
1596  TPoint3D po1, po2;
1597  TSegment3D s1, s2;
1598  if (obj1.getPoint(po1))
1599  s1 = TSegment3D(po1, po1);
1600  else
1601  obj1.getSegment(s1);
1602  if (obj2.getPoint(po2))
1603  s2 = TSegment3D(po2, po2);
1604  else
1605  obj2.getSegment(s2);
1606  return intersectInCommonLine(s1, s2, ln, obj);
1607  }
1608  return false;
1609 }
1610 
1612  const TPoint3D& min1, const TPoint3D& max1, const TPoint3D& min2,
1613  const TPoint3D& max2)
1614 {
1615  for (size_t i = 0; i < 3; i++)
1616  if ((min1[i] > max2[i]) || (min2[i] > max1[i])) return false;
1617  return true;
1618 }
1619 // End of auxiliary functions
1620 
1621 bool math::intersect(const TPolygon3D& p1, const TPolygon3D& p2, TObject3D& obj)
1622 {
1623  TPoint3D min1, max1, min2, max2;
1624  getPrismBounds(p1, min1, max1);
1625  getPrismBounds(p2, min2, max2);
1626  if (!compatibleBounds(min1, max1, min2, max2)) return false;
1627  TPlane pl1, pl2;
1628  if (!p1.getPlane(pl1)) return false;
1629  if (!p2.getPlane(pl2)) return false;
1630  return intersectAux(p1, pl1, p2, pl2, obj);
1631 }
1632 
1633 // Auxiliary function
1634 inline void getPlanes(
1635  const std::vector<TPolygon3D>& polys, std::vector<TPlane>& planes)
1636 {
1637  size_t N = polys.size();
1638  planes.resize(N);
1639  for (size_t i = 0; i < N; i++) getRegressionPlane(polys[i], planes[i]);
1640 }
1641 
1642 // Auxiliary functions
1644  const std::vector<TPolygon3D>& v1, std::vector<TPoint3D>& minP,
1645  std::vector<TPoint3D>& maxP)
1646 {
1647  minP.resize(0);
1648  maxP.resize(0);
1649  size_t N = v1.size();
1650  minP.reserve(N);
1651  maxP.reserve(N);
1652  TPoint3D p1, p2;
1653  for (const auto& it : v1)
1654  {
1655  getPrismBounds(it, p1, p2);
1656  minP.push_back(p1);
1657  maxP.push_back(p2);
1658  }
1659 }
1660 
1662  const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2,
1664 {
1665  std::vector<TPlane> w1, w2;
1666  getPlanes(v1, w1);
1667  getPlanes(v2, w2);
1668  std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
1669  getMinAndMaxBounds(v1, minBounds1, maxBounds1);
1670  getMinAndMaxBounds(v2, minBounds2, maxBounds2);
1671  size_t M = v1.size(), N = v2.size();
1672  objs.clear();
1673  objs.resize(M, N);
1674  TObject3D obj;
1675  for (size_t i = 0; i < M; i++)
1676  for (size_t j = 0; j < N; j++)
1677  if (!compatibleBounds(
1678  minBounds1[i], maxBounds1[i], minBounds2[j], maxBounds2[j]))
1679  continue;
1680  else if (intersectAux(v1[i], w1[i], v2[j], w2[j], obj))
1681  objs(i, j) = obj;
1682  return objs.getNonNullElements();
1683 }
1684 
1686  const std::vector<TPolygon3D>& v1, const std::vector<TPolygon3D>& v2,
1687  std::vector<TObject3D>& objs)
1688 {
1689  objs.resize(0);
1690  std::vector<TPlane> w1, w2;
1691  getPlanes(v1, w1);
1692  getPlanes(v2, w2);
1693  std::vector<TPoint3D> minBounds1, maxBounds1, minBounds2, maxBounds2;
1694  getMinAndMaxBounds(v1, minBounds1, maxBounds1);
1695  getMinAndMaxBounds(v2, minBounds2, maxBounds2);
1696  TObject3D obj;
1697  auto itP1 = w1.begin();
1698  auto itMin1 = minBounds1.begin();
1699  auto itMax1 = maxBounds1.begin();
1700  for (auto it1 = v1.begin(); it1 != v1.end();
1701  ++it1, ++itP1, ++itMin1, ++itMax1)
1702  {
1703  const TPolygon3D& poly1 = *it1;
1704  const TPlane& plane1 = *itP1;
1705  auto itP2 = w2.begin();
1706  const TPoint3D &min1 = *itMin1, max1 = *itMax1;
1707  auto itMin2 = minBounds2.begin();
1708  auto itMax2 = maxBounds2.begin();
1709  for (auto it2 = v2.begin(); it2 != v2.end();
1710  ++it2, ++itP2, ++itMin2, ++itMax2)
1711  if (!compatibleBounds(min1, max1, *itMin2, *itMax2))
1712  continue;
1713  else if (intersectAux(poly1, plane1, *it2, *itP2, obj))
1714  objs.push_back(obj);
1715  }
1716  return objs.size();
1717 }
1718 
1719 bool math::intersect(const TObject2D& o1, const TObject2D& o2, TObject2D& obj)
1720 {
1721  TPoint2D p1, p2;
1722  TSegment2D s1, s2;
1723  TLine2D l1, l2;
1724  TPolygon2D po1, po2;
1725  if (o1.getPoint(p1))
1726  {
1727  obj = p1;
1728  if (o2.getPoint(p2))
1729  return distance(p1, p2) < geometryEpsilon;
1730  else if (o2.getSegment(s2))
1731  return s2.contains(p1);
1732  else if (o2.getLine(l2))
1733  return l2.contains(p1);
1734  else if (o2.getPolygon(po2))
1735  return po2.contains(p1); // else return false;
1736  }
1737  else if (o1.getSegment(s1))
1738  {
1739  if (o2.getPoint(p2))
1740  {
1741  if (s1.contains(p2))
1742  {
1743  obj = p2;
1744  return true;
1745  } // else return false;
1746  }
1747  else if (o2.getSegment(s2))
1748  return intersect(s1, s2, obj);
1749  else if (o2.getLine(l2))
1750  return intersect(s1, l2, obj);
1751  else if (o2.getPolygon(po2))
1752  return intersect(s1, po2, obj); // else return false;
1753  }
1754  else if (o1.getLine(l1))
1755  {
1756  if (o2.getPoint(p2))
1757  {
1758  if (l1.contains(p2))
1759  {
1760  obj = p2;
1761  return true;
1762  } // else return false;
1763  }
1764  else if (o2.getSegment(s2))
1765  return intersect(l1, s2, obj);
1766  else if (o2.getLine(l2))
1767  return intersect(l1, l2, obj);
1768  else if (o2.getPolygon(po2))
1769  return intersect(l1, po2, obj); // else return false;
1770  }
1771  else if (o1.getPolygon(po1))
1772  {
1773  if (o2.getPoint(p2))
1774  {
1775  if (po1.contains(p2))
1776  {
1777  obj = p2;
1778  return true;
1779  } // else return false;
1780  }
1781  else if (o2.getSegment(s2))
1782  return intersect(po1, s2, obj);
1783  else if (o2.getLine(l2))
1784  return intersect(po1, l2, obj);
1785  else if (o2.getPolygon(po2))
1786  return intersect(po1, po2, obj); // else return false;
1787  } // else return false;
1788  return false;
1789 }
1790 
1791 bool math::intersect(const TObject3D& o1, const TObject3D& o2, TObject3D& obj)
1792 {
1793  TPoint3D p1, p2;
1794  TSegment3D s1, s2;
1795  TLine3D l1, l2;
1796  TPolygon3D po1, po2;
1797  TPlane pl1, pl2;
1798  if (o1.getPoint(p1))
1799  {
1800  obj = p1;
1801  if (o2.getPoint(p2))
1802  return distance(p1, p2) < geometryEpsilon;
1803  else if (o2.getSegment(s2))
1804  return s2.contains(p1);
1805  else if (o2.getLine(l2))
1806  return l2.contains(p1);
1807  else if (o2.getPolygon(po2))
1808  return po2.contains(p1);
1809  else if (o2.getPlane(pl2))
1810  return pl2.contains(p1); // else return false;
1811  }
1812  else if (o1.getSegment(s1))
1813  {
1814  if (o2.getPoint(p2))
1815  {
1816  if (s1.contains(p2))
1817  {
1818  obj = p2;
1819  return true;
1820  } // else return false;
1821  }
1822  else if (o2.getSegment(s2))
1823  return intersect(s1, s2, obj);
1824  else if (o2.getLine(l2))
1825  return intersect(s1, l2, obj);
1826  else if (o2.getPolygon(po2))
1827  return intersect(s1, po2, obj);
1828  else if (o2.getPlane(pl2))
1829  return intersect(s1, pl2, obj); // else return false;
1830  }
1831  else if (o1.getLine(l1))
1832  {
1833  if (o2.getPoint(p2))
1834  {
1835  if (l1.contains(p2))
1836  {
1837  obj = p2;
1838  return true;
1839  } // else return false;
1840  }
1841  else if (o2.getSegment(s2))
1842  return intersect(l1, s2, obj);
1843  else if (o2.getLine(l2))
1844  return intersect(l1, l2, obj);
1845  else if (o2.getPolygon(po2))
1846  return intersect(l1, po2, obj);
1847  else if (o2.getPlane(pl2))
1848  return intersect(l2, pl2, obj); // else return false;
1849  }
1850  else if (o1.getPolygon(po1))
1851  {
1852  if (o2.getPoint(p2))
1853  {
1854  if (po1.contains(p2))
1855  {
1856  obj = p2;
1857  return true;
1858  } // else return false;
1859  }
1860  else if (o2.getSegment(s2))
1861  return intersect(po1, s2, obj);
1862  else if (o2.getLine(l2))
1863  return intersect(po1, l2, obj);
1864  else if (o2.getPolygon(po2))
1865  return intersect(po1, po2, obj);
1866  else if (o2.getPlane(pl2))
1867  return intersect(po1, pl2, obj); // else return false;
1868  }
1869  else if (o1.getPlane(pl1))
1870  {
1871  if (o2.getPoint(p2))
1872  {
1873  if (pl1.contains(p2))
1874  {
1875  obj = p2;
1876  return true;
1877  } // else return false;
1878  }
1879  else if (o2.getSegment(s2))
1880  return intersect(pl1, s2, obj);
1881  else if (o2.getLine(l2))
1882  return intersect(pl1, l2, obj);
1883  else if (o2.getPlane(pl2))
1884  return intersect(pl1, pl2, obj); // else return false;
1885  } // else return false;
1886  return false;
1887 }
1888 
1889 double math::distance(const TPoint2D& p1, const TPoint2D& p2)
1890 {
1891  double dx = p2.x - p1.x;
1892  double dy = p2.y - p1.y;
1893  return sqrt(dx * dx + dy * dy);
1894 }
1895 
1896 double math::distance(const TPoint3D& p1, const TPoint3D& p2)
1897 {
1898  double dx = p2.x - p1.x;
1899  double dy = p2.y - p1.y;
1900  double dz = p2.z - p1.z;
1901  return sqrt(dx * dx + dy * dy + dz * dz);
1902 }
1903 
1905  const std::vector<TPoint2D>& poly, TPoint2D& pMin, TPoint2D& pMax)
1906 {
1907  size_t N = poly.size();
1908  if (N < 1) throw logic_error("Empty polygon");
1909  pMin = poly[0];
1910  pMax = poly[0];
1911  for (size_t i = 1; i < N; i++)
1912  {
1913  pMin.x = min(pMin.x, poly[i].x);
1914  pMin.y = min(pMin.y, poly[i].y);
1915  pMax.x = max(pMax.x, poly[i].x);
1916  pMax.y = max(pMax.y, poly[i].y);
1917  }
1918 }
1919 
1920 double math::distance(const TLine2D& r1, const TLine2D& r2)
1921 {
1922  if (abs(r1.coefs[0] * r2.coefs[1] - r2.coefs[0] * r1.coefs[1]) <
1924  {
1925  // Lines are parallel
1926  size_t i1 = (abs(r1.coefs[0]) < geometryEpsilon) ? 0 : 1;
1927  TPoint2D p;
1928  p[i1] = 0.0;
1929  p[1 - i1] = -r1.coefs[2] / r1.coefs[1 - i1];
1930  return r2.distance(p);
1931  }
1932  else
1933  return 0; // Lines cross in some point
1934 }
1935 
1936 double math::distance(const TLine3D& r1, const TLine3D& r2)
1937 {
1938  if (abs(getAngle(r1, r2)) < geometryEpsilon)
1939  return r1.distance(r2.pBase); // Lines are parallel
1940  else
1941  {
1942  // We build a plane parallel to r2 which contains r1
1943  TPlane p;
1944  crossProduct3D(r1.director, r2.director, p.coefs);
1945  p.coefs[3] =
1946  -(p.coefs[0] * r1.pBase[0] + p.coefs[1] * r1.pBase[1] +
1947  p.coefs[2] * r1.pBase[2]);
1948  return p.distance(r2.pBase);
1949  }
1950 }
1951 
1952 double math::distance(const TPlane& p1, const TPlane& p2)
1953 {
1954  if (abs(getAngle(p1, p2)) < geometryEpsilon)
1955  {
1956  // Planes are parallel
1957  TPoint3D p(0, 0, 0);
1958  for (size_t i = 0; i < 3; i++)
1959  if (abs(p1.coefs[i]) >= geometryEpsilon)
1960  {
1961  p[i] = -p1.coefs[3] / p1.coefs[i];
1962  break;
1963  }
1964  return p2.distance(p);
1965  }
1966  else
1967  return 0; // Planes cross in a line
1968 }
1969 
1970 double math::distance(const TPolygon2D& p1, const TPolygon2D& p2)
1971 {
1972  MRPT_UNUSED_PARAM(p1);
1973  MRPT_UNUSED_PARAM(p2);
1974  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TPolygon2D)");
1975 }
1976 
1977 double math::distance(const TPolygon2D& p1, const TSegment2D& s2)
1978 {
1979  MRPT_UNUSED_PARAM(p1);
1980  MRPT_UNUSED_PARAM(s2);
1981  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TSegment)");
1982 }
1983 
1984 double math::distance(const TPolygon2D& p1, const TLine2D& l2)
1985 {
1986  MRPT_UNUSED_PARAM(p1);
1987  MRPT_UNUSED_PARAM(l2);
1988  THROW_EXCEPTION("TO DO:distance(TPolygon2D,TLine2D)");
1989 }
1990 
1991 double math::distance(const TPolygon3D& p1, const TPolygon3D& p2)
1992 {
1993  MRPT_UNUSED_PARAM(p1);
1994  MRPT_UNUSED_PARAM(p2);
1995  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPolygon3D");
1996 }
1997 
1998 double math::distance(const TPolygon3D& p1, const TSegment3D& s2)
1999 {
2000  MRPT_UNUSED_PARAM(p1);
2001  MRPT_UNUSED_PARAM(s2);
2002  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TSegment3D");
2003 }
2004 
2005 double math::distance(const TPolygon3D& p1, const TLine3D& l2)
2006 {
2007  MRPT_UNUSED_PARAM(p1);
2008  MRPT_UNUSED_PARAM(l2);
2009  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TLine3D");
2010 }
2011 
2012 double math::distance(const TPolygon3D& po, const TPlane& pl)
2013 {
2014  MRPT_UNUSED_PARAM(po);
2015  MRPT_UNUSED_PARAM(pl);
2016  THROW_EXCEPTION("TO DO:distance(TPolygon3D,TPlane");
2017 }
2018 
2020  const std::vector<TPoint3D>& poly, TPoint3D& pMin, TPoint3D& pMax)
2021 {
2022  size_t N = poly.size();
2023  if (N < 1) throw logic_error("Empty polygon");
2024  pMin = poly[0];
2025  pMax = poly[0];
2026  for (size_t i = 1; i < N; i++)
2027  {
2028  pMin.x = min(pMin.x, poly[i].x);
2029  pMin.y = min(pMin.y, poly[i].y);
2030  pMin.z = min(pMin.z, poly[i].z);
2031  pMax.x = max(pMax.x, poly[i].x);
2032  pMax.y = max(pMax.y, poly[i].y);
2033  pMax.z = max(pMax.z, poly[i].z);
2034  }
2035 }
2036 
2037 void createPlaneFromPoseAndAxis(const TPose3D& pose, TPlane& plane, size_t axis)
2038 {
2039  plane.coefs[3] = 0;
2040  CMatrixDouble44 m;
2041  pose.getHomogeneousMatrix(m);
2042  for (size_t i = 0; i < 3; i++)
2043  {
2044  plane.coefs[i] = m(i, axis);
2045  plane.coefs[3] -= plane.coefs[i] * m(i, 3);
2046  }
2047 }
2048 
2049 void math::createPlaneFromPoseXY(const TPose3D& pose, TPlane& plane)
2050 {
2051  createPlaneFromPoseAndAxis(pose, plane, 2);
2052 }
2053 
2054 void math::createPlaneFromPoseXZ(const TPose3D& pose, TPlane& plane)
2055 {
2056  createPlaneFromPoseAndAxis(pose, plane, 1);
2057 }
2058 
2059 void math::createPlaneFromPoseYZ(const TPose3D& pose, TPlane& plane)
2060 {
2061  createPlaneFromPoseAndAxis(pose, plane, 0);
2062 }
2063 
2065  const TPose3D& pose, const double (&normal)[3], TPlane& plane)
2066 {
2067  plane.coefs[3] = 0;
2068  CMatrixDouble44 m;
2069  pose.getHomogeneousMatrix(m);
2070  for (size_t i = 0; i < 3; i++)
2071  {
2072  plane.coefs[i] = 0;
2073  for (size_t j = 0; j < 3; j++) plane.coefs[i] += normal[j] * m(i, j);
2074  plane.coefs[3] -= plane.coefs[i] * m(i, 3);
2075  }
2076 }
2077 
2079  const double (&vec)[3], uint8_t coord, CMatrixDouble44& m)
2080 {
2081  // Assumes vector is unitary.
2082  // coord: 0=x, 1=y, 2=z.
2083  const uint8_t coord1 = (coord + 1) % 3;
2084  const uint8_t coord2 = (coord + 2) % 3;
2085  m.setZero();
2086  m(3, 3) = 1.0;
2087  for (size_t i = 0; i < 3; i++) m(i, coord) = vec[i];
2088  m(0, coord1) = 0;
2089  double h = hypot(vec[1], vec[2]);
2090  if (h < geometryEpsilon)
2091  {
2092  m(1, coord1) = 1;
2093  m(2, coord1) = 0;
2094  }
2095  else
2096  {
2097  m(1, coord1) = -vec[2] / h;
2098  m(2, coord1) = vec[1] / h;
2099  }
2100  m(0, coord2) = m(1, coord) * m(2, coord1) - m(2, coord) * m(1, coord1);
2101  m(1, coord2) = m(2, coord) * m(0, coord1) - m(0, coord) * m(2, coord1);
2102  m(2, coord2) = m(0, coord) * m(1, coord1) - m(1, coord) * m(0, coord1);
2103 }
2104 
2105 double math::getRegressionLine(const vector<TPoint2D>& points, TLine2D& line)
2106 {
2107  CVectorFixedDouble<2> means;
2108  CMatrixDouble22 covars;
2109  covariancesAndMean(points, covars, means);
2110 
2111  std::vector<double> eigenVals;
2112  CMatrixDouble22 eigenVecs;
2113  covars.eig_symmetric(eigenVecs, eigenVals);
2114 
2115  // size_t selected = (eigenVal[0] >= eigenVal[1]) ? 0 : 1;
2116  line.coefs[0] = -eigenVecs(1, 1); // 1: largest eigenVal
2117  line.coefs[1] = eigenVecs(0, 1);
2118  line.coefs[2] = -line.coefs[0] * means[0] - line.coefs[1] * means[1];
2119  return std::sqrt(eigenVals[0] / eigenVals[1]);
2120 }
2121 
2122 template <class T>
2123 inline size_t getIndexOfMin(const T& e1, const T& e2, const T& e3)
2124 {
2125  return (e1 < e2) ? ((e1 < e3) ? 0 : 2) : ((e2 < e3) ? 1 : 2);
2126 }
2127 
2128 double math::getRegressionLine(const vector<TPoint3D>& points, TLine3D& line)
2129 {
2130  CVectorFixedDouble<3> means;
2131  CMatrixDouble33 covars;
2132  covariancesAndMean(points, covars, means);
2133 
2134  std::vector<double> eigenVal;
2135  CMatrixDouble33 eigenVec;
2136  covars.eig_symmetric(eigenVec, eigenVal);
2137 
2138  const size_t selected = 2; // sorted: max eigenvalue
2139  for (size_t i = 0; i < 3; i++)
2140  {
2141  line.pBase[i] = means[i];
2142  line.director[i] = eigenVec(i, selected);
2143  }
2144  size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
2145  return std::sqrt((eigenVal[i1] + eigenVal[i2]) / eigenVal[selected]);
2146 }
2147 
2148 double math::getRegressionPlane(const vector<TPoint3D>& points, TPlane& plane)
2149 {
2150  vector<double> means;
2151  CMatrixDouble33 covars;
2152  covariancesAndMean(points, covars, means);
2153 
2154  std::vector<double> eigenVal;
2155  CMatrixDouble33 eigenVec;
2156  covars.eig_symmetric(eigenVec, eigenVal);
2157 
2158  for (size_t i = 0; i < 3; ++i)
2159  if (eigenVal[i] < 0 && std::abs(eigenVal[i]) < geometryEpsilon)
2160  eigenVal[i] = 0;
2161 
2162  const size_t selected = 0; // sorted: minimum eigenVal
2163  plane.coefs[3] = 0;
2164  for (size_t i = 0; i < 3; i++)
2165  {
2166  plane.coefs[i] = eigenVec(i, selected);
2167  plane.coefs[3] -= plane.coefs[i] * means[i];
2168  }
2169  size_t i1 = (selected + 1) % 3, i2 = (selected + 2) % 3;
2170  return std::sqrt(eigenVal[selected] / (eigenVal[i1] + eigenVal[i2]));
2171 }
2172 
2174  const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys)
2175 {
2176  std::vector<TSegment3D> tmp;
2177  assemblePolygons(segms, polys, tmp);
2178 }
2179 
2181 {
2182  size_t seg1;
2183  size_t seg2;
2184  bool seg1Point; // true for point2, false for point1
2185  bool seg2Point; // same
2186  MatchingVertex() = default;
2187  MatchingVertex(size_t s1, size_t s2, bool s1p, bool s2p)
2188  : seg1(s1), seg2(s2), seg1Point(s1p), seg2Point(s2p)
2189  {
2190  }
2191 };
2193 {
2194  public:
2195  const std::vector<TSegment3D>& segs;
2196  FCreatePolygon(const std::vector<TSegment3D>& s) : segs(s) {}
2197  TPolygon3D operator()(const std::vector<MatchingVertex>& vertices)
2198  {
2199  TPolygon3D res;
2200  size_t N = vertices.size();
2201  res.reserve(N);
2202  for (const auto& vertice : vertices)
2203  res.push_back(segs[vertice.seg2][vertice.seg2Point ? 1 : 0]);
2204  return res;
2205  }
2206 };
2207 inline bool firstOrNonPresent(size_t i, const std::vector<MatchingVertex>& v)
2208 {
2209  if (v.size() > 0 && v[0].seg1 == i) return true;
2210  for (const auto& it : v)
2211  if (it.seg1 == i || it.seg2 == i) return false;
2212  return true;
2213 }
2216  std::vector<std::vector<MatchingVertex>>& res, std::vector<bool>& used,
2217  size_t searching, unsigned char mask, std::vector<MatchingVertex>& current)
2218 {
2219  for (size_t i = 0; i < mat.cols(); i++)
2220  if (!used[i] && mat.isNotNull(searching, i))
2221  {
2222  unsigned char match = mat(searching, i) & mask;
2223  if (!match)
2224  continue;
2225  else if (firstOrNonPresent(i, current))
2226  {
2227  bool s1p, s2p;
2228  if (true == (s1p = (!(match & 3)))) match >>= 2;
2229  s2p = !(match & 1);
2230  if (current.size() >= 2 && current[0].seg1 == i)
2231  {
2232  if (s2p != current[0].seg1Point)
2233  {
2234  current.emplace_back(searching, i, s1p, s2p);
2235  for (auto it = current.begin(); it != current.end();
2236  ++it)
2237  used[it->seg2] = true;
2238  res.push_back(current);
2239  return true;
2240  }
2241  else
2242  continue; // Strange shape... not a polygon, although
2243  // it'll be without the first segment
2244  }
2245  else
2246  {
2247  current.emplace_back(searching, i, s1p, s2p);
2248  if (depthFirstSearch(
2249  mat, res, used, i, s2p ? 0x3 : 0xC, current))
2250  return true;
2251  current.pop_back();
2252  }
2253  }
2254  }
2255  // No match has been found. Backtrack
2256  return false;
2257 }
2260  std::vector<std::vector<MatchingVertex>>& res, std::vector<bool>& used)
2261 {
2262  vector<MatchingVertex> cur;
2263  for (size_t i = 0; i < used.size(); i++)
2264  if (!used[i])
2265  if (depthFirstSearch(mat, res, used, i, 0xf, cur)) cur.clear();
2266 }
2268  const std::vector<TSegment3D>& segms, std::vector<TPolygon3D>& polys,
2269  std::vector<TSegment3D>& remainder)
2270 {
2271  std::vector<TSegment3D> tmp;
2272  tmp.reserve(segms.size());
2273  for (const auto& segm : segms)
2274  if (segm.length() >= geometryEpsilon)
2275  tmp.push_back(segm);
2276  else
2277  remainder.push_back(segm);
2278  size_t N = tmp.size();
2280  for (size_t i = 0; i < N - 1; i++)
2281  for (size_t j = i + 1; j < N; j++)
2282  {
2283  if (distance(tmp[i].point1, tmp[j].point1) < geometryEpsilon)
2284  {
2285  matches(i, j) |= 1;
2286  matches(j, i) |= 1;
2287  }
2288  if (distance(tmp[i].point1, tmp[j].point2) < geometryEpsilon)
2289  {
2290  matches(i, j) |= 2;
2291  matches(j, i) |= 4;
2292  }
2293  if (distance(tmp[i].point2, tmp[j].point1) < geometryEpsilon)
2294  {
2295  matches(i, j) |= 4;
2296  matches(j, i) |= 2;
2297  }
2298  if (distance(tmp[i].point2, tmp[j].point2) < geometryEpsilon)
2299  {
2300  matches(i, j) |= 8;
2301  matches(j, i) |= 8;
2302  }
2303  }
2304  std::vector<std::vector<MatchingVertex>> results;
2305  std::vector<bool> usedSegments(N, false);
2306  depthFirstSearch(matches, results, usedSegments);
2307  polys.resize(results.size());
2308  transform(
2309  results.begin(), results.end(), polys.begin(), FCreatePolygon(segms));
2310  for (size_t i = 0; i < N; i++)
2311  if (!usedSegments[i]) remainder.push_back(tmp[i]);
2312 }
2313 
2315  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
2316 {
2317  std::vector<TObject3D> tmp;
2318  std::vector<TSegment3D> sgms;
2319  TObject3D::getPolygons(objs, polys, tmp);
2320  TObject3D::getSegments(tmp, sgms);
2321  assemblePolygons(sgms, polys);
2322 }
2323 
2325  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
2326  std::vector<TObject3D>& remainder)
2327 {
2328  std::vector<TObject3D> tmp;
2329  std::vector<TSegment3D> sgms, remainderSgms;
2330  TObject3D::getPolygons(objs, polys, tmp);
2331  TObject3D::getSegments(tmp, sgms, remainder);
2332  assemblePolygons(sgms, polys, remainderSgms);
2333  remainder.insert(
2334  remainder.end(), remainderSgms.begin(), remainderSgms.end());
2335 }
2336 
2338  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
2339  std::vector<TSegment3D>& remainder1, std::vector<TObject3D>& remainder2)
2340 {
2341  std::vector<TObject3D> tmp;
2342  std::vector<TSegment3D> sgms;
2343  TObject3D::getPolygons(objs, polys, tmp);
2344  TObject3D::getSegments(tmp, sgms, remainder2);
2345  assemblePolygons(sgms, polys, remainder1);
2346 }
2347 
2348 bool intersect(const TLine2D& l1, const TSegmentWithLine& s2, TObject2D& obj)
2349 {
2350  if (intersect(l1, s2.line, obj))
2351  {
2352  if (obj.isLine())
2353  {
2354  obj = s2.segment;
2355  return true;
2356  }
2357  else
2358  {
2359  TPoint2D p;
2360  obj.getPoint(p);
2361  return s2.segment.contains(p);
2362  }
2363  }
2364  else
2365  return false;
2366 }
2367 
2368 inline bool intersect(
2369  const TSegmentWithLine& s1, const TLine2D& l2, TObject2D& obj)
2370 {
2371  return intersect(l2, s1, obj);
2372 }
2373 
2375  const TPolygon2D& poly, vector<TPolygon2D>& components)
2376 {
2377  components.clear();
2378  size_t N = poly.size();
2379  if (N <= 3) return false;
2380  vector<TSegmentWithLine> segms(N);
2381  for (size_t i = 0; i < N - 1; i++)
2382  segms[i] = TSegmentWithLine(poly[i], poly[i + 1]);
2383  segms[N - 1] = TSegmentWithLine(poly[N - 1], poly[0]);
2384  TObject2D obj;
2385  TPoint2D pnt;
2386  for (size_t i = 0; i < N; i++)
2387  {
2388  size_t ii = (i + 2) % N, i_ = (i + N - 1) % N;
2389  for (size_t j = ii; j != i_; j = (j + 1) % N)
2390  if (intersect(segms[i].line, segms[j], obj) && obj.getPoint(pnt))
2391  {
2393  pnt, segms[i].segment
2394  [(distance(pnt, segms[i].segment.point1) <
2395  distance(pnt, segms[i].segment.point2))
2396  ? 0
2397  : 1]);
2398  bool cross = false;
2399  TPoint2D pTmp;
2400  for (size_t k = 0; (k < N) && !cross; k++)
2401  if (intersect(sTmp, segms[k], obj))
2402  {
2403  if (obj.getPoint(pTmp) &&
2404  (distance(pTmp, sTmp.segment[0]) >=
2405  geometryEpsilon) &&
2406  (distance(pTmp, sTmp.segment[1]) >=
2407  geometryEpsilon))
2408  cross = true;
2409  }
2410  if (cross) continue;
2411  // At this point, we have a suitable point, although we must
2412  // check if the division is right.
2413  // We do this by evaluating, in the expanded segment's line, the
2414  // next and previous points. If both signs differ, proceed.
2415  if (sign(segms[i].line.evaluatePoint(poly[(i + N - 1) % N])) ==
2416  sign(segms[i].line.evaluatePoint(poly[(i + 2) % N])))
2417  continue;
2418  TPolygon2D p1, p2;
2419  if (i > j)
2420  {
2421  p1.insert(p1.end(), poly.begin() + i + 1, poly.end());
2422  p1.insert(p1.end(), poly.begin(), poly.begin() + j + 1);
2423  p2.insert(
2424  p2.end(), poly.begin() + j + 1, poly.begin() + i + 1);
2425  }
2426  else
2427  {
2428  p1.insert(
2429  p1.end(), poly.begin() + i + 1, poly.begin() + j + 1);
2430  p2.insert(p2.end(), poly.begin() + j + 1, poly.end());
2431  p2.insert(p2.end(), poly.begin(), poly.begin() + i + 1);
2432  }
2433  if (distance(*(p1.rbegin()), pnt) >= geometryEpsilon)
2434  p1.push_back(pnt);
2435  if (distance(*(p2.rbegin()), pnt) >= geometryEpsilon)
2436  p2.push_back(pnt);
2439  vector<TPolygon2D> tempComps;
2440  if (splitInConvexComponents(p1, tempComps))
2441  components.insert(
2442  components.end(), tempComps.begin(), tempComps.end());
2443  else
2444  components.push_back(p1);
2445  if (splitInConvexComponents(p2, tempComps))
2446  components.insert(
2447  components.end(), tempComps.begin(), tempComps.end());
2448  else
2449  components.push_back(p2);
2450  return true;
2451  }
2452  }
2453  return false;
2454 }
2455 
2457 {
2458  public:
2459  const TPose3D& pose;
2461  FUnprojectPolygon2D(const TPose3D& p) : pose(p), tmp1(0), tmp2(0) {}
2463  {
2464  tmp1 = TPolygon3D(poly2D);
2465  project3D(tmp1, pose, tmp2);
2466  return tmp2;
2467  }
2468 };
2470  const TPolygon3D& poly, vector<TPolygon3D>& components)
2471 {
2472  TPlane p;
2473  if (!poly.getPlane(p)) throw std::logic_error("Polygon is skew");
2474  TPose3D pose1;
2475  p.getAsPose3DForcingOrigin(poly[0], pose1);
2476  const TPose3D pose2 = -pose1;
2477  TPolygon3D polyTmp;
2478  project3D(poly, pose2, polyTmp);
2479  TPolygon2D poly2D = TPolygon2D(polyTmp);
2480  vector<TPolygon2D> components2D;
2481  if (splitInConvexComponents(poly2D, components2D))
2482  {
2483  components.resize(components2D.size());
2484  transform(
2485  components2D.begin(), components2D.end(), components.begin(),
2486  FUnprojectPolygon2D(pose1));
2487  return true;
2488  }
2489  else
2490  return false;
2491 }
2492 
2494 {
2495  TPoint2D p;
2496  sgm.getCenter(p);
2497  bis.coefs[0] = sgm.point2.x - sgm.point1.x;
2498  bis.coefs[1] = sgm.point2.y - sgm.point1.y;
2499  bis.coefs[2] = -bis.coefs[0] * p.x - bis.coefs[1] * p.y;
2500  bis.unitarize();
2501 }
2502 
2504 {
2505  TPoint3D p;
2506  sgm.getCenter(p);
2507  bis.coefs[0] = sgm.point2.x - sgm.point1.x;
2508  bis.coefs[1] = sgm.point2.y - sgm.point1.y;
2509  bis.coefs[2] = sgm.point2.z - sgm.point1.z;
2510  bis.coefs[2] =
2511  -bis.coefs[0] * p.x - bis.coefs[1] * p.y - bis.coefs[2] * p.z;
2512  bis.unitarize();
2513 }
2514 
2515 void math::getAngleBisector(const TLine2D& l1, const TLine2D& l2, TLine2D& bis)
2516 {
2517  TPoint2D p;
2518  TObject2D obj;
2519  if (!intersect(l1, l2, obj))
2520  {
2521  // Both lines are parallel
2522  double mod1 = sqrt(square(l1.coefs[0]) + square(l1.coefs[1]));
2523  double mod2 = sqrt(square(l2.coefs[0]) + square(l2.coefs[2]));
2524  bis.coefs[0] = l1.coefs[0] / mod1;
2525  bis.coefs[1] = l1.coefs[1] / mod1;
2526  bool sameSign;
2527  if (abs(bis.coefs[0]) < geometryEpsilon)
2528  sameSign = (l1.coefs[1] * l2.coefs[1]) > 0;
2529  else
2530  sameSign = (l1.coefs[0] * l2.coefs[0]) > 0;
2531  if (sameSign)
2532  bis.coefs[2] = (l1.coefs[2] / mod1) + (l2.coefs[2] / mod2);
2533  else
2534  bis.coefs[2] = (l1.coefs[2] / mod1) - (l2.coefs[2] / mod2);
2535  }
2536  else if (obj.getPoint(p))
2537  {
2538  // Both lines cross
2539  double ang1 = atan2(-l1.coefs[0], l1.coefs[1]);
2540  double ang2 = atan2(-l2.coefs[0], l2.coefs[1]);
2541  double ang = (ang1 + ang2) / 2;
2542  bis.coefs[0] = -sin(ang);
2543  bis.coefs[1] = cos(ang);
2544  bis.coefs[2] = -bis.coefs[0] * p.x - bis.coefs[1] * p.y;
2545  }
2546  else
2547  {
2548  bis = l1;
2549  bis.unitarize();
2550  }
2551 }
2552 
2553 void math::getAngleBisector(const TLine3D& l1, const TLine3D& l2, TLine3D& bis)
2554 {
2555  TPlane p = TPlane(l1, l2); // May throw an exception
2556  TLine3D l1P, l2P;
2557  TLine2D bis2D;
2558  TPose3D pose, pose2;
2559  p.getAsPose3D(pose);
2560  pose2 = -pose;
2561  project3D(l1, pose2, l1P);
2562  project3D(l2, pose2, l2P);
2563  getAngleBisector(TLine2D(l1P), TLine2D(l2P), bis2D);
2564  project3D(TLine3D(bis2D), pose, bis);
2565 }
2566 
2568  const vector<TPolygonWithPlane>& vec, const TPose3D& pose, double& dist)
2569 {
2570  dist = HUGE_VAL;
2571  double nDist = 0;
2572  TLine3D lin;
2573  createFromPoseX(pose, lin);
2574  lin.unitarize();
2575  bool res = false;
2576  for (const auto& it : vec)
2577  if (::intersect(it, lin, nDist, dist))
2578  {
2579  res = true;
2580  dist = nDist;
2581  }
2582  return res;
2583 }
2584 
2586  double dx, double dy, double dz)
2587 {
2588  MRPT_START
2589 
2590  if (std::abs(dx) < 1e-10 && std::abs(dy) < 1e-10 && std::abs(dz) < 1e-10)
2591  THROW_EXCEPTION("Invalid input: Direction vector is (0,0,0);");
2592 
2593  CMatrixDouble33 P;
2594 
2595  // 1st vector:
2596  double n_xy = square(dx) + square(dy);
2597  double n = sqrt(n_xy + square(dz));
2598  n_xy = sqrt(n_xy);
2599  P(0, 0) = dx / n;
2600  P(1, 0) = dy / n;
2601  P(2, 0) = dz / n;
2602 
2603  // 2nd perpendicular vector:
2604  if (fabs(dx) > 1e-4 || fabs(dy) > 1e-4)
2605  {
2606  P(0, 1) = -dy / n_xy;
2607  P(1, 1) = dx / n_xy;
2608  P(2, 1) = 0;
2609  }
2610  else
2611  {
2612  // Any vector in the XY plane will work:
2613  P(0, 1) = 1;
2614  P(1, 1) = 0;
2615  P(2, 1) = 0;
2616  }
2617 
2618  // 3rd perpendicular vector: cross product of the two last vectors:
2619  Eigen::Vector3d c2;
2620  crossProduct3D(P.col(0), P.col(1), c2);
2621  P.col(2) = c2;
2622 
2623  return P;
2624  MRPT_END
2625 }
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:2078
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:1421
TPolygon3D operator()(const TPolygon2D &poly2D)
Definition: geometry.cpp:2462
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:2374
GLdouble GLdouble z
Definition: glext.h:3879
#define min(a, b)
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:1634
double x
X,Y coordinates.
Definition: TPose2D.h:30
bool firstOrNonPresent(size_t i, const std::vector< MatchingVertex > &v)
Definition: geometry.cpp:2207
#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:2214
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:2567
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:2461
void destroy()
Definition: geometry.cpp:1331
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:2173
void getSegmentBisector(const TSegment2D &sgm, TLine2D &bis)
Gets the bisector of a 2D segment.
Definition: geometry.cpp:2493
mrpt::math::TPoint2D composePoint(const TPoint2D l) const
char fromObject(const TObject2D &obj)
Definition: geometry.cpp:1446
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:2049
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:1417
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
void getSegmentsWithLine(const TPolygon2D &poly, vector< TSegmentWithLine > &segs)
Definition: geometry.cpp:1437
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:2064
size_t getIndexOfMin(const T &e1, const T &e2, const T &e3)
Definition: geometry.cpp:2123
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:1415
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:2515
GLuint coord
Definition: glext.h:7245
A sparse matrix container (with cells of any type), with iterators.
TTempIntersection(const TCommonRegion &common)
Definition: geometry.cpp:1391
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.
unsigned char uint8_t
Definition: rptypes.h:44
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
T square(const T x)
Inline function for the square of a number.
const std::vector< TSegment3D > & segs
Definition: geometry.cpp:2195
TTempIntersection & operator=(const TTempIntersection &t)
Definition: geometry.cpp:1396
unsigned char type
Definition: geometry.cpp:1326
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:1350
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
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:1904
void createPlaneFromPoseXZ(const mrpt::math::TPose3D &pose, TPlane &plane)
Given a pose, creates a plane orthogonal to its Y vector.
Definition: geometry.cpp:2054
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
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:2148
void getCenter(TPoint3D &p) const
Segment&#39;s central point.
Definition: TSegment3D.h:81
FCreatePolygon(const std::vector< TSegment3D > &s)
Definition: geometry.cpp:2196
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:1344
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:1387
TPolygon3D operator()(const std::vector< MatchingVertex > &vertices)
Definition: geometry.cpp:2197
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:2105
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:2459
mrpt::math::TPose3D pose
Plane&#39;s pose.
Definition: geometry.h:40
T2ListsOfSegments * segms
Definition: geometry.cpp:1371
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:2019
TPoint2D * point
Definition: geometry.cpp:1328
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:2585
#define MRPT_END
Definition: exceptions.h:245
void createPlaneFromPoseAndAxis(const TPose3D &pose, TPlane &plane, size_t axis)
Definition: geometry.cpp:2037
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:2187
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:2059
GLenum GLint GLint y
Definition: glext.h:3542
TCommonRegion * common
Definition: geometry.cpp:1372
int sign(T x)
Returns the sign of X as "1" or "-1".
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:1611
GLfloat GLfloat GLfloat v2
Definition: glext.h:4123
TCommonRegion(const TCommonRegion &r)
Definition: geometry.cpp:1365
bool intersectAux(const TPolygon3D &p1, const TPlane &pl1, const TPolygon3D &p2, const TPlane &pl2, TObject3D &obj)
Definition: geometry.cpp:1580
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:1411
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:1369
double phi
Orientation (rads)
Definition: TPose2D.h:32
TCommonRegion(const TSegment2D &s)
Definition: geometry.cpp:1345
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:1889
void getMinAndMaxBounds(const std::vector< TPolygon3D > &v1, std::vector< TPoint3D > &minP, std::vector< TPoint3D > &maxP)
Definition: geometry.cpp:1643
TSegment2D * segment
Definition: geometry.cpp:1329
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: ee555d257 Fri Aug 16 10:05:39 2019 +0200 at vie ago 16 10:10:14 CEST 2019