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