1/**
2 * @file SimPolygon2D.cpp
3 * @author Alex Cunningham
4 */
5
6#include <iostream>
7#include <random>
8
9#include <gtsam_unstable/geometry/SimPolygon2D.h>
10
11namespace gtsam {
12
13using namespace std;
14
15const size_t max_it = 100000;
16std::minstd_rand SimPolygon2D::rng(42u);
17
18/* ************************************************************************* */
19void SimPolygon2D::seedGenerator(unsigned long seed) {
20 rng = std::minstd_rand(seed);
21}
22
23/* ************************************************************************* */
24SimPolygon2D SimPolygon2D::createTriangle(const Point2& pA, const Point2& pB, const Point2& pC) {
25 SimPolygon2D result;
26 result.landmarks_.push_back(x: pA);
27 result.landmarks_.push_back(x: pB);
28 result.landmarks_.push_back(x: pC);
29 return result;
30}
31
32/* ************************************************************************* */
33SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, double width) {
34 SimPolygon2D result;
35 result.landmarks_.push_back(x: p);
36 result.landmarks_.push_back(x: p + Point2(width, 0.0));
37 result.landmarks_.push_back(x: p + Point2(width, height));
38 result.landmarks_.push_back(x: p + Point2(0.0, height));
39 return result;
40}
41
42/* ************************************************************************* */
43bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
44 if (p.size() != size()) return false;
45 for (size_t i=0; i<size(); ++i)
46 if (!traits<Point2>::Equals(v1: landmarks_[i], v2: p.landmarks_[i], tol))
47 return false;
48 return true;
49}
50
51/* ************************************************************************* */
52void SimPolygon2D::print(const string& s) const {
53 cout << "SimPolygon " << s << ": " << endl;
54 for(const Point2& p: landmarks_)
55 traits<Point2>::Print(m: p, str: " ");
56}
57
58/* ************************************************************************* */
59vector<SimWall2D> SimPolygon2D::walls() const {
60 vector<SimWall2D> result;
61 for (size_t i=0; i<size()-1; ++i)
62 result.push_back(x: SimWall2D(landmarks_[i], landmarks_[i+1]));
63 result.push_back(x: SimWall2D(landmarks_[size()-1], landmarks_[0]));
64 return result;
65}
66
67/* ************************************************************************* */
68bool SimPolygon2D::contains(const Point2& c) const {
69 vector<SimWall2D> edges = walls();
70 bool initialized = false;
71 bool lastSide = false;
72 for(const SimWall2D& ab: edges) {
73 // compute cross product of ab and ac
74 Point2 dab = ab.b() - ab.a();
75 Point2 dac = c - ab.a();
76 double cross = dab.x() * dac.y() - dab.y() * dac.x();
77 if (std::abs(x: cross) < 1e-6) // check for on one of the edges
78 return true;
79 bool side = cross > 0;
80 // save the first side found
81 if (!initialized) {
82 lastSide = side;
83 initialized = true;
84 continue;
85 }
86
87 // to be inside the polygon, point must be on the same side of all lines
88 if (lastSide != side)
89 return false;
90 }
91 return true;
92}
93
94/* ************************************************************************* */
95bool SimPolygon2D::overlaps(const SimPolygon2D& p) const {
96 for(const Point2& a: landmarks_)
97 if (p.contains(c: a))
98 return true;
99 for(const Point2& a: p.landmarks_)
100 if (contains(c: a))
101 return true;
102 return false;
103}
104
105/* ***************************************************************** */
106bool SimPolygon2D::anyContains(const Point2& p, const vector<SimPolygon2D>& obstacles) {
107 for(const SimPolygon2D& poly: obstacles)
108 if (poly.contains(c: p))
109 return true;
110 return false;
111}
112
113/* ************************************************************************* */
114bool SimPolygon2D::anyOverlaps(const SimPolygon2D& p, const vector<SimPolygon2D>& obstacles) {
115 for(const SimPolygon2D& poly: obstacles)
116 if (poly.overlaps(p))
117 return true;
118 return false;
119}
120
121/* ************************************************************************* */
122SimPolygon2D SimPolygon2D::randomTriangle(
123 double side_len, double mean_side_len, double sigma_side_len,
124 double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
125 // get the current set of landmarks
126 Point2Vector lms;
127 double d2 = side_len/2.0;
128 lms.push_back(x: Point2( d2, d2));
129 lms.push_back(x: Point2(-d2, d2));
130 lms.push_back(x: Point2(-d2,-d2));
131 lms.push_back(x: Point2( d2,-d2));
132
133 for(const SimPolygon2D& poly: existing_polys)
134 lms.insert(position: lms.begin(), first: poly.vertices().begin(), last: poly.vertices().end());
135
136 for (size_t i=0; i<max_it; ++i) {
137
138 // find a random pose for line AB
139 Pose2 xA(randomAngle(), randomBoundedPoint2(boundary_size: side_len, landmarks: lms, obstacles: existing_polys, min_landmark_dist: min_vertex_dist));
140
141 // extend line by random dist and angle to get BC
142 double dAB = randomDistance(mu: mean_side_len, sigma: sigma_side_len, min_dist: min_side_len);
143 double tABC = randomAngle().theta();
144 Pose2 xB = xA.retract(v: (Vector(3) << dAB, 0.0, tABC).finished());
145
146 // extend from B to find C
147 double dBC = randomDistance(mu: mean_side_len, sigma: sigma_side_len, min_dist: min_side_len);
148 Pose2 xC = xB.retract(v: Vector::Unit(newSize: 3,i: 0)*dBC);
149
150 // use triangle equality to verify non-degenerate triangle
151 double dAC = distance2(p1: xA.t(), q: xC.t());
152
153 // form a triangle and test if it meets requirements
154 SimPolygon2D test_tri = SimPolygon2D::createTriangle(pA: xA.t(), pB: xB.t(), pC: xC.t());
155
156 // check inside walls, long enough edges, far away from landmarks
157 const double thresh = mean_side_len / 2.0;
158 if ((dAB + dBC + thresh > dAC) && // triangle inequality
159 (dAB + dAC + thresh > dBC) &&
160 (dAC + dBC + thresh > dAB) &&
161 insideBox(s: side_len, p: test_tri.landmark(i: 0)) &&
162 insideBox(s: side_len, p: test_tri.landmark(i: 1)) &&
163 insideBox(s: side_len, p: test_tri.landmark(i: 2)) &&
164 distance2(p1: test_tri.landmark(i: 1), q: test_tri.landmark(i: 2)) > min_side_len &&
165 !nearExisting(S: lms, p: test_tri.landmark(i: 0), threshold: min_vertex_dist) &&
166 !nearExisting(S: lms, p: test_tri.landmark(i: 1), threshold: min_vertex_dist) &&
167 !nearExisting(S: lms, p: test_tri.landmark(i: 2), threshold: min_vertex_dist) &&
168 !anyOverlaps(p: test_tri, obstacles: existing_polys)) {
169 return test_tri;
170 }
171 }
172 throw runtime_error("Could not find space for a triangle");
173 return SimPolygon2D::createTriangle(pA: Point2(99,99), pB: Point2(99,99), pC: Point2(99,99));
174}
175
176/* ************************************************************************* */
177SimPolygon2D SimPolygon2D::randomRectangle(
178 double side_len, double mean_side_len, double sigma_side_len,
179 double min_vertex_dist, double min_side_len, const vector<SimPolygon2D>& existing_polys) {
180 // get the current set of landmarks
181 Point2Vector lms;
182 double d2 = side_len/2.0;
183 lms.push_back(x: Point2( d2, d2));
184 lms.push_back(x: Point2(-d2, d2));
185 lms.push_back(x: Point2(-d2,-d2));
186 lms.push_back(x: Point2( d2,-d2));
187 for(const SimPolygon2D& poly: existing_polys)
188 lms.insert(position: lms.begin(), first: poly.vertices().begin(), last: poly.vertices().end());
189
190 const Point2 lower_corner(-side_len,-side_len);
191 const Point2 upper_corner( side_len, side_len);
192
193 for (size_t i=0; i<max_it; ++i) {
194
195 // pick height and width to be viable distances
196 double height = randomDistance(mu: mean_side_len, sigma: sigma_side_len, min_dist: min_side_len);
197 double width = randomDistance(mu: mean_side_len, sigma: sigma_side_len, min_dist: min_side_len);
198
199 // find a starting point - limited to region viable for this height/width
200 Point2 pA = randomBoundedPoint2(LL_corner: lower_corner, UR_corner: upper_corner - Point2(width, height),
201 landmarks: lms, obstacles: existing_polys, min_landmark_dist: min_vertex_dist);
202
203 // verify
204 SimPolygon2D rect = SimPolygon2D::createRectangle(p: pA, height, width);
205
206 // check inside walls, long enough edges, far away from landmarks
207 if (insideBox(s: side_len, p: rect.landmark(i: 0)) &&
208 insideBox(s: side_len, p: rect.landmark(i: 1)) &&
209 insideBox(s: side_len, p: rect.landmark(i: 2)) &&
210 insideBox(s: side_len, p: rect.landmark(i: 3)) &&
211 !nearExisting(S: lms, p: rect.landmark(i: 0), threshold: min_vertex_dist) &&
212 !nearExisting(S: lms, p: rect.landmark(i: 1), threshold: min_vertex_dist) &&
213 !nearExisting(S: lms, p: rect.landmark(i: 2), threshold: min_vertex_dist) &&
214 !nearExisting(S: lms, p: rect.landmark(i: 3), threshold: min_vertex_dist) &&
215 !anyOverlaps(p: rect, obstacles: existing_polys)) {
216 return rect;
217 }
218 }
219 throw runtime_error("Could not find space for a rectangle");
220 return SimPolygon2D::createRectangle(p: Point2(99,99), height: 100, width: 100);
221}
222
223/* ***************************************************************** */
224Point2 SimPolygon2D::randomPoint2(double s) {
225 std::uniform_real_distribution<> gen_t(-s/2.0, s/2.0);
226 return Point2(gen_t(rng), gen_t(rng));
227}
228
229/* ***************************************************************** */
230Rot2 SimPolygon2D::randomAngle() {
231 // modified range to avoid degenerate cases in triangles:
232 std::uniform_real_distribution<> gen_r(-M_PI, M_PI);
233 return Rot2::fromAngle(theta: gen_r(rng));
234}
235
236/* ***************************************************************** */
237double SimPolygon2D::randomDistance(double mu, double sigma, double min_dist) {
238 std::normal_distribution<> norm_dist(mu, sigma);
239 double d = -10.0;
240 for (size_t i=0; i<max_it; ++i) {
241 d = std::abs(x: norm_dist(rng));
242 if (d > min_dist)
243 return d;
244 }
245 cout << "Non viable distance: " << d << " with mu = " << mu << " sigma = " << sigma
246 << " min_dist = " << min_dist << endl;
247 throw runtime_error("Failed to find a viable distance");
248 return std::abs(x: norm_dist(rng));
249}
250
251/* ***************************************************************** */
252Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
253 const vector<SimPolygon2D>& obstacles) {
254 for (size_t i=0; i<max_it; ++i) {
255 Point2 p = randomPoint2(s: boundary_size);
256 if (!anyContains(p, obstacles))
257 return p;
258 }
259 throw runtime_error("Failed to find a place for a landmark!");
260 return Point2(0,0);
261}
262
263/* ***************************************************************** */
264Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
265 const Point2Vector& landmarks, double min_landmark_dist) {
266 for (size_t i=0; i<max_it; ++i) {
267 Point2 p = randomPoint2(s: boundary_size);
268 if (!nearExisting(S: landmarks, p, threshold: min_landmark_dist))
269 return p;
270 }
271 throw runtime_error("Failed to find a place for a landmark!");
272 return Point2(0,0);
273}
274
275/* ***************************************************************** */
276Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
277 const Point2Vector& landmarks,
278 const vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
279 for (size_t i=0; i<max_it; ++i) {
280 Point2 p = randomPoint2(s: boundary_size);
281 if (!nearExisting(S: landmarks, p, threshold: min_landmark_dist) && !anyContains(p, obstacles))
282 return p;
283 }
284 throw runtime_error("Failed to find a place for a landmark!");
285 return Point2(0,0);
286}
287
288/* ***************************************************************** */
289Point2 SimPolygon2D::randomBoundedPoint2(
290 const Point2& LL_corner, const Point2& UR_corner,
291 const Point2Vector& landmarks,
292 const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist) {
293
294 std::uniform_real_distribution<> gen_x(0.0, UR_corner.x() - LL_corner.x());
295 std::uniform_real_distribution<> gen_y(0.0, UR_corner.y() - LL_corner.y());
296
297 for (size_t i=0; i<max_it; ++i) {
298 Point2 p = Point2(gen_x(rng), gen_y(rng)) + LL_corner;
299 if (!nearExisting(S: landmarks, p, threshold: min_landmark_dist) && !anyContains(p, obstacles))
300 return p;
301 }
302 throw runtime_error("Failed to find a place for a landmark!");
303 return Point2(0,0);
304}
305
306/* ***************************************************************** */
307Pose2 SimPolygon2D::randomFreePose(double boundary_size, const vector<SimPolygon2D>& obstacles) {
308 return Pose2(randomAngle(), randomBoundedPoint2(boundary_size, obstacles));
309}
310
311/* ***************************************************************** */
312bool SimPolygon2D::insideBox(double s, const Point2& p) {
313 return std::abs(x: p.x()) < s/2.0 && std::abs(x: p.y()) < s/2.0;
314}
315
316/* ***************************************************************** */
317bool SimPolygon2D::nearExisting(const Point2Vector& S,
318 const Point2& p, double threshold) {
319 for(const Point2& Sp: S)
320 if (distance2(p1: Sp, q: p) < threshold)
321 return true;
322 return false;
323}
324
325} //\namespace gtsam
326
327