MRPT  2.0.3
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 }
160 TPolygon2D::TPolygon2D(const TPolygon3D& p) : std::vector<TPoint2D>()
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 }
mrpt::math::TPolygon2D::createRegularPolygon
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
mrpt::containers::end
const_iterator end() const
Definition: ts_hash_map.h:246
mrpt::math::removeUnusedVertices
void removeUnusedVertices(T &poly)
Definition: polygons_utils.h:33
polygons_utils.h
isLeft
double isLeft(const mrpt::math::TPoint2D &P0, const mrpt::math::TPoint2D &P1, const mrpt::math::TPoint2D &P2)
Definition: TPolygon2D.cpp:62
mrpt::math::FAddPoint
Auxiliary functor class to compute polygon's center.
Definition: polygons_utils.h:18
mrpt::containers::operator[]
VALUE & operator[](const KEY &key)
Write/read via [i] operator, that creates an element if it didn't exist already.
Definition: ts_hash_map.h:216
mrpt::math::TPoint2D_< double >
resize
images resize(NUM_IMGS)
math-precomp.h
mrpt::containers::empty
bool empty() const
Definition: ts_hash_map.h:191
TPose2D.h
mrpt::math::TPolygon2D::getAsSegmentList
void getAsSegmentList(std::vector< TSegment2D > &v) const
Gets as set of segments, instead of points.
Definition: TPolygon2D.cpp:97
mrpt::math::TPolygon2D::contains
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
Definition: TPolygon2D.cpp:69
mrpt::math::TPolygon2D::removeRepeatedVertices
void removeRepeatedVertices()
Erase repeated vertices.
Definition: TPolygon2D.cpp:140
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
TPolygon3D.h
mrpt::math::TPolygon2D::distance
double distance(const TPoint2D &point) const
Distance to a point (always >=0)
Definition: TPolygon2D.cpp:22
TLine2D.h
mrpt::containers::begin
const_iterator begin() const
Definition: ts_hash_map.h:240
mrpt::math::TPolygon2D::isConvex
bool isConvex() const
Checks whether is convex.
Definition: TPolygon2D.cpp:117
TPolygon2D.h
mrpt::math::TPolygon2D::TPolygon2D
TPolygon2D()
Default constructor
Definition: TPolygon2D.h:48
mrpt::math::getEpsilon
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
TSegment2D.h
mrpt::math::size
size_t size(const MATRIXLIKE &m, const int dim)
Definition: math/include/mrpt/math/bits_math.h:21
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::keep_max
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.
Definition: core/include/mrpt/core/bits_math.h:152
mrpt::keep_min
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.
Definition: core/include/mrpt/core/bits_math.h:145
mrpt::math::TPoint2D_data::y
T y
Definition: TPoint2D.h:25
mrpt::math::TPolygon2D
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
mrpt::math::TPolygon3D
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:20
mrpt::math::TSegment2D
2D segment, consisting of two points.
Definition: TSegment2D.h:20
mrpt::math::TPolygon2D::removeRedundantVertices
void removeRedundantVertices()
Erase every redundant vertex from the polygon, saving space.
Definition: TPolygon2D.cpp:141
mrpt::math::TPolygon2D::getPlotData
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
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::math::TPolygon2D::getCenter
void getCenter(TPoint2D &p) const
Polygon's central point.
Definition: TPolygon2D.cpp:110
mrpt::math::TPoint2D_data::x
T x
X,Y coordinates.
Definition: TPoint2D.h:25
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::math::removeRepVertices
void removeRepVertices(T &poly)
Definition: polygons_utils.h:68
mrpt::math::TPoint2D
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::TLine2D
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19
mrpt::math::TPolygon2D::getBoundingBox
void getBoundingBox(TPoint2D &min_coords, TPoint2D &max_coords) const
Get polygon bounding box.
Definition: TPolygon2D.cpp:41
mrpt::math::TPose2D::composePoint
mrpt::math::TPoint2D composePoint(const TPoint2D l) const
Definition: TPose2D.cpp:64
epsilon.h
mrpt::math::TPolygon2D::generate3DObject
void generate3DObject(TPolygon3D &p) const
Projects into 3D space, zeroing the z.
Definition: TPolygon2D.cpp:106



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020