Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
11 #include <mrpt/math/geometry.h>
12 #include <mrpt/math/CPolygon.h>
13 #include <gtest/gtest.h>
14 #include <algorithm>
15 
16 using namespace mrpt;
17 using namespace mrpt::utils;
18 using namespace mrpt::math;
19 using namespace std;
20 
21 
22 TEST(Geometry, Line2DIntersect)
23 {
24  // Two lines that should intersect at (0.5,0.5)
25  const TLine2D l1(TPoint2D(0,1), TPoint2D(1,0));
26  const TLine2D l2(TPoint2D(-1,0.5), TPoint2D(4,0.5));
27 
28  TObject2D inter;
29  bool do_inter = intersect(l1, l2, inter);
30 
31  EXPECT_TRUE(do_inter);
32  EXPECT_EQ( inter.getType(), GEOMETRIC_TYPE_POINT );
33 
34  TPoint2D i(0,0);
35  inter.getPoint(i);
36  EXPECT_NEAR(i.x, 0.5, 1e-9);
37  EXPECT_NEAR(i.y, 0.5, 1e-9);
38 }
39 
40 TEST(Geometry, Line2DAngle)
41 {
42  const TLine2D l1(TPoint2D(0, 0), TPoint2D(1, 0));
43  const TLine2D l2(TPoint2D(-1, -1), TPoint2D(5, 5));
44 
45  // Angles in 2D do have sign:
46  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l1, l2)), +45.0, 1e-5);
47  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l2, l1)), -45.0, 1e-5);
48 
49  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l1, l1)), 0.0, 1e-5);
50  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l2, l2)), 0.0, 1e-5);
51 
52  const TLine2D l3(TPoint2D(1, 0), TPoint2D(0, 0));
53  EXPECT_NEAR(RAD2DEG(std::abs(mrpt::math::getAngle(l1, l3))), 180.0, 1e-5);
54  EXPECT_NEAR(RAD2DEG(std::abs(mrpt::math::getAngle(l3, l1))), 180.0, 1e-5);
55 }
56 
57 TEST(Geometry, Line3DAngle)
58 {
59  const TLine3D l1(TPoint3D(0, 0, 0), TPoint3D(1, 0, 0));
60  const TLine3D l2(TPoint3D(-1, -1, 0), TPoint3D(5, 5, 0));
61 
62  // Angles in 3D don't have sign:
63  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l1, l2)), 45.0, 1e-5);
64  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l2, l1)), 45.0, 1e-5);
65 
66  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l1, l1)), 0.0, 1e-5);
67  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l2, l2)), 0.0, 1e-5);
68 
69  const TLine3D l3(TPoint3D(1, 0, 0), TPoint3D(0, 0, 0));
70  EXPECT_NEAR(RAD2DEG(std::abs(mrpt::math::getAngle(l1, l3))), 180.0, 1e-5);
71  EXPECT_NEAR(RAD2DEG(std::abs(mrpt::math::getAngle(l3, l1))), 180.0, 1e-5);
72 
73  const TLine3D l4(
74  TPoint3D(0, 0, 0), TPoint3D(cos(DEG2RAD(30.0)), sin(DEG2RAD(30.0)), 0));
75  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l1, l4)), 30.0, 1e-5);
76  EXPECT_NEAR(RAD2DEG(mrpt::math::getAngle(l4, l1)), 30.0, 1e-5);
77 }
78 
79 
80 TEST(Geometry, Segment2DIntersect)
81 {
82  {
83  // Two segments that should intersect at (0.5,0.5)
84  const TSegment2D s1(TPoint2D(0,1), TPoint2D(1,0));
85  const TSegment2D s2(TPoint2D(-1,0.5), TPoint2D(4,0.5));
86 
87  TObject2D inter;
88  bool do_inter = intersect(s1, s2, inter);
89 
90  EXPECT_TRUE(do_inter);
91  EXPECT_EQ( inter.getType(), GEOMETRIC_TYPE_POINT );
92 
93  TPoint2D i(0,0);
94  inter.getPoint(i);
95  EXPECT_NEAR(i.x, 0.5, 1e-9);
96  EXPECT_NEAR(i.y, 0.5, 1e-9);
97  }
98 
99  {
100  // Two segments that do NOT intersect
101  const TSegment2D s1(TPoint2D(0,1), TPoint2D(1,0));
102  const TSegment2D s2(TPoint2D(0.6,0.5), TPoint2D(4,0.5));
103 
104  TObject2D inter;
105  bool do_inter = intersect(s1, s2, inter);
106 
107  EXPECT_FALSE(do_inter);
108  }
109 
110 #if 0
111  {
112  // Two parallel segments that do NOT intersect: result is a "segment in the middle".
113  const TSegment2D s1(TPoint2D(-0.05,0.05), TPoint2D(-0.05,-0.05));
114  const TSegment2D s2(TPoint2D(0,0.135), TPoint2D(0,-0.0149999));
115 
116  TObject2D inter;
117  bool do_inter = intersect(s1, s2, inter);
118 
119  EXPECT_TRUE(do_inter && inter.getType()==GEOMETRIC_TYPE_SEGMENT);
120  }
121 #endif
122 }
123 
124 void myTestPolygonContainsPoint(std::vector<TPoint2D> &vs, bool convex)
125 {
126  const mrpt::math::TPolygon2D poly(vs);
127 
128  EXPECT_EQ(poly.isConvex(),convex);
129 
130  EXPECT_TRUE( poly.contains( TPoint2D(0.0, 0.0) ) );
131  EXPECT_TRUE( poly.contains( TPoint2D(0.0, 0.9) ) );
132  EXPECT_TRUE( poly.contains( TPoint2D(-0.9, -0.9) ) );
133  EXPECT_TRUE( poly.contains( TPoint2D(0.9, -0.9) ) );
134 
135  EXPECT_FALSE( poly.contains( TPoint2D(-4.0, -5.1) ) );
136  EXPECT_FALSE( poly.contains( TPoint2D(-5.0, -0.1) ) );
137  EXPECT_FALSE( poly.contains( TPoint2D( 1.1, -6.1) ) );
138  EXPECT_FALSE( poly.contains( TPoint2D( 0, 5.1) ) );
139  EXPECT_FALSE( poly.contains( TPoint2D( 0, -1.1) ) );
140 }
141 
142 TEST(Geometry, PolygonConvexContainsPoint)
143 {
144  // Test with a polygon in one winding order:
145  std::vector<TPoint2D> vs;
146  vs.push_back(TPoint2D(-1.0, -1.0));
147  vs.push_back(TPoint2D( 0.0, 1.0));
148  vs.push_back(TPoint2D( 1.0, -1.0));
149  myTestPolygonContainsPoint(vs, true);
150 
151  // and the other:
152  std::reverse(vs.begin(),vs.end());
153  myTestPolygonContainsPoint(vs, true);
154 
155  {
157  p.AddVertex(0, -0.322);
158  p.AddVertex(-0.644, -0.322);
159  p.AddVertex(-0.210377, -0.324673);
160  p.AddVertex(0.433623, -0.324673);
161 
162  EXPECT_FALSE (p.contains(TPoint2D(0.73175, -0.325796)));
163  }
164 }
165 
166 TEST(Geometry, PolygonConcaveContainsPoint)
167 {
168  // Test with a polygon in one winding order:
169  std::vector<TPoint2D> vs;
170  vs.push_back(TPoint2D(-2.0, 3.0));
171  vs.push_back(TPoint2D( 2.0, 2.0));
172  vs.push_back(TPoint2D( 3.0, -4.0));
173  vs.push_back(TPoint2D( 0.1, -3.0));
174  vs.push_back(TPoint2D( 0.1, -0.1));
175  vs.push_back(TPoint2D(-0.1, -0.1));
176  vs.push_back(TPoint2D(-0.1, -3.0));
177  vs.push_back(TPoint2D(-2.0, -2.0));
178 
179  myTestPolygonContainsPoint(vs,false);
180 
181  // and the other:
182  std::reverse(vs.begin(),vs.end());
183  myTestPolygonContainsPoint(vs,false);
184 }
185 
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)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
unsigned char getType() const
Gets content type.
const unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
Standard type for storing any lightweight 2D type.
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:25
STL namespace.
TEST(Geometry, Line2DIntersect)
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
2D segment, consisting of two points.
const unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:82
double BASE_IMPEXP getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:726
bool isConvex() const
Checks whether is convex.
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double RAD2DEG(const double x)
Radians to degrees.
Definition: bits.h:88
Lightweight 3D point.
Lightweight 2D point.
GLfloat GLfloat p
Definition: glext.h:5587
bool BASE_IMPEXP intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:568
2D polygon, inheriting from std::vector<TPoint2D>.
3D line, represented by a base point and a director vector.
2D line without bounds, represented by its equation .



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019