UAV Coverage Path Planner
test_cgutil.cpp
Go to the documentation of this file.
1 
7 /*
8  * Copyright (c) 2017 Takaki Ueno
9  * Released under the MIT license
10  */
11 
12 #include <cgutil.hpp>
13 
14 // gtest
15 #include <gtest/gtest.h>
16 
17 // c++ libraries
18 #include <array>
19 #include <cmath>
20 #include <vector>
21 
22 // geometry_msgs
23 #include <geometry_msgs/Point.h>
24 
25 TEST(CGUtilTest, IntersectTest1)
26 {
27  geometry_msgs::Point p1, p2, p3, p4, p5, p6, p7, p8;
28  std::array<geometry_msgs::Point, 2> e1, e2, e3, e4;
29  std::vector<std::array<geometry_msgs::Point, 2> > segments;
30 
31  p1.x = 0.0;
32  p1.y = 0.0;
33  p2.x = 5.0;
34  p2.y = 1.0;
35  p3.x = 2.0;
36  p3.y = 5.0;
37  p4.x = 6.0;
38  p4.y = 3.0;
39  p5.x = 7.0;
40  p5.y = 1.0;
41  p6.x = 8.0;
42  p6.y = 5.0;
43  p7.x = 3.0;
44  p7.y = 3.0;
45  p8.x = 5.0;
46  p8.y = 5.0;
47 
48  e1.at(0) = p1;
49  e1.at(1) = p2;
50  e2.at(0) = p3;
51  e2.at(1) = p4;
52  e3.at(0) = p5;
53  e3.at(1) = p6;
54  e4.at(0) = p7;
55  e4.at(1) = p8;
56 
57  segments.push_back(e1);
58  segments.push_back(e2);
59  segments.push_back(e3);
60  segments.push_back(e4);
61 
62  std::vector<std::array<std::array<geometry_msgs::Point, 2>, 2> > intersecting_segments = intersect(segments);
63  EXPECT_EQ(1, intersecting_segments.size());
64  EXPECT_DOUBLE_EQ(p3.x, intersecting_segments.at(0).at(0).at(0).x);
65  EXPECT_DOUBLE_EQ(p3.y, intersecting_segments.at(0).at(0).at(0).y);
66  EXPECT_DOUBLE_EQ(p4.x, intersecting_segments.at(0).at(0).at(1).x);
67  EXPECT_DOUBLE_EQ(p4.y, intersecting_segments.at(0).at(0).at(1).y);
68  EXPECT_DOUBLE_EQ(p7.x, intersecting_segments.at(0).at(1).at(0).x);
69  EXPECT_DOUBLE_EQ(p7.y, intersecting_segments.at(0).at(1).at(0).y);
70  EXPECT_DOUBLE_EQ(p8.x, intersecting_segments.at(0).at(1).at(1).x);
71  EXPECT_DOUBLE_EQ(p8.y, intersecting_segments.at(0).at(1).at(1).y);
72 }
73 
74 TEST(CGUtilTest, IntersectTest2)
75 {
76  geometry_msgs::Point p1, p2, p3, p4;
77  std::array<geometry_msgs::Point, 2> e1, e2;
78 
79  // TestCase1
80  // p1: (0, 0)
81  // p2: (3, 2)
82  // p3: (2, 0)
83  // p4: (0, 2)
84  // Expected value: true
85  p1.x = 0.0;
86  p1.y = 0.0;
87  p2.x = 3.0;
88  p2.y = 2.0;
89  p3.x = 2.0;
90  p3.y = 0.0;
91  p4.x = 0.0;
92  p4.y = 2.0;
93 
94  e1.at(0) = p1;
95  e1.at(1) = p2;
96  e2.at(0) = p3;
97  e2.at(1) = p4;
98 
99  EXPECT_TRUE(intersect(e1, e2));
100 
101  // TestCase2
102  // p1: (0, 0)
103  // p2: (4, 2)
104  // p3: (2, 1)
105  // p4: (0, 2)
106  // Expected value: true
107  p1.x = 0.0;
108  p1.y = 0.0;
109  p2.x = 4.0;
110  p2.y = 2.0;
111  p3.x = 2.0;
112  p3.y = 1.0;
113  p4.x = 0.0;
114  p4.y = 2.0;
115 
116  e1.at(0) = p1;
117  e1.at(1) = p2;
118  e2.at(0) = p3;
119  e2.at(1) = p4;
120 
121  EXPECT_TRUE(intersect(e1, e2));
122 
123  // TestCase3
124  // p1: (0, 0)
125  // p2: (4, 2)
126  // p3: (1, 1)
127  // p4: (0, 2)
128  // Expected value: false
129  p1.x = 0.0;
130  p1.y = 0.0;
131  p2.x = 4.0;
132  p2.y = 2.0;
133  p3.x = 1.0;
134  p3.y = 1.0;
135  p4.x = 0.0;
136  p4.y = 2.0;
137 
138  e1.at(0) = p1;
139  e1.at(1) = p2;
140  e2.at(0) = p3;
141  e2.at(1) = p4;
142 
143  EXPECT_FALSE(intersect(e1, e2));
144 }
145 
146 TEST(CGUtilTest, InBetweenTest)
147 {
148  geometry_msgs::Point p1, p2, p3;
149 
150  // TestCase1
151  // p1: (0, 0)
152  // p2: (2, 2)
153  // p3: (1, 1)
154  // Expected value: true
155  p1.x = 0.0;
156  p1.y = 0.0;
157  p2.x = 2.0;
158  p2.y = 2.0;
159  p3.x = 1.0;
160  p3.y = 1.0;
161  EXPECT_TRUE(inBetween(p1, p2, p3));
162 
163  // TestCase2
164  // p1: (0, 0)
165  // p2: (2, 2)
166  // p3: (1, 0)
167  // Expected value: false
168  p1.x = 0.0;
169  p1.y = 0.0;
170  p2.x = 2.0;
171  p2.y = 2.0;
172  p3.x = 1.0;
173  p3.y = 0.0;
174  EXPECT_FALSE(inBetween(p1, p2, p3));
175 
176  // TestCase3
177  // p1: (0, 0)
178  // p2: (0, 0)
179  // p3: (0, 0)
180  // Expected value: true
181  p1.x = 0.0;
182  p1.y = 0.0;
183  p2.x = 0.0;
184  p2.y = 0.0;
185  p3.x = 0.0;
186  p3.y = 0.0;
187  EXPECT_TRUE(inBetween(p1, p2, p3));
188 }
189 
193 TEST(CGUtilTest, CalcDistanceTest)
194 {
195  // TestCase1
196  // coordinate of edge's start point: (0.0, 0.0)
197  // coordinate of edge's end point: (1.0, 0.0)
198  // coordinate of opposite vertex: (0.5, 0.5)
199  // expected distance: 0.5
200  std::array<geometry_msgs::Point, 2> edge;
201  geometry_msgs::Point e1, e2, vertex;
202 
203  e1.x = 0.0;
204  e1.y = 0.0;
205  e2.x = 1.0;
206  e2.y = 0.0;
207  edge.front() = e1;
208  edge.back() = e2;
209 
210  vertex.x = 0.5;
211  vertex.y = 0.5;
212 
213  EXPECT_NEAR(0.5, distance(edge, vertex), 1.0e-5);
214 
215  // TestCase2
216  // coordinate of edge's start point: (0.0, 0.0)
217  // coordinate of edge's end point: (0.0, sqrt(3.0)/2)
218  // coordinate of opposite vertex: (0.0, 0.5)
219  // expected distance: 0.5
220  e1.x = 0.0;
221  e1.y = 0.0;
222  e2.x = 0.866;
223  e2.y = 0.0;
224  edge.front() = e1;
225  edge.back() = e2;
226 
227  vertex.x = 0.0;
228  vertex.y = 0.5;
229 
230  EXPECT_NEAR(0.5, distance(edge, vertex), 1.0e-5);
231 
232  // TestCase3
233  // coordinate of edge's start point: (0.0, 0.0)
234  // coordinate of edge's end point: (1.0, 0.0)
235  // coordinate of opposite vertex: (-1.0, 1.0)
236  // expected distance: 1.0
237  e1.x = 0.0;
238  e1.y = 0.0;
239  e2.x = 1.0;
240  e2.y = 0.0;
241  edge.front() = e1;
242  edge.back() = e2;
243 
244  vertex.x = -1.0;
245  vertex.y = 1.0;
246 
247  EXPECT_NEAR(1.0, distance(edge, vertex), 1.0e-5);
248 
249  // TestCase4
250  // coordinate of edge's start point: (0.0, 0.0)
251  // coordinate of edge's end point: (1.0, 0.0)
252  // coordinate of opposite vertex: (1.5, 0.5)
253  // expected distance: 0.5
254  e1.x = 0.0;
255  e1.y = 0.0;
256  e2.x = 1.0;
257  e2.y = 0.0;
258  edge.front() = e1;
259  edge.back() = e2;
260 
261  vertex.x = 1.5;
262  vertex.y = 0.5;
263 
264  EXPECT_NEAR(0.5, distance(edge, vertex), 1.0e-5);
265 
266  // TestCase5
267  // coordinate of edge's start point: (0.0, 0.0)
268  // coordinate of edge's end point: (1.0, -1/2)
269  // coordinate of opposite vertex: (2/3, 1/3)
270  // expected distance: 4*sqrt(5)/15
271  e1.x = 0.0;
272  e1.y = 0.0;
273  e2.x = 1.0;
274  e2.y = -0.5;
275  edge.front() = e1;
276  edge.back() = e2;
277 
278  vertex.x = 2.0 / 3.0;
279  vertex.y = 1.0 / 3.0;
280 
281  EXPECT_NEAR(0.596284794, distance(edge, vertex), 1.0e-5);
282 
283  // TestCase6
284  // coordinate of edge's start point: (1.5, 1.0)
285  // coordinate of edge's end point: (0.0, 2.5)
286  // coordinate of opposite vertex: (0.5, 1.5)
287  // expected distance: sqrt(2)/4
288  e1.x = 1.5;
289  e1.y = 1.0;
290  e2.x = 0;
291  e2.y = 2.5;
292  edge.front() = e1;
293  edge.back() = e2;
294 
295  vertex.x = 0.5;
296  vertex.y = 1.5;
297 
298  EXPECT_NEAR(0.3535533906, distance(edge, vertex), 1.0e-5);
299 
300  // TestCase7
301  // coordinate of edge's start point: (6.0, 1.0)
302  // coordinate of edge's end point: (0.5, 1.0)
303  // coordinate of opposite vertex: (1.5, 2.5)
304  // expected distance: 3/2
305  e1.x = 6.0;
306  e1.y = 1.0;
307  e2.x = 0.5;
308  e2.y = 1.0;
309  edge.front() = e1;
310  edge.back() = e2;
311 
312  vertex.x = 1.5;
313  vertex.y = 2.5;
314 
315  EXPECT_NEAR(1.5, distance(edge, vertex), 1.0e-5);
316 }
317 
321 TEST(CGUtilTest, CalcVertexAngleTest)
322 {
323  geometry_msgs::Point p1, p2, p3;
324 
325  // TestCase1
326  // p1: (0, 0)
327  // p2: (1, 0)
328  // p3: (1, 1)
329  // Expected angle[rad]: pi/4
330  p1.x = 0.0;
331  p1.y = 0.0;
332  p2.x = 1.0;
333  p2.y = 0.0;
334  p3.x = 1.0;
335  p3.y = 1.0;
336  EXPECT_DOUBLE_EQ(M_PI_4, vertexAngle(p1, p2, p3));
337 
338  // TestCase2
339  // p1: (0, 0)
340  // p2: (1, 0)
341  // p3: (0, 0)
342  // Expected angle[rad]: 0
343  p1.x = 0.0;
344  p1.y = 0.0;
345  p2.x = 1.0;
346  p2.y = 0.0;
347  p3.x = 0.0;
348  p3.y = 0.0;
349  EXPECT_DOUBLE_EQ(0.0, vertexAngle(p1, p2, p3));
350 
351  // TestCase3
352  // p1: (0, 0)
353  // p2: (1, 0)
354  // p3: (0, 1)
355  // Expected angle[rad]: pi/2
356  p1.x = 0.0;
357  p1.y = 0.0;
358  p2.x = 1.0;
359  p2.y = 0.0;
360  p3.x = 0.0;
361  p3.y = 1.0;
362  EXPECT_DOUBLE_EQ(M_PI_2, vertexAngle(p1, p2, p3));
363 
364  // TestCase4
365  // p1: (0, 0)
366  // p2: (1, 0)
367  // p3: (-1, 1)
368  // Expected angle[rad]: 3*pi/4
369  p1.x = 0.0;
370  p1.y = 0.0;
371  p2.x = 1.0;
372  p2.y = 0.0;
373  p3.x = -1.0;
374  p3.y = 1.0;
375  EXPECT_DOUBLE_EQ(3 * M_PI_4, vertexAngle(p1, p2, p3));
376 
377  // TestCase5
378  // p1: (0, 0)
379  // p2: (-1, 0)
380  // p3: (0.5, sqrt(3)/2)
381  // Expected angle[rad]: 2*pi/3
382  p1.x = 0.0;
383  p1.y = 0.0;
384  p2.x = -1.0;
385  p2.y = 0.0;
386  p3.x = 0.5;
387  p3.y = std::sqrt(3.0) / 2.0;
388  EXPECT_DOUBLE_EQ(2 * M_PI / 3.0, vertexAngle(p1, p2, p3));
389 
390  // TestCase6
391  // p1: (0, 0)
392  // p2: (1, 0)
393  // p3: (-sqrt(3)/2, -0.5)
394  // Expected angle[rad]: 2*pi/3
395  p1.x = 0.0;
396  p1.y = 0.0;
397  p2.x = 1.0;
398  p2.y = 0.0;
399  p3.x = -std::sqrt(3.0) / 2.0;
400  p3.y = -0.5;
401  EXPECT_DOUBLE_EQ(5 * M_PI / 6.0, vertexAngle(p1, p2, p3));
402 
403  // TestCase7
404  // p1: (0, 0)
405  // p2: (0, 0)
406  // p3: (0, 0)
407  // Expected angle[rad]: 0.0
408  p1.x = 0.0;
409  p1.y = 0.0;
410  p2.x = 0.0;
411  p2.y = 0.0;
412  p3.x = 0.0;
413  p3.y = 0.0;
414  EXPECT_DOUBLE_EQ(0.0, vertexAngle(p1, p2, p3));
415 }
416 
420 TEST(CGUtilTest, CalcHorizontalAngleTest)
421 {
422  geometry_msgs::Point p1, p2;
423 
424  // TestCase1
425  // p1: (0, 0)
426  // p2: (1, 0)
427  // Expected angle[rad]: 0
428  p1.x = 0.0;
429  p1.y = 0.0;
430  p2.x = 1.0;
431  p2.y = 0.0;
432  EXPECT_DOUBLE_EQ(0, horizontalAngle(p1, p2));
433 
434  // TestCase2
435  // p1: (0, 0)
436  // p2: (1, 1)
437  // Expected angle[rad]: pi/4
438  p1.x = 0.0;
439  p1.y = 0.0;
440  p2.x = 1.0;
441  p2.y = 1.0;
442  EXPECT_DOUBLE_EQ(M_PI_4, horizontalAngle(p1, p2));
443 
444  // TestCase3
445  // p1: (0, 0)
446  // p2: (-0.5, sqrt(3)/2)
447  // Expected angle[rad]: 3*pi/4
448  p1.x = 0.0;
449  p1.y = 0.0;
450  p2.x = -0.5;
451  p2.y = std::sqrt(3) / 2.0;
452  EXPECT_DOUBLE_EQ(2 * M_PI / 3, horizontalAngle(p1, p2));
453 
454  // TestCase4
455  // p1: (0, 0)
456  // p2: (0, -1)
457  // Expected angle[rad]: pi/2
458  p1.x = 0.0;
459  p1.y = 0.0;
460  p2.x = 0.0;
461  p2.y = -1.0;
462  EXPECT_DOUBLE_EQ(M_PI_2, horizontalAngle(p1, p2));
463 
464  // TestCase5
465  // p1: (0, 0)
466  // p2: (0, 0)
467  // Expected angle[rad]: 0.0
468  p1.x = 0.0;
469  p1.y = 0.0;
470  p2.x = 0.0;
471  p2.y = 0.0;
472  EXPECT_DOUBLE_EQ(0.0, horizontalAngle(p1, p2));
473 }
474 
478 TEST(CGUtilTest, CalcsignedAreaTest)
479 {
480  geometry_msgs::Point p1, p2, p3;
481 
482  // TestCase1
483  // p1: (0, 0)
484  // p2: (1, 0)
485  // p3: (0, 1)
486  // Expected value: 1
487  p1.x = 0.0;
488  p1.y = 0.0;
489  p2.x = 1.0;
490  p2.y = 0.0;
491  p3.x = 0.0;
492  p3.y = 1.0;
493  EXPECT_DOUBLE_EQ(1.0, signedArea(p1, p2, p3));
494 
495  // TestCase2
496  // p1: (0, 0)
497  // p2: (-1, 0)
498  // p3: (0, 1)
499  // Expected value: -1
500  p1.x = 0.0;
501  p1.y = 0.0;
502  p2.x = -1.0;
503  p2.y = 0.0;
504  p3.x = 0.0;
505  p3.y = 1.0;
506  EXPECT_DOUBLE_EQ(-1.0, signedArea(p1, p2, p3));
507 
508  // TestCase3
509  // p1: (-1, 0)
510  // p2: (1, 1)
511  // p3: (2, -1)
512  // Expected value: -5
513  p1.x = -1.0;
514  p1.y = 0.0;
515  p2.x = 1.0;
516  p2.y = 1.0;
517  p3.x = 2.0;
518  p3.y = -1.0;
519  EXPECT_DOUBLE_EQ(-5.0, signedArea(p1, p2, p3));
520 
521  // TestCase3
522  // p1: (-1, 0)
523  // p2: (1, 1)
524  // p3: (1, 1)
525  // Expected value: 0
526  p1.x = -1.0;
527  p1.y = 0.0;
528  p2.x = 1.0;
529  p2.y = 1.0;
530  p3.x = 1.0;
531  p3.y = 1.0;
532  EXPECT_DOUBLE_EQ(0.0, signedArea(p1, p2, p3));
533 }
534 
538 TEST(CGUtilTest, GrahamScanTest)
539 {
540  geometry_msgs::Point p1, p2, p3, p4, p5, p6, p7;
541  std::vector<geometry_msgs::Point> points, convex_hull;
542 
543  // TestCase1
544  // p1: (0, 0)
545  // p2: (4, 1)
546  // p3: (6, 4)
547  // p4: (3, 5)
548  // p5: (0, 3)
549  // Expected points in convex hull: (p1, p2, p3, p4, p5)
550  p1.x = 0.0;
551  p1.y = 0.0;
552  p2.x = 4.0;
553  p2.y = 1.0;
554  p3.x = 6.0;
555  p3.y = 4.0;
556  p4.x = 3.0;
557  p4.y = 5.0;
558  p5.x = 0.0;
559  p5.y = 3.0;
560 
561  points.push_back(p1);
562  points.push_back(p2);
563  points.push_back(p3);
564  points.push_back(p4);
565  points.push_back(p5);
566 
567  convex_hull = grahamScan(points);
568 
569  EXPECT_DOUBLE_EQ(p1.x, convex_hull.at(0).x);
570  EXPECT_DOUBLE_EQ(p1.y, convex_hull.at(0).y);
571  EXPECT_DOUBLE_EQ(p2.x, convex_hull.at(1).x);
572  EXPECT_DOUBLE_EQ(p2.y, convex_hull.at(1).y);
573  EXPECT_DOUBLE_EQ(p3.x, convex_hull.at(2).x);
574  EXPECT_DOUBLE_EQ(p3.y, convex_hull.at(2).y);
575  EXPECT_DOUBLE_EQ(p4.x, convex_hull.at(3).x);
576  EXPECT_DOUBLE_EQ(p4.y, convex_hull.at(3).y);
577  EXPECT_DOUBLE_EQ(p5.x, convex_hull.at(4).x);
578  EXPECT_DOUBLE_EQ(p5.y, convex_hull.at(4).y);
579 
580  points.clear();
581 
582  // TestCase2
583  // p1: (0, 0)
584  // p2: (0, 3)
585  // p3: (3, 3)
586  // p4: (5, 4)
587  // p5: (3, 0)
588  // p6: (2, 5)
589  // Expected points in convex hull: (p1, p5, p4, p6, p2)
590  p1.x = 0.0;
591  p1.y = 0.0;
592  p2.x = 0.0;
593  p2.y = 3.0;
594  p3.x = 3.0;
595  p3.y = 3.0;
596  p4.x = 5.0;
597  p4.y = 4.0;
598  p5.x = 3.0;
599  p5.y = 0.0;
600  p6.x = 2.0;
601  p6.y = 5.0;
602 
603  points.push_back(p1);
604  points.push_back(p2);
605  points.push_back(p3);
606  points.push_back(p4);
607  points.push_back(p5);
608  points.push_back(p6);
609 
610  convex_hull = grahamScan(points);
611 
612  EXPECT_DOUBLE_EQ(p1.x, convex_hull.at(0).x);
613  EXPECT_DOUBLE_EQ(p1.y, convex_hull.at(0).y);
614  EXPECT_DOUBLE_EQ(p5.x, convex_hull.at(1).x);
615  EXPECT_DOUBLE_EQ(p5.y, convex_hull.at(1).y);
616  EXPECT_DOUBLE_EQ(p4.x, convex_hull.at(2).x);
617  EXPECT_DOUBLE_EQ(p4.y, convex_hull.at(2).y);
618  EXPECT_DOUBLE_EQ(p6.x, convex_hull.at(3).x);
619  EXPECT_DOUBLE_EQ(p6.y, convex_hull.at(3).y);
620  EXPECT_DOUBLE_EQ(p2.x, convex_hull.at(4).x);
621  EXPECT_DOUBLE_EQ(p2.y, convex_hull.at(4).y);
622 
623  points.clear();
624 
625  // TestCase3
626  // p1: (0, 0)
627  // p2: (5, 1)
628  // p3: (5, 4)
629  // p4: (3, 2)
630  // p5: (1, 3)
631  // p6: (2, 5)
632  // p7: (0, 4)
633  // Expected points in convex hull: (p1, p2, p3, p6, p7)
634  p1.x = 0.0;
635  p1.y = 0.0;
636  p2.x = 5.0;
637  p2.y = 1.0;
638  p3.x = 5.0;
639  p3.y = 4.0;
640  p4.x = 3.0;
641  p4.y = 2.0;
642  p5.x = 1.0;
643  p5.y = 3.0;
644  p6.x = 2.0;
645  p6.y = 5.0;
646  p7.x = 0.0;
647  p7.y = 4.0;
648 
649  points.push_back(p1);
650  points.push_back(p2);
651  points.push_back(p3);
652  points.push_back(p4);
653  points.push_back(p5);
654  points.push_back(p6);
655  points.push_back(p7);
656 
657  convex_hull = grahamScan(points);
658 
659  EXPECT_DOUBLE_EQ(p1.x, convex_hull.at(0).x);
660  EXPECT_DOUBLE_EQ(p1.y, convex_hull.at(0).y);
661  EXPECT_DOUBLE_EQ(p2.x, convex_hull.at(1).x);
662  EXPECT_DOUBLE_EQ(p2.y, convex_hull.at(1).y);
663  EXPECT_DOUBLE_EQ(p3.x, convex_hull.at(2).x);
664  EXPECT_DOUBLE_EQ(p3.y, convex_hull.at(2).y);
665  EXPECT_DOUBLE_EQ(p6.x, convex_hull.at(3).x);
666  EXPECT_DOUBLE_EQ(p6.y, convex_hull.at(3).y);
667  EXPECT_DOUBLE_EQ(p7.x, convex_hull.at(4).x);
668  EXPECT_DOUBLE_EQ(p7.y, convex_hull.at(4).y);
669 
670  points.clear();
671 
672  // TestCase4
673  // no points
674  // Expected points in convex hull: ()
675  convex_hull = grahamScan(points);
676 
677  EXPECT_TRUE(convex_hull.empty());
678 
679  points.clear();
680 
681  // TestCase5
682  // p1: (0, 0)
683  // Expected points in convex hull: ()
684  p1.x = 0.0;
685  p1.y = 0.0;
686 
687  points.push_back(p1);
688 
689  convex_hull = grahamScan(points);
690 
691  EXPECT_TRUE(convex_hull.empty());
692 
693  points.clear();
694 
695  // TestCase6
696  // p1: (0, 0)
697  // p2: (1, 0)
698  // Expected points in convex hull: ()
699  p1.x = 0.0;
700  p1.y = 0.0;
701  p2.x = 1.0;
702  p2.y = 0.0;
703 
704  points.push_back(p1);
705  points.push_back(p2);
706 
707  convex_hull = grahamScan(points);
708 
709  EXPECT_TRUE(convex_hull.empty());
710 
711  points.clear();
712 
713  // TestCase7
714  // p1: (0, 0)
715  // p2: (1, 0)
716  // p3: (1, 0)
717  // Expected points in convex hull: ()
718  p1.x = 0.0;
719  p1.y = 0.0;
720  p2.x = 1.0;
721  p2.y = 0.0;
722  p3.x = 1.0;
723  p3.y = 0.0;
724 
725  points.push_back(p1);
726  points.push_back(p2);
727  points.push_back(p3);
728 
729  convex_hull = grahamScan(points);
730 
731  EXPECT_TRUE(convex_hull.empty());
732 
733  points.clear();
734 
735  // TestCase8
736  // p1: (0, 0)
737  // p2: (1, 0)
738  // p3: (1, 0)
739  // p4: (1, 1)
740  // Expected points in convex hull: (p1, p3, p4)
741  p1.x = 0.0;
742  p1.y = 0.0;
743  p2.x = 1.0;
744  p2.y = 0.0;
745  p3.x = 1.0;
746  p3.y = 0.0;
747  p4.x = 1.0;
748  p4.y = 1.0;
749 
750  points.push_back(p1);
751  points.push_back(p2);
752  points.push_back(p3);
753  points.push_back(p4);
754 
755  convex_hull = grahamScan(points);
756 
757  EXPECT_DOUBLE_EQ(p1.x, convex_hull.at(0).x);
758  EXPECT_DOUBLE_EQ(p1.y, convex_hull.at(0).y);
759  EXPECT_DOUBLE_EQ(p3.x, convex_hull.at(1).x);
760  EXPECT_DOUBLE_EQ(p3.y, convex_hull.at(1).y);
761  EXPECT_DOUBLE_EQ(p4.x, convex_hull.at(2).x);
762  EXPECT_DOUBLE_EQ(p4.y, convex_hull.at(2).y);
763 
764  points.clear();
765 }
766 
767 TEST(CGUtilTest, IsConvexTest1)
768 {
769  geometry_msgs::Point p1, p2, p3, p4, p5;
770  std::vector<geometry_msgs::Point> points;
771 
772  // TestCase1
773  // p1: (0, 0)
774  // p2: (4, 1)
775  // p3: (6, 4)
776  // p4: (3, 5)
777  // p5: (0, 3)
778  // Expected value: True
779  p1.x = 0.0;
780  p1.y = 0.0;
781  p2.x = 4.0;
782  p2.y = 1.0;
783  p3.x = 6.0;
784  p3.y = 4.0;
785  p4.x = 3.0;
786  p4.y = 5.0;
787  p5.x = 0.0;
788  p5.y = 3.0;
789 
790  points.push_back(p1);
791  points.push_back(p2);
792  points.push_back(p3);
793  points.push_back(p4);
794  points.push_back(p5);
795 
796  EXPECT_TRUE(isConvex(points));
797 }
798 
799 TEST(CGUtilTest, IsConvexTest2)
800 {
801  geometry_msgs::Point p1, p2, p3, p4, p5;
802  std::vector<geometry_msgs::Point> points;
803 
804  // TestCase2
805  // p1: (0, 0)
806  // p2: (1, 0)
807  // p3: (1, 0)
808  // p4: (1, 1)
809  // Expected points in convex hull: (p1, p3, p4)
810  p1.x = 0.0;
811  p1.y = 0.0;
812  p2.x = 1.0;
813  p2.y = 0.0;
814  p3.x = 1.0;
815  p3.y = 0.0;
816  p4.x = 1.0;
817  p4.y = 1.0;
818 
819  points.push_back(p1);
820  points.push_back(p2);
821  points.push_back(p3);
822  points.push_back(p4);
823 
824  EXPECT_FALSE(isConvex(points));
825 }
826 
827 int main(int argc, char** argv)
828 {
829  ::testing::InitGoogleTest(&argc, argv);
830  return RUN_ALL_TESTS();
831 }
TEST
TEST(CGUtilTest, IntersectTest1)
Definition: test_cgutil.cpp:25
isConvex
bool isConvex(PointVector points)
Checks if given polygon is convex or not.
Definition: cgutil.hpp:324
main
int main(int argc, char **argv)
Definition: test_cgutil.cpp:827
Generated by   doxygen 1.8.11