135 lines
4.7 KiB
C++
135 lines
4.7 KiB
C++
/**
|
|
* @file SimPolygon2D.h
|
|
* @brief Polygons for simulation use
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <gtsam_unstable/geometry/SimWall2D.h>
|
|
|
|
#include <map>
|
|
#include <random>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* General polygon class for convex polygons
|
|
*/
|
|
class GTSAM_UNSTABLE_EXPORT SimPolygon2D {
|
|
protected:
|
|
Point2Vector landmarks_;
|
|
static std::minstd_rand rng;
|
|
|
|
public:
|
|
|
|
/** Don't use this constructor, use a named one instead */
|
|
SimPolygon2D() {}
|
|
|
|
/** seed the random number generator - only needs to be done once */
|
|
static void seedGenerator(unsigned long seed);
|
|
|
|
/** Named constructor for creating triangles */
|
|
static SimPolygon2D createTriangle(const Point2& pA, const Point2& pB, const Point2& pC);
|
|
|
|
/**
|
|
* Named constructor for creating axis-aligned rectangles
|
|
* @param p is the lower-left corner
|
|
*/
|
|
static SimPolygon2D createRectangle(const Point2& p, double height, double width);
|
|
|
|
/**
|
|
* Randomly generate a triangle that does not conflict with others
|
|
* Uniformly distributed over box area, with normally distributed lengths of edges
|
|
* THROWS: std::runtime_error if can't find a position
|
|
*/
|
|
static SimPolygon2D randomTriangle(double side_len, double mean_side_len, double sigma_side_len,
|
|
double min_vertex_dist, double min_side_len, const std::vector<SimPolygon2D>& existing_polys);
|
|
|
|
/**
|
|
* Randomly generate a rectangle that does not conflict with others
|
|
* Uniformly distributed over box area, with normally distributed lengths of edges
|
|
* THROWS: std::runtime_error if can't find a position
|
|
*/
|
|
static SimPolygon2D randomRectangle(double side_len, double mean_side_len, double sigma_side_len,
|
|
double min_vertex_dist, double min_side_len, const std::vector<SimPolygon2D>& existing_polys);
|
|
|
|
// access to underlying points
|
|
const Point2& landmark(size_t i) const { return landmarks_[i]; }
|
|
size_t size() const { return landmarks_.size(); }
|
|
const Point2Vector& vertices() const { return landmarks_; }
|
|
|
|
// testable requirements
|
|
bool equals(const SimPolygon2D& p, double tol=1e-5) const;
|
|
void print(const std::string& s="") const;
|
|
|
|
/**
|
|
* Get a set of walls along the edges
|
|
*/
|
|
std::vector<SimWall2D> walls() const;
|
|
|
|
/**
|
|
* Core function for randomly generating scenarios.
|
|
* Polygons are closed, convex shapes.
|
|
* @return true if the given point is contained by this polygon
|
|
*/
|
|
bool contains(const Point2& p) const;
|
|
|
|
/**
|
|
* Checks two polygons to determine if they overlap
|
|
* @return true iff at least one vertex of one polygon is contained in the other
|
|
*/
|
|
bool overlaps(const SimPolygon2D& p) const;
|
|
|
|
/** returns true iff p is contained in any of a set of polygons */
|
|
static bool anyContains(const Point2& p, const std::vector<SimPolygon2D>& obstacles);
|
|
|
|
/** returns true iff polygon p overlaps with any of a set of polygons */
|
|
static bool anyOverlaps(const SimPolygon2D& p, const std::vector<SimPolygon2D>& obstacles);
|
|
|
|
/** returns true iff p is inside a square centered at zero with side s */
|
|
static bool insideBox(double s, const Point2& p);
|
|
|
|
/** returns true iff p is within threshold of any point in S */
|
|
static bool nearExisting(const Point2Vector& S,
|
|
const Point2& p, double threshold);
|
|
|
|
/** pick a random point uniformly over a box of side s */
|
|
static Point2 randomPoint2(double s);
|
|
|
|
/** randomly generate a Rot2 with a uniform distribution over theta */
|
|
static Rot2 randomAngle();
|
|
|
|
/** generate a distance from a normal distribution given a mean and sigma */
|
|
static double randomDistance(double mu, double sigma, double min_dist = -1.0);
|
|
|
|
/** pick a random point within a box that is further than dist d away from existing landmarks */
|
|
static Point2 randomBoundedPoint2(double boundary_size,
|
|
const Point2Vector& landmarks, double min_landmark_dist);
|
|
|
|
/** pick a random point within a box that meets above requirements, as well as staying out of obstacles */
|
|
static Point2 randomBoundedPoint2(double boundary_size,
|
|
const Point2Vector& landmarks,
|
|
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
|
|
|
|
/** pick a random point that only avoid obstacles */
|
|
static Point2 randomBoundedPoint2(double boundary_size,
|
|
const std::vector<SimPolygon2D>& obstacles);
|
|
|
|
/** pick a random point in box defined by lower left and upper right corners */
|
|
static Point2 randomBoundedPoint2(
|
|
const Point2& LL_corner, const Point2& UR_corner,
|
|
const Point2Vector& landmarks,
|
|
const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);
|
|
|
|
/** pick a random pose in a bounded area that is not in an obstacle */
|
|
static Pose2 randomFreePose(double boundary_size, const std::vector<SimPolygon2D>& obstacles);
|
|
|
|
};
|
|
|
|
typedef std::vector<SimPolygon2D> SimPolygon2DVector;
|
|
|
|
} //\namespace gtsam
|
|
|