MRPT  1.9.9
TPolygon2D.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 |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/TLine2D.h>
13 #include <mrpt/math/TPolygon2D.h>
14 #include <mrpt/math/TPolygon3D.h>
15 #include <mrpt/math/TPose2D.h>
16 #include <mrpt/math/TSegment2D.h>
17 #include <mrpt/math/epsilon.h>
18 #include "polygons_utils.h"
19 
20 using namespace mrpt::math;
21 
22 double TPolygon2D::distance(const TPoint2D& point) const
23 {
24  if (contains(point)) return 0;
25  std::vector<TSegment2D> sgs;
26  getAsSegmentList(sgs);
27 
28  if (sgs.empty())
29  THROW_EXCEPTION("Cannot compute distance to an empty polygon.");
30 
31  double distance = std::numeric_limits<double>::max();
32 
33  for (auto it = sgs.begin(); it != sgs.end(); ++it)
34  {
35  double d = (*it).distance(point);
36  if (d < distance) distance = d;
37  }
38  return distance;
39 }
40 
42  TPoint2D& min_coords, TPoint2D& max_coords) const
43 {
44  ASSERTMSG_(!this->empty(), "getBoundingBox() called on an empty polygon!");
45  min_coords.x = min_coords.y = std::numeric_limits<double>::max();
46  max_coords.x = max_coords.y = -std::numeric_limits<double>::max();
47  for (size_t i = 0; i < size(); i++)
48  {
49  mrpt::keep_min(min_coords.x, (*this)[i].x);
50  mrpt::keep_min(min_coords.y, (*this)[i].y);
51  mrpt::keep_max(max_coords.x, (*this)[i].x);
52  mrpt::keep_max(max_coords.y, (*this)[i].y);
53  }
54 }
55 
56 // isLeft(): tests if a point is Left|On|Right of an infinite line.
57 // Input: three points P0, P1, and P2
58 // Return: >0 for P2 left of the line through P0 and P1
59 // =0 for P2 on the line
60 // <0 for P2 right of the line
61 // See: Algorithm 1 "Area of Triangles and Polygons"
62 inline double isLeft(
63  const mrpt::math::TPoint2D& P0, const mrpt::math::TPoint2D& P1,
64  const mrpt::math::TPoint2D& P2)
65 {
66  return ((P1.x - P0.x) * (P2.y - P0.y) - (P2.x - P0.x) * (P1.y - P0.y));
67 }
68 
69 bool TPolygon2D::contains(const TPoint2D& P) const
70 {
71  int wn = 0; // the winding number counter
72 
73  // loop through all edges of the polygon
74  const size_t n = this->size();
75  for (size_t i = 0; i < n; i++) // edge from V[i] to V[i+1]
76  {
77  if ((*this)[i].y <= P.y)
78  {
79  // start y <= P.y
80  if ((*this)[(i + 1) % n].y > P.y) // an upward crossing
81  if (isLeft((*this)[i], (*this)[(i + 1) % n], P) >
82  0) // P left of edge
83  ++wn; // have a valid up intersect
84  }
85  else
86  {
87  // start y > P.y (no test needed)
88  if ((*this)[(i + 1) % n].y <= P.y) // a downward crossing
89  if (isLeft((*this)[i], (*this)[(i + 1) % n], P) <
90  0) // P right of edge
91  --wn; // have a valid down intersect
92  }
93  }
94 
95  return wn != 0;
96 }
97 void TPolygon2D::getAsSegmentList(vector<TSegment2D>& v) const
98 {
99  size_t N = size();
100  v.resize(N);
101  for (size_t i = 0; i < N - 1; i++)
102  v[i] = TSegment2D(operator[](i), operator[](i + 1));
103  v[N - 1] = TSegment2D(operator[](N - 1), operator[](0));
104 }
105 
107 {
108  p = TPolygon3D(*this);
109 }
111 {
112  for_each(begin(), end(), FAddPoint<TPoint2D, 2>(p));
113  size_t N = size();
114  p.x /= N;
115  p.y /= N;
116 }
118 {
119  size_t N = size();
120  if (N <= 3) return true;
121  vector<TSegment2D> sgms;
122  getAsSegmentList(sgms);
123  for (size_t i = 0; i < N; i++)
124  {
125  char s = 0;
126  auto l = TLine2D(sgms[i]);
127  for (size_t j = 0; j < N; j++)
128  {
129  double d = l.evaluatePoint(operator[](j));
130  if (std::abs(d) < getEpsilon())
131  continue;
132  else if (!s)
133  s = (d > 0) ? 1 : -1;
134  else if (s != ((d > 0) ? 1 : -1))
135  return false;
136  }
137  }
138  return true;
139 }
142 {
144  removeUnusedVertices(*this);
145 }
147  std::vector<double>& x, std::vector<double>& y) const
148 {
149  size_t N = size();
150  x.resize(N + 1);
151  y.resize(N + 1);
152  for (size_t i = 0; i < N; i++)
153  {
154  x[i] = operator[](i).x;
155  y[i] = operator[](i).y;
156  }
157  x[N] = operator[](0).x;
158  y[N] = operator[](0).y;
159 }
161 {
162  size_t N = p.size();
163  resize(N);
164  for (size_t i = 0; i < N; i++) operator[](i) = TPoint2D(p[i]);
165 }
167  size_t numEdges, double radius, TPolygon2D& poly)
168 {
169  if (numEdges < 3 || std::abs(radius) < getEpsilon())
170  throw std::logic_error(
171  "Invalid arguments for regular polygon creations");
172  poly.resize(numEdges);
173  for (size_t i = 0; i < numEdges; i++)
174  {
175  double angle = i * M_PI * 2 / numEdges;
176  poly[i] = TPoint2D(radius * cos(angle), radius * sin(angle));
177  }
178 }
179 
181  size_t numEdges, double radius, TPolygon2D& poly, const TPose2D& pose)
182 {
183  createRegularPolygon(numEdges, radius, poly);
184  for (size_t i = 0; i < numEdges; i++) poly[i] = pose.composePoint(poly[i]);
185 }
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...
Auxiliary functor class to compute polygon&#39;s center.
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
T x
X,Y coordinates.
Definition: TPoint2D.h:25
size_t size(const MATRIXLIKE &m, const int dim)
static void createRegularPolygon(size_t numEdges, double radius, TPolygon2D &poly)
Static method to create a regular polygon, given its size and radius.
Definition: TPolygon2D.cpp:166
void getCenter(TPoint2D &p) const
Polygon&#39;s central point.
Definition: TPolygon2D.cpp:110
STL namespace.
void removeRepVertices(T &poly)
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
Definition: TPolygon2D.cpp:69
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:216
void removeRepeatedVertices()
Erase repeated vertices.
Definition: TPolygon2D.cpp:140
void removeRedundantVertices()
Erase every redundant vertex from the polygon, saving space.
Definition: TPolygon2D.cpp:141
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
Definition: TSegment2D.h:20
void getAsSegmentList(std::vector< TSegment2D > &v) const
Gets as set of segments, instead of points.
Definition: TPolygon2D.cpp:97
bool isConvex() const
Checks whether is convex.
Definition: TPolygon2D.cpp:117
double distance(const TPoint2D &point) const
Distance to a point (always >=0)
Definition: TPolygon2D.cpp:22
void getPlotData(std::vector< double > &x, std::vector< double > &y) const
Gets plot data, ready to use on a 2D plot.
Definition: TPolygon2D.cpp:146
TPolygon2D()
Default constructor.
Definition: TPolygon2D.h:48
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
void getBoundingBox(TPoint2D &min_coords, TPoint2D &max_coords) const
Get polygon bounding box.
Definition: TPolygon2D.cpp:41
bool empty() const
Definition: ts_hash_map.h:191
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...
double isLeft(const mrpt::math::TPoint2D &P0, const mrpt::math::TPoint2D &P1, const mrpt::math::TPoint2D &P2)
Definition: TPolygon2D.cpp:62
void removeUnusedVertices(T &poly)
const_iterator end() const
Definition: ts_hash_map.h:246
const_iterator begin() const
Definition: ts_hash_map.h:240
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
mrpt::math::TPoint2D composePoint(const TPoint2D l) const
Definition: TPose2D.cpp:64
Lightweight 2D pose.
Definition: TPose2D.h:22
void generate3DObject(TPolygon3D &p) const
Projects into 3D space, zeroing the z.
Definition: TPolygon2D.cpp:106
images resize(NUM_IMGS)
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:20
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020