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 
44 namespace
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 
73 bool cmpDouble(double a, double b, const double eps)
74 {
75  return fabs(a - b) < eps;
76 }
77 
78 bool equalPoints(Point p0, Point p1)
79 {
80  return cmpDouble(p0.first, p1.first) && cmpDouble(p0.second, p1.second);
81 }
82 
83 ConvexPolygon::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.
127 bool 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 
144 PolyWorld::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 
155 const std::string &PolyWorld::worldName() const
156 {
157  return worldName_;
158 }
159 
160 std::pair<double, double> PolyWorld::xBounds() const
161 {
162  assert(bounds_.size() > 0);
163  return bounds_[0];
164 }
165 
166 std::pair<double, double> PolyWorld::yBounds() const
167 {
168  assert(bounds_.size() > 1);
169  return bounds_[1];
170 }
171 
172 size_t PolyWorld::numObstacles() const
173 {
174  return obstacles_.size();
175 }
176 
177 const std::vector<ConvexPolygon> &PolyWorld::obstacles() const
178 {
179  return obstacles_;
180 }
181 
182 const ConvexPolygon &PolyWorld::obstacle(size_t i) const
183 {
184  assert(i < obstacles_.size());
185  return obstacles_[i];
186 }
187 
188 void PolyWorld::addObstacle(const ConvexPolygon &polygon)
189 {
190  obstacles_.push_back(polygon);
191 }
192 
193 bool 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 
199 bool 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 
209 void 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