1/**
2 * @file testSimPolygon2D.cpp
3 * @author Alex Cunningham
4 */
5
6#include <gtsam/base/TestableAssertions.h>
7#include <gtsam_unstable/geometry/SimPolygon2D.h>
8
9#include <CppUnitLite/TestHarness.h>
10
11using namespace std;
12using namespace gtsam;
13
14const double tol=1e-5;
15
16/* ************************************************************************* */
17TEST(testPolygon, triangle_basic) {
18
19 // create a triangle from points, extract landmarks/walls, check occupancy
20 Point2 pA(0,0), pB(2.0, 0.0), pC(0.0, 1.0);
21
22 // construct and extract data
23 SimPolygon2D actTriangle = SimPolygon2D::createTriangle(pA, pB, pC);
24 LONGS_EQUAL(3, actTriangle.size());
25 EXPECT(assert_equal(pA, actTriangle.landmark(0)));
26 EXPECT(assert_equal(pB, actTriangle.landmark(1)));
27 EXPECT(assert_equal(pC, actTriangle.landmark(2)));
28
29 // get walls out
30 vector<SimWall2D> actWalls = actTriangle.walls();
31 vector<SimWall2D> expWalls;
32 expWalls.push_back(x: SimWall2D(pA, pB));
33 expWalls.push_back(x: SimWall2D(pB, pC));
34 expWalls.push_back(x: SimWall2D(pC, pA));
35 EXPECT(assert_container_equal(expWalls, actWalls, tol));
36
37 // check occupancy - used in sampling more points
38 // treat as closed polygon - points along edges also count
39 EXPECT(actTriangle.contains(Point2(0.25, 0.5)));
40 EXPECT(actTriangle.contains(pA));
41 EXPECT(actTriangle.contains(pB));
42 EXPECT(actTriangle.contains(pC));
43 EXPECT(actTriangle.contains(Point2(1.0, 0.0)));
44
45 EXPECT(!actTriangle.contains(Point2(1.0, 1.0)));
46 EXPECT(!actTriangle.contains(Point2(-1.0, 1.0)));
47}
48
49/* ************************************************************************* */
50TEST(testPolygon, rectangle_basic) {
51
52 // creates an axis-aligned rectangle given a lower left corner and a height and width
53 double height = 3.0, width = 2.0;
54 Point2 pA(1.0, 0.0), pB(3.0, 0.0), pC(3.0, 3.0), pD(1.0, 3.0);
55
56 // construct and extract data
57 SimPolygon2D actRectangle = SimPolygon2D::createRectangle(p: pA, height, width);
58 LONGS_EQUAL(4, actRectangle.size());
59 EXPECT(assert_equal(pA, actRectangle.landmark(0)));
60 EXPECT(assert_equal(pB, actRectangle.landmark(1)));
61 EXPECT(assert_equal(pC, actRectangle.landmark(2)));
62 EXPECT(assert_equal(pD, actRectangle.landmark(3)));
63
64 // get walls out
65 vector<SimWall2D> actWalls = actRectangle.walls();
66 vector<SimWall2D> expWalls;
67 expWalls.push_back(x: SimWall2D(pA, pB));
68 expWalls.push_back(x: SimWall2D(pB, pC));
69 expWalls.push_back(x: SimWall2D(pC, pD));
70 expWalls.push_back(x: SimWall2D(pD, pA));
71 EXPECT(assert_container_equal(expWalls, actWalls, tol));
72
73 // check occupancy - used in sampling more points
74 // treat as closed polygon - points along edges also count
75 EXPECT(actRectangle.contains(Point2(2.0, 1.0)));
76 EXPECT(actRectangle.contains(pA));
77 EXPECT(actRectangle.contains(pB));
78 EXPECT(actRectangle.contains(pC));
79 EXPECT(actRectangle.contains(pD));
80 EXPECT(actRectangle.contains(Point2(1.0, 0.0)));
81
82 EXPECT(!actRectangle.contains(Point2(0.9, 0.5)));
83 EXPECT(!actRectangle.contains(Point2(-1.0, 1.0)));
84}
85
86/* ************************************************************************* */
87TEST(testPolygon, triangle_generator) {
88 // generate random triangles in a bounded region with no overlap
89 double side_len = 10.0; // box of length 10, centered on origin
90 double mean_side_len = 2.0; // mean length of sides
91 double sigma_side_len = 0.5; // stddev for length of sides
92 double min_vertex_dist = 0.4; // minimum allowable distance between vertices
93 double min_side_len = 0.1;
94
95 // initialize the random number generator for consistency
96 SimPolygon2D::seedGenerator(seed: 42u);
97
98 vector<SimPolygon2D> existing_polys;
99
100 SimPolygon2D actual = SimPolygon2D::randomTriangle(side_len, mean_side_len, sigma_side_len,
101 min_vertex_dist, min_side_len, existing_polys);
102
103 // use a rectangle to check that it is within boundaries
104 SimPolygon2D bounding_rect = SimPolygon2D::createRectangle(p: Point2(-5.0,-5.0), height: side_len, width: side_len);
105
106 EXPECT(bounding_rect.contains(actual.landmark(0)));
107 EXPECT(bounding_rect.contains(actual.landmark(1)));
108 EXPECT(bounding_rect.contains(actual.landmark(2)));
109}
110
111/* ************************************************************************* */
112TEST(testPolygon, rectangle_generator) {
113 // generate random rectangles in a bounded region with no overlap
114 double side_len = 10.0; // box of length 10, centered on origin
115 double mean_side_len = 2.0; // mean length of sides
116 double sigma_side_len = 0.5; // stddev for length of sides
117 double min_vertex_dist = 0.4; // minimum allowable distance between vertices
118 double min_side_len = 0.1;
119
120 // initialize the random number generator for consistency
121 SimPolygon2D::seedGenerator(seed: 42u);
122
123 vector<SimPolygon2D> existing_polys;
124
125 SimPolygon2D actual = SimPolygon2D::randomRectangle(side_len, mean_side_len, sigma_side_len,
126 min_vertex_dist, min_side_len, existing_polys);
127
128 // use a rectangle to check that it is within boundaries
129 SimPolygon2D bounding_rect = SimPolygon2D::createRectangle(p: Point2(-5.0,-5.0), height: side_len, width: side_len);
130
131 EXPECT(bounding_rect.contains(actual.landmark(0)));
132 EXPECT(bounding_rect.contains(actual.landmark(1)));
133 EXPECT(bounding_rect.contains(actual.landmark(2)));
134 EXPECT(bounding_rect.contains(actual.landmark(3)));
135}
136
137/* ************************************************************************* */
138int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
139/* ************************************************************************* */
140