1/**
2 * @file SimPolygon2D.h
3 * @brief Polygons for simulation use
4 * @author Alex Cunningham
5 */
6
7#pragma once
8
9#include <gtsam/geometry/Pose2.h>
10#include <gtsam_unstable/geometry/SimWall2D.h>
11
12#include <map>
13#include <random>
14
15namespace gtsam {
16
17/**
18 * General polygon class for convex polygons
19 */
20class GTSAM_UNSTABLE_EXPORT SimPolygon2D {
21protected:
22 Point2Vector landmarks_;
23 static std::minstd_rand rng;
24
25public:
26
27 /** Don't use this constructor, use a named one instead */
28 SimPolygon2D() {}
29
30 /** seed the random number generator - only needs to be done once */
31 static void seedGenerator(unsigned long seed);
32
33 /** Named constructor for creating triangles */
34 static SimPolygon2D createTriangle(const Point2& pA, const Point2& pB, const Point2& pC);
35
36 /**
37 * Named constructor for creating axis-aligned rectangles
38 * @param p is the lower-left corner
39 */
40 static SimPolygon2D createRectangle(const Point2& p, double height, double width);
41
42 /**
43 * Randomly generate a triangle that does not conflict with others
44 * Uniformly distributed over box area, with normally distributed lengths of edges
45 * THROWS: std::runtime_error if can't find a position
46 */
47 static SimPolygon2D randomTriangle(double side_len, double mean_side_len, double sigma_side_len,
48 double min_vertex_dist, double min_side_len, const std::vector<SimPolygon2D>& existing_polys);
49
50 /**
51 * Randomly generate a rectangle that does not conflict with others
52 * Uniformly distributed over box area, with normally distributed lengths of edges
53 * THROWS: std::runtime_error if can't find a position
54 */
55 static SimPolygon2D randomRectangle(double side_len, double mean_side_len, double sigma_side_len,
56 double min_vertex_dist, double min_side_len, const std::vector<SimPolygon2D>& existing_polys);
57
58 // access to underlying points
59 const Point2& landmark(size_t i) const { return landmarks_[i]; }
60 size_t size() const { return landmarks_.size(); }
61 const Point2Vector& vertices() const { return landmarks_; }
62
63 // testable requirements
64 bool equals(const SimPolygon2D& p, double tol=1e-5) const;
65 void print(const std::string& s="") const;
66
67 /**
68 * Get a set of walls along the edges
69 */
70 std::vector<SimWall2D> walls() const;
71
72 /**
73 * Core function for randomly generating scenarios.
74 * Polygons are closed, convex shapes.
75 * @return true if the given point is contained by this polygon
76 */
77 bool contains(const Point2& p) const;
78
79 /**
80 * Checks two polygons to determine if they overlap
81 * @return true iff at least one vertex of one polygon is contained in the other
82 */
83 bool overlaps(const SimPolygon2D& p) const;
84
85 /** returns true iff p is contained in any of a set of polygons */
86 static bool anyContains(const Point2& p, const std::vector<SimPolygon2D>& obstacles);
87
88 /** returns true iff polygon p overlaps with any of a set of polygons */
89 static bool anyOverlaps(const SimPolygon2D& p, const std::vector<SimPolygon2D>& obstacles);
90
91 /** returns true iff p is inside a square centered at zero with side s */
92 static bool insideBox(double s, const Point2& p);
93
94 /** returns true iff p is within threshold of any point in S */
95 static bool nearExisting(const Point2Vector& S,
96 const Point2& p, double threshold);
97
98 /** pick a random point uniformly over a box of side s */
99 static Point2 randomPoint2(double s);
100
101 /** randomly generate a Rot2 with a uniform distribution over theta */
102 static Rot2 randomAngle();
103
104 /** generate a distance from a normal distribution given a mean and sigma */
105 static double randomDistance(double mu, double sigma, double min_dist = -1.0);
106
107 /** pick a random point within a box that is further than dist d away from existing landmarks */
108 static Point2 randomBoundedPoint2(double boundary_size,
109 const Point2Vector& landmarks, double min_landmark_dist);
110
111 /** pick a random point within a box that meets above requirements, as well as staying out of obstacles */
112 static Point2 randomBoundedPoint2(double boundary_size,
113 const Point2Vector& landmarks,
114 const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
115
116 /** pick a random point that only avoid obstacles */
117 static Point2 randomBoundedPoint2(double boundary_size,
118 const std::vector<SimPolygon2D>& obstacles);
119
120 /** pick a random point in box defined by lower left and upper right corners */
121 static Point2 randomBoundedPoint2(
122 const Point2& LL_corner, const Point2& UR_corner,
123 const Point2Vector& landmarks,
124 const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
125
126 /** pick a random pose in a bounded area that is not in an obstacle */
127 static Pose2 randomFreePose(double boundary_size, const std::vector<SimPolygon2D>& obstacles);
128
129};
130
131typedef std::vector<SimPolygon2D> SimPolygon2DVector;
132
133} //\namespace gtsam
134
135