MRPT  2.0.4
TLine2D.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/TLine2D.h>
13 #include <mrpt/math/TLine3D.h>
14 #include <mrpt/math/TPoint2D.h>
15 #include <mrpt/math/TPose2D.h>
16 #include <mrpt/math/TSegment2D.h>
17 #include <mrpt/math/epsilon.h>
18 #include <mrpt/serialization/CArchive.h> // impl of << operator
19 #include <cmath>
20
21 using namespace mrpt::math;
22
23 static_assert(std::is_trivially_copyable_v<TLine2D>);
24
25 double TLine2D::evaluatePoint(const TPoint2D& point) const
26 {
27  return coefs[0] * point.x + coefs[1] * point.y + coefs[2];
28 }
29 bool TLine2D::contains(const TPoint2D& point) const
30 {
31  return std::abs(distance(point)) < getEpsilon();
32 }
33 double TLine2D::distance(const TPoint2D& point) const
34 {
35  return std::abs(evaluatePoint(point)) /
36  sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
37 }
38 double TLine2D::signedDistance(const TPoint2D& point) const
39 {
40  return evaluatePoint(point) /
41  sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
42 }
43 void TLine2D::getNormalVector(double (&vector)[2]) const
44 {
45  vector[0] = coefs[0];
46  vector[1] = coefs[1];
47 }
49 {
50  double s = sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
51  for (double& coef : coefs) coef /= s;
52 }
53 void TLine2D::getDirectorVector(double (&vector)[2]) const
54 {
55  vector[0] = -coefs[1];
56  vector[1] = coefs[0];
57 }
58 void TLine2D::generate3DObject(TLine3D& l) const { l = TLine3D(*this); }
59 void TLine2D::getAsPose2D(TPose2D& outPose) const
60 {
61  // Line's director vector is (-coefs[1],coefs[0]).
62  // If line is horizontal, force x=0. Else, force y=0. In both cases, we'll
63  // find a suitable point.
64  outPose.phi = atan2(coefs[0], -coefs[1]);
65  if (std::abs(coefs[0]) < getEpsilon())
66  {
67  outPose.x = 0;
68  outPose.y = -coefs[2] / coefs[1];
69  }
70  else
71  {
72  outPose.x = -coefs[2] / coefs[0];
73  outPose.y = 0;
74  }
75 }
77  const TPoint2D& origin, TPose2D& outPose) const
78 {
79  if (!contains(origin))
80  throw std::logic_error("Base point is not contained in the line");
81  outPose = origin;
82  // Line's director vector is (-coefs[1],coefs[0]).
83  outPose.phi = atan2(coefs[0], -coefs[1]);
84 }
85 TLine2D::TLine2D(const TPoint2D& p1, const TPoint2D& p2)
86 {
87  if (p1 == p2) throw std::logic_error("Both points are the same");
88  coefs[0] = p2.y - p1.y;
89  coefs[1] = p1.x - p2.x;
90  coefs[2] = p2.x * p1.y - p2.y * p1.x;
91 }
93 {
94  coefs[0] = s.point2.y - s.point1.y;
95  coefs[1] = s.point1.x - s.point2.x;
96  coefs[2] = s.point2.x * s.point1.y - s.point2.y * s.point1.x;
97  // unitarize(); //¿?
98 }
100 {
101  // Line's projection to Z plane may be a point.
102  if (hypot(l.director[0], l.director[1]) < getEpsilon())
103  throw std::logic_error("Line is normal to projection plane");
104  coefs[0] = -l.director[1];
105  coefs[1] = l.director[0];
106  coefs[2] = l.pBase.x * l.director[1] - l.pBase.y * l.director[0];
107 }
108
111 {
112  return in >> l.coefs[0] >> l.coefs[1] >> l.coefs[2];
113 }
116 {
117  return out << l.coefs[0] << l.coefs[1] << l.coefs[2];
118 }
void getDirectorVector(double(&vector)[2]) const
Get line&#39;s director vector.
Definition: TLine2D.cpp:53
double x
X,Y coordinates.
Definition: TPose2D.h:30
T x
X,Y coordinates.
Definition: TPoint2D.h:25
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
void unitarize()
Unitarize line&#39;s normal vector.
Definition: TLine2D.cpp:48
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
Definition: TSegment2D.h:20
double evaluatePoint(const TPoint2D &point) const
Evaluate point in the line&#39;s equation.
Definition: TLine2D.cpp:25
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
void generate3DObject(TLine3D &l) const
Project into 3D space, setting the z to 0.
Definition: TLine2D.cpp:58
void getAsPose2D(TPose2D &outPose) const
Definition: TLine2D.cpp:59
double distance(const TPoint2D &point) const
Distance from a given point.
Definition: TLine2D.cpp:33
void getAsPose2DForcingOrigin(const TPoint2D &origin, TPose2D &outPose) const
Definition: TLine2D.cpp:76
TPoint2D point2
Destiny point.
Definition: TSegment2D.h:30
TPoint2D point1
Origin point.
Definition: TSegment2D.h:26
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:23
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
Lightweight 2D pose.
Definition: TPose2D.h:22
bool contains(const TPoint2D &point) const
Check whether a point is inside the line.
Definition: TLine2D.cpp:29
TLine2D()=default
Fast default constructor.
double phi
Definition: TPose2D.h:32
void getNormalVector(double(&vector)[2]) const
Get line&#39;s normal vector.
Definition: TLine2D.cpp:43
double signedDistance(const TPoint2D &point) const