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

 Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at mar mar 31 20:00:11 CEST 2020