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 |
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  {
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:19
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020