PolygonTest.cpp
Go to the documentation of this file.
1 /*
2  * PolygonTest.cpp
3  *
4  * Created on: Mar 24, 2015
5  * Author: Martin Wermelinger, Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
9 #include "grid_map_core/Polygon.hpp"
10 
11 // gtest
12 #include <gtest/gtest.h>
13 
14 // Eigen
15 #include <Eigen/Core>
16 
17 using Eigen::MatrixXd;
18 using Eigen::Vector2d;
19 using Eigen::VectorXd;
20 
21 using grid_map::Length;
22 using grid_map::Polygon;
23 using grid_map::Position;
24 
25 TEST(Polygon, getCentroidTriangle)
26 {
27  Polygon triangle;
28  triangle.addVertex(Vector2d(0.0, 0.0));
29  triangle.addVertex(Vector2d(1.0, 0.0));
30  triangle.addVertex(Vector2d(0.5, 1.0));
31 
32  Position expectedCentroid;
33  expectedCentroid.x() = 1.0 / 3.0 * (1.0 + 0.5);
34  expectedCentroid.y() = 1.0 / 3.0;
35  Position centroid = triangle.getCentroid();
36  EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
37  EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
38 }
39 
40 TEST(Polygon, getCentroidRectangle)
41 {
42  Polygon rectangle;
43  rectangle.addVertex(Vector2d(-2.0, -1.0));
44  rectangle.addVertex(Vector2d(-2.0, 2.0));
45  rectangle.addVertex(Vector2d(1.0, 2.0));
46  rectangle.addVertex(Vector2d(1.0, -1.0));
47 
48  Position expectedCentroid(-0.5, 0.5);
49  Position centroid = rectangle.getCentroid();
50  EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
51  EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
52 }
53 
54 TEST(Polygon, getBoundingBox)
55 {
56  Polygon triangle;
57  triangle.addVertex(Vector2d(0.0, 0.0));
58  triangle.addVertex(Vector2d(0.5, -1.2));
59  triangle.addVertex(Vector2d(1.0, 0.0));
60 
61  Position expectedCenter(0.5, -0.6);
62  Length expectedLength(1.0, 1.2);
63  Position center;
64  Length length;
65  triangle.getBoundingBox(center, length);
66 
67  EXPECT_DOUBLE_EQ(expectedCenter.x(), center.x());
68  EXPECT_DOUBLE_EQ(expectedCenter.y(), center.y());
69  EXPECT_DOUBLE_EQ(expectedLength.x(), length.x());
70  EXPECT_DOUBLE_EQ(expectedLength.y(), length.y());
71 }
72 
73 TEST(Polygon, convexHullPoints)
74 {
75  // Test that points which already create a convex shape (square) can be used to create a convex polygon.
76  std::vector<Position> points1;
77  points1.emplace_back(0.0, 0.0);
78  points1.emplace_back(1.0, 0.0);
79  points1.emplace_back(1.0, 1.0);
80  points1.emplace_back(0.0, 1.0);
81  Polygon polygon1 = Polygon::monotoneChainConvexHullOfPoints(points1);
82  EXPECT_EQ(4, polygon1.nVertices());
83  EXPECT_TRUE(polygon1.isInside(Vector2d(0.5, 0.5)));
84  EXPECT_FALSE(polygon1.isInside(Vector2d(-0.01, 0.5)));
85 
86  // Test that a random set of points can be used to create a convex polygon.
87  std::vector<Position> points2;
88  points2.emplace_back(0.0, 0.0);
89  points2.emplace_back(1.0, 0.0);
90  points2.emplace_back(2.0, 1.0);
91  points2.emplace_back(1.0, 2.0);
92  points2.emplace_back(-1.0, 2.0);
93  points2.emplace_back(-1.0, -2.0);
94  points2.emplace_back(0.0, 1.0);
95  points2.emplace_back(1.0, 1.0);
96  Polygon polygon2 = Polygon::monotoneChainConvexHullOfPoints(points2);
97  EXPECT_EQ(4, polygon2.nVertices());
98  EXPECT_TRUE(polygon2.isInside(Vector2d(0.5, 0.5)));
99  EXPECT_TRUE(polygon2.isInside(Vector2d(0.0, 1.0)));
100  EXPECT_TRUE(polygon2.isInside(Vector2d(-0.5, -0.5)));
101  EXPECT_FALSE(polygon2.isInside(Vector2d(2.0, 0.0)));
102  EXPECT_FALSE(polygon2.isInside(Vector2d(-0.5, -2)));
103  EXPECT_FALSE(polygon2.isInside(Vector2d(1.75, 1.75)));
104 }
105 
106 TEST(Polygon, convexHullPolygon)
107 {
108  Polygon polygon1;
109  polygon1.addVertex(Vector2d(0.0, 0.0));
110  polygon1.addVertex(Vector2d(1.0, 1.0));
111  polygon1.addVertex(Vector2d(0.0, 1.0));
112  polygon1.addVertex(Vector2d(1.0, 0.0));
113 
114  Polygon polygon2;
115  polygon2.addVertex(Vector2d(0.5, 0.5));
116  polygon2.addVertex(Vector2d(0.5, 1.5));
117  polygon2.addVertex(Vector2d(1.5, 0.5));
118  polygon2.addVertex(Vector2d(1.5, 1.5));
119 
120  Polygon hull = Polygon::convexHull(polygon1, polygon2);
121 
122  EXPECT_EQ(6, hull.nVertices());
123  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.5)));
124  EXPECT_FALSE(hull.isInside(Vector2d(0.01, 1.49)));
125 }
126 
127 TEST(Polygon, convexHullCircles)
128 {
129  Position center1(0.0, 0.0);
130  Position center2(1.0, 0.0);
131  double radius = 0.5;
132  const int nVertices = 15;
133 
134  Polygon hull = Polygon::convexHullOfTwoCircles(center1, center2, radius);
135  EXPECT_EQ(20, hull.nVertices());
136  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
137  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
138  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
139  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
140  EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));
141 
142  hull = Polygon::convexHullOfTwoCircles(center1, center2, radius, nVertices);
143  EXPECT_EQ(nVertices + 1, hull.nVertices());
144  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
145  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
146  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
147  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
148  EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));
149 
150  hull = Polygon::convexHullOfTwoCircles(center1, center1, radius);
151  EXPECT_EQ(20, hull.nVertices());
152  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
153  EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
154  EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
155  EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
156  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
157  EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
158  EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
159  EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
160  EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));
161 
162  hull = Polygon::convexHullOfTwoCircles(center1, center1, radius, nVertices);
163  EXPECT_EQ(nVertices, hull.nVertices());
164  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
165  EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
166  EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
167  EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
168  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
169  EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
170  EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
171  EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
172  EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));
173 }
174 
175 TEST(Polygon, convexHullCircle)
176 {
177  Position center(0.0, 0.0);
178  double radius = 0.5;
179  const int nVertices = 15;
180 
181  Polygon hull = Polygon::fromCircle(center, radius);
182 
183  EXPECT_EQ(20, hull.nVertices());
184  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
185  EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
186  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
187  EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));
188 
189  hull = Polygon::fromCircle(center, radius, nVertices);
190  EXPECT_EQ(nVertices, hull.nVertices());
191  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
192  EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
193  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
194  EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));
195 }
196 
197 TEST(convertToInequalityConstraints, triangle1)
198 {
199  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.1, -1.1)});
200  MatrixXd A;
201  VectorXd b;
202  ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
203  EXPECT_NEAR(-1.3636, A(0, 0), 1e-4);
204  EXPECT_NEAR( 1.3636, A(0, 1), 1e-4);
205  EXPECT_NEAR(-1.5000, A(1, 0), 1e-4);
206  EXPECT_NEAR(-1.5000, A(1, 1), 1e-4);
207  EXPECT_NEAR( 2.8636, A(2, 0), 1e-4);
208  EXPECT_NEAR( 0.1364, A(2, 1), 1e-4);
209  EXPECT_NEAR( 0.0000, b(0), 1e-4);
210  EXPECT_NEAR( 0.0000, b(1), 1e-4);
211  EXPECT_NEAR( 3.0000, b(2), 1e-4);
212 }
213 
214 TEST(convertToInequalityConstraints, triangle2)
215 {
216  Polygon polygon({Position(-1.0, 0.5), Position(-1.0, -0.5), Position(1.0, -0.5)});
217  MatrixXd A;
218  VectorXd b;
219  ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
220  EXPECT_NEAR(-1.5000, A(0, 0), 1e-4);
221  EXPECT_NEAR( 0.0000, A(0, 1), 1e-4);
222  EXPECT_NEAR( 0.0000, A(1, 0), 1e-4);
223  EXPECT_NEAR(-3.0000, A(1, 1), 1e-4);
224  EXPECT_NEAR( 1.5000, A(2, 0), 1e-4);
225  EXPECT_NEAR( 3.0000, A(2, 1), 1e-4);
226  EXPECT_NEAR( 1.5000, b(0), 1e-4);
227  EXPECT_NEAR( 1.5000, b(1), 1e-4);
228  EXPECT_NEAR( 0.0000, b(2), 1e-4);
229 }
230 
231 TEST(offsetInward, triangle)
232 {
233  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
234  polygon.offsetInward(0.1);
235  EXPECT_NEAR(0.9, polygon.getVertex(0)(0), 1e-4);
236  EXPECT_NEAR(0.758579, polygon.getVertex(0)(1), 1e-4);
237  EXPECT_NEAR(0.141421, polygon.getVertex(1)(0), 1e-4);
238  EXPECT_NEAR(0.0, polygon.getVertex(1)(1), 1e-4);
239  EXPECT_NEAR(0.9, polygon.getVertex(2)(0), 1e-4);
240  EXPECT_NEAR(-0.758579, polygon.getVertex(2)(1), 1e-4);
241 }
242 
243 TEST(triangulation, triangle)
244 {
245  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
246  std::vector<Polygon> polygons;
247  polygons = polygon.triangulate();
248  ASSERT_EQ(1, polygons.size());
249  EXPECT_EQ(polygon.getVertex(0).x(), polygons[0].getVertex(0).x());
250  EXPECT_EQ(polygon.getVertex(0).y(), polygons[0].getVertex(0).y());
251  EXPECT_EQ(polygon.getVertex(1).x(), polygons[0].getVertex(1).x());
252  EXPECT_EQ(polygon.getVertex(1).y(), polygons[0].getVertex(1).y());
253  EXPECT_EQ(polygon.getVertex(2).x(), polygons[0].getVertex(2).x());
254  EXPECT_EQ(polygon.getVertex(2).y(), polygons[0].getVertex(2).y());
255 }
256 
257 TEST(triangulation, rectangle)
258 {
259  Polygon rectangle;
260  rectangle.addVertex(Vector2d(-2.0, -1.0));
261  rectangle.addVertex(Vector2d(-2.0, 2.0));
262  rectangle.addVertex(Vector2d(1.0, 2.0));
263  rectangle.addVertex(Vector2d(1.0, -1.0));
264  std::vector<Polygon> polygons;
265  polygons = rectangle.triangulate();
266  ASSERT_EQ(2, polygons.size());
267  // TODO Extend.
268 }
grid_map::Polygon::nVertices
size_t nVertices() const
Definition: Polygon.cpp:69
grid_map::Polygon::getCentroid
Position getCentroid() const
Definition: Polygon.cpp:111
grid_map::Polygon
Definition: Polygon.hpp:21
grid_map::Position
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
TEST
TEST(Polygon, getCentroidTriangle)
Definition: PolygonTest.cpp:25
grid_map::Polygon::addVertex
void addVertex(const Position &vertex)
Definition: Polygon.cpp:44
grid_map::Length
Eigen::Array2d Length
Definition: TypeDefs.hpp:24
grid_map::Polygon::offsetInward
bool offsetInward(double margin)
Definition: Polygon.cpp:197
grid_map::Polygon::isInside
bool isInside(const Position &point) const
Definition: Polygon.cpp:30
grid_map::Polygon::triangulate
std::vector< Polygon > triangulate(const TriangulationMethods &method=TriangulationMethods::FAN) const
Definition: Polygon.cpp:221
grid_map::Polygon::getBoundingBox
void getBoundingBox(Position &center, Length &length) const
Definition: Polygon.cpp:128

grid_map_core
Author(s): Péter Fankhauser
autogenerated on Wed Jul 5 2023 02:23:35