SimPolygon2D.h

Go to the documentation of this file.

#pragma once

#include <gtsam/geometry/Pose2.h>
#include <gtsam_unstable/geometry/SimWall2D.h>

#include <map>
#include <random>

namespace gtsam {

class GTSAM_UNSTABLE_EXPORT SimPolygon2D {
protected:
  Point2Vector landmarks_;
  static std::minstd_rand rng;

public:

  SimPolygon2D() {}

  static void seedGenerator(unsigned long seed);

  static SimPolygon2D createTriangle(const Point2& pA, const Point2& pB, const Point2& pC);

  static SimPolygon2D createRectangle(const Point2& p, double height, double width);

  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);

  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;

  std::vector<SimWall2D> walls() const;

  bool contains(const Point2& p) const;

  bool overlaps(const SimPolygon2D& p) const;

  static bool anyContains(const Point2& p, const std::vector<SimPolygon2D>& obstacles);

  static bool anyOverlaps(const SimPolygon2D& p, const std::vector<SimPolygon2D>& obstacles);

  static bool insideBox(double s, const Point2& p);

  static bool nearExisting(const Point2Vector& S,
      const Point2& p, double threshold);

  static Point2 randomPoint2(double s);

  static Rot2 randomAngle();

  static double randomDistance(double mu, double sigma, double min_dist = -1.0);

  static Point2 randomBoundedPoint2(double boundary_size,
      const Point2Vector& landmarks, double min_landmark_dist);

  static Point2 randomBoundedPoint2(double boundary_size,
      const Point2Vector& landmarks,
      const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);

  static Point2 randomBoundedPoint2(double boundary_size,
      const std::vector<SimPolygon2D>& obstacles);

  static Point2 randomBoundedPoint2(
      const Point2& LL_corner, const Point2& UR_corner,
      const Point2Vector& landmarks,
      const std::vector<SimPolygon2D>& obstacles, double min_landmark_dist);

  static Pose2 randomFreePose(double boundary_size, const std::vector<SimPolygon2D>& obstacles);

};

typedef std::vector<SimPolygon2D> SimPolygon2DVector;

} //\namespace gtsam