MRPT  2.0.5
TSegment3D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9
10 #include "math-precomp.h" // Precompiled headers
11
12 #include <mrpt/math/TLine3D.h>
13 #include <mrpt/math/TSegment3D.h>
14 #include <mrpt/math/geometry.h> // distance()
15 #include <mrpt/serialization/CArchive.h> // impl of << operator
16 #include <Eigen/Dense>
17
18 using namespace mrpt::math;
19
21 {
22  s = TSegment2D(*this);
23 }
24
25 double TSegment3D::length() const { return math::distance(point1, point2); }
26 double TSegment3D::distance(const TPoint3D& point) const
27 {
28  return std::min(
29  std::min(math::distance(point, point1), math::distance(point, point2)),
30  TLine3D(*this).distance(point));
31 }
32 double TSegment3D::distance(const TSegment3D& segment) const
33 {
34  Eigen::Vector3d u, v, w;
35  TPoint3D diff_vect = point2 - point1;
36  diff_vect.asVector(u);
37  diff_vect = segment.point2 - segment.point1;
38  diff_vect.asVector(v);
39  diff_vect = point1 - segment.point1;
40  diff_vect.asVector(w);
41  double a = u.dot(u); // always >= 0
42  double b = u.dot(v);
43  double c = v.dot(v); // always >= 0
44  double d = u.dot(w);
45  double e = v.dot(w);
46  double D = a * c - b * b; // always >= 0
47  double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
48  double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
49
50  // compute the line parameters of the two closest points
51  if (D < 0.00000001)
52  { // the lines are almost parallel
53  sN = 0.0; // force using point P0 on segment S1
54  sD = 1.0; // to prevent possible division by 0.0 later
55  tN = e;
56  tD = c;
57  }
58  else
59  { // get the closest points on the infinite lines
60  sN = (b * e - c * d);
61  tN = (a * e - b * d);
62  if (sN < 0.0)
63  { // sc < 0 => the s=0 edge is visible
64  sN = 0.0;
65  tN = e;
66  tD = c;
67  }
68  else if (sN > sD)
69  { // sc > 1 => the s=1 edge is visible
70  sN = sD;
71  tN = e + b;
72  tD = c;
73  }
74  }
75
76  if (tN < 0.0)
77  { // tc < 0 => the t=0 edge is visible
78  tN = 0.0;
79  // recompute sc for this edge
80  if (-d < 0.0)
81  sN = 0.0;
82  else if (-d > a)
83  sN = sD;
84  else
85  {
86  sN = -d;
87  sD = a;
88  }
89  }
90  else if (tN > tD)
91  { // tc > 1 => the t=1 edge is visible
92  tN = tD;
93  // recompute sc for this edge
94  if ((-d + b) < 0.0)
95  sN = 0;
96  else if ((-d + b) > a)
97  sN = sD;
98  else
99  {
100  sN = (-d + b);
101  sD = a;
102  }
103  }
104  // finally do the division to get sc and tc
105  sc = (fabs(sN) < 0.00000001 ? 0.0 : sN / sD);
106  tc = (fabs(tN) < 0.00000001 ? 0.0 : tN / tD);
107
108  // get the difference of the two closest points
109  const auto dP = (w + (sc * u) - (tc * v)).eval();
110  return dP.norm(); // return the closest distance
111 }
112 bool TSegment3D::contains(const TPoint3D& point) const
113 {
114  // Not very intuitive, but very fast, method.
115  return std::abs(
116  math::distance(point1, point) + math::distance(point2, point) -
118 }
119
120 bool TSegment3D::operator<(const TSegment3D& s) const
121 {
122  if (point1 < s.point1)
123  return true;
124  else if (s.point1 < point1)
125  return false;
126  else
127  return point2 < s.point2;
128 }
129
132 {
133  return in >> s.point1 >> s.point2;
134 }
137 {
138  return out << s.point1 << s.point2;
139 }
bool contains(const TPoint3D &point) const
Check whether a point is inside the segment.
Definition: TSegment3D.cpp:112
void asVector(VECTORLIKE &v) const
Transformation into vector.
Definition: TPoint3D.h:174
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
TPoint3D point1
origin point
Definition: TSegment3D.h:23
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
Definition: TSegment2D.h:20
3D segment, consisting of two points.
Definition: TSegment3D.h:20
TPoint3D point2
final point
Definition: TSegment3D.h:24
double length() const
Segment length.
Definition: TSegment3D.cpp:25
bool operator<(const TSegment3D &s) const
Definition: TSegment3D.cpp:120
double distance(const TPoint3D &point) const
Distance to point.
Definition: TSegment3D.cpp:26
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
mrpt::vision::TStereoCalibResults out
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:626
double distance(const TPoint3D &point) const
Distance between the line and a point.
Definition: TLine3D.cpp:54
void generate2DObject(TSegment2D &s) const
Projection into 2D space, discarding the z.
Definition: TSegment3D.cpp:20
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19

 Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: b26ce1112 Mon Jul 6 09:39:08 2020 +0200 at lun jul 6 09:45:13 CEST 2020 