Main MRPT website > C++ reference for MRPT 1.9.9
geometry_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <mrpt/math/geometry.h>
11 #include <mrpt/math/CPolygon.h>
12 #include <gtest/gtest.h>
13 #include <algorithm>
14 
15 using namespace mrpt;
16 using namespace mrpt::math;
17 using namespace std;
18 
19 TEST(Geometry, Line2DIntersect)
20 {
21  // Two lines that should intersect at (0.5,0.5)
22  const TLine2D l1(TPoint2D(0, 1), TPoint2D(1, 0));
23  const TLine2D l2(TPoint2D(-1, 0.5), TPoint2D(4, 0.5));
24 
25  TObject2D inter;
26  bool do_inter = intersect(l1, l2, inter);
27 
28  EXPECT_TRUE(do_inter);
29  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_POINT);
30 
31  TPoint2D i(0, 0);
32  inter.getPoint(i);
33  EXPECT_NEAR(i.x, 0.5, 1e-9);
34  EXPECT_NEAR(i.y, 0.5, 1e-9);
35 }
36 
37 TEST(Geometry, Segment2DIntersect)
38 {
39  {
40  // Two segments that should intersect at (0.5,0.5)
41  const TSegment2D s1(TPoint2D(0, 1), TPoint2D(1, 0));
42  const TSegment2D s2(TPoint2D(-1, 0.5), TPoint2D(4, 0.5));
43 
44  TObject2D inter;
45  bool do_inter = intersect(s1, s2, inter);
46 
47  EXPECT_TRUE(do_inter);
48  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_POINT);
49 
50  TPoint2D i(0, 0);
51  inter.getPoint(i);
52  EXPECT_NEAR(i.x, 0.5, 1e-9);
53  EXPECT_NEAR(i.y, 0.5, 1e-9);
54  }
55 
56  {
57  // Two segments that do NOT intersect
58  const TSegment2D s1(TPoint2D(0, 1), TPoint2D(1, 0));
59  const TSegment2D s2(TPoint2D(0.6, 0.5), TPoint2D(4, 0.5));
60 
61  TObject2D inter;
62  bool do_inter = intersect(s1, s2, inter);
63 
64  EXPECT_FALSE(do_inter);
65  }
66  {
67  // Two parallel segments that do NOT intersect: result is a "segment in the middle".
68  const TSegment2D s1(TPoint2D(-0.05,0.05), TPoint2D(-0.05,-0.05));
69  const TSegment2D s2(TPoint2D(0,0.135), TPoint2D(0,-0.0149999));
70 
71  TObject2D inter;
72  bool do_inter = intersect(s1, s2, inter);
73 
74  //EXPECT_TRUE(do_inter && inter.getType()==GEOMETRIC_TYPE_SEGMENT);
75  EXPECT_FALSE(do_inter);
76  }
77 }
78 
79 TEST(Geometry, Intersection3D)
80 {
81  {
82  TPolygon3D p3d({
83  { 1,0,0},
84  { 0,1,0},
85  { 0,0,1}
86  });
87  TSegment3D s3d({
88  { 1,0,0},
89  { 0,1,0},
90  });
91 
92  TObject3D inter;
93  EXPECT_TRUE(intersect(p3d, s3d, inter));
94  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_SEGMENT);
95  TSegment3D test;
96  inter.getSegment(test);
97  //Should this be true? EXPECT_EQ(s3d, test);
98  }
99  {
100  TPolygon3D p3d({
101  { 1,0,0},
102  { 0,1,0},
103  { 0,0,1}
104  });
105  TSegment3D s3d({
106  { 0,0,0},
107  { 1,1,1},
108  });
109 
110  TObject3D inter;
111  EXPECT_TRUE(intersect(p3d, s3d, inter));
112  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_POINT);
113  }
114  {
115  TSegment3D s3d1({
116  { 1,0,0},
117  { 0,1,0},
118  });
119  TSegment3D s3d2({
120  { 2,-1.0,0},
121  { 0, 1.0,0},
122  });
123 
124  TObject3D inter;
125  EXPECT_TRUE(intersect(s3d1, s3d2, inter));
126  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_SEGMENT);
127 
128  }
129  {
130  TSegment3D s3d1({
131  { 1,0,0},
132  { 0,1,0},
133  });
134  TSegment3D s3d2({
135  { 0,0,0},
136  { 1,1,0},
137  });
138 
139  TObject3D inter;
140  EXPECT_TRUE(intersect(s3d1, s3d2, inter));
141  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_POINT);
142 
143  TPoint3D test;
144  TPoint3D expect{0.5, 0.5, 0};
145  inter.getPoint(test);
146  EXPECT_EQ(expect, test);
147  }
148 }
149 
150 TEST(Geometry, IntersectionPlanePlane)
151 {
152  {
153  //Parallel planes
154  TPlane plane1({
155  { 1,0,0},
156  { 0,1,0},
157  { 0,0,1},
158  });
159  TPlane plane2({
160  { 2,0,0},
161  { 0,2,0},
162  { 0,0,2},
163  });
164 
165  TObject3D inter;
166  EXPECT_FALSE(intersect(plane1, plane2, inter));
167  }
168  {
169  //Same plane
170  TPlane plane1({
171  { 1,0,0},
172  { 0,1,0},
173  { 0,0,1},
174  });
175  TPlane plane2({
176  { -1,1,1},
177  { 1,-1,1},
178  { 1,1,-1},
179  });
180 
181  TObject3D inter;
182  EXPECT_TRUE(intersect(plane1, plane2, inter));
183  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_PLANE);
184  }
185  {
186  //Intersecting planes
187  TPlane plane1({
188  { 1,0,0},
189  { 0,1,0},
190  { 0,0,1},
191  });
192  TPlane plane2({
193  { 1,0,0},
194  { 0,-1,0},
195  { 0,0,-1},
196  });
197 
198  TObject3D inter;
199  EXPECT_TRUE(intersect(plane1, plane2, inter));
200  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_LINE);
201  }
202 }
203 
204 void myTestPolygonContainsPoint(std::vector<TPoint2D>& vs, bool convex)
205 {
206  const mrpt::math::TPolygon2D poly(vs);
207 
208  EXPECT_EQ(poly.isConvex(), convex);
209 
210  EXPECT_TRUE(poly.contains(TPoint2D(0.0, 0.0)));
211  EXPECT_TRUE(poly.contains(TPoint2D(0.0, 0.9)));
212  EXPECT_TRUE(poly.contains(TPoint2D(-0.9, -0.9)));
213  EXPECT_TRUE(poly.contains(TPoint2D(0.9, -0.9)));
214 
215  EXPECT_FALSE(poly.contains(TPoint2D(-4.0, -5.1)));
216  EXPECT_FALSE(poly.contains(TPoint2D(-5.0, -0.1)));
217  EXPECT_FALSE(poly.contains(TPoint2D(1.1, -6.1)));
218  EXPECT_FALSE(poly.contains(TPoint2D(0, 5.1)));
219  EXPECT_FALSE(poly.contains(TPoint2D(0, -1.1)));
220 }
221 
222 TEST(Geometry, PolygonConvexContainsPoint)
223 {
224  // Test with a polygon in one winding order:
225  std::vector<TPoint2D> vs;
226  vs.push_back(TPoint2D(-1.0, -1.0));
227  vs.push_back(TPoint2D(0.0, 1.0));
228  vs.push_back(TPoint2D(1.0, -1.0));
229  myTestPolygonContainsPoint(vs, true);
230 
231  // and the other:
232  std::reverse(vs.begin(), vs.end());
233  myTestPolygonContainsPoint(vs, true);
234 
235  {
237  p.AddVertex(0, -0.322);
238  p.AddVertex(-0.644, -0.322);
239  p.AddVertex(-0.210377, -0.324673);
240  p.AddVertex(0.433623, -0.324673);
241 
242  EXPECT_FALSE(p.contains(TPoint2D(0.73175, -0.325796)));
243  }
244 }
245 
246 TEST(Geometry, PolygonConcaveContainsPoint)
247 {
248  // Test with a polygon in one winding order:
249  std::vector<TPoint2D> vs;
250  vs.push_back(TPoint2D(-2.0, 3.0));
251  vs.push_back(TPoint2D(2.0, 2.0));
252  vs.push_back(TPoint2D(3.0, -4.0));
253  vs.push_back(TPoint2D(0.1, -3.0));
254  vs.push_back(TPoint2D(0.1, -0.1));
255  vs.push_back(TPoint2D(-0.1, -0.1));
256  vs.push_back(TPoint2D(-0.1, -3.0));
257  vs.push_back(TPoint2D(-2.0, -2.0));
258 
259  myTestPolygonContainsPoint(vs, false);
260 
261  // and the other:
262  std::reverse(vs.begin(), vs.end());
263  myTestPolygonContainsPoint(vs, false);
264 }
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
void myTestPolygonContainsPoint(std::vector< TPoint2D > &vs, bool convex)
static constexpr unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
unsigned char getType() const
Gets content type.
Standard type for storing any lightweight 2D type.
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:21
Standard object for storing any 3D lightweight object.
STL namespace.
static constexpr unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
TEST(Geometry, Line2DIntersect)
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
3D segment, consisting of two points.
3D Plane, represented by its equation
bool isConvex() const
Checks whether is convex.
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 3D point.
Lightweight 2D point.
GLfloat GLfloat p
Definition: glext.h:6305
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:631
static constexpr unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
2D polygon, inheriting from std::vector<TPoint2D>.
3D polygon, inheriting from std::vector<TPoint3D>
2D line without bounds, represented by its equation .



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019