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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019