Loading...
Searching...
No Matches
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).
45typedef std::pair<double, double> Point;
46
47// Returns true if the difference between a and b is < eps.
48bool cmpDouble(double a, double b, const double eps = 1e-12);
49
50// Returns true if the two points are the same.
51bool equalPoints(Point p0, Point p1);
52
53// Definition of a planar convex polygon.
55{
56public:
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
75private:
76 std::vector<Point> coordinates_;
77};
78
79// A representation of a bounded planar world composed of polygonal obstacles.
81{
82public:
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
107protected:
108 std::string worldName_;
109 std::vector<std::pair<double, double>> bounds_;
110
111 std::vector<ConvexPolygon> obstacles_;
112};
113
114#endif