test_geometry_util.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 #include <gtest/gtest.h>
31 
32 #include <swri_geometry_util/geometry_util.h>
33 
34 TEST(GeometryUtilTests, ClosestPointToLinesInvalid)
35 {
36  tf::Vector3 point;
37  ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
38  tf::Vector3(1, 0, 0),
39  tf::Vector3(1, 0, 0),
40  tf::Vector3(0, 1, 0),
41  tf::Vector3(1, 0, 1),
42  point));
43 
44  ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
45  tf::Vector3(1, 0, 1),
46  tf::Vector3(0, 1, 0),
47  tf::Vector3(0, 0, 1),
48  tf::Vector3(0, 0, 1),
49  point));
50 
51  ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
52  tf::Vector3(0, 0, 0),
53  tf::Vector3(10, 0, 0),
54  tf::Vector3(20, 0, 0),
55  tf::Vector3(30, 0, 0),
56  point));
57 
58  ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
59  tf::Vector3(0, 0, 0),
60  tf::Vector3(10, 0, 0),
61  tf::Vector3(30, 0, 0),
62  tf::Vector3(10, 0, 0),
63  point));
64 
65  ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
66  tf::Vector3(0, 0, 0),
67  tf::Vector3(10, 0, 0),
68  tf::Vector3(20, 10, 10),
69  tf::Vector3(30, 10, 10),
70  point));
71 
72 
73  ASSERT_FALSE(swri_geometry_util::ClosestPointToLines(
74  tf::Vector3(0, 0, 0),
75  tf::Vector3(10, 0, 0),
76  tf::Vector3(30, 10, 10),
77  tf::Vector3(10, 10, 10),
78  point));
79 }
80 
81 TEST(GeometryUtilTests, ClosestPointToLines)
82 {
83  tf::Vector3 point;
84  ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
85  tf::Vector3(0, 0, 0),
86  tf::Vector3(10, 0, 0),
87  tf::Vector3(0, 0, 0),
88  tf::Vector3(0, 10, 0),
89  point));
90  EXPECT_FLOAT_EQ(point.x(), 0);
91  EXPECT_FLOAT_EQ(point.y(), 0);
92  EXPECT_FLOAT_EQ(point.z(), 0);
93 
94  ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
95  tf::Vector3(0, 0, 0),
96  tf::Vector3(10, 0, 0),
97  tf::Vector3(0, 5, 0),
98  tf::Vector3(0, 10, 0),
99  point));
100  EXPECT_FLOAT_EQ(point.x(), 0);
101  EXPECT_FLOAT_EQ(point.y(), 0);
102  EXPECT_FLOAT_EQ(point.z(), 0);
103 
104  ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
105  tf::Vector3(5, 0, 0),
106  tf::Vector3(10, 0, 0),
107  tf::Vector3(0, 5, 0),
108  tf::Vector3(0, 10, 0),
109  point));
110  EXPECT_FLOAT_EQ(point.x(), 0);
111  EXPECT_FLOAT_EQ(point.y(), 0);
112  EXPECT_FLOAT_EQ(point.z(), 0);
113 
114  ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
115  tf::Vector3(0, 0, 0),
116  tf::Vector3(10, 0, 0),
117  tf::Vector3(0, -5, 0),
118  tf::Vector3(0, 10, 0),
119  point));
120  EXPECT_FLOAT_EQ(point.x(), 0);
121  EXPECT_FLOAT_EQ(point.y(), 0);
122  EXPECT_FLOAT_EQ(point.z(), 0);
123 
124  ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
125  tf::Vector3(0, 0, 0),
126  tf::Vector3(10, 0, 0),
127  tf::Vector3(0, 0, 20),
128  tf::Vector3(0, 10, 20),
129  point));
130  EXPECT_FLOAT_EQ(point.x(), 0);
131  EXPECT_FLOAT_EQ(point.y(), 0);
132  EXPECT_FLOAT_EQ(point.z(), 10);
133 
134  ASSERT_TRUE(swri_geometry_util::ClosestPointToLines(
135  tf::Vector3(0, 0, 0),
136  tf::Vector3(10, 0, 0),
137  tf::Vector3(0, 10, 20),
138  tf::Vector3(0, 0, 20),
139  point));
140  EXPECT_FLOAT_EQ(point.x(), 0);
141  EXPECT_FLOAT_EQ(point.y(), 0);
142  EXPECT_FLOAT_EQ(point.z(), 10);
143 }
144 
145 TEST(GeometryUtilTests, ProjectPointToLineSegment3D)
146 {
147  tf::Vector3 p1(0, 0, 0);
148  tf::Vector3 p2(10, 0, 0);
149  tf::Vector3 p3(2, 0, 0);
150  tf::Vector3 p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
151  EXPECT_FLOAT_EQ(p3.x(), p4.x());
152  EXPECT_FLOAT_EQ(p3.y(), p4.y());
153  EXPECT_FLOAT_EQ(p3.z(), p4.z());
154 
155  p4 = swri_geometry_util::ProjectToLineSegment(p2, p1, p3);
156  EXPECT_FLOAT_EQ(p3.x(), p4.x());
157  EXPECT_FLOAT_EQ(p3.y(), p4.y());
158  EXPECT_FLOAT_EQ(p3.z(), p4.z());
159 
160  p3 = tf::Vector3(0, 0, 0);
161  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
162  EXPECT_FLOAT_EQ(p3.x(), p4.x());
163  EXPECT_FLOAT_EQ(p3.y(), p4.y());
164  EXPECT_FLOAT_EQ(p3.z(), p4.z());
165 
166  p3 = tf::Vector3(0, 1, 0);
167  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
168  EXPECT_FLOAT_EQ(0, p4.x());
169  EXPECT_FLOAT_EQ(0, p4.y());
170  EXPECT_FLOAT_EQ(0, p4.z());
171 
172  p3 = tf::Vector3(5, -1, 0);
173  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
174  EXPECT_FLOAT_EQ(5, p4.x());
175  EXPECT_FLOAT_EQ(0, p4.y());
176  EXPECT_FLOAT_EQ(0, p4.z());
177 
178  p3 = tf::Vector3(5, -1, 10);
179  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
180  EXPECT_FLOAT_EQ(5, p4.x());
181  EXPECT_FLOAT_EQ(0, p4.y());
182  EXPECT_FLOAT_EQ(0, p4.z());
183 
184  p3 = tf::Vector3(-5, -1, 10);
185  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
186  EXPECT_FLOAT_EQ(0, p4.x());
187  EXPECT_FLOAT_EQ(0, p4.y());
188  EXPECT_FLOAT_EQ(0, p4.z());
189 
190  p3 = tf::Vector3(15, -1, 10);
191  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
192  EXPECT_FLOAT_EQ(10, p4.x());
193  EXPECT_FLOAT_EQ(0, p4.y());
194  EXPECT_FLOAT_EQ(0, p4.z());
195 
196  p1 = tf::Vector3(0, 0, 0);
197  p2 = tf::Vector3(10, 10, 10);
198  p3 = tf::Vector3(1, 1, 1);
199  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
200  EXPECT_FLOAT_EQ(1, p4.x());
201  EXPECT_FLOAT_EQ(1, p4.y());
202  EXPECT_FLOAT_EQ(1, p4.z());
203 
204  p2 = tf::Vector3(10, 10, 0);
205  p3 = tf::Vector3(0, 10, 0);
206  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
207  EXPECT_FLOAT_EQ(5, p4.x());
208  EXPECT_FLOAT_EQ(5, p4.y());
209  EXPECT_FLOAT_EQ(0, p4.z());
210 
211  p4 = swri_geometry_util::ProjectToLineSegment(p2, p1, p3);
212  EXPECT_FLOAT_EQ(5, p4.x());
213  EXPECT_FLOAT_EQ(5, p4.y());
214  EXPECT_FLOAT_EQ(0, p4.z());
215 
216  p3 = tf::Vector3(-100, 10, 0);
217  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
218  EXPECT_FLOAT_EQ(0, p4.x());
219  EXPECT_FLOAT_EQ(0, p4.y());
220  EXPECT_FLOAT_EQ(0, p4.z());
221 }
222 
223 TEST(GeometryUtilTests, ProjectPointToLineSegment2D)
224 {
225  cv::Vec2d p1(0, 0);
226  cv::Vec2d p2(10, 0);
227  cv::Vec2d p3(2, 0);
228  cv::Vec2d p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
229  EXPECT_FLOAT_EQ(p3[0], p4[0]);
230  EXPECT_FLOAT_EQ(p3[1], p4[1]);
231 
232  p4 = swri_geometry_util::ProjectToLineSegment(p2, p1, p3);
233  EXPECT_FLOAT_EQ(p3[0], p4[0]);
234  EXPECT_FLOAT_EQ(p3[1], p4[1]);
235 
236  p3 = cv::Vec2d(0, 0);
237  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
238  EXPECT_FLOAT_EQ(p3[0], p4[0]);
239  EXPECT_FLOAT_EQ(p3[1], p4[1]);
240 
241  p3 = cv::Vec2d(0, 1);
242  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
243  EXPECT_FLOAT_EQ(0, p4[0]);
244  EXPECT_FLOAT_EQ(0, p4[1]);
245 
246  p3 = cv::Vec2d(5, -1);
247  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
248  EXPECT_FLOAT_EQ(5, p4[0]);
249  EXPECT_FLOAT_EQ(0, p4[1]);
250 
251  p3 = cv::Vec2d(-5, -1);
252  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
253  EXPECT_FLOAT_EQ(0, p4[0]);
254  EXPECT_FLOAT_EQ(0, p4[1]);
255 
256  p3 = cv::Vec2d(15, -1);
257  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
258  EXPECT_FLOAT_EQ(10, p4[0]);
259  EXPECT_FLOAT_EQ(0, p4[1]);
260 
261  p1 = cv::Vec2d(0, 0);
262  p2 = cv::Vec2d(10, 10);
263  p3 = cv::Vec2d(1, 1);
264  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
265  EXPECT_FLOAT_EQ(1, p4[0]);
266  EXPECT_FLOAT_EQ(1, p4[1]);
267 
268  p2 = cv::Vec2d(10, 10);
269  p3 = cv::Vec2d(0, 10);
270  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
271  EXPECT_FLOAT_EQ(5, p4[0]);
272  EXPECT_FLOAT_EQ(5, p4[1]);
273 
274  p4 = swri_geometry_util::ProjectToLineSegment(p2, p1, p3);
275  EXPECT_FLOAT_EQ(5, p4[0]);
276  EXPECT_FLOAT_EQ(5, p4[1]);
277 
278  p3 = cv::Vec2d(-100, 10);
279  p4 = swri_geometry_util::ProjectToLineSegment(p1, p2, p3);
280  EXPECT_FLOAT_EQ(0, p4[0]);
281  EXPECT_FLOAT_EQ(0, p4[1]);
282 }
283 
284 TEST(GeometryUtilTests, DistanceFromLineSegment3D)
285 {
286  tf::Vector3 p1(0, 0, 0);
287  tf::Vector3 p2(10, 0, 0);
288  tf::Vector3 p3(2, 0, 0);
289  EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
290  EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
291 
292  p3 = tf::Vector3(0, 0, 0);
293  EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
294 
295  p3 = tf::Vector3(0, 1, 0);
296  EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
297 
298  p3 = tf::Vector3(5, -1, 0);
299  EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
300 
301  p3 = tf::Vector3(5, -1, 10);
302  EXPECT_FLOAT_EQ(std::sqrt(101), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
303 
304  p3 = tf::Vector3(-5, -1, 10);
305  EXPECT_FLOAT_EQ(std::sqrt(126), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
306 
307  p3 = tf::Vector3(15, -1, 10);
308  EXPECT_FLOAT_EQ(std::sqrt(126), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
309 
310  p1 = tf::Vector3(0, 0, 0);
311  p2 = tf::Vector3(10, 10, 10);
312  p3 = tf::Vector3(1, 1, 1);
313  EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
314 
315  p2 = tf::Vector3(10, 10, 0);
316  p3 = tf::Vector3(0, 10, 0);
317  EXPECT_FLOAT_EQ(std::sqrt(50), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
318  EXPECT_FLOAT_EQ(std::sqrt(50), swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
319 
320  p3 = tf::Vector3(-100, 10, 0);
321  EXPECT_FLOAT_EQ(std::sqrt(10100), swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
322 }
323 
324 TEST(GeometryUtilTests, DistanceFromLineSegment2D)
325 {
326  cv::Vec2d p1(0, 0);
327  cv::Vec2d p2(10, 0);
328  cv::Vec2d p3(2, 0);
329  EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
330  EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
331 
332  p3 = cv::Vec2d(0, 0);
333  EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
334 
335  p3 = cv::Vec2d(0, 1);
336  EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
337 
338  p3 = cv::Vec2d(5, -1);
339  EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
340 
341  p1 = cv::Vec2d(0, 0);
342  p2 = cv::Vec2d(10, 10);
343  p3 = cv::Vec2d(1, 1);
344  EXPECT_FLOAT_EQ(0, swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
345 
346  p2 = cv::Vec2d(10, 10);
347  p3 = cv::Vec2d(0, 10);
348  EXPECT_FLOAT_EQ(std::sqrt(50), swri_geometry_util::DistanceFromLineSegment(p2, p1, p3));
349  EXPECT_FLOAT_EQ(std::sqrt(50), swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
350 
351  p3 = cv::Vec2d(-100, 10);
352  EXPECT_FLOAT_EQ(std::sqrt(10100), swri_geometry_util::DistanceFromLineSegment(p1, p2, p3));
353 }
354 
355 
356 TEST(GeometryUtilTests, PointInPolygon)
357 {
358  std::vector<cv::Vec2d> polygon1;
359  polygon1.push_back(cv::Vec2d(1, 1));
360  polygon1.push_back(cv::Vec2d(1, -1));
361  polygon1.push_back(cv::Vec2d(-1, -1));
362  polygon1.push_back(cv::Vec2d(-1, 1));
363  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(0, 0)));
364  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(0.99, 0.99)));
365  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(-0.99, 0.99)));
366  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(-0.99, -0.99)));
367  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(0.99, -0.99)));
368  EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(2, 0)));
369  EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(-2, 0)));
370  EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon1, cv::Vec2d(2, -2)));
371 
372  std::vector<cv::Vec2d> polygon2;
373  polygon2.push_back(cv::Vec2d(1, 1));
374  polygon2.push_back(cv::Vec2d(1, -1));
375  polygon2.push_back(cv::Vec2d(-1, -1));
376  polygon2.push_back(cv::Vec2d(-1, 1));
377  polygon2.push_back(cv::Vec2d(1, 1));
378  polygon2.push_back(cv::Vec2d(1, 1));
379  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(0, 0)));
380  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(0.99, 0.99)));
381  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(-0.99, 0.99)));
382  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(-0.99, -0.99)));
383  EXPECT_TRUE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(0.99, -0.99)));
384  EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(2, 0)));
385  EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(-2, 0)));
386  EXPECT_FALSE(swri_geometry_util::PointInPolygon(polygon2, cv::Vec2d(2, -2)));
387 }
388 
389 TEST(GeometryUtilTests, DistanceFromPolygon)
390 {
391  std::vector<cv::Vec2d> polygon1;
392  polygon1.push_back(cv::Vec2d(1, 1));
393  polygon1.push_back(cv::Vec2d(1, -1));
394  polygon1.push_back(cv::Vec2d(-1, -1));
395  polygon1.push_back(cv::Vec2d(-1, 1));
396  EXPECT_FLOAT_EQ(1.0, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(0, 0)));
397  EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(0.99, 0.99)));
398  EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(-0.99, 0.99)));
399  EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(-0.99, -0.99)));
400  EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(0.99, -0.99)));
401  EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(2, 0)));
402  EXPECT_FLOAT_EQ(1,swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(-2, 0)));
403  EXPECT_FLOAT_EQ(std::sqrt(2),swri_geometry_util::DistanceFromPolygon(polygon1, cv::Vec2d(2, -2)));
404 
405  std::vector<cv::Vec2d> polygon2;
406  polygon2.push_back(cv::Vec2d(1, 1));
407  polygon2.push_back(cv::Vec2d(1, -1));
408  polygon2.push_back(cv::Vec2d(-1, -1));
409  polygon2.push_back(cv::Vec2d(-1, 1));
410  polygon2.push_back(cv::Vec2d(1, 1));
411  polygon2.push_back(cv::Vec2d(1, 1));
412  EXPECT_FLOAT_EQ(1.0, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(0, 0)));
413  EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(0.99, 0.99)));
414  EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(-0.99, 0.99)));
415  EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(-0.99, -0.99)));
416  EXPECT_FLOAT_EQ(0.01, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(0.99, -0.99)));
417  EXPECT_FLOAT_EQ(1, swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(2, 0)));
418  EXPECT_FLOAT_EQ(1,swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(-2, 0)));
419  EXPECT_FLOAT_EQ(std::sqrt(2),swri_geometry_util::DistanceFromPolygon(polygon2, cv::Vec2d(2, -2)));
420 }
421 
422 // Run all the tests that were declared with TEST()
423 int main(int argc, char **argv)
424 {
425  testing::InitGoogleTest(&argc, argv);
426 
427  return RUN_ALL_TESTS();
428 }
swri_geometry_util::PointInPolygon
bool PointInPolygon(const std::vector< cv::Vec2d > &polygon, const cv::Vec2d &point)
Definition: geometry_util.cpp:119
swri_geometry_util::DistanceFromPolygon
double DistanceFromPolygon(const std::vector< cv::Vec2d > &polygon, const cv::Vec2d &point)
Definition: geometry_util.cpp:149
TEST
TEST(GeometryUtilTests, ClosestPointToLinesInvalid)
Definition: test_geometry_util.cpp:34
tf::Vector3::x
TFSIMD_FORCE_INLINE const tfScalar & x() const
main
int main(int argc, char **argv)
Definition: test_geometry_util.cpp:423
tf::Vector3::z
TFSIMD_FORCE_INLINE const tfScalar & z() const
tf::Vector3::y
TFSIMD_FORCE_INLINE const tfScalar & y() const
swri_geometry_util::ProjectToLineSegment
tf::Vector3 ProjectToLineSegment(const tf::Vector3 &line_start, const tf::Vector3 &line_end, const tf::Vector3 &point)
Definition: geometry_util.cpp:70
geometry_util.h
swri_geometry_util::ClosestPointToLines
bool ClosestPointToLines(const tf::Vector3 &a1, const tf::Vector3 &a2, const tf::Vector3 &b1, const tf::Vector3 &b2, tf::Vector3 &point)
Definition: geometry_util.cpp:167
swri_geometry_util::DistanceFromLineSegment
double DistanceFromLineSegment(const tf::Vector3 &line_start, const tf::Vector3 &line_end, const tf::Vector3 &point)
Definition: geometry_util.cpp:51

swri_geometry_util
Author(s): Marc Alban
autogenerated on Fri Jun 7 2019 22:05:39