Loading...
Searching...
No Matches
PolyWorld.cpp
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#include "PolyWorld.h"
38
39#include <cassert>
40#include <fstream>
41#include <ompl/util/Console.h>
42#include <yaml-cpp/yaml.h>
43
44namespace
45{
46 // Given a triple of points p,q, and r, classifies the two line segments (pq)
47 // and (qr) as a 'left turn', 'right turn' or collinear.
48 // Return value:
49 // 1 indicates a "left" turn
50 // -1 indicates a "right" turn
51 // 0 indicates no turn (collinear)
52 int turn(Point p, Point q, Point r)
53 {
54 const double orn = (q.first - p.first) * (r.second - p.second) - (r.first - p.first) * (q.second - p.second);
55
56 if (orn < 0.0)
57 return -1;
58 if (orn > 0.0)
59 return 1;
60 return 0;
61 }
62
63 // Returns the squared distance between p1 and p2.
64 double sqrDistance(Point p1, Point p2)
65 {
66 const double dx = p2.first - p1.first;
67 const double dy = p2.second - p1.second;
68 return dx * dx + dy * dy;
69 }
70
71} // namespace
72
73bool cmpDouble(double a, double b, const double eps)
74{
75 return fabs(a - b) < eps;
76}
77
78bool equalPoints(Point p0, Point p1)
79{
80 return cmpDouble(p0.first, p1.first) && cmpDouble(p0.second, p1.second);
81}
82
83ConvexPolygon::ConvexPolygon(const std::vector<Point> &coords)
84{
85 assert(coords.size() >= 3);
86
87 // Compute convex hull of points
88 // Find the left-most point.
89 size_t left_idx = 0;
90 for (size_t i = 1; i < coords.size(); ++i)
91 {
92 if (coords[i].first < coords[left_idx].first)
93 left_idx = i;
94 else if (coords[i].first == coords[left_idx].first)
95 {
96 if (coords[i].second < coords[left_idx].second)
97 left_idx = i;
98 }
99 }
100
101 // Giftwrap algorithm.
102 coordinates_.push_back(coords[left_idx]);
103 for (size_t i = 0; i < coordinates_.size(); ++i)
104 {
105 const Point p = coordinates_[i];
106 Point q(p);
107 // Find the furthest, 'right-most' vertex and store this in q.
108 for (size_t j = 0; j < coords.size(); ++j)
109 {
110 const int t = turn(p, q, coords[j]);
111 if (t == -1 || (t == 0 && sqrDistance(p, coords[j]) > sqrDistance(p, q)))
112 {
113 q = coords[j];
114 }
115 }
116
117 if (!equalPoints(q, coordinates_[0]))
118 coordinates_.push_back(q);
119 }
120}
121
122// This algorithm originally came from PNPOLY:
123// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
124// Count the number of times a ray extended horizontally from the y coordinate
125// of the point intersects the polygon.
126// If it intersects an odd number of times, the point is inside the polygon.
127bool ConvexPolygon::inside(Point point) const
128{
129 size_t i, j;
130 bool c = false;
131 for (i = 0, j = coordinates_.size() - 1; i < coordinates_.size(); j = i++)
132 {
133 if (((coordinates_[i].second > point.second) != (coordinates_[j].second > point.second)) &&
134 (point.first < (coordinates_[j].first - coordinates_[i].first) * (point.second - coordinates_[i].second) /
135 (coordinates_[j].second - coordinates_[i].second) +
136 coordinates_[i].first))
137 {
138 c = !c;
139 }
140 }
141 return c;
142}
143
144PolyWorld::PolyWorld(const std::string &worldName, const std::pair<double, double> &xBounds,
145 const std::pair<double, double> &yBounds)
146 : worldName_(worldName)
147{
148 assert(xBounds.first < xBounds.second);
149 assert(yBounds.first < yBounds.second);
150 bounds_.resize(2);
151 bounds_[0] = xBounds;
152 bounds_[1] = yBounds;
153}
154
155const std::string &PolyWorld::worldName() const
156{
157 return worldName_;
158}
159
160std::pair<double, double> PolyWorld::xBounds() const
161{
162 assert(bounds_.size() > 0);
163 return bounds_[0];
164}
165
166std::pair<double, double> PolyWorld::yBounds() const
167{
168 assert(bounds_.size() > 1);
169 return bounds_[1];
170}
171
172size_t PolyWorld::numObstacles() const
173{
174 return obstacles_.size();
175}
176
177const std::vector<ConvexPolygon> &PolyWorld::obstacles() const
178{
179 return obstacles_;
180}
181
182const ConvexPolygon &PolyWorld::obstacle(size_t i) const
183{
184 assert(i < obstacles_.size());
185 return obstacles_[i];
186}
187
188void PolyWorld::addObstacle(const ConvexPolygon &polygon)
189{
190 obstacles_.push_back(polygon);
191}
192
193bool PolyWorld::outOfBounds(const Point p) const
194{
195 return p.first <= bounds_[0].first || p.first >= bounds_[0].second || p.second <= bounds_[1].first ||
196 p.second >= bounds_[1].second;
197}
198
199bool PolyWorld::pointCollisionFree(const Point p) const
200{
201 bool valid = true;
202 for (size_t i = 0; i < obstacles_.size() && valid; ++i)
203 {
204 valid = !(obstacles_[i].inside(p));
205 }
206 return valid;
207}
208
209void PolyWorld::writeWorld(const char *filename) const
210{
211 std::ofstream fout;
212 fout.open(filename);
213 if (!fout)
214 {
215 OMPL_ERROR("Failed to open %s for writing", filename);
216 return;
217 }
218
219 YAML::Emitter out;
220 out << YAML::BeginMap;
221 out << YAML::Key << "name";
222 out << YAML::Value << worldName_;
223
224 out << YAML::Key << "bounds";
225 out << YAML::BeginMap;
226 out << YAML::Key << "x";
227 out << YAML::BeginMap;
228 out << YAML::Key << "lower";
229 out << YAML::Value << bounds_[0].first;
230 out << YAML::Key << "upper";
231 out << YAML::Value << bounds_[0].second;
232 out << YAML::EndMap;
233 out << YAML::Key << "y";
234 out << YAML::BeginMap;
235 out << YAML::Key << "lower";
236 out << YAML::Value << bounds_[1].first;
237 out << YAML::Key << "upper";
238 out << YAML::Value << bounds_[1].second;
239 out << YAML::EndMap;
240 out << YAML::EndMap;
241
242 out << YAML::Key << "obstacles";
243 out << YAML::BeginSeq;
244
245 for (size_t i = 0; i < obstacles_.size(); ++i)
246 {
247 const ConvexPolygon &polygon = obstacles_[i];
248
249 out << YAML::BeginMap;
250 out << "polygon";
251 out << YAML::BeginSeq;
252 for (size_t j = 0; j < polygon.numPoints(); ++j)
253 {
254 Point p = polygon[j];
255
256 out << YAML::BeginMap;
257 out << YAML::Key << "vertex";
258
259 std::stringstream str;
260 str << "(" << p.first << "," << p.second << ")";
261 out << YAML::Value << str.str();
262 out << YAML::EndMap;
263 }
264 out << YAML::EndSeq;
265 out << YAML::EndMap;
266 }
267 out << YAML::EndSeq;
268
269 // writing file
270 fout << out.c_str();
271 fout.close();
272}
273
274// // Read the YAML world file
275// void PolyWorld::readWorld(const char *worldFile)
276// {
277// YAML::Node worldConfig = YAML::LoadFile(worldFile);
278
279// if (worldConfig["name"])
280// worldName_ = worldConfig["name"].as<std::string>();
281// else
282// {
283// std::cerr << "[Warning] Name not specified in world file. Giving default name." << std::endl;
284// worldName_ = "MyWorld";
285// }
286
287// // Reading in bounds
288// bounds_.clear();
289// if (!worldConfig["bounds"])
290// {
291// std::cerr << "[Warning] Bounds for world not specified. Bounds will be inferred as the minimum bounding
292// box."
293// << std::endl;
294// }
295// else
296// {
297// YAML::Node boundsConfig = worldConfig["bounds"];
298// YAML::Node xBoundConfig = boundsConfig["x"];
299// YAML::Node yBoundConfig = boundsConfig["y"];
300
301// if (!xBoundConfig || !yBoundConfig)
302// std::cerr << "[Warning] Malformed bounds entry. Bounds will be inferred as the minimum bounding box."
303// << std::endl;
304// else
305// {
306// YAML::Node xLowerConfig = xBoundConfig["lower"];
307// YAML::Node xUpperConfig = xBoundConfig["upper"];
308
309// if (!xLowerConfig || !xUpperConfig)
310// std::cerr << "[Warning] Malformed X bounds entry. Bounds will be inferred as the minimum bounding
311// box."
312// << std::endl;
313// else
314// {
315// double lower = xLowerConfig.as<double>();
316// double upper = xUpperConfig.as<double>();
317// bounds_.push_back(std::make_pair(lower, upper));
318// }
319
320// YAML::Node yLowerConfig = yBoundConfig["lower"];
321// YAML::Node yUpperConfig = yBoundConfig["upper"];
322
323// if (!yLowerConfig || !yUpperConfig)
324// {
325// std::cerr << "[Warning] Malformed Y bounds entry. Bounds will be inferred as the minimum bounding
326// box."
327// << std::endl;
328// bounds_.clear();
329// }
330// else
331// {
332// double lower = yLowerConfig.as<double>();
333// double upper = yUpperConfig.as<double>();
334// bounds_.push_back(std::make_pair(lower, upper));
335// }
336// }
337// }
338
339// // Reading in obstacles
340// YAML::Node obstacleConfig = worldConfig["obstacles"];
341// if (!obstacleConfig)
342// std::cerr << "[Warning] No obstacles specified!" << std::endl;
343// else
344// {
345// if (!obstacleConfig.IsSequence())
346// throw std::runtime_error("Expected a sequence of geometries for obstacle tag");
347
348// for (size_t i = 0; i < obstacleConfig.size(); i++)
349// {
350// YAML::Node obstacleNode = obstacleConfig[i];
351// Geometry::Geometry *obs = readGeometry(obstacleNode);
352
353// if (obs)
354// obstacles_.push_back(obs);
355// }
356// }
357// }
358
359// Geometry::Geometry *PolyWorld::readGeometry(const YAML::Node &node) const
360// {
361// if (node["polygon"])
362// {
363// return readPolygon(node["polygon"]);
364// }
365// else if (node["rectangle"])
366// {
367// return readRectangle(node["rectangle"]);
368// }
369
370// std::cerr << "[ERROR] Unknown geometry node specified. Skipping" << std::endl;
371// return nullptr;
372// }
373
374// Geometry::ConvexPolygon *PolyWorld::readPolygon(const YAML::Node &polygonNode) const
375// {
376// if (!polygonNode.IsSequence())
377// {
378// std::cerr << "[ERROR] Expected a sequence of vertices for the polygon" << std::endl;
379// return nullptr;
380// }
381// if (polygonNode.size() < 3)
382// {
383// std::cerr << "[ERROR] Polygons must have at least three vertices" << std::endl;
384// return nullptr;
385// }
386
387// std::vector<Geometry::Point> points;
388// for (size_t i = 0; i < polygonNode.size(); i++)
389// {
390// YAML::Node vertexNode = polygonNode[i]["vertex"];
391
392// Geometry::Point p = readCoordinate(vertexNode.as<std::string>());
393// points.push_back(p);
394// }
395
396// Geometry::ConvexPolygon *polygon = new Geometry::ConvexPolygon();
397// polygon->initialize(points);
398// return polygon;
399// }
400
401// Geometry::ConvexPolygon *PolyWorld::readRectangle(const YAML::Node &rectNode) const
402// {
403// std::string upperleft = rectNode["upperleft"].as<std::string>();
404// double width = rectNode["width"].as<double>();
405// double height = rectNode["height"].as<double>();
406
407// Geometry::Point coord = readCoordinate(upperleft);
408
409// std::vector<Geometry::Point> points;
410// points.push_back(coord);
411// points.push_back(Geometry::Point(coord.first, coord.second - height));
412// points.push_back(Geometry::Point(coord.first + width, coord.second - height));
413// points.push_back(Geometry::Point(coord.first + width, coord.second));
414
415// Geometry::ConvexPolygon *polygon = new Geometry::ConvexPolygon();
416// polygon->initialize(points);
417// return polygon;
418// }
419
420// Geometry::Point PolyWorld::readCoordinate(const std::string &str) const
421// {
422// std::string coord = str;
423// // Remove all spaces
424// boost::erase_all(coord, " ");
425
426// // Split at comma
427// std::vector<std::string> strs;
428// boost::algorithm::split(strs, coord, boost::is_any_of(","));
429
430// Geometry::Point p;
431
432// // Only expecting two coordinates
433// if (strs.size() != 2)
434// {
435// std::cerr << "[Warning] Expected 2D coordinate, got " << str << std::endl;
436// return p;
437// }
438
439// // Casting coordinates to doubles. Parenthesis are stripped off, if they are there
440// p.first = ompl::stod(strs[0][0] == '(' ? strs[0].substr(1) : strs[0]);
441// p.second = ompl::stod(strs[1][strs[1].size() - 1] == ')' ? strs[1].substr(0, strs[1].size() - 1) : strs[1]);
442
443// return p;
444// }
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52