PolyWorld.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, Rice University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Rice University nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ryan Luna */
36 
37 #ifndef POLYWORLD_H_
38 #define POLYWORLD_H_
39 
40 #include <cmath>
41 #include <string>
42 #include <vector>
43 
44 // Definition of a point (x,y).
45 typedef std::pair<double, double> Point;
46 
47 // Returns true if the difference between a and b is < eps.
48 bool cmpDouble(double a, double b, const double eps = 1e-12);
49 
50 // Returns true if the two points are the same.
51 bool equalPoints(Point p0, Point p1);
52 
53 // Definition of a planar convex polygon.
55 {
56 public:
57  // Initializes a new convex polygon from the set of points. The
58  // convex hull of the points are taken to guarantee convexity.
59  ConvexPolygon(const std::vector<Point> &coords);
60 
61  // Returns the number of vertices on the convex polygon.
62  size_t numPoints() const
63  {
64  return coordinates_.size();
65  }
66  // Returns the ith point of this polygon.
67  Point operator[](size_t i) const
68  {
69  return coordinates_[i];
70  }
71 
72  // Returns true if this polygon contains the given point.
73  bool inside(Point point) const;
74 
75 private:
76  std::vector<Point> coordinates_;
77 };
78 
79 // A representation of a bounded planar world composed of polygonal obstacles.
80 class PolyWorld
81 {
82 public:
83  PolyWorld(const std::string &worldName, const std::pair<double, double> &xBounds,
84  const std::pair<double, double> &yBounds);
85 
86  const std::string &worldName() const;
87  std::pair<double, double> xBounds() const;
88  std::pair<double, double> yBounds() const;
89 
90  size_t numObstacles() const;
91  const std::vector<ConvexPolygon> &obstacles() const;
92  const ConvexPolygon &obstacle(size_t i) const;
93 
94  // Adds the obstacle to the world.
95  void addObstacle(const ConvexPolygon &polygon);
96 
97  // Returns true if the given point is outside of the bounds
98  // defined for this world, false otherwise.
99  bool outOfBounds(Point p) const;
100 
101  // Returns true if the given point does not collide with any obstacle.
102  bool pointCollisionFree(Point p) const;
103 
104  // Write the world to the given filename in YAML format
105  void writeWorld(const char *filename) const;
106 
107 protected:
108  std::string worldName_;
109  std::vector<std::pair<double, double>> bounds_;
110 
111  std::vector<ConvexPolygon> obstacles_;
112 };
113 
114 #endif