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

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020