Main MRPT website > C++ reference for MRPT 1.9.9
lightweight_geom_data.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 |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/core/exceptions.h>
14 #include <mrpt/math/CQuaternion.h>
16 #include <mrpt/math/geometry.h> // distance()
20 #include <mrpt/serialization/CArchive.h> // impl of << operator
21 
22 using namespace std; // For min/max, etc...
23 using namespace mrpt::serialization; // CArchive, << operator for STL
24 
25 using mrpt::DEG2RAD;
26 using mrpt::RAD2DEG;
27 
28 namespace mrpt
29 {
30 namespace math
31 {
32 TPoint2D::TPoint2D(const TPose2D& p) : x(p.x), y(p.y) {}
33 TPoint2D::TPoint2D(const TPoint3D& p) : x(p.x), y(p.y) {}
34 TPoint2D::TPoint2D(const TPose3D& p) : x(p.x), y(p.y) {}
35 bool TPoint2D::operator<(const TPoint2D& p) const
36 {
37  if (x < p.x)
38  return true;
39  else if (x > p.x)
40  return false;
41  else
42  return y < p.y;
43 }
45 {
46  CMatrixDouble m;
47  if (!m.fromMatlabStringFormat(s))
48  THROW_EXCEPTION("Malformed expression in ::fromString");
49  ASSERTMSG_(
50  m.rows() == 1 && m.cols() == 2, "Wrong size of vector in ::fromString");
51  x = m.get_unsafe(0, 0);
52  y = m.get_unsafe(0, 1);
53 }
54 
55 TPose2D::TPose2D(const TPoint2D& p) : x(p.x), y(p.y), phi(0.0) {}
56 TPose2D::TPose2D(const TPoint3D& p) : x(p.x), y(p.y), phi(0.0) {}
57 TPose2D::TPose2D(const TPose3D& p) : x(p.x), y(p.y), phi(p.yaw) {}
59 {
60  s = mrpt::format("[%f %f %f]", x, y, RAD2DEG(phi));
61 }
63 {
64  CMatrixDouble m;
65  if (!m.fromMatlabStringFormat(s))
66  THROW_EXCEPTION("Malformed expression in ::fromString");
67  ASSERTMSG_(
68  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
69  x = m.get_unsafe(0, 0);
70  y = m.get_unsafe(0, 1);
71  phi = DEG2RAD(m.get_unsafe(0, 2));
72 }
74  const mrpt::math::TPose2D& b) const
75 {
76  const double A_cosphi = cos(this->phi), A_sinphi = sin(this->phi);
77  // Use temporary variables for the cases (A==this) or (B==this)
78  const double new_x = this->x + b.x * A_cosphi - b.y * A_sinphi;
79  const double new_y = this->y + b.x * A_sinphi + b.y * A_cosphi;
80  const double new_phi = mrpt::math::wrapToPi(this->phi + b.phi);
81 
82  return mrpt::math::TPose2D(new_x, new_y, new_phi);
83 }
84 
86  const mrpt::math::TPose2D& b) const
87 {
88  const double B_cosphi = cos(b.phi), B_sinphi = sin(b.phi);
89 
90  const double new_x =
91  (this->x - b.x) * B_cosphi + (this->y - b.y) * B_sinphi;
92  const double new_y =
93  -(this->x - b.x) * B_sinphi + (this->y - b.y) * B_cosphi;
94  const double new_phi = mrpt::math::wrapToPi(this->phi - b.phi);
95 
96  return mrpt::math::TPose2D(new_x, new_y, new_phi);
97 }
98 
99 // ----
101 {
102  s = mrpt::format("[%f %f %f]", vx, vy, RAD2DEG(omega));
103 }
105 {
106  CMatrixDouble m;
107  if (!m.fromMatlabStringFormat(s))
108  THROW_EXCEPTION("Malformed expression in ::fromString");
109  ASSERTMSG_(
110  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
111  vx = m.get_unsafe(0, 0);
112  vy = m.get_unsafe(0, 1);
113  omega = DEG2RAD(m.get_unsafe(0, 2));
114 }
115 // Transform the (vx,vy) components for a counterclockwise rotation of `ang`
116 // radians
117 void TTwist2D::rotate(const double ang)
118 {
119  const double nvx = vx * cos(ang) - vy * sin(ang);
120  const double nvy = vx * sin(ang) + vy * cos(ang);
121  vx = nvx;
122  vy = nvy;
123 }
124 bool TTwist2D::operator==(const TTwist2D& o) const
125 {
126  return vx == o.vx && vy == o.vy && omega == o.omega;
127 }
128 bool TTwist2D::operator!=(const TTwist2D& o) const { return !(*this == o); }
130 {
131  return mrpt::math::TPose2D(vx * dt, vy * dt, omega * dt);
132 }
133 
134 // ----
136 {
137  s = mrpt::format(
138  "[%f %f %f %f %f %f]", vx, vy, vz, RAD2DEG(wx), RAD2DEG(wy),
139  RAD2DEG(wz));
140 }
142 {
143  CMatrixDouble m;
144  if (!m.fromMatlabStringFormat(s))
145  THROW_EXCEPTION("Malformed expression in ::fromString");
146  ASSERTMSG_(
147  m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
148  for (int i = 0; i < 3; i++) (*this)[i] = m.get_unsafe(0, i);
149  for (int i = 0; i < 3; i++)
150  (*this)[3 + i] = DEG2RAD(m.get_unsafe(0, 3 + i));
151 }
152 // Transform all 6 components for a change of reference frame from "A" to
153 // another frame "B" whose rotation with respect to "A" is given by `rot`. The
154 // translational part of the pose is ignored
155 void TTwist3D::rotate(const TPose3D& rot)
156 {
157  const TTwist3D t = *this;
159  rot.getRotationMatrix(R);
160  vx = R(0, 0) * t.vx + R(0, 1) * t.vy + R(0, 2) * t.vz;
161  vy = R(1, 0) * t.vx + R(1, 1) * t.vy + R(1, 2) * t.vz;
162  vz = R(2, 0) * t.vx + R(2, 1) * t.vy + R(2, 2) * t.vz;
163 
164  wx = R(0, 0) * t.wx + R(0, 1) * t.wy + R(0, 2) * t.wz;
165  wy = R(1, 0) * t.wx + R(1, 1) * t.wy + R(1, 2) * t.wz;
166  wz = R(2, 0) * t.wx + R(2, 1) * t.wy + R(2, 2) * t.wz;
167 }
168 bool TTwist3D::operator==(const TTwist3D& o) const
169 {
170  return vx == o.vx && vy == o.vy && vz == o.vz && wx == o.wx && wy == o.wy &&
171  wz == o.wz;
172 }
173 bool TTwist3D::operator!=(const TTwist3D& o) const { return !(*this == o); }
174 TPoint3D::TPoint3D(const TPoint2D& p) : x(p.x), y(p.y), z(0.0) {}
175 TPoint3D::TPoint3D(const TPose2D& p) : x(p.x), y(p.y), z(0.0) {}
176 TPoint3D::TPoint3D(const TPose3D& p) : x(p.x), y(p.y), z(p.z) {}
177 bool TPoint3D::operator<(const TPoint3D& p) const
178 {
179  if (x < p.x)
180  return true;
181  else if (x > p.x)
182  return false;
183  else if (y < p.y)
184  return true;
185  else if (y > p.y)
186  return false;
187  else
188  return z < p.z;
189 }
191 {
192  CMatrixDouble m;
193  if (!m.fromMatlabStringFormat(s))
194  THROW_EXCEPTION("Malformed expression in ::fromString");
195  ASSERTMSG_(
196  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
197  x = m.get_unsafe(0, 0);
198  y = m.get_unsafe(0, 1);
199  z = m.get_unsafe(0, 2);
200 }
201 
203  : x(p.x), y(p.y), z(0.0), yaw(0.0), pitch(0.0), roll(0.0)
204 {
205 }
207  : x(p.x), y(p.y), z(0.0), yaw(p.phi), pitch(0.0), roll(0.0)
208 {
209 }
211  : x(p.x), y(p.y), z(p.z), yaw(0.0), pitch(0.0), roll(0.0)
212 {
213 }
215 {
216  s = mrpt::format(
217  "[%f %f %f %f %f %f]", x, y, z, RAD2DEG(yaw), RAD2DEG(pitch),
218  RAD2DEG(roll));
219 }
223 {
224  // See:
225  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
226  const double cy = cos(yaw * 0.5), sy = sin(yaw * 0.5);
227  const double cp = cos(pitch * 0.5), sp = sin(pitch * 0.5);
228  const double cr = cos(roll * 0.5), sr = sin(roll * 0.5);
229 
230  const double ccc = cr * cp * cy;
231  const double ccs = cr * cp * sy;
232  const double css = cr * sp * sy;
233  const double sss = sr * sp * sy;
234  const double scc = sr * cp * cy;
235  const double ssc = sr * sp * cy;
236  const double csc = cr * sp * cy;
237  const double scs = sr * cp * sy;
238 
239  q[0] = ccc + sss;
240  q[1] = scc - css;
241  q[2] = csc + scs;
242  q[3] = ccs - ssc;
243 
244  // Compute 4x3 Jacobian: for details, see technical report:
245  // Parameterizations of SE(3) transformations: equivalences, compositions
246  // and uncertainty, J.L. Blanco (2010).
247  // http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
248  if (out_dq_dr)
249  {
250  alignas(MRPT_MAX_ALIGN_BYTES) const double nums[4 * 3] = {
251  -0.5 * q[3], 0.5 * (-csc + scs), -0.5 * q[1],
252  -0.5 * q[2], 0.5 * (-ssc - ccs), 0.5 * q[0],
253  0.5 * q[1], 0.5 * (ccc - sss), 0.5 * q[3],
254  0.5 * q[0], 0.5 * (-css - scc), -0.5 * q[2]};
255  out_dq_dr->loadFromArray(nums);
256  }
257 }
259 {
261  this->getRotationMatrix(R);
262  TPoint3D res;
263  res.x = R(0, 0) * l.x + R(0, 1) * l.y + R(0, 2) * l.z + this->x;
264  res.y = R(1, 0) * l.x + R(1, 1) * l.y + R(1, 2) * l.z + this->y;
265  res.z = R(2, 0) * l.x + R(2, 1) * l.y + R(2, 2) * l.z + this->z;
266 
267  g = res;
268 }
270 {
271  CMatrixDouble44 H;
273  TPoint3D res;
274  res.x = H(0, 0) * g.x + H(0, 1) * g.y + H(0, 2) * g.z + H(0, 3);
275  res.y = H(1, 0) * g.x + H(1, 1) * g.y + H(1, 2) * g.z + H(1, 3);
276  res.z = H(2, 0) * g.x + H(2, 1) * g.y + H(2, 2) * g.z + H(2, 3);
277 
278  l = res;
279 }
281 {
282  const double cy = cos(yaw);
283  const double sy = sin(yaw);
284  const double cp = cos(pitch);
285  const double sp = sin(pitch);
286  const double cr = cos(roll);
287  const double sr = sin(roll);
288 
289  alignas(MRPT_MAX_ALIGN_BYTES) const double rot_vals[] = {cy * cp,
290  cy * sp * sr - sy * cr,
291  cy * sp * cr + sy * sr,
292  sy * cp,
293  sy * sp * sr + cy * cr,
294  sy * sp * cr - cy * sr,
295  -sp,
296  cp * sr,
297  cp * cr};
298  R.loadFromArray(rot_vals);
299 }
301  const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
302  double& roll)
303 {
305  std::abs(
306  sqrt(square(R(0, 0)) + square(R(1, 0)) + square(R(2, 0))) - 1) <
307  3e-3,
308  "Homogeneous matrix is not orthogonal & normalized!: " +
309  R.inMatlabFormat());
311  std::abs(
312  sqrt(square(R(0, 1)) + square(R(1, 1)) + square(R(2, 1))) - 1) <
313  3e-3,
314  "Homogeneous matrix is not orthogonal & normalized!: " +
315  R.inMatlabFormat());
317  std::abs(
318  sqrt(square(R(0, 2)) + square(R(1, 2)) + square(R(2, 2))) - 1) <
319  3e-3,
320  "Homogeneous matrix is not orthogonal & normalized!: " +
321  R.inMatlabFormat());
322 
323  // Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
324  pitch = atan2(-R(2, 0), hypot(R(0, 0), R(1, 0)));
325 
326  // Roll:
327  if ((fabs(R(2, 1)) + fabs(R(2, 2))) <
328  10 * std::numeric_limits<double>::epsilon())
329  {
330  // Gimbal lock between yaw and roll. This one is arbitrarily forced to
331  // be zero.
332  // Check
333  // http://reference.mrpt.org/devel/classmrpt_1_1poses_1_1_c_pose3_d.html.
334  // If cos(pitch)==0, the homogeneous matrix is:
335  // When sin(pitch)==1:
336  // /0 cysr-sycr cycr+sysr x\ /0 sin(r-y) cos(r-y) x\.
337  // |0 sysr+cycr sycr-cysr y| = |0 cos(r-y) -sin(r-y) y|
338  // |-1 0 0 z| |-1 0 0 z|
339  // \0 0 0 1/ \0 0 0 1/
340  //
341  // And when sin(pitch)=-1:
342  // /0 -cysr-sycr -cycr+sysr x\ /0 -sin(r+y) -cos(r+y) x\.
343  // |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y) -sin(r+y) y|
344  // |1 0 0 z| |1 0 0 z|
345  // \0 0 0 1/ \0 0 0 1/
346  //
347  // Both cases are in a "gimbal lock" status. This happens because pitch
348  // is vertical.
349 
350  roll = 0.0;
351  if (pitch > 0)
352  yaw = atan2(R(1, 2), R(0, 2));
353  else
354  yaw = atan2(-R(1, 2), -R(0, 2));
355  }
356  else
357  {
358  roll = atan2(R(2, 1), R(2, 2));
359  // Yaw:
360  yaw = atan2(R(1, 0), R(0, 0));
361  }
362 }
363 
365 {
366  SO3_to_yaw_pitch_roll(HG.block<3, 3>(0, 0), yaw, pitch, roll);
367  x = HG(0, 3);
368  y = HG(1, 3);
369  z = HG(2, 3);
370 }
371 void TPose3D::composePose(const TPose3D other, TPose3D& result) const
372 {
373  CMatrixDouble44 me_H, o_H;
374  this->getHomogeneousMatrix(me_H);
375  other.getHomogeneousMatrix(o_H);
376  result.fromHomogeneousMatrix(me_H * o_H);
377 }
379 {
382  HG.block<3, 3>(0, 0) = R;
383  HG(0, 3) = x;
384  HG(1, 3) = y;
385  HG(2, 3) = z;
386  HG(3, 0) = HG(3, 1) = HG(3, 2) = 0.;
387  HG(3, 3) = 1.;
388 }
390 { // Get current HM & inverse in-place:
391  this->getHomogeneousMatrix(HG);
393 }
394 
396 {
397  CMatrixDouble m;
398  if (!m.fromMatlabStringFormat(s))
399  THROW_EXCEPTION("Malformed expression in ::fromString");
400  ASSERTMSG_(
401  m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
402  x = m.get_unsafe(0, 0);
403  y = m.get_unsafe(0, 1);
404  z = m.get_unsafe(0, 2);
405  yaw = DEG2RAD(m.get_unsafe(0, 3));
406  pitch = DEG2RAD(m.get_unsafe(0, 4));
407  roll = DEG2RAD(m.get_unsafe(0, 5));
408 }
409 
411 {
412  CMatrixDouble m;
413  if (!m.fromMatlabStringFormat(s))
414  THROW_EXCEPTION("Malformed expression in ::fromString");
415  ASSERTMSG_(
416  m.rows() == 1 && m.cols() == 7, "Wrong size of vector in ::fromString");
417  for (int i = 0; i < m.cols(); i++) (*this)[i] = m.get_unsafe(0, i);
418 }
420 {
421  CMatrixDouble44 H;
422  p.getInverseHomogeneousMatrix(H);
423  TPose3D ret;
424  ret.fromHomogeneousMatrix(H);
425  return ret;
426 }
428 {
429  // b - a = A^{-1} * B
430  CMatrixDouble44 Hainv, Hb;
431  a.getInverseHomogeneousMatrix(Hainv);
432  b.getHomogeneousMatrix(Hb);
433  TPose3D ret;
434  ret.fromHomogeneousMatrix(Hainv * Hb);
435  return ret;
436 }
437 
438 // Text streaming:
439 std::ostream& operator<<(std::ostream& o, const TPoint2D& p)
440 {
441  return (o << p.asString());
442 }
443 std::ostream& operator<<(std::ostream& o, const TPoint3D& p)
444 {
445  return (o << p.asString());
446 }
447 std::ostream& operator<<(std::ostream& o, const TPose2D& p)
448 {
449  return (o << p.asString());
450 }
451 std::ostream& operator<<(std::ostream& o, const TPose3D& p)
452 {
453  return (o << p.asString());
454 }
455 std::ostream& operator<<(std::ostream& o, const TPose3DQuat& p)
456 {
457  return (o << p.asString());
458 }
459 
461 {
462  return in >> s.point1 >> s.point2;
463 }
465 {
466  return out << s.point1 << s.point2;
467 }
469 {
470  return in >> l.coefs[0] >> l.coefs[1] >> l.coefs[2];
471 }
473 {
474  return out << l.coefs[0] << l.coefs[1] << l.coefs[2];
475 }
476 
478 {
479  return in >> s.point1 >> s.point2;
480 }
482 {
483  return out << s.point1 << s.point2;
484 }
486 {
487  return in >> l.pBase >> l.director[0] >> l.director[1] >> l.director[2];
488 }
490 {
491  return out << l.pBase << l.director[0] << l.director[1] << l.director[2];
492 }
494 {
495  return in >> p.coefs[0] >> p.coefs[1] >> p.coefs[2] >> p.coefs[3];
496 }
498 {
499  return out << p.coefs[0] << p.coefs[1] << p.coefs[2] << p.coefs[3];
500 }
501 
502 double TSegment2D::length() const { return math::distance(point1, point2); }
503 double TSegment2D::distance(const TPoint2D& point) const
504 {
505  return std::abs(signedDistance(point));
506 }
507 double TSegment2D::signedDistance(const TPoint2D& point) const
508 {
509  // It is reckoned whether the perpendicular line to the TSegment2D which
510  // passes through point crosses or not the referred segment,
511  // or what is the same, whether point makes an obtuse triangle with the
512  // segment or not (being the longest segment one between the point and
513  // either end of TSegment2D).
514  const double d1 = math::distance(point, point1);
515  if (point1 == point2) return d1;
516 
517  const double d2 = math::distance(point, point2);
518  const double d3 = length();
519  const double ds1 = square(d1);
520  const double ds2 = square(d2);
521  const double ds3 = square(d3);
522  if (ds1 > (ds2 + ds3) || ds2 > (ds1 + ds3))
523  // Fix sign:
524  return min(d1, d2) *
525  (TLine2D(*this).signedDistance(point) < 0 ? -1 : 1);
526  else
527  return TLine2D(*this).signedDistance(point);
528 }
529 bool TSegment2D::contains(const TPoint2D& point) const
530 {
531  return abs(math::distance(point1, point) + math::distance(point2, point) -
533 }
535 {
536  s = TSegment3D(*this);
537 }
539 {
540  point1 = TPoint2D(s.point1);
541  point2 = TPoint2D(s.point2);
542  if (point1 == point2)
543  throw std::logic_error("Segment is normal to projection plane");
544 }
545 
547 {
548  if (point1 < s.point1)
549  return true;
550  else if (s.point1 < point1)
551  return false;
552  else
553  return point2 < s.point2;
554 }
555 
556 double TSegment3D::length() const { return math::distance(point1, point2); }
557 double TSegment3D::distance(const TPoint3D& point) const
558 {
559  return min(
560  min(math::distance(point, point1), math::distance(point, point2)),
561  TLine3D(*this).distance(point));
562 }
563 double TSegment3D::distance(const TSegment3D& segment) const
564 {
565  Eigen::Vector3d u, v, w;
566  TPoint3D diff_vect = point2 - point1;
567  diff_vect.getAsVector(u);
568  diff_vect = segment.point2 - segment.point1;
569  diff_vect.getAsVector(v);
570  diff_vect = point1 - segment.point1;
571  diff_vect.getAsVector(w);
572  double a = u.dot(u); // always >= 0
573  double b = u.dot(v);
574  double c = v.dot(v); // always >= 0
575  double d = u.dot(w);
576  double e = v.dot(w);
577  double D = a * c - b * b; // always >= 0
578  double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
579  double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
580 
581  // compute the line parameters of the two closest points
582  if (D < 0.00000001)
583  { // the lines are almost parallel
584  sN = 0.0; // force using point P0 on segment S1
585  sD = 1.0; // to prevent possible division by 0.0 later
586  tN = e;
587  tD = c;
588  }
589  else
590  { // get the closest points on the infinite lines
591  sN = (b * e - c * d);
592  tN = (a * e - b * d);
593  if (sN < 0.0)
594  { // sc < 0 => the s=0 edge is visible
595  sN = 0.0;
596  tN = e;
597  tD = c;
598  }
599  else if (sN > sD)
600  { // sc > 1 => the s=1 edge is visible
601  sN = sD;
602  tN = e + b;
603  tD = c;
604  }
605  }
606 
607  if (tN < 0.0)
608  { // tc < 0 => the t=0 edge is visible
609  tN = 0.0;
610  // recompute sc for this edge
611  if (-d < 0.0)
612  sN = 0.0;
613  else if (-d > a)
614  sN = sD;
615  else
616  {
617  sN = -d;
618  sD = a;
619  }
620  }
621  else if (tN > tD)
622  { // tc > 1 => the t=1 edge is visible
623  tN = tD;
624  // recompute sc for this edge
625  if ((-d + b) < 0.0)
626  sN = 0;
627  else if ((-d + b) > a)
628  sN = sD;
629  else
630  {
631  sN = (-d + b);
632  sD = a;
633  }
634  }
635  // finally do the division to get sc and tc
636  sc = (fabs(sN) < 0.00000001 ? 0.0 : sN / sD);
637  tc = (fabs(tN) < 0.00000001 ? 0.0 : tN / tD);
638 
639  // get the difference of the two closest points
640  CVectorDouble dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
641 
642  return dP.norm(); // return the closest distance
643 }
644 bool TSegment3D::contains(const TPoint3D& point) const
645 {
646  // Not very intuitive, but very fast, method.
647  return abs(math::distance(point1, point) + math::distance(point2, point) -
649 }
650 
652 {
653  if (point1 < s.point1)
654  return true;
655  else if (s.point1 < point1)
656  return false;
657  else
658  return point2 < s.point2;
659 }
660 
661 double TLine2D::evaluatePoint(const TPoint2D& point) const
662 {
663  return coefs[0] * point.x + coefs[1] * point.y + coefs[2];
664 }
665 bool TLine2D::contains(const TPoint2D& point) const
666 {
667  return abs(distance(point)) < getEpsilon();
668 }
669 double TLine2D::distance(const TPoint2D& point) const
670 {
671  return abs(evaluatePoint(point)) /
672  sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
673 }
674 double TLine2D::signedDistance(const TPoint2D& point) const
675 {
676  return evaluatePoint(point) /
677  sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
678 }
679 void TLine2D::getNormalVector(double (&vector)[2]) const
680 {
681  vector[0] = coefs[0];
682  vector[1] = coefs[1];
683 }
685 {
686  double s = sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
687  for (size_t i = 0; i < 3; i++) coefs[i] /= s;
688 }
689 void TLine2D::getDirectorVector(double (&vector)[2]) const
690 {
691  vector[0] = -coefs[1];
692  vector[1] = coefs[0];
693 }
694 void TLine2D::generate3DObject(TLine3D& l) const { l = TLine3D(*this); }
695 void TLine2D::getAsPose2D(TPose2D& outPose) const
696 {
697  // Line's director vector is (-coefs[1],coefs[0]).
698  // If line is horizontal, force x=0. Else, force y=0. In both cases, we'll
699  // find a suitable point.
700  outPose.phi = atan2(coefs[0], -coefs[1]);
701  if (abs(coefs[0]) < getEpsilon())
702  {
703  outPose.x = 0;
704  outPose.y = -coefs[2] / coefs[1];
705  }
706  else
707  {
708  outPose.x = -coefs[2] / coefs[0];
709  outPose.y = 0;
710  }
711 }
713  const TPoint2D& origin, TPose2D& outPose) const
714 {
715  if (!contains(origin))
716  throw std::logic_error("Base point is not contained in the line");
717  outPose = origin;
718  // Line's director vector is (-coefs[1],coefs[0]).
719  outPose.phi = atan2(coefs[0], -coefs[1]);
720 }
721 TLine2D::TLine2D(const TPoint2D& p1, const TPoint2D& p2)
722 {
723  if (p1 == p2) throw logic_error("Both points are the same");
724  coefs[0] = p2.y - p1.y;
725  coefs[1] = p1.x - p2.x;
726  coefs[2] = p2.x * p1.y - p2.y * p1.x;
727 }
729 {
730  coefs[0] = s.point2.y - s.point1.y;
731  coefs[1] = s.point1.x - s.point2.x;
732  coefs[2] = s.point2.x * s.point1.y - s.point2.y * s.point1.x;
733  // unitarize(); //¿?
734 }
736 {
737  // Line's projection to Z plane may be a point.
738  if (hypot(l.director[0], l.director[1]) < getEpsilon())
739  throw std::logic_error("Line is normal to projection plane");
740  coefs[0] = -l.director[1];
741  coefs[1] = l.director[0];
742  coefs[2] = l.pBase.x * l.director[1] - l.pBase.y * l.director[0];
743 }
744 
745 bool TLine3D::contains(const TPoint3D& point) const
746 {
747  double dx = point.x - pBase.x;
748  double dy = point.y - pBase.y;
749  double dz = point.z - pBase.z;
750  if (abs(dx) < getEpsilon() && abs(dy) < getEpsilon() &&
751  abs(dz) < getEpsilon())
752  return true;
753  // dx dy dz
754  // if -----------=-----------=-----------, point is inside the line.
755  // director[0] director[1] director[2]
756  return (abs(dx * director[1] - dy * director[0]) < getEpsilon()) &&
757  (abs(dx * director[2] - dz * director[0]) < getEpsilon()) &&
758  (abs(dy * director[2] - dz * director[1]) < getEpsilon());
759 }
760 double TLine3D::distance(const TPoint3D& point) const
761 {
762  // Let d be line's base point minus the argument. Then,
763  // d·director/(|d|·|director|) equals both vector's cosine.
764  // So, d·director/|director| equals d's projection over director. Then,
765  // distance is sqrt(|d|²-(d·director/|director|)²).
766  double d[3] = {point.x - pBase.x, point.y - pBase.y, point.z - pBase.z};
767  double dv = 0, d2 = 0, v2 = 0;
768  for (size_t i = 0; i < 3; i++)
769  {
770  dv += d[i] * director[i];
771  d2 += d[i] * d[i];
772  v2 += director[i] * director[i];
773  }
774  return sqrt(d2 - (dv * dv) / v2);
775 }
777 {
778  double s = sqrt(squareNorm<3, double>(director));
779  for (size_t i = 0; i < 3; i++) director[i] /= s;
780 }
781 TLine3D::TLine3D(const TPoint3D& p1, const TPoint3D& p2)
782 {
783  if (abs(math::distance(p1, p2)) < getEpsilon())
784  throw logic_error("Both points are the same");
785  pBase = p1;
786  director[0] = p2.x - p1.x;
787  director[1] = p2.y - p1.y;
788  director[2] = p2.z - p1.z;
789 }
791 {
792  pBase = s.point1;
793  director[0] = s.point2.x - s.point1.x;
794  director[1] = s.point2.y - s.point1.y;
795  director[2] = s.point2.z - s.point1.z;
796 }
798 {
799  director[0] = -l.coefs[1];
800  director[1] = l.coefs[0];
801  director[2] = 0;
802  // We assume that either l.coefs[0] or l.coefs[1] is not null. Respectively,
803  // either y or x can be used as free cordinate.
804  if (abs(l.coefs[0]) >= getEpsilon())
805  {
806  pBase.x = -l.coefs[2] / l.coefs[0];
807  pBase.y = 0;
808  }
809  else
810  {
811  pBase.x = 0;
812  pBase.y = -l.coefs[1] / l.coefs[0];
813  }
814  pBase.z = 0;
815 }
816 
817 double TPlane::evaluatePoint(const TPoint3D& point) const
818 {
819  return dotProduct<3, double>(coefs, point) + coefs[3];
820 }
821 bool TPlane::contains(const TPoint3D& point) const
822 {
823  return distance(point) < getEpsilon();
824 }
825 bool TPlane::contains(const TLine3D& line) const
826 {
827  if (!contains(line.pBase)) return false; // Base point must be contained
828  return abs(getAngle(*this, line)) <
829  getEpsilon(); // Plane's normal must be normal to director vector
830 }
831 double TPlane::distance(const TPoint3D& point) const
832 {
833  return abs(evaluatePoint(point)) / sqrt(squareNorm<3, double>(coefs));
834 }
835 double TPlane::distance(const TLine3D& line) const
836 {
837  if (abs(getAngle(*this, line)) >= getEpsilon())
838  return 0; // Plane crosses with line
839  else
840  return distance(line.pBase);
841 }
842 void TPlane::getNormalVector(double (&vector)[3]) const
843 {
844  vector[0] = coefs[0];
845  vector[1] = coefs[1];
846  vector[2] = coefs[2];
847 }
849 {
850  double s = sqrt(squareNorm<3, double>(coefs));
851  for (size_t i = 0; i < 4; i++) coefs[i] /= s;
852 }
853 
854 // Returns a 6D pose such as its XY plane coincides with the plane
856 {
857  double normal[3];
858  getUnitaryNormalVector(normal);
859  CMatrixDouble44 AXIS;
860  generateAxisBaseFromDirectionAndAxis(normal, 2, AXIS);
861  for (size_t i = 0; i < 3; i++)
862  if (abs(coefs[i]) >= getEpsilon())
863  {
864  AXIS.set_unsafe(i, 3, -coefs[3] / coefs[i]);
865  break;
866  }
867  outPose.fromHomogeneousMatrix(AXIS);
868 }
869 void TPlane::getAsPose3DForcingOrigin(const TPoint3D& newOrigin, TPose3D& pose)
870 {
871  if (!contains(newOrigin))
872  throw std::logic_error("Base point is not in the plane.");
873  double normal[3];
874  getUnitaryNormalVector(normal);
875  CMatrixDouble44 AXIS;
876  generateAxisBaseFromDirectionAndAxis(normal, 2, AXIS);
877  for (size_t i = 0; i < 3; i++) AXIS.set_unsafe(i, 3, newOrigin[i]);
878  pose.fromHomogeneousMatrix(AXIS);
879 }
880 TPlane::TPlane(const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3)
881 {
882  double dx1 = p2.x - p1.x;
883  double dy1 = p2.y - p1.y;
884  double dz1 = p2.z - p1.z;
885  double dx2 = p3.x - p1.x;
886  double dy2 = p3.y - p1.y;
887  double dz2 = p3.z - p1.z;
888  coefs[0] = dy1 * dz2 - dy2 * dz1;
889  coefs[1] = dz1 * dx2 - dz2 * dx1;
890  coefs[2] = dx1 * dy2 - dx2 * dy1;
891  if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
892  abs(coefs[2]) < getEpsilon())
893  throw logic_error("Points are linearly dependent");
894  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
895 }
896 TPlane::TPlane(const TPoint3D& p1, const TLine3D& r2)
897 {
898  double dx1 = p1.x - r2.pBase.x;
899  double dy1 = p1.y - r2.pBase.y;
900  double dz1 = p1.z - r2.pBase.z;
901  coefs[0] = dy1 * r2.director[2] - dz1 * r2.director[1];
902  coefs[1] = dz1 * r2.director[0] - dx1 * r2.director[2];
903  coefs[2] = dx1 * r2.director[1] - dy1 * r2.director[0];
904  if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
905  abs(coefs[2]) < getEpsilon())
906  throw logic_error("Point is contained in the line");
907  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
908 }
909 TPlane::TPlane(const TLine3D& r1, const TLine3D& r2)
910 {
912  coefs[3] =
913  -coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y - coefs[2] * r1.pBase.z;
914  if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
915  abs(coefs[2]) < getEpsilon())
916  {
917  // Lines are parallel
918  if (r1.contains(r2.pBase)) throw std::logic_error("Lines are the same");
919  // Use a line's director vector and both pBase's difference to create
920  // the plane.
921  double d[3];
922  for (size_t i = 0; i < 3; i++) d[i] = r1.pBase[i] - r2.pBase[i];
923  crossProduct3D(r1.director, d, coefs);
924  coefs[3] = -coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y -
925  coefs[2] * r1.pBase.z;
926  }
927  else if (abs(evaluatePoint(r2.pBase)) >= getEpsilon())
928  throw logic_error("Lines do not intersect");
929 }
930 
931 template <class T>
932 inline void removeUnusedVertices(T& poly)
933 {
934  size_t N = poly.size();
935  if (N < 3) return;
936  std::vector<size_t> unused;
937  if (abs(mrpt::math::distance(poly[N - 1], poly[0]) +
938  mrpt::math::distance(poly[0], poly[1]) -
939  mrpt::math::distance(poly[N - 1], poly[1])) <
941  unused.push_back(0);
942  for (size_t i = 1; i < N - 1; i++)
943  if (abs(mrpt::math::distance(poly[i - 1], poly[i]) +
944  mrpt::math::distance(poly[i], poly[i + 1]) -
945  mrpt::math::distance(poly[i - 1], poly[i + 1])) <
947  unused.push_back(i);
948  if (abs(mrpt::math::distance(poly[N - 2], poly[N - 1]) +
949  mrpt::math::distance(poly[N - 1], poly[0]) -
950  mrpt::math::distance(poly[N - 2], poly[0])) <
952  unused.push_back(N - 1);
953  unused.push_back(N);
954  size_t diff = 1;
955  for (size_t i = 0; i < unused.size() - 1; i++)
956  {
957  size_t last = unused[i + 1];
958  for (size_t j = unused[i] + 1 - diff; j < last - diff; j++)
959  poly[j] = poly[j + diff];
960  }
961  poly.resize(N + 1 - unused.size());
962 }
963 template <class T>
964 inline void removeRepVertices(T& poly)
965 {
966  size_t N = poly.size();
967  if (N < 3) return;
968  std::vector<size_t> rep;
969  for (size_t i = 0; i < N - 1; i++)
970  if (mrpt::math::distance(poly[i], poly[i + 1]) < getEpsilon())
971  rep.push_back(i);
972  if (mrpt::math::distance(poly[N - 1], poly[0]) < getEpsilon())
973  rep.push_back(N - 1);
974  rep.push_back(N);
975  size_t diff = 1;
976  for (size_t i = 0; i < rep.size() - 1; i++)
977  {
978  size_t last = rep[i + 1];
979  for (size_t j = rep[i] + 1 - diff; j < last - diff; j++)
980  poly[j] = poly[j + diff];
981  }
982  poly.resize(N + 1 - rep.size());
983 }
984 
985 double TPolygon2D::distance(const TPoint2D& point) const
986 {
987  if (contains(point)) return 0;
988  std::vector<TSegment2D> sgs;
989  getAsSegmentList(sgs);
990 
991  if (sgs.empty())
992  THROW_EXCEPTION("Cannot compute distance to an empty polygon.");
993 
994  double distance = std::numeric_limits<double>::max();
995 
996  for (vector<TSegment2D>::const_iterator it = sgs.begin(); it != sgs.end();
997  ++it)
998  {
999  double d = (*it).distance(point);
1000  if (d < distance) distance = d;
1001  }
1002  return distance;
1003 }
1004 
1006  TPoint2D& min_coords, TPoint2D& max_coords) const
1007 {
1008  ASSERTMSG_(!this->empty(), "getBoundingBox() called on an empty polygon!");
1009  min_coords.x = min_coords.y = std::numeric_limits<double>::max();
1010  max_coords.x = max_coords.y = -std::numeric_limits<double>::max();
1011  for (size_t i = 0; i < size(); i++)
1012  {
1013  mrpt::keep_min(min_coords.x, (*this)[i].x);
1014  mrpt::keep_min(min_coords.y, (*this)[i].y);
1015  mrpt::keep_max(max_coords.x, (*this)[i].x);
1016  mrpt::keep_max(max_coords.y, (*this)[i].y);
1017  }
1018 }
1019 
1020 // isLeft(): tests if a point is Left|On|Right of an infinite line.
1021 // Input: three points P0, P1, and P2
1022 // Return: >0 for P2 left of the line through P0 and P1
1023 // =0 for P2 on the line
1024 // <0 for P2 right of the line
1025 // See: Algorithm 1 "Area of Triangles and Polygons"
1026 inline double isLeft(
1027  const mrpt::math::TPoint2D& P0, const mrpt::math::TPoint2D& P1,
1028  const mrpt::math::TPoint2D& P2)
1029 {
1030  return ((P1.x - P0.x) * (P2.y - P0.y) - (P2.x - P0.x) * (P1.y - P0.y));
1031 }
1032 
1033 bool TPolygon2D::contains(const TPoint2D& P) const
1034 {
1035  int wn = 0; // the winding number counter
1036 
1037  // loop through all edges of the polygon
1038  const size_t n = this->size();
1039  for (size_t i = 0; i < n; i++) // edge from V[i] to V[i+1]
1040  {
1041  if ((*this)[i].y <= P.y)
1042  {
1043  // start y <= P.y
1044  if ((*this)[(i + 1) % n].y > P.y) // an upward crossing
1045  if (isLeft((*this)[i], (*this)[(i + 1) % n], P) >
1046  0) // P left of edge
1047  ++wn; // have a valid up intersect
1048  }
1049  else
1050  {
1051  // start y > P.y (no test needed)
1052  if ((*this)[(i + 1) % n].y <= P.y) // a downward crossing
1053  if (isLeft((*this)[i], (*this)[(i + 1) % n], P) <
1054  0) // P right of edge
1055  --wn; // have a valid down intersect
1056  }
1057  }
1058 
1059  return wn != 0;
1060 }
1061 void TPolygon2D::getAsSegmentList(vector<TSegment2D>& v) const
1062 {
1063  size_t N = size();
1064  v.resize(N);
1065  for (size_t i = 0; i < N - 1; i++)
1066  v[i] = TSegment2D(operator[](i), operator[](i + 1));
1067  v[N - 1] = TSegment2D(operator[](N - 1), operator[](0));
1068 }
1069 
1071 {
1072  p = TPolygon3D(*this);
1073 }
1074 // Auxiliar functor class to compute polygon's center
1075 template <class T, int N>
1077 {
1078  public:
1080  FAddPoint(T& o) : object(o)
1081  {
1082  for (size_t i = 0; i < N; i++) object[i] = 0.0;
1083  }
1084  void operator()(const T& o)
1085  {
1086  for (size_t i = 0; i < N; i++) object[i] += o[i];
1087  }
1088 };
1090 {
1091  for_each(begin(), end(), FAddPoint<TPoint2D, 2>(p));
1092  size_t N = size();
1093  p.x /= N;
1094  p.y /= N;
1095 }
1097 {
1098  size_t N = size();
1099  if (N <= 3) return true;
1100  vector<TSegment2D> sgms;
1101  getAsSegmentList(sgms);
1102  for (size_t i = 0; i < N; i++)
1103  {
1104  char s = 0;
1105  TLine2D l = TLine2D(sgms[i]);
1106  for (size_t j = 0; j < N; j++)
1107  {
1108  double d = l.evaluatePoint(operator[](j));
1109  if (abs(d) < getEpsilon())
1110  continue;
1111  else if (!s)
1112  s = (d > 0) ? 1 : -1;
1113  else if (s != ((d > 0) ? 1 : -1))
1114  return false;
1115  }
1116  }
1117  return true;
1118 }
1121 {
1123  removeUnusedVertices(*this);
1124 }
1126  std::vector<double>& x, std::vector<double>& y) const
1127 {
1128  size_t N = size();
1129  x.resize(N + 1);
1130  y.resize(N + 1);
1131  for (size_t i = 0; i < N; i++)
1132  {
1133  x[i] = operator[](i).x;
1134  y[i] = operator[](i).y;
1135  }
1136  x[N] = operator[](0).x;
1137  y[N] = operator[](0).y;
1138 }
1140 {
1141  size_t N = p.size();
1142  resize(N);
1143  for (size_t i = 0; i < N; i++) operator[](i) = TPoint2D(p[i]);
1144 }
1146  size_t numEdges, double radius, TPolygon2D& poly)
1147 {
1148  if (numEdges < 3 || abs(radius) < getEpsilon())
1149  throw std::logic_error(
1150  "Invalid arguments for regular polygon creations");
1151  poly.resize(numEdges);
1152  for (size_t i = 0; i < numEdges; i++)
1153  {
1154  double angle = i * M_PI * 2 / numEdges;
1155  poly[i] = TPoint2D(radius * cos(angle), radius * sin(angle));
1156  }
1157 }
1159  size_t numEdges, double radius, TPolygon2D& poly, const TPose2D& pose)
1160 {
1161  createRegularPolygon(numEdges, radius, poly);
1162  for (size_t i = 0; i < numEdges; i++) poly[i] = pose.composePoint(poly[i]);
1163 }
1164 
1165 double TPolygon3D::distance(const TPoint3D& point) const
1166 {
1167  TPlane pl;
1168  if (!getPlane(pl))
1169  throw std::logic_error("Polygon does not conform a plane");
1170  TPoint3D newPoint;
1171  TPolygon3D newPoly;
1172  TPose3D pose;
1173  pl.getAsPose3DForcingOrigin(operator[](0), pose);
1174  project3D(point, pose, newPoint);
1175  project3D(*this, pose, newPoly);
1176  double distance2D = TPolygon2D(newPoly).distance(TPoint2D(newPoint));
1177  return sqrt(newPoint.z * newPoint.z + distance2D * distance2D);
1178 }
1179 bool TPolygon3D::contains(const TPoint3D& point) const
1180 {
1181  TPoint3D pMin, pMax;
1182  getPrismBounds(*this, pMin, pMax);
1183  if (point.x + getEpsilon() < pMin.x || point.y + getEpsilon() < pMin.y ||
1184  point.z + getEpsilon() < pMin.z || point.x > pMax.x + getEpsilon() ||
1185  point.y > pMax.y + getEpsilon() || point.z > pMax.z + getEpsilon())
1186  return false;
1187  TPlane plane;
1188  if (!getPlane(plane))
1189  throw std::logic_error("Polygon does not conform a plane");
1190  TPolygon3D projectedPoly;
1191  TPoint3D projectedPoint;
1192  TPose3D pose;
1193  // plane.getAsPose3DForcingOrigin(operator[](0),pose);
1194  plane.getAsPose3D(pose);
1195  CMatrixDouble44 P_inv;
1196  pose.getInverseHomogeneousMatrix(P_inv);
1197  pose.fromHomogeneousMatrix(P_inv);
1198  project3D(point, pose, projectedPoint);
1199  if (abs(projectedPoint.z) >= getEpsilon())
1200  return false; // Point is not inside the polygon's plane.
1201  project3D(*this, pose, projectedPoly);
1202  return TPolygon2D(projectedPoly).contains(TPoint2D(projectedPoint));
1203 }
1204 void TPolygon3D::getAsSegmentList(vector<TSegment3D>& v) const
1205 {
1206  size_t N = size();
1207  v.resize(N);
1208  for (size_t i = 0; i < N - 1; i++)
1209  v[i] = TSegment3D(operator[](i), operator[](i + 1));
1210  v[N - 1] = TSegment3D(operator[](N - 1), operator[](0));
1211 }
1212 bool TPolygon3D::getPlane(TPlane& p) const { return conformAPlane(*this, p); }
1214 {
1215  getRegressionPlane(*this, p);
1216 }
1218 {
1219  for_each(begin(), end(), FAddPoint<TPoint3D, 3>(p));
1220  size_t N = size();
1221  p.x /= N;
1222  p.y /= N;
1223  p.z /= N;
1224 }
1225 bool TPolygon3D::isSkew() const { return !mrpt::math::conformAPlane(*this); }
1228 {
1230  removeUnusedVertices(*this);
1231 }
1233 {
1234  size_t N = p.size();
1235  resize(N);
1236  for (size_t i = 0; i < N; i++) operator[](i) = p[i];
1237 }
1239  size_t numEdges, double radius, TPolygon3D& poly)
1240 {
1241  if (numEdges < 3 || abs(radius) < getEpsilon())
1242  throw std::logic_error(
1243  "Invalid arguments for regular polygon creations");
1244  poly.resize(numEdges);
1245  for (size_t i = 0; i < numEdges; i++)
1246  {
1247  double angle = i * 2 * M_PI / numEdges;
1248  poly[i] = TPoint3D(radius * cos(angle), radius * sin(angle), 0);
1249  }
1250 }
1252  size_t numEdges, double radius, TPolygon3D& poly, const TPose3D& pose)
1253 {
1254  createRegularPolygon(numEdges, radius, poly);
1255  for (size_t i = 0; i < numEdges; i++) pose.composePoint(poly[i], poly[i]);
1256 }
1257 
1259 {
1260  switch (type)
1261  {
1262  case GEOMETRIC_TYPE_POINT:
1263  obj = TPoint3D(data.point);
1264  break;
1266  obj = TSegment3D(data.segment);
1267  break;
1268  case GEOMETRIC_TYPE_LINE:
1269  obj = TLine3D(data.line);
1270  break;
1272  obj = TPolygon3D(*(data.polygon));
1273  break;
1274  default:
1275  obj = TObject3D();
1276  break;
1277  }
1278 }
1280  const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts)
1281 {
1282  for (vector<TObject2D>::const_iterator it = objs.begin(); it != objs.end();
1283  ++it)
1284  if (it->isPoint()) pnts.push_back(it->data.point);
1285 }
1287  const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms)
1288 {
1289  for (vector<TObject2D>::const_iterator it = objs.begin(); it != objs.end();
1290  ++it)
1291  if (it->isSegment()) sgms.push_back(it->data.segment);
1292 }
1294  const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins)
1295 {
1296  for (vector<TObject2D>::const_iterator it = objs.begin(); it != objs.end();
1297  ++it)
1298  if (it->isLine()) lins.push_back(it->data.line);
1299 }
1301  const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys)
1302 {
1303  for (vector<TObject2D>::const_iterator it = objs.begin(); it != objs.end();
1304  ++it)
1305  if (it->isPolygon()) polys.push_back(*(it->data.polygon));
1306 }
1308  const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts,
1309  std::vector<TObject2D>& remainder)
1310 {
1311  for (vector<TObject2D>::const_iterator it = objs.begin(); it != objs.end();
1312  ++it)
1313  if (it->isPoint())
1314  pnts.push_back(it->data.point);
1315  else
1316  remainder.push_back(*it);
1317 }
1319  const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms,
1320  std::vector<TObject2D>& remainder)
1321 {
1322  for (vector<TObject2D>::const_iterator it = objs.begin(); it != objs.end();
1323  ++it)
1324  if (it->isSegment())
1325  sgms.push_back(it->data.segment);
1326  else
1327  remainder.push_back(*it);
1328 }
1330  const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins,
1331  std::vector<TObject2D>& remainder)
1332 {
1333  for (vector<TObject2D>::const_iterator it = objs.begin(); it != objs.end();
1334  ++it)
1335  if (it->isLine())
1336  lins.push_back(it->data.line);
1337  else
1338  remainder.push_back(*it);
1339 }
1341  const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys,
1342  vector<TObject2D>& remainder)
1343 {
1344  for (vector<TObject2D>::const_iterator it = objs.begin(); it != objs.end();
1345  ++it)
1346  if (it->isPolygon())
1347  polys.push_back(*(it->data.polygon));
1348  else
1349  remainder.push_back(*it);
1350 }
1352  const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts)
1353 {
1354  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1355  ++it)
1356  if (it->isPoint()) pnts.push_back(it->data.point);
1357 }
1359  const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms)
1360 {
1361  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1362  ++it)
1363  if (it->isSegment()) sgms.push_back(it->data.segment);
1364 }
1366  const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins)
1367 {
1368  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1369  ++it)
1370  if (it->isLine()) lins.push_back(it->data.line);
1371 }
1373  const std::vector<TObject3D>& objs, std::vector<TPlane>& plns)
1374 {
1375  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1376  ++it)
1377  if (it->isPlane()) plns.push_back(it->data.plane);
1378 }
1380  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
1381 {
1382  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1383  ++it)
1384  if (it->isPolygon()) polys.push_back(*(it->data.polygon));
1385 }
1387  const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts,
1388  std::vector<TObject3D>& remainder)
1389 {
1390  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1391  ++it)
1392  if (it->isPoint())
1393  pnts.push_back(it->data.point);
1394  else
1395  remainder.push_back(*it);
1396 }
1398  const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms,
1399  std::vector<TObject3D>& remainder)
1400 {
1401  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1402  ++it)
1403  if (it->isSegment())
1404  sgms.push_back(it->data.segment);
1405  else
1406  remainder.push_back(*it);
1407 }
1409  const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins,
1410  std::vector<TObject3D>& remainder)
1411 {
1412  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1413  ++it)
1414  if (it->isLine())
1415  lins.push_back(it->data.line);
1416  else
1417  remainder.push_back(*it);
1418 }
1420  const std::vector<TObject3D>& objs, std::vector<TPlane>& plns,
1421  std::vector<TObject3D>& remainder)
1422 {
1423  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1424  ++it)
1425  if (it->isPlane())
1426  plns.push_back(it->data.plane);
1427  else
1428  remainder.push_back(*it);
1429 }
1431  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
1432  vector<TObject3D>& remainder)
1433 {
1434  for (vector<TObject3D>::const_iterator it = objs.begin(); it != objs.end();
1435  ++it)
1436  if (it->isPolygon())
1437  polys.push_back(*(it->data.polygon));
1438  else
1439  remainder.push_back(*it);
1440 }
1441 
1443 {
1444  for (unsigned int i = 0; i < o.size(); i++) in >> o[i];
1445  return in;
1446 }
1448 {
1449  for (unsigned int i = 0; i < o.size(); i++) out << o[i];
1450  return out;
1451 }
1452 
1454 {
1455  for (unsigned int i = 0; i < o.size(); i++) in >> o[i];
1456  return in;
1457 }
1459 {
1460  for (unsigned int i = 0; i < o.size(); i++) out << o[i];
1461  return out;
1462 }
1463 
1465 {
1466  uint16_t type;
1467  in >> type;
1468  switch (static_cast<unsigned char>(type))
1469  {
1470  case GEOMETRIC_TYPE_POINT:
1471  {
1472  TPoint2D p;
1473  in >> p;
1474  o = p;
1475  }
1476  break;
1478  {
1479  TSegment2D s;
1480  in >> s;
1481  o = s;
1482  }
1483  break;
1484  case GEOMETRIC_TYPE_LINE:
1485  {
1486  TLine2D l;
1487  in >> l;
1488  o = l;
1489  }
1490  break;
1492  {
1493  TPolygon2D p;
1494  in >> p;
1495  o = p;
1496  }
1497  break;
1499  {
1500  o = TObject2D();
1501  }
1502  break;
1503  default:
1504  throw std::logic_error(
1505  "Unknown TObject2D type found while reading stream");
1506  }
1507  return in;
1508 }
1510 {
1511  out << static_cast<uint16_t>(o.getType());
1512  switch (o.getType())
1513  {
1514  case GEOMETRIC_TYPE_POINT:
1515  {
1516  TPoint2D p;
1517  o.getPoint(p);
1518  return out << p;
1519  };
1521  {
1522  TSegment2D s;
1523  o.getSegment(s);
1524  return out << s;
1525  };
1526  case GEOMETRIC_TYPE_LINE:
1527  {
1528  TLine2D l;
1529  o.getLine(l);
1530  return out << l;
1531  };
1533  {
1534  TPolygon2D p;
1535  o.getPolygon(p);
1536  return out << p;
1537  };
1538  }
1539  return out;
1540 }
1541 
1543 {
1544  uint16_t type;
1545  in >> type;
1546  switch (static_cast<unsigned char>(type))
1547  {
1548  case GEOMETRIC_TYPE_POINT:
1549  {
1550  TPoint3D p;
1551  in >> p;
1552  o = p;
1553  }
1554  break;
1556  {
1557  TSegment3D s;
1558  in >> s;
1559  o = s;
1560  }
1561  break;
1562  case GEOMETRIC_TYPE_LINE:
1563  {
1564  TLine3D l;
1565  in >> l;
1566  o = l;
1567  }
1568  break;
1569  case GEOMETRIC_TYPE_PLANE:
1570  {
1571  TPlane p;
1572  in >> p;
1573  o = p;
1574  }
1575  break;
1577  {
1578  TPolygon3D p;
1579  in >> p;
1580  o = p;
1581  }
1582  break;
1584  {
1585  o = TObject3D();
1586  }
1587  break;
1588  default:
1589  throw std::logic_error(
1590  "Unknown TObject3D type found while reading stream");
1591  }
1592  return in;
1593 }
1595 {
1596  out << static_cast<uint16_t>(o.getType());
1597  switch (o.getType())
1598  {
1599  case GEOMETRIC_TYPE_POINT:
1600  {
1601  TPoint3D p;
1602  o.getPoint(p);
1603  return out << p;
1604  };
1606  {
1607  TSegment3D s;
1608  o.getSegment(s);
1609  return out << s;
1610  };
1611  case GEOMETRIC_TYPE_LINE:
1612  {
1613  TLine3D l;
1614  o.getLine(l);
1615  return out << l;
1616  };
1617  case GEOMETRIC_TYPE_PLANE:
1618  {
1619  TPlane p;
1620  o.getPlane(p);
1621  return out << p;
1622  };
1624  {
1625  TPolygon3D p;
1626  o.getPolygon(p);
1627  return out << p;
1628  };
1629  }
1630  return out;
1631 }
1632 } // namespace math
1633 } // namespace mrpt
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:328
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.
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 fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04]" ) ...
static constexpr unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
EIGEN_STRONG_INLINE bool empty() const
GLdouble GLdouble t
Definition: glext.h:3689
#define MRPT_MAX_ALIGN_BYTES
GLdouble GLdouble z
Definition: glext.h:3872
#define min(a, b)
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:44
double RAD2DEG(const double x)
Radians to degrees.
unsigned char getType() const
Gets content type.
double x
X,Y coordinates.
bool operator==(const TTwist3D &o) const
double x
X,Y coordinates.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &R) const
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
double distance(const TPoint3D &point) const
Distance between the line and a point.
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.
TLine2D()
Fast default constructor.
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
bool contains(const TPoint2D &point) const
Check whether a point is inside a segment.
double roll
Roll coordinate (rotation angle over X coordinate).
void getAsPose3D(mrpt::math::TPose3D &outPose)
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.
TPolygon3D()
Default constructor.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
GLenum GLsizei n
Definition: glext.h:5074
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...
double x
X,Y,Z, coords.
mrpt::math::TPoint2D composePoint(const TPoint2D l) const
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:29
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
TPoint3D()
Default fast constructor.
double signedDistance(const TPoint2D &point) const
Distance with sign from a given point (sign indicates side).
Standard type for storing any lightweight 2D type.
TLine3D()
Fast default constructor.
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
void getAsSegmentList(std::vector< TSegment2D > &v) const
Gets as set of segments, instead of points.
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.
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).
GLdouble s
Definition: glext.h:3676
void composePoint(const TPoint3D l, TPoint3D &g) const
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
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]" ) ...
static constexpr size_t size()
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:199
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
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:803
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
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.
TPose3D()
Default fast constructor.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
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
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)
void unitarize()
Unitarize line&#39;s normal vector.
std::string asString() const
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.
static constexpr unsigned char GEOMETRIC_TYPE_UNDEFINED
Object type identifier for empty TObject2D or TObject3D.
2D segment, consisting of two points.
double distance(const TPoint3D &point) const
Distance to point.
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:6362
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.
double length() const
Segment length.
void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44 &HG)
TPoint3D point2
Destiny point.
bool operator<(const TSegment2D &s) const
std::ostream & operator<<(std::ostream &o, const TPoint2D &p)
const GLubyte * c
Definition: glext.h:6313
double vx
Velocity components: X,Y (m/s)
mrpt::math::TPose2D operator+(const mrpt::math::TPose2D &b) const
Operator "oplus" pose composition: "ret=this \oplus b".
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:3528
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:6279
3D Plane, represented by its equation
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon.
GLubyte GLubyte b
Definition: glext.h:6279
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
TPolygon2D()
Default constructor.
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
void removeRedundantVertices()
Erase every redundant vertex, thus saving space.
TPoint2D point2
Destiny point.
void inverseComposePoint(const TPoint3D g, TPoint3D &l) const
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:859
GLsizei const GLchar ** string
Definition: glext.h:4101
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
void unitarize()
Unitarize director vector.
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.
double pitch
Pitch coordinate (rotation angle over Y axis).
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
double distance(const TPoint3D &point) const
Distance to point (always >=0)
double director[3]
Director vector.
TPoint2D point1
Origin point.
bool isConvex() const
Checks whether is convex.
TPoint2D()
Default fast constructor.
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:208
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)
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void getAsPose2DForcingOrigin(const TPoint2D &origin, TPose2D &outPose) const
TPlane()
Fast default constructor.
double wx
Angular velocity (rad/s)
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
double vx
Velocity components: X,Y (m/s)
void getAsPose3DForcingOrigin(const TPoint3D &newOrigin, TPose3D &pose)
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:2021
void getUnitaryNormalVector(double(&vec)[3])
Unitarize, then get normal vector.
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
TSegment2D()
Fast default constructor.
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 TTwist3D &o) const
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
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.
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane&#39;s equation.
std::string asString() 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 TPoint3D &p) const
double evaluatePoint(const TPoint2D &point) const
Evaluate point in the line&#39;s equation.
GLenum GLint GLint y
Definition: glext.h:3538
TPose2D()
Default fast constructor.
GLfloat GLfloat GLfloat v2
Definition: glext.h:4107
unsigned char getType() const
Gets object type.
mrpt::math::TPose2D operator-(const mrpt::math::TPose2D &b) const
Operator "ominus" pose composition: "ret=this \ominus b".
GLuint res
Definition: glext.h:7268
static constexpr unsigned char GEOMETRIC_TYPE_POLYGON
Object type identifier for TPolygon2D or TPolygon3D.
GLenum GLint x
Definition: glext.h:3538
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:46
Lightweight 3D point.
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.
std::string asString() const
Lightweight 2D point.
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
std::string asString() const
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305
TPose3D operator-(const TPose3D &p)
Unary $$ operator: computes inverse SE(3) element.
static void getPlanes(const std::vector< TObject3D > &objs, std::vector< TPlane > &plns)
Static method to retrieve every plane included in a vector of objects.
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrix::Ptr &pObj)
double phi
Orientation (rads)
bool operator<(const TPoint2D &p) const
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3528
static constexpr unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
void getAsVector(VECTORLIKE &v) const
Transformation into vector.
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 getCenter(TPoint3D &p) const
Get polygon&#39;s central point.
double omega
Angular velocity (rad/s)
bool contains(const TPoint3D &point) const
Check whether a point is contained into the plane.
double length() const
Segment length.
bool operator!=(const TTwist2D &o) 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
void getNormalVector(double(&vec)[3]) const
Get plane&#39;s normal vector.
3D line, represented by a base point and a director vector.
void composePose(const TPose3D other, TPose3D &result) const
2D line without bounds, represented by its equation .
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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019