MRPT  1.9.9
TPoseOrPoint.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/core/exceptions.h>
13 #include <mrpt/math/CMatrixFixed.h>
14 #include <mrpt/math/CQuaternion.h>
16 #include <mrpt/math/TLine2D.h>
17 #include <mrpt/math/TLine3D.h>
18 #include <mrpt/math/TObject2D.h>
19 #include <mrpt/math/TObject3D.h>
20 #include <mrpt/math/TPlane.h>
21 #include <mrpt/math/TPoint2D.h>
22 #include <mrpt/math/TPoint3D.h>
23 #include <mrpt/math/TPolygon2D.h>
24 #include <mrpt/math/TPolygon3D.h>
25 #include <mrpt/math/TPose2D.h>
26 #include <mrpt/math/TPose3D.h>
27 #include <mrpt/math/TPose3DQuat.h>
28 #include <mrpt/math/TSegment2D.h>
29 #include <mrpt/math/TSegment3D.h>
30 #include <mrpt/math/TTwist2D.h>
31 #include <mrpt/math/TTwist3D.h>
32 #include <mrpt/math/geometry.h> // distance()
34 #include <mrpt/math/ops_containers.h> // squareNorm()
35 #include <mrpt/serialization/CArchive.h> // impl of << operator
37 #include <Eigen/Dense>
38 
39 using namespace std; // For min/max, etc...
40 using namespace mrpt::serialization; // CArchive, << operator for STL
41 
42 using mrpt::DEG2RAD;
43 using mrpt::RAD2DEG;
44 
45 namespace mrpt::math
46 {
47 static_assert(std::is_trivial_v<TPoint2D_data>);
48 static_assert(std::is_trivially_copyable_v<TPoint2D>);
49 static_assert(std::is_trivial_v<TPoint3D_data>);
50 static_assert(std::is_trivially_copyable_v<TPoint3D>);
51 static_assert(std::is_trivially_copyable_v<TPose2D>);
52 static_assert(std::is_trivially_copyable_v<TPose3D>);
53 static_assert(std::is_trivially_copyable_v<TPlane3D>);
54 static_assert(std::is_trivially_copyable_v<TLine2D>);
55 static_assert(std::is_trivially_copyable_v<TLine3D>);
56 static_assert(std::is_trivially_copyable_v<TTwist2D>);
57 static_assert(std::is_trivially_copyable_v<TTwist3D>);
58 
59 TPoint2D::TPoint2D(const TPose2D& p)
60 {
61  x = p.x;
62  y = p.y;
63 }
64 TPoint2D::TPoint2D(const TPoint3D& p)
65 {
66  x = p.x;
67  y = p.y;
68 }
69 TPoint2D::TPoint2D(const TPose3D& p)
70 {
71  x = p.x;
72  y = p.y;
73 }
74 
75 bool TPoint2D::operator<(const TPoint2D& p) const
76 {
77  if (x < p.x)
78  return true;
79  else if (x > p.x)
80  return false;
81  else
82  return y < p.y;
83 }
84 void TPoint2D::fromString(const std::string& s)
85 {
86  CMatrixDouble m;
87  if (!m.fromMatlabStringFormat(s))
88  THROW_EXCEPTION("Malformed expression in ::fromString");
89  ASSERTMSG_(
90  m.rows() == 1 && m.cols() == 2, "Wrong size of vector in ::fromString");
91  x = m(0, 0);
92  y = m(0, 1);
93 }
94 
95 TPose2D::TPose2D(const TPoint2D& p) : x(p.x), y(p.y), phi(0.0) {}
96 TPose2D::TPose2D(const TPoint3D& p) : x(p.x), y(p.y), phi(0.0) {}
97 TPose2D::TPose2D(const TPose3D& p) : x(p.x), y(p.y), phi(p.yaw) {}
99 {
100  s = mrpt::format("[%f %f %f]", x, y, RAD2DEG(phi));
101 }
103 {
104  CMatrixDouble m;
105  if (!m.fromMatlabStringFormat(s))
106  THROW_EXCEPTION("Malformed expression in ::fromString");
107  ASSERTMSG_(
108  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
109  x = m(0, 0);
110  y = m(0, 1);
111  phi = DEG2RAD(m(0, 2));
112 }
114  const mrpt::math::TPose2D& b) const
115 {
116  const double A_cosphi = cos(this->phi), A_sinphi = sin(this->phi);
117  // Use temporary variables for the cases (A==this) or (B==this)
118  const double new_x = this->x + b.x * A_cosphi - b.y * A_sinphi;
119  const double new_y = this->y + b.x * A_sinphi + b.y * A_cosphi;
120  const double new_phi = mrpt::math::wrapToPi(this->phi + b.phi);
121 
122  return mrpt::math::TPose2D(new_x, new_y, new_phi);
123 }
124 
126  const mrpt::math::TPose2D& b) const
127 {
128  const double B_cosphi = cos(b.phi), B_sinphi = sin(b.phi);
129 
130  const double new_x =
131  (this->x - b.x) * B_cosphi + (this->y - b.y) * B_sinphi;
132  const double new_y =
133  -(this->x - b.x) * B_sinphi + (this->y - b.y) * B_cosphi;
134  const double new_phi = mrpt::math::wrapToPi(this->phi - b.phi);
135 
136  return mrpt::math::TPose2D(new_x, new_y, new_phi);
137 }
139 {
140  const double ccos = ::cos(phi), csin = ::sin(phi);
141  return {x + l.x * ccos - l.y * csin, y + l.x * csin + l.y * ccos};
142 }
144 {
145  return this->composePoint(b);
146 }
147 
149 {
150  const double Ax = g.x - x, Ay = g.y - y, ccos = ::cos(phi),
151  csin = ::sin(phi);
152  return {Ax * ccos + Ay * csin, -Ax * csin + Ay * ccos};
153 }
154 
155 // ----
157 {
158  s = mrpt::format("[%f %f %f]", vx, vy, RAD2DEG(omega));
159 }
161 {
162  CMatrixDouble m;
163  if (!m.fromMatlabStringFormat(s))
164  THROW_EXCEPTION("Malformed expression in ::fromString");
165  ASSERTMSG_(
166  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
167  vx = m(0, 0);
168  vy = m(0, 1);
169  omega = DEG2RAD(m(0, 2));
170 }
171 // Transform the (vx,vy) components for a counterclockwise rotation of `ang`
172 // radians
173 void TTwist2D::rotate(const double ang)
174 {
175  const double nvx = vx * cos(ang) - vy * sin(ang);
176  const double nvy = vx * sin(ang) + vy * cos(ang);
177  vx = nvx;
178  vy = nvy;
179 }
180 bool TTwist2D::operator==(const TTwist2D& o) const
181 {
182  return vx == o.vx && vy == o.vy && omega == o.omega;
183 }
184 bool TTwist2D::operator!=(const TTwist2D& o) const { return !(*this == o); }
186 {
187  return mrpt::math::TPose2D(vx * dt, vy * dt, omega * dt);
188 }
189 
190 // ----
192 {
193  s = mrpt::format(
194  "[%f %f %f %f %f %f]", vx, vy, vz, RAD2DEG(wx), RAD2DEG(wy),
195  RAD2DEG(wz));
196 }
198 {
199  CMatrixDouble m;
200  if (!m.fromMatlabStringFormat(s))
201  THROW_EXCEPTION("Malformed expression in ::fromString");
202  ASSERTMSG_(
203  m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
204  for (int i = 0; i < 3; i++) (*this)[i] = m(0, i);
205  for (int i = 0; i < 3; i++) (*this)[3 + i] = DEG2RAD(m(0, 3 + i));
206 }
207 // Transform all 6 components for a change of reference frame from "A" to
208 // another frame "B" whose rotation with respect to "A" is given by `rot`. The
209 // translational part of the pose is ignored
210 void TTwist3D::rotate(const TPose3D& rot)
211 {
212  const TTwist3D t = *this;
214  rot.getRotationMatrix(R);
215  vx = R(0, 0) * t.vx + R(0, 1) * t.vy + R(0, 2) * t.vz;
216  vy = R(1, 0) * t.vx + R(1, 1) * t.vy + R(1, 2) * t.vz;
217  vz = R(2, 0) * t.vx + R(2, 1) * t.vy + R(2, 2) * t.vz;
218 
219  wx = R(0, 0) * t.wx + R(0, 1) * t.wy + R(0, 2) * t.wz;
220  wy = R(1, 0) * t.wx + R(1, 1) * t.wy + R(1, 2) * t.wz;
221  wz = R(2, 0) * t.wx + R(2, 1) * t.wy + R(2, 2) * t.wz;
222 }
223 bool TTwist3D::operator==(const TTwist3D& o) const
224 {
225  return vx == o.vx && vy == o.vy && vz == o.vz && wx == o.wx && wy == o.wy &&
226  wz == o.wz;
227 }
228 bool TTwist3D::operator!=(const TTwist3D& o) const { return !(*this == o); }
230 {
231  x = p.x;
232  y = p.y;
233  z = 0;
234 }
236 {
237  x = p.x;
238  y = p.y;
239  z = 0;
240 }
242 {
243  x = p.x;
244  y = p.y;
245  z = p.z;
246 }
247 bool TPoint3D::operator<(const TPoint3D& p) const
248 {
249  if (x < p.x)
250  return true;
251  else if (x > p.x)
252  return false;
253  else if (y < p.y)
254  return true;
255  else if (y > p.y)
256  return false;
257  else
258  return z < p.z;
259 }
261 {
262  CMatrixDouble m;
263  if (!m.fromMatlabStringFormat(s))
264  THROW_EXCEPTION("Malformed expression in ::fromString");
265  ASSERTMSG_(
266  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
267  x = m(0, 0);
268  y = m(0, 1);
269  z = m(0, 2);
270 }
271 
273  : x(p.x), y(p.y), z(0.0), yaw(0.0), pitch(0.0), roll(0.0)
274 {
275 }
277  : x(p.x), y(p.y), z(0.0), yaw(p.phi), pitch(0.0), roll(0.0)
278 {
279 }
281  : x(p.x), y(p.y), z(p.z), yaw(0.0), pitch(0.0), roll(0.0)
282 {
283 }
285 {
286  s = mrpt::format(
287  "[%f %f %f %f %f %f]", x, y, z, RAD2DEG(yaw), RAD2DEG(pitch),
288  RAD2DEG(roll));
289 }
293 {
294  // See:
295  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
296  const double cy = cos(yaw * 0.5), sy = sin(yaw * 0.5);
297  const double cp = cos(pitch * 0.5), sp = sin(pitch * 0.5);
298  const double cr = cos(roll * 0.5), sr = sin(roll * 0.5);
299 
300  const double ccc = cr * cp * cy;
301  const double ccs = cr * cp * sy;
302  const double css = cr * sp * sy;
303  const double sss = sr * sp * sy;
304  const double scc = sr * cp * cy;
305  const double ssc = sr * sp * cy;
306  const double csc = cr * sp * cy;
307  const double scs = sr * cp * sy;
308 
309  q[0] = ccc + sss;
310  q[1] = scc - css;
311  q[2] = csc + scs;
312  q[3] = ccs - ssc;
313 
314  // Compute 4x3 Jacobian: for details, see technical report:
315  // Parameterizations of SE(3) transformations: equivalences, compositions
316  // and uncertainty, J.L. Blanco (2010).
317  // https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
318  if (out_dq_dr)
319  {
320  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[4 * 3] = {
321  -0.5 * q[3], 0.5 * (-csc + scs), -0.5 * q[1],
322  -0.5 * q[2], 0.5 * (-ssc - ccs), 0.5 * q[0],
323  0.5 * q[1], 0.5 * (ccc - sss), 0.5 * q[3],
324  0.5 * q[0], 0.5 * (-css - scc), -0.5 * q[2]};
325  out_dq_dr->loadFromArray(nums);
326  }
327 }
328 void TPose3D::composePoint(const TPoint3D& l, TPoint3D& g) const
329 {
331  this->getRotationMatrix(R);
332  TPoint3D res;
333  res.x = R(0, 0) * l.x + R(0, 1) * l.y + R(0, 2) * l.z + this->x;
334  res.y = R(1, 0) * l.x + R(1, 1) * l.y + R(1, 2) * l.z + this->y;
335  res.z = R(2, 0) * l.x + R(2, 1) * l.y + R(2, 2) * l.z + this->z;
336 
337  g = res;
338 }
340 {
341  TPoint3D g;
342  composePoint(l, g);
343  return g;
344 }
345 
347 {
348  CMatrixDouble44 H;
350  TPoint3D res;
351  res.x = H(0, 0) * g.x + H(0, 1) * g.y + H(0, 2) * g.z + H(0, 3);
352  res.y = H(1, 0) * g.x + H(1, 1) * g.y + H(1, 2) * g.z + H(1, 3);
353  res.z = H(2, 0) * g.x + H(2, 1) * g.y + H(2, 2) * g.z + H(2, 3);
354 
355  l = res;
356 }
358 {
359  TPoint3D l;
361  return l;
362 }
363 
365 {
366  const double cy = cos(yaw);
367  const double sy = sin(yaw);
368  const double cp = cos(pitch);
369  const double sp = sin(pitch);
370  const double cr = cos(roll);
371  const double sr = sin(roll);
372 
373  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
374  const double rot_vals[] = {cy * cp,
375  cy * sp * sr - sy * cr,
376  cy * sp * cr + sy * sr,
377  sy * cp,
378  sy * sp * sr + cy * cr,
379  sy * sp * cr - cy * sr,
380  -sp,
381  cp * sr,
382  cp * cr};
383  R.loadFromArray(rot_vals);
384 }
386  const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
387  double& roll)
388 {
390  std::abs(
391  sqrt(square(R(0, 0)) + square(R(1, 0)) + square(R(2, 0))) - 1) <
392  3e-3,
393  "Homogeneous matrix is not orthogonal & normalized!: " +
394  R.inMatlabFormat());
396  std::abs(
397  sqrt(square(R(0, 1)) + square(R(1, 1)) + square(R(2, 1))) - 1) <
398  3e-3,
399  "Homogeneous matrix is not orthogonal & normalized!: " +
400  R.inMatlabFormat());
402  std::abs(
403  sqrt(square(R(0, 2)) + square(R(1, 2)) + square(R(2, 2))) - 1) <
404  3e-3,
405  "Homogeneous matrix is not orthogonal & normalized!: " +
406  R.inMatlabFormat());
407 
408  // Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
409  pitch = atan2(-R(2, 0), hypot(R(0, 0), R(1, 0)));
410 
411  // Roll:
412  if ((fabs(R(2, 1)) + fabs(R(2, 2))) <
413  10 * std::numeric_limits<double>::epsilon())
414  {
415  // Gimbal lock between yaw and roll. This one is arbitrarily forced to
416  // be zero.
417  // Check
418  // https://reference.mrpt.org/devel/classmrpt_1_1poses_1_1_c_pose3_d.html.
419  // If cos(pitch)==0, the homogeneous matrix is:
420  // When sin(pitch)==1:
421  // /0 cysr-sycr cycr+sysr x\ /0 sin(r-y) cos(r-y) x\.
422  // |0 sysr+cycr sycr-cysr y| = |0 cos(r-y) -sin(r-y) y|
423  // |-1 0 0 z| |-1 0 0 z|
424  // \0 0 0 1/ \0 0 0 1/
425  //
426  // And when sin(pitch)=-1:
427  // /0 -cysr-sycr -cycr+sysr x\ /0 -sin(r+y) -cos(r+y) x\.
428  // |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y) -sin(r+y) y|
429  // |1 0 0 z| |1 0 0 z|
430  // \0 0 0 1/ \0 0 0 1/
431  //
432  // Both cases are in a "gimbal lock" status. This happens because pitch
433  // is vertical.
434 
435  roll = 0.0;
436  if (pitch > 0)
437  yaw = atan2(R(1, 2), R(0, 2));
438  else
439  yaw = atan2(-R(1, 2), -R(0, 2));
440  }
441  else
442  {
443  roll = atan2(R(2, 1), R(2, 2));
444  // Yaw:
445  yaw = atan2(R(1, 0), R(0, 0));
446  }
447 }
448 
450 {
452  CMatrixDouble33(HG.blockCopy<3, 3>(0, 0)), yaw, pitch, roll);
453  x = HG(0, 3);
454  y = HG(1, 3);
455  z = HG(2, 3);
456 }
457 void TPose3D::composePose(const TPose3D other, TPose3D& result) const
458 {
459  CMatrixDouble44 me_H, o_H;
460  this->getHomogeneousMatrix(me_H);
461  other.getHomogeneousMatrix(o_H);
462  result.fromHomogeneousMatrix(
463  CMatrixDouble44(me_H.asEigen() * o_H.asEigen()));
464 }
466 {
469  HG.block<3, 3>(0, 0) = R.asEigen();
470  HG(0, 3) = x;
471  HG(1, 3) = y;
472  HG(2, 3) = z;
473  HG(3, 0) = HG(3, 1) = HG(3, 2) = 0.;
474  HG(3, 3) = 1.;
475 }
477 { // Get current HM & inverse in-place:
478  this->getHomogeneousMatrix(HG);
480 }
481 
483 {
484  CMatrixDouble m;
485  if (!m.fromMatlabStringFormat(s))
486  THROW_EXCEPTION("Malformed expression in ::fromString");
487  ASSERTMSG_(
488  m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
489  x = m(0, 0);
490  y = m(0, 1);
491  z = m(0, 2);
492  yaw = DEG2RAD(m(0, 3));
493  pitch = DEG2RAD(m(0, 4));
494  roll = DEG2RAD(m(0, 5));
495 }
496 
498 {
499  CMatrixDouble m;
500  if (!m.fromMatlabStringFormat(s))
501  THROW_EXCEPTION("Malformed expression in ::fromString");
502  ASSERTMSG_(
503  m.rows() == 1 && m.cols() == 7, "Wrong size of vector in ::fromString");
504  for (int i = 0; i < m.cols(); i++) (*this)[i] = m(0, i);
505 }
507 {
508  CMatrixDouble44 H;
509  p.getInverseHomogeneousMatrix(H);
510  TPose3D ret;
511  ret.fromHomogeneousMatrix(H);
512  return ret;
513 }
515 {
516  // b - a = A^{-1} * B
517  CMatrixDouble44 Hainv, Hb;
518  a.getInverseHomogeneousMatrix(Hainv);
519  b.getHomogeneousMatrix(Hb);
520  TPose3D ret;
522  return ret;
523 }
524 
525 // Text streaming:
526 std::ostream& operator<<(std::ostream& o, const TPoint2D& p)
527 {
528  return (o << p.asString());
529 }
530 std::ostream& operator<<(std::ostream& o, const TPoint3D& p)
531 {
532  return (o << p.asString());
533 }
534 std::ostream& operator<<(std::ostream& o, const TPose2D& p)
535 {
536  return (o << p.asString());
537 }
538 std::ostream& operator<<(std::ostream& o, const TPose3D& p)
539 {
540  return (o << p.asString());
541 }
542 std::ostream& operator<<(std::ostream& o, const TPose3DQuat& p)
543 {
544  return (o << p.asString());
545 }
546 
548 {
549  return in >> s.point1 >> s.point2;
550 }
552 {
553  return out << s.point1 << s.point2;
554 }
556 {
557  return in >> l.coefs[0] >> l.coefs[1] >> l.coefs[2];
558 }
560 {
561  return out << l.coefs[0] << l.coefs[1] << l.coefs[2];
562 }
563 
565 {
566  return in >> s.point1 >> s.point2;
567 }
569 {
570  return out << s.point1 << s.point2;
571 }
573 {
574  return in >> l.pBase >> l.director[0] >> l.director[1] >> l.director[2];
575 }
577 {
578  return out << l.pBase << l.director[0] << l.director[1] << l.director[2];
579 }
581 {
582  return in >> p.coefs[0] >> p.coefs[1] >> p.coefs[2] >> p.coefs[3];
583 }
585 {
586  return out << p.coefs[0] << p.coefs[1] << p.coefs[2] << p.coefs[3];
587 }
588 
589 double TSegment2D::length() const { return math::distance(point1, point2); }
590 double TSegment2D::distance(const TPoint2D& point) const
591 {
592  return std::abs(signedDistance(point));
593 }
594 double TSegment2D::signedDistance(const TPoint2D& point) const
595 {
596  // It is reckoned whether the perpendicular line to the TSegment2D which
597  // passes through point crosses or not the referred segment,
598  // or what is the same, whether point makes an obtuse triangle with the
599  // segment or not (being the longest segment one between the point and
600  // either end of TSegment2D).
601  const double d1 = math::distance(point, point1);
602  if (point1 == point2) return d1;
603 
604  const double d2 = math::distance(point, point2);
605  const double d3 = length();
606  const double ds1 = square(d1);
607  const double ds2 = square(d2);
608  const double ds3 = square(d3);
609  if (ds1 > (ds2 + ds3) || ds2 > (ds1 + ds3))
610  // Fix sign:
611  return min(d1, d2) *
612  (TLine2D(*this).signedDistance(point) < 0 ? -1 : 1);
613  else
614  return TLine2D(*this).signedDistance(point);
615 }
616 bool TSegment2D::contains(const TPoint2D& point) const
617 {
618  return abs(math::distance(point1, point) + math::distance(point2, point) -
620 }
622 {
623  s = TSegment3D(*this);
624 }
626 {
627  point1 = TPoint2D(s.point1);
628  point2 = TPoint2D(s.point2);
629  if (point1 == point2)
630  throw std::logic_error("Segment is normal to projection plane");
631 }
632 
634 {
635  if (point1 < s.point1)
636  return true;
637  else if (s.point1 < point1)
638  return false;
639  else
640  return point2 < s.point2;
641 }
642 
644 {
645  s = TSegment2D(*this);
646 }
647 
648 double TSegment3D::length() const { return math::distance(point1, point2); }
649 double TSegment3D::distance(const TPoint3D& point) const
650 {
651  return min(
652  min(math::distance(point, point1), math::distance(point, point2)),
653  TLine3D(*this).distance(point));
654 }
655 double TSegment3D::distance(const TSegment3D& segment) const
656 {
657  Eigen::Vector3d u, v, w;
658  TPoint3D diff_vect = point2 - point1;
659  diff_vect.asVector(u);
660  diff_vect = segment.point2 - segment.point1;
661  diff_vect.asVector(v);
662  diff_vect = point1 - segment.point1;
663  diff_vect.asVector(w);
664  double a = u.dot(u); // always >= 0
665  double b = u.dot(v);
666  double c = v.dot(v); // always >= 0
667  double d = u.dot(w);
668  double e = v.dot(w);
669  double D = a * c - b * b; // always >= 0
670  double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
671  double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
672 
673  // compute the line parameters of the two closest points
674  if (D < 0.00000001)
675  { // the lines are almost parallel
676  sN = 0.0; // force using point P0 on segment S1
677  sD = 1.0; // to prevent possible division by 0.0 later
678  tN = e;
679  tD = c;
680  }
681  else
682  { // get the closest points on the infinite lines
683  sN = (b * e - c * d);
684  tN = (a * e - b * d);
685  if (sN < 0.0)
686  { // sc < 0 => the s=0 edge is visible
687  sN = 0.0;
688  tN = e;
689  tD = c;
690  }
691  else if (sN > sD)
692  { // sc > 1 => the s=1 edge is visible
693  sN = sD;
694  tN = e + b;
695  tD = c;
696  }
697  }
698 
699  if (tN < 0.0)
700  { // tc < 0 => the t=0 edge is visible
701  tN = 0.0;
702  // recompute sc for this edge
703  if (-d < 0.0)
704  sN = 0.0;
705  else if (-d > a)
706  sN = sD;
707  else
708  {
709  sN = -d;
710  sD = a;
711  }
712  }
713  else if (tN > tD)
714  { // tc > 1 => the t=1 edge is visible
715  tN = tD;
716  // recompute sc for this edge
717  if ((-d + b) < 0.0)
718  sN = 0;
719  else if ((-d + b) > a)
720  sN = sD;
721  else
722  {
723  sN = (-d + b);
724  sD = a;
725  }
726  }
727  // finally do the division to get sc and tc
728  sc = (fabs(sN) < 0.00000001 ? 0.0 : sN / sD);
729  tc = (fabs(tN) < 0.00000001 ? 0.0 : tN / tD);
730 
731  // get the difference of the two closest points
732  const auto dP = (w + (sc * u) - (tc * v)).eval();
733  return dP.norm(); // return the closest distance
734 }
735 bool TSegment3D::contains(const TPoint3D& point) const
736 {
737  // Not very intuitive, but very fast, method.
738  return abs(math::distance(point1, point) + math::distance(point2, point) -
740 }
741 
743 {
744  if (point1 < s.point1)
745  return true;
746  else if (s.point1 < point1)
747  return false;
748  else
749  return point2 < s.point2;
750 }
751 
752 double TLine2D::evaluatePoint(const TPoint2D& point) const
753 {
754  return coefs[0] * point.x + coefs[1] * point.y + coefs[2];
755 }
756 bool TLine2D::contains(const TPoint2D& point) const
757 {
758  return abs(distance(point)) < getEpsilon();
759 }
760 double TLine2D::distance(const TPoint2D& point) const
761 {
762  return abs(evaluatePoint(point)) /
763  sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
764 }
765 double TLine2D::signedDistance(const TPoint2D& point) const
766 {
767  return evaluatePoint(point) /
768  sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
769 }
770 void TLine2D::getNormalVector(double (&vector)[2]) const
771 {
772  vector[0] = coefs[0];
773  vector[1] = coefs[1];
774 }
776 {
777  double s = sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
778  for (double& coef : coefs) coef /= s;
779 }
780 void TLine2D::getDirectorVector(double (&vector)[2]) const
781 {
782  vector[0] = -coefs[1];
783  vector[1] = coefs[0];
784 }
785 void TLine2D::generate3DObject(TLine3D& l) const { l = TLine3D(*this); }
786 void TLine2D::getAsPose2D(TPose2D& outPose) const
787 {
788  // Line's director vector is (-coefs[1],coefs[0]).
789  // If line is horizontal, force x=0. Else, force y=0. In both cases, we'll
790  // find a suitable point.
791  outPose.phi = atan2(coefs[0], -coefs[1]);
792  if (abs(coefs[0]) < getEpsilon())
793  {
794  outPose.x = 0;
795  outPose.y = -coefs[2] / coefs[1];
796  }
797  else
798  {
799  outPose.x = -coefs[2] / coefs[0];
800  outPose.y = 0;
801  }
802 }
804  const TPoint2D& origin, TPose2D& outPose) const
805 {
806  if (!contains(origin))
807  throw std::logic_error("Base point is not contained in the line");
808  outPose = origin;
809  // Line's director vector is (-coefs[1],coefs[0]).
810  outPose.phi = atan2(coefs[0], -coefs[1]);
811 }
812 TLine2D::TLine2D(const TPoint2D& p1, const TPoint2D& p2)
813 {
814  if (p1 == p2) throw logic_error("Both points are the same");
815  coefs[0] = p2.y - p1.y;
816  coefs[1] = p1.x - p2.x;
817  coefs[2] = p2.x * p1.y - p2.y * p1.x;
818 }
820 {
821  coefs[0] = s.point2.y - s.point1.y;
822  coefs[1] = s.point1.x - s.point2.x;
823  coefs[2] = s.point2.x * s.point1.y - s.point2.y * s.point1.x;
824  // unitarize(); //¿?
825 }
827 {
828  // Line's projection to Z plane may be a point.
829  if (hypot(l.director[0], l.director[1]) < getEpsilon())
830  throw std::logic_error("Line is normal to projection plane");
831  coefs[0] = -l.director[1];
832  coefs[1] = l.director[0];
833  coefs[2] = l.pBase.x * l.director[1] - l.pBase.y * l.director[0];
834 }
835 
836 void TLine3D::generate2DObject(TLine2D& l) const { l = TLine2D(*this); }
837 
838 bool TLine3D::contains(const TPoint3D& point) const
839 {
840  double dx = point.x - pBase.x;
841  double dy = point.y - pBase.y;
842  double dz = point.z - pBase.z;
843  if (abs(dx) < getEpsilon() && abs(dy) < getEpsilon() &&
844  abs(dz) < getEpsilon())
845  return true;
846  // dx dy dz
847  // if -----------=-----------=-----------, point is inside the line.
848  // director[0] director[1] director[2]
849  return (abs(dx * director[1] - dy * director[0]) < getEpsilon()) &&
850  (abs(dx * director[2] - dz * director[0]) < getEpsilon()) &&
851  (abs(dy * director[2] - dz * director[1]) < getEpsilon());
852 }
853 double TLine3D::distance(const TPoint3D& point) const
854 {
855  // Let d be line's base point minus the argument. Then,
856  // d·director/(|d|·|director|) equals both vector's cosine.
857  // So, d·director/|director| equals d's projection over director. Then,
858  // distance is sqrt(|d|²-(d·director/|director|)²).
859  double d[3] = {point.x - pBase.x, point.y - pBase.y, point.z - pBase.z};
860  double dv = 0, d2 = 0, v2 = 0;
861  for (size_t i = 0; i < 3; i++)
862  {
863  dv += d[i] * director[i];
864  d2 += d[i] * d[i];
865  v2 += director[i] * director[i];
866  }
867  return sqrt(d2 - (dv * dv) / v2);
868 }
870 {
871  double s = sqrt(squareNorm<3, double>(director));
872  for (double& i : director) i /= s;
873 }
874 TLine3D::TLine3D(const TPoint3D& p1, const TPoint3D& p2)
875 {
876  if (abs(math::distance(p1, p2)) < getEpsilon())
877  throw logic_error("Both points are the same");
878  pBase = p1;
879  director[0] = p2.x - p1.x;
880  director[1] = p2.y - p1.y;
881  director[2] = p2.z - p1.z;
882 }
884 {
885  pBase = s.point1;
886  director[0] = s.point2.x - s.point1.x;
887  director[1] = s.point2.y - s.point1.y;
888  director[2] = s.point2.z - s.point1.z;
889 }
891 {
892  director[0] = -l.coefs[1];
893  director[1] = l.coefs[0];
894  director[2] = 0;
895  // We assume that either l.coefs[0] or l.coefs[1] is not null. Respectively,
896  // either y or x can be used as free cordinate.
897  if (abs(l.coefs[0]) >= getEpsilon())
898  {
899  pBase.x = -l.coefs[2] / l.coefs[0];
900  pBase.y = 0;
901  }
902  else
903  {
904  pBase.x = 0;
905  pBase.y = -l.coefs[1] / l.coefs[0];
906  }
907  pBase.z = 0;
908 }
909 
910 double TPlane::evaluatePoint(const TPoint3D& point) const
911 {
912  return dotProduct<3, double>(coefs, point) + coefs[3];
913 }
914 bool TPlane::contains(const TPoint3D& point) const
915 {
916  return distance(point) < getEpsilon();
917 }
918 bool TPlane::contains(const TLine3D& line) const
919 {
920  if (!contains(line.pBase)) return false; // Base point must be contained
921  return abs(getAngle(*this, line)) <
922  getEpsilon(); // Plane's normal must be normal to director vector
923 }
924 double TPlane::distance(const TPoint3D& point) const
925 {
926  return abs(evaluatePoint(point)) / sqrt(squareNorm<3, double>(coefs));
927 }
928 double TPlane::distance(const TLine3D& line) const
929 {
930  if (abs(getAngle(*this, line)) >= getEpsilon())
931  return 0; // Plane crosses with line
932  else
933  return distance(line.pBase);
934 }
935 void TPlane::getNormalVector(double (&vector)[3]) const
936 {
937  for (int i = 0; i < 3; i++) vector[i] = coefs[i];
938 }
940 {
941  TVector3D v;
942  for (int i = 0; i < 3; i++) v[i] = coefs[i];
943  return v;
944 }
945 
946 void TPlane::getUnitaryNormalVector(double (&vec)[3]) const
947 {
948  const double s = sqrt(squareNorm<3, double>(coefs));
950  const double k = 1.0 / s;
951  for (int i = 0; i < 3; i++) vec[i] = coefs[i] * k;
952 }
953 
955 {
956  double s = sqrt(squareNorm<3, double>(coefs));
957  for (double& coef : coefs) coef /= s;
958 }
959 
960 // Returns a 6D pose such as its XY plane coincides with the plane
962 {
963  double normal[3];
964  getUnitaryNormalVector(normal);
965  CMatrixDouble44 AXIS;
966  generateAxisBaseFromDirectionAndAxis(normal, 2, AXIS);
967  for (size_t i = 0; i < 3; i++)
968  if (abs(coefs[i]) >= getEpsilon())
969  {
970  AXIS(i, 3) = -coefs[3] / coefs[i];
971  break;
972  }
973  outPose.fromHomogeneousMatrix(AXIS);
974 }
976  const TPoint3D& center, TPose3D& pose) const
977 {
978  if (!contains(center))
979  throw std::logic_error("Base point is not in the plane.");
980  double normal[3];
981  getUnitaryNormalVector(normal);
982  CMatrixDouble44 AXIS;
983  generateAxisBaseFromDirectionAndAxis(normal, 2, AXIS);
984  for (size_t i = 0; i < 3; i++) AXIS(i, 3) = center[i];
985  pose.fromHomogeneousMatrix(AXIS);
986 }
987 TPlane::TPlane(const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3)
988 {
989  double dx1 = p2.x - p1.x;
990  double dy1 = p2.y - p1.y;
991  double dz1 = p2.z - p1.z;
992  double dx2 = p3.x - p1.x;
993  double dy2 = p3.y - p1.y;
994  double dz2 = p3.z - p1.z;
995  coefs[0] = dy1 * dz2 - dy2 * dz1;
996  coefs[1] = dz1 * dx2 - dz2 * dx1;
997  coefs[2] = dx1 * dy2 - dx2 * dy1;
998  if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
999  abs(coefs[2]) < getEpsilon())
1000  throw logic_error("Points are linearly dependent");
1001  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
1002 }
1003 TPlane::TPlane(const TPoint3D& p1, const TLine3D& r2)
1004 {
1005  double dx1 = p1.x - r2.pBase.x;
1006  double dy1 = p1.y - r2.pBase.y;
1007  double dz1 = p1.z - r2.pBase.z;
1008  coefs[0] = dy1 * r2.director[2] - dz1 * r2.director[1];
1009  coefs[1] = dz1 * r2.director[0] - dx1 * r2.director[2];
1010  coefs[2] = dx1 * r2.director[1] - dy1 * r2.director[0];
1011  if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
1012  abs(coefs[2]) < getEpsilon())
1013  throw logic_error("Point is contained in the line");
1014  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
1015 }
1016 TPlane::TPlane(const TPoint3D& p1, const TVector3D& normal)
1017 {
1018  const double normal_norm = normal.norm();
1019  ASSERT_ABOVE_(normal_norm, getEpsilon());
1020 
1021  // Ensure we have a unit vector:
1022  const auto n = normal * (1. / normal_norm);
1023  coefs[0] = n.x;
1024  coefs[1] = n.y;
1025  coefs[2] = n.z;
1026  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
1027 }
1028 TPlane::TPlane(const TLine3D& r1, const TLine3D& r2)
1029 {
1031  coefs[3] =
1032  -coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y - coefs[2] * r1.pBase.z;
1033  if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
1034  abs(coefs[2]) < getEpsilon())
1035  {
1036  // Lines are parallel
1037  if (r1.contains(r2.pBase)) throw std::logic_error("Lines are the same");
1038  // Use a line's director vector and both pBase's difference to create
1039  // the plane.
1040  double d[3];
1041  for (size_t i = 0; i < 3; i++) d[i] = r1.pBase[i] - r2.pBase[i];
1042  crossProduct3D(r1.director, d, coefs);
1043  coefs[3] = -coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y -
1044  coefs[2] * r1.pBase.z;
1045  }
1046  else if (abs(evaluatePoint(r2.pBase)) >= getEpsilon())
1047  throw logic_error("Lines do not intersect");
1048 }
1049 
1050 template <class T>
1051 inline void removeUnusedVertices(T& poly)
1052 {
1053  size_t N = poly.size();
1054  if (N < 3) return;
1055  std::vector<size_t> unused;
1056  if (abs(mrpt::math::distance(poly[N - 1], poly[0]) +
1057  mrpt::math::distance(poly[0], poly[1]) -
1058  mrpt::math::distance(poly[N - 1], poly[1])) <
1060  unused.push_back(0);
1061  for (size_t i = 1; i < N - 1; i++)
1062  if (abs(mrpt::math::distance(poly[i - 1], poly[i]) +
1063  mrpt::math::distance(poly[i], poly[i + 1]) -
1064  mrpt::math::distance(poly[i - 1], poly[i + 1])) <
1066  unused.push_back(i);
1067  if (abs(mrpt::math::distance(poly[N - 2], poly[N - 1]) +
1068  mrpt::math::distance(poly[N - 1], poly[0]) -
1069  mrpt::math::distance(poly[N - 2], poly[0])) <
1071  unused.push_back(N - 1);
1072  unused.push_back(N);
1073  size_t diff = 1;
1074  for (size_t i = 0; i < unused.size() - 1; i++)
1075  {
1076  size_t last = unused[i + 1];
1077  for (size_t j = unused[i] + 1 - diff; j < last - diff; j++)
1078  poly[j] = poly[j + diff];
1079  }
1080  poly.resize(N + 1 - unused.size());
1081 }
1082 template <class T>
1083 inline void removeRepVertices(T& poly)
1084 {
1085  size_t N = poly.size();
1086  if (N < 3) return;
1087  std::vector<size_t> rep;
1088  for (size_t i = 0; i < N - 1; i++)
1089  if (mrpt::math::distance(poly[i], poly[i + 1]) < getEpsilon())
1090  rep.push_back(i);
1091  if (mrpt::math::distance(poly[N - 1], poly[0]) < getEpsilon())
1092  rep.push_back(N - 1);
1093  rep.push_back(N);
1094  size_t diff = 1;
1095  for (size_t i = 0; i < rep.size() - 1; i++)
1096  {
1097  size_t last = rep[i + 1];
1098  for (size_t j = rep[i] + 1 - diff; j < last - diff; j++)
1099  poly[j] = poly[j + diff];
1100  }
1101  poly.resize(N + 1 - rep.size());
1102 }
1103 
1104 double TPolygon2D::distance(const TPoint2D& point) const
1105 {
1106  if (contains(point)) return 0;
1107  std::vector<TSegment2D> sgs;
1108  getAsSegmentList(sgs);
1109 
1110  if (sgs.empty())
1111  THROW_EXCEPTION("Cannot compute distance to an empty polygon.");
1112 
1113  double distance = std::numeric_limits<double>::max();
1114 
1115  for (auto it = sgs.begin(); it != sgs.end(); ++it)
1116  {
1117  double d = (*it).distance(point);
1118  if (d < distance) distance = d;
1119  }
1120  return distance;
1121 }
1122 
1124  TPoint2D& min_coords, TPoint2D& max_coords) const
1125 {
1126  ASSERTMSG_(!this->empty(), "getBoundingBox() called on an empty polygon!");
1127  min_coords.x = min_coords.y = std::numeric_limits<double>::max();
1128  max_coords.x = max_coords.y = -std::numeric_limits<double>::max();
1129  for (size_t i = 0; i < size(); i++)
1130  {
1131  mrpt::keep_min(min_coords.x, (*this)[i].x);
1132  mrpt::keep_min(min_coords.y, (*this)[i].y);
1133  mrpt::keep_max(max_coords.x, (*this)[i].x);
1134  mrpt::keep_max(max_coords.y, (*this)[i].y);
1135  }
1136 }
1137 
1138 // isLeft(): tests if a point is Left|On|Right of an infinite line.
1139 // Input: three points P0, P1, and P2
1140 // Return: >0 for P2 left of the line through P0 and P1
1141 // =0 for P2 on the line
1142 // <0 for P2 right of the line
1143 // See: Algorithm 1 "Area of Triangles and Polygons"
1144 inline double isLeft(
1145  const mrpt::math::TPoint2D& P0, const mrpt::math::TPoint2D& P1,
1146  const mrpt::math::TPoint2D& P2)
1147 {
1148  return ((P1.x - P0.x) * (P2.y - P0.y) - (P2.x - P0.x) * (P1.y - P0.y));
1149 }
1150 
1151 bool TPolygon2D::contains(const TPoint2D& P) const
1152 {
1153  int wn = 0; // the winding number counter
1154 
1155  // loop through all edges of the polygon
1156  const size_t n = this->size();
1157  for (size_t i = 0; i < n; i++) // edge from V[i] to V[i+1]
1158  {
1159  if ((*this)[i].y <= P.y)
1160  {
1161  // start y <= P.y
1162  if ((*this)[(i + 1) % n].y > P.y) // an upward crossing
1163  if (isLeft((*this)[i], (*this)[(i + 1) % n], P) >
1164  0) // P left of edge
1165  ++wn; // have a valid up intersect
1166  }
1167  else
1168  {
1169  // start y > P.y (no test needed)
1170  if ((*this)[(i + 1) % n].y <= P.y) // a downward crossing
1171  if (isLeft((*this)[i], (*this)[(i + 1) % n], P) <
1172  0) // P right of edge
1173  --wn; // have a valid down intersect
1174  }
1175  }
1176 
1177  return wn != 0;
1178 }
1179 void TPolygon2D::getAsSegmentList(vector<TSegment2D>& v) const
1180 {
1181  size_t N = size();
1182  v.resize(N);
1183  for (size_t i = 0; i < N - 1; i++)
1184  v[i] = TSegment2D(operator[](i), operator[](i + 1));
1185  v[N - 1] = TSegment2D(operator[](N - 1), operator[](0));
1186 }
1187 
1189 {
1190  p = TPolygon3D(*this);
1191 }
1192 // Auxiliar functor class to compute polygon's center
1193 template <class T, int N>
1195 {
1196  public:
1198  FAddPoint(T& o) : object(o)
1199  {
1200  for (size_t i = 0; i < N; i++) object[i] = 0.0;
1201  }
1202  void operator()(const T& o)
1203  {
1204  for (size_t i = 0; i < N; i++) object[i] += o[i];
1205  }
1206 };
1208 {
1209  for_each(begin(), end(), FAddPoint<TPoint2D, 2>(p));
1210  size_t N = size();
1211  p.x /= N;
1212  p.y /= N;
1213 }
1215 {
1216  size_t N = size();
1217  if (N <= 3) return true;
1218  vector<TSegment2D> sgms;
1219  getAsSegmentList(sgms);
1220  for (size_t i = 0; i < N; i++)
1221  {
1222  char s = 0;
1223  auto l = TLine2D(sgms[i]);
1224  for (size_t j = 0; j < N; j++)
1225  {
1226  double d = l.evaluatePoint(operator[](j));
1227  if (abs(d) < getEpsilon())
1228  continue;
1229  else if (!s)
1230  s = (d > 0) ? 1 : -1;
1231  else if (s != ((d > 0) ? 1 : -1))
1232  return false;
1233  }
1234  }
1235  return true;
1236 }
1239 {
1241  removeUnusedVertices(*this);
1242 }
1244  std::vector<double>& x, std::vector<double>& y) const
1245 {
1246  size_t N = size();
1247  x.resize(N + 1);
1248  y.resize(N + 1);
1249  for (size_t i = 0; i < N; i++)
1250  {
1251  x[i] = operator[](i).x;
1252  y[i] = operator[](i).y;
1253  }
1254  x[N] = operator[](0).x;
1255  y[N] = operator[](0).y;
1256 }
1258 {
1259  size_t N = p.size();
1260  resize(N);
1261  for (size_t i = 0; i < N; i++) operator[](i) = TPoint2D(p[i]);
1262 }
1264  size_t numEdges, double radius, TPolygon2D& poly)
1265 {
1266  if (numEdges < 3 || abs(radius) < getEpsilon())
1267  throw std::logic_error(
1268  "Invalid arguments for regular polygon creations");
1269  poly.resize(numEdges);
1270  for (size_t i = 0; i < numEdges; i++)
1271  {
1272  double angle = i * M_PI * 2 / numEdges;
1273  poly[i] = TPoint2D(radius * cos(angle), radius * sin(angle));
1274  }
1275 }
1277  size_t numEdges, double radius, TPolygon2D& poly, const TPose2D& pose)
1278 {
1279  createRegularPolygon(numEdges, radius, poly);
1280  for (size_t i = 0; i < numEdges; i++) poly[i] = pose.composePoint(poly[i]);
1281 }
1282 
1284 {
1285  p = TPolygon2D(*this);
1286 }
1287 
1288 double TPolygon3D::distance(const TPoint3D& point) const
1289 {
1290  TPlane pl;
1291  if (!getPlane(pl))
1292  throw std::logic_error("Polygon does not conform a plane");
1293  TPoint3D newPoint;
1294  TPolygon3D newPoly;
1295  TPose3D pose;
1296  pl.getAsPose3DForcingOrigin(operator[](0), pose);
1297  project3D(point, pose, newPoint);
1298  project3D(*this, pose, newPoly);
1299  double distance2D = TPolygon2D(newPoly).distance(TPoint2D(newPoint));
1300  return sqrt(newPoint.z * newPoint.z + distance2D * distance2D);
1301 }
1302 bool TPolygon3D::contains(const TPoint3D& point) const
1303 {
1304  TPoint3D pMin, pMax;
1305  getPrismBounds(*this, pMin, pMax);
1306  if (point.x + getEpsilon() < pMin.x || point.y + getEpsilon() < pMin.y ||
1307  point.z + getEpsilon() < pMin.z || point.x > pMax.x + getEpsilon() ||
1308  point.y > pMax.y + getEpsilon() || point.z > pMax.z + getEpsilon())
1309  return false;
1310  TPlane plane;
1311  if (!getPlane(plane))
1312  throw std::logic_error("Polygon does not conform a plane");
1313  TPolygon3D projectedPoly;
1314  TPoint3D projectedPoint;
1315  TPose3D pose;
1316  // plane.getAsPose3DForcingOrigin(operator[](0),pose);
1317  plane.getAsPose3D(pose);
1318  CMatrixDouble44 P_inv;
1319  pose.getInverseHomogeneousMatrix(P_inv);
1320  pose.fromHomogeneousMatrix(P_inv);
1321  project3D(point, pose, projectedPoint);
1322  if (abs(projectedPoint.z) >= getEpsilon())
1323  return false; // Point is not inside the polygon's plane.
1324  project3D(*this, pose, projectedPoly);
1325  return TPolygon2D(projectedPoly).contains(TPoint2D(projectedPoint));
1326 }
1327 void TPolygon3D::getAsSegmentList(vector<TSegment3D>& v) const
1328 {
1329  size_t N = size();
1330  v.resize(N);
1331  for (size_t i = 0; i < N - 1; i++)
1332  v[i] = TSegment3D(operator[](i), operator[](i + 1));
1333  v[N - 1] = TSegment3D(operator[](N - 1), operator[](0));
1334 }
1335 bool TPolygon3D::getPlane(TPlane& p) const { return conformAPlane(*this, p); }
1337 {
1338  getRegressionPlane(*this, p);
1339 }
1341 {
1342  for_each(begin(), end(), FAddPoint<TPoint3D, 3>(p));
1343  size_t N = size();
1344  p.x /= N;
1345  p.y /= N;
1346  p.z /= N;
1347 }
1348 bool TPolygon3D::isSkew() const { return !mrpt::math::conformAPlane(*this); }
1351 {
1353  removeUnusedVertices(*this);
1354 }
1356 {
1357  size_t N = p.size();
1358  resize(N);
1359  for (size_t i = 0; i < N; i++) operator[](i) = p[i];
1360 }
1362  size_t numEdges, double radius, TPolygon3D& poly)
1363 {
1364  if (numEdges < 3 || abs(radius) < getEpsilon())
1365  throw std::logic_error(
1366  "Invalid arguments for regular polygon creations");
1367  poly.resize(numEdges);
1368  for (size_t i = 0; i < numEdges; i++)
1369  {
1370  double angle = i * 2 * M_PI / numEdges;
1371  poly[i] = TPoint3D(radius * cos(angle), radius * sin(angle), 0);
1372  }
1373 }
1375  size_t numEdges, double radius, TPolygon3D& poly, const TPose3D& pose)
1376 {
1377  createRegularPolygon(numEdges, radius, poly);
1378  for (size_t i = 0; i < numEdges; i++) pose.composePoint(poly[i], poly[i]);
1379 }
1380 
1382 {
1383  switch (type)
1384  {
1385  case GEOMETRIC_TYPE_POINT:
1386  obj = TPoint3D(data.point);
1387  break;
1389  obj = TSegment3D(data.segment);
1390  break;
1391  case GEOMETRIC_TYPE_LINE:
1392  obj = TLine3D(data.line);
1393  break;
1395  obj = TPolygon3D(*(data.polygon));
1396  break;
1397  default:
1398  obj = TObject3D();
1399  break;
1400  }
1401 }
1403  const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts)
1404 {
1405  for (const auto& obj : objs)
1406  if (obj.isPoint()) pnts.push_back(obj.data.point);
1407 }
1409  const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms)
1410 {
1411  for (const auto& obj : objs)
1412  if (obj.isSegment()) sgms.push_back(obj.data.segment);
1413 }
1415  const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins)
1416 {
1417  for (const auto& obj : objs)
1418  if (obj.isLine()) lins.push_back(obj.data.line);
1419 }
1421  const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys)
1422 {
1423  for (const auto& obj : objs)
1424  if (obj.isPolygon()) polys.push_back(*(obj.data.polygon));
1425 }
1427  const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts,
1428  std::vector<TObject2D>& remainder)
1429 {
1430  for (const auto& obj : objs)
1431  if (obj.isPoint())
1432  pnts.push_back(obj.data.point);
1433  else
1434  remainder.push_back(obj);
1435 }
1437  const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms,
1438  std::vector<TObject2D>& remainder)
1439 {
1440  for (const auto& obj : objs)
1441  if (obj.isSegment())
1442  sgms.push_back(obj.data.segment);
1443  else
1444  remainder.push_back(obj);
1445 }
1447  const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins,
1448  std::vector<TObject2D>& remainder)
1449 {
1450  for (const auto& obj : objs)
1451  if (obj.isLine())
1452  lins.push_back(obj.data.line);
1453  else
1454  remainder.push_back(obj);
1455 }
1457  const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys,
1458  vector<TObject2D>& remainder)
1459 {
1460  for (const auto& obj : objs)
1461  if (obj.isPolygon())
1462  polys.push_back(*(obj.data.polygon));
1463  else
1464  remainder.push_back(obj);
1465 }
1466 
1468 {
1469  switch (type)
1470  {
1471  case GEOMETRIC_TYPE_POINT:
1472  obj = TPoint2D(data.point);
1473  break;
1475  obj = TSegment2D(data.segment);
1476  break;
1477  case GEOMETRIC_TYPE_LINE:
1478  obj = TLine2D(data.line);
1479  break;
1481  obj = TPolygon2D(*(data.polygon));
1482  break;
1483  case GEOMETRIC_TYPE_PLANE:
1484  throw std::logic_error("Too many dimensions");
1485  default:
1486  obj = TObject2D();
1487  break;
1488  }
1489 }
1490 
1492  const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts)
1493 {
1494  for (const auto& obj : objs)
1495  if (obj.isPoint()) pnts.push_back(obj.data.point);
1496 }
1498  const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms)
1499 {
1500  for (const auto& obj : objs)
1501  if (obj.isSegment()) sgms.push_back(obj.data.segment);
1502 }
1504  const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins)
1505 {
1506  for (const auto& obj : objs)
1507  if (obj.isLine()) lins.push_back(obj.data.line);
1508 }
1510  const std::vector<TObject3D>& objs, std::vector<TPlane>& plns)
1511 {
1512  for (const auto& obj : objs)
1513  if (obj.isPlane()) plns.push_back(obj.data.plane);
1514 }
1516  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
1517 {
1518  for (const auto& obj : objs)
1519  if (obj.isPolygon()) polys.push_back(*(obj.data.polygon));
1520 }
1522  const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts,
1523  std::vector<TObject3D>& remainder)
1524 {
1525  for (const auto& obj : objs)
1526  if (obj.isPoint())
1527  pnts.push_back(obj.data.point);
1528  else
1529  remainder.push_back(obj);
1530 }
1532  const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms,
1533  std::vector<TObject3D>& remainder)
1534 {
1535  for (const auto& obj : objs)
1536  if (obj.isSegment())
1537  sgms.push_back(obj.data.segment);
1538  else
1539  remainder.push_back(obj);
1540 }
1542  const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins,
1543  std::vector<TObject3D>& remainder)
1544 {
1545  for (const auto& obj : objs)
1546  if (obj.isLine())
1547  lins.push_back(obj.data.line);
1548  else
1549  remainder.push_back(obj);
1550 }
1552  const std::vector<TObject3D>& objs, std::vector<TPlane>& plns,
1553  std::vector<TObject3D>& remainder)
1554 {
1555  for (const auto& obj : objs)
1556  if (obj.isPlane())
1557  plns.push_back(obj.data.plane);
1558  else
1559  remainder.push_back(obj);
1560 }
1562  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
1563  vector<TObject3D>& remainder)
1564 {
1565  for (const auto& obj : objs)
1566  if (obj.isPolygon())
1567  polys.push_back(*(obj.data.polygon));
1568  else
1569  remainder.push_back(obj);
1570 }
1571 
1573 {
1574  for (unsigned int i = 0; i < o.size(); i++) in >> o[i];
1575  return in;
1576 }
1578 {
1579  for (unsigned int i = 0; i < o.size(); i++) out << o[i];
1580  return out;
1581 }
1582 
1584 {
1585  for (unsigned int i = 0; i < o.size(); i++) in >> o[i];
1586  return in;
1587 }
1589 {
1590  for (unsigned int i = 0; i < o.size(); i++) out << o[i];
1591  return out;
1592 }
1593 
1595 {
1596  uint16_t type;
1597  in >> type;
1598  switch (static_cast<unsigned char>(type))
1599  {
1600  case GEOMETRIC_TYPE_POINT:
1601  {
1602  TPoint2D p;
1603  in >> p;
1604  o = p;
1605  }
1606  break;
1608  {
1609  TSegment2D s;
1610  in >> s;
1611  o = s;
1612  }
1613  break;
1614  case GEOMETRIC_TYPE_LINE:
1615  {
1616  TLine2D l;
1617  in >> l;
1618  o = l;
1619  }
1620  break;
1622  {
1623  TPolygon2D p;
1624  in >> p;
1625  o = p;
1626  }
1627  break;
1629  {
1630  o = TObject2D();
1631  }
1632  break;
1633  default:
1634  throw std::logic_error(
1635  "Unknown TObject2D type found while reading stream");
1636  }
1637  return in;
1638 }
1640 {
1641  out << static_cast<uint16_t>(o.getType());
1642  switch (o.getType())
1643  {
1644  case GEOMETRIC_TYPE_POINT:
1645  {
1646  TPoint2D p;
1647  o.getPoint(p);
1648  return out << p;
1649  };
1651  {
1652  TSegment2D s;
1653  o.getSegment(s);
1654  return out << s;
1655  };
1656  case GEOMETRIC_TYPE_LINE:
1657  {
1658  TLine2D l;
1659  o.getLine(l);
1660  return out << l;
1661  };
1663  {
1664  TPolygon2D p;
1665  o.getPolygon(p);
1666  return out << p;
1667  };
1668  }
1669  return out;
1670 }
1671 
1673 {
1674  uint16_t type;
1675  in >> type;
1676  switch (static_cast<unsigned char>(type))
1677  {
1678  case GEOMETRIC_TYPE_POINT:
1679  {
1680  TPoint3D p;
1681  in >> p;
1682  o = p;
1683  }
1684  break;
1686  {
1687  TSegment3D s;
1688  in >> s;
1689  o = s;
1690  }
1691  break;
1692  case GEOMETRIC_TYPE_LINE:
1693  {
1694  TLine3D l;
1695  in >> l;
1696  o = l;
1697  }
1698  break;
1699  case GEOMETRIC_TYPE_PLANE:
1700  {
1701  TPlane p;
1702  in >> p;
1703  o = p;
1704  }
1705  break;
1707  {
1708  TPolygon3D p;
1709  in >> p;
1710  o = p;
1711  }
1712  break;
1714  {
1715  o = TObject3D();
1716  }
1717  break;
1718  default:
1719  throw std::logic_error(
1720  "Unknown TObject3D type found while reading stream");
1721  }
1722  return in;
1723 }
1725 {
1726  out << static_cast<uint16_t>(o.getType());
1727  switch (o.getType())
1728  {
1729  case GEOMETRIC_TYPE_POINT:
1730  {
1731  TPoint3D p;
1732  o.getPoint(p);
1733  return out << p;
1734  };
1736  {
1737  TSegment3D s;
1738  o.getSegment(s);
1739  return out << s;
1740  };
1741  case GEOMETRIC_TYPE_LINE:
1742  {
1743  TLine3D l;
1744  o.getLine(l);
1745  return out << l;
1746  };
1747  case GEOMETRIC_TYPE_PLANE:
1748  {
1749  TPlane p;
1750  o.getPlane(p);
1751  return out << p;
1752  };
1754  {
1755  TPolygon3D p;
1756  o.getPolygon(p);
1757  return out << p;
1758  };
1759  }
1760  return out;
1761 }
1762 } // namespace mrpt::math
void project3D(const TPoint3D &point, const mrpt::math::TPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
Definition: geometry.h:329
std::vector< T1 > operator-(const std::vector< T1 > &v1, const std::vector< T2 > &v2)
Definition: ops_vectors.h:92
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
Definition: MatrixBase.h:233
static void getSegments(const std::vector< TObject2D > &objs, std::vector< TSegment2D > &sgms)
Static method to retrieve all the segments in a vector of TObject2D.
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
Definition: TObject2D.h:109
void generateAxisBaseFromDirectionAndAxis(const double(&vec)[3], uint8_t coord, CMatrixDouble44 &matrix)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
Definition: geometry.cpp:2078
static void createRegularPolygon(size_t numEdges, double radius, TPolygon3D &poly)
Static method to create a regular polygon, given its size and radius.
static void createRegularPolygon(size_t numEdges, double radius, TPolygon2D &poly)
Static method to create a regular polygon, given its size and radius.
void inverseComposePoint(const TPoint3D &g, TPoint3D &l) const
double x
X,Y coordinates.
Definition: TPoint2D.h:23
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
Definition: TObject2D.h:150
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
GLdouble GLdouble t
Definition: glext.h:3695
GLdouble GLdouble z
Definition: glext.h:3879
#define min(a, b)
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
static void SO3_to_yaw_pitch_roll(const mrpt::math::CMatrixDouble33 &R, double &yaw, double &pitch, double &roll)
unsigned __int16 uint16_t
Definition: rptypes.h:47
double RAD2DEG(const double x)
Radians to degrees.
unsigned char getType() const
Gets content type.
Definition: TObject2D.h:105
bool operator==(const TTwist3D &o) const
double x
X,Y coordinates.
Definition: TPose2D.h:30
void getRotationMatrix(mrpt::math::CMatrixDouble33 &R) const
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double distance(const TPoint3D &point) const
Distance between the line and a point.
static constexpr unsigned char GEOMETRIC_TYPE_POLYGON
Object type identifier for TPolygon2D or TPolygon3D.
Definition: TPoseOrPoint.h:116
void getPlotData(std::vector< double > &x, std::vector< double > &y) const
Gets plot data, ready to use on a 2D plot.
size_t size(const MATRIXLIKE &m, const int dim)
bool isSkew() const
Check whether the polygon is skew.
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
Definition: TObject3D.h:134
bool contains(const TPoint2D &point) const
Check whether a point is inside a segment.
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:37
static void getPolygons(const std::vector< TObject2D > &objs, std::vector< TPolygon2D > &polys)
Static method to retrieve all the polygons in a vector of TObject2D.
double DEG2RAD(const double x)
Degrees to radians.
mrpt::math::TPoint2D composePoint(const TPoint2D l) const
TPolygon3D()
Default constructor.
Definition: TPolygon3D.h:46
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
GLenum GLsizei n
Definition: glext.h:5136
double distance(const TPoint2D &point) const
Distance to point.
static void getLines(const std::vector< TObject2D > &objs, std::vector< TLine2D > &lins)
Static method to retrieve all the lines in a vector of TObject2D.
This file implements several operations that operate element-wise on individual or pairs of container...
mrpt::math::CMatrixDouble33 getRotationMatrix() const
Definition: TPose3D.h:167
double x
X,Y,Z, coords.
Definition: TPose3D.h:31
double norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:177
double signedDistance(const TPoint2D &point) const
Distance with sign from a given point (sign indicates side).
Standard type for storing any lightweight 2D type.
Definition: TObject2D.h:24
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
Definition: TObject2D.h:136
void getAsSegmentList(std::vector< TSegment2D > &v) const
Gets as set of segments, instead of points.
static constexpr unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
Definition: TPoseOrPoint.h:101
bool getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
Definition: TObject3D.h:175
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
Standard object for storing any 3D lightweight object.
Definition: TObject3D.h:25
STL namespace.
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
Definition: TObject2D.h:123
bool contains(const TPoint2D &point) const
Check whether a point is inside the line.
void removeRepeatedVertices()
Erase repeated vertices.
double signedDistance(const TPoint2D &point) const
Distance with sign to point (sign indicates which side the point is).
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
void generate3DObject(TLine3D &l) const
Project into 3D space, setting the z to 0.
void removeRepVertices(T &poly)
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -45...
void getAsPose2D(TPose2D &outPose) const
double distance(const TPoint3D &point) const
Distance to 3D point.
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:33
GLdouble s
Definition: glext.h:3682
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[vx vy vz wx wy wz]" ) ...
mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const
Definition: TPose3D.h:174
bool operator<(const TSegment3D &s) const
VALUE & operator[](const KEY &key)
Write/read via [i] operator, that creates an element if it didn&#39;t exist already.
Definition: ts_hash_map.h:193
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
Definition: TTwist3D.h:18
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
Definition: geometry.h:804
TPoint3D point1
Origin point.
Definition: TSegment3D.h:27
bool conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
Definition: geometry.cpp:989
static void getSegments(const std::vector< TObject3D > &objs, std::vector< TSegment3D > &sgms)
Static method to retrieve every segment included in a vector of objects.
void asVector(VECTORLIKE &v) const
Transformation into vector.
Definition: TPoint3D.h:191
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
T square(const T x)
Inline function for the square of a number.
void getNormalVector(double(&vector)[2]) const
Get line&#39;s normal vector.
void generate3DObject(TPolygon3D &p) const
Projects into 3D space, zeroing the z.
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
double isLeft(const mrpt::math::TPoint2D &P0, const mrpt::math::TPoint2D &P1, const mrpt::math::TPoint2D &P2)
static constexpr unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
Definition: TPoseOrPoint.h:121
void loadFromArray(const VECTOR &vals)
Definition: CMatrixFixed.h:171
void unitarize()
Unitarize line&#39;s normal vector.
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:352
std::string asString() const
Definition: TTwist2D.h:97
void getBoundingBox(TPoint2D &min_coords, TPoint2D &max_coords) const
Get polygon bounding box.
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
Definition: TObject3D.h:161
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
Definition: TSegment2D.h:20
double distance(const TPoint3D &point) const
Distance to point.
void getUnitaryNormalVector(double(&vec)[3]) const
Get normal vector.
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -45...
const GLfloat * tc
Definition: glext.h:6459
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
3D segment, consisting of two points.
Definition: TSegment3D.h:21
TSegment2D()=default
Fast default constructor.
double length() const
Segment length.
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44 &HG)
TPoint3D point2
Destiny point.
Definition: TSegment3D.h:31
bool operator<(const TSegment2D &s) const
const GLubyte * c
Definition: glext.h:6406
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
mrpt::math::TPose2D operator+(const mrpt::math::TPose2D &b) const
Operator "oplus" pose composition: "ret=this \oplus b".
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
constexpr TPose3D()=default
Default fast constructor.
constexpr TPose2D()=default
Default fast constructor.
static void getLines(const std::vector< TObject3D > &objs, std::vector< TLine3D > &lins)
Static method to retrieve every line included in a vector of objects.
void unitarize()
Unitarize normal vector.
mrpt::math::TPose2D operator*(const double dt) const
Returns the pose increment of multiplying each twist component times "dt" seconds.
GLuint GLuint end
Definition: glext.h:3532
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::math::CMatrixFixed< double, 4, 3 > *out_dq_dr=nullptr) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
void generate2DObject(TPolygon2D &p) const
Projects into a 2D space, discarding the z.
static void getPolygons(const std::vector< TObject3D > &objs, std::vector< TPolygon3D > &polys)
Static method to retrieve every polygon included in a vector of objects.
GLubyte g
Definition: glext.h:6372
3D Plane, represented by its equation
Definition: TPlane.h:22
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon.
GLubyte GLubyte b
Definition: glext.h:6372
double getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
Definition: geometry.cpp:2148
TPolygon2D()
Default constructor.
Definition: TPolygon2D.h:48
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
void removeRedundantVertices()
Erase every redundant vertex, thus saving space.
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
TPoint2D point2
Destiny point.
Definition: TSegment2D.h:30
void getCenter(TPoint2D &p) const
Polygon&#39;s central point.
void rotate(const mrpt::math::TPose3D &rot)
Transform all 6 components for a change of reference frame from "A" to another frame "B" whose rotati...
double getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:866
GLsizei const GLchar ** string
Definition: glext.h:4116
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
TVector3D getNormalVector() const
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
void unitarize()
Unitarize director vector.
static constexpr unsigned char GEOMETRIC_TYPE_UNDEFINED
Object type identifier for empty TObject2D or TObject3D.
Definition: TPoseOrPoint.h:126
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
bool contains(const TPoint3D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
Definition: TObject3D.h:120
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:35
bool empty() const
Definition: ts_hash_map.h:190
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
Definition: TPose3DQuat.h:19
double distance(const TPoint3D &point) const
Distance to point (always >=0)
TPoint2D point1
Origin point.
Definition: TSegment2D.h:26
bool isConvex() const
Checks whether is convex.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
void generate3DObject(TSegment3D &s) const
Project into 3D space, setting the z to 0.
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
void removeUnusedVertices(T &poly)
double distance(const TPoint2D &point) const
Distance to a point (always >=0)
constexpr TPoint3D()
Default constructor.
Definition: TPoint3D.h:100
const GLdouble * v
Definition: glext.h:3684
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:23
void getAsPose2DForcingOrigin(const TPoint2D &origin, TPose2D &outPose) const
constexpr std::size_t size() const
Definition: TPoseOrPoint.h:58
const_iterator begin() const
Definition: ts_hash_map.h:229
double wx
Angular velocity (rad/s)
Definition: TTwist3D.h:28
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
double vx
Velocity components: X,Y (m/s)
Definition: TTwist3D.h:26
void generate3DObject(TObject3D &obj) const
Project into 3D space.
void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...
const float R
static void getPoints(const std::vector< TObject2D > &objs, std::vector< TPoint2D > &pnts)
Static method to retrieve all the points in a vector of TObject2D.
void getPrismBounds(const std::vector< TPoint3D > &poly, TPoint3D &pMin, TPoint3D &pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
Definition: geometry.cpp:2019
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:630
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -0...
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
Definition: TPose3D.h:181
bool operator!=(const TTwist3D &o) const
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
void getAsPose3D(mrpt::math::TPose3D &outPose) const
GLuint in
Definition: glext.h:7391
Lightweight 2D pose.
Definition: TPose2D.h:22
void removeRedundantVertices()
Erase every redundant vertex from the polygon, saving space.
void removeRepeatedVertices()
Remove polygon&#39;s repeated vertices.
bool contains(const TPoint3D &point) const
Check whether a point is inside the segment.
bool getLine(TLine3D &r) const
Gets the content as a line, returning false if the type is not adequate.
Definition: TObject3D.h:147
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane&#39;s equation.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
std::string asString() const
Definition: TPose2D.h:108
void generate2DObject(TSegment2D &s) const
Projection into 2D space, discarding the z.
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -0...
void generate2DObject(TObject2D &obj) const
Projects into 2D space.
TLine3D()=default
Fast default constructor.
bool operator<(const TPoint3D &p) const
double evaluatePoint(const TPoint2D &point) const
Evaluate point in the line&#39;s equation.
GLenum GLint GLint y
Definition: glext.h:3542
GLfloat GLfloat GLfloat v2
Definition: glext.h:4123
unsigned char getType() const
Gets object type.
Definition: TObject3D.h:115
mrpt::math::TPose2D operator-(const mrpt::math::TPose2D &b) const
Operator "ominus" pose composition: "ret=this \ominus b".
GLuint res
Definition: glext.h:7385
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
Definition: TPoseOrPoint.h:106
GLenum GLint x
Definition: glext.h:3542
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
Lightweight 3D point.
Definition: TPoint3D.h:90
double distance(const TPoint2D &point) const
Distance from a given point.
void getAsSegmentList(std::vector< TSegment3D > &v) const
Gets as set of segments, instead of set of points.
void getBestFittingPlane(TPlane &p) const
Gets the best fitting plane, disregarding whether the polygon actually fits inside or not...
void getDirectorVector(double(&vector)[2]) const
Get line&#39;s director vector.
bool operator<(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2)
std::string asString() const
Definition: TPose3D.h:135
CMatrixFixed< double, 4, 4 > CMatrixDouble44
Definition: CMatrixFixed.h:353
Lightweight 2D point.
Definition: TPoint2D.h:31
TLine2D()=default
Fast default constructor.
TPlane()=default
Fast default constructor.
std::array< double, 4 > coefs
Plane coefficients, stored as an array: .
Definition: TPlane.h:26
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
std::string asString() const
Definition: TTwist3D.h:135
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
static void getPlanes(const std::vector< TObject3D > &objs, std::vector< TPlane > &plns)
Static method to retrieve every plane included in a vector of objects.
double phi
Orientation (rads)
Definition: TPose2D.h:32
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3532
static constexpr unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
Definition: TPoseOrPoint.h:111
void operator()(const T &o)
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:18
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1889
void getCenter(TPoint3D &p) const
Get polygon&#39;s central point.
double omega
Angular velocity (rad/s)
Definition: TTwist2D.h:28
bool contains(const TPoint3D &point) const
Check whether a point is contained into the plane.
double length() const
Segment length.
void composePoint(const TPoint3D &l, TPoint3D &g) const
bool operator!=(const TTwist2D &o) const
void getAsPose3DForcingOrigin(const TPoint3D &center, TPose3D &pose) const
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -0...
bool operator==(const TTwist2D &o) const
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
void composePose(const TPose3D other, TPose3D &result) const
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19
static void getPoints(const std::vector< TObject3D > &objs, std::vector< TPoint3D > &pnts)
Static method to retrieve every point included in a vector of objects.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: a429c6f2f Mon Aug 19 15:45:44 2019 +0200 at lun ago 19 15:50:16 CEST 2019