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

grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:27