From 5fe5021fd7ee295e7be2788100206422ebcd70e0 Mon Sep 17 00:00:00 2001 From: Alessandro Ranellucci Date: Tue, 13 May 2014 20:06:01 +0200 Subject: [PATCH 1/5] Implemented avoid_crossing_perimeters with VisiLibity --- lib/Slic3r/GCode.pm | 4 +- lib/Slic3r/Print.pm | 5 +- xs/MANIFEST | 4 + xs/src/BoundingBox.cpp | 8 + xs/src/BoundingBox.hpp | 1 + xs/src/ExPolygon.cpp | 11 +- xs/src/ExPolygon.hpp | 2 + xs/src/ExPolygonCollection.cpp | 16 + xs/src/ExPolygonCollection.hpp | 8 + xs/src/MotionPlanner.cpp | 181 ++ xs/src/MotionPlanner.hpp | 42 + xs/src/Polyline.cpp | 8 +- xs/src/visilibity.cpp | 3659 ++++++++++++++++++++++++++++++++ xs/src/visilibity.hpp | 2169 +++++++++++++++++++ xs/t/18_motionplanner.t | 89 + xs/xsp/ExPolygon.xsp | 2 + xs/xsp/MotionPlanner.xsp | 14 + xs/xsp/my.map | 3 + xs/xsp/typemap.xspt | 3 + 19 files changed, 6216 insertions(+), 13 deletions(-) create mode 100644 xs/src/MotionPlanner.cpp create mode 100644 xs/src/MotionPlanner.hpp create mode 100644 xs/src/visilibity.cpp create mode 100644 xs/src/visilibity.hpp create mode 100644 xs/t/18_motionplanner.t create mode 100644 xs/xsp/MotionPlanner.xsp diff --git a/lib/Slic3r/GCode.pm b/lib/Slic3r/GCode.pm index 907a0048a..362128947 100644 --- a/lib/Slic3r/GCode.pm +++ b/lib/Slic3r/GCode.pm @@ -111,8 +111,8 @@ sub change_layer { $self->_layer_islands($layer->islands); $self->_upper_layer_islands($layer->upper_layer ? $layer->upper_layer->islands : []); if ($self->print_config->avoid_crossing_perimeters) { - $self->layer_mp(Slic3r::GCode::MotionPlanner->new( - islands => union_ex([ map @$_, @{$layer->slices} ], 1), + $self->layer_mp(Slic3r::MotionPlanner->new( + union_ex([ map @$_, @{$layer->slices} ], 1), )); } diff --git a/lib/Slic3r/Print.pm b/lib/Slic3r/Print.pm index cdf26c9e0..af52517a1 100644 --- a/lib/Slic3r/Print.pm +++ b/lib/Slic3r/Print.pm @@ -909,10 +909,7 @@ sub write_gcode { } } } - $gcodegen->external_mp(Slic3r::GCode::MotionPlanner->new( - islands => union_ex([ map @$_, @islands ]), - internal => 0, - )); + $gcodegen->external_mp(Slic3r::MotionPlanner->new(union_ex([ map @$_, @islands ]))); } # calculate wiping points if needed diff --git a/xs/MANIFEST b/xs/MANIFEST index d8334bcd8..184998f8e 100644 --- a/xs/MANIFEST +++ b/xs/MANIFEST @@ -1676,6 +1676,8 @@ src/Line.cpp src/Line.hpp src/Model.cpp src/Model.hpp +src/MotionPlanner.cpp +src/MotionPlanner.hpp src/MultiPoint.cpp src/MultiPoint.hpp src/myinit.h @@ -1716,6 +1718,8 @@ src/SVG.hpp src/TriangleMesh.cpp src/TriangleMesh.hpp src/utils.cpp +src/visilibity.cpp +src/visilibity.hpp t/01_trianglemesh.t t/03_point.t t/04_expolygon.t diff --git a/xs/src/BoundingBox.cpp b/xs/src/BoundingBox.cpp index 9975eb025..c498f45a7 100644 --- a/xs/src/BoundingBox.cpp +++ b/xs/src/BoundingBox.cpp @@ -58,6 +58,14 @@ BoundingBox::polygon(Polygon* polygon) const polygon->points[3].y = this->max.y; } +Polygon +BoundingBox::polygon() const +{ + Polygon p; + this->polygon(&p); + return p; +} + template void BoundingBoxBase::scale(double factor) { diff --git a/xs/src/BoundingBox.hpp b/xs/src/BoundingBox.hpp index 92c2d063f..39af4bf32 100644 --- a/xs/src/BoundingBox.hpp +++ b/xs/src/BoundingBox.hpp @@ -46,6 +46,7 @@ class BoundingBox : public BoundingBoxBase { public: void polygon(Polygon* polygon) const; + Polygon polygon() const; BoundingBox() {}; BoundingBox(const Points &points) : BoundingBoxBase(points) {}; diff --git a/xs/src/ExPolygon.cpp b/xs/src/ExPolygon.cpp index e2ee364a8..9d4c58094 100644 --- a/xs/src/ExPolygon.cpp +++ b/xs/src/ExPolygon.cpp @@ -84,11 +84,14 @@ ExPolygon::is_valid() const bool ExPolygon::contains_line(const Line &line) const { - Polylines pl; - pl.push_back(line); - + return this->contains_polyline(line); +} + +bool +ExPolygon::contains_polyline(const Polyline &polyline) const +{ Polylines pl_out; - diff(pl, *this, pl_out); + diff((Polylines)polyline, *this, pl_out); return pl_out.empty(); } diff --git a/xs/src/ExPolygon.hpp b/xs/src/ExPolygon.hpp index e7c56d8d8..be6065914 100644 --- a/xs/src/ExPolygon.hpp +++ b/xs/src/ExPolygon.hpp @@ -2,6 +2,7 @@ #define slic3r_ExPolygon_hpp_ #include "Polygon.hpp" +#include "Polyline.hpp" #include namespace Slic3r { @@ -22,6 +23,7 @@ class ExPolygon double area() const; bool is_valid() const; bool contains_line(const Line &line) const; + bool contains_polyline(const Polyline &polyline) const; bool contains_point(const Point &point) const; Polygons simplify_p(double tolerance) const; ExPolygons simplify(double tolerance) const; diff --git a/xs/src/ExPolygonCollection.cpp b/xs/src/ExPolygonCollection.cpp index 67a22682e..3de86e7c6 100644 --- a/xs/src/ExPolygonCollection.cpp +++ b/xs/src/ExPolygonCollection.cpp @@ -3,6 +3,17 @@ namespace Slic3r { +ExPolygonCollection::operator Points() const +{ + Points points; + Polygons pp = *this; + for (Polygons::const_iterator poly = pp.begin(); poly != pp.end(); ++poly) { + for (Points::const_iterator point = poly->points.begin(); point != poly->points.end(); ++point) + points.push_back(*point); + } + return points; +} + ExPolygonCollection::operator Polygons() const { Polygons polygons; @@ -15,6 +26,11 @@ ExPolygonCollection::operator Polygons() const return polygons; } +ExPolygonCollection::operator ExPolygons&() +{ + return this->expolygons; +} + void ExPolygonCollection::scale(double factor) { diff --git a/xs/src/ExPolygonCollection.hpp b/xs/src/ExPolygonCollection.hpp index 75f2f5052..f6a27284f 100644 --- a/xs/src/ExPolygonCollection.hpp +++ b/xs/src/ExPolygonCollection.hpp @@ -6,11 +6,19 @@ namespace Slic3r { +class ExPolygonCollection; +typedef std::vector ExPolygonCollections; + class ExPolygonCollection { public: ExPolygons expolygons; + + ExPolygonCollection() {}; + ExPolygonCollection(const ExPolygons &expolygons) : expolygons(expolygons) {}; + operator Points() const; operator Polygons() const; + operator ExPolygons&(); void scale(double factor); void translate(double x, double y); void rotate(double angle, const Point ¢er); diff --git a/xs/src/MotionPlanner.cpp b/xs/src/MotionPlanner.cpp new file mode 100644 index 000000000..2df76efd9 --- /dev/null +++ b/xs/src/MotionPlanner.cpp @@ -0,0 +1,181 @@ +#include "BoundingBox.hpp" +#include "MotionPlanner.hpp" + +namespace Slic3r { + +MotionPlanner::MotionPlanner(const ExPolygons &islands) + : islands(islands), initialized(false) +{} + +MotionPlanner::~MotionPlanner() +{ + for (std::vector::iterator env = this->envs.begin(); env != this->envs.end(); ++env) + delete *env; + for (std::vector::iterator graph = this->graphs.begin(); graph != this->graphs.end(); ++graph) + delete *graph; +} + +void +MotionPlanner::initialize() +{ + if (this->initialized) return; + + // loop through islands in order to create inner expolygons and collect their contours + this->inner.reserve(this->islands.size()); + Polygons outer_holes; + for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) { + this->inner.push_back(ExPolygonCollection()); + offset_ex(*island, this->inner.back(), -MP_INNER_MARGIN); + + outer_holes.push_back(island->contour); + } + + // grow island contours in order to prepare holes of the outer environment + // This is actually wrong because it might merge contours that are close, + // thus confusing the island check in shortest_path() below + //offset(outer_holes, outer_holes, +MP_OUTER_MARGIN); + + // generate outer contour as bounding box of everything + Points points; + for (Polygons::const_iterator contour = outer_holes.begin(); contour != outer_holes.end(); ++contour) + points.insert(points.end(), contour->points.begin(), contour->points.end()); + BoundingBox bb(points); + + // grow outer contour + Polygons contour; + offset(bb.polygon(), contour, +MP_OUTER_MARGIN); + assert(contour.size() == 1); + + // make expolygon for outer environment + ExPolygons outer; + diff(contour, outer_holes, outer); + assert(outer.size() == 1); + this->outer = outer.front(); + + this->envs.resize(this->islands.size() + 1, NULL); + this->graphs.resize(this->islands.size() + 1, NULL); + this->initialized = true; +} + +void +MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyline) +{ + if (!this->initialized) this->initialize(); + + // Are both points in the same island? + int island_idx = -1; + for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) { + if (island->contains_point(from) && island->contains_point(to)) { + island_idx = island - this->islands.begin(); + break; + } + } + + // Generate environment. + this->generate_environment(island_idx); + + // Now check whether points are inside the environment. + Point inner_from = from; + Point inner_to = to; + bool from_is_inside, to_is_inside; + if (island_idx == -1) { + if (!(from_is_inside = this->outer.contains_point(from))) { + // Find the closest inner point to start from. + from.nearest_point(this->outer, &inner_from); + } + if (!(to_is_inside = this->outer.contains_point(to))) { + // Find the closest inner point to start from. + to.nearest_point(this->outer, &inner_to); + } + } else { + if (!(from_is_inside = this->inner[island_idx].contains_point(from))) { + // Find the closest inner point to start from. + from.nearest_point(this->inner[island_idx], &inner_from); + } + if (!(to_is_inside = this->inner[island_idx].contains_point(to))) { + // Find the closest inner point to start from. + to.nearest_point(this->inner[island_idx], &inner_to); + } + } + + // perform actual path search + *polyline = convert_polyline(this->envs[island_idx + 1]->shortest_path(convert_point(inner_from), + convert_point(inner_to), *this->graphs[island_idx + 1], SCALED_EPSILON)); + + // if start point was outside the environment, prepend it + if (!from_is_inside) polyline->points.insert(polyline->points.begin(), from); + + // if end point was outside the environment, append it + if (!to_is_inside) polyline->points.push_back(to); +} + +void +MotionPlanner::generate_environment(int island_idx) +{ + if (this->envs[island_idx + 1] != NULL) return; + + Polygons pp; + if (island_idx == -1) { + pp = this->outer; + } else { + pp = this->inner[island_idx]; + } + + // populate VisiLibity polygons + std::vector v_polygons; + for (Polygons::const_iterator p = pp.begin(); p != pp.end(); ++p) + v_polygons.push_back(convert_polygon(*p)); + + // generate graph and environment + this->envs[island_idx + 1] = new VisiLibity::Environment(v_polygons); + this->graphs[island_idx + 1] = new VisiLibity::Visibility_Graph(*this->envs[island_idx + 1], SCALED_EPSILON); +} + +VisiLibity::Polyline +MotionPlanner::convert_polyline(const Polyline &polyline) +{ + VisiLibity::Polyline v_polyline; + for (Points::const_iterator point = polyline.points.begin(); point != polyline.points.end(); ++point) { + v_polyline.push_back(convert_point(*point)); + } + return v_polyline; +} + +Polyline +MotionPlanner::convert_polyline(const VisiLibity::Polyline &v_polyline) +{ + Polyline polyline; + polyline.points.reserve(v_polyline.size()); + for (size_t i = 0; i < v_polyline.size(); ++i) { + polyline.points.push_back(convert_point(v_polyline[i])); + } + return polyline; +} + +VisiLibity::Polygon +MotionPlanner::convert_polygon(const Polygon &polygon) +{ + VisiLibity::Polygon v_polygon; + for (Points::const_iterator point = polygon.points.begin(); point != polygon.points.end(); ++point) { + v_polygon.push_back(convert_point(*point)); + } + return v_polygon; +} + +VisiLibity::Point +MotionPlanner::convert_point(const Point &point) +{ + return VisiLibity::Point(point.x, point.y); +} + +Point +MotionPlanner::convert_point(const VisiLibity::Point &v_point) +{ + return Point((coord_t)v_point.x(), (coord_t)v_point.y()); +} + +#ifdef SLIC3RXS +REGISTER_CLASS(MotionPlanner, "MotionPlanner"); +#endif + +} diff --git a/xs/src/MotionPlanner.hpp b/xs/src/MotionPlanner.hpp new file mode 100644 index 000000000..b0fd29ee0 --- /dev/null +++ b/xs/src/MotionPlanner.hpp @@ -0,0 +1,42 @@ +#ifndef slic3r_MotionPlanner_hpp_ +#define slic3r_MotionPlanner_hpp_ + +#include +#include "ClipperUtils.hpp" +#include "ExPolygonCollection.hpp" +#include "Polyline.hpp" +#include "visilibity.hpp" +#include + +#define MP_INNER_MARGIN scale_(1.0) +#define MP_OUTER_MARGIN scale_(2.0) + +namespace Slic3r { + +class MotionPlanner +{ + public: + MotionPlanner(const ExPolygons &islands); + ~MotionPlanner(); + void shortest_path(const Point &from, const Point &to, Polyline* polyline); + + private: + ExPolygons islands; + bool initialized; + ExPolygon outer; + ExPolygonCollections inner; + std::vector envs; + std::vector graphs; + + void initialize(); + void generate_environment(int island_idx); + static VisiLibity::Polyline convert_polyline(const Polyline &polyline); + static Polyline convert_polyline(const VisiLibity::Polyline &v_polyline); + static VisiLibity::Polygon convert_polygon(const Polygon &polygon); + static VisiLibity::Point convert_point(const Point &point); + static Point convert_point(const VisiLibity::Point &v_point); +}; + +} + +#endif diff --git a/xs/src/Polyline.cpp b/xs/src/Polyline.cpp index 63778d5a5..d9dcb7203 100644 --- a/xs/src/Polyline.cpp +++ b/xs/src/Polyline.cpp @@ -30,9 +30,11 @@ Lines Polyline::lines() const { Lines lines; - lines.reserve(this->points.size() - 1); - for (Points::const_iterator it = this->points.begin(); it != this->points.end()-1; ++it) { - lines.push_back(Line(*it, *(it + 1))); + if (this->points.size() >= 2) { + lines.reserve(this->points.size() - 1); + for (Points::const_iterator it = this->points.begin(); it != this->points.end()-1; ++it) { + lines.push_back(Line(*it, *(it + 1))); + } } return lines; } diff --git a/xs/src/visilibity.cpp b/xs/src/visilibity.cpp new file mode 100644 index 000000000..5f1f5bb9f --- /dev/null +++ b/xs/src/visilibity.cpp @@ -0,0 +1,3659 @@ +/** + * \file visilibity.cpp + * \author Karl J. Obermeyer + * \date March 20, 2008 + * +\remarks +VisiLibity: A Floating-Point Visibility Algorithms Library, +Copyright (C) 2008 Karl J. Obermeyer (karl.obermeyer [ at ] gmail.com) + +This file is part of VisiLibity. + +VisiLibity is free software: you can redistribute it and/or modify it under +the terms of the GNU Lesser General Public License as published by the +Free Software Foundation, either version 3 of the License, or (at your +option) any later version. + +VisiLibity is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or +FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public +License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with VisiLibity. If not, see . +*/ + + +#include "visilibity.hpp" //VisiLibity header +#include //math functions in std namespace +#include +#include //queue and priority_queue +#include //priority queues with iteration, + //integrated keys +#include +#include //sorting, min, max, reverse +#include //rand and srand +#include //Unix time +#include //file I/O +#include +#include //gives C-string manipulation +#include //string class +#include //assertions + + +///Hide helping functions in unnamed namespace (local to .C file). +namespace +{ + +} + + +/// VisiLibity's sole namespace +namespace VisiLibity +{ + + double uniform_random_sample(double lower_bound, double upper_bound) + { + assert( lower_bound <= upper_bound ); + if( lower_bound == upper_bound ) + return lower_bound; + double sample_point; + double span = upper_bound - lower_bound; + sample_point = lower_bound + + span * static_cast( std::rand() ) + / static_cast( RAND_MAX ); + return sample_point; + } + + + //Point + + + Point Point::projection_onto(const Line_Segment& line_segment_temp) const + { + assert( *this == *this + and line_segment_temp.size() > 0 ); + + if(line_segment_temp.size() == 1) + return line_segment_temp.first(); + //The projection of point_temp onto the line determined by + //line_segment_temp can be represented as an affine combination + //expressed in the form projection of Point = + //theta*line_segment_temp.first + + //(1.0-theta)*line_segment_temp.second. if theta is outside + //the interval [0,1], then one of the Line_Segment's endpoints + //must be closest to calling Point. + double theta = + ( (line_segment_temp.second().x()-x()) + *(line_segment_temp.second().x() + -line_segment_temp.first().x()) + + (line_segment_temp.second().y()-y()) + *(line_segment_temp.second().y() + -line_segment_temp.first().y()) ) + / ( pow(line_segment_temp.second().x() + -line_segment_temp.first().x(),2) + + pow(line_segment_temp.second().y() + -line_segment_temp.first().y(),2) ); + //std::cout << "\E[1;37;40m" << "Theta is: " << theta << "\x1b[0m" + //<< std::endl; + if( (0.0<=theta) and (theta<=1.0) ) + return theta*line_segment_temp.first() + + (1.0-theta)*line_segment_temp.second(); + //Else pick closest endpoint. + if( distance(*this, line_segment_temp.first()) + < distance(*this, line_segment_temp.second()) ) + return line_segment_temp.first(); + return line_segment_temp.second(); + } + + + Point Point::projection_onto(const Ray& ray_temp) const + { + assert( *this == *this + and ray_temp == ray_temp ); + + //Construct a Line_Segment parallel with the Ray which is so long, + //that the projection of the the calling Point onto that + //Line_Segment must be the same as the projection of the calling + //Point onto the Ray. + double R = distance( *this , ray_temp.base_point() ); + Line_Segment seg_approx = + Line_Segment( ray_temp.base_point(), ray_temp.base_point() + + Point( R*std::cos(ray_temp.bearing().get()), + R*std::sin(ray_temp.bearing().get()) ) ); + return projection_onto( seg_approx ); + } + + + Point Point::projection_onto(const Polyline& polyline_temp) const + { + assert( *this == *this + and polyline_temp.size() > 0 ); + + Point running_projection = polyline_temp[0]; + double running_min = distance(*this, running_projection); + Point point_temp; + for(unsigned i=0; i<=polyline_temp.size()-1; i++){ + point_temp = projection_onto( Line_Segment(polyline_temp[i], + polyline_temp[i+1]) ); + if( distance(*this, point_temp) < running_min ){ + running_projection = point_temp; + running_min = distance(*this, running_projection); + } + } + return running_projection; + } + + + Point Point::projection_onto_vertices_of(const Polygon& polygon_temp) const + { + assert(*this == *this + and polygon_temp.vertices_.size() > 0 ); + + Point running_projection = polygon_temp[0]; + double running_min = distance(*this, running_projection); + for(unsigned i=1; i<=polygon_temp.n()-1; i++){ + if( distance(*this, polygon_temp[i]) < running_min ){ + running_projection = polygon_temp[i]; + running_min = distance(*this, running_projection); + } + } + return running_projection; + } + + + Point Point::projection_onto_vertices_of(const Environment& + environment_temp) const + { + assert(*this == *this + and environment_temp.n() > 0 ); + + Point running_projection + = projection_onto_vertices_of(environment_temp.outer_boundary_); + double running_min = distance(*this, running_projection); + Point point_temp; + for(unsigned i=0; i 0 ); + + Point running_projection = polygon_temp[0]; + double running_min = distance(*this, running_projection); + Point point_temp; + for(unsigned i=0; i<=polygon_temp.n()-1; i++){ + point_temp = projection_onto( Line_Segment(polygon_temp[i], + polygon_temp[i+1]) ); + if( distance(*this, point_temp) < running_min ){ + running_projection = point_temp; + running_min = distance(*this, running_projection); + } + } + return running_projection; + } + + + Point Point::projection_onto_boundary_of(const Environment& + environment_temp) const + { + assert( *this == *this + and environment_temp.n() > 0 ); + + Point running_projection + = projection_onto_boundary_of(environment_temp.outer_boundary_); + double running_min = distance(*this, running_projection); + Point point_temp; + for(unsigned i=0; i 0 ); + + if( distance(*this, projection_onto_boundary_of(polygon_temp) ) + <= epsilon ){ + return true; + } + return false; + } + + + bool Point::on_boundary_of(const Environment& environment_temp, + double epsilon) const + { + assert( *this == *this + and environment_temp.outer_boundary_.n() > 0 ); + + if( distance(*this, projection_onto_boundary_of(environment_temp) ) + <= epsilon ){ + return true; + } + return false; + } + + + bool Point::in(const Line_Segment& line_segment_temp, + double epsilon) const + { + assert( *this == *this + and line_segment_temp.size() > 0 ); + + if( distance(*this, line_segment_temp) < epsilon ) + return true; + return false; + } + + + bool Point::in_relative_interior_of(const Line_Segment& line_segment_temp, + double epsilon) const + { + assert( *this == *this + and line_segment_temp.size() > 0 ); + + return in(line_segment_temp, epsilon) + and distance(*this, line_segment_temp.first()) > epsilon + and distance(*this, line_segment_temp.second()) > epsilon; + } + + + bool Point::in(const Polygon& polygon_temp, + double epsilon) const + { + assert( *this == *this + and polygon_temp.vertices_.size() > 0 ); + + int n = polygon_temp.vertices_.size(); + if( on_boundary_of(polygon_temp, epsilon) ) + return true; + // Then check the number of times a ray emanating from the Point + // crosses the boundary of the Polygon. An odd number of + // crossings indicates the Point is in the interior of the + // Polygon. Based on + // http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html + int i, j; bool c = false; + for (i = 0, j = n-1; i < n; j = i++){ + if ( (((polygon_temp[i].y() <= y()) + and (y() < polygon_temp[j].y())) + or ((polygon_temp[j].y() <= y()) + and (y() < polygon_temp[i].y()))) + and ( x() < (polygon_temp[j].x() + - polygon_temp[i].x()) + * (y() - polygon_temp[i].y()) + / (polygon_temp[j].y() + - polygon_temp[i].y()) + + polygon_temp[i].x()) ) + c = !c; + } + return c; + } + + + bool Point::in(const Environment& environment_temp, double epsilon) const + { + assert( *this == *this + and environment_temp.outer_boundary_.n() > 0 ); + + //On outer boundary? + if( on_boundary_of(environment_temp, epsilon) ) + return true; + //Not in outer boundary? + if( !in(environment_temp.outer_boundary_, epsilon) ) + return false; + //In hole? + for(unsigned i=0; i 0 ); + + if( distance(line_segment_temp.first(), *this)<=epsilon + or distance(line_segment_temp.second(), *this)<=epsilon ) + return true; + return false; + } + + + void Point::snap_to_vertices_of(const Polygon& polygon_temp, + double epsilon) + { + assert( *this == *this + and polygon_temp.n() > 0 ); + + Point point_temp( this->projection_onto_vertices_of(polygon_temp) ); + if( distance( *this , point_temp ) <= epsilon ) + *this = point_temp; + } + void Point::snap_to_vertices_of(const Environment& environment_temp, + double epsilon) + { + assert( *this == *this + and environment_temp.n() > 0 ); + + Point point_temp( this->projection_onto_vertices_of(environment_temp) ); + if( distance( *this , point_temp ) <= epsilon ) + *this = point_temp; + } + + + void Point::snap_to_boundary_of(const Polygon& polygon_temp, + double epsilon) + { + assert( *this == *this + and polygon_temp.n() > 0 ); + + Point point_temp( this->projection_onto_boundary_of(polygon_temp) ); + if( distance( *this , point_temp ) <= epsilon ) + *this = point_temp; + } + void Point::snap_to_boundary_of(const Environment& environment_temp, + double epsilon) + { + assert( *this == *this + and environment_temp.n() > 0 ); + + Point point_temp( this->projection_onto_boundary_of(environment_temp) ); + if( distance( *this , point_temp ) <= epsilon ) + *this = point_temp; + } + + + bool operator == (const Point& point1, const Point& point2) + { return ( ( point1.x() == point2.x() ) + and ( point1.y() == point2.y() ) ); } + bool operator != (const Point& point1, const Point& point2) + { return !( point1 == point2 ); } + + + bool operator < (const Point& point1, const Point& point2) + { + if( point1 != point1 or point2 != point2 ) + return false; + if(point1.x() < point2.x()) + return true; + else if( ( point1.x() == point2.x() ) + and ( point1.y() < point2.y() ) ) + return true; + return false; + } + bool operator > (const Point& point1, const Point& point2) + { + if( point1 != point1 or point2 != point2 ) + return false; + if( point1.x() > point2.x() ) + return true; + else if( ( point1.x() == point2.x() ) + and ( point1.y() > point2.y() ) ) + return true; + return false; + } + bool operator >= (const Point& point1, const Point& point2) + { + if( point1 != point1 or point2 != point2 ) + return false; + return !( point1 < point2 ); + } + bool operator <= (const Point& point1, const Point& point2) + { + if( point1 != point1 or point2 != point2 ) + return false; + return !( point1 > point2 ); + } + + + Point operator + (const Point& point1, const Point& point2) + { + return Point( point1.x() + point2.x(), + point1.y() + point2.y() ); + } + Point operator - (const Point& point1, const Point& point2) + { + return Point( point1.x() - point2.x(), + point1.y() - point2.y() ); + } + + + Point operator * (const Point& point1, const Point& point2) + { + return Point( point1.x()*point2.x(), + point1.y()*point2.y() ); + } + + + Point operator * (double scalar, const Point& point2) + { + return Point( scalar*point2.x(), + scalar*point2.y()); + } + Point operator * (const Point& point1, double scalar) + { + return Point( scalar*point1.x(), + scalar*point1.y()); + } + + + double cross(const Point& point1, const Point& point2) + { + assert( point1 == point1 + and point2 == point2 ); + + //The area of the parallelogram created by the Points viewed as vectors. + return point1.x()*point2.y() - point2.x()*point1.y(); + } + + + double distance(const Point& point1, const Point& point2) + { + assert( point1 == point1 + and point2 == point2 ); + + return sqrt( pow( point1.x() - point2.x() , 2 ) + + pow( point1.y() - point2.y() , 2 ) ); + } + + + double distance(const Point& point_temp, + const Line_Segment& line_segment_temp) + { + assert( point_temp == point_temp + and line_segment_temp.size() > 0 ); + + return distance( point_temp, + point_temp.projection_onto(line_segment_temp) ); + } + double distance(const Line_Segment& line_segment_temp, + const Point& point_temp) + { + return distance( point_temp, + line_segment_temp ); + } + + + double distance(const Point& point_temp, + const Ray& ray_temp) + { + assert( point_temp == point_temp + and ray_temp == ray_temp ); + return distance( point_temp, + point_temp.projection_onto(ray_temp) ); + } + double distance(const Ray& ray_temp, + const Point& point_temp) + { + return distance( point_temp, + point_temp.projection_onto(ray_temp) ); + } + + + double distance(const Point& point_temp, + const Polyline& polyline_temp) + { + assert( point_temp == point_temp + and polyline_temp.size() > 0 ); + + double running_min = distance(point_temp, polyline_temp[0]); + double distance_temp; + for(unsigned i=0; i 0); + + double running_min = distance(point_temp, polygon_temp[0]); + double distance_temp; + for(unsigned i=0; i<=polygon_temp.n(); i++){ + distance_temp = distance(point_temp, Line_Segment(polygon_temp[i], + polygon_temp[i+1]) ); + if(distance_temp < running_min) + running_min = distance_temp; + } + return running_min; + } + double boundary_distance(const Polygon& polygon_temp, const Point& point_temp) + { + return boundary_distance(point_temp, polygon_temp); + } + + + double boundary_distance(const Point& point_temp, + const Environment& environment_temp) + { + assert( point_temp == point_temp + and environment_temp.n() > 0 ); + + double running_min = distance(point_temp, environment_temp[0][0]); + double distance_temp; + for(unsigned i=0; i <= environment_temp.h(); i++){ + distance_temp = boundary_distance(point_temp, environment_temp[i]); + if(distance_temp < running_min) + running_min = distance_temp; + } + return running_min; + } + double boundary_distance(const Environment& environment_temp, + const Point& point_temp) + { + return boundary_distance(point_temp, environment_temp); + } + + + std::ostream& operator << (std::ostream& outs, const Point& point_temp) + { + outs << point_temp.x() << " " << point_temp.y(); + return outs; + } + + + //Line_Segment + + + Line_Segment::Line_Segment() + { + endpoints_ = NULL; + size_ = 0; + } + + + Line_Segment::Line_Segment(const Line_Segment& line_segment_temp) + { + switch(line_segment_temp.size_){ + case 0: + endpoints_ = NULL; + size_ = 0; + break; + case 1: + endpoints_ = new Point[1]; + endpoints_[0] = line_segment_temp.endpoints_[0]; + size_ = 1; + break; + case 2: + endpoints_ = new Point[2]; + endpoints_[0] = line_segment_temp.endpoints_[0]; + endpoints_[1] = line_segment_temp.endpoints_[1]; + size_ = 2; + } + } + + + Line_Segment::Line_Segment(const Point& point_temp) + { + endpoints_ = new Point[1]; + endpoints_[0] = point_temp; + size_ = 1; + } + + + Line_Segment::Line_Segment(const Point& first_point_temp, + const Point& second_point_temp, double epsilon) + { + if( distance(first_point_temp, second_point_temp) <= epsilon ){ + endpoints_ = new Point[1]; + endpoints_[0] = first_point_temp; + size_ = 1; + } + else{ + endpoints_ = new Point[2]; + endpoints_[0] = first_point_temp; + endpoints_[1] = second_point_temp; + size_ = 2; + } + } + + + Point Line_Segment::first() const + { + assert( size() > 0 ); + + return endpoints_[0]; + } + + + Point Line_Segment::second() const + { + assert( size() > 0 ); + + if(size_==2) + return endpoints_[1]; + else + return endpoints_[0]; + } + + + Point Line_Segment::midpoint() const + { + assert( size_ > 0 ); + + return 0.5*( first() + second() ); + } + + + double Line_Segment::length() const + { + assert( size_ > 0 ); + + return distance(first(), second()); + } + + + bool Line_Segment::is_in_standard_form() const + { + assert( size_ > 0); + + if(size_<2) + return true; + return first() <= second(); + } + + + Line_Segment& Line_Segment::operator = (const Line_Segment& line_segment_temp) + { + //Makes sure not to delete dynamic vars before they're copied. + if(this==&line_segment_temp) + return *this; + delete [] endpoints_; + switch(line_segment_temp.size_){ + case 0: + endpoints_ = NULL; + size_ = 0; + break; + case 1: + endpoints_ = new Point[1]; + endpoints_[0] = line_segment_temp.endpoints_[0]; + size_ = 1; + break; + case 2: + endpoints_ = new Point[2]; + endpoints_[0] = line_segment_temp.endpoints_[0]; + endpoints_[1] = line_segment_temp.endpoints_[1]; + size_ = 2; + } + return *this; + } + + + void Line_Segment::set_first(const Point& point_temp, double epsilon) + { + Point second_point_temp; + switch(size_){ + case 0: + endpoints_ = new Point[1]; + endpoints_[0] = point_temp; + size_ = 1; + break; + case 1: + if( distance(endpoints_[0], point_temp) <= epsilon ) + { endpoints_[0] = point_temp; return; } + second_point_temp = endpoints_[0]; + delete [] endpoints_; + endpoints_ = new Point[2]; + endpoints_[0] = point_temp; + endpoints_[1] = second_point_temp; + size_ = 2; + break; + case 2: + if( distance(point_temp, endpoints_[1]) > epsilon ) + { endpoints_[0] = point_temp; return; } + delete [] endpoints_; + endpoints_ = new Point[1]; + endpoints_[0] = point_temp; + size_ = 1; + } + } + + + void Line_Segment::set_second(const Point& point_temp, double epsilon) + { + Point first_point_temp; + switch(size_){ + case 0: + endpoints_ = new Point[1]; + endpoints_[0] = point_temp; + size_ = 1; + break; + case 1: + if( distance(endpoints_[0], point_temp) <= epsilon ) + { endpoints_[0] = point_temp; return; } + first_point_temp = endpoints_[0]; + delete [] endpoints_; + endpoints_ = new Point[2]; + endpoints_[0] = first_point_temp; + endpoints_[1] = point_temp; + size_ = 2; + break; + case 2: + if( distance(endpoints_[0], point_temp) > epsilon ) + { endpoints_[1] = point_temp; return; } + delete [] endpoints_; + endpoints_ = new Point[1]; + endpoints_[0] = point_temp; + size_ = 1; + } + } + + + void Line_Segment::reverse() + { + if(size_<2) + return; + Point point_temp(first()); + endpoints_[0] = second(); + endpoints_[1] = point_temp; + } + + + void Line_Segment::enforce_standard_form() + { + if(first() > second()) + reverse(); + } + + + void Line_Segment::clear() + { + delete [] endpoints_; + endpoints_ = NULL; + size_ = 0; + } + + + Line_Segment::~Line_Segment() + { + delete [] endpoints_; + } + + + bool operator == (const Line_Segment& line_segment1, + const Line_Segment& line_segment2) + { + if( line_segment1.size() != line_segment2.size() + or line_segment1.size() == 0 + or line_segment2.size() == 0 ) + return false; + else if( line_segment1.first() == line_segment2.first() + and line_segment1.second() == line_segment2.second() ) + return true; + else + return false; + } + + + bool operator != (const Line_Segment& line_segment1, + const Line_Segment& line_segment2) + { + return !( line_segment1 == line_segment2 ); + } + + + bool equivalent(Line_Segment line_segment1, + Line_Segment line_segment2, double epsilon) + { + if( line_segment1.size() != line_segment2.size() + or line_segment1.size() == 0 + or line_segment2.size() == 0 ) + return false; + else if( ( distance( line_segment1.first(), + line_segment2.first() ) <= epsilon + and distance( line_segment1.second(), + line_segment2.second() ) <= epsilon ) + or ( distance( line_segment1.first(), + line_segment2.second() ) <= epsilon + and distance( line_segment1.second(), + line_segment2.first() ) <= epsilon ) ) + return true; + else + return false; + } + + + double distance(const Line_Segment& line_segment1, + const Line_Segment& line_segment2) + { + assert( line_segment1.size() > 0 and line_segment2.size() > 0 ); + + if(intersect_proper(line_segment1, line_segment2)) + return 0; + //But if two line segments intersect improperly, the distance + //between them is equal to the minimum of the distances between + //all 4 endpoints_ and their respective projections onto the line + //segment they don't belong to. + double running_min, distance_temp; + running_min = distance(line_segment1.first(), line_segment2); + distance_temp = distance(line_segment1.second(), line_segment2); + if(distance_temp 0 and polygon.n() > 0 ); + + double running_min = distance( line_segment , polygon[0] ); + if( polygon.n() > 1 ) + for(unsigned i=0; i d ) + running_min = d; + } + return running_min; + } + double boundary_distance(const Polygon& polygon, + const Line_Segment& line_segment) + { return boundary_distance( line_segment , polygon ); } + + + bool intersect(const Line_Segment& line_segment1, + const Line_Segment& line_segment2, double epsilon) + { + if( line_segment1.size() == 0 + or line_segment2.size() == 0 ) + return false; + if( distance(line_segment1, line_segment2) <= epsilon ) + return true; + return false; + } + + + bool intersect_proper(const Line_Segment& line_segment1, + const Line_Segment& line_segment2, double epsilon) + { + if( line_segment1.size() == 0 + or line_segment2.size() == 0 ) + return false; + + //Declare new vars just for readability. + Point a( line_segment1.first() ); + Point b( line_segment1.second() ); + Point c( line_segment2.first() ); + Point d( line_segment2.second() ); + //First find the minimum of the distances between all 4 endpoints_ + //and their respective projections onto the opposite line segment. + double running_min, distance_temp; + running_min = distance(a, line_segment2); + distance_temp = distance(b, line_segment2); + if(distance_temp return empty segment. + if( !intersect(line_segment1, line_segment2, epsilon) ) + return line_segment_temp; + //Declare new vars just for readability. + Point a( line_segment1.first() ); + Point b( line_segment1.second() ); + Point c( line_segment2.first() ); + Point d( line_segment2.second() ); + if( intersect_proper(line_segment1, line_segment2, epsilon) ){ + //Use formula from O'Rourke's "Computational Geometry in C", p. 221. + //Note D=0 iff the line segments are parallel. + double D = a.x()*( d.y() - c.y() ) + + b.x()*( c.y() - d.y() ) + + d.x()*( b.y() - a.y() ) + + c.x()*( a.y() - b.y() ); + double s = ( a.x()*( d.y() - c.y() ) + + c.x()*( a.y() - d.y() ) + + d.x()*( c.y() - a.y() ) ) / D; + line_segment_temp.set_first( a + s * ( b - a ) ); + return line_segment_temp; + } + //Otherwise if improper... + double distance_temp_a = distance(a, line_segment2); + double distance_temp_b = distance(b, line_segment2); + double distance_temp_c = distance(c, line_segment1); + double distance_temp_d = distance(d, line_segment1); + //Check if the intersection is nondegenerate segment. + if( distance_temp_a <= epsilon and distance_temp_b <= epsilon ){ + line_segment_temp.set_first(a, epsilon); + line_segment_temp.set_second(b, epsilon); + return line_segment_temp; + } + else if( distance_temp_c <= epsilon and distance_temp_d <= epsilon ){ + line_segment_temp.set_first(c, epsilon); + line_segment_temp.set_second(d, epsilon); + return line_segment_temp; + } + else if( distance_temp_a <= epsilon and distance_temp_c <= epsilon ){ + line_segment_temp.set_first(a, epsilon); + line_segment_temp.set_second(c, epsilon); + return line_segment_temp; + } + else if( distance_temp_a <= epsilon and distance_temp_d <= epsilon ){ + line_segment_temp.set_first(a, epsilon); + line_segment_temp.set_second(d, epsilon); + return line_segment_temp; + } + else if( distance_temp_b <= epsilon and distance_temp_c <= epsilon ){ + line_segment_temp.set_first(b, epsilon); + line_segment_temp.set_second(c, epsilon); + return line_segment_temp; + } + else if( distance_temp_b <= epsilon and distance_temp_d <= epsilon ){ + line_segment_temp.set_first(b, epsilon); + line_segment_temp.set_second(d, epsilon); + return line_segment_temp; + } + //Check if the intersection is a single point. + else if( distance_temp_a <= epsilon ){ + line_segment_temp.set_first(a, epsilon); + return line_segment_temp; + } + else if( distance_temp_b <= epsilon ){ + line_segment_temp.set_first(b, epsilon); + return line_segment_temp; + } + else if( distance_temp_c <= epsilon ){ + line_segment_temp.set_first(c, epsilon); + return line_segment_temp; + } + else if( distance_temp_d <= epsilon ){ + line_segment_temp.set_first(d, epsilon); + return line_segment_temp; + } + return line_segment_temp; + } + + + std::ostream& operator << (std::ostream& outs, + const Line_Segment& line_segment_temp) + { + switch(line_segment_temp.size()){ + case 0: + return outs; + break; + case 1: + outs << line_segment_temp.first() << std::endl + << line_segment_temp.second() << std::endl; + return outs; + break; + case 2: + outs << line_segment_temp.first() << std::endl + << line_segment_temp.second() << std::endl; + return outs; + } + return outs; + } + + + //Angle + + + Angle::Angle(double data_temp) + { + if(data_temp >= 0) + angle_radians_ = fmod(data_temp, 2*M_PI); + else{ + angle_radians_ = 2*M_PI + fmod(data_temp, -2*M_PI); + if(angle_radians_ == 2*M_PI) + angle_radians_ = 0; + } + } + + + Angle::Angle(double rise_temp, double run_temp) + { + if( rise_temp == 0 and run_temp == 0 ) + angle_radians_ = 0; + //First calculate 4 quadrant inverse tangent into [-pi,+pi]. + angle_radians_ = std::atan2(rise_temp, run_temp); + //Correct so angles specified in [0, 2*PI). + if(angle_radians_ < 0) + angle_radians_ = 2*M_PI + angle_radians_; + } + + + void Angle::set(double data_temp) + { + *this = Angle(data_temp); + } + + + void Angle::randomize() + { + angle_radians_ = fmod( uniform_random_sample(0, 2*M_PI), 2*M_PI ); + } + + + bool operator == (const Angle& angle1, const Angle& angle2) + { + return (angle1.get() == angle2.get()); + } + bool operator != (const Angle& angle1, const Angle& angle2) + { + return !(angle1.get() == angle2.get()); + } + + + bool operator > (const Angle& angle1, const Angle& angle2) + { + return angle1.get() > angle2.get(); + } + bool operator < (const Angle& angle1, const Angle& angle2) + { + return angle1.get() < angle2.get(); + } + bool operator >= (const Angle& angle1, const Angle& angle2) + { + return angle1.get() >= angle2.get(); + } + bool operator <= (const Angle& angle1, const Angle& angle2) + { + return angle1.get() <= angle2.get(); + } + + + Angle operator + (const Angle& angle1, const Angle& angle2) + { + return Angle( angle1.get() + angle2.get() ); + } + Angle operator - (const Angle& angle1, const Angle& angle2) + { + return Angle( angle1.get() - angle2.get() ); + } + + + double geodesic_distance(const Angle& angle1, const Angle& angle2) + { + assert( angle1.get() == angle1.get() + and angle2.get() == angle2.get() ); + + double distance1 = std::fabs( angle1.get() + - angle2.get() ); + double distance2 = 2*M_PI - distance1; + if(distance1 < distance2) + return distance1; + return distance2; + } + + + double geodesic_direction(const Angle& angle1, const Angle& angle2) + { + assert( angle1.get() == angle1.get() + and angle2.get() == angle2.get() ); + + double distance1 = std::fabs( angle1.get() + - angle2.get() ); + double distance2 = 2*M_PI - distance1; + if(angle1 <= angle2){ + if(distance1 < distance2) + return 1.0; + return -1.0; + } + //Otherwise angle1 > angle2. + if(distance1 < distance2) + return -1.0; + return 1.0; + } + + + std::ostream& operator << (std::ostream& outs, const Angle& angle_temp) + { + outs << angle_temp.get(); + return outs; + } + + + //Polar_Point + + + Polar_Point::Polar_Point(const Point& polar_origin_temp, + const Point& point_temp, + double epsilon) : Point(point_temp) + { + polar_origin_ = polar_origin_temp; + if( polar_origin_==polar_origin_ + and point_temp==point_temp + and distance(polar_origin_, point_temp) <= epsilon ){ + bearing_ = Angle(0.0); + range_ = 0.0; + } + else if( polar_origin_==polar_origin_ + and point_temp==point_temp){ + bearing_ = Angle( point_temp.y()-polar_origin_temp.y(), + point_temp.x()-polar_origin_temp.x() ); + range_ = distance(polar_origin_temp, point_temp); + } + } + + + void Polar_Point::set_polar_origin(const Point& polar_origin_temp) + { + *this = Polar_Point( polar_origin_temp, Point(x(), y()) ); + } + + + void Polar_Point::set_x(double x_temp) + { + *this = Polar_Point( polar_origin_, Point(x_temp, y()) ); + } + + + void Polar_Point::set_y(double y_temp) + { + *this = Polar_Point( polar_origin_, Point(x(), y_temp) ); + } + + + void Polar_Point::set_range(double range_temp) + { + range_ = range_temp; + x_ = polar_origin_.x() + + range_*std::cos( bearing_.get() ); + y_ = polar_origin_.y() + + range_*std::sin( bearing_.get() ); + } + + + void Polar_Point::set_bearing(const Angle& bearing_temp) + { + bearing_ = bearing_temp; + x_ = polar_origin_.x() + + range_*std::cos( bearing_.get() ); + y_ = polar_origin_.y() + + range_*std::sin( bearing_.get() ); + } + + + bool operator == (const Polar_Point& polar_point1, + const Polar_Point& polar_point2) + { + if( polar_point1.polar_origin() == polar_point2.polar_origin() + and polar_point1.range() == polar_point2.range() + and polar_point1.bearing() == polar_point2.bearing() + ) + return true; + return false; + } + bool operator != (const Polar_Point& polar_point1, + const Polar_Point& polar_point2) + { + return !( polar_point1 == polar_point2 ); + } + + + bool operator > (const Polar_Point& polar_point1, + const Polar_Point& polar_point2) + { + if( polar_point1.polar_origin() != polar_point1.polar_origin() + or polar_point1.range() != polar_point1.range() + or polar_point1.bearing() != polar_point1.bearing() + or polar_point2.polar_origin() != polar_point2.polar_origin() + or polar_point2.range() != polar_point2.range() + or polar_point2.bearing() != polar_point2.bearing() + ) + return false; + + if( polar_point1.bearing() > polar_point2.bearing() ) + return true; + else if( polar_point1.bearing() == polar_point2.bearing() + and polar_point1.range() > polar_point2.range() ) + return true; + return false; + } + bool operator < (const Polar_Point& polar_point1, + const Polar_Point& polar_point2) + { + if( polar_point1.polar_origin() != polar_point1.polar_origin() + or polar_point1.range() != polar_point1.range() + or polar_point1.bearing() != polar_point1.bearing() + or polar_point2.polar_origin() != polar_point2.polar_origin() + or polar_point2.range() != polar_point2.range() + or polar_point2.bearing() != polar_point2.bearing() + ) + return false; + + if( polar_point1.bearing() < polar_point2.bearing() ) + return true; + else if( polar_point1.bearing() == polar_point2.bearing() + and polar_point1.range() < polar_point2.range() ) + return true; + return false; + + } + bool operator >= (const Polar_Point& polar_point1, + const Polar_Point& polar_point2) + { + if( polar_point1.polar_origin() != polar_point1.polar_origin() + or polar_point1.range() != polar_point1.range() + or polar_point1.bearing() != polar_point1.bearing() + or polar_point2.polar_origin() != polar_point2.polar_origin() + or polar_point2.range() != polar_point2.range() + or polar_point2.bearing() != polar_point2.bearing() + ) + return false; + + return !(polar_point1polar_point2); + } + + + std::ostream& operator << (std::ostream& outs, + const Polar_Point& polar_point_temp) + { + outs << polar_point_temp.bearing() << " " << polar_point_temp.range(); + return outs; + } + + + //Ray + + + Ray::Ray(Point base_point_temp, Point bearing_point) + { + assert( !( base_point_temp == bearing_point ) ); + + base_point_ = base_point_temp; + bearing_ = Angle( bearing_point.y()-base_point_temp.y(), + bearing_point.x()-base_point_temp.x() ); + } + + bool operator == (const Ray& ray1, + const Ray& ray2) + { + if( ray1.base_point() == ray2.base_point() + and ray1.bearing() == ray2.bearing() ) + return true; + else + return false; + } + + + bool operator != (const Ray& ray1, + const Ray& ray2) + { + return !( ray1 == ray2 ); + } + + + Line_Segment intersection(const Ray ray_temp, + const Line_Segment& line_segment_temp, + double epsilon) + { + assert( ray_temp == ray_temp + and line_segment_temp.size() > 0 ); + + //First construct a Line_Segment parallel with the Ray which is so + //long, that it's intersection with line_segment_temp will be + //equal to the intersection of ray_temp with line_segment_temp. + double R = distance(ray_temp.base_point(), line_segment_temp) + + line_segment_temp.length(); + Line_Segment seg_approx = + Line_Segment( ray_temp.base_point(), ray_temp.base_point() + + Point( R*std::cos(ray_temp.bearing().get()), + R*std::sin(ray_temp.bearing().get()) ) ); + Line_Segment intersect_seg = intersection(line_segment_temp, + seg_approx, + epsilon); + //Make sure point closer to ray_temp's base_point is listed first. + if( intersect_seg.size() == 2 + and distance( intersect_seg.first(), ray_temp.base_point() ) > + distance( intersect_seg.second(), ray_temp.base_point() ) ){ + intersect_seg.reverse(); + } + return intersect_seg; + } + + + Line_Segment intersection(const Line_Segment& line_segment_temp, + const Ray& ray_temp, + double epsilon) + { + return intersection( ray_temp , line_segment_temp , epsilon ); + } + + + //Polyline + + + double Polyline::length() const + { + double length_temp = 0; + for(unsigned i=1; i <= vertices_.size()-1; i++) + length_temp += distance( vertices_[i-1] , vertices_[i] ); + return length_temp; + } + + + double Polyline::diameter() const + { + //Precondition: nonempty Polyline. + assert( size() > 0 ); + + double running_max=0; + for(unsigned i=0; i running_max ) + running_max = distance( (*this)[i] , (*this)[j] ); + }} + return running_max; + } + + + Bounding_Box Polyline::bbox () const + { + //Precondition: nonempty Polyline. + assert( vertices_.size() > 0 ); + + Bounding_Box bounding_box; + double x_min=vertices_[0].x(), x_max=vertices_[0].x(), + y_min=vertices_[0].y(), y_max=vertices_[0].y(); + for(unsigned i = 1; i < vertices_.size(); i++){ + if(x_min > vertices_[i].x()) { x_min=vertices_[i].x(); } + if(x_max < vertices_[i].x()) { x_max=vertices_[i].x(); } + if(y_min > vertices_[i].y()) { y_min=vertices_[i].y(); } + if(y_max < vertices_[i].y()) { y_max=vertices_[i].y(); } + } + bounding_box.x_min=x_min; bounding_box.x_max=x_max; + bounding_box.y_min=y_min; bounding_box.y_max=y_max; + return bounding_box; + } + + + void Polyline::eliminate_redundant_vertices(double epsilon) + { + //Trivial case + if(vertices_.size() < 3) + return; + + //Store new minimal length list of vertices + std::vector vertices_temp; + vertices_temp.reserve(vertices_.size()); + + //Place holders + unsigned first = 0; + unsigned second = 1; + unsigned third = 2; + + //Add first vertex + vertices_temp.push_back((*this)[first]); + + while( third < vertices_.size() ){ + //if second redundant + if( distance( Line_Segment( (*this)[first], + (*this)[third] ) , + (*this)[second] ) + <= epsilon ){ + //=>skip it + second = third; + third++; + } + //else second not redundant + else{ + //=>add it. + vertices_temp.push_back((*this)[second]); + first = second; + second = third; + third++; + } + } + + //Add last vertex + vertices_temp.push_back(vertices_.back()); + + //Update list of vertices + vertices_ = vertices_temp; + } + + + void Polyline::reverse() + { + std::reverse( vertices_.begin() , vertices_.end() ); + } + + + std::ostream& operator << (std::ostream& outs, + const Polyline& polyline_temp) + { + for(unsigned i=0; i> x_temp and fin >> y_temp){ + point_temp.set_x(x_temp); + point_temp.set_y(y_temp); + vertices_.push_back(point_temp); + } + fin.close(); + } + + + Polygon::Polygon(const std::vector& vertices_temp) + { + vertices_ = vertices_temp; + } + + + Polygon::Polygon(const Point& point0, + const Point& point1, + const Point& point2) + { + vertices_.push_back(point0); + vertices_.push_back(point1); + vertices_.push_back(point2); + } + + + unsigned Polygon::r () const + { + int r_count = 0; + if( vertices_.size() > 1 ){ + //Use cross product to count right turns. + for(unsigned i=0; i<=n()-1; i++) + if( ((*this)[i+1].x()-(*this)[i].x()) + *((*this)[i+2].y()-(*this)[i].y()) + - ((*this)[i+1].y()-(*this)[i].y()) + *((*this)[i+2].x()-(*this)[i].x()) < 0 ) + r_count++; + if( area() < 0 ){ + r_count = n() - r_count; + } + } + return r_count; + } + + + bool Polygon::is_simple(double epsilon) const + { + + if(n()==0 or n()==1 or n()==2) + return false; + + //Make sure adjacent edges only intersect at a single point. + for(unsigned i=0; i<=n()-1; i++) + if( intersection( Line_Segment((*this)[i],(*this)[i+1]) , + Line_Segment((*this)[i+1],(*this)[i+2]) , + epsilon ).size() > 1 ) + return false; + + //Make sure nonadjacent edges do not intersect. + for(unsigned i=0; i 1) //if more than one point in the polygon. + for(unsigned i=1; i vertices_[i]) + return false; + return true; + } + + + double Polygon::boundary_length() const + { + double length_temp=0; + if(n()==0 or n()==1) + return 0; + for(unsigned i=0; i 0 ); + + double area_temp=area(); + if(area_temp==0) + { std::cerr << "\x1b[5;31m" + << "Warning: tried to compute centoid of polygon with zero area!" + << "\x1b[0m\n" << "\a \n"; exit(1); } + double x_temp=0; + for(unsigned i=0; i<=n()-1; i++) + x_temp += ( (*this)[i].x() + (*this)[i+1].x() ) + * ( (*this)[i].x()*(*this)[i+1].y() + - (*this)[i+1].x()*(*this)[i].y() ); + double y_temp=0; + for(unsigned i=0; i<=n()-1; i++) + y_temp += ( (*this)[i].y() + (*this)[i+1].y() ) + * ( (*this)[i].x()*(*this)[i+1].y() + - (*this)[i+1].x()*(*this)[i].y() ); + return Point(x_temp/(6*area_temp), y_temp/(6*area_temp)); + } + + + double Polygon::diameter() const + { + //Precondition: nonempty Polygon. + assert( n() > 0 ); + + double running_max=0; + for(unsigned i=0; i running_max ) + running_max = distance( (*this)[i] , (*this)[j] ); + }} + return running_max; + } + + + Bounding_Box Polygon::bbox () const + { + //Precondition: nonempty Polygon. + assert( vertices_.size() > 0 ); + + Bounding_Box bounding_box; + double x_min=vertices_[0].x(), x_max=vertices_[0].x(), + y_min=vertices_[0].y(), y_max=vertices_[0].y(); + for(unsigned i = 1; i < vertices_.size(); i++){ + if(x_min > vertices_[i].x()) { x_min=vertices_[i].x(); } + if(x_max < vertices_[i].x()) { x_max=vertices_[i].x(); } + if(y_min > vertices_[i].y()) { y_min=vertices_[i].y(); } + if(y_max < vertices_[i].y()) { y_max=vertices_[i].y(); } + } + bounding_box.x_min=x_min; bounding_box.x_max=x_max; + bounding_box.y_min=y_min; bounding_box.y_max=y_max; + return bounding_box; + } + + + std::vector Polygon::random_points(const unsigned& count, + double epsilon) const + { + //Precondition: nonempty Polygon. + assert( vertices_.size() > 0 ); + + Bounding_Box bounding_box = bbox(); + std::vector pts_in_polygon; pts_in_polygon.reserve(count); + Point pt_temp( uniform_random_sample(bounding_box.x_min, + bounding_box.x_max), + uniform_random_sample(bounding_box.y_min, + bounding_box.y_max) ); + while(pts_in_polygon.size() < count){ + while(!pt_temp.in(*this, epsilon)){ + pt_temp.set_x( uniform_random_sample(bounding_box.x_min, + bounding_box.x_max) ); + pt_temp.set_y( uniform_random_sample(bounding_box.y_min, + bounding_box.y_max) ); + } + pts_in_polygon.push_back(pt_temp); + pt_temp.set_x( uniform_random_sample(bounding_box.x_min, + bounding_box.x_max) ); + pt_temp.set_y( uniform_random_sample(bounding_box.y_min, + bounding_box.y_max) ); + } + return pts_in_polygon; + } + + + void Polygon::write_to_file(const std::string& filename, + int fios_precision_temp) + { + assert( fios_precision_temp >= 1 ); + + std::ofstream fout( filename.c_str() ); + //fout.open( filename.c_str() ); //Alternatives. + //fout << *this; + fout.setf(std::ios::fixed); + fout.setf(std::ios::showpoint); + fout.precision(fios_precision_temp); + for(unsigned i=0; i 1){ //if more than one point in the polygon. + std::vector vertices_temp; + vertices_temp.reserve(point_count); + //Find index of lexicographically smallest point. + int index_of_smallest=0; + int i; //counter. + for(i=1; i vertices_temp; + vertices_temp.reserve( vertices_.size() ); + + //Place holders. + unsigned first = 0; + unsigned second = 1; + unsigned third = 2; + + while( third <= vertices_.size() ){ + //if second is redundant + if( distance( Line_Segment( (*this)[first], + (*this)[third] ) , + (*this)[second] ) + <= epsilon ){ + //=>skip it + second = third; + third++; + } + //else second not redundant + else{ + //=>add it + vertices_temp.push_back( (*this)[second] ); + first = second; + second = third; + third++; + } + } + + //decide whether to add original first point + if( distance( Line_Segment( vertices_temp.front(), + vertices_temp.back() ) , + vertices_.front() ) + > epsilon ) + vertices_temp.push_back( vertices_.front() ); + + //Update list of vertices. + vertices_ = vertices_temp; + } + + + void Polygon::reverse() + { + if( n() > 2 ) + std::reverse( ++vertices_.begin() , vertices_.end() ); + } + + + bool operator == (Polygon polygon1, Polygon polygon2) + { + if( polygon1.n() != polygon2.n() + or polygon1.n() == 0 + or polygon2.n() == 0 ) + return false; + for(unsigned i=0; i epsilon ) + { successful_match = false; break; } + } + if( successful_match ) + return true; + } + return false; + } + + + double boundary_distance(const Polygon& polygon1, const Polygon& polygon2) + { + assert( polygon1.n() > 0 and polygon2.n() > 0 ); + + //Handle single point degeneracy. + if(polygon1.n() == 1) + return boundary_distance(polygon1[0], polygon2); + else if(polygon2.n() == 1) + return boundary_distance(polygon2[0], polygon1); + //Handle cases where each polygon has at least 2 points. + //Initialize to an upper bound. + double running_min = boundary_distance(polygon1[0], polygon2); + double distance_temp; + //Loop over all possible pairs of line segments. + for(unsigned i=0; i<=polygon1.n()-1; i++){ + for(unsigned j=0; j<=polygon2.n()-1; j++){ + distance_temp = distance( Line_Segment(polygon1[i], polygon1[i+1]) , + Line_Segment(polygon2[j], polygon2[j+1]) ); + if(distance_temp < running_min) + running_min = distance_temp; + }} + return running_min; + } + + + std::ostream& operator << (std::ostream& outs, + const Polygon& polygon_temp) + { + for(unsigned i=0; i& polygons) + { + outer_boundary_ = polygons[0]; + for(unsigned i=1; i vertices_temp; + + //Skip comments + while( fin.peek() == '/' ) + fin.ignore(200,'\n'); + + //Read outer_boundary. + while ( fin.peek() != '/' ){ + fin >> x_temp >> y_temp; + //Skip to next line. + fin.ignore(1); + if( fin.eof() ) + { + outer_boundary_.set_vertices(vertices_temp); + fin.close(); + update_flattened_index_key(); return; + } + vertices_temp.push_back( Point(x_temp, y_temp) ); + } + outer_boundary_.set_vertices(vertices_temp); + vertices_temp.clear(); + + //Read holes. + Polygon polygon_temp; + while(1){ + //Skip comments + while( fin.peek() == '/' ) + fin.ignore(200,'\n'); + if( fin.eof() ) + { fin.close(); update_flattened_index_key(); return; } + while( fin.peek() != '/' ){ + fin >> x_temp >> y_temp; + if( fin.eof() ) + { + polygon_temp.set_vertices(vertices_temp); + holes_.push_back(polygon_temp); + fin.close(); + update_flattened_index_key(); return; + } + vertices_temp.push_back( Point(x_temp, y_temp) ); + //Skips to next line. + fin.ignore(1); + } + polygon_temp.set_vertices(vertices_temp); + holes_.push_back(polygon_temp); + vertices_temp.clear(); + } + + update_flattened_index_key(); + } + + + const Point& Environment::operator () (unsigned k) const + { + //std::pair ij(one_to_two(k)); + std::pair ij( flattened_index_key_[k] ); + return (*this)[ ij.first ][ ij.second ]; + } + + + unsigned Environment::n() const + { + int n_count = 0; + n_count = outer_boundary_.n(); + for(unsigned i=0; i 0 ) + return false; + return true; + } + + + bool Environment::is_valid(double epsilon) const + { + if( n() <= 2 ) + return false; + + //Check all Polygons are simple. + if( !outer_boundary_.is_simple(epsilon) ){ + std::cerr << std::endl << "\x1b[31m" + << "The outer boundary is not simple." + << "\x1b[0m" << std::endl; + return false; + } + for(unsigned i=0; i= 0 ){ + std::cerr << std::endl << "\x1b[31m" + << "The vertices of hole " << i << " are not listed cw." + << "\x1b[0m" << std::endl; + return false; + } + + return true; + } + + + double Environment::boundary_length() const + { + //Precondition: nonempty Environment. + assert( outer_boundary_.n() > 0 ); + + double length_temp = outer_boundary_.boundary_length(); + for(unsigned i=0; i Environment::random_points(const unsigned& count, + double epsilon) const + { + assert( area() > 0 ); + + Bounding_Box bounding_box = bbox(); + std::vector pts_in_environment; + pts_in_environment.reserve(count); + Point pt_temp( uniform_random_sample(bounding_box.x_min, + bounding_box.x_max), + uniform_random_sample(bounding_box.y_min, + bounding_box.y_max) ); + while(pts_in_environment.size() < count){ + while(!pt_temp.in(*this, epsilon)){ + pt_temp.set_x( uniform_random_sample(bounding_box.x_min, + bounding_box.x_max) ); + pt_temp.set_y( uniform_random_sample(bounding_box.y_min, + bounding_box.y_max) ); + } + pts_in_environment.push_back(pt_temp); + pt_temp.set_x( uniform_random_sample(bounding_box.x_min, + bounding_box.x_max) ); + pt_temp.set_y( uniform_random_sample(bounding_box.y_min, + bounding_box.y_max) ); + } + return pts_in_environment; + } + + + Polyline Environment::shortest_path(const Point& start, + const Point& finish, + const Visibility_Graph& visibility_graph, + double epsilon) + { + //true => data printed to terminal + //false => silent + const bool PRINTING_DEBUG_DATA = false; + + //For now, just find one shortest path, later change this to a + //vector to find all shortest paths (w/in epsilon). + Polyline shortest_path_output; + Visibility_Polygon start_visibility_polygon(start, *this, epsilon); + + //Trivial cases + if( distance(start,finish) <= epsilon ){ + shortest_path_output.push_back(start); + return shortest_path_output; + } + else if( finish.in(start_visibility_polygon, epsilon) ){ + shortest_path_output.push_back(start); + shortest_path_output.push_back(finish); + return shortest_path_output; + } + + Visibility_Polygon finish_visibility_polygon(finish, *this, epsilon); + + //Connect start and finish Points to the visibility graph + bool *start_visible; //start row of visibility graph + bool *finish_visible; //finish row of visibility graph + start_visible = new bool[n()]; + finish_visible = new bool[n()]; + for(unsigned k=0; k T; + //:WARNING: + //If T is a vector it is crucial to make T large enough that it + //will not be resized. If T were resized, any iterators pointing + //to its contents would be invalidated, thus causing the program + //to fail. + //T.reserve( n() + 3 ); + + //Initialize priority queue of unexpanded nodes + std::set Q; + + //Construct initial node + Shortest_Path_Node current_node; + //convention vertex_index == n() => corresponds to start Point + //vertex_index == n() + 1 => corresponds to finish Point + current_node.vertex_index = n(); + current_node.cost_to_come = 0; + current_node.estimated_cost_to_go = distance( start , finish ); + //Put in T and on Q + T.push_back( current_node ); + T.begin()->search_tree_location = T.begin(); + current_node.search_tree_location = T.begin(); + T.begin()->parent_search_tree_location = T.begin(); + current_node.parent_search_tree_location = T.begin(); + Q.insert( current_node ); + + //Initialize temporary variables + Shortest_Path_Node child; //children of current_node + std::vector children; + //flags + bool solution_found = false; + bool child_already_visited = false; + //-----------Begin Main Loop----------- + while( !Q.empty() ){ + + //Pop top element off Q onto current_node + current_node = *Q.begin(); Q.erase( Q.begin() ); + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + <<"==============" + <<" current_node just poped off of Q " + <<"==============" + << std::endl; + current_node.print(); + std::cout << std::endl; + } + + //Check for goal state + //(if current node corresponds to finish) + if( current_node.vertex_index == n() + 1 ){ + + if( PRINTING_DEBUG_DATA ){ + std::cout <<"solution found!" + << std::endl + << std::endl; + } + + solution_found = true; + break; + } + + //Expand current_node (compute children) + children.clear(); + + if( PRINTING_DEBUG_DATA ){ + std::cout << "-------------------------------------------" + << std::endl + << "Expanding Current Node (Computing Children)" + << std::endl + << "current size of search tree T = " + << T.size() + << std::endl + << "-------------------------------------------" + << std::endl; + } + + //if current_node corresponds to start + if( current_node.vertex_index == n() ){ + //loop over environment vertices + for(unsigned i=0; i < n(); i++){ + if( start_visible[i] ){ + child.vertex_index = i; + child.parent_search_tree_location + = current_node.search_tree_location; + child.cost_to_come = distance( start , (*this)(i) ); + child.estimated_cost_to_go = distance( (*this)(i) , finish ); + children.push_back( child ); + + if( PRINTING_DEBUG_DATA ){ + std::cout << std::endl << "computed child: " + << std::endl; + child.print(); + } + + } + } + } + //else current_node corresponds to a vertex of the environment + else{ + //check which environment vertices are visible + for(unsigned i=0; i < n(); i++){ + if( current_node.vertex_index != i ) + if( visibility_graph( current_node.vertex_index , i ) ){ + child.vertex_index = i; + child.parent_search_tree_location + = current_node.search_tree_location; + child.cost_to_come = current_node.cost_to_come + + distance( (*this)(current_node.vertex_index), + (*this)(i) ); + child.estimated_cost_to_go = distance( (*this)(i) , finish ); + children.push_back( child ); + + if( PRINTING_DEBUG_DATA ){ + std::cout << std::endl << "computed child: " + << std::endl; + child.print(); + } + + } + } + //check if finish is visible + if( finish_visible[ current_node.vertex_index ] ){ + child.vertex_index = n() + 1; + child.parent_search_tree_location + = current_node.search_tree_location; + child.cost_to_come = current_node.cost_to_come + + distance( (*this)(current_node.vertex_index) , finish ); + child.estimated_cost_to_go = 0; + children.push_back( child ); + + if( PRINTING_DEBUG_DATA ){ + std::cout << std::endl << "computed child: " + << std::endl; + child.print(); + } + + } + } + + if( PRINTING_DEBUG_DATA ){ + std::cout << std::endl + <<"-----------------------------------------" + << std::endl + << "Processing " << children.size() + << " children" << std::endl + << "-----------------------------------------" + << std::endl; + } + + //Process children + for( std::vector::iterator + children_itr = children.begin(); + children_itr != children.end(); + children_itr++ ){ + child_already_visited = false; + + if( PRINTING_DEBUG_DATA ){ + std::cout << std::endl << "current child being processed: " + << std::endl; + children_itr->print(); + } + + //Check if child state has already been visited + //(by looking in search tree T) + for( std::list::iterator T_itr = T.begin(); + T_itr != T.end(); T_itr++ ){ + if( children_itr->vertex_index + == T_itr->vertex_index ){ + children_itr->search_tree_location = T_itr; + child_already_visited = true; + break; + } + } + + if( !child_already_visited ){ + //Add child to search tree T + T.push_back( *children_itr ); + (--T.end())->search_tree_location = --T.end(); + children_itr->search_tree_location = --T.end(); + Q.insert( *children_itr ); + } + else if( children_itr->search_tree_location->cost_to_come > + children_itr->cost_to_come ){ + //redirect parent pointer in search tree + children_itr->search_tree_location->parent_search_tree_location + = children_itr->parent_search_tree_location; + //and update cost data + children_itr->search_tree_location->cost_to_come + = children_itr->cost_to_come; + //update Q + for(std::set::iterator + Q_itr = Q.begin(); + Q_itr!= Q.end(); + Q_itr++){ + if( children_itr->vertex_index == Q_itr->vertex_index ){ + Q.erase( Q_itr ); + break; + } + } + Q.insert( *children_itr ); + } + + //If not already visited, insert into Q + if( !child_already_visited ) + Q.insert( *children_itr ); + + if( PRINTING_DEBUG_DATA ){ + std::cout << "child already visited? " + << child_already_visited + << std::endl; + } + + } + } + //-----------End Main Loop----------- + + //Recover solution + if( solution_found ){ + shortest_path_output.push_back( finish ); + std::list::iterator + backtrace_itr = current_node.parent_search_tree_location; + Point waypoint; + + if( PRINTING_DEBUG_DATA ){ + std::cout << "----------------------------" << std::endl + << "backtracing to find solution" << std::endl + << "----------------------------" << std::endl; + + } + + while( true ){ + + if( PRINTING_DEBUG_DATA ){ + std::cout << "backtrace node is " + << std::endl; + backtrace_itr->print(); + std::cout << std::endl; + } + + if( backtrace_itr->vertex_index < n() ) + waypoint = (*this)( backtrace_itr->vertex_index ); + else if( backtrace_itr->vertex_index == n() ) + waypoint = start; + //Add vertex if not redundant + if( shortest_path_output.size() > 0 + and distance( shortest_path_output[ shortest_path_output.size() + - 1 ], + waypoint ) > epsilon ) + shortest_path_output.push_back( waypoint ); + if( backtrace_itr->cost_to_come == 0 ) + break; + backtrace_itr = backtrace_itr->parent_search_tree_location; + } + shortest_path_output.reverse(); + } + + //free memory + delete [] start_visible; + delete [] finish_visible; + + //shortest_path_output.eliminate_redundant_vertices( epsilon ); + //May not be desirable to eliminate redundant vertices, because + //those redundant vertices can make successive waypoints along the + //shortest path robustly visible (and thus easier for a robot to + //navigate) + + return shortest_path_output; + } + Polyline Environment::shortest_path(const Point& start, + const Point& finish, + double epsilon) + { + return shortest_path( start, + finish, + Visibility_Graph(*this, epsilon), + epsilon ); + } + + + void Environment::write_to_file(const std::string& filename, + int fios_precision_temp) + { + assert( fios_precision_temp >= 1 ); + + std::ofstream fout( filename.c_str() ); + //fout.open( filename.c_str() ); //Alternatives. + //fout << *this; + fout.setf(std::ios::fixed); + fout.setf(std::ios::showpoint); + fout.precision(fios_precision_temp); + fout << "//Environment Model" << std::endl; + fout << "//Outer Boundary" << std::endl << outer_boundary_; + for(unsigned i=0; i ij( one_to_two(k) ); + std::pair ij( flattened_index_key_[k] ); + return (*this)[ ij.first ][ ij.second ]; + } + + + void Environment::enforce_standard_form() + { + if( outer_boundary_.area() < 0 ) + outer_boundary_.reverse(); + outer_boundary_.enforce_standard_form(); + for(unsigned i=0; i 0 ) + holes_[i].reverse(); + holes_[i].enforce_standard_form(); + } + } + + + void Environment::eliminate_redundant_vertices(double epsilon) + { + outer_boundary_.eliminate_redundant_vertices(epsilon); + for(unsigned i=0; i pair_temp; + for(unsigned i=0; i<=h(); i++){ + for(unsigned j=0; j<(*this)[i].n(); j++){ + pair_temp.first = i; + pair_temp.second = j; + flattened_index_key_.push_back( pair_temp ); + }} + } + + + std::pair Environment::one_to_two(unsigned k) const + { + std::pair two(0,0); + //Strategy: add up vertex count of each Polygon (outer boundary + + //holes) until greater than k + unsigned current_polygon_index = 0; + unsigned vertex_count_up_to_current_polygon = (*this)[0].n(); + unsigned vertex_count_up_to_last_polygon = 0; + + while( k >= vertex_count_up_to_current_polygon + and current_polygon_index < (*this).h() ){ + current_polygon_index++; + two.first = two.first + 1; + vertex_count_up_to_last_polygon = vertex_count_up_to_current_polygon; + vertex_count_up_to_current_polygon += (*this)[current_polygon_index].n(); + } + two.second = k - vertex_count_up_to_last_polygon; + + return two; + } + + + std::ostream& operator << (std::ostream& outs, + const Environment& environment_temp) + { + outs << "//Environment Model" << std::endl; + outs << "//Outer Boundary" << std::endl << environment_temp[0]; + for(unsigned i=1; i<=environment_temp.h(); i++){ + outs << "//Hole" << std::endl << environment_temp[i]; + } + //outs << "//EOF marker"; + return outs; + } + + + //Guards + + + Guards::Guards(const std::string& filename) + { + std::ifstream fin(filename.c_str()); + //if(fin.fail()) { std::cerr << "\x1b[5;31m" << "Input file + //opening failed." << "\x1b[0m\n" << "\a \n"; exit(1);} + assert( !fin.fail() ); + + //Temp vars for numbers to be read from file. + double x_temp, y_temp; + + //Skip comments + while( fin.peek() == '/' ) + fin.ignore(200,'\n'); + + //Read positions. + while (1){ + fin >> x_temp >> y_temp; + if( fin.eof() ) + { fin.close(); return; } + positions_.push_back( Point(x_temp, y_temp) ); + //Skip to next line. + fin.ignore(1); + //Skip comments + while( fin.peek() == '/' ) + fin.ignore(200,'\n'); + } + } + + + bool Guards::are_lex_ordered() const + { + //if more than one guard. + if(positions_.size() > 1) + for(unsigned i=0; i positions_[i+1]) + return false; + return true; + } + + + bool Guards::noncolocated(double epsilon) const + { + for(unsigned i=0; i 0 ); + + double running_max=0; + for(unsigned i=0; i running_max ) + running_max = distance( (*this)[i] , (*this)[j] ); + }} + return running_max; + } + + + Bounding_Box Guards::bbox() const + { + //Precondition: nonempty Guard set + assert( positions_.size() > 0 ); + + Bounding_Box bounding_box; + double x_min=positions_[0].x(), x_max=positions_[0].x(), + y_min=positions_[0].y(), y_max=positions_[0].y(); + for(unsigned i = 1; i < positions_.size(); i++){ + if(x_min > positions_[i].x()) { x_min=positions_[i].x(); } + if(x_max < positions_[i].x()) { x_max=positions_[i].x(); } + if(y_min > positions_[i].y()) { y_min=positions_[i].y(); } + if(y_max < positions_[i].y()) { y_max=positions_[i].y(); } + } + bounding_box.x_min=x_min; bounding_box.x_max=x_max; + bounding_box.y_min=y_min; bounding_box.y_max=y_max; + return bounding_box; + } + + + void Guards::write_to_file(const std::string& filename, + int fios_precision_temp) + { + assert( fios_precision_temp >= 1 ); + + std::ofstream fout( filename.c_str() ); + //fout.open( filename.c_str() ); //Alternatives. + //fout << *this; + fout.setf(std::ios::fixed); + fout.setf(std::ios::showpoint); + fout.precision(fios_precision_temp); + fout << "//Guard Positions" << std::endl; + for(unsigned i=0; i epsilon + and distance( observer , point2 ) > epsilon + and distance( observer , point3 ) > epsilon + //Test whether there is a spike with point2 as the tip + and ( ( distance(observer,point2) + >= distance(observer,point1) + and distance(observer,point2) + >= distance(observer,point3) ) + or ( distance(observer,point2) + <= distance(observer,point1) + and distance(observer,point2) + <= distance(observer,point3) ) ) + //and the pike is sufficiently sharp, + and std::max( distance( Ray(observer, point1), point2 ), + distance( Ray(observer, point3), point2 ) ) + <= epsilon + ); + //Formerly used + //std::fabs( Polygon(point1, point2, point3).area() ) < epsilon + } + + + void Visibility_Polygon::chop_spikes_at_back(const Point& observer, + double epsilon) + { + //Eliminate "special case" vertices of the visibility polygon. + //While the top three vertices form a spike. + while( vertices_.size() >= 3 + and is_spike( observer, + vertices_[vertices_.size()-3], + vertices_[vertices_.size()-2], + vertices_[vertices_.size()-1], epsilon ) ){ + vertices_[vertices_.size()-2] = vertices_[vertices_.size()-1]; + vertices_.pop_back(); + } + } + + + void Visibility_Polygon::chop_spikes_at_wrap_around(const Point& observer, + double epsilon) + { + //Eliminate "special case" vertices of the visibility polygon at + //wrap-around. While the there's a spike at the wrap-around, + while( vertices_.size() >= 3 + and is_spike( observer, + vertices_[vertices_.size()-2], + vertices_[vertices_.size()-1], + vertices_[0], epsilon ) ){ + //Chop off the tip of the spike. + vertices_.pop_back(); + } + } + + + void Visibility_Polygon::chop_spikes(const Point& observer, + double epsilon) + { + std::set spike_tips; + std::vector vertices_temp; + //Middle point is potentially the tip of a spike + for(unsigned i=0; i::iterator& + active_edge) + { + std::cout << " current_vertex [x y bearing range is_first] = [" + << current_vertex.x() << " " + << current_vertex.y() << " " + << current_vertex.bearing() << " " + << current_vertex.range() << " " + << current_vertex.is_first << "]" << std::endl; + std::cout << "1st point of current_vertex's edge [x y bearing range] = [" + << (current_vertex.incident_edge->first).x() << " " + << (current_vertex.incident_edge->first).y() << " " + << (current_vertex.incident_edge->first).bearing() << " " + << (current_vertex.incident_edge->first).range() << "]" + << std::endl; + std::cout << "2nd point of current_vertex's edge [x y bearing range] = [" + << (current_vertex.incident_edge->second).x() << " " + << (current_vertex.incident_edge->second).y() << " " + << (current_vertex.incident_edge->second).bearing() << " " + << (current_vertex.incident_edge->second).range() << "]" + << std::endl; + std::cout << " 1st point of active_edge [x y bearing range] = [" + << (active_edge->first).x() << " " + << (active_edge->first).y() << " " + << (active_edge->first).bearing() << " " + << (active_edge->first).range() << "]" << std::endl; + std::cout << " 2nd point of active_edge [x y bearing range] = [" + << (active_edge->second).x() << " " + << (active_edge->second).y() << " " + << (active_edge->second).bearing() << " " + << (active_edge->second).range() << "]" << std::endl; + } + + + Visibility_Polygon::Visibility_Polygon(const Point& observer, + const Environment& environment_temp, + double epsilon) + : observer_(observer) + { + //Visibility polygon algorithm for environments with holes + //Radial line (AKA angular plane) sweep technique. + // + //Based on algorithms described in + // + //[1] "Automated Camera Layout to Satisfy Task-Specific and + //Floorplan-Specific Coverage Requirements" by Ugur Murat Erdem + //and Stan Scarloff, April 15, 2004 + //available at BUCS Technical Report Archive: + //http://www.cs.bu.edu/techreports/pdf/2004-015-camera-layout.pdf + // + //[2] "Art Gallery Theorems and Algorithms" by Joseph O'Rourke + // + //[3] "Visibility Algorithms in the Plane" by Ghosh + // + + //We define a k-point is a point seen on the other side of a + //visibility occluding corner. This name is appropriate because + //the vertical line in the letter "k" is like a line-of-sight past + //the corner of the "k". + + // + //Preconditions: + //(1) the Environment is epsilon-valid, + //(2) the Point observer is actually in the Environment + // environment_temp, + //(3) the guard has been epsilon-snapped to the boundary, followed + // by vertices of the environment (the order of the snapping + // is important). + // + //:WARNING: + //For efficiency, the assertions corresponding to these + //preconditions have been excluded. + // + //assert( environment_temp.is_valid(epsilon) ); + //assert( environment_temp.is_in_standard_form() ); + //assert( observer.in(environment_temp, epsilon) ); + + //true => data printed to terminal + //false => silent + const bool PRINTING_DEBUG_DATA = false; + + //The visibility polygon cannot have more vertices than the environment. + vertices_.reserve( environment_temp.n() ); + + // + //--------PREPROCESSING-------- + // + + //Construct a POLAR EDGE LIST from environment_temp's outer + //boundary and holes. During this construction, those edges are + //split which either (1) cross the ray emanating from the observer + //parallel to the x-axis (of world coords), or (2) contain the + //observer in their relative interior (w/in epsilon). Also, edges + //having first vertex bearing >= second vertex bearing are + //eliminated because they cannot possibly contribute to the + //visibility polygon. + std::list elp; + Polar_Point ppoint1, ppoint2; + Polar_Point split_bottom, split_top; + double t; + //If the observer is standing on the Enviroment boundary with its + //back to the wall, these will be the bearings of the next vertex + //to the right and to the left, respectively. + Angle right_wall_bearing; + Angle left_wall_bearing; + for(unsigned i=0; i<=environment_temp.h(); i++){ + for(unsigned j=0; j epsilon ){ + //Possible source of numerical instability? + t = ( observer.y() - ppoint2.y() ) + / ( ppoint1.y() - ppoint2.y() ); + //If edge crosses the ray emanating horizontal and right of + //the observer. + if( 0 < t and t < 1 and + observer.x() < t*ppoint1.x() + (1-t)*ppoint2.x() ){ + //If first point is above, omit edge because it runs + //'against the grain'. + if( ppoint1.y() > observer.y() ) + continue; + //Otherwise split the edge, making sure angles are assigned + //correctly on each side of the split point. + split_bottom = split_top + = Polar_Point( observer, + Point( t*ppoint1.x() + (1-t)*ppoint2.x(), + observer.y() ) ); + split_top.set_bearing( Angle(0.0) ); + split_bottom.set_bearing_to_2pi(); + elp.push_back( Polar_Edge( ppoint1 , split_bottom ) ); + elp.push_back( Polar_Edge( split_top , ppoint2 ) ); + continue; + } + //If the edge is not horizontal and doesn't cross the ray + //emanating horizontal and right of the observer. + else if( ppoint1.bearing() >= ppoint2.bearing() + and ppoint2.bearing() == Angle(0.0) + and ppoint1.bearing() > Angle(M_PI) ) + ppoint2.set_bearing_to_2pi(); + //Filter out edges which run 'against the grain'. + else if( ( ppoint1.bearing() == Angle(0,0) + and ppoint2.bearing() > Angle(M_PI) ) + or ppoint1.bearing() >= ppoint2.bearing() ) + continue; + elp.push_back( Polar_Edge( ppoint1, ppoint2 ) ); + continue; + } + //If edge is horizontal (w/in epsilon). + else{ + //Filter out edges which run 'against the grain'. + if( ppoint1.bearing() >= ppoint2.bearing() ) + continue; + elp.push_back( Polar_Edge( ppoint1, ppoint2 ) ); + } + }} + + //Construct a SORTED LIST, q1, OF VERTICES represented by + //Polar_Point_With_Edge_Info objects. A + //Polar_Point_With_Edge_Info is a derived class of Polar_Point + //which includes (1) a pointer to the corresponding edge + //(represented as a Polar_Edge) in the polar edge list elp, and + //(2) a boolean (is_first) which is true iff that vertex is the + //first Point of the respective edge (is_first == false => it's + //second Point). q1 is sorted according to lex. order of polar + //coordinates just as Polar_Points are, but with the additional + //requirement that if two vertices have equal polar coordinates, + //the vertex which is the first point of its respective edge is + //considered greater. q1 will serve as an event point queue for + //the radial sweep. + std::list q1; + Polar_Point_With_Edge_Info ppoint_wei1, ppoint_wei2; + std::list::iterator elp_iterator; + for(elp_iterator=elp.begin(); + elp_iterator!=elp.end(); + elp_iterator++){ + ppoint_wei1.set_polar_point( elp_iterator->first ); + ppoint_wei1.incident_edge = elp_iterator; + ppoint_wei1.is_first = true; + ppoint_wei2.set_polar_point( elp_iterator->second ); + ppoint_wei2.incident_edge = elp_iterator; + ppoint_wei2.is_first = false; + //If edge contains the observer, then adjust the bearing of + //the Polar_Point containing the observer. + if( distance(observer, ppoint_wei1) <= epsilon ){ + if( right_wall_bearing > left_wall_bearing ){ + ppoint_wei1.set_bearing( right_wall_bearing ); + (elp_iterator->first).set_bearing( right_wall_bearing ); + } + else{ + ppoint_wei1.set_bearing( Angle(0.0) ); + (elp_iterator->first).set_bearing( Angle(0.0) ); + } + } + else if( distance(observer, ppoint_wei2) <= epsilon ){ + if( right_wall_bearing > left_wall_bearing ){ + ppoint_wei2.set_bearing(right_wall_bearing); + (elp_iterator->second).set_bearing( right_wall_bearing ); + } + else{ + ppoint_wei2.set_bearing_to_2pi(); + (elp_iterator->second).set_bearing_to_2pi(); + } + } + q1.push_back(ppoint_wei1); + q1.push_back(ppoint_wei2); + } + //Put event point in correct order. + //STL list's sort method is a stable sort. + q1.sort(); + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[1;37;40m" + << "COMPUTING VISIBILITY POLYGON " << std::endl + << "for an observer located at [x y] = [" + << observer << "]" + << "\x1b[0m" + << std::endl << std::endl + << "\E[1;37;40m" <<"PREPROCESSING" << "\x1b[0m" + << std::endl << std::endl + << "q1 is" << std::endl; + std::list::iterator q1_itr; + for(q1_itr=q1.begin(); q1_itr!=q1.end(); q1_itr++){ + std::cout << "[x y bearing range is_first] = [" + << q1_itr->x() << " " + << q1_itr->y() << " " + << q1_itr->bearing() << " " + << q1_itr->range() << " " + << q1_itr->is_first << "]" + << std::endl; + } + } + + // + //-------PREPARE FOR MAIN LOOP------- + // + + //current_vertex is used to hold the event point (from q1) + //considered at iteration of the main loop. + Polar_Point_With_Edge_Info current_vertex; + //Note active_edge and e are not actually edges themselves, but + //iterators pointing to edges. active_edge keeps track of the + //current edge visibile during the sweep. e is an auxiliary + //variable used in calculation of k-points + std::list::iterator active_edge, e; + //More aux vars for computing k-points. + Polar_Point k; + double k_range; + Line_Segment xing; + + //Priority queue of edges, where higher priority indicates closer + //range to observer along current ray (of ray sweep). + Incident_Edge_Compare my_iec(observer, current_vertex, epsilon); + std::priority_queue::iterator, + std::vector::iterator>, + Incident_Edge_Compare> q2(my_iec); + + //Initialize main loop. + current_vertex = q1.front(); q1.pop_front(); + active_edge = current_vertex.incident_edge; + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[1;37;40m" + << "INITIALIZATION" + << "\x1b[0m" + << std::endl << std::endl + << "\x1b[35m" + << "Pop first vertex off q1" + << "\x1b[0m" + << ", set as current_vertex, \n" + << "and set active_edge to the corresponding " + << "incident edge." + << std::endl; + print_cv_and_ae(current_vertex, active_edge); + } + + //Insert e into q2 as long as it doesn't contain the + //observer. + if( distance(observer,active_edge->first) > epsilon + and distance(observer,active_edge->second) > epsilon ){ + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "Push current_vertex's edge onto q2." + << std::endl; + } + + q2.push(active_edge); + } + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[32m" + << "Add current_vertex to visibility polygon." + << "\x1b[0m" + << std::endl << std::endl + << "\E[1;37;40m" + << "MAIN LOOP" + << "\x1b[0m" + << std::endl; + } + + vertices_.push_back(current_vertex); + + //-------BEGIN MAIN LOOP-------// + // + //Perform radial sweep by sequentially considering each vertex + //(event point) in q1. + while( !q1.empty() ){ + + //Pop current_vertex from q1. + current_vertex = q1.front(); q1.pop_front(); + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\x1b[35m" + << "Pop next vertex off q1" << "\x1b[0m" + << " and set as current_vertex." + << std::endl; + print_cv_and_ae(current_vertex, active_edge); + } + + //---Handle Event Point--- + + //TYPE 1: current_vertex is the _second_vertex_ of active_edge. + if( current_vertex.incident_edge == active_edge + and !current_vertex.is_first ){ + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[36m" << "TYPE 1:" << "\x1b[0m" + << " current_vertex is the second vertex of active_edge." + << std::endl; + } + + if( !q1.empty() ){ + //If the next vertex in q1 is contiguous. + if( distance( current_vertex, q1.front() ) <= epsilon ){ + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "current_vertex is contiguous " + << "with the next vertex in q1." + << std::endl; + } + + continue; + } + } + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[32m" << "Add current_vertex to visibility polygon." + << "\x1b[0m" << std::endl; + } + + //Push current_vertex onto visibility polygon + vertices_.push_back( current_vertex ); + chop_spikes_at_back(observer, epsilon); + + while( !q2.empty() ){ + e = q2.top(); + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "Examine edge at top of q2." << std::endl + << "1st point of e [x y bearing range] = [" + << (e->first).x() << " " + << (e->first).y() << " " + << (e->first).bearing() << " " + << (e->first).range() << "]" << std::endl + << "2nd point of e [x y bearing range] = [" + << (e->second).x() << " " + << (e->second).y() << " " + << (e->second).bearing() << " " + << (e->second).range() << "]" << std::endl; + } + + //If the current_vertex bearing has not passed, in the + //lex. order sense, the bearing of the second point of the + //edge at the front of q2. + if( ( current_vertex.bearing().get() + <= e->second.bearing().get() ) + //For robustness. + and distance( Ray(observer, current_vertex.bearing()), + e->second ) >= epsilon + /* was + and std::min( distance(Ray(observer, current_vertex.bearing()), + e->second), + distance(Ray(observer, e->second.bearing()), + current_vertex) + ) >= epsilon + */ + ){ + //Find intersection point k of ray (through + //current_vertex) with edge e. + xing = intersection( Ray(observer, current_vertex.bearing()), + Line_Segment(e->first, + e->second), + epsilon ); + + //assert( xing.size() > 0 ); + + if( xing.size() > 0 ){ + k = Polar_Point( observer , xing.first() ); + } + else{ //Error contingency. + k = current_vertex; + e = current_vertex.incident_edge; + } + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[32m" + << "Add a type 1 k-point to visibility polygon." + << "\x1b[0m" << std::endl + << std::endl + << "Set active_edge to edge at top of q2." + << std::endl; + } + + //Push k onto the visibility polygon. + vertices_.push_back(k); + chop_spikes_at_back(observer, epsilon); + active_edge = e; + break; + } + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "Pop edge off top of q2." << std::endl; + } + + q2.pop(); + } + } //Close Type 1. + + //If current_vertex is the _first_vertex_ of its edge. + if( current_vertex.is_first ){ + //Find intersection point k of ray (through current_vertex) + //with active_edge. + xing = intersection( Ray(observer, current_vertex.bearing()), + Line_Segment(active_edge->first, + active_edge->second), + epsilon ); + if( xing.size() == 0 + or ( distance(active_edge->first, observer) <= epsilon + and active_edge->second.bearing() + <= current_vertex.bearing() ) + or active_edge->second < current_vertex ){ + k_range = INFINITY; + } + else{ + k = Polar_Point( observer , xing.first() ); + k_range = k.range(); + } + + //Incident edge of current_vertex. + e = current_vertex.incident_edge; + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << " k_range = " + << k_range + << " (range of active edge along " + << "bearing of current vertex)" << std::endl + << "current_vertex.range() = " + << current_vertex.range() << std::endl; + } + + //Insert e into q2 as long as it doesn't contain the + //observer. + if( distance(observer, e->first) > epsilon + and distance(observer, e->second) > epsilon ){ + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "Push current_vertex's edge onto q2." + << std::endl; + } + + q2.push(e); + } + + //TYPE 2: current_vertex is (1) a first vertex of some edge + //other than active_edge, and (2) that edge should not become + //the next active_edge. This happens, e.g., if that edge is + //(rangewise) in back along the current bearing. + if( k_range < current_vertex.range() ){ + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[36m" << "TYPE 2:" << "\x1b[0m" + << " current_vertex is" << std::endl + << "(1) a first vertex of some edge " + "other than active_edge, and" << std::endl + << "(2) that edge should not become " + << "the next active_edge." + << std::endl; + + } + + } //Close Type 2. + + //TYPE 3: current_vertex is (1) the first vertex of some edge + //other than active_edge, and (2) that edge should become the + //next active_edge. This happens, e.g., if that edge is + //(rangewise) in front along the current bearing. + if( k_range >= current_vertex.range() + ){ + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[36m" << "TYPE 3:" << "\x1b[0m" + << " current_vertex is" << std::endl + << "(1) the first vertex of some edge " + "other than active edge, and" << std::endl + << "(2) that edge should become " + << "the next active_edge." + << std::endl; + } + + //Push k onto the visibility polygon unless effectively + //contiguous with current_vertex. + if( xing.size() > 0 + //and k == k + and k_range != INFINITY + and distance(k, current_vertex) > epsilon + and distance(active_edge->first, observer) > epsilon + ){ + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[32m" + << "Add type 3 k-point to visibility polygon." + << "\x1b[0m" << std::endl; + } + + //Push k-point onto the visibility polygon. + vertices_.push_back(k); + chop_spikes_at_back(observer, epsilon); + } + + //Push current_vertex onto the visibility polygon. + vertices_.push_back(current_vertex); + chop_spikes_at_back(observer, epsilon); + //Set active_edge to edge of current_vertex. + active_edge = e; + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "\E[32m" << "Add current_vertex to visibility polygon." + << "\x1b[0m" << std::endl + << std::endl + << "Set active_edge to edge of current_vertex." + << std::endl; + } + + } //Close Type 3. + } + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "visibility polygon vertices so far are \n" + << Polygon(vertices_) << std::endl + << std::endl; + } + } // + // + //-------END MAIN LOOP-------// + + //The Visibility_Polygon should have a minimal representation + chop_spikes_at_wrap_around( observer , epsilon ); + eliminate_redundant_vertices( epsilon ); + chop_spikes( observer, epsilon ); + enforce_standard_form(); + + if(PRINTING_DEBUG_DATA){ + std::cout << std::endl + << "Final visibility polygon vertices are \n" + << Polygon(vertices_) << std::endl + << std::endl; + } + + } + Visibility_Polygon::Visibility_Polygon(const Point& observer, + const Polygon& polygon_temp, + double epsilon) + { + *this = Visibility_Polygon( observer, Environment(polygon_temp), epsilon ); + } + + + //Visibility_Graph + + + Visibility_Graph::Visibility_Graph( const Visibility_Graph& vg2 ) + { + n_ = vg2.n_; + vertex_counts_ = vg2.vertex_counts_; + + //Allocate adjacency matrix + adjacency_matrix_ = new bool*[n_]; + adjacency_matrix_[0] = new bool[n_*n_]; + for(unsigned i=1; i points, + const Environment& environment, + double epsilon) + { + n_ = points.size(); + + //fill vertex_counts_ + vertex_counts_.push_back( n_ ); + + //allocate a contiguous chunk of memory for adjacency_matrix_ + adjacency_matrix_ = new bool*[n_]; + adjacency_matrix_[0] = new bool[n_*n_]; + for(unsigned i=1; i. +*/ + +/** + * \mainpage + *
+ * see also the VisiLibity Project Page + *
+ * Authors: Karl J. Obermeyer + *
+ * \section developers For Developers + * Coding Standards + *
+ * \section release_notes Release Notes + * Current Functionality + *
    + *
  • visibility polygons in polygonal environments with holes
    • + *
  • visibility graphs
    • + *
  • shortest path planning for a point
    • + *
+ */ + +#ifndef VISILIBITY_H +#define VISILIBITY_H + + +//Uncomment these lines when compiling under +//Microsoft Visual Studio +/* +#include +#define NAN std::numeric_limits::quiet_NaN() +#define INFINITY std::numeric_limits::infinity() +#define M_PI 3.141592653589793238462643 +#define and && +#define or || +*/ + +#include //math functions in std namespace +#include +#include //queue and priority_queue. +#include //priority queues with iteration, + //integrated keys +#include +#include //sorting, min, max, reverse +#include //rand and srand +#include //Unix time +#include //file I/O +#include +#include //C-string manipulation +#include //string class +#include //assertions + + +/// VisiLibity's sole namespace +namespace VisiLibity +{ + + //Fwd declaration of all classes and structs serves as index. + struct Bounding_Box; + class Point; + class Line_Segment; + class Angle; + class Ray; + class Polar_Point; + class Polyline; + class Polygon; + class Environment; + class Guards; + class Visibility_Polygon; + class Visibility_Graph; + + + /** \brief floating-point display precision. + * + * This is the default precision with which floating point + * numbers are displayed or written to files for classes with a + * write_to_file() method. + */ + const int FIOS_PRECISION = 10; + + + /** \brief get a uniform random sample from an (inclusive) interval + * on the real line + * + * \author Karl J. Obermeyer + * \param lower_bound lower bound of the real interval + * \param upper_bound upper bound of the real interval + * \pre \a lower_bound <= \a upper_bound + * \return a random sample from a uniform probability distribution + * on the real interval [\a lower_bound, \a upper_bound] + * \remarks Uses the Standard Library's rand() function. rand() + * should be seeded (only necessary once at the beginning of the + * program) using the command + * std::srand( std::time( NULL ) ); rand(); + * \warning performance degrades as upper_bound - lower_bound + * approaches RAND_MAX. + */ + double uniform_random_sample(double lower_bound, double upper_bound); + + + /** \brief rectangle with sides parallel to the x- and y-axes + * + * \author Karl J. Obermeyer + * Useful for enclosing other geometric objects. + */ + struct Bounding_Box { double x_min, x_max, y_min, y_max; }; + + + /// Point in the plane represented by Cartesian coordinates + class Point + { + public: + //Constructors + /** \brief default + * + * \remarks Data defaults to NAN so that checking whether the + * data are numbers can be used as a precondition in functions. + */ + Point() : x_(NAN) , y_(NAN) { } + /// costruct from raw coordinates + Point(double x_temp, double y_temp) + { x_=x_temp; y_=y_temp; } + //Accessors + /// get x coordinate + double x () const { return x_; } + /// get y coordinate + double y () const { return y_; } + /** \brief closest Point on \a line_segment_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers + * and \a line_segment_temp is nonempty + * \return the Point on \a line_segment_temp which is the smallest + * Euclidean distance from the calling Point + */ + Point projection_onto(const Line_Segment& line_segment_temp) const; + /** \brief closest Point on \a ray_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point and \a ray_temp data are numbers + * \return the Point on \a ray_temp which is the smallest + * Euclidean distance from the calling Point + */ + Point projection_onto(const Ray& ray_temp) const; + /** \brief closest Point on \a polyline_temp + * + * \pre the calling Point data are numbers and \a polyline_temp + * is nonempty + * \return the Point on \a polyline_temp which is the smallest + * Euclidean distance from the calling Point + */ + Point projection_onto(const Polyline& polyline_temp) const; + /** \brief closest vertex of \a polygon_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a polygon_temp + * is nonempty + * \return the vertex of \a polygon_temp which is the + * smallest Euclidean distance from the calling Point + */ + Point projection_onto_vertices_of(const Polygon& polygon_temp) const; + /** \brief closest vertex of \a environment_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a environment_temp + * is nonempty + * \return the vertex of \a environment_temp which is + * the smallest Euclidean distance from the calling Point + */ + Point projection_onto_vertices_of(const Environment& + enviroment_temp) const; + /** \brief closest Point on boundary of \a polygon_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a polygon_temp + * is nonempty + * \return the Point on the boundary of \a polygon_temp which is the + * smallest Euclidean distance from the calling Point + */ + Point projection_onto_boundary_of(const Polygon& polygon_temp) const; + /** \brief closest Point on boundary of \a environment_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a environment_temp + * is nonempty + * \return the Point on the boundary of \a environment_temp which is + * the smalles Euclidean distance from the calling Point + */ + Point projection_onto_boundary_of(const Environment& + enviroment_temp) const; + /** \brief true iff w/in \a epsilon of boundary of \a polygon_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a polygon_temp + * is nonempty + * \return true iff the calling Point is within Euclidean distance + * \a epsilon of \a polygon_temp 's boundary + * \remarks O(n) time complexity, where n is the number + * of vertices of \a polygon_temp + */ + bool on_boundary_of(const Polygon& polygon_temp, + double epsilon=0.0) const; + /** \brief true iff w/in \a epsilon of boundary of \a environment_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a environment_temp + * is nonempty + * \return true iff the calling Point is within Euclidean distance + * \a epsilon of \a environment_temp 's boundary + * \remarks O(n) time complexity, where n is the number + * of vertices of \a environment_temp + */ + bool on_boundary_of(const Environment& environment_temp, + double epsilon=0.0) const; + /** \brief true iff w/in \a epsilon of \a line_segment_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a line_segment_temp + * is nonempty + * \return true iff the calling Point is within distance + * \a epsilon of the (closed) Line_Segment \a line_segment_temp + */ + bool in(const Line_Segment& line_segment_temp, + double epsilon=0.0) const; + /** \brief true iff w/in \a epsilon of interior but greater than + * \a espilon away from endpoints of \a line_segment_temp + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a line_segment_temp + * is nonempty + * \return true iff the calling Point is within distance \a + * epsilon of \line_segment_temp, but distance (strictly) greater + * than epsilon from \a line_segment_temp 's endpoints. + */ + bool in_relative_interior_of(const Line_Segment& line_segment_temp, + double epsilon=0.0) const; + /** \brief true iff w/in \a epsilon of \a polygon_temp + * + * \author Karl J. Obermeyer + * + * \pre the calling Point data are numbers and \a polygon_temp is + * \a epsilon -simple. Test simplicity with + * Polygon::is_simple(epsilon) + * + * \return true iff the calling Point is a Euclidean distance no greater + * than \a epsilon from the (closed) Polygon (with vertices listed + * either cw or ccw) \a polygon_temp. + * \remarks O(n) time complexity, where n is the number of vertices + * in \a polygon_temp + */ + bool in(const Polygon& polygon_temp, + double epsilon=0.0) const; + /** \brief true iff w/in \a epsilon of \a environment_temp + * + * \author Karl J. Obermeyer + * + * \pre the calling Point data are numbers and \a environment_temp + * is nonempty and \a epsilon -valid. Test validity with + * Enviroment::is_valid(epsilon) + * + * \return true iff the calling Point is a Euclidean distance no greater + * than \a epsilon from the in the (closed) Environment \a environment_temp + * \remarks O(n) time complexity, where n is the number of + * vertices in \a environment_temp + */ + bool in(const Environment& environment_temp, + double epsilon=0.0) const; + /** \brief true iff w/in \a epsilon of some endpoint + * of \a line_segment_temp + * + * \pre the calling Point data are numbers and \a line_segment_temp + * is nonempty + * \return true iff calling Point is a Euclidean distance no greater + * than \a epsilon from some endpoint of \a line_segment_temp + */ + bool is_endpoint_of(const Line_Segment& line_segment_temp, + double epsilon=0.0) const; + //Mutators + /// change x coordinate + void set_x(double x_temp) { x_ = x_temp;} + /// change y coordinate + void set_y(double y_temp) { y_ = y_temp;} + /** \brief relocate to closest vertex if w/in \a epsilon of some + * vertex (of \a polygon_temp) + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a polygon_temp + * is nonempty + * \post If the calling Point was a Euclidean distance no greater + * than \a epsilon from any vertex of \a polygon_temp, then it + * will be repositioned to coincide with the closest such vertex + * \remarks O(n) time complexity, where n is the number of + * vertices in \a polygon_temp. + */ + void snap_to_vertices_of(const Polygon& polygon_temp, + double epsilon=0.0); + /** \brief relocate to closest vertex if w/in \a epsilon of some + * vertex (of \a environment_temp) + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a environment_temp + * is nonempty + * \post If the calling Point was a Euclidean distance no greater + * than \a epsilon from any vertex of \a environment_temp, then it + * will be repositioned to coincide with the closest such vertex + * \remarks O(n) time complexity, where n is the number of + * vertices in \a environment_temp. + */ + void snap_to_vertices_of(const Environment& environment_temp, + double epsilon=0.0); + /** \brief relocate to closest Point on boundary if w/in \a epsilon + * of the boundary (of \a polygon_temp) + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a polygon_temp + * is nonempty + * \post if the calling Point was a Euclidean distance no greater + * than \a epsilon from the boundary of \a polygon_temp, then it + * will be repositioned to it's projection onto that boundary + * \remarks O(n) time complexity, where n is the number of + * vertices in \a polygon_temp. + */ + void snap_to_boundary_of(const Polygon& polygon_temp, + double epsilon=0.0); + /** \brief relocate to closest Point on boundary if w/in \a epsilon + * of the boundary (of \a environment_temp) + * + * \author Karl J. Obermeyer + * \pre the calling Point data are numbers and \a environment_temp + * is nonempty + * \post if the calling Point was a Euclidean distance no greater + * than \a epsilon from the boundary of \a environment_temp, then it + * will be repositioned to it's projection onto that boundary + * \remarks O(n) time complexity, where n is the number of + * vertices in \a environment_temp. + */ + void snap_to_boundary_of(const Environment& environment_temp, + double epsilon=0.0); + protected: + double x_; + double y_; + }; + + + /** \brief True iff Points' coordinates are identical. + * + * \remarks NAN==NAN returns false, so if either point has + * not been assigned real number coordinates, they will not be == + */ + bool operator == (const Point& point1, const Point& point2); + /// True iff Points' coordinates are not identical. + bool operator != (const Point& point1, const Point& point2); + + + /** \brief compare lexicographic order of points + * + * For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or + * p1.x()==p2.x() and p1.y() (const Point& point1, const Point& point2); + /** \brief compare lexicographic order of points + * + * For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or + * p1.x()==p2.x() and p1.y()= (const Point& point1, const Point& point2); + /** \brief compare lexicographic order of points + * + * For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or + * p1.x()==p2.x() and p1.y() 0 + * \return the first Point of the Line_Segment + * \remarks If size() == 1, then both first() and second() are valid + * and will return the same Point + */ + Point first() const; + /** \brief second endpoint + * + * \pre size() > 0 + * \return the second Point of the Line_Segment + * \remarks If size() == 1, then both first() and second() are valid + * and will return the same Point + */ + Point second() const; + /** \brief number of distinct endpoints + * + * \remarks + * size 0 => empty line segment; + * size 1 => degenerate (single point) line segment; + * size 2 => full-fledged (bona fide) line segment + */ + unsigned size() const { return size_; } + /** \brief midpoint + * + * \pre size() > 0 + */ + Point midpoint() const; + /** \brief Euclidean length + * + * \pre size() > 0 + */ + double length() const; + /** \brief true iff vertices in lex. order + * + * \pre size() > 0 + * \return true iff vertices are listed beginning with the vertex + * which is lexicographically smallest (lowest x, then lowest y) + * \remarks lex. comparison is very sensitive to perturbations if + * two Points nearly define a line parallel to one of the axes + */ + bool is_in_standard_form() const; + //Mutators + /// assignment operator + Line_Segment& operator = (const Line_Segment& line_segment_temp); + /** \brief set first endpoint + * + * \remarks if \a point_temp is w/in a distance \a epsilon of an existing + * endpoint, the coordinates of \a point_temp are used and size is set to + * 1 as appropriate + */ + void set_first(const Point& point_temp, double epsilon=0.0); + /** \brief set second endpoint + * + * \remarks if \a point_temp is w/in a distance \a epsilon of an existing + * endpoint, the coordinates of \a point_temp are used and size is set to + * 1 as appropriate + */ + void set_second(const Point& point_temp, double epsilon=0.0); + /** \brief reverse order of endpoints + * + * \post order of endpoints is reversed. + */ + void reverse(); + /** \brief enforce that lex. smallest endpoint first + * + * \post the lexicographically smallest endpoint (lowest x, then lowest y) + * is first + * \remarks lex. comparison is very sensitive to perturbations if + * two Points nearly define a line parallel to one of the axes + */ + void enforce_standard_form(); + /// erase both endpoints and set line segment empty (size 0) + void clear(); + /// destructor + virtual ~Line_Segment(); + protected: + //Pointer to dynamic array of endpoints. + Point *endpoints_; + //See size() comments. + unsigned size_; + }; + + + /** \brief true iff endpoint coordinates are exactly equal, but + * false if either Line_Segment has size 0 + * + * \remarks respects ordering of vertices, i.e., even if the line segments + * overlap exactly, they are not considered == unless the orientations are + * the same + */ + bool operator == (const Line_Segment& line_segment1, + const Line_Segment& line_segment2); + /// true iff endpoint coordinates are not == + bool operator != (const Line_Segment& line_segment1, + const Line_Segment& line_segment2); + + + /** \brief true iff line segments' endpoints match up w/in a (closed) + * \a epsilon ball of each other, but false if either + * Line_Segment has size 0 + * + * \author Karl J. Obermeyer + * \remarks this function will return true even if it has to flip + * the orientation of one of the segments to get the vertices to + * match up + */ + bool equivalent(Line_Segment line_segment1, + Line_Segment line_segment2, double epsilon=0); + + + /** \brief Euclidean distance between Line_Segments + * + * \author Karl J. Obermeyer + * \pre \a line_segment1.size() > 0 and \a line_segment2.size() > 0 + */ + double distance(const Line_Segment& line_segment1, + const Line_Segment& line_segment2); + + + /** \brief Euclidean distance between a Line_Segment and the + * boundary of a Polygon + * + * \author Karl J. Obermeyer + * \pre \a line_segment.size() > 0 and \a polygon.n() > 0 + */ + double boundary_distance(const Line_Segment& line_segment, + const Polygon& polygon); + + + /** \brief Euclidean distance between a Line_Segment and the + * boundary of a Polygon + * + * \author Karl J. Obermeyer + * \pre \a line_segment.size() > 0 and \a polygon.n() > 0 + */ + double boundary_distance(const Polygon& polygon, + const Line_Segment& line_segment); + + + /** \brief true iff the Euclidean distance between Line_Segments is + * no greater than \a epsilon, false if either line segment + * has size 0 + * + * \author Karl J. Obermeyer + */ + bool intersect(const Line_Segment& line_segment1, + const Line_Segment& line_segment2, + double epsilon=0.0); + + + /** \brief true iff line segments intersect properly w/in epsilon, + * false if either line segment has size 0 + * + * \author Karl J. Obermeyer + * \return true iff Line_Segments intersect exactly at a single + * point in their relative interiors. For robustness, here the + * relative interior of a Line_Segment is consider to be any Point + * in the Line_Segment which is a distance greater than \a epsilon + * from both endpoints. + */ + bool intersect_proper(const Line_Segment& line_segment1, + const Line_Segment& line_segment2, + double epsilon=0.0); + + + /** \brief intersection of Line_Segments + * + * \author Karl J. Obermeyer + * \return a Line_Segment of size 0, 1, or 2 + * \remarks size 0 results if the distance (or at least the + * floating-point computed distance) between line_segment1 and + * line_segment2 is (strictly) greater than epsilon. size 1 results + * if the segments intersect poperly, form a T intersection, or -- + * intersection. size 2 results when two or more endpoints are a + * Euclidean distance no greater than \a epsilon from the opposite + * segment, and the overlap of the segments has a length greater + * than \a epsilon. + */ + Line_Segment intersection(const Line_Segment& line_segment1, + const Line_Segment& line_segment2, + double epsilon=0.0); + + + /// print a Line_Segment + std::ostream& operator << (std::ostream& outs, + const Line_Segment& line_segment_temp); + + + /** \brief angle in radians represented by a value in + * the interval [0,2*M_PI] + * + * \remarks the intended interpretation is that angles 0 and 2*M_PI + * correspond to the positive x-axis of the coordinate system + */ + class Angle + { + public: + //Constructors + /** \brief default + * + * \remarks data defaults to NAN so that checking whether the + * data are numbers can be used as a precondition in functions + */ + Angle() : angle_radians_(NAN) { } + /// construct from real value, mod into interval [0, 2*M_PI) + Angle(double data_temp); + /** \brief construct using 4 quadrant inverse tangent into [0, 2*M_PI), + * where 0 points along the x-axis + */ + Angle(double rise_temp, double run_temp); + //Accessors + /// get radians + double get() const { return angle_radians_; } + //Mutators + /// set angle, mod into interval [0, 2*PI) + void set(double data_temp); + /** \brief set angle data to 2*M_PI + * + * \remarks sometimes it is necessary to set the angle value to + * 2*M_PI instead of 0, so that the lex. inequalities behave + * appropriately during a radial line sweep + */ + void set_to_2pi() { angle_radians_=2*M_PI; } + /// set to new random angle in [0, 2*M_PI) + void randomize(); + private: + double angle_radians_; + }; + + + /// compare angle radians + bool operator == (const Angle& angle1, const Angle& angle2); + /// compare angle radians + bool operator != (const Angle& angle1, const Angle& angle2); + + + /// compare angle radians + bool operator > (const Angle& angle1, const Angle& angle2); + /// compare angle radians + bool operator < (const Angle& angle1, const Angle& angle2); + /// compare angle radians + bool operator >= (const Angle& angle1, const Angle& angle2); + /// compare angle radians + bool operator <= (const Angle& angle1, const Angle& angle2); + + + /// add angles' radians and mod into [0, 2*M_PI) + Angle operator + (const Angle& angle1, const Angle& angle2); + /// subtract angles' radians and mod into [0, 2*M_PI) + Angle operator - (const Angle& angle1, const Angle& angle2); + + + /** \brief geodesic distance in radians between Angles + * + * \author Karl J. Obermeyer + * \pre \a angle1 and \a angle2 data are numbers + */ + double geodesic_distance(const Angle& angle1, const Angle& angle2); + + + /** \brief 1.0 => geodesic path from angle1 to angle2 + * is couterclockwise, -1.0 => clockwise + * + * \author Karl J. Obermeyer + * \pre \a angle1 and \a angle2 data are numbers + */ + double geodesic_direction(const Angle& angle1, const Angle& angle2); + + + /// print Angle + std::ostream& operator << (std::ostream& outs, const Angle& angle_temp); + + + /** \brief Point in the plane packaged together with polar + * coordinates w.r.t. specified origin + * + * The origin of the polar coordinate system is stored with the + * Polar_Point (in \a polar_origin_) and bearing is measured ccw from the + * positive x-axis. + * \remarks used, e.g., for radial line sweeps + */ + class Polar_Point : public Point + { + public: + //Constructors + /** \brief default + * + * \remarks Data defaults to NAN so that checking whether the + * data are numbers can be used as a precondition in functions. + */ + Polar_Point() : Point(), range_(NAN), bearing_(NAN) { } + /** \brief construct from (Cartesian) Points + * + * \pre member data of \a polar_origin_temp and \a point_temp have + * been assigned (numbers) + * \param polar_origin_temp the origin of the polar coordinate system + * \param point_temp the point to be represented + * \remarks if polar_origin_temp == point_temp, the default + * bearing is Angle(0.0) + */ + Polar_Point(const Point& polar_origin_temp, + const Point& point_temp, + double epsilon=0.0); + //Accessors + /** \brief origin of the polar coordinate system in which the point is + * represented + */ + Point polar_origin() const { return polar_origin_; } + /// Euclidean distance from the point represented to the origin of + /// the polar coordinate system + double range() const { return range_; } + /// bearing from polar origin w.r.t. direction parallel to x-axis + Angle bearing() const { return bearing_; } + //Mutators + /** \brief set the origin of the polar coordinate system + * + * \remarks x and y held constant, bearing and range modified + * accordingly + */ + void set_polar_origin(const Point& polar_origin_temp); + /** \brief set x + * + * \remarks polar_origin held constant, bearing and range modified + * accordingly + */ + void set_x(double x_temp); + /** \brief set y + * + * \remarks polar_origin held constant, bearing and range modified + * accordingly + */ + void set_y(double y_temp); + /** \brief set range + * + * \remarks polar_origin held constant, x and y modified + * accordingly + */ + void set_range(double range_temp); + /** \brief set bearing + * + * \remarks polar_origin and range held constant, x and y modified + * accordingly + */ + void set_bearing(const Angle& bearing_temp); + /** \brief set bearing Angle data to 2*M_PI + * + * \remarks Special function for use in computations involving a + * radial line sweep; sometimes it is necessary to set the angle + * value to 2*PI instead of 0, so that the lex. inequalities + * behave appropriately + */ + void set_bearing_to_2pi() { bearing_.set_to_2pi(); } + protected: + //Origin of the polar coordinate system in world coordinates. + Point polar_origin_; + //Polar coordinates where radius always positive, and angle + //measured ccw from the world coordinate system's x-axis. + double range_; + Angle bearing_; + }; + + + /** \brief compare member data + * + * \remarks returns false if any member data are NaN + */ + bool operator == (const Polar_Point& polar_point1, + const Polar_Point& polar_point2); + bool operator != (const Polar_Point& polar_point1, + const Polar_Point& polar_point2); + + + /** \brief compare according to polar lexicographic order + * (smaller bearing, then smaller range) + * + * false if any member data have not been assigned (numbers) + * \remarks lex. comparison is very sensitive to perturbations if + * two Points nearly define a radial line + */ + bool operator > (const Polar_Point& polar_point1, + const Polar_Point& polar_point2); + /** \brief compare according to polar lexicographic order + * (smaller bearing, then smaller range) + * + * false if any member data have not been assigned (numbers) + * \remarks lex. comparison is very sensitive to perturbations if + * two Points nearly define a radial line + */ + bool operator < (const Polar_Point& polar_point1, + const Polar_Point& polar_point2); + /** \brief compare according to polar lexicographic order + * (smaller bearing, then smaller range) + * + * false if any member data have not been assigned (numbers) + * \remarks lex. comparison is very sensitive to perturbations if + * two Points nearly define a radial line + */ + bool operator >= (const Polar_Point& polar_point1, + const Polar_Point& polar_point2); + /** \brief compare according to polar lexicographic order + * (smaller bearing, then smaller range) + * + * false if any member data have not been assigned (numbers) + * \remarks lex. comparison is very sensitive to perturbations if + * two Points nearly define a radial line + */ + bool operator <= (const Polar_Point& polar_point1, + const Polar_Point& polar_point2); + + + /// print Polar_Point + std::ostream& operator << (std::ostream& outs, + const Polar_Point& polar_point_temp); + + + /// ray in the plane represented by base Point and bearing Angle + class Ray + { + public: + //Constructors + /** \brief default + * + * \remarks data defaults to NAN so that checking whether the data + * are numbers can be used as a precondition in functions + */ + Ray() { } + /// construct ray emanating from \a base_point_temp in the direction + /// \a bearing_temp + Ray(Point base_point_temp, Angle bearing_temp) : + base_point_(base_point_temp) , bearing_(bearing_temp) {} + /// construct ray emanating from \a base_point_temp towards + /// \a bearing_point + Ray(Point base_point_temp, Point bearing_point); + //Accessors + /// get base point + Point base_point() const { return base_point_; } + /// get bearing + Angle bearing() const { return bearing_; } + //Mutators + /// set base point + void set_base_point(const Point& point_temp) + { base_point_ = point_temp; } + /// set bearing + void set_bearing(const Angle& angle_temp) + { bearing_ = angle_temp; } + private: + Point base_point_; + Angle bearing_; + }; + + + /** \brief compare member data + * + * \remarks returns false if any member data are NaN + */ + bool operator == (const Ray& ray1, + const Ray& ray2); + /** \brief compare member data + * + * \remarks negation of == + */ + bool operator != (const Ray& ray1, + const Ray& ray2); + + + /** \brief compute the intersection of a Line_Segment with a Ray + * + * \author Karl J. Obermeyer + * \pre member data of \a ray_temp has been assigned (numbers) and + * \a line_segment_temp has size greater than 0 + * \remarks as a convention, if the intersection has positive + * length, the Line_Segment returned has the first point closest to + * the Ray's base point + */ + Line_Segment intersection(const Ray ray_temp, + const Line_Segment& line_segment_temp, + double epsilon=0.0); + /** \brief compute the intersection of a Line_Segment with a Ray + * + * \author Karl J. Obermeyer + * \pre member data of \a ray_temp has been assigned (numbers) and + * \a line_segment_temp has size greater than 0 + * \remarks as a convention, if the intersection has positive + * length, the Line_Segment returned has the first point closest to + * the Ray's base point + */ + Line_Segment intersection(const Line_Segment& line_segment_temp, + const Ray& ray_temp, + double epsilon=0.0); + + + ///oriented polyline in the plane represented by list of vertices + class Polyline + { + public: + friend class Point; + //Constructors + /// default to empty + Polyline() { } + /// construct from vector of vertices + Polyline(const std::vector& vertices_temp) + { vertices_ = vertices_temp; } + //Accessors + /** \brief raw access + * + * \remarks for efficiency, no bounds checks; usually trying to + * access out of bounds causes a bus error + */ + Point operator [] (unsigned i) const + { return vertices_[i]; } + /// vertex count + unsigned size() const + { return vertices_.size(); } + /// Euclidean length of the Polyline + double length() const; + /** \brief Euclidean diameter + * + * \pre Polyline has greater than 0 vertices + * \return the maximum Euclidean distance between all pairs of + * vertices + * \remarks time complexity O(n^2), where n is the number of + * vertices representing the Polyline + */ + double diameter() const; + //a box which fits snugly around the Polyline + Bounding_Box bbox() const; + //Mutators + /** \brief raw access + * + * \remarks for efficiency, no bounds checks; usually trying to + * access out of bounds causes a bus error + */ + Point& operator [] (unsigned i) + { return vertices_[i]; } + /// erase all points + void clear() + { vertices_.clear(); } + /// add a vertex to the back (end) of the list + void push_back(const Point& point_temp) + { vertices_.push_back(point_temp); } + /// delete a vertex to the back (end) of the list + void pop_back() + { vertices_.pop_back(); } + /// reset the whole list of vertices at once + void set_vertices(const std::vector& vertices_temp) + { vertices_ = vertices_temp; } + /** \brief eliminates vertices which are (\a epsilon) - colinear + * with their respective neighbors + * + * \author Karl J. Obermeyer + * \post the Euclidean distance between each vertex and the line + * segment connecting its neighbors is at least \a epsilon + * \remarks time complexity O(n), where n is the number of + * vertices representing the Polyline. + */ + void eliminate_redundant_vertices(double epsilon=0.0); + //Reduce number of vertices in representation... + //void smooth(double epsilon); + /// reverse order of vertices + void reverse(); + /// append the points from another polyline + void append( const Polyline& polyline ); + private: + std::vector vertices_; + }; + + + //print Polyline + std::ostream& operator << (std::ostream& outs, + const Polyline& polyline_temp); + + + /** \brief simple polygon in the plane represented by list of vertices + * + * Simple here means non-self-intersecting. More precisely, edges + * should not (i) intersect with an edge not adjacent to it, nor + * (ii) intersect at more than one Point with an adjacent edge. + * \remarks vertices may be listed cw or ccw + */ + class Polygon + { + public: + friend class Point; + //Constructors + ///default to empty + Polygon() { } + /** \brief construct from *.polygon file + * + * \author Karl J. Obermeyer + * \remarks for efficiency, simplicity check not called here + */ + Polygon(const std::string& filename); + /** \brief construct from vector of vertices + * + * \remarks for efficiency, simplicity check not called here + */ + Polygon(const std::vector& vertices_temp); + /** \brief construct triangle from 3 Points + * + * \remarks for efficiency, simplicity check not called here + */ + Polygon(const Point& point0, const Point& point1, const Point& point2); + //Accessors + /** \brief access with automatic wrap-around in forward direction + * + * \remarks For efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + const Point& operator [] (unsigned i) const + { return vertices_[i % vertices_.size()]; } + /** \brief vertex count + * + * \remarks O(1) time complexity + */ + unsigned n() const { return vertices_.size(); } + /** \brief reflex vertex count (nonconvex vertices) + * + * \author Karl J. Obermeyer + * \remarks Works regardless of polygon orientation (ccw vs cw), + * but assumes no redundant vertices. Time complexity O(n), where + * n is the number of vertices representing the Polygon + */ + unsigned r() const; + /** \brief true iff Polygon is (\a epsilon) simple + * + * \author Karl J. Obermeyer + * + * \remarks A Polygon is considered \a epsilon -simple iff (i) the + * Euclidean distance between nonadjacent edges is no greater than + * \a epsilon, (ii) adjacent edges intersect only at their single + * shared Point, (iii) and it has at least 3 vertices. One + * consequence of these conditions is that there can be no + * redundant vertices. + */ + bool is_simple(double epsilon=0.0) const; + /** \brief true iff lexicographically smallest vertex is first in + * the list of vertices representing the Polygon + * + * \author Karl J. Obermeyer + * \remarks lex. comparison is very sensitive to perturbations if + * two Points nearly define a line parallel to one of the axes + */ + bool is_in_standard_form() const; + /// perimeter length + double boundary_length() const; + /** oriented area of the Polygon + * + * \author Karl J. Obermeyer + * \pre Polygon is simple, but for efficiency simplicity is not asserted. + * area > 0 => vertices listed ccw, + * area < 0 => cw + * \remarks O(n) time complexity, where n is the number + * of vertices representing the Polygon + */ + double area() const; + /** \brief Polygon's centroid (center of mass) + * + * \author Karl J. Obermeyer + * \pre Polygon has greater than 0 vertices and is simple, + * but for efficiency simplicity is not asserted + */ + Point centroid() const; + /** \brief Euclidean diameter + * + * \pre Polygon has greater than 0 vertices + * \return maximum Euclidean distance between all pairs of + * vertices + * \remarks time complexity O(n^2), where n is the number of + * vertices representing the Polygon + */ + double diameter() const; + /** \brief box which fits snugly around the Polygon + * + * \author Karl J. Obermeyer + * \pre Polygon has greater than 0 vertices + */ + Bounding_Box bbox() const; + // Returns a vector of n pts randomly situated in the polygon. + std::vector random_points(const unsigned& count, + double epsilon=0.0) const; + /** \brief write list of vertices to *.polygon file + * + * \author Karl J. Obermeyer + * Uses intuitive human and computer readable decimal format with + * display precision \a fios_precision_temp. + * \pre \a fios_precision_temp >=1 + */ + void write_to_file(const std::string& filename, + int fios_precision_temp=FIOS_PRECISION); + //Mutators + /** \brief access with automatic wrap-around in forward direction + * + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + Point& operator [] (unsigned i) { return vertices_[i % vertices_.size()]; } + /// set vertices using STL vector of Points + void set_vertices(const std::vector& vertices_temp) + { vertices_ = vertices_temp; } + /// push a Point onto the back of the vertex list + void push_back(const Point& vertex_temp ) + { vertices_.push_back( vertex_temp ); } + /// erase all vertices + void clear() + { vertices_.clear(); } + /** \brief enforces that the lexicographically smallest vertex is first + * in the list of vertices representing the Polygon + * + * \author Karl J. Obermeyer + * \remarks O(n) time complexity, where n is the number of + * vertices representing the Polygon. Lex. comparison is very + * sensitive to perturbations if two Points nearly define a line + * parallel to one of the axes + */ + void enforce_standard_form(); + /** \brief eliminates vertices which are (\a epsilon) - colinear + * with their respective neighbors + * + * \author Karl J. Obermeyer + * \post the Euclidean distance between each vertex and the line + * segment connecting its neighbors is at least \a epsilon, and the + * Polygon is in standard form + * \remarks time complexity O(n), where n is the number of + * vertices representing the the Polygon + */ + void eliminate_redundant_vertices(double epsilon=0.0); + /** \brief reverse (cyclic) order of vertices + * + * \remarks vertex first in list is held first + */ + void reverse(); + protected: + std::vector vertices_; + }; + + + /** \brief true iff vertex lists are identical, but false if either + * Polygon has size 0 + * + * \remarks returns false if either Polygon has size 0 + * \remarks O(n) time complexity + */ + bool operator == (Polygon polygon1, Polygon polygon2); + bool operator != (Polygon polygon1, Polygon polygon2); + /** \brief true iff the Polygon's vertices match up w/in a (closed) + * epsilon ball of each other, but false if either Polygon + * has size 0 + * + * Respects number, ordering, and orientation of vertices, i.e., + * even if the (conceptual) polygons represented by two Polygons are + * identical, they are not considered \a epsilon - equivalent unless + * the number of vertices is the same, the orientations are the same + * (cw vs. ccw list), and the Points of the vertex lists match up + * within epsilon. This function does attempt to match the polygons + * for all possible cyclic permutations, hence the quadratic time + * complexity. + * \author Karl J. Obermeyer + * \remarks O(n^2) time complexity, where n is the number of + * vertices representing the polygon + */ + bool equivalent(Polygon polygon1, Polygon polygon2, + double epsilon=0.0); + + + /** \brief Euclidean distance between Polygons' boundaries + * + * \author Karl J. Obermeyer + * \pre \a polygon1 and \a polygon2 each have greater than 0 vertices + */ + double boundary_distance( const Polygon& polygon1, + const Polygon& polygon2 ); + + + //print Polygon + std::ostream& operator << (std::ostream& outs, + const Polygon& polygon_temp); + + + /** \brief environment represented by simple polygonal outer boundary + * with simple polygonal holes + * + * \remarks For methods to work correctly, the outer boundary vertices must + * be listed ccw and the hole vertices cw + */ + class Environment + { + public: + friend class Point; + //Constructors + /// default to empty + Environment() { } + /** \brief construct Environment without holes + * + * \remarks time complexity O(n), where n is the number of vertices + * representing the Environment + */ + Environment(const Polygon& polygon_temp) + { outer_boundary_=polygon_temp; update_flattened_index_key(); } + /** \brief construct Environment with holes from STL vector of Polygons + * + * the first Polygon in the vector becomes the outer boundary, + * the rest become the holes + * \remarks time complexity O(n), where n is the number of vertices + * representing the Environment + */ + Environment(const std::vector& polygons); + /** construct from *.environment file. + * + * \author Karl J. Obermeyer + * \remarks time complexity O(n), where n is the number of vertices + * representing the Environment + */ + Environment(const std::string& filename); + //Accessors + /** \brief raw access to Polygons + * + * An argument of 0 accesses the outer boundary, 1 and above + * access the holes. + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + const Polygon& operator [] (unsigned i) const + { if(i==0){return outer_boundary_;} else{return holes_[i-1];} } + /** \brief raw access to vertices via flattened index + * + * By flattened index is intended the label given to a vertex if + * you were to label all the vertices from 0 to n-1 (where n is + * the number of vertices representing the Environment) starting + * with the first vertex of the outer boundary and progressing in + * order through all the remaining vertices of the outer boundary + * and holes. + * + * \remarks Time complexity O(1). For efficiency, no bounds + * check; usually trying to access out of bounds causes a bus + * error. + */ + const Point& operator () (unsigned k) const; + /// hole count + unsigned h() const { return holes_.size(); } + /** \brief vertex count + * + * \remarks time complexity O(h) + */ + unsigned n() const; + /** \brief total reflex vertex count (nonconvex vertices) + * + * \author Karl J. Obermeyer + * \remarks time complexity O(n), where n is the number of + * vertices representing the Environment + */ + unsigned r() const; + /** \brief true iff lexicographically smallest vertex is first in + * each list of vertices representing a Polygon of the + * Environment + * + * \author Karl J. Obermeyer + * \remarks lex. comparison is very sensitive to perturbations if + * two Points nearly define a line parallel to one of the axes + */ + bool is_in_standard_form() const; + /** \brief true iff \a epsilon -valid + * + * \a epsilon -valid means (i) the outer boundary and holes are + * pairwise \a epsilon -disjoint (no two features should come + * within \a epsilon of each other) simple polygons, (ii) outer + * boundary is oriented ccw, and (iii) holes are oriented cw. + * + * \author Karl J. Obermeyer + * + * \pre Environment has greater than 2 vertices + * (otherwise it can't even have nonzero area) + * \remarks time complexity O(h^2*n^2), where h is the number of + * holes and n is the number of vertices representing the + * Environment + */ + bool is_valid(double epsilon=0.0) const; + /** \brief sum of perimeter lengths of outer boundary and holes + * + * \author Karl J. Obermeyer + * \remarks O(n) time complexity, where n is the number of + * vertices representing the Environment + */ + double boundary_length() const; + /** \brief (obstacle/hole free) area of the Environment + * + * \author Karl J. Obermeyer + * \remarks O(n) time complexity, where n is the number of + * vertices representing the Environment + */ + double area() const; + /** \brief Euclidean diameter + * + * \author Karl J. Obermeyer + * \pre Environment has greater than 0 vertices + * \return maximum Euclidean distance between all pairs of + * vertices + * \remarks time complexity O(n^2), where n is the number of + * vertices representing the Environment + */ + double diameter() const { return outer_boundary_.diameter(); } + /** \brief box which fits snugly around the Environment + * + * \author Karl J. Obermeyer + * \pre Environment has greater than 0 vertices + */ + Bounding_Box bbox() const { return outer_boundary_.bbox(); } + /** \brief get STL vector of \a count Points randomly situated + * within \a epsilon of the Environment + * + * \author Karl J. Obermeyer + * \pre the Environment has positive area + */ + std::vector random_points(const unsigned& count, + double epsilon=0.0) const; + /** \brief compute a shortest path between 2 Points + * + * Uses the classical visibility graph method as described, e.g., + * in ``Robot Motion Planning" (Ch. 4 Sec. 1) by J.C. Latombe. + * Specifically, an A* search is performed on the visibility graph + * using the Euclidean distance as the heuristic function. + * + * \author Karl J. Obermeyer + * + * \pre \a start and \a finish must be in the environment. + * Environment must be \a epsilon -valid. Test with + * Environment::is_valid(epsilon). + * + * \remarks If multiple shortest path queries are made for the + * same Envrionment, it is better to precompute the + * Visibility_Graph. For a precomputed Visibility_Graph, the time + * complexity of a shortest_path() query is O(n^2), where n is the + * number of vertices representing the Environment. + * + * \todo return not just one, but all shortest paths (w/in + * epsilon), e.g., returning a std::vector) + */ + Polyline shortest_path(const Point& start, + const Point& finish, + const Visibility_Graph& visibility_graph, + double epsilon=0.0); + /** \brief compute shortest path between 2 Points + * + * \author Karl J. Obermeyer + * + * \pre \a start and \a finish must be in the environment. + * Environment must be \a epsilon -valid. Test with + * Environment::is_valid(epsilon). + * + * \remarks For single shortest path query, visibility graph is + * not precomputed. Time complexity O(n^3), where n is the number + * of vertices representing the Environment. + */ + Polyline shortest_path(const Point& start, + const Point& finish, + double epsilon=0.0); + /** \brief compute the faces (partition cells) of an arrangement + * of Line_Segments inside the Environment + * + * \author Karl J. Obermeyer + * \todo finish this + */ + std::vector compute_partition_cells( std::vector + partition_inducing_segments, + double epsilon=0.0 ) + { + std::vector cells; + return cells; + } + /** \brief write lists of vertices to *.environment file + * + * uses intuitive human and computer readable decimal format with + * display precision \a fios_precision_temp + * \author Karl J. Obermeyer + * \pre \a fios_precision_temp >=1 + */ + void write_to_file(const std::string& filename, + int fios_precision_temp=FIOS_PRECISION); + //Mutators + /** \brief raw access to Polygons + * + * An argument of 0 accesses the outer boundary, 1 and above + * access the holes. + * \author Karl J. Obermeyer + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + Polygon& operator [] (unsigned i) + { if(i==0){return outer_boundary_;} else{return holes_[i-1];} } + //Mutators + /** \brief raw access to vertices via flattened index + * + * By flattened index is intended the label given to a vertex if + * you were to label all the vertices from 0 to n-1 (where n is + * the number of vertices representing the Environment) starting + * with the first vertex of the outer boundary and progressing in + * order through all the remaining vertices of the outer boundary + * and holes. + * \author Karl J. Obermeyer + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error. + */ + Point& operator () (unsigned k); + /// set outer boundary + void set_outer_boundary(const Polygon& polygon_temp) + { outer_boundary_ = polygon_temp; update_flattened_index_key(); } + /// add hole + void add_hole(const Polygon& polygon_temp) + { holes_.push_back(polygon_temp); update_flattened_index_key(); } + /** \brief enforces outer boundary vertices are listed ccw and + * holes listed cw, and that these lists begin with respective + * lexicographically smallest vertex + * + * \author Karl J. Obermeyer + * \remarks O(n) time complexity, where n is the number of + * vertices representing the Environment. Lex. comparison is very + * sensitive to perturbations if two Points nearly define a line + * parallel to one of the axes. + */ + void enforce_standard_form(); + /** \brief eliminates vertices which are (\a epsilon) - colinear + * with their respective neighbors + * + * \author Karl J. Obermeyer + * \post the Euclidean distance between each vertex and the line + * segment connecting its neighbors is at least \a epsilon + * \remarks time complexity O(n), where n is the number of + * vertices representing the the Environment + */ + void eliminate_redundant_vertices(double epsilon=0.0); + /** \brief reverse (cyclic) order of vertices belonging to holes + * only + * + * \remarks vertex first in each hole's list is held first + */ + void reverse_holes(); + private: + Polygon outer_boundary_; + //obstacles + std::vector holes_; + //allows constant time access to vertices via operator () with + //flattened index as argument + std::vector< std::pair > flattened_index_key_; + //Must call if size of outer_boundary and/or holes_ changes. Time + //complexity O(n), where n is the number of vertices representing + //the Environment. + void update_flattened_index_key(); + //converts flattened index to index pair (hole #, vertex #) in + //time O(n), where n is the number of vertices representing the + //Environment + std::pair one_to_two(unsigned k) const; + //node used for search tree of A* search in shortest_path() method + class Shortest_Path_Node + { + public: + //flattened index of corresponding Environment vertex + //convention vertex_index = n() => corresponds to start Point + //vertex_index = n() + 1 => corresponds to finish Point + unsigned vertex_index; + //pointer to self in search tree. + std::list::iterator search_tree_location; + //pointer to parent in search tree. + std::list::iterator parent_search_tree_location; + //Geodesic distance from start Point. + double cost_to_come; + //Euclidean distance to finish Point. + double estimated_cost_to_go; + //std::vector expand(); + bool operator < (const Shortest_Path_Node& spn2) const + { + double f1 = this->cost_to_come + this->estimated_cost_to_go; + double f2 = spn2.cost_to_come + spn2.estimated_cost_to_go; + if( f1 < f2 ) + return true; + else if( f2 < f1 ) + return false; + else if( this->vertex_index < spn2.vertex_index ) + return true; + else if( this->vertex_index > spn2.vertex_index ) + return false; + else if( &(*(this->parent_search_tree_location)) + < &(*(spn2.parent_search_tree_location)) ) + return true; + else + return false; + } + // print member data for debugging + void print() const + { + std::cout << " vertex_index = " << vertex_index << std::endl + << "parent's vertex_index = " + << parent_search_tree_location->vertex_index + << std::endl + << " cost_to_come = " << cost_to_come << std::endl + << " estimated_cost_to_go = " + << estimated_cost_to_go << std::endl; + } + }; + }; + + + /// printing Environment + std::ostream& operator << (std::ostream& outs, + const Environment& environment_temp); + + + /** \brief set of Guards represented by a list of Points + */ + class Guards + { + public: + friend class Visibility_Graph; + //Constructors + /// default to empty + Guards() { } + /** \brief construct from *.guards file + * + * \author Karl J. Obermeyer + */ + Guards(const std::string& filename); + /// construct from STL vector of Points + Guards(const std::vector& positions) + { positions_ = positions; } + //Accessors + /** \brief raw access to guard position Points + * + * \author Karl J. Obermeyer + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + const Point& operator [] (unsigned i) const { return positions_[i]; } + /// guard count + unsigned N() const { return positions_.size(); } + /// true iff positions are lexicographically ordered + bool are_lex_ordered() const; + /// true iff no two guards are w/in epsilon of each other + bool noncolocated(double epsilon=0.0) const; + /// true iff all guards are located in \a polygon_temp + bool in(const Polygon& polygon_temp, double epsilon=0.0) const; + /// true iff all guards are located in \a environment_temp + bool in(const Environment& environment_temp, double epsilon=0.0) const; + /** \brief Euclidean diameter + * + * \author Karl J. Obermeyer + * \pre greater than 0 guards + * \return maximum Euclidean distance between all pairs of + * vertices + * \remarks time complexity O(N^2), where N is the number of + * guards + */ + double diameter() const; + /** \brief box which fits snugly around the Guards + * + * \author Karl J. Obermeyer + * \pre greater than 0 guards + */ + Bounding_Box bbox() const; + /** \brief write list of positions to *.guards file + * + * Uses intuitive human and computer readable decimal format with + * display precision \a fios_precision_temp. + * \author Karl J. Obermeyer + * \pre \a fios_precision_temp >=1 + */ + void write_to_file(const std::string& filename, + int fios_precision_temp=FIOS_PRECISION); + //Mutators + /** \brief raw access to guard position Points + * + * \author Karl J. Obermeyer + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + Point& operator [] (unsigned i) { return positions_[i]; } + /// add a guard + void push_back(const Point& point_temp) + { positions_.push_back(point_temp); } + /// set positions with STL vector of Points + void set_positions(const std::vector& positions_temp) + { positions_ = positions_temp; } + /** \brief sort positions in lexicographic order + * + * from (lowest x, then lowest y) to (highest x, then highest y) + * \author Karl J. Obermeyer + * \remarks time complexity O(N logN), where N is the guard count. + * Lex. comparison is very sensitive to perturbations if two + * Points nearly define a line parallel to one of the axes. + */ + void enforce_lex_order(); + /// reverse order of positions + void reverse(); + /** \brief relocate each guard to closest vertex if within + * \a epsilon of some vertex (of \a environment_temp) + * + * \author Karl J. Obermeyer + * \pre the guards' position data are numbers and \a environment_temp + * is nonempty + * \post if a guard was a Euclidean distance no greater + * than \a epsilon from any vertex of \a environment_temp, then it + * will be repositioned to coincide with the closest such vertex + * \remarks O(N*n) time complexity, where N is the guard count + * and n is the number of vertices in \a environment_temp. + */ + void snap_to_vertices_of(const Environment& environment_temp, + double epsilon=0.0); + + /** \brief relocate each guard to closest vertex if within + * \a epsilon of some vertex (of \a environment_temp) + * + * \author Karl J. Obermeyer + * \pre the guards' position data are numbers and \a polygon_temp + * is nonempty + * \post if a guard was a Euclidean distance no greater + * than \a epsilon from any vertex of \a polygon_temp, then it + * will be repositioned to coincide with the closest such vertex + * \remarks O(N*n) time complexity, where N is the guard count + * and n is the number of vertices in \a polygon_temp + */ + void snap_to_vertices_of(const Polygon& polygon_temp, + double epsilon=0.0); + /** \brief relocate each guard to closest Point on boundary if + * within \a epsilon of the boundary (of \a environment_temp) + * + * \author Karl J. Obermeyer + * \pre the guards' position data are numbers and \a environment_temp + * is nonempty + * \post If the calling Point was a Euclidean distance no greater + * than \a epsilon from the boundary of \a environment_temp, then it + * will be repositioned to it's projection onto that boundary + * \remarks O(N*n) time complexity, where N is the guard count and + * n is the number of vertices in \a environment_temp + */ + void snap_to_boundary_of(const Environment& environment_temp, + double epsilon=0.0); + /** \brief relocate each guard to closest Point on boundary if + * within \a epsilon of the boundary (of \a polygon_temp) + * + * \author Karl J. Obermeyer + * \pre the guards' position data are numbers and \a polygon_temp + * is nonempty + * \post If the calling Point was a Euclidean distance no greater + * than \a epsilon from the boundary of \a polygon_temp, then it + * will be repositioned to it's projection onto that boundary + * \remarks O(N*n) time complexity, where N is the guard count and + * n is the number of vertices in \a polygon_temp + */ + void snap_to_boundary_of(const Polygon& polygon_temp, + double epsilon=0.0); + private: + std::vector positions_; + }; + + + /// print Guards + std::ostream& operator << (std::ostream& outs, + const Guards& guards); + + + /** \brief visibility polygon of a Point in an Environment or Polygon + * + * A Visibility_Polygon represents the closure of the set of all + * points in an environment which are {\it clearly visible} from a + * point (the observer). Two Points p1 and p2 are (mutually) {\it + * clearly visible} in an environment iff the relative interior of + * the line segment connecting p1 and p2 does not intersect the + * boundary of the environment. + * + * \remarks average case time complexity O(n log(n)), where n is the + * number of vertices in the Evironment (resp. Polygon). Note the + * Standard Library's sort() function performs O(n log(n)) + * comparisons (both average and worst-case) and the sort() member + * function of an STL list performs "approximately O(n log(n)) + * comparisons". For robustness, any Point (observer) should be \a + * epsilon -snapped to the environment boundary and vertices before + * computing its Visibility_Polygon (use the Point methods + * snap_to_vertices_of(...) and snap_to_boundary_of(...) ). + */ + class Visibility_Polygon : public Polygon + { + public: + //Constructors + /// default to empty + Visibility_Polygon() { } + //:TRICKY: + /** \brief visibility set of a Point in an Environment + * + * \author Karl J. Obermeyer + * + * \pre \a observer is in \a environment_temp (w/in \a epsilon ) + * and has been epsilon-snapped to the Environment using the + * method Point::snap_to_boundary_of() followed by (order is + * important) Point::snap_to_vertices_of(). \a environment_temp + * must be \a epsilon -valid. Test with + * Environment::is_valid(epsilon). + * + * \remarks O(n log(n)) average case time complexity, where n is the + * number of vertices in the Evironment (resp. Polygon). + */ + Visibility_Polygon(const Point& observer, + const Environment& environment_temp, + double epsilon=0.0); + /** \brief visibility set of a Point in a Polygon + * + * \pre \a observer is in \a polygon_temp (w/in \a epsilon ) and + * has been epsilon-snapped to the Polygon using the methods + * Point::snap_to_vertices_of() and Point::snap_to_boundary_of(). + * \a environment_temp must be \a epsilon -valid. Test with + * Environment::is_valid(epsilon). + * + * \remarks less efficient because constructs an Environment from + * a Polygon and then calls the other Visibility_Polygon constructor. + * O(n log(n)) average case time complexity, where n is the + * number of vertices in the Evironment (resp. Polygon). + */ + Visibility_Polygon(const Point& observer, + const Polygon& polygon_temp, + double epsilon=0.0); + //Accessors + //std::vector get_gap_edges(double epsilon=0.0) { return gap_edges_; } + /// location of observer which induced the visibility polygon + Point observer() const + { return observer_; } + //Mutators + private: + //ith entry of gap_edges is true iff the edge following ith vertex + //is a gap edge (not solid). + //std::vector gap_edges_; + Point observer_; + + struct Polar_Edge + { + Polar_Point first; + Polar_Point second; + Polar_Edge() { } + Polar_Edge(const Polar_Point& ppoint1, + const Polar_Point& ppoint2) : + first(ppoint1), second(ppoint2) {} + }; + + class Polar_Point_With_Edge_Info : public Polar_Point + { + public: + std::list::iterator incident_edge; + bool is_first; //True iff polar_point is the first_point of the + //Polar_Edge pointed to by + //incident_edge. + void set_polar_point(const Polar_Point& ppoint_temp) + { + set_polar_origin( ppoint_temp.polar_origin() ); + set_x( ppoint_temp.x() ); + set_y( ppoint_temp.y() ); + set_range( ppoint_temp.range() ); + set_bearing( ppoint_temp.bearing() ); + } + //The operator < is the same as for Polar_Point with one + //exception. If two vertices have equal coordinates, but one is + //the first point of its respecitve edge and the other is the + //second point of its respective edge, then the vertex which is + //the second point of its respective edge is considered + //lexicographically smaller. + friend bool operator < (const Polar_Point_With_Edge_Info& ppwei1, + const Polar_Point_With_Edge_Info& ppwei2) + { + if( Polar_Point(ppwei1) == Polar_Point(ppwei2) + and !ppwei1.is_first and ppwei2.is_first ) + return true; + else + return Polar_Point(ppwei1) < Polar_Point(ppwei2); + } + }; + + //Strict weak ordering (acts like <) for pointers to Polar_Edges. + //Used to sort the priority_queue q2 used in the radial line sweep + //of Visibility_Polygon constructors. Let p1 be a pointer to + //Polar_Edge e1 and p2 be a pointer to Polar_Edge e2. Then p1 is + //considered greater (higher priority) than p2 if the distance + //from the observer (pointed to by observer_pointer) to e1 along + //the direction to current_vertex is smaller than the distance + //from the observer to e2 along the direction to current_vertex. + class Incident_Edge_Compare + { + const Point *const observer_pointer; + const Polar_Point_With_Edge_Info *const current_vertex_pointer; + double epsilon; + public: + Incident_Edge_Compare(const Point& observer, + const Polar_Point_With_Edge_Info& current_vertex, + double epsilon_temp) : + observer_pointer(&observer), + current_vertex_pointer(¤t_vertex), + epsilon(epsilon_temp) { } + bool operator () (std::list::iterator e1, + std::list::iterator e2) const + { + Polar_Point k1, k2; + Line_Segment xing1 = intersection( Ray(*observer_pointer, + current_vertex_pointer->bearing()), + Line_Segment(e1->first, + e1->second), + epsilon); + Line_Segment xing2 = intersection( Ray(*observer_pointer, + current_vertex_pointer->bearing()), + Line_Segment(e2->first, + e2->second), + epsilon); + if( xing1.size() > 0 and xing2.size() > 0 ){ + k1 = Polar_Point( *observer_pointer, + xing1.first() ); + k2 = Polar_Point( *observer_pointer, + xing2.first() ); + if( k1.range() <= k2.range() ) + return false; + return true; + } + //Otherwise infeasible edges are given higher priority, so they + //get pushed out the top of the priority_queue's (q2's) + //heap. + else if( xing1.size() == 0 and xing2.size() > 0 ) + return false; + else if( xing1.size() > 0 and xing2.size() == 0 ) + return true; + else + return true; + } + }; + + bool is_spike( const Point& observer, + const Point& point1, + const Point& point2, + const Point& point3, + double epsilon=0.0 ) const; + + //For eliminating spikes as they appear. In the + //Visibility_Polygon constructors, these are checked every time a + //Point is added to vertices. + void chop_spikes_at_back(const Point& observer, + double epsilon); + void chop_spikes_at_wrap_around(const Point& observer, + double epsilon); + void chop_spikes(const Point& observer, + double epsilon); + //For debugging Visibility_Polygon constructors. + //Prints current_vertex and active_edge data to screen. + void print_cv_and_ae(const Polar_Point_With_Edge_Info& current_vertex, + const std::list::iterator& + active_edge); + }; + + + /** \brief visibility graph of points in an Environment, + * represented by adjacency matrix + * + * \remarks used for shortest path planning in the + * Environment::shortest_path() method + * + * \todo Add method to prune edges for faster shortest path + * calculation, e.g., exclude concave vertices and only include + * tangent edges as described in ``Robot Motion Planning" (Ch. 4 + * Sec. 1) by J.C. Latombe. + */ + class Visibility_Graph + { + public: + //Constructors + /// default to empty + Visibility_Graph() { n_=0; adjacency_matrix_ = NULL; } + /// copy + Visibility_Graph( const Visibility_Graph& vg2 ); + /** \brief construct the visibility graph of Environment vertices + * + * \author Karl J. Obermeyer + * + * \pre \a environment must be \a epsilon -valid. Test with + * Environment::is_valid(epsilon). + * + * \remarks Currently this constructor simply computes the + * Visibility_Polygon of each vertex and checks inclusion of the + * other vertices, taking time complexity O(n^3), where n is the + * number of vertices representing the Environment. This time + * complexity is not optimal. As mentioned in ``Robot Motion + * Planning" by J.C. Latombe p.157, there are algorithms able to + * construct a visibility graph for a polygonal environment with + * holes in time O(n^2). The nonoptimal algorithm is being used + * temporarily because of (1) its ease to implement using the + * Visibility_Polygon class, and (2) its apparent robustness. + * Implementing the optimal algorithm robustly is future work. + */ + Visibility_Graph(const Environment& environment, double epsilon=0.0); + //Constructors + /** \brief construct the visibility graph of Points in an Environment + * + * \pre \a environment must be \a epsilon -valid. Test with + * Environment::is_valid(epsilon). + * + * \author Karl J. Obermeyer \remarks Currently this constructor + * simply computes the Visibility_Polygon of each Point and checks + * inclusion of the other Points, taking time complexity + * O(N n log(n) + N^2 n), where N is the number of Points and n is + * the number of vertices representing the Environment. This time + * complexity is not optimal, but has been used for + * simplicity. More efficient algorithms are discussed in ``Robot + * Motion Planning" by J.C. Latombe p.157. + */ + Visibility_Graph(const std::vector points, + const Environment& environment, double epsilon=0.0); + //Constructors + /** \brief construct the visibility graph of Guards in an Environment + * + * \pre \a start and \a finish must be in the environment. + * Environment must be \a epsilon -valid. Test with + * Environment::is_valid(epsilon). + * + * \author Karl J. Obermeyer + * \remarks Currently this constructor simply computes the + * Visibility_Polygon of each guard and checks inclusion of the + * other guards, taking time complexity O(N n log(n) + N^2 n), + * where N is the number of guards and n is the number of vertices + * representing the Environment. This time complexity is not + * optimal, but has been used for simplicity. More efficient + * algorithms are discussed in ``Robot Motion Planning" by + * J.C. Latombe p.157. + */ + Visibility_Graph(const Guards& guards, + const Environment& environment, double epsilon=0.0); + //Accessors + /** \brief raw access to adjacency matrix data + * + * \author Karl J. Obermeyer + * \param i1 Polygon index of first vertex + * \param j1 index of first vertex within its Polygon + * \param i2 Polygon index of second vertex + * \param j2 index of second vertex within its Polygon + * \return true iff first vertex is visible from second vertex + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + bool operator () (unsigned i1, + unsigned j1, + unsigned i2, + unsigned j2) const; + /** \brief raw access to adjacency matrix data via flattened + * indices + * + * By flattened index is intended the label given to a vertex if + * you were to label all the vertices from 0 to n-1 (where n is + * the number of vertices representing the Environment) starting + * with the first vertex of the outer boundary and progressing in + * order through all the remaining vertices of the outer boundary + * and holes. + * \author Karl J. Obermeyer + * \param k1 flattened index of first vertex + * \param k1 flattened index of second vertex + * \return true iff first vertex is visible from second vertex + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + bool operator () (unsigned k1, + unsigned k2) const; + /// \brief total number of vertices in corresponding Environment + unsigned n() const { return n_; } + //Mutators + /** \brief raw access to adjacency matrix data + * + * \author Karl J. Obermeyer + * \param i1 Polygon index of first vertex + * \param j1 index of first vertex within its Polygon + * \param i2 Polygon index of second vertex + * \param j2 index of second vertex within its Polygon + * \return true iff first vertex is visible from second vertex + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + bool& operator () (unsigned i1, + unsigned j1, + unsigned i2, + unsigned j2); + /** \brief raw access to adjacency matrix data via flattened + * indices + * + * By flattened index is intended the label given to a vertex if + * you were to label all the vertices from 0 to n-1 (where n is + * the number of vertices representing the Environment) starting + * with the first vertex of the outer boundary and progressing in + * order through all the remaining vertices of the outer boundary + * and holes. + * \author Karl J. Obermeyer + * \param k1 flattened index of first vertex + * \param k1 flattened index of second vertex + * \return true iff first vertex is visible from second vertex + * \remarks for efficiency, no bounds check; usually trying to + * access out of bounds causes a bus error + */ + bool& operator () (unsigned k1, + unsigned k2); + /// assignment operator + Visibility_Graph& operator = + (const Visibility_Graph& visibility_graph_temp); + /// destructor + virtual ~Visibility_Graph(); + private: + //total number of vertices of corresponding Environment + unsigned n_; + //the number of vertices in each Polygon of corresponding Environment + std::vector vertex_counts_; + // n_-by-n_ adjacency matrix data stored as 2D dynamic array + bool **adjacency_matrix_; + //converts vertex pairs (hole #, vertex #) to flattened index + unsigned two_to_one(unsigned i, + unsigned j) const; + }; + + + /// print Visibility_Graph adjacency matrix + std::ostream& operator << (std::ostream& outs, + const Visibility_Graph& visibility_graph); + +} + +#endif //VISILIBITY_H diff --git a/xs/t/18_motionplanner.t b/xs/t/18_motionplanner.t new file mode 100644 index 000000000..2a4af8991 --- /dev/null +++ b/xs/t/18_motionplanner.t @@ -0,0 +1,89 @@ +#!/usr/bin/perl + +use strict; +use warnings; + +use Slic3r::XS; +use Test::More tests => 17; + +my $square = Slic3r::Polygon->new( # ccw + [100, 100], + [200, 100], + [200, 200], + [100, 200], +); +my $hole_in_square = Slic3r::Polygon->new( # cw + [140, 140], + [140, 160], + [160, 160], + [160, 140], +); +$_->scale(1/0.000001) for $square, $hole_in_square; + +my $expolygon = Slic3r::ExPolygon->new($square, $hole_in_square); + +{ + my $mp = Slic3r::MotionPlanner->new([ $expolygon ]); + isa_ok $mp, 'Slic3r::MotionPlanner'; + + my $from = Slic3r::Point->new(120, 120); + my $to = Slic3r::Point->new(180,180); + $_->scale(1/0.000001) for $from, $to; + my $path = $mp->shortest_path($from, $to); + ok $path->is_valid(), 'return path is valid'; + ok $path->length > Slic3r::Line->new($from, $to)->length, 'path length is greater than straight line'; + ok $path->first_point->coincides_with($from), 'first path point coincides with initial point'; + ok $path->last_point->coincides_with($to), 'last path point coincides with destination point'; + ok $expolygon->contains_polyline($path), 'path is fully contained in expolygon'; +} + +{ + my $mp = Slic3r::MotionPlanner->new([ $expolygon ]); + isa_ok $mp, 'Slic3r::MotionPlanner'; + + my $from = Slic3r::Point->new(80, 100); + my $to = Slic3r::Point->new(220,200); + $_->scale(1/0.000001) for $from, $to; + + my $path = $mp->shortest_path($from, $to); + ok $path->is_valid(), 'return path is valid'; + ok $path->length > Slic3r::Line->new($from, $to)->length, 'path length is greater than straight line'; + ok $path->first_point->coincides_with($from), 'first path point coincides with initial point'; + ok $path->last_point->coincides_with($to), 'last path point coincides with destination point'; + is scalar(@{ Slic3r::Geometry::Clipper::intersection_pl([$path], [@$expolygon]) }), 0, 'path has no intersection with expolygon'; +} + +{ + my $expolygon2 = $expolygon->clone; + $expolygon2->translate(300/0.000001, 0); + my $mp = Slic3r::MotionPlanner->new([ $expolygon, $expolygon2 ]); + isa_ok $mp, 'Slic3r::MotionPlanner'; + + my $from = Slic3r::Point->new(120, 120); + my $to = Slic3r::Point->new(120 + 300, 120); + $_->scale(1/0.000001) for $from, $to; + ok $expolygon->contains_point($from), 'start point is contained in first expolygon'; + ok $expolygon2->contains_point($to), 'end point is contained in second expolygon'; + my $path = $mp->shortest_path($from, $to); + ok $path->is_valid(), 'return path is valid'; + ok $path->length > Slic3r::Line->new($from, $to)->length, 'path length is greater than straight line'; +} + +{ + my $expolygons = [ + Slic3r::ExPolygon->new([[123800962,89330311],[123959159,89699438],[124000004,89898430],[124000012,110116427],[123946510,110343065],[123767391,110701303],[123284087,111000001],[102585791,111000009],[102000004,110414223],[102000004,89585787],[102585790,89000000],[123300022,88999993]]), + Slic3r::ExPolygon->new([[97800954,89330311],[97959151,89699438],[97999996,89898430],[98000004,110116427],[97946502,110343065],[97767383,110701303],[97284079,111000001],[76585783,111000009],[75999996,110414223],[75999996,89585787],[76585782,89000000],[97300014,88999993]]), + ]; + my $mp = Slic3r::MotionPlanner->new($expolygons); + isa_ok $mp, 'Slic3r::MotionPlanner'; + + my $from = Slic3r::Point->new(79120520, 107839491); + my $to = Slic3r::Point->new(104664164, 108335852); + ok $expolygons->[1]->contains_point($from), 'start point is contained in second expolygon'; + ok $expolygons->[0]->contains_point($to), 'end point is contained in first expolygon'; + my $path = $mp->shortest_path($from, $to); + ok $path->is_valid(), 'return path is valid'; + ok $path->length > Slic3r::Line->new($from, $to)->length, 'path length is greater than straight line'; +} + +__END__ diff --git a/xs/xsp/ExPolygon.xsp b/xs/xsp/ExPolygon.xsp index df0f203d3..aefbd1aed 100644 --- a/xs/xsp/ExPolygon.xsp +++ b/xs/xsp/ExPolygon.xsp @@ -23,6 +23,8 @@ bool is_valid(); bool contains_line(Line* line) %code{% RETVAL = THIS->contains_line(*line); %}; + bool contains_polyline(Polyline* polyline) + %code{% RETVAL = THIS->contains_polyline(*polyline); %}; bool contains_point(Point* point) %code{% RETVAL = THIS->contains_point(*point); %}; ExPolygons simplify(double tolerance); diff --git a/xs/xsp/MotionPlanner.xsp b/xs/xsp/MotionPlanner.xsp new file mode 100644 index 000000000..57abef937 --- /dev/null +++ b/xs/xsp/MotionPlanner.xsp @@ -0,0 +1,14 @@ +%module{Slic3r::XS}; + +%{ +#include +#include "MotionPlanner.hpp" +%} + +%name{Slic3r::MotionPlanner} class MotionPlanner { + MotionPlanner(ExPolygons islands); + ~MotionPlanner(); + + Polyline* shortest_path(Point* from, Point* to) + %code%{ RETVAL = new Polyline(); THIS->shortest_path(*from, *to, RETVAL); %}; +}; diff --git a/xs/xsp/my.map b/xs/xsp/my.map index b122d95d7..46c3e271b 100644 --- a/xs/xsp/my.map +++ b/xs/xsp/my.map @@ -110,6 +110,9 @@ ModelInstance* O_OBJECT_SLIC3R Ref O_OBJECT_SLIC3R_T Clone O_OBJECT_SLIC3R_T +MotionPlanner* O_OBJECT_SLIC3R +Ref O_OBJECT_SLIC3R_T +Clone O_OBJECT_SLIC3R_T ExtrusionRole T_UV FlowRole T_UV diff --git a/xs/xsp/typemap.xspt b/xs/xsp/typemap.xspt index 945b104bf..1b060bb32 100644 --- a/xs/xsp/typemap.xspt +++ b/xs/xsp/typemap.xspt @@ -67,6 +67,9 @@ %typemap{PolylineCollection*}; %typemap{Ref}{simple}; %typemap{Clone}{simple}; +%typemap{MotionPlanner*}; +%typemap{Ref}{simple}; +%typemap{Clone}{simple}; %typemap{Points}; %typemap{Pointfs}; From 65dd95aaacd73ce005e547857430add5a3482908 Mon Sep 17 00:00:00 2001 From: Alessandro Ranellucci Date: Tue, 27 May 2014 00:03:23 +0200 Subject: [PATCH 2/5] Update MANIFEST and test --- xs/MANIFEST | 2 ++ xs/t/18_motionplanner.t | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/xs/MANIFEST b/xs/MANIFEST index 184998f8e..f9ae27d70 100644 --- a/xs/MANIFEST +++ b/xs/MANIFEST @@ -1736,6 +1736,7 @@ t/14_geometry.t t/15_config.t t/16_flow.t t/17_boundingbox.t +t/18_motionplanner.t xsp/BoundingBox.xsp xsp/Clipper.xsp xsp/Config.xsp @@ -1749,6 +1750,7 @@ xsp/Flow.xsp xsp/Geometry.xsp xsp/Line.xsp xsp/Model.xsp +xsp/MotionPlanner.xsp xsp/my.map xsp/mytype.map xsp/Point.xsp diff --git a/xs/t/18_motionplanner.t b/xs/t/18_motionplanner.t index 2a4af8991..c7bf78f88 100644 --- a/xs/t/18_motionplanner.t +++ b/xs/t/18_motionplanner.t @@ -4,7 +4,7 @@ use strict; use warnings; use Slic3r::XS; -use Test::More tests => 17; +use Test::More tests => 22; my $square = Slic3r::Polygon->new( # ccw [100, 100], From 07b915fb4fd7c2bca7b2554f62c9834c7118019a Mon Sep 17 00:00:00 2001 From: Alessandro Ranellucci Date: Tue, 27 May 2014 00:05:45 +0200 Subject: [PATCH 3/5] Fix MANIFEST --- xs/MANIFEST | 46 ---------------------------------------------- 1 file changed, 46 deletions(-) diff --git a/xs/MANIFEST b/xs/MANIFEST index 1ec17eaff..dc868f678 100644 --- a/xs/MANIFEST +++ b/xs/MANIFEST @@ -1,26 +1,13 @@ -<<<<<<< HEAD -======= ->>>>>>> master Build.PL -buildtmp/main.xs -buildtmp/typemap -buildtmp/XS.c -buildtmp/XS.o lib/Slic3r/XS.pm MANIFEST This list of files src/admesh/connect.c -src/admesh/connect.o src/admesh/normals.c -src/admesh/normals.o src/admesh/shared.c -src/admesh/shared.o src/admesh/stl.h src/admesh/stl_io.c -src/admesh/stl_io.o src/admesh/stlinit.c -src/admesh/stlinit.o src/admesh/util.c -src/admesh/util.o src/boost/aligned_storage.hpp src/boost/array.hpp src/boost/assert.hpp @@ -1663,109 +1650,76 @@ src/boost/version.hpp src/boost/visit_each.hpp src/BoundingBox.cpp src/BoundingBox.hpp -src/BoundingBox.o src/clipper.cpp src/clipper.hpp -src/clipper.o src/ClipperUtils.cpp src/ClipperUtils.hpp -src/ClipperUtils.o src/Config.cpp src/Config.hpp -src/Config.o src/ExPolygon.cpp src/ExPolygon.hpp -src/ExPolygon.o src/ExPolygonCollection.cpp src/ExPolygonCollection.hpp -src/ExPolygonCollection.o src/Extruder.cpp src/Extruder.hpp -src/Extruder.o src/ExtrusionEntity.cpp src/ExtrusionEntity.hpp -src/ExtrusionEntity.o src/ExtrusionEntityCollection.cpp src/ExtrusionEntityCollection.hpp -src/ExtrusionEntityCollection.o src/Flow.cpp src/Flow.hpp -src/Flow.o src/GCode.hpp src/Geometry.cpp src/Geometry.hpp -src/Geometry.o src/Layer.hpp src/Line.cpp src/Line.hpp -src/Line.o src/Model.cpp src/Model.hpp -src/Model.o src/MotionPlanner.cpp src/MotionPlanner.hpp -src/MotionPlanner.o src/MultiPoint.cpp src/MultiPoint.hpp -src/MultiPoint.o src/myinit.h src/perlglue.hpp src/Point.cpp src/Point.hpp -src/Point.o src/poly2tri/common/shapes.cc src/poly2tri/common/shapes.h -src/poly2tri/common/shapes.o src/poly2tri/common/utils.h src/poly2tri/poly2tri.h src/poly2tri/sweep/advancing_front.cc src/poly2tri/sweep/advancing_front.h -src/poly2tri/sweep/advancing_front.o src/poly2tri/sweep/cdt.cc src/poly2tri/sweep/cdt.h -src/poly2tri/sweep/cdt.o src/poly2tri/sweep/sweep.cc src/poly2tri/sweep/sweep.h -src/poly2tri/sweep/sweep.o src/poly2tri/sweep/sweep_context.cc src/poly2tri/sweep/sweep_context.h -src/poly2tri/sweep/sweep_context.o src/Polygon.cpp src/Polygon.hpp -src/Polygon.o src/Polyline.cpp src/Polyline.hpp -src/Polyline.o src/PolylineCollection.cpp src/PolylineCollection.hpp -src/PolylineCollection.o src/polypartition.cpp src/polypartition.h -src/polypartition.o src/ppport.h src/Print.cpp src/Print.hpp -src/Print.o src/PrintConfig.cpp src/PrintConfig.hpp -src/PrintConfig.o src/Surface.cpp src/Surface.hpp -src/Surface.o src/SurfaceCollection.cpp src/SurfaceCollection.hpp -src/SurfaceCollection.o src/SVG.cpp src/SVG.hpp -src/SVG.o src/TriangleMesh.cpp src/TriangleMesh.hpp -src/TriangleMesh.o src/utils.cpp -src/utils.o src/visilibity.cpp src/visilibity.hpp -src/visilibity.o t/01_trianglemesh.t t/03_point.t t/04_expolygon.t From f018828bfd34dedefea25768dd071f63256f28b7 Mon Sep 17 00:00:00 2001 From: Alessandro Ranellucci Date: Tue, 27 May 2014 01:11:40 +0200 Subject: [PATCH 4/5] Simplify islands before initializing MotionPlanner --- xs/src/MotionPlanner.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/xs/src/MotionPlanner.cpp b/xs/src/MotionPlanner.cpp index 2df76efd9..4883b931a 100644 --- a/xs/src/MotionPlanner.cpp +++ b/xs/src/MotionPlanner.cpp @@ -20,6 +20,12 @@ MotionPlanner::initialize() { if (this->initialized) return; + ExPolygons expp; + for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) { + island->simplify(SCALED_EPSILON, expp); + } + this->islands = expp; + // loop through islands in order to create inner expolygons and collect their contours this->inner.reserve(this->islands.size()); Polygons outer_holes; @@ -66,6 +72,13 @@ MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyl int island_idx = -1; for (ExPolygons::const_iterator island = this->islands.begin(); island != this->islands.end(); ++island) { if (island->contains_point(from) && island->contains_point(to)) { + // since both points are in the same island, is a direct move possible? + // if so, we avoid generating the visibility environment + if (island->contains_line(Line(from, to))) { + polyline->points.push_back(from); + polyline->points.push_back(to); + return; + } island_idx = island - this->islands.begin(); break; } From f5b9df241341cb5bd305a7d0f7798d7f014583aa Mon Sep 17 00:00:00 2001 From: Alessandro Ranellucci Date: Wed, 28 May 2014 10:16:58 +0200 Subject: [PATCH 5/5] New implementation of Avoid crossing perimeters using Voronoi diagrams --- xs/MANIFEST | 2 - xs/src/MotionPlanner.cpp | 224 ++- xs/src/MotionPlanner.hpp | 37 +- xs/src/visilibity.cpp | 3659 -------------------------------------- xs/src/visilibity.hpp | 2169 ---------------------- xs/t/18_motionplanner.t | 17 + 6 files changed, 199 insertions(+), 5909 deletions(-) delete mode 100644 xs/src/visilibity.cpp delete mode 100644 xs/src/visilibity.hpp diff --git a/xs/MANIFEST b/xs/MANIFEST index dc868f678..46dd2e929 100644 --- a/xs/MANIFEST +++ b/xs/MANIFEST @@ -1718,8 +1718,6 @@ src/SVG.hpp src/TriangleMesh.cpp src/TriangleMesh.hpp src/utils.cpp -src/visilibity.cpp -src/visilibity.hpp t/01_trianglemesh.t t/03_point.t t/04_expolygon.t diff --git a/xs/src/MotionPlanner.cpp b/xs/src/MotionPlanner.cpp index 4883b931a..82108afd9 100644 --- a/xs/src/MotionPlanner.cpp +++ b/xs/src/MotionPlanner.cpp @@ -1,5 +1,10 @@ #include "BoundingBox.hpp" #include "MotionPlanner.hpp" +#include // for numeric_limits + +#include "boost/polygon/voronoi.hpp" +using boost::polygon::voronoi_builder; +using boost::polygon::voronoi_diagram; namespace Slic3r { @@ -9,9 +14,7 @@ MotionPlanner::MotionPlanner(const ExPolygons &islands) MotionPlanner::~MotionPlanner() { - for (std::vector::iterator env = this->envs.begin(); env != this->envs.end(); ++env) - delete *env; - for (std::vector::iterator graph = this->graphs.begin(); graph != this->graphs.end(); ++graph) + for (std::vector::iterator graph = this->graphs.begin(); graph != this->graphs.end(); ++graph) delete *graph; } @@ -58,7 +61,6 @@ MotionPlanner::initialize() assert(outer.size() == 1); this->outer = outer.front(); - this->envs.resize(this->islands.size() + 1, NULL); this->graphs.resize(this->islands.size() + 1, NULL); this->initialized = true; } @@ -84,9 +86,6 @@ MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyl } } - // Generate environment. - this->generate_environment(island_idx); - // Now check whether points are inside the environment. Point inner_from = from; Point inner_to = to; @@ -112,79 +111,164 @@ MotionPlanner::shortest_path(const Point &from, const Point &to, Polyline* polyl } // perform actual path search - *polyline = convert_polyline(this->envs[island_idx + 1]->shortest_path(convert_point(inner_from), - convert_point(inner_to), *this->graphs[island_idx + 1], SCALED_EPSILON)); + MotionPlannerGraph* graph = this->init_graph(island_idx); + graph->shortest_path(graph->find_node(inner_from), graph->find_node(inner_to), polyline); - // if start point was outside the environment, prepend it - if (!from_is_inside) polyline->points.insert(polyline->points.begin(), from); - - // if end point was outside the environment, append it - if (!to_is_inside) polyline->points.push_back(to); + polyline->points.insert(polyline->points.begin(), from); + polyline->points.push_back(to); +} + +MotionPlannerGraph* +MotionPlanner::init_graph(int island_idx) +{ + if (this->graphs[island_idx + 1] == NULL) { + Polygons pp; + if (island_idx == -1) { + pp = this->outer; + } else { + pp = this->inner[island_idx]; + } + + MotionPlannerGraph* graph = this->graphs[island_idx + 1] = new MotionPlannerGraph(); + + // add polygon boundaries as edges + size_t node_idx = 0; + Lines lines; + for (Polygons::const_iterator polygon = pp.begin(); polygon != pp.end(); ++polygon) { + graph->nodes.push_back(polygon->points.back()); + node_idx++; + for (Points::const_iterator p = polygon->points.begin(); p != polygon->points.end(); ++p) { + graph->nodes.push_back(*p); + double dist = graph->nodes[node_idx-1].distance_to(*p); + graph->add_edge(node_idx-1, node_idx, dist); + graph->add_edge(node_idx, node_idx-1, dist); + node_idx++; + } + polygon->lines(&lines); + } + + // add Voronoi edges as internal edges + { + typedef voronoi_diagram VD; + typedef std::map t_vd_vertices; + VD vd; + t_vd_vertices vd_vertices; + + boost::polygon::construct_voronoi(lines.begin(), lines.end(), &vd); + for (VD::const_edge_iterator edge = vd.edges().begin(); edge != vd.edges().end(); ++edge) { + if (edge->is_infinite()) continue; + + const VD::vertex_type* v0 = edge->vertex0(); + const VD::vertex_type* v1 = edge->vertex1(); + Point p0 = Point(v0->x(), v0->y()); + Point p1 = Point(v1->x(), v1->y()); + // contains_point() should probably be faster than contains_line(), + // and should it fail on any boundary points it's not a big problem + if (island_idx == -1) { + if (!this->outer.contains_point(p0) || !this->outer.contains_point(p1)) continue; + } else { + if (!this->inner[island_idx].contains_point(p0) || !this->inner[island_idx].contains_point(p1)) continue; + } + + t_vd_vertices::const_iterator i_v0 = vd_vertices.find(v0); + size_t v0_idx; + if (i_v0 == vd_vertices.end()) { + graph->nodes.push_back(p0); + v0_idx = node_idx; + vd_vertices[v0] = node_idx; + node_idx++; + } else { + v0_idx = i_v0->second; + } + + t_vd_vertices::const_iterator i_v1 = vd_vertices.find(v1); + size_t v1_idx; + if (i_v1 == vd_vertices.end()) { + graph->nodes.push_back(p1); + v1_idx = node_idx; + vd_vertices[v1] = node_idx; + node_idx++; + } else { + v1_idx = i_v1->second; + } + + double dist = graph->nodes[v0_idx].distance_to(graph->nodes[v1_idx]); + graph->add_edge(v0_idx, v1_idx, dist); + } + } + + return graph; + } + return this->graphs[island_idx + 1]; } void -MotionPlanner::generate_environment(int island_idx) +MotionPlannerGraph::add_edge(size_t from, size_t to, double weight) { - if (this->envs[island_idx + 1] != NULL) return; + // extend adjacency list until this start node + if (this->adjacency_list.size() < from+1) + this->adjacency_list.resize(from+1); - Polygons pp; - if (island_idx == -1) { - pp = this->outer; - } else { - pp = this->inner[island_idx]; + this->adjacency_list[from].push_back(neighbor(to, weight)); +} + +size_t +MotionPlannerGraph::find_node(const Point &point) const +{ + /* + for (Points::const_iterator p = this->nodes.begin(); p != this->nodes.end(); ++p) { + if (p->coincides_with(point)) return p - this->nodes.begin(); } - - // populate VisiLibity polygons - std::vector v_polygons; - for (Polygons::const_iterator p = pp.begin(); p != pp.end(); ++p) - v_polygons.push_back(convert_polygon(*p)); + */ + return point.nearest_point_index(this->nodes); +} + +void +MotionPlannerGraph::shortest_path(size_t from, size_t to, Polyline* polyline) +{ + const weight_t max_weight = std::numeric_limits::infinity(); - // generate graph and environment - this->envs[island_idx + 1] = new VisiLibity::Environment(v_polygons); - this->graphs[island_idx + 1] = new VisiLibity::Visibility_Graph(*this->envs[island_idx + 1], SCALED_EPSILON); -} + std::vector min_distance; + std::vector previous; + { + int n = this->adjacency_list.size(); + min_distance.clear(); + min_distance.resize(n, max_weight); + min_distance[from] = 0; + previous.clear(); + previous.resize(n, -1); + std::set > vertex_queue; + vertex_queue.insert(std::make_pair(min_distance[from], from)); + + while (!vertex_queue.empty()) + { + weight_t dist = vertex_queue.begin()->first; + node_t u = vertex_queue.begin()->second; + vertex_queue.erase(vertex_queue.begin()); + + // Visit each edge exiting u + const std::vector &neighbors = this->adjacency_list[u]; + for (std::vector::const_iterator neighbor_iter = neighbors.begin(); + neighbor_iter != neighbors.end(); + neighbor_iter++) + { + node_t v = neighbor_iter->target; + weight_t weight = neighbor_iter->weight; + weight_t distance_through_u = dist + weight; + if (distance_through_u < min_distance[v]) { + vertex_queue.erase(std::make_pair(min_distance[v], v)); + min_distance[v] = distance_through_u; + previous[v] = u; + vertex_queue.insert(std::make_pair(min_distance[v], v)); + } -VisiLibity::Polyline -MotionPlanner::convert_polyline(const Polyline &polyline) -{ - VisiLibity::Polyline v_polyline; - for (Points::const_iterator point = polyline.points.begin(); point != polyline.points.end(); ++point) { - v_polyline.push_back(convert_point(*point)); + } + } } - return v_polyline; -} - -Polyline -MotionPlanner::convert_polyline(const VisiLibity::Polyline &v_polyline) -{ - Polyline polyline; - polyline.points.reserve(v_polyline.size()); - for (size_t i = 0; i < v_polyline.size(); ++i) { - polyline.points.push_back(convert_point(v_polyline[i])); - } - return polyline; -} - -VisiLibity::Polygon -MotionPlanner::convert_polygon(const Polygon &polygon) -{ - VisiLibity::Polygon v_polygon; - for (Points::const_iterator point = polygon.points.begin(); point != polygon.points.end(); ++point) { - v_polygon.push_back(convert_point(*point)); - } - return v_polygon; -} - -VisiLibity::Point -MotionPlanner::convert_point(const Point &point) -{ - return VisiLibity::Point(point.x, point.y); -} - -Point -MotionPlanner::convert_point(const VisiLibity::Point &v_point) -{ - return Point((coord_t)v_point.x(), (coord_t)v_point.y()); + + for (node_t vertex = to; vertex != -1; vertex = previous[vertex]) + polyline->points.push_back(this->nodes[vertex]); + polyline->reverse(); } #ifdef SLIC3RXS diff --git a/xs/src/MotionPlanner.hpp b/xs/src/MotionPlanner.hpp index b0fd29ee0..78fd5c72b 100644 --- a/xs/src/MotionPlanner.hpp +++ b/xs/src/MotionPlanner.hpp @@ -5,7 +5,8 @@ #include "ClipperUtils.hpp" #include "ExPolygonCollection.hpp" #include "Polyline.hpp" -#include "visilibity.hpp" +#include +#include #include #define MP_INNER_MARGIN scale_(1.0) @@ -13,6 +14,8 @@ namespace Slic3r { +class MotionPlannerGraph; + class MotionPlanner { public: @@ -25,16 +28,32 @@ class MotionPlanner bool initialized; ExPolygon outer; ExPolygonCollections inner; - std::vector envs; - std::vector graphs; + std::vector graphs; void initialize(); - void generate_environment(int island_idx); - static VisiLibity::Polyline convert_polyline(const Polyline &polyline); - static Polyline convert_polyline(const VisiLibity::Polyline &v_polyline); - static VisiLibity::Polygon convert_polygon(const Polygon &polygon); - static VisiLibity::Point convert_point(const Point &point); - static Point convert_point(const VisiLibity::Point &v_point); + MotionPlannerGraph* init_graph(int island_idx); +}; + +class MotionPlannerGraph +{ + private: + typedef size_t node_t; + typedef double weight_t; + struct neighbor { + node_t target; + weight_t weight; + neighbor(node_t arg_target, weight_t arg_weight) + : target(arg_target), weight(arg_weight) { } + }; + typedef std::vector< std::vector > adjacency_list_t; + adjacency_list_t adjacency_list; + + public: + Points nodes; + //std::map, double> edges; + void add_edge(size_t from, size_t to, double weight); + size_t find_node(const Point &point) const; + void shortest_path(size_t from, size_t to, Polyline* polyline); }; } diff --git a/xs/src/visilibity.cpp b/xs/src/visilibity.cpp deleted file mode 100644 index 5f1f5bb9f..000000000 --- a/xs/src/visilibity.cpp +++ /dev/null @@ -1,3659 +0,0 @@ -/** - * \file visilibity.cpp - * \author Karl J. Obermeyer - * \date March 20, 2008 - * -\remarks -VisiLibity: A Floating-Point Visibility Algorithms Library, -Copyright (C) 2008 Karl J. Obermeyer (karl.obermeyer [ at ] gmail.com) - -This file is part of VisiLibity. - -VisiLibity is free software: you can redistribute it and/or modify it under -the terms of the GNU Lesser General Public License as published by the -Free Software Foundation, either version 3 of the License, or (at your -option) any later version. - -VisiLibity is distributed in the hope that it will be useful, but WITHOUT -ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public -License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with VisiLibity. If not, see . -*/ - - -#include "visilibity.hpp" //VisiLibity header -#include //math functions in std namespace -#include -#include //queue and priority_queue -#include //priority queues with iteration, - //integrated keys -#include -#include //sorting, min, max, reverse -#include //rand and srand -#include //Unix time -#include //file I/O -#include -#include //gives C-string manipulation -#include //string class -#include //assertions - - -///Hide helping functions in unnamed namespace (local to .C file). -namespace -{ - -} - - -/// VisiLibity's sole namespace -namespace VisiLibity -{ - - double uniform_random_sample(double lower_bound, double upper_bound) - { - assert( lower_bound <= upper_bound ); - if( lower_bound == upper_bound ) - return lower_bound; - double sample_point; - double span = upper_bound - lower_bound; - sample_point = lower_bound - + span * static_cast( std::rand() ) - / static_cast( RAND_MAX ); - return sample_point; - } - - - //Point - - - Point Point::projection_onto(const Line_Segment& line_segment_temp) const - { - assert( *this == *this - and line_segment_temp.size() > 0 ); - - if(line_segment_temp.size() == 1) - return line_segment_temp.first(); - //The projection of point_temp onto the line determined by - //line_segment_temp can be represented as an affine combination - //expressed in the form projection of Point = - //theta*line_segment_temp.first + - //(1.0-theta)*line_segment_temp.second. if theta is outside - //the interval [0,1], then one of the Line_Segment's endpoints - //must be closest to calling Point. - double theta = - ( (line_segment_temp.second().x()-x()) - *(line_segment_temp.second().x() - -line_segment_temp.first().x()) - + (line_segment_temp.second().y()-y()) - *(line_segment_temp.second().y() - -line_segment_temp.first().y()) ) - / ( pow(line_segment_temp.second().x() - -line_segment_temp.first().x(),2) - + pow(line_segment_temp.second().y() - -line_segment_temp.first().y(),2) ); - //std::cout << "\E[1;37;40m" << "Theta is: " << theta << "\x1b[0m" - //<< std::endl; - if( (0.0<=theta) and (theta<=1.0) ) - return theta*line_segment_temp.first() - + (1.0-theta)*line_segment_temp.second(); - //Else pick closest endpoint. - if( distance(*this, line_segment_temp.first()) - < distance(*this, line_segment_temp.second()) ) - return line_segment_temp.first(); - return line_segment_temp.second(); - } - - - Point Point::projection_onto(const Ray& ray_temp) const - { - assert( *this == *this - and ray_temp == ray_temp ); - - //Construct a Line_Segment parallel with the Ray which is so long, - //that the projection of the the calling Point onto that - //Line_Segment must be the same as the projection of the calling - //Point onto the Ray. - double R = distance( *this , ray_temp.base_point() ); - Line_Segment seg_approx = - Line_Segment( ray_temp.base_point(), ray_temp.base_point() + - Point( R*std::cos(ray_temp.bearing().get()), - R*std::sin(ray_temp.bearing().get()) ) ); - return projection_onto( seg_approx ); - } - - - Point Point::projection_onto(const Polyline& polyline_temp) const - { - assert( *this == *this - and polyline_temp.size() > 0 ); - - Point running_projection = polyline_temp[0]; - double running_min = distance(*this, running_projection); - Point point_temp; - for(unsigned i=0; i<=polyline_temp.size()-1; i++){ - point_temp = projection_onto( Line_Segment(polyline_temp[i], - polyline_temp[i+1]) ); - if( distance(*this, point_temp) < running_min ){ - running_projection = point_temp; - running_min = distance(*this, running_projection); - } - } - return running_projection; - } - - - Point Point::projection_onto_vertices_of(const Polygon& polygon_temp) const - { - assert(*this == *this - and polygon_temp.vertices_.size() > 0 ); - - Point running_projection = polygon_temp[0]; - double running_min = distance(*this, running_projection); - for(unsigned i=1; i<=polygon_temp.n()-1; i++){ - if( distance(*this, polygon_temp[i]) < running_min ){ - running_projection = polygon_temp[i]; - running_min = distance(*this, running_projection); - } - } - return running_projection; - } - - - Point Point::projection_onto_vertices_of(const Environment& - environment_temp) const - { - assert(*this == *this - and environment_temp.n() > 0 ); - - Point running_projection - = projection_onto_vertices_of(environment_temp.outer_boundary_); - double running_min = distance(*this, running_projection); - Point point_temp; - for(unsigned i=0; i 0 ); - - Point running_projection = polygon_temp[0]; - double running_min = distance(*this, running_projection); - Point point_temp; - for(unsigned i=0; i<=polygon_temp.n()-1; i++){ - point_temp = projection_onto( Line_Segment(polygon_temp[i], - polygon_temp[i+1]) ); - if( distance(*this, point_temp) < running_min ){ - running_projection = point_temp; - running_min = distance(*this, running_projection); - } - } - return running_projection; - } - - - Point Point::projection_onto_boundary_of(const Environment& - environment_temp) const - { - assert( *this == *this - and environment_temp.n() > 0 ); - - Point running_projection - = projection_onto_boundary_of(environment_temp.outer_boundary_); - double running_min = distance(*this, running_projection); - Point point_temp; - for(unsigned i=0; i 0 ); - - if( distance(*this, projection_onto_boundary_of(polygon_temp) ) - <= epsilon ){ - return true; - } - return false; - } - - - bool Point::on_boundary_of(const Environment& environment_temp, - double epsilon) const - { - assert( *this == *this - and environment_temp.outer_boundary_.n() > 0 ); - - if( distance(*this, projection_onto_boundary_of(environment_temp) ) - <= epsilon ){ - return true; - } - return false; - } - - - bool Point::in(const Line_Segment& line_segment_temp, - double epsilon) const - { - assert( *this == *this - and line_segment_temp.size() > 0 ); - - if( distance(*this, line_segment_temp) < epsilon ) - return true; - return false; - } - - - bool Point::in_relative_interior_of(const Line_Segment& line_segment_temp, - double epsilon) const - { - assert( *this == *this - and line_segment_temp.size() > 0 ); - - return in(line_segment_temp, epsilon) - and distance(*this, line_segment_temp.first()) > epsilon - and distance(*this, line_segment_temp.second()) > epsilon; - } - - - bool Point::in(const Polygon& polygon_temp, - double epsilon) const - { - assert( *this == *this - and polygon_temp.vertices_.size() > 0 ); - - int n = polygon_temp.vertices_.size(); - if( on_boundary_of(polygon_temp, epsilon) ) - return true; - // Then check the number of times a ray emanating from the Point - // crosses the boundary of the Polygon. An odd number of - // crossings indicates the Point is in the interior of the - // Polygon. Based on - // http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html - int i, j; bool c = false; - for (i = 0, j = n-1; i < n; j = i++){ - if ( (((polygon_temp[i].y() <= y()) - and (y() < polygon_temp[j].y())) - or ((polygon_temp[j].y() <= y()) - and (y() < polygon_temp[i].y()))) - and ( x() < (polygon_temp[j].x() - - polygon_temp[i].x()) - * (y() - polygon_temp[i].y()) - / (polygon_temp[j].y() - - polygon_temp[i].y()) - + polygon_temp[i].x()) ) - c = !c; - } - return c; - } - - - bool Point::in(const Environment& environment_temp, double epsilon) const - { - assert( *this == *this - and environment_temp.outer_boundary_.n() > 0 ); - - //On outer boundary? - if( on_boundary_of(environment_temp, epsilon) ) - return true; - //Not in outer boundary? - if( !in(environment_temp.outer_boundary_, epsilon) ) - return false; - //In hole? - for(unsigned i=0; i 0 ); - - if( distance(line_segment_temp.first(), *this)<=epsilon - or distance(line_segment_temp.second(), *this)<=epsilon ) - return true; - return false; - } - - - void Point::snap_to_vertices_of(const Polygon& polygon_temp, - double epsilon) - { - assert( *this == *this - and polygon_temp.n() > 0 ); - - Point point_temp( this->projection_onto_vertices_of(polygon_temp) ); - if( distance( *this , point_temp ) <= epsilon ) - *this = point_temp; - } - void Point::snap_to_vertices_of(const Environment& environment_temp, - double epsilon) - { - assert( *this == *this - and environment_temp.n() > 0 ); - - Point point_temp( this->projection_onto_vertices_of(environment_temp) ); - if( distance( *this , point_temp ) <= epsilon ) - *this = point_temp; - } - - - void Point::snap_to_boundary_of(const Polygon& polygon_temp, - double epsilon) - { - assert( *this == *this - and polygon_temp.n() > 0 ); - - Point point_temp( this->projection_onto_boundary_of(polygon_temp) ); - if( distance( *this , point_temp ) <= epsilon ) - *this = point_temp; - } - void Point::snap_to_boundary_of(const Environment& environment_temp, - double epsilon) - { - assert( *this == *this - and environment_temp.n() > 0 ); - - Point point_temp( this->projection_onto_boundary_of(environment_temp) ); - if( distance( *this , point_temp ) <= epsilon ) - *this = point_temp; - } - - - bool operator == (const Point& point1, const Point& point2) - { return ( ( point1.x() == point2.x() ) - and ( point1.y() == point2.y() ) ); } - bool operator != (const Point& point1, const Point& point2) - { return !( point1 == point2 ); } - - - bool operator < (const Point& point1, const Point& point2) - { - if( point1 != point1 or point2 != point2 ) - return false; - if(point1.x() < point2.x()) - return true; - else if( ( point1.x() == point2.x() ) - and ( point1.y() < point2.y() ) ) - return true; - return false; - } - bool operator > (const Point& point1, const Point& point2) - { - if( point1 != point1 or point2 != point2 ) - return false; - if( point1.x() > point2.x() ) - return true; - else if( ( point1.x() == point2.x() ) - and ( point1.y() > point2.y() ) ) - return true; - return false; - } - bool operator >= (const Point& point1, const Point& point2) - { - if( point1 != point1 or point2 != point2 ) - return false; - return !( point1 < point2 ); - } - bool operator <= (const Point& point1, const Point& point2) - { - if( point1 != point1 or point2 != point2 ) - return false; - return !( point1 > point2 ); - } - - - Point operator + (const Point& point1, const Point& point2) - { - return Point( point1.x() + point2.x(), - point1.y() + point2.y() ); - } - Point operator - (const Point& point1, const Point& point2) - { - return Point( point1.x() - point2.x(), - point1.y() - point2.y() ); - } - - - Point operator * (const Point& point1, const Point& point2) - { - return Point( point1.x()*point2.x(), - point1.y()*point2.y() ); - } - - - Point operator * (double scalar, const Point& point2) - { - return Point( scalar*point2.x(), - scalar*point2.y()); - } - Point operator * (const Point& point1, double scalar) - { - return Point( scalar*point1.x(), - scalar*point1.y()); - } - - - double cross(const Point& point1, const Point& point2) - { - assert( point1 == point1 - and point2 == point2 ); - - //The area of the parallelogram created by the Points viewed as vectors. - return point1.x()*point2.y() - point2.x()*point1.y(); - } - - - double distance(const Point& point1, const Point& point2) - { - assert( point1 == point1 - and point2 == point2 ); - - return sqrt( pow( point1.x() - point2.x() , 2 ) - + pow( point1.y() - point2.y() , 2 ) ); - } - - - double distance(const Point& point_temp, - const Line_Segment& line_segment_temp) - { - assert( point_temp == point_temp - and line_segment_temp.size() > 0 ); - - return distance( point_temp, - point_temp.projection_onto(line_segment_temp) ); - } - double distance(const Line_Segment& line_segment_temp, - const Point& point_temp) - { - return distance( point_temp, - line_segment_temp ); - } - - - double distance(const Point& point_temp, - const Ray& ray_temp) - { - assert( point_temp == point_temp - and ray_temp == ray_temp ); - return distance( point_temp, - point_temp.projection_onto(ray_temp) ); - } - double distance(const Ray& ray_temp, - const Point& point_temp) - { - return distance( point_temp, - point_temp.projection_onto(ray_temp) ); - } - - - double distance(const Point& point_temp, - const Polyline& polyline_temp) - { - assert( point_temp == point_temp - and polyline_temp.size() > 0 ); - - double running_min = distance(point_temp, polyline_temp[0]); - double distance_temp; - for(unsigned i=0; i 0); - - double running_min = distance(point_temp, polygon_temp[0]); - double distance_temp; - for(unsigned i=0; i<=polygon_temp.n(); i++){ - distance_temp = distance(point_temp, Line_Segment(polygon_temp[i], - polygon_temp[i+1]) ); - if(distance_temp < running_min) - running_min = distance_temp; - } - return running_min; - } - double boundary_distance(const Polygon& polygon_temp, const Point& point_temp) - { - return boundary_distance(point_temp, polygon_temp); - } - - - double boundary_distance(const Point& point_temp, - const Environment& environment_temp) - { - assert( point_temp == point_temp - and environment_temp.n() > 0 ); - - double running_min = distance(point_temp, environment_temp[0][0]); - double distance_temp; - for(unsigned i=0; i <= environment_temp.h(); i++){ - distance_temp = boundary_distance(point_temp, environment_temp[i]); - if(distance_temp < running_min) - running_min = distance_temp; - } - return running_min; - } - double boundary_distance(const Environment& environment_temp, - const Point& point_temp) - { - return boundary_distance(point_temp, environment_temp); - } - - - std::ostream& operator << (std::ostream& outs, const Point& point_temp) - { - outs << point_temp.x() << " " << point_temp.y(); - return outs; - } - - - //Line_Segment - - - Line_Segment::Line_Segment() - { - endpoints_ = NULL; - size_ = 0; - } - - - Line_Segment::Line_Segment(const Line_Segment& line_segment_temp) - { - switch(line_segment_temp.size_){ - case 0: - endpoints_ = NULL; - size_ = 0; - break; - case 1: - endpoints_ = new Point[1]; - endpoints_[0] = line_segment_temp.endpoints_[0]; - size_ = 1; - break; - case 2: - endpoints_ = new Point[2]; - endpoints_[0] = line_segment_temp.endpoints_[0]; - endpoints_[1] = line_segment_temp.endpoints_[1]; - size_ = 2; - } - } - - - Line_Segment::Line_Segment(const Point& point_temp) - { - endpoints_ = new Point[1]; - endpoints_[0] = point_temp; - size_ = 1; - } - - - Line_Segment::Line_Segment(const Point& first_point_temp, - const Point& second_point_temp, double epsilon) - { - if( distance(first_point_temp, second_point_temp) <= epsilon ){ - endpoints_ = new Point[1]; - endpoints_[0] = first_point_temp; - size_ = 1; - } - else{ - endpoints_ = new Point[2]; - endpoints_[0] = first_point_temp; - endpoints_[1] = second_point_temp; - size_ = 2; - } - } - - - Point Line_Segment::first() const - { - assert( size() > 0 ); - - return endpoints_[0]; - } - - - Point Line_Segment::second() const - { - assert( size() > 0 ); - - if(size_==2) - return endpoints_[1]; - else - return endpoints_[0]; - } - - - Point Line_Segment::midpoint() const - { - assert( size_ > 0 ); - - return 0.5*( first() + second() ); - } - - - double Line_Segment::length() const - { - assert( size_ > 0 ); - - return distance(first(), second()); - } - - - bool Line_Segment::is_in_standard_form() const - { - assert( size_ > 0); - - if(size_<2) - return true; - return first() <= second(); - } - - - Line_Segment& Line_Segment::operator = (const Line_Segment& line_segment_temp) - { - //Makes sure not to delete dynamic vars before they're copied. - if(this==&line_segment_temp) - return *this; - delete [] endpoints_; - switch(line_segment_temp.size_){ - case 0: - endpoints_ = NULL; - size_ = 0; - break; - case 1: - endpoints_ = new Point[1]; - endpoints_[0] = line_segment_temp.endpoints_[0]; - size_ = 1; - break; - case 2: - endpoints_ = new Point[2]; - endpoints_[0] = line_segment_temp.endpoints_[0]; - endpoints_[1] = line_segment_temp.endpoints_[1]; - size_ = 2; - } - return *this; - } - - - void Line_Segment::set_first(const Point& point_temp, double epsilon) - { - Point second_point_temp; - switch(size_){ - case 0: - endpoints_ = new Point[1]; - endpoints_[0] = point_temp; - size_ = 1; - break; - case 1: - if( distance(endpoints_[0], point_temp) <= epsilon ) - { endpoints_[0] = point_temp; return; } - second_point_temp = endpoints_[0]; - delete [] endpoints_; - endpoints_ = new Point[2]; - endpoints_[0] = point_temp; - endpoints_[1] = second_point_temp; - size_ = 2; - break; - case 2: - if( distance(point_temp, endpoints_[1]) > epsilon ) - { endpoints_[0] = point_temp; return; } - delete [] endpoints_; - endpoints_ = new Point[1]; - endpoints_[0] = point_temp; - size_ = 1; - } - } - - - void Line_Segment::set_second(const Point& point_temp, double epsilon) - { - Point first_point_temp; - switch(size_){ - case 0: - endpoints_ = new Point[1]; - endpoints_[0] = point_temp; - size_ = 1; - break; - case 1: - if( distance(endpoints_[0], point_temp) <= epsilon ) - { endpoints_[0] = point_temp; return; } - first_point_temp = endpoints_[0]; - delete [] endpoints_; - endpoints_ = new Point[2]; - endpoints_[0] = first_point_temp; - endpoints_[1] = point_temp; - size_ = 2; - break; - case 2: - if( distance(endpoints_[0], point_temp) > epsilon ) - { endpoints_[1] = point_temp; return; } - delete [] endpoints_; - endpoints_ = new Point[1]; - endpoints_[0] = point_temp; - size_ = 1; - } - } - - - void Line_Segment::reverse() - { - if(size_<2) - return; - Point point_temp(first()); - endpoints_[0] = second(); - endpoints_[1] = point_temp; - } - - - void Line_Segment::enforce_standard_form() - { - if(first() > second()) - reverse(); - } - - - void Line_Segment::clear() - { - delete [] endpoints_; - endpoints_ = NULL; - size_ = 0; - } - - - Line_Segment::~Line_Segment() - { - delete [] endpoints_; - } - - - bool operator == (const Line_Segment& line_segment1, - const Line_Segment& line_segment2) - { - if( line_segment1.size() != line_segment2.size() - or line_segment1.size() == 0 - or line_segment2.size() == 0 ) - return false; - else if( line_segment1.first() == line_segment2.first() - and line_segment1.second() == line_segment2.second() ) - return true; - else - return false; - } - - - bool operator != (const Line_Segment& line_segment1, - const Line_Segment& line_segment2) - { - return !( line_segment1 == line_segment2 ); - } - - - bool equivalent(Line_Segment line_segment1, - Line_Segment line_segment2, double epsilon) - { - if( line_segment1.size() != line_segment2.size() - or line_segment1.size() == 0 - or line_segment2.size() == 0 ) - return false; - else if( ( distance( line_segment1.first(), - line_segment2.first() ) <= epsilon - and distance( line_segment1.second(), - line_segment2.second() ) <= epsilon ) - or ( distance( line_segment1.first(), - line_segment2.second() ) <= epsilon - and distance( line_segment1.second(), - line_segment2.first() ) <= epsilon ) ) - return true; - else - return false; - } - - - double distance(const Line_Segment& line_segment1, - const Line_Segment& line_segment2) - { - assert( line_segment1.size() > 0 and line_segment2.size() > 0 ); - - if(intersect_proper(line_segment1, line_segment2)) - return 0; - //But if two line segments intersect improperly, the distance - //between them is equal to the minimum of the distances between - //all 4 endpoints_ and their respective projections onto the line - //segment they don't belong to. - double running_min, distance_temp; - running_min = distance(line_segment1.first(), line_segment2); - distance_temp = distance(line_segment1.second(), line_segment2); - if(distance_temp 0 and polygon.n() > 0 ); - - double running_min = distance( line_segment , polygon[0] ); - if( polygon.n() > 1 ) - for(unsigned i=0; i d ) - running_min = d; - } - return running_min; - } - double boundary_distance(const Polygon& polygon, - const Line_Segment& line_segment) - { return boundary_distance( line_segment , polygon ); } - - - bool intersect(const Line_Segment& line_segment1, - const Line_Segment& line_segment2, double epsilon) - { - if( line_segment1.size() == 0 - or line_segment2.size() == 0 ) - return false; - if( distance(line_segment1, line_segment2) <= epsilon ) - return true; - return false; - } - - - bool intersect_proper(const Line_Segment& line_segment1, - const Line_Segment& line_segment2, double epsilon) - { - if( line_segment1.size() == 0 - or line_segment2.size() == 0 ) - return false; - - //Declare new vars just for readability. - Point a( line_segment1.first() ); - Point b( line_segment1.second() ); - Point c( line_segment2.first() ); - Point d( line_segment2.second() ); - //First find the minimum of the distances between all 4 endpoints_ - //and their respective projections onto the opposite line segment. - double running_min, distance_temp; - running_min = distance(a, line_segment2); - distance_temp = distance(b, line_segment2); - if(distance_temp return empty segment. - if( !intersect(line_segment1, line_segment2, epsilon) ) - return line_segment_temp; - //Declare new vars just for readability. - Point a( line_segment1.first() ); - Point b( line_segment1.second() ); - Point c( line_segment2.first() ); - Point d( line_segment2.second() ); - if( intersect_proper(line_segment1, line_segment2, epsilon) ){ - //Use formula from O'Rourke's "Computational Geometry in C", p. 221. - //Note D=0 iff the line segments are parallel. - double D = a.x()*( d.y() - c.y() ) - + b.x()*( c.y() - d.y() ) - + d.x()*( b.y() - a.y() ) - + c.x()*( a.y() - b.y() ); - double s = ( a.x()*( d.y() - c.y() ) - + c.x()*( a.y() - d.y() ) - + d.x()*( c.y() - a.y() ) ) / D; - line_segment_temp.set_first( a + s * ( b - a ) ); - return line_segment_temp; - } - //Otherwise if improper... - double distance_temp_a = distance(a, line_segment2); - double distance_temp_b = distance(b, line_segment2); - double distance_temp_c = distance(c, line_segment1); - double distance_temp_d = distance(d, line_segment1); - //Check if the intersection is nondegenerate segment. - if( distance_temp_a <= epsilon and distance_temp_b <= epsilon ){ - line_segment_temp.set_first(a, epsilon); - line_segment_temp.set_second(b, epsilon); - return line_segment_temp; - } - else if( distance_temp_c <= epsilon and distance_temp_d <= epsilon ){ - line_segment_temp.set_first(c, epsilon); - line_segment_temp.set_second(d, epsilon); - return line_segment_temp; - } - else if( distance_temp_a <= epsilon and distance_temp_c <= epsilon ){ - line_segment_temp.set_first(a, epsilon); - line_segment_temp.set_second(c, epsilon); - return line_segment_temp; - } - else if( distance_temp_a <= epsilon and distance_temp_d <= epsilon ){ - line_segment_temp.set_first(a, epsilon); - line_segment_temp.set_second(d, epsilon); - return line_segment_temp; - } - else if( distance_temp_b <= epsilon and distance_temp_c <= epsilon ){ - line_segment_temp.set_first(b, epsilon); - line_segment_temp.set_second(c, epsilon); - return line_segment_temp; - } - else if( distance_temp_b <= epsilon and distance_temp_d <= epsilon ){ - line_segment_temp.set_first(b, epsilon); - line_segment_temp.set_second(d, epsilon); - return line_segment_temp; - } - //Check if the intersection is a single point. - else if( distance_temp_a <= epsilon ){ - line_segment_temp.set_first(a, epsilon); - return line_segment_temp; - } - else if( distance_temp_b <= epsilon ){ - line_segment_temp.set_first(b, epsilon); - return line_segment_temp; - } - else if( distance_temp_c <= epsilon ){ - line_segment_temp.set_first(c, epsilon); - return line_segment_temp; - } - else if( distance_temp_d <= epsilon ){ - line_segment_temp.set_first(d, epsilon); - return line_segment_temp; - } - return line_segment_temp; - } - - - std::ostream& operator << (std::ostream& outs, - const Line_Segment& line_segment_temp) - { - switch(line_segment_temp.size()){ - case 0: - return outs; - break; - case 1: - outs << line_segment_temp.first() << std::endl - << line_segment_temp.second() << std::endl; - return outs; - break; - case 2: - outs << line_segment_temp.first() << std::endl - << line_segment_temp.second() << std::endl; - return outs; - } - return outs; - } - - - //Angle - - - Angle::Angle(double data_temp) - { - if(data_temp >= 0) - angle_radians_ = fmod(data_temp, 2*M_PI); - else{ - angle_radians_ = 2*M_PI + fmod(data_temp, -2*M_PI); - if(angle_radians_ == 2*M_PI) - angle_radians_ = 0; - } - } - - - Angle::Angle(double rise_temp, double run_temp) - { - if( rise_temp == 0 and run_temp == 0 ) - angle_radians_ = 0; - //First calculate 4 quadrant inverse tangent into [-pi,+pi]. - angle_radians_ = std::atan2(rise_temp, run_temp); - //Correct so angles specified in [0, 2*PI). - if(angle_radians_ < 0) - angle_radians_ = 2*M_PI + angle_radians_; - } - - - void Angle::set(double data_temp) - { - *this = Angle(data_temp); - } - - - void Angle::randomize() - { - angle_radians_ = fmod( uniform_random_sample(0, 2*M_PI), 2*M_PI ); - } - - - bool operator == (const Angle& angle1, const Angle& angle2) - { - return (angle1.get() == angle2.get()); - } - bool operator != (const Angle& angle1, const Angle& angle2) - { - return !(angle1.get() == angle2.get()); - } - - - bool operator > (const Angle& angle1, const Angle& angle2) - { - return angle1.get() > angle2.get(); - } - bool operator < (const Angle& angle1, const Angle& angle2) - { - return angle1.get() < angle2.get(); - } - bool operator >= (const Angle& angle1, const Angle& angle2) - { - return angle1.get() >= angle2.get(); - } - bool operator <= (const Angle& angle1, const Angle& angle2) - { - return angle1.get() <= angle2.get(); - } - - - Angle operator + (const Angle& angle1, const Angle& angle2) - { - return Angle( angle1.get() + angle2.get() ); - } - Angle operator - (const Angle& angle1, const Angle& angle2) - { - return Angle( angle1.get() - angle2.get() ); - } - - - double geodesic_distance(const Angle& angle1, const Angle& angle2) - { - assert( angle1.get() == angle1.get() - and angle2.get() == angle2.get() ); - - double distance1 = std::fabs( angle1.get() - - angle2.get() ); - double distance2 = 2*M_PI - distance1; - if(distance1 < distance2) - return distance1; - return distance2; - } - - - double geodesic_direction(const Angle& angle1, const Angle& angle2) - { - assert( angle1.get() == angle1.get() - and angle2.get() == angle2.get() ); - - double distance1 = std::fabs( angle1.get() - - angle2.get() ); - double distance2 = 2*M_PI - distance1; - if(angle1 <= angle2){ - if(distance1 < distance2) - return 1.0; - return -1.0; - } - //Otherwise angle1 > angle2. - if(distance1 < distance2) - return -1.0; - return 1.0; - } - - - std::ostream& operator << (std::ostream& outs, const Angle& angle_temp) - { - outs << angle_temp.get(); - return outs; - } - - - //Polar_Point - - - Polar_Point::Polar_Point(const Point& polar_origin_temp, - const Point& point_temp, - double epsilon) : Point(point_temp) - { - polar_origin_ = polar_origin_temp; - if( polar_origin_==polar_origin_ - and point_temp==point_temp - and distance(polar_origin_, point_temp) <= epsilon ){ - bearing_ = Angle(0.0); - range_ = 0.0; - } - else if( polar_origin_==polar_origin_ - and point_temp==point_temp){ - bearing_ = Angle( point_temp.y()-polar_origin_temp.y(), - point_temp.x()-polar_origin_temp.x() ); - range_ = distance(polar_origin_temp, point_temp); - } - } - - - void Polar_Point::set_polar_origin(const Point& polar_origin_temp) - { - *this = Polar_Point( polar_origin_temp, Point(x(), y()) ); - } - - - void Polar_Point::set_x(double x_temp) - { - *this = Polar_Point( polar_origin_, Point(x_temp, y()) ); - } - - - void Polar_Point::set_y(double y_temp) - { - *this = Polar_Point( polar_origin_, Point(x(), y_temp) ); - } - - - void Polar_Point::set_range(double range_temp) - { - range_ = range_temp; - x_ = polar_origin_.x() - + range_*std::cos( bearing_.get() ); - y_ = polar_origin_.y() - + range_*std::sin( bearing_.get() ); - } - - - void Polar_Point::set_bearing(const Angle& bearing_temp) - { - bearing_ = bearing_temp; - x_ = polar_origin_.x() - + range_*std::cos( bearing_.get() ); - y_ = polar_origin_.y() - + range_*std::sin( bearing_.get() ); - } - - - bool operator == (const Polar_Point& polar_point1, - const Polar_Point& polar_point2) - { - if( polar_point1.polar_origin() == polar_point2.polar_origin() - and polar_point1.range() == polar_point2.range() - and polar_point1.bearing() == polar_point2.bearing() - ) - return true; - return false; - } - bool operator != (const Polar_Point& polar_point1, - const Polar_Point& polar_point2) - { - return !( polar_point1 == polar_point2 ); - } - - - bool operator > (const Polar_Point& polar_point1, - const Polar_Point& polar_point2) - { - if( polar_point1.polar_origin() != polar_point1.polar_origin() - or polar_point1.range() != polar_point1.range() - or polar_point1.bearing() != polar_point1.bearing() - or polar_point2.polar_origin() != polar_point2.polar_origin() - or polar_point2.range() != polar_point2.range() - or polar_point2.bearing() != polar_point2.bearing() - ) - return false; - - if( polar_point1.bearing() > polar_point2.bearing() ) - return true; - else if( polar_point1.bearing() == polar_point2.bearing() - and polar_point1.range() > polar_point2.range() ) - return true; - return false; - } - bool operator < (const Polar_Point& polar_point1, - const Polar_Point& polar_point2) - { - if( polar_point1.polar_origin() != polar_point1.polar_origin() - or polar_point1.range() != polar_point1.range() - or polar_point1.bearing() != polar_point1.bearing() - or polar_point2.polar_origin() != polar_point2.polar_origin() - or polar_point2.range() != polar_point2.range() - or polar_point2.bearing() != polar_point2.bearing() - ) - return false; - - if( polar_point1.bearing() < polar_point2.bearing() ) - return true; - else if( polar_point1.bearing() == polar_point2.bearing() - and polar_point1.range() < polar_point2.range() ) - return true; - return false; - - } - bool operator >= (const Polar_Point& polar_point1, - const Polar_Point& polar_point2) - { - if( polar_point1.polar_origin() != polar_point1.polar_origin() - or polar_point1.range() != polar_point1.range() - or polar_point1.bearing() != polar_point1.bearing() - or polar_point2.polar_origin() != polar_point2.polar_origin() - or polar_point2.range() != polar_point2.range() - or polar_point2.bearing() != polar_point2.bearing() - ) - return false; - - return !(polar_point1polar_point2); - } - - - std::ostream& operator << (std::ostream& outs, - const Polar_Point& polar_point_temp) - { - outs << polar_point_temp.bearing() << " " << polar_point_temp.range(); - return outs; - } - - - //Ray - - - Ray::Ray(Point base_point_temp, Point bearing_point) - { - assert( !( base_point_temp == bearing_point ) ); - - base_point_ = base_point_temp; - bearing_ = Angle( bearing_point.y()-base_point_temp.y(), - bearing_point.x()-base_point_temp.x() ); - } - - bool operator == (const Ray& ray1, - const Ray& ray2) - { - if( ray1.base_point() == ray2.base_point() - and ray1.bearing() == ray2.bearing() ) - return true; - else - return false; - } - - - bool operator != (const Ray& ray1, - const Ray& ray2) - { - return !( ray1 == ray2 ); - } - - - Line_Segment intersection(const Ray ray_temp, - const Line_Segment& line_segment_temp, - double epsilon) - { - assert( ray_temp == ray_temp - and line_segment_temp.size() > 0 ); - - //First construct a Line_Segment parallel with the Ray which is so - //long, that it's intersection with line_segment_temp will be - //equal to the intersection of ray_temp with line_segment_temp. - double R = distance(ray_temp.base_point(), line_segment_temp) - + line_segment_temp.length(); - Line_Segment seg_approx = - Line_Segment( ray_temp.base_point(), ray_temp.base_point() + - Point( R*std::cos(ray_temp.bearing().get()), - R*std::sin(ray_temp.bearing().get()) ) ); - Line_Segment intersect_seg = intersection(line_segment_temp, - seg_approx, - epsilon); - //Make sure point closer to ray_temp's base_point is listed first. - if( intersect_seg.size() == 2 - and distance( intersect_seg.first(), ray_temp.base_point() ) > - distance( intersect_seg.second(), ray_temp.base_point() ) ){ - intersect_seg.reverse(); - } - return intersect_seg; - } - - - Line_Segment intersection(const Line_Segment& line_segment_temp, - const Ray& ray_temp, - double epsilon) - { - return intersection( ray_temp , line_segment_temp , epsilon ); - } - - - //Polyline - - - double Polyline::length() const - { - double length_temp = 0; - for(unsigned i=1; i <= vertices_.size()-1; i++) - length_temp += distance( vertices_[i-1] , vertices_[i] ); - return length_temp; - } - - - double Polyline::diameter() const - { - //Precondition: nonempty Polyline. - assert( size() > 0 ); - - double running_max=0; - for(unsigned i=0; i running_max ) - running_max = distance( (*this)[i] , (*this)[j] ); - }} - return running_max; - } - - - Bounding_Box Polyline::bbox () const - { - //Precondition: nonempty Polyline. - assert( vertices_.size() > 0 ); - - Bounding_Box bounding_box; - double x_min=vertices_[0].x(), x_max=vertices_[0].x(), - y_min=vertices_[0].y(), y_max=vertices_[0].y(); - for(unsigned i = 1; i < vertices_.size(); i++){ - if(x_min > vertices_[i].x()) { x_min=vertices_[i].x(); } - if(x_max < vertices_[i].x()) { x_max=vertices_[i].x(); } - if(y_min > vertices_[i].y()) { y_min=vertices_[i].y(); } - if(y_max < vertices_[i].y()) { y_max=vertices_[i].y(); } - } - bounding_box.x_min=x_min; bounding_box.x_max=x_max; - bounding_box.y_min=y_min; bounding_box.y_max=y_max; - return bounding_box; - } - - - void Polyline::eliminate_redundant_vertices(double epsilon) - { - //Trivial case - if(vertices_.size() < 3) - return; - - //Store new minimal length list of vertices - std::vector vertices_temp; - vertices_temp.reserve(vertices_.size()); - - //Place holders - unsigned first = 0; - unsigned second = 1; - unsigned third = 2; - - //Add first vertex - vertices_temp.push_back((*this)[first]); - - while( third < vertices_.size() ){ - //if second redundant - if( distance( Line_Segment( (*this)[first], - (*this)[third] ) , - (*this)[second] ) - <= epsilon ){ - //=>skip it - second = third; - third++; - } - //else second not redundant - else{ - //=>add it. - vertices_temp.push_back((*this)[second]); - first = second; - second = third; - third++; - } - } - - //Add last vertex - vertices_temp.push_back(vertices_.back()); - - //Update list of vertices - vertices_ = vertices_temp; - } - - - void Polyline::reverse() - { - std::reverse( vertices_.begin() , vertices_.end() ); - } - - - std::ostream& operator << (std::ostream& outs, - const Polyline& polyline_temp) - { - for(unsigned i=0; i> x_temp and fin >> y_temp){ - point_temp.set_x(x_temp); - point_temp.set_y(y_temp); - vertices_.push_back(point_temp); - } - fin.close(); - } - - - Polygon::Polygon(const std::vector& vertices_temp) - { - vertices_ = vertices_temp; - } - - - Polygon::Polygon(const Point& point0, - const Point& point1, - const Point& point2) - { - vertices_.push_back(point0); - vertices_.push_back(point1); - vertices_.push_back(point2); - } - - - unsigned Polygon::r () const - { - int r_count = 0; - if( vertices_.size() > 1 ){ - //Use cross product to count right turns. - for(unsigned i=0; i<=n()-1; i++) - if( ((*this)[i+1].x()-(*this)[i].x()) - *((*this)[i+2].y()-(*this)[i].y()) - - ((*this)[i+1].y()-(*this)[i].y()) - *((*this)[i+2].x()-(*this)[i].x()) < 0 ) - r_count++; - if( area() < 0 ){ - r_count = n() - r_count; - } - } - return r_count; - } - - - bool Polygon::is_simple(double epsilon) const - { - - if(n()==0 or n()==1 or n()==2) - return false; - - //Make sure adjacent edges only intersect at a single point. - for(unsigned i=0; i<=n()-1; i++) - if( intersection( Line_Segment((*this)[i],(*this)[i+1]) , - Line_Segment((*this)[i+1],(*this)[i+2]) , - epsilon ).size() > 1 ) - return false; - - //Make sure nonadjacent edges do not intersect. - for(unsigned i=0; i 1) //if more than one point in the polygon. - for(unsigned i=1; i vertices_[i]) - return false; - return true; - } - - - double Polygon::boundary_length() const - { - double length_temp=0; - if(n()==0 or n()==1) - return 0; - for(unsigned i=0; i 0 ); - - double area_temp=area(); - if(area_temp==0) - { std::cerr << "\x1b[5;31m" - << "Warning: tried to compute centoid of polygon with zero area!" - << "\x1b[0m\n" << "\a \n"; exit(1); } - double x_temp=0; - for(unsigned i=0; i<=n()-1; i++) - x_temp += ( (*this)[i].x() + (*this)[i+1].x() ) - * ( (*this)[i].x()*(*this)[i+1].y() - - (*this)[i+1].x()*(*this)[i].y() ); - double y_temp=0; - for(unsigned i=0; i<=n()-1; i++) - y_temp += ( (*this)[i].y() + (*this)[i+1].y() ) - * ( (*this)[i].x()*(*this)[i+1].y() - - (*this)[i+1].x()*(*this)[i].y() ); - return Point(x_temp/(6*area_temp), y_temp/(6*area_temp)); - } - - - double Polygon::diameter() const - { - //Precondition: nonempty Polygon. - assert( n() > 0 ); - - double running_max=0; - for(unsigned i=0; i running_max ) - running_max = distance( (*this)[i] , (*this)[j] ); - }} - return running_max; - } - - - Bounding_Box Polygon::bbox () const - { - //Precondition: nonempty Polygon. - assert( vertices_.size() > 0 ); - - Bounding_Box bounding_box; - double x_min=vertices_[0].x(), x_max=vertices_[0].x(), - y_min=vertices_[0].y(), y_max=vertices_[0].y(); - for(unsigned i = 1; i < vertices_.size(); i++){ - if(x_min > vertices_[i].x()) { x_min=vertices_[i].x(); } - if(x_max < vertices_[i].x()) { x_max=vertices_[i].x(); } - if(y_min > vertices_[i].y()) { y_min=vertices_[i].y(); } - if(y_max < vertices_[i].y()) { y_max=vertices_[i].y(); } - } - bounding_box.x_min=x_min; bounding_box.x_max=x_max; - bounding_box.y_min=y_min; bounding_box.y_max=y_max; - return bounding_box; - } - - - std::vector Polygon::random_points(const unsigned& count, - double epsilon) const - { - //Precondition: nonempty Polygon. - assert( vertices_.size() > 0 ); - - Bounding_Box bounding_box = bbox(); - std::vector pts_in_polygon; pts_in_polygon.reserve(count); - Point pt_temp( uniform_random_sample(bounding_box.x_min, - bounding_box.x_max), - uniform_random_sample(bounding_box.y_min, - bounding_box.y_max) ); - while(pts_in_polygon.size() < count){ - while(!pt_temp.in(*this, epsilon)){ - pt_temp.set_x( uniform_random_sample(bounding_box.x_min, - bounding_box.x_max) ); - pt_temp.set_y( uniform_random_sample(bounding_box.y_min, - bounding_box.y_max) ); - } - pts_in_polygon.push_back(pt_temp); - pt_temp.set_x( uniform_random_sample(bounding_box.x_min, - bounding_box.x_max) ); - pt_temp.set_y( uniform_random_sample(bounding_box.y_min, - bounding_box.y_max) ); - } - return pts_in_polygon; - } - - - void Polygon::write_to_file(const std::string& filename, - int fios_precision_temp) - { - assert( fios_precision_temp >= 1 ); - - std::ofstream fout( filename.c_str() ); - //fout.open( filename.c_str() ); //Alternatives. - //fout << *this; - fout.setf(std::ios::fixed); - fout.setf(std::ios::showpoint); - fout.precision(fios_precision_temp); - for(unsigned i=0; i 1){ //if more than one point in the polygon. - std::vector vertices_temp; - vertices_temp.reserve(point_count); - //Find index of lexicographically smallest point. - int index_of_smallest=0; - int i; //counter. - for(i=1; i vertices_temp; - vertices_temp.reserve( vertices_.size() ); - - //Place holders. - unsigned first = 0; - unsigned second = 1; - unsigned third = 2; - - while( third <= vertices_.size() ){ - //if second is redundant - if( distance( Line_Segment( (*this)[first], - (*this)[third] ) , - (*this)[second] ) - <= epsilon ){ - //=>skip it - second = third; - third++; - } - //else second not redundant - else{ - //=>add it - vertices_temp.push_back( (*this)[second] ); - first = second; - second = third; - third++; - } - } - - //decide whether to add original first point - if( distance( Line_Segment( vertices_temp.front(), - vertices_temp.back() ) , - vertices_.front() ) - > epsilon ) - vertices_temp.push_back( vertices_.front() ); - - //Update list of vertices. - vertices_ = vertices_temp; - } - - - void Polygon::reverse() - { - if( n() > 2 ) - std::reverse( ++vertices_.begin() , vertices_.end() ); - } - - - bool operator == (Polygon polygon1, Polygon polygon2) - { - if( polygon1.n() != polygon2.n() - or polygon1.n() == 0 - or polygon2.n() == 0 ) - return false; - for(unsigned i=0; i epsilon ) - { successful_match = false; break; } - } - if( successful_match ) - return true; - } - return false; - } - - - double boundary_distance(const Polygon& polygon1, const Polygon& polygon2) - { - assert( polygon1.n() > 0 and polygon2.n() > 0 ); - - //Handle single point degeneracy. - if(polygon1.n() == 1) - return boundary_distance(polygon1[0], polygon2); - else if(polygon2.n() == 1) - return boundary_distance(polygon2[0], polygon1); - //Handle cases where each polygon has at least 2 points. - //Initialize to an upper bound. - double running_min = boundary_distance(polygon1[0], polygon2); - double distance_temp; - //Loop over all possible pairs of line segments. - for(unsigned i=0; i<=polygon1.n()-1; i++){ - for(unsigned j=0; j<=polygon2.n()-1; j++){ - distance_temp = distance( Line_Segment(polygon1[i], polygon1[i+1]) , - Line_Segment(polygon2[j], polygon2[j+1]) ); - if(distance_temp < running_min) - running_min = distance_temp; - }} - return running_min; - } - - - std::ostream& operator << (std::ostream& outs, - const Polygon& polygon_temp) - { - for(unsigned i=0; i& polygons) - { - outer_boundary_ = polygons[0]; - for(unsigned i=1; i vertices_temp; - - //Skip comments - while( fin.peek() == '/' ) - fin.ignore(200,'\n'); - - //Read outer_boundary. - while ( fin.peek() != '/' ){ - fin >> x_temp >> y_temp; - //Skip to next line. - fin.ignore(1); - if( fin.eof() ) - { - outer_boundary_.set_vertices(vertices_temp); - fin.close(); - update_flattened_index_key(); return; - } - vertices_temp.push_back( Point(x_temp, y_temp) ); - } - outer_boundary_.set_vertices(vertices_temp); - vertices_temp.clear(); - - //Read holes. - Polygon polygon_temp; - while(1){ - //Skip comments - while( fin.peek() == '/' ) - fin.ignore(200,'\n'); - if( fin.eof() ) - { fin.close(); update_flattened_index_key(); return; } - while( fin.peek() != '/' ){ - fin >> x_temp >> y_temp; - if( fin.eof() ) - { - polygon_temp.set_vertices(vertices_temp); - holes_.push_back(polygon_temp); - fin.close(); - update_flattened_index_key(); return; - } - vertices_temp.push_back( Point(x_temp, y_temp) ); - //Skips to next line. - fin.ignore(1); - } - polygon_temp.set_vertices(vertices_temp); - holes_.push_back(polygon_temp); - vertices_temp.clear(); - } - - update_flattened_index_key(); - } - - - const Point& Environment::operator () (unsigned k) const - { - //std::pair ij(one_to_two(k)); - std::pair ij( flattened_index_key_[k] ); - return (*this)[ ij.first ][ ij.second ]; - } - - - unsigned Environment::n() const - { - int n_count = 0; - n_count = outer_boundary_.n(); - for(unsigned i=0; i 0 ) - return false; - return true; - } - - - bool Environment::is_valid(double epsilon) const - { - if( n() <= 2 ) - return false; - - //Check all Polygons are simple. - if( !outer_boundary_.is_simple(epsilon) ){ - std::cerr << std::endl << "\x1b[31m" - << "The outer boundary is not simple." - << "\x1b[0m" << std::endl; - return false; - } - for(unsigned i=0; i= 0 ){ - std::cerr << std::endl << "\x1b[31m" - << "The vertices of hole " << i << " are not listed cw." - << "\x1b[0m" << std::endl; - return false; - } - - return true; - } - - - double Environment::boundary_length() const - { - //Precondition: nonempty Environment. - assert( outer_boundary_.n() > 0 ); - - double length_temp = outer_boundary_.boundary_length(); - for(unsigned i=0; i Environment::random_points(const unsigned& count, - double epsilon) const - { - assert( area() > 0 ); - - Bounding_Box bounding_box = bbox(); - std::vector pts_in_environment; - pts_in_environment.reserve(count); - Point pt_temp( uniform_random_sample(bounding_box.x_min, - bounding_box.x_max), - uniform_random_sample(bounding_box.y_min, - bounding_box.y_max) ); - while(pts_in_environment.size() < count){ - while(!pt_temp.in(*this, epsilon)){ - pt_temp.set_x( uniform_random_sample(bounding_box.x_min, - bounding_box.x_max) ); - pt_temp.set_y( uniform_random_sample(bounding_box.y_min, - bounding_box.y_max) ); - } - pts_in_environment.push_back(pt_temp); - pt_temp.set_x( uniform_random_sample(bounding_box.x_min, - bounding_box.x_max) ); - pt_temp.set_y( uniform_random_sample(bounding_box.y_min, - bounding_box.y_max) ); - } - return pts_in_environment; - } - - - Polyline Environment::shortest_path(const Point& start, - const Point& finish, - const Visibility_Graph& visibility_graph, - double epsilon) - { - //true => data printed to terminal - //false => silent - const bool PRINTING_DEBUG_DATA = false; - - //For now, just find one shortest path, later change this to a - //vector to find all shortest paths (w/in epsilon). - Polyline shortest_path_output; - Visibility_Polygon start_visibility_polygon(start, *this, epsilon); - - //Trivial cases - if( distance(start,finish) <= epsilon ){ - shortest_path_output.push_back(start); - return shortest_path_output; - } - else if( finish.in(start_visibility_polygon, epsilon) ){ - shortest_path_output.push_back(start); - shortest_path_output.push_back(finish); - return shortest_path_output; - } - - Visibility_Polygon finish_visibility_polygon(finish, *this, epsilon); - - //Connect start and finish Points to the visibility graph - bool *start_visible; //start row of visibility graph - bool *finish_visible; //finish row of visibility graph - start_visible = new bool[n()]; - finish_visible = new bool[n()]; - for(unsigned k=0; k T; - //:WARNING: - //If T is a vector it is crucial to make T large enough that it - //will not be resized. If T were resized, any iterators pointing - //to its contents would be invalidated, thus causing the program - //to fail. - //T.reserve( n() + 3 ); - - //Initialize priority queue of unexpanded nodes - std::set Q; - - //Construct initial node - Shortest_Path_Node current_node; - //convention vertex_index == n() => corresponds to start Point - //vertex_index == n() + 1 => corresponds to finish Point - current_node.vertex_index = n(); - current_node.cost_to_come = 0; - current_node.estimated_cost_to_go = distance( start , finish ); - //Put in T and on Q - T.push_back( current_node ); - T.begin()->search_tree_location = T.begin(); - current_node.search_tree_location = T.begin(); - T.begin()->parent_search_tree_location = T.begin(); - current_node.parent_search_tree_location = T.begin(); - Q.insert( current_node ); - - //Initialize temporary variables - Shortest_Path_Node child; //children of current_node - std::vector children; - //flags - bool solution_found = false; - bool child_already_visited = false; - //-----------Begin Main Loop----------- - while( !Q.empty() ){ - - //Pop top element off Q onto current_node - current_node = *Q.begin(); Q.erase( Q.begin() ); - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - <<"==============" - <<" current_node just poped off of Q " - <<"==============" - << std::endl; - current_node.print(); - std::cout << std::endl; - } - - //Check for goal state - //(if current node corresponds to finish) - if( current_node.vertex_index == n() + 1 ){ - - if( PRINTING_DEBUG_DATA ){ - std::cout <<"solution found!" - << std::endl - << std::endl; - } - - solution_found = true; - break; - } - - //Expand current_node (compute children) - children.clear(); - - if( PRINTING_DEBUG_DATA ){ - std::cout << "-------------------------------------------" - << std::endl - << "Expanding Current Node (Computing Children)" - << std::endl - << "current size of search tree T = " - << T.size() - << std::endl - << "-------------------------------------------" - << std::endl; - } - - //if current_node corresponds to start - if( current_node.vertex_index == n() ){ - //loop over environment vertices - for(unsigned i=0; i < n(); i++){ - if( start_visible[i] ){ - child.vertex_index = i; - child.parent_search_tree_location - = current_node.search_tree_location; - child.cost_to_come = distance( start , (*this)(i) ); - child.estimated_cost_to_go = distance( (*this)(i) , finish ); - children.push_back( child ); - - if( PRINTING_DEBUG_DATA ){ - std::cout << std::endl << "computed child: " - << std::endl; - child.print(); - } - - } - } - } - //else current_node corresponds to a vertex of the environment - else{ - //check which environment vertices are visible - for(unsigned i=0; i < n(); i++){ - if( current_node.vertex_index != i ) - if( visibility_graph( current_node.vertex_index , i ) ){ - child.vertex_index = i; - child.parent_search_tree_location - = current_node.search_tree_location; - child.cost_to_come = current_node.cost_to_come - + distance( (*this)(current_node.vertex_index), - (*this)(i) ); - child.estimated_cost_to_go = distance( (*this)(i) , finish ); - children.push_back( child ); - - if( PRINTING_DEBUG_DATA ){ - std::cout << std::endl << "computed child: " - << std::endl; - child.print(); - } - - } - } - //check if finish is visible - if( finish_visible[ current_node.vertex_index ] ){ - child.vertex_index = n() + 1; - child.parent_search_tree_location - = current_node.search_tree_location; - child.cost_to_come = current_node.cost_to_come - + distance( (*this)(current_node.vertex_index) , finish ); - child.estimated_cost_to_go = 0; - children.push_back( child ); - - if( PRINTING_DEBUG_DATA ){ - std::cout << std::endl << "computed child: " - << std::endl; - child.print(); - } - - } - } - - if( PRINTING_DEBUG_DATA ){ - std::cout << std::endl - <<"-----------------------------------------" - << std::endl - << "Processing " << children.size() - << " children" << std::endl - << "-----------------------------------------" - << std::endl; - } - - //Process children - for( std::vector::iterator - children_itr = children.begin(); - children_itr != children.end(); - children_itr++ ){ - child_already_visited = false; - - if( PRINTING_DEBUG_DATA ){ - std::cout << std::endl << "current child being processed: " - << std::endl; - children_itr->print(); - } - - //Check if child state has already been visited - //(by looking in search tree T) - for( std::list::iterator T_itr = T.begin(); - T_itr != T.end(); T_itr++ ){ - if( children_itr->vertex_index - == T_itr->vertex_index ){ - children_itr->search_tree_location = T_itr; - child_already_visited = true; - break; - } - } - - if( !child_already_visited ){ - //Add child to search tree T - T.push_back( *children_itr ); - (--T.end())->search_tree_location = --T.end(); - children_itr->search_tree_location = --T.end(); - Q.insert( *children_itr ); - } - else if( children_itr->search_tree_location->cost_to_come > - children_itr->cost_to_come ){ - //redirect parent pointer in search tree - children_itr->search_tree_location->parent_search_tree_location - = children_itr->parent_search_tree_location; - //and update cost data - children_itr->search_tree_location->cost_to_come - = children_itr->cost_to_come; - //update Q - for(std::set::iterator - Q_itr = Q.begin(); - Q_itr!= Q.end(); - Q_itr++){ - if( children_itr->vertex_index == Q_itr->vertex_index ){ - Q.erase( Q_itr ); - break; - } - } - Q.insert( *children_itr ); - } - - //If not already visited, insert into Q - if( !child_already_visited ) - Q.insert( *children_itr ); - - if( PRINTING_DEBUG_DATA ){ - std::cout << "child already visited? " - << child_already_visited - << std::endl; - } - - } - } - //-----------End Main Loop----------- - - //Recover solution - if( solution_found ){ - shortest_path_output.push_back( finish ); - std::list::iterator - backtrace_itr = current_node.parent_search_tree_location; - Point waypoint; - - if( PRINTING_DEBUG_DATA ){ - std::cout << "----------------------------" << std::endl - << "backtracing to find solution" << std::endl - << "----------------------------" << std::endl; - - } - - while( true ){ - - if( PRINTING_DEBUG_DATA ){ - std::cout << "backtrace node is " - << std::endl; - backtrace_itr->print(); - std::cout << std::endl; - } - - if( backtrace_itr->vertex_index < n() ) - waypoint = (*this)( backtrace_itr->vertex_index ); - else if( backtrace_itr->vertex_index == n() ) - waypoint = start; - //Add vertex if not redundant - if( shortest_path_output.size() > 0 - and distance( shortest_path_output[ shortest_path_output.size() - - 1 ], - waypoint ) > epsilon ) - shortest_path_output.push_back( waypoint ); - if( backtrace_itr->cost_to_come == 0 ) - break; - backtrace_itr = backtrace_itr->parent_search_tree_location; - } - shortest_path_output.reverse(); - } - - //free memory - delete [] start_visible; - delete [] finish_visible; - - //shortest_path_output.eliminate_redundant_vertices( epsilon ); - //May not be desirable to eliminate redundant vertices, because - //those redundant vertices can make successive waypoints along the - //shortest path robustly visible (and thus easier for a robot to - //navigate) - - return shortest_path_output; - } - Polyline Environment::shortest_path(const Point& start, - const Point& finish, - double epsilon) - { - return shortest_path( start, - finish, - Visibility_Graph(*this, epsilon), - epsilon ); - } - - - void Environment::write_to_file(const std::string& filename, - int fios_precision_temp) - { - assert( fios_precision_temp >= 1 ); - - std::ofstream fout( filename.c_str() ); - //fout.open( filename.c_str() ); //Alternatives. - //fout << *this; - fout.setf(std::ios::fixed); - fout.setf(std::ios::showpoint); - fout.precision(fios_precision_temp); - fout << "//Environment Model" << std::endl; - fout << "//Outer Boundary" << std::endl << outer_boundary_; - for(unsigned i=0; i ij( one_to_two(k) ); - std::pair ij( flattened_index_key_[k] ); - return (*this)[ ij.first ][ ij.second ]; - } - - - void Environment::enforce_standard_form() - { - if( outer_boundary_.area() < 0 ) - outer_boundary_.reverse(); - outer_boundary_.enforce_standard_form(); - for(unsigned i=0; i 0 ) - holes_[i].reverse(); - holes_[i].enforce_standard_form(); - } - } - - - void Environment::eliminate_redundant_vertices(double epsilon) - { - outer_boundary_.eliminate_redundant_vertices(epsilon); - for(unsigned i=0; i pair_temp; - for(unsigned i=0; i<=h(); i++){ - for(unsigned j=0; j<(*this)[i].n(); j++){ - pair_temp.first = i; - pair_temp.second = j; - flattened_index_key_.push_back( pair_temp ); - }} - } - - - std::pair Environment::one_to_two(unsigned k) const - { - std::pair two(0,0); - //Strategy: add up vertex count of each Polygon (outer boundary + - //holes) until greater than k - unsigned current_polygon_index = 0; - unsigned vertex_count_up_to_current_polygon = (*this)[0].n(); - unsigned vertex_count_up_to_last_polygon = 0; - - while( k >= vertex_count_up_to_current_polygon - and current_polygon_index < (*this).h() ){ - current_polygon_index++; - two.first = two.first + 1; - vertex_count_up_to_last_polygon = vertex_count_up_to_current_polygon; - vertex_count_up_to_current_polygon += (*this)[current_polygon_index].n(); - } - two.second = k - vertex_count_up_to_last_polygon; - - return two; - } - - - std::ostream& operator << (std::ostream& outs, - const Environment& environment_temp) - { - outs << "//Environment Model" << std::endl; - outs << "//Outer Boundary" << std::endl << environment_temp[0]; - for(unsigned i=1; i<=environment_temp.h(); i++){ - outs << "//Hole" << std::endl << environment_temp[i]; - } - //outs << "//EOF marker"; - return outs; - } - - - //Guards - - - Guards::Guards(const std::string& filename) - { - std::ifstream fin(filename.c_str()); - //if(fin.fail()) { std::cerr << "\x1b[5;31m" << "Input file - //opening failed." << "\x1b[0m\n" << "\a \n"; exit(1);} - assert( !fin.fail() ); - - //Temp vars for numbers to be read from file. - double x_temp, y_temp; - - //Skip comments - while( fin.peek() == '/' ) - fin.ignore(200,'\n'); - - //Read positions. - while (1){ - fin >> x_temp >> y_temp; - if( fin.eof() ) - { fin.close(); return; } - positions_.push_back( Point(x_temp, y_temp) ); - //Skip to next line. - fin.ignore(1); - //Skip comments - while( fin.peek() == '/' ) - fin.ignore(200,'\n'); - } - } - - - bool Guards::are_lex_ordered() const - { - //if more than one guard. - if(positions_.size() > 1) - for(unsigned i=0; i positions_[i+1]) - return false; - return true; - } - - - bool Guards::noncolocated(double epsilon) const - { - for(unsigned i=0; i 0 ); - - double running_max=0; - for(unsigned i=0; i running_max ) - running_max = distance( (*this)[i] , (*this)[j] ); - }} - return running_max; - } - - - Bounding_Box Guards::bbox() const - { - //Precondition: nonempty Guard set - assert( positions_.size() > 0 ); - - Bounding_Box bounding_box; - double x_min=positions_[0].x(), x_max=positions_[0].x(), - y_min=positions_[0].y(), y_max=positions_[0].y(); - for(unsigned i = 1; i < positions_.size(); i++){ - if(x_min > positions_[i].x()) { x_min=positions_[i].x(); } - if(x_max < positions_[i].x()) { x_max=positions_[i].x(); } - if(y_min > positions_[i].y()) { y_min=positions_[i].y(); } - if(y_max < positions_[i].y()) { y_max=positions_[i].y(); } - } - bounding_box.x_min=x_min; bounding_box.x_max=x_max; - bounding_box.y_min=y_min; bounding_box.y_max=y_max; - return bounding_box; - } - - - void Guards::write_to_file(const std::string& filename, - int fios_precision_temp) - { - assert( fios_precision_temp >= 1 ); - - std::ofstream fout( filename.c_str() ); - //fout.open( filename.c_str() ); //Alternatives. - //fout << *this; - fout.setf(std::ios::fixed); - fout.setf(std::ios::showpoint); - fout.precision(fios_precision_temp); - fout << "//Guard Positions" << std::endl; - for(unsigned i=0; i epsilon - and distance( observer , point2 ) > epsilon - and distance( observer , point3 ) > epsilon - //Test whether there is a spike with point2 as the tip - and ( ( distance(observer,point2) - >= distance(observer,point1) - and distance(observer,point2) - >= distance(observer,point3) ) - or ( distance(observer,point2) - <= distance(observer,point1) - and distance(observer,point2) - <= distance(observer,point3) ) ) - //and the pike is sufficiently sharp, - and std::max( distance( Ray(observer, point1), point2 ), - distance( Ray(observer, point3), point2 ) ) - <= epsilon - ); - //Formerly used - //std::fabs( Polygon(point1, point2, point3).area() ) < epsilon - } - - - void Visibility_Polygon::chop_spikes_at_back(const Point& observer, - double epsilon) - { - //Eliminate "special case" vertices of the visibility polygon. - //While the top three vertices form a spike. - while( vertices_.size() >= 3 - and is_spike( observer, - vertices_[vertices_.size()-3], - vertices_[vertices_.size()-2], - vertices_[vertices_.size()-1], epsilon ) ){ - vertices_[vertices_.size()-2] = vertices_[vertices_.size()-1]; - vertices_.pop_back(); - } - } - - - void Visibility_Polygon::chop_spikes_at_wrap_around(const Point& observer, - double epsilon) - { - //Eliminate "special case" vertices of the visibility polygon at - //wrap-around. While the there's a spike at the wrap-around, - while( vertices_.size() >= 3 - and is_spike( observer, - vertices_[vertices_.size()-2], - vertices_[vertices_.size()-1], - vertices_[0], epsilon ) ){ - //Chop off the tip of the spike. - vertices_.pop_back(); - } - } - - - void Visibility_Polygon::chop_spikes(const Point& observer, - double epsilon) - { - std::set spike_tips; - std::vector vertices_temp; - //Middle point is potentially the tip of a spike - for(unsigned i=0; i::iterator& - active_edge) - { - std::cout << " current_vertex [x y bearing range is_first] = [" - << current_vertex.x() << " " - << current_vertex.y() << " " - << current_vertex.bearing() << " " - << current_vertex.range() << " " - << current_vertex.is_first << "]" << std::endl; - std::cout << "1st point of current_vertex's edge [x y bearing range] = [" - << (current_vertex.incident_edge->first).x() << " " - << (current_vertex.incident_edge->first).y() << " " - << (current_vertex.incident_edge->first).bearing() << " " - << (current_vertex.incident_edge->first).range() << "]" - << std::endl; - std::cout << "2nd point of current_vertex's edge [x y bearing range] = [" - << (current_vertex.incident_edge->second).x() << " " - << (current_vertex.incident_edge->second).y() << " " - << (current_vertex.incident_edge->second).bearing() << " " - << (current_vertex.incident_edge->second).range() << "]" - << std::endl; - std::cout << " 1st point of active_edge [x y bearing range] = [" - << (active_edge->first).x() << " " - << (active_edge->first).y() << " " - << (active_edge->first).bearing() << " " - << (active_edge->first).range() << "]" << std::endl; - std::cout << " 2nd point of active_edge [x y bearing range] = [" - << (active_edge->second).x() << " " - << (active_edge->second).y() << " " - << (active_edge->second).bearing() << " " - << (active_edge->second).range() << "]" << std::endl; - } - - - Visibility_Polygon::Visibility_Polygon(const Point& observer, - const Environment& environment_temp, - double epsilon) - : observer_(observer) - { - //Visibility polygon algorithm for environments with holes - //Radial line (AKA angular plane) sweep technique. - // - //Based on algorithms described in - // - //[1] "Automated Camera Layout to Satisfy Task-Specific and - //Floorplan-Specific Coverage Requirements" by Ugur Murat Erdem - //and Stan Scarloff, April 15, 2004 - //available at BUCS Technical Report Archive: - //http://www.cs.bu.edu/techreports/pdf/2004-015-camera-layout.pdf - // - //[2] "Art Gallery Theorems and Algorithms" by Joseph O'Rourke - // - //[3] "Visibility Algorithms in the Plane" by Ghosh - // - - //We define a k-point is a point seen on the other side of a - //visibility occluding corner. This name is appropriate because - //the vertical line in the letter "k" is like a line-of-sight past - //the corner of the "k". - - // - //Preconditions: - //(1) the Environment is epsilon-valid, - //(2) the Point observer is actually in the Environment - // environment_temp, - //(3) the guard has been epsilon-snapped to the boundary, followed - // by vertices of the environment (the order of the snapping - // is important). - // - //:WARNING: - //For efficiency, the assertions corresponding to these - //preconditions have been excluded. - // - //assert( environment_temp.is_valid(epsilon) ); - //assert( environment_temp.is_in_standard_form() ); - //assert( observer.in(environment_temp, epsilon) ); - - //true => data printed to terminal - //false => silent - const bool PRINTING_DEBUG_DATA = false; - - //The visibility polygon cannot have more vertices than the environment. - vertices_.reserve( environment_temp.n() ); - - // - //--------PREPROCESSING-------- - // - - //Construct a POLAR EDGE LIST from environment_temp's outer - //boundary and holes. During this construction, those edges are - //split which either (1) cross the ray emanating from the observer - //parallel to the x-axis (of world coords), or (2) contain the - //observer in their relative interior (w/in epsilon). Also, edges - //having first vertex bearing >= second vertex bearing are - //eliminated because they cannot possibly contribute to the - //visibility polygon. - std::list elp; - Polar_Point ppoint1, ppoint2; - Polar_Point split_bottom, split_top; - double t; - //If the observer is standing on the Enviroment boundary with its - //back to the wall, these will be the bearings of the next vertex - //to the right and to the left, respectively. - Angle right_wall_bearing; - Angle left_wall_bearing; - for(unsigned i=0; i<=environment_temp.h(); i++){ - for(unsigned j=0; j epsilon ){ - //Possible source of numerical instability? - t = ( observer.y() - ppoint2.y() ) - / ( ppoint1.y() - ppoint2.y() ); - //If edge crosses the ray emanating horizontal and right of - //the observer. - if( 0 < t and t < 1 and - observer.x() < t*ppoint1.x() + (1-t)*ppoint2.x() ){ - //If first point is above, omit edge because it runs - //'against the grain'. - if( ppoint1.y() > observer.y() ) - continue; - //Otherwise split the edge, making sure angles are assigned - //correctly on each side of the split point. - split_bottom = split_top - = Polar_Point( observer, - Point( t*ppoint1.x() + (1-t)*ppoint2.x(), - observer.y() ) ); - split_top.set_bearing( Angle(0.0) ); - split_bottom.set_bearing_to_2pi(); - elp.push_back( Polar_Edge( ppoint1 , split_bottom ) ); - elp.push_back( Polar_Edge( split_top , ppoint2 ) ); - continue; - } - //If the edge is not horizontal and doesn't cross the ray - //emanating horizontal and right of the observer. - else if( ppoint1.bearing() >= ppoint2.bearing() - and ppoint2.bearing() == Angle(0.0) - and ppoint1.bearing() > Angle(M_PI) ) - ppoint2.set_bearing_to_2pi(); - //Filter out edges which run 'against the grain'. - else if( ( ppoint1.bearing() == Angle(0,0) - and ppoint2.bearing() > Angle(M_PI) ) - or ppoint1.bearing() >= ppoint2.bearing() ) - continue; - elp.push_back( Polar_Edge( ppoint1, ppoint2 ) ); - continue; - } - //If edge is horizontal (w/in epsilon). - else{ - //Filter out edges which run 'against the grain'. - if( ppoint1.bearing() >= ppoint2.bearing() ) - continue; - elp.push_back( Polar_Edge( ppoint1, ppoint2 ) ); - } - }} - - //Construct a SORTED LIST, q1, OF VERTICES represented by - //Polar_Point_With_Edge_Info objects. A - //Polar_Point_With_Edge_Info is a derived class of Polar_Point - //which includes (1) a pointer to the corresponding edge - //(represented as a Polar_Edge) in the polar edge list elp, and - //(2) a boolean (is_first) which is true iff that vertex is the - //first Point of the respective edge (is_first == false => it's - //second Point). q1 is sorted according to lex. order of polar - //coordinates just as Polar_Points are, but with the additional - //requirement that if two vertices have equal polar coordinates, - //the vertex which is the first point of its respective edge is - //considered greater. q1 will serve as an event point queue for - //the radial sweep. - std::list q1; - Polar_Point_With_Edge_Info ppoint_wei1, ppoint_wei2; - std::list::iterator elp_iterator; - for(elp_iterator=elp.begin(); - elp_iterator!=elp.end(); - elp_iterator++){ - ppoint_wei1.set_polar_point( elp_iterator->first ); - ppoint_wei1.incident_edge = elp_iterator; - ppoint_wei1.is_first = true; - ppoint_wei2.set_polar_point( elp_iterator->second ); - ppoint_wei2.incident_edge = elp_iterator; - ppoint_wei2.is_first = false; - //If edge contains the observer, then adjust the bearing of - //the Polar_Point containing the observer. - if( distance(observer, ppoint_wei1) <= epsilon ){ - if( right_wall_bearing > left_wall_bearing ){ - ppoint_wei1.set_bearing( right_wall_bearing ); - (elp_iterator->first).set_bearing( right_wall_bearing ); - } - else{ - ppoint_wei1.set_bearing( Angle(0.0) ); - (elp_iterator->first).set_bearing( Angle(0.0) ); - } - } - else if( distance(observer, ppoint_wei2) <= epsilon ){ - if( right_wall_bearing > left_wall_bearing ){ - ppoint_wei2.set_bearing(right_wall_bearing); - (elp_iterator->second).set_bearing( right_wall_bearing ); - } - else{ - ppoint_wei2.set_bearing_to_2pi(); - (elp_iterator->second).set_bearing_to_2pi(); - } - } - q1.push_back(ppoint_wei1); - q1.push_back(ppoint_wei2); - } - //Put event point in correct order. - //STL list's sort method is a stable sort. - q1.sort(); - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[1;37;40m" - << "COMPUTING VISIBILITY POLYGON " << std::endl - << "for an observer located at [x y] = [" - << observer << "]" - << "\x1b[0m" - << std::endl << std::endl - << "\E[1;37;40m" <<"PREPROCESSING" << "\x1b[0m" - << std::endl << std::endl - << "q1 is" << std::endl; - std::list::iterator q1_itr; - for(q1_itr=q1.begin(); q1_itr!=q1.end(); q1_itr++){ - std::cout << "[x y bearing range is_first] = [" - << q1_itr->x() << " " - << q1_itr->y() << " " - << q1_itr->bearing() << " " - << q1_itr->range() << " " - << q1_itr->is_first << "]" - << std::endl; - } - } - - // - //-------PREPARE FOR MAIN LOOP------- - // - - //current_vertex is used to hold the event point (from q1) - //considered at iteration of the main loop. - Polar_Point_With_Edge_Info current_vertex; - //Note active_edge and e are not actually edges themselves, but - //iterators pointing to edges. active_edge keeps track of the - //current edge visibile during the sweep. e is an auxiliary - //variable used in calculation of k-points - std::list::iterator active_edge, e; - //More aux vars for computing k-points. - Polar_Point k; - double k_range; - Line_Segment xing; - - //Priority queue of edges, where higher priority indicates closer - //range to observer along current ray (of ray sweep). - Incident_Edge_Compare my_iec(observer, current_vertex, epsilon); - std::priority_queue::iterator, - std::vector::iterator>, - Incident_Edge_Compare> q2(my_iec); - - //Initialize main loop. - current_vertex = q1.front(); q1.pop_front(); - active_edge = current_vertex.incident_edge; - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[1;37;40m" - << "INITIALIZATION" - << "\x1b[0m" - << std::endl << std::endl - << "\x1b[35m" - << "Pop first vertex off q1" - << "\x1b[0m" - << ", set as current_vertex, \n" - << "and set active_edge to the corresponding " - << "incident edge." - << std::endl; - print_cv_and_ae(current_vertex, active_edge); - } - - //Insert e into q2 as long as it doesn't contain the - //observer. - if( distance(observer,active_edge->first) > epsilon - and distance(observer,active_edge->second) > epsilon ){ - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "Push current_vertex's edge onto q2." - << std::endl; - } - - q2.push(active_edge); - } - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[32m" - << "Add current_vertex to visibility polygon." - << "\x1b[0m" - << std::endl << std::endl - << "\E[1;37;40m" - << "MAIN LOOP" - << "\x1b[0m" - << std::endl; - } - - vertices_.push_back(current_vertex); - - //-------BEGIN MAIN LOOP-------// - // - //Perform radial sweep by sequentially considering each vertex - //(event point) in q1. - while( !q1.empty() ){ - - //Pop current_vertex from q1. - current_vertex = q1.front(); q1.pop_front(); - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\x1b[35m" - << "Pop next vertex off q1" << "\x1b[0m" - << " and set as current_vertex." - << std::endl; - print_cv_and_ae(current_vertex, active_edge); - } - - //---Handle Event Point--- - - //TYPE 1: current_vertex is the _second_vertex_ of active_edge. - if( current_vertex.incident_edge == active_edge - and !current_vertex.is_first ){ - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[36m" << "TYPE 1:" << "\x1b[0m" - << " current_vertex is the second vertex of active_edge." - << std::endl; - } - - if( !q1.empty() ){ - //If the next vertex in q1 is contiguous. - if( distance( current_vertex, q1.front() ) <= epsilon ){ - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "current_vertex is contiguous " - << "with the next vertex in q1." - << std::endl; - } - - continue; - } - } - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[32m" << "Add current_vertex to visibility polygon." - << "\x1b[0m" << std::endl; - } - - //Push current_vertex onto visibility polygon - vertices_.push_back( current_vertex ); - chop_spikes_at_back(observer, epsilon); - - while( !q2.empty() ){ - e = q2.top(); - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "Examine edge at top of q2." << std::endl - << "1st point of e [x y bearing range] = [" - << (e->first).x() << " " - << (e->first).y() << " " - << (e->first).bearing() << " " - << (e->first).range() << "]" << std::endl - << "2nd point of e [x y bearing range] = [" - << (e->second).x() << " " - << (e->second).y() << " " - << (e->second).bearing() << " " - << (e->second).range() << "]" << std::endl; - } - - //If the current_vertex bearing has not passed, in the - //lex. order sense, the bearing of the second point of the - //edge at the front of q2. - if( ( current_vertex.bearing().get() - <= e->second.bearing().get() ) - //For robustness. - and distance( Ray(observer, current_vertex.bearing()), - e->second ) >= epsilon - /* was - and std::min( distance(Ray(observer, current_vertex.bearing()), - e->second), - distance(Ray(observer, e->second.bearing()), - current_vertex) - ) >= epsilon - */ - ){ - //Find intersection point k of ray (through - //current_vertex) with edge e. - xing = intersection( Ray(observer, current_vertex.bearing()), - Line_Segment(e->first, - e->second), - epsilon ); - - //assert( xing.size() > 0 ); - - if( xing.size() > 0 ){ - k = Polar_Point( observer , xing.first() ); - } - else{ //Error contingency. - k = current_vertex; - e = current_vertex.incident_edge; - } - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[32m" - << "Add a type 1 k-point to visibility polygon." - << "\x1b[0m" << std::endl - << std::endl - << "Set active_edge to edge at top of q2." - << std::endl; - } - - //Push k onto the visibility polygon. - vertices_.push_back(k); - chop_spikes_at_back(observer, epsilon); - active_edge = e; - break; - } - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "Pop edge off top of q2." << std::endl; - } - - q2.pop(); - } - } //Close Type 1. - - //If current_vertex is the _first_vertex_ of its edge. - if( current_vertex.is_first ){ - //Find intersection point k of ray (through current_vertex) - //with active_edge. - xing = intersection( Ray(observer, current_vertex.bearing()), - Line_Segment(active_edge->first, - active_edge->second), - epsilon ); - if( xing.size() == 0 - or ( distance(active_edge->first, observer) <= epsilon - and active_edge->second.bearing() - <= current_vertex.bearing() ) - or active_edge->second < current_vertex ){ - k_range = INFINITY; - } - else{ - k = Polar_Point( observer , xing.first() ); - k_range = k.range(); - } - - //Incident edge of current_vertex. - e = current_vertex.incident_edge; - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << " k_range = " - << k_range - << " (range of active edge along " - << "bearing of current vertex)" << std::endl - << "current_vertex.range() = " - << current_vertex.range() << std::endl; - } - - //Insert e into q2 as long as it doesn't contain the - //observer. - if( distance(observer, e->first) > epsilon - and distance(observer, e->second) > epsilon ){ - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "Push current_vertex's edge onto q2." - << std::endl; - } - - q2.push(e); - } - - //TYPE 2: current_vertex is (1) a first vertex of some edge - //other than active_edge, and (2) that edge should not become - //the next active_edge. This happens, e.g., if that edge is - //(rangewise) in back along the current bearing. - if( k_range < current_vertex.range() ){ - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[36m" << "TYPE 2:" << "\x1b[0m" - << " current_vertex is" << std::endl - << "(1) a first vertex of some edge " - "other than active_edge, and" << std::endl - << "(2) that edge should not become " - << "the next active_edge." - << std::endl; - - } - - } //Close Type 2. - - //TYPE 3: current_vertex is (1) the first vertex of some edge - //other than active_edge, and (2) that edge should become the - //next active_edge. This happens, e.g., if that edge is - //(rangewise) in front along the current bearing. - if( k_range >= current_vertex.range() - ){ - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[36m" << "TYPE 3:" << "\x1b[0m" - << " current_vertex is" << std::endl - << "(1) the first vertex of some edge " - "other than active edge, and" << std::endl - << "(2) that edge should become " - << "the next active_edge." - << std::endl; - } - - //Push k onto the visibility polygon unless effectively - //contiguous with current_vertex. - if( xing.size() > 0 - //and k == k - and k_range != INFINITY - and distance(k, current_vertex) > epsilon - and distance(active_edge->first, observer) > epsilon - ){ - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[32m" - << "Add type 3 k-point to visibility polygon." - << "\x1b[0m" << std::endl; - } - - //Push k-point onto the visibility polygon. - vertices_.push_back(k); - chop_spikes_at_back(observer, epsilon); - } - - //Push current_vertex onto the visibility polygon. - vertices_.push_back(current_vertex); - chop_spikes_at_back(observer, epsilon); - //Set active_edge to edge of current_vertex. - active_edge = e; - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "\E[32m" << "Add current_vertex to visibility polygon." - << "\x1b[0m" << std::endl - << std::endl - << "Set active_edge to edge of current_vertex." - << std::endl; - } - - } //Close Type 3. - } - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "visibility polygon vertices so far are \n" - << Polygon(vertices_) << std::endl - << std::endl; - } - } // - // - //-------END MAIN LOOP-------// - - //The Visibility_Polygon should have a minimal representation - chop_spikes_at_wrap_around( observer , epsilon ); - eliminate_redundant_vertices( epsilon ); - chop_spikes( observer, epsilon ); - enforce_standard_form(); - - if(PRINTING_DEBUG_DATA){ - std::cout << std::endl - << "Final visibility polygon vertices are \n" - << Polygon(vertices_) << std::endl - << std::endl; - } - - } - Visibility_Polygon::Visibility_Polygon(const Point& observer, - const Polygon& polygon_temp, - double epsilon) - { - *this = Visibility_Polygon( observer, Environment(polygon_temp), epsilon ); - } - - - //Visibility_Graph - - - Visibility_Graph::Visibility_Graph( const Visibility_Graph& vg2 ) - { - n_ = vg2.n_; - vertex_counts_ = vg2.vertex_counts_; - - //Allocate adjacency matrix - adjacency_matrix_ = new bool*[n_]; - adjacency_matrix_[0] = new bool[n_*n_]; - for(unsigned i=1; i points, - const Environment& environment, - double epsilon) - { - n_ = points.size(); - - //fill vertex_counts_ - vertex_counts_.push_back( n_ ); - - //allocate a contiguous chunk of memory for adjacency_matrix_ - adjacency_matrix_ = new bool*[n_]; - adjacency_matrix_[0] = new bool[n_*n_]; - for(unsigned i=1; i. -*/ - -/** - * \mainpage - *
- * see also the VisiLibity Project Page - *
- * Authors: Karl J. Obermeyer - *
- * \section developers For Developers - * Coding Standards - *
- * \section release_notes Release Notes - * Current Functionality - *
    - *
  • visibility polygons in polygonal environments with holes
    • - *
  • visibility graphs
    • - *
  • shortest path planning for a point
    • - *
- */ - -#ifndef VISILIBITY_H -#define VISILIBITY_H - - -//Uncomment these lines when compiling under -//Microsoft Visual Studio -/* -#include -#define NAN std::numeric_limits::quiet_NaN() -#define INFINITY std::numeric_limits::infinity() -#define M_PI 3.141592653589793238462643 -#define and && -#define or || -*/ - -#include //math functions in std namespace -#include -#include //queue and priority_queue. -#include //priority queues with iteration, - //integrated keys -#include -#include //sorting, min, max, reverse -#include //rand and srand -#include //Unix time -#include //file I/O -#include -#include //C-string manipulation -#include //string class -#include //assertions - - -/// VisiLibity's sole namespace -namespace VisiLibity -{ - - //Fwd declaration of all classes and structs serves as index. - struct Bounding_Box; - class Point; - class Line_Segment; - class Angle; - class Ray; - class Polar_Point; - class Polyline; - class Polygon; - class Environment; - class Guards; - class Visibility_Polygon; - class Visibility_Graph; - - - /** \brief floating-point display precision. - * - * This is the default precision with which floating point - * numbers are displayed or written to files for classes with a - * write_to_file() method. - */ - const int FIOS_PRECISION = 10; - - - /** \brief get a uniform random sample from an (inclusive) interval - * on the real line - * - * \author Karl J. Obermeyer - * \param lower_bound lower bound of the real interval - * \param upper_bound upper bound of the real interval - * \pre \a lower_bound <= \a upper_bound - * \return a random sample from a uniform probability distribution - * on the real interval [\a lower_bound, \a upper_bound] - * \remarks Uses the Standard Library's rand() function. rand() - * should be seeded (only necessary once at the beginning of the - * program) using the command - * std::srand( std::time( NULL ) ); rand(); - * \warning performance degrades as upper_bound - lower_bound - * approaches RAND_MAX. - */ - double uniform_random_sample(double lower_bound, double upper_bound); - - - /** \brief rectangle with sides parallel to the x- and y-axes - * - * \author Karl J. Obermeyer - * Useful for enclosing other geometric objects. - */ - struct Bounding_Box { double x_min, x_max, y_min, y_max; }; - - - /// Point in the plane represented by Cartesian coordinates - class Point - { - public: - //Constructors - /** \brief default - * - * \remarks Data defaults to NAN so that checking whether the - * data are numbers can be used as a precondition in functions. - */ - Point() : x_(NAN) , y_(NAN) { } - /// costruct from raw coordinates - Point(double x_temp, double y_temp) - { x_=x_temp; y_=y_temp; } - //Accessors - /// get x coordinate - double x () const { return x_; } - /// get y coordinate - double y () const { return y_; } - /** \brief closest Point on \a line_segment_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers - * and \a line_segment_temp is nonempty - * \return the Point on \a line_segment_temp which is the smallest - * Euclidean distance from the calling Point - */ - Point projection_onto(const Line_Segment& line_segment_temp) const; - /** \brief closest Point on \a ray_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point and \a ray_temp data are numbers - * \return the Point on \a ray_temp which is the smallest - * Euclidean distance from the calling Point - */ - Point projection_onto(const Ray& ray_temp) const; - /** \brief closest Point on \a polyline_temp - * - * \pre the calling Point data are numbers and \a polyline_temp - * is nonempty - * \return the Point on \a polyline_temp which is the smallest - * Euclidean distance from the calling Point - */ - Point projection_onto(const Polyline& polyline_temp) const; - /** \brief closest vertex of \a polygon_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a polygon_temp - * is nonempty - * \return the vertex of \a polygon_temp which is the - * smallest Euclidean distance from the calling Point - */ - Point projection_onto_vertices_of(const Polygon& polygon_temp) const; - /** \brief closest vertex of \a environment_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a environment_temp - * is nonempty - * \return the vertex of \a environment_temp which is - * the smallest Euclidean distance from the calling Point - */ - Point projection_onto_vertices_of(const Environment& - enviroment_temp) const; - /** \brief closest Point on boundary of \a polygon_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a polygon_temp - * is nonempty - * \return the Point on the boundary of \a polygon_temp which is the - * smallest Euclidean distance from the calling Point - */ - Point projection_onto_boundary_of(const Polygon& polygon_temp) const; - /** \brief closest Point on boundary of \a environment_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a environment_temp - * is nonempty - * \return the Point on the boundary of \a environment_temp which is - * the smalles Euclidean distance from the calling Point - */ - Point projection_onto_boundary_of(const Environment& - enviroment_temp) const; - /** \brief true iff w/in \a epsilon of boundary of \a polygon_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a polygon_temp - * is nonempty - * \return true iff the calling Point is within Euclidean distance - * \a epsilon of \a polygon_temp 's boundary - * \remarks O(n) time complexity, where n is the number - * of vertices of \a polygon_temp - */ - bool on_boundary_of(const Polygon& polygon_temp, - double epsilon=0.0) const; - /** \brief true iff w/in \a epsilon of boundary of \a environment_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a environment_temp - * is nonempty - * \return true iff the calling Point is within Euclidean distance - * \a epsilon of \a environment_temp 's boundary - * \remarks O(n) time complexity, where n is the number - * of vertices of \a environment_temp - */ - bool on_boundary_of(const Environment& environment_temp, - double epsilon=0.0) const; - /** \brief true iff w/in \a epsilon of \a line_segment_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a line_segment_temp - * is nonempty - * \return true iff the calling Point is within distance - * \a epsilon of the (closed) Line_Segment \a line_segment_temp - */ - bool in(const Line_Segment& line_segment_temp, - double epsilon=0.0) const; - /** \brief true iff w/in \a epsilon of interior but greater than - * \a espilon away from endpoints of \a line_segment_temp - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a line_segment_temp - * is nonempty - * \return true iff the calling Point is within distance \a - * epsilon of \line_segment_temp, but distance (strictly) greater - * than epsilon from \a line_segment_temp 's endpoints. - */ - bool in_relative_interior_of(const Line_Segment& line_segment_temp, - double epsilon=0.0) const; - /** \brief true iff w/in \a epsilon of \a polygon_temp - * - * \author Karl J. Obermeyer - * - * \pre the calling Point data are numbers and \a polygon_temp is - * \a epsilon -simple. Test simplicity with - * Polygon::is_simple(epsilon) - * - * \return true iff the calling Point is a Euclidean distance no greater - * than \a epsilon from the (closed) Polygon (with vertices listed - * either cw or ccw) \a polygon_temp. - * \remarks O(n) time complexity, where n is the number of vertices - * in \a polygon_temp - */ - bool in(const Polygon& polygon_temp, - double epsilon=0.0) const; - /** \brief true iff w/in \a epsilon of \a environment_temp - * - * \author Karl J. Obermeyer - * - * \pre the calling Point data are numbers and \a environment_temp - * is nonempty and \a epsilon -valid. Test validity with - * Enviroment::is_valid(epsilon) - * - * \return true iff the calling Point is a Euclidean distance no greater - * than \a epsilon from the in the (closed) Environment \a environment_temp - * \remarks O(n) time complexity, where n is the number of - * vertices in \a environment_temp - */ - bool in(const Environment& environment_temp, - double epsilon=0.0) const; - /** \brief true iff w/in \a epsilon of some endpoint - * of \a line_segment_temp - * - * \pre the calling Point data are numbers and \a line_segment_temp - * is nonempty - * \return true iff calling Point is a Euclidean distance no greater - * than \a epsilon from some endpoint of \a line_segment_temp - */ - bool is_endpoint_of(const Line_Segment& line_segment_temp, - double epsilon=0.0) const; - //Mutators - /// change x coordinate - void set_x(double x_temp) { x_ = x_temp;} - /// change y coordinate - void set_y(double y_temp) { y_ = y_temp;} - /** \brief relocate to closest vertex if w/in \a epsilon of some - * vertex (of \a polygon_temp) - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a polygon_temp - * is nonempty - * \post If the calling Point was a Euclidean distance no greater - * than \a epsilon from any vertex of \a polygon_temp, then it - * will be repositioned to coincide with the closest such vertex - * \remarks O(n) time complexity, where n is the number of - * vertices in \a polygon_temp. - */ - void snap_to_vertices_of(const Polygon& polygon_temp, - double epsilon=0.0); - /** \brief relocate to closest vertex if w/in \a epsilon of some - * vertex (of \a environment_temp) - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a environment_temp - * is nonempty - * \post If the calling Point was a Euclidean distance no greater - * than \a epsilon from any vertex of \a environment_temp, then it - * will be repositioned to coincide with the closest such vertex - * \remarks O(n) time complexity, where n is the number of - * vertices in \a environment_temp. - */ - void snap_to_vertices_of(const Environment& environment_temp, - double epsilon=0.0); - /** \brief relocate to closest Point on boundary if w/in \a epsilon - * of the boundary (of \a polygon_temp) - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a polygon_temp - * is nonempty - * \post if the calling Point was a Euclidean distance no greater - * than \a epsilon from the boundary of \a polygon_temp, then it - * will be repositioned to it's projection onto that boundary - * \remarks O(n) time complexity, where n is the number of - * vertices in \a polygon_temp. - */ - void snap_to_boundary_of(const Polygon& polygon_temp, - double epsilon=0.0); - /** \brief relocate to closest Point on boundary if w/in \a epsilon - * of the boundary (of \a environment_temp) - * - * \author Karl J. Obermeyer - * \pre the calling Point data are numbers and \a environment_temp - * is nonempty - * \post if the calling Point was a Euclidean distance no greater - * than \a epsilon from the boundary of \a environment_temp, then it - * will be repositioned to it's projection onto that boundary - * \remarks O(n) time complexity, where n is the number of - * vertices in \a environment_temp. - */ - void snap_to_boundary_of(const Environment& environment_temp, - double epsilon=0.0); - protected: - double x_; - double y_; - }; - - - /** \brief True iff Points' coordinates are identical. - * - * \remarks NAN==NAN returns false, so if either point has - * not been assigned real number coordinates, they will not be == - */ - bool operator == (const Point& point1, const Point& point2); - /// True iff Points' coordinates are not identical. - bool operator != (const Point& point1, const Point& point2); - - - /** \brief compare lexicographic order of points - * - * For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or - * p1.x()==p2.x() and p1.y() (const Point& point1, const Point& point2); - /** \brief compare lexicographic order of points - * - * For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or - * p1.x()==p2.x() and p1.y()= (const Point& point1, const Point& point2); - /** \brief compare lexicographic order of points - * - * For Points p1 and p2, p1 < p2 iff either p1.x() < p2.x() or - * p1.x()==p2.x() and p1.y() 0 - * \return the first Point of the Line_Segment - * \remarks If size() == 1, then both first() and second() are valid - * and will return the same Point - */ - Point first() const; - /** \brief second endpoint - * - * \pre size() > 0 - * \return the second Point of the Line_Segment - * \remarks If size() == 1, then both first() and second() are valid - * and will return the same Point - */ - Point second() const; - /** \brief number of distinct endpoints - * - * \remarks - * size 0 => empty line segment; - * size 1 => degenerate (single point) line segment; - * size 2 => full-fledged (bona fide) line segment - */ - unsigned size() const { return size_; } - /** \brief midpoint - * - * \pre size() > 0 - */ - Point midpoint() const; - /** \brief Euclidean length - * - * \pre size() > 0 - */ - double length() const; - /** \brief true iff vertices in lex. order - * - * \pre size() > 0 - * \return true iff vertices are listed beginning with the vertex - * which is lexicographically smallest (lowest x, then lowest y) - * \remarks lex. comparison is very sensitive to perturbations if - * two Points nearly define a line parallel to one of the axes - */ - bool is_in_standard_form() const; - //Mutators - /// assignment operator - Line_Segment& operator = (const Line_Segment& line_segment_temp); - /** \brief set first endpoint - * - * \remarks if \a point_temp is w/in a distance \a epsilon of an existing - * endpoint, the coordinates of \a point_temp are used and size is set to - * 1 as appropriate - */ - void set_first(const Point& point_temp, double epsilon=0.0); - /** \brief set second endpoint - * - * \remarks if \a point_temp is w/in a distance \a epsilon of an existing - * endpoint, the coordinates of \a point_temp are used and size is set to - * 1 as appropriate - */ - void set_second(const Point& point_temp, double epsilon=0.0); - /** \brief reverse order of endpoints - * - * \post order of endpoints is reversed. - */ - void reverse(); - /** \brief enforce that lex. smallest endpoint first - * - * \post the lexicographically smallest endpoint (lowest x, then lowest y) - * is first - * \remarks lex. comparison is very sensitive to perturbations if - * two Points nearly define a line parallel to one of the axes - */ - void enforce_standard_form(); - /// erase both endpoints and set line segment empty (size 0) - void clear(); - /// destructor - virtual ~Line_Segment(); - protected: - //Pointer to dynamic array of endpoints. - Point *endpoints_; - //See size() comments. - unsigned size_; - }; - - - /** \brief true iff endpoint coordinates are exactly equal, but - * false if either Line_Segment has size 0 - * - * \remarks respects ordering of vertices, i.e., even if the line segments - * overlap exactly, they are not considered == unless the orientations are - * the same - */ - bool operator == (const Line_Segment& line_segment1, - const Line_Segment& line_segment2); - /// true iff endpoint coordinates are not == - bool operator != (const Line_Segment& line_segment1, - const Line_Segment& line_segment2); - - - /** \brief true iff line segments' endpoints match up w/in a (closed) - * \a epsilon ball of each other, but false if either - * Line_Segment has size 0 - * - * \author Karl J. Obermeyer - * \remarks this function will return true even if it has to flip - * the orientation of one of the segments to get the vertices to - * match up - */ - bool equivalent(Line_Segment line_segment1, - Line_Segment line_segment2, double epsilon=0); - - - /** \brief Euclidean distance between Line_Segments - * - * \author Karl J. Obermeyer - * \pre \a line_segment1.size() > 0 and \a line_segment2.size() > 0 - */ - double distance(const Line_Segment& line_segment1, - const Line_Segment& line_segment2); - - - /** \brief Euclidean distance between a Line_Segment and the - * boundary of a Polygon - * - * \author Karl J. Obermeyer - * \pre \a line_segment.size() > 0 and \a polygon.n() > 0 - */ - double boundary_distance(const Line_Segment& line_segment, - const Polygon& polygon); - - - /** \brief Euclidean distance between a Line_Segment and the - * boundary of a Polygon - * - * \author Karl J. Obermeyer - * \pre \a line_segment.size() > 0 and \a polygon.n() > 0 - */ - double boundary_distance(const Polygon& polygon, - const Line_Segment& line_segment); - - - /** \brief true iff the Euclidean distance between Line_Segments is - * no greater than \a epsilon, false if either line segment - * has size 0 - * - * \author Karl J. Obermeyer - */ - bool intersect(const Line_Segment& line_segment1, - const Line_Segment& line_segment2, - double epsilon=0.0); - - - /** \brief true iff line segments intersect properly w/in epsilon, - * false if either line segment has size 0 - * - * \author Karl J. Obermeyer - * \return true iff Line_Segments intersect exactly at a single - * point in their relative interiors. For robustness, here the - * relative interior of a Line_Segment is consider to be any Point - * in the Line_Segment which is a distance greater than \a epsilon - * from both endpoints. - */ - bool intersect_proper(const Line_Segment& line_segment1, - const Line_Segment& line_segment2, - double epsilon=0.0); - - - /** \brief intersection of Line_Segments - * - * \author Karl J. Obermeyer - * \return a Line_Segment of size 0, 1, or 2 - * \remarks size 0 results if the distance (or at least the - * floating-point computed distance) between line_segment1 and - * line_segment2 is (strictly) greater than epsilon. size 1 results - * if the segments intersect poperly, form a T intersection, or -- - * intersection. size 2 results when two or more endpoints are a - * Euclidean distance no greater than \a epsilon from the opposite - * segment, and the overlap of the segments has a length greater - * than \a epsilon. - */ - Line_Segment intersection(const Line_Segment& line_segment1, - const Line_Segment& line_segment2, - double epsilon=0.0); - - - /// print a Line_Segment - std::ostream& operator << (std::ostream& outs, - const Line_Segment& line_segment_temp); - - - /** \brief angle in radians represented by a value in - * the interval [0,2*M_PI] - * - * \remarks the intended interpretation is that angles 0 and 2*M_PI - * correspond to the positive x-axis of the coordinate system - */ - class Angle - { - public: - //Constructors - /** \brief default - * - * \remarks data defaults to NAN so that checking whether the - * data are numbers can be used as a precondition in functions - */ - Angle() : angle_radians_(NAN) { } - /// construct from real value, mod into interval [0, 2*M_PI) - Angle(double data_temp); - /** \brief construct using 4 quadrant inverse tangent into [0, 2*M_PI), - * where 0 points along the x-axis - */ - Angle(double rise_temp, double run_temp); - //Accessors - /// get radians - double get() const { return angle_radians_; } - //Mutators - /// set angle, mod into interval [0, 2*PI) - void set(double data_temp); - /** \brief set angle data to 2*M_PI - * - * \remarks sometimes it is necessary to set the angle value to - * 2*M_PI instead of 0, so that the lex. inequalities behave - * appropriately during a radial line sweep - */ - void set_to_2pi() { angle_radians_=2*M_PI; } - /// set to new random angle in [0, 2*M_PI) - void randomize(); - private: - double angle_radians_; - }; - - - /// compare angle radians - bool operator == (const Angle& angle1, const Angle& angle2); - /// compare angle radians - bool operator != (const Angle& angle1, const Angle& angle2); - - - /// compare angle radians - bool operator > (const Angle& angle1, const Angle& angle2); - /// compare angle radians - bool operator < (const Angle& angle1, const Angle& angle2); - /// compare angle radians - bool operator >= (const Angle& angle1, const Angle& angle2); - /// compare angle radians - bool operator <= (const Angle& angle1, const Angle& angle2); - - - /// add angles' radians and mod into [0, 2*M_PI) - Angle operator + (const Angle& angle1, const Angle& angle2); - /// subtract angles' radians and mod into [0, 2*M_PI) - Angle operator - (const Angle& angle1, const Angle& angle2); - - - /** \brief geodesic distance in radians between Angles - * - * \author Karl J. Obermeyer - * \pre \a angle1 and \a angle2 data are numbers - */ - double geodesic_distance(const Angle& angle1, const Angle& angle2); - - - /** \brief 1.0 => geodesic path from angle1 to angle2 - * is couterclockwise, -1.0 => clockwise - * - * \author Karl J. Obermeyer - * \pre \a angle1 and \a angle2 data are numbers - */ - double geodesic_direction(const Angle& angle1, const Angle& angle2); - - - /// print Angle - std::ostream& operator << (std::ostream& outs, const Angle& angle_temp); - - - /** \brief Point in the plane packaged together with polar - * coordinates w.r.t. specified origin - * - * The origin of the polar coordinate system is stored with the - * Polar_Point (in \a polar_origin_) and bearing is measured ccw from the - * positive x-axis. - * \remarks used, e.g., for radial line sweeps - */ - class Polar_Point : public Point - { - public: - //Constructors - /** \brief default - * - * \remarks Data defaults to NAN so that checking whether the - * data are numbers can be used as a precondition in functions. - */ - Polar_Point() : Point(), range_(NAN), bearing_(NAN) { } - /** \brief construct from (Cartesian) Points - * - * \pre member data of \a polar_origin_temp and \a point_temp have - * been assigned (numbers) - * \param polar_origin_temp the origin of the polar coordinate system - * \param point_temp the point to be represented - * \remarks if polar_origin_temp == point_temp, the default - * bearing is Angle(0.0) - */ - Polar_Point(const Point& polar_origin_temp, - const Point& point_temp, - double epsilon=0.0); - //Accessors - /** \brief origin of the polar coordinate system in which the point is - * represented - */ - Point polar_origin() const { return polar_origin_; } - /// Euclidean distance from the point represented to the origin of - /// the polar coordinate system - double range() const { return range_; } - /// bearing from polar origin w.r.t. direction parallel to x-axis - Angle bearing() const { return bearing_; } - //Mutators - /** \brief set the origin of the polar coordinate system - * - * \remarks x and y held constant, bearing and range modified - * accordingly - */ - void set_polar_origin(const Point& polar_origin_temp); - /** \brief set x - * - * \remarks polar_origin held constant, bearing and range modified - * accordingly - */ - void set_x(double x_temp); - /** \brief set y - * - * \remarks polar_origin held constant, bearing and range modified - * accordingly - */ - void set_y(double y_temp); - /** \brief set range - * - * \remarks polar_origin held constant, x and y modified - * accordingly - */ - void set_range(double range_temp); - /** \brief set bearing - * - * \remarks polar_origin and range held constant, x and y modified - * accordingly - */ - void set_bearing(const Angle& bearing_temp); - /** \brief set bearing Angle data to 2*M_PI - * - * \remarks Special function for use in computations involving a - * radial line sweep; sometimes it is necessary to set the angle - * value to 2*PI instead of 0, so that the lex. inequalities - * behave appropriately - */ - void set_bearing_to_2pi() { bearing_.set_to_2pi(); } - protected: - //Origin of the polar coordinate system in world coordinates. - Point polar_origin_; - //Polar coordinates where radius always positive, and angle - //measured ccw from the world coordinate system's x-axis. - double range_; - Angle bearing_; - }; - - - /** \brief compare member data - * - * \remarks returns false if any member data are NaN - */ - bool operator == (const Polar_Point& polar_point1, - const Polar_Point& polar_point2); - bool operator != (const Polar_Point& polar_point1, - const Polar_Point& polar_point2); - - - /** \brief compare according to polar lexicographic order - * (smaller bearing, then smaller range) - * - * false if any member data have not been assigned (numbers) - * \remarks lex. comparison is very sensitive to perturbations if - * two Points nearly define a radial line - */ - bool operator > (const Polar_Point& polar_point1, - const Polar_Point& polar_point2); - /** \brief compare according to polar lexicographic order - * (smaller bearing, then smaller range) - * - * false if any member data have not been assigned (numbers) - * \remarks lex. comparison is very sensitive to perturbations if - * two Points nearly define a radial line - */ - bool operator < (const Polar_Point& polar_point1, - const Polar_Point& polar_point2); - /** \brief compare according to polar lexicographic order - * (smaller bearing, then smaller range) - * - * false if any member data have not been assigned (numbers) - * \remarks lex. comparison is very sensitive to perturbations if - * two Points nearly define a radial line - */ - bool operator >= (const Polar_Point& polar_point1, - const Polar_Point& polar_point2); - /** \brief compare according to polar lexicographic order - * (smaller bearing, then smaller range) - * - * false if any member data have not been assigned (numbers) - * \remarks lex. comparison is very sensitive to perturbations if - * two Points nearly define a radial line - */ - bool operator <= (const Polar_Point& polar_point1, - const Polar_Point& polar_point2); - - - /// print Polar_Point - std::ostream& operator << (std::ostream& outs, - const Polar_Point& polar_point_temp); - - - /// ray in the plane represented by base Point and bearing Angle - class Ray - { - public: - //Constructors - /** \brief default - * - * \remarks data defaults to NAN so that checking whether the data - * are numbers can be used as a precondition in functions - */ - Ray() { } - /// construct ray emanating from \a base_point_temp in the direction - /// \a bearing_temp - Ray(Point base_point_temp, Angle bearing_temp) : - base_point_(base_point_temp) , bearing_(bearing_temp) {} - /// construct ray emanating from \a base_point_temp towards - /// \a bearing_point - Ray(Point base_point_temp, Point bearing_point); - //Accessors - /// get base point - Point base_point() const { return base_point_; } - /// get bearing - Angle bearing() const { return bearing_; } - //Mutators - /// set base point - void set_base_point(const Point& point_temp) - { base_point_ = point_temp; } - /// set bearing - void set_bearing(const Angle& angle_temp) - { bearing_ = angle_temp; } - private: - Point base_point_; - Angle bearing_; - }; - - - /** \brief compare member data - * - * \remarks returns false if any member data are NaN - */ - bool operator == (const Ray& ray1, - const Ray& ray2); - /** \brief compare member data - * - * \remarks negation of == - */ - bool operator != (const Ray& ray1, - const Ray& ray2); - - - /** \brief compute the intersection of a Line_Segment with a Ray - * - * \author Karl J. Obermeyer - * \pre member data of \a ray_temp has been assigned (numbers) and - * \a line_segment_temp has size greater than 0 - * \remarks as a convention, if the intersection has positive - * length, the Line_Segment returned has the first point closest to - * the Ray's base point - */ - Line_Segment intersection(const Ray ray_temp, - const Line_Segment& line_segment_temp, - double epsilon=0.0); - /** \brief compute the intersection of a Line_Segment with a Ray - * - * \author Karl J. Obermeyer - * \pre member data of \a ray_temp has been assigned (numbers) and - * \a line_segment_temp has size greater than 0 - * \remarks as a convention, if the intersection has positive - * length, the Line_Segment returned has the first point closest to - * the Ray's base point - */ - Line_Segment intersection(const Line_Segment& line_segment_temp, - const Ray& ray_temp, - double epsilon=0.0); - - - ///oriented polyline in the plane represented by list of vertices - class Polyline - { - public: - friend class Point; - //Constructors - /// default to empty - Polyline() { } - /// construct from vector of vertices - Polyline(const std::vector& vertices_temp) - { vertices_ = vertices_temp; } - //Accessors - /** \brief raw access - * - * \remarks for efficiency, no bounds checks; usually trying to - * access out of bounds causes a bus error - */ - Point operator [] (unsigned i) const - { return vertices_[i]; } - /// vertex count - unsigned size() const - { return vertices_.size(); } - /// Euclidean length of the Polyline - double length() const; - /** \brief Euclidean diameter - * - * \pre Polyline has greater than 0 vertices - * \return the maximum Euclidean distance between all pairs of - * vertices - * \remarks time complexity O(n^2), where n is the number of - * vertices representing the Polyline - */ - double diameter() const; - //a box which fits snugly around the Polyline - Bounding_Box bbox() const; - //Mutators - /** \brief raw access - * - * \remarks for efficiency, no bounds checks; usually trying to - * access out of bounds causes a bus error - */ - Point& operator [] (unsigned i) - { return vertices_[i]; } - /// erase all points - void clear() - { vertices_.clear(); } - /// add a vertex to the back (end) of the list - void push_back(const Point& point_temp) - { vertices_.push_back(point_temp); } - /// delete a vertex to the back (end) of the list - void pop_back() - { vertices_.pop_back(); } - /// reset the whole list of vertices at once - void set_vertices(const std::vector& vertices_temp) - { vertices_ = vertices_temp; } - /** \brief eliminates vertices which are (\a epsilon) - colinear - * with their respective neighbors - * - * \author Karl J. Obermeyer - * \post the Euclidean distance between each vertex and the line - * segment connecting its neighbors is at least \a epsilon - * \remarks time complexity O(n), where n is the number of - * vertices representing the Polyline. - */ - void eliminate_redundant_vertices(double epsilon=0.0); - //Reduce number of vertices in representation... - //void smooth(double epsilon); - /// reverse order of vertices - void reverse(); - /// append the points from another polyline - void append( const Polyline& polyline ); - private: - std::vector vertices_; - }; - - - //print Polyline - std::ostream& operator << (std::ostream& outs, - const Polyline& polyline_temp); - - - /** \brief simple polygon in the plane represented by list of vertices - * - * Simple here means non-self-intersecting. More precisely, edges - * should not (i) intersect with an edge not adjacent to it, nor - * (ii) intersect at more than one Point with an adjacent edge. - * \remarks vertices may be listed cw or ccw - */ - class Polygon - { - public: - friend class Point; - //Constructors - ///default to empty - Polygon() { } - /** \brief construct from *.polygon file - * - * \author Karl J. Obermeyer - * \remarks for efficiency, simplicity check not called here - */ - Polygon(const std::string& filename); - /** \brief construct from vector of vertices - * - * \remarks for efficiency, simplicity check not called here - */ - Polygon(const std::vector& vertices_temp); - /** \brief construct triangle from 3 Points - * - * \remarks for efficiency, simplicity check not called here - */ - Polygon(const Point& point0, const Point& point1, const Point& point2); - //Accessors - /** \brief access with automatic wrap-around in forward direction - * - * \remarks For efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - const Point& operator [] (unsigned i) const - { return vertices_[i % vertices_.size()]; } - /** \brief vertex count - * - * \remarks O(1) time complexity - */ - unsigned n() const { return vertices_.size(); } - /** \brief reflex vertex count (nonconvex vertices) - * - * \author Karl J. Obermeyer - * \remarks Works regardless of polygon orientation (ccw vs cw), - * but assumes no redundant vertices. Time complexity O(n), where - * n is the number of vertices representing the Polygon - */ - unsigned r() const; - /** \brief true iff Polygon is (\a epsilon) simple - * - * \author Karl J. Obermeyer - * - * \remarks A Polygon is considered \a epsilon -simple iff (i) the - * Euclidean distance between nonadjacent edges is no greater than - * \a epsilon, (ii) adjacent edges intersect only at their single - * shared Point, (iii) and it has at least 3 vertices. One - * consequence of these conditions is that there can be no - * redundant vertices. - */ - bool is_simple(double epsilon=0.0) const; - /** \brief true iff lexicographically smallest vertex is first in - * the list of vertices representing the Polygon - * - * \author Karl J. Obermeyer - * \remarks lex. comparison is very sensitive to perturbations if - * two Points nearly define a line parallel to one of the axes - */ - bool is_in_standard_form() const; - /// perimeter length - double boundary_length() const; - /** oriented area of the Polygon - * - * \author Karl J. Obermeyer - * \pre Polygon is simple, but for efficiency simplicity is not asserted. - * area > 0 => vertices listed ccw, - * area < 0 => cw - * \remarks O(n) time complexity, where n is the number - * of vertices representing the Polygon - */ - double area() const; - /** \brief Polygon's centroid (center of mass) - * - * \author Karl J. Obermeyer - * \pre Polygon has greater than 0 vertices and is simple, - * but for efficiency simplicity is not asserted - */ - Point centroid() const; - /** \brief Euclidean diameter - * - * \pre Polygon has greater than 0 vertices - * \return maximum Euclidean distance between all pairs of - * vertices - * \remarks time complexity O(n^2), where n is the number of - * vertices representing the Polygon - */ - double diameter() const; - /** \brief box which fits snugly around the Polygon - * - * \author Karl J. Obermeyer - * \pre Polygon has greater than 0 vertices - */ - Bounding_Box bbox() const; - // Returns a vector of n pts randomly situated in the polygon. - std::vector random_points(const unsigned& count, - double epsilon=0.0) const; - /** \brief write list of vertices to *.polygon file - * - * \author Karl J. Obermeyer - * Uses intuitive human and computer readable decimal format with - * display precision \a fios_precision_temp. - * \pre \a fios_precision_temp >=1 - */ - void write_to_file(const std::string& filename, - int fios_precision_temp=FIOS_PRECISION); - //Mutators - /** \brief access with automatic wrap-around in forward direction - * - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - Point& operator [] (unsigned i) { return vertices_[i % vertices_.size()]; } - /// set vertices using STL vector of Points - void set_vertices(const std::vector& vertices_temp) - { vertices_ = vertices_temp; } - /// push a Point onto the back of the vertex list - void push_back(const Point& vertex_temp ) - { vertices_.push_back( vertex_temp ); } - /// erase all vertices - void clear() - { vertices_.clear(); } - /** \brief enforces that the lexicographically smallest vertex is first - * in the list of vertices representing the Polygon - * - * \author Karl J. Obermeyer - * \remarks O(n) time complexity, where n is the number of - * vertices representing the Polygon. Lex. comparison is very - * sensitive to perturbations if two Points nearly define a line - * parallel to one of the axes - */ - void enforce_standard_form(); - /** \brief eliminates vertices which are (\a epsilon) - colinear - * with their respective neighbors - * - * \author Karl J. Obermeyer - * \post the Euclidean distance between each vertex and the line - * segment connecting its neighbors is at least \a epsilon, and the - * Polygon is in standard form - * \remarks time complexity O(n), where n is the number of - * vertices representing the the Polygon - */ - void eliminate_redundant_vertices(double epsilon=0.0); - /** \brief reverse (cyclic) order of vertices - * - * \remarks vertex first in list is held first - */ - void reverse(); - protected: - std::vector vertices_; - }; - - - /** \brief true iff vertex lists are identical, but false if either - * Polygon has size 0 - * - * \remarks returns false if either Polygon has size 0 - * \remarks O(n) time complexity - */ - bool operator == (Polygon polygon1, Polygon polygon2); - bool operator != (Polygon polygon1, Polygon polygon2); - /** \brief true iff the Polygon's vertices match up w/in a (closed) - * epsilon ball of each other, but false if either Polygon - * has size 0 - * - * Respects number, ordering, and orientation of vertices, i.e., - * even if the (conceptual) polygons represented by two Polygons are - * identical, they are not considered \a epsilon - equivalent unless - * the number of vertices is the same, the orientations are the same - * (cw vs. ccw list), and the Points of the vertex lists match up - * within epsilon. This function does attempt to match the polygons - * for all possible cyclic permutations, hence the quadratic time - * complexity. - * \author Karl J. Obermeyer - * \remarks O(n^2) time complexity, where n is the number of - * vertices representing the polygon - */ - bool equivalent(Polygon polygon1, Polygon polygon2, - double epsilon=0.0); - - - /** \brief Euclidean distance between Polygons' boundaries - * - * \author Karl J. Obermeyer - * \pre \a polygon1 and \a polygon2 each have greater than 0 vertices - */ - double boundary_distance( const Polygon& polygon1, - const Polygon& polygon2 ); - - - //print Polygon - std::ostream& operator << (std::ostream& outs, - const Polygon& polygon_temp); - - - /** \brief environment represented by simple polygonal outer boundary - * with simple polygonal holes - * - * \remarks For methods to work correctly, the outer boundary vertices must - * be listed ccw and the hole vertices cw - */ - class Environment - { - public: - friend class Point; - //Constructors - /// default to empty - Environment() { } - /** \brief construct Environment without holes - * - * \remarks time complexity O(n), where n is the number of vertices - * representing the Environment - */ - Environment(const Polygon& polygon_temp) - { outer_boundary_=polygon_temp; update_flattened_index_key(); } - /** \brief construct Environment with holes from STL vector of Polygons - * - * the first Polygon in the vector becomes the outer boundary, - * the rest become the holes - * \remarks time complexity O(n), where n is the number of vertices - * representing the Environment - */ - Environment(const std::vector& polygons); - /** construct from *.environment file. - * - * \author Karl J. Obermeyer - * \remarks time complexity O(n), where n is the number of vertices - * representing the Environment - */ - Environment(const std::string& filename); - //Accessors - /** \brief raw access to Polygons - * - * An argument of 0 accesses the outer boundary, 1 and above - * access the holes. - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - const Polygon& operator [] (unsigned i) const - { if(i==0){return outer_boundary_;} else{return holes_[i-1];} } - /** \brief raw access to vertices via flattened index - * - * By flattened index is intended the label given to a vertex if - * you were to label all the vertices from 0 to n-1 (where n is - * the number of vertices representing the Environment) starting - * with the first vertex of the outer boundary and progressing in - * order through all the remaining vertices of the outer boundary - * and holes. - * - * \remarks Time complexity O(1). For efficiency, no bounds - * check; usually trying to access out of bounds causes a bus - * error. - */ - const Point& operator () (unsigned k) const; - /// hole count - unsigned h() const { return holes_.size(); } - /** \brief vertex count - * - * \remarks time complexity O(h) - */ - unsigned n() const; - /** \brief total reflex vertex count (nonconvex vertices) - * - * \author Karl J. Obermeyer - * \remarks time complexity O(n), where n is the number of - * vertices representing the Environment - */ - unsigned r() const; - /** \brief true iff lexicographically smallest vertex is first in - * each list of vertices representing a Polygon of the - * Environment - * - * \author Karl J. Obermeyer - * \remarks lex. comparison is very sensitive to perturbations if - * two Points nearly define a line parallel to one of the axes - */ - bool is_in_standard_form() const; - /** \brief true iff \a epsilon -valid - * - * \a epsilon -valid means (i) the outer boundary and holes are - * pairwise \a epsilon -disjoint (no two features should come - * within \a epsilon of each other) simple polygons, (ii) outer - * boundary is oriented ccw, and (iii) holes are oriented cw. - * - * \author Karl J. Obermeyer - * - * \pre Environment has greater than 2 vertices - * (otherwise it can't even have nonzero area) - * \remarks time complexity O(h^2*n^2), where h is the number of - * holes and n is the number of vertices representing the - * Environment - */ - bool is_valid(double epsilon=0.0) const; - /** \brief sum of perimeter lengths of outer boundary and holes - * - * \author Karl J. Obermeyer - * \remarks O(n) time complexity, where n is the number of - * vertices representing the Environment - */ - double boundary_length() const; - /** \brief (obstacle/hole free) area of the Environment - * - * \author Karl J. Obermeyer - * \remarks O(n) time complexity, where n is the number of - * vertices representing the Environment - */ - double area() const; - /** \brief Euclidean diameter - * - * \author Karl J. Obermeyer - * \pre Environment has greater than 0 vertices - * \return maximum Euclidean distance between all pairs of - * vertices - * \remarks time complexity O(n^2), where n is the number of - * vertices representing the Environment - */ - double diameter() const { return outer_boundary_.diameter(); } - /** \brief box which fits snugly around the Environment - * - * \author Karl J. Obermeyer - * \pre Environment has greater than 0 vertices - */ - Bounding_Box bbox() const { return outer_boundary_.bbox(); } - /** \brief get STL vector of \a count Points randomly situated - * within \a epsilon of the Environment - * - * \author Karl J. Obermeyer - * \pre the Environment has positive area - */ - std::vector random_points(const unsigned& count, - double epsilon=0.0) const; - /** \brief compute a shortest path between 2 Points - * - * Uses the classical visibility graph method as described, e.g., - * in ``Robot Motion Planning" (Ch. 4 Sec. 1) by J.C. Latombe. - * Specifically, an A* search is performed on the visibility graph - * using the Euclidean distance as the heuristic function. - * - * \author Karl J. Obermeyer - * - * \pre \a start and \a finish must be in the environment. - * Environment must be \a epsilon -valid. Test with - * Environment::is_valid(epsilon). - * - * \remarks If multiple shortest path queries are made for the - * same Envrionment, it is better to precompute the - * Visibility_Graph. For a precomputed Visibility_Graph, the time - * complexity of a shortest_path() query is O(n^2), where n is the - * number of vertices representing the Environment. - * - * \todo return not just one, but all shortest paths (w/in - * epsilon), e.g., returning a std::vector) - */ - Polyline shortest_path(const Point& start, - const Point& finish, - const Visibility_Graph& visibility_graph, - double epsilon=0.0); - /** \brief compute shortest path between 2 Points - * - * \author Karl J. Obermeyer - * - * \pre \a start and \a finish must be in the environment. - * Environment must be \a epsilon -valid. Test with - * Environment::is_valid(epsilon). - * - * \remarks For single shortest path query, visibility graph is - * not precomputed. Time complexity O(n^3), where n is the number - * of vertices representing the Environment. - */ - Polyline shortest_path(const Point& start, - const Point& finish, - double epsilon=0.0); - /** \brief compute the faces (partition cells) of an arrangement - * of Line_Segments inside the Environment - * - * \author Karl J. Obermeyer - * \todo finish this - */ - std::vector compute_partition_cells( std::vector - partition_inducing_segments, - double epsilon=0.0 ) - { - std::vector cells; - return cells; - } - /** \brief write lists of vertices to *.environment file - * - * uses intuitive human and computer readable decimal format with - * display precision \a fios_precision_temp - * \author Karl J. Obermeyer - * \pre \a fios_precision_temp >=1 - */ - void write_to_file(const std::string& filename, - int fios_precision_temp=FIOS_PRECISION); - //Mutators - /** \brief raw access to Polygons - * - * An argument of 0 accesses the outer boundary, 1 and above - * access the holes. - * \author Karl J. Obermeyer - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - Polygon& operator [] (unsigned i) - { if(i==0){return outer_boundary_;} else{return holes_[i-1];} } - //Mutators - /** \brief raw access to vertices via flattened index - * - * By flattened index is intended the label given to a vertex if - * you were to label all the vertices from 0 to n-1 (where n is - * the number of vertices representing the Environment) starting - * with the first vertex of the outer boundary and progressing in - * order through all the remaining vertices of the outer boundary - * and holes. - * \author Karl J. Obermeyer - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error. - */ - Point& operator () (unsigned k); - /// set outer boundary - void set_outer_boundary(const Polygon& polygon_temp) - { outer_boundary_ = polygon_temp; update_flattened_index_key(); } - /// add hole - void add_hole(const Polygon& polygon_temp) - { holes_.push_back(polygon_temp); update_flattened_index_key(); } - /** \brief enforces outer boundary vertices are listed ccw and - * holes listed cw, and that these lists begin with respective - * lexicographically smallest vertex - * - * \author Karl J. Obermeyer - * \remarks O(n) time complexity, where n is the number of - * vertices representing the Environment. Lex. comparison is very - * sensitive to perturbations if two Points nearly define a line - * parallel to one of the axes. - */ - void enforce_standard_form(); - /** \brief eliminates vertices which are (\a epsilon) - colinear - * with their respective neighbors - * - * \author Karl J. Obermeyer - * \post the Euclidean distance between each vertex and the line - * segment connecting its neighbors is at least \a epsilon - * \remarks time complexity O(n), where n is the number of - * vertices representing the the Environment - */ - void eliminate_redundant_vertices(double epsilon=0.0); - /** \brief reverse (cyclic) order of vertices belonging to holes - * only - * - * \remarks vertex first in each hole's list is held first - */ - void reverse_holes(); - private: - Polygon outer_boundary_; - //obstacles - std::vector holes_; - //allows constant time access to vertices via operator () with - //flattened index as argument - std::vector< std::pair > flattened_index_key_; - //Must call if size of outer_boundary and/or holes_ changes. Time - //complexity O(n), where n is the number of vertices representing - //the Environment. - void update_flattened_index_key(); - //converts flattened index to index pair (hole #, vertex #) in - //time O(n), where n is the number of vertices representing the - //Environment - std::pair one_to_two(unsigned k) const; - //node used for search tree of A* search in shortest_path() method - class Shortest_Path_Node - { - public: - //flattened index of corresponding Environment vertex - //convention vertex_index = n() => corresponds to start Point - //vertex_index = n() + 1 => corresponds to finish Point - unsigned vertex_index; - //pointer to self in search tree. - std::list::iterator search_tree_location; - //pointer to parent in search tree. - std::list::iterator parent_search_tree_location; - //Geodesic distance from start Point. - double cost_to_come; - //Euclidean distance to finish Point. - double estimated_cost_to_go; - //std::vector expand(); - bool operator < (const Shortest_Path_Node& spn2) const - { - double f1 = this->cost_to_come + this->estimated_cost_to_go; - double f2 = spn2.cost_to_come + spn2.estimated_cost_to_go; - if( f1 < f2 ) - return true; - else if( f2 < f1 ) - return false; - else if( this->vertex_index < spn2.vertex_index ) - return true; - else if( this->vertex_index > spn2.vertex_index ) - return false; - else if( &(*(this->parent_search_tree_location)) - < &(*(spn2.parent_search_tree_location)) ) - return true; - else - return false; - } - // print member data for debugging - void print() const - { - std::cout << " vertex_index = " << vertex_index << std::endl - << "parent's vertex_index = " - << parent_search_tree_location->vertex_index - << std::endl - << " cost_to_come = " << cost_to_come << std::endl - << " estimated_cost_to_go = " - << estimated_cost_to_go << std::endl; - } - }; - }; - - - /// printing Environment - std::ostream& operator << (std::ostream& outs, - const Environment& environment_temp); - - - /** \brief set of Guards represented by a list of Points - */ - class Guards - { - public: - friend class Visibility_Graph; - //Constructors - /// default to empty - Guards() { } - /** \brief construct from *.guards file - * - * \author Karl J. Obermeyer - */ - Guards(const std::string& filename); - /// construct from STL vector of Points - Guards(const std::vector& positions) - { positions_ = positions; } - //Accessors - /** \brief raw access to guard position Points - * - * \author Karl J. Obermeyer - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - const Point& operator [] (unsigned i) const { return positions_[i]; } - /// guard count - unsigned N() const { return positions_.size(); } - /// true iff positions are lexicographically ordered - bool are_lex_ordered() const; - /// true iff no two guards are w/in epsilon of each other - bool noncolocated(double epsilon=0.0) const; - /// true iff all guards are located in \a polygon_temp - bool in(const Polygon& polygon_temp, double epsilon=0.0) const; - /// true iff all guards are located in \a environment_temp - bool in(const Environment& environment_temp, double epsilon=0.0) const; - /** \brief Euclidean diameter - * - * \author Karl J. Obermeyer - * \pre greater than 0 guards - * \return maximum Euclidean distance between all pairs of - * vertices - * \remarks time complexity O(N^2), where N is the number of - * guards - */ - double diameter() const; - /** \brief box which fits snugly around the Guards - * - * \author Karl J. Obermeyer - * \pre greater than 0 guards - */ - Bounding_Box bbox() const; - /** \brief write list of positions to *.guards file - * - * Uses intuitive human and computer readable decimal format with - * display precision \a fios_precision_temp. - * \author Karl J. Obermeyer - * \pre \a fios_precision_temp >=1 - */ - void write_to_file(const std::string& filename, - int fios_precision_temp=FIOS_PRECISION); - //Mutators - /** \brief raw access to guard position Points - * - * \author Karl J. Obermeyer - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - Point& operator [] (unsigned i) { return positions_[i]; } - /// add a guard - void push_back(const Point& point_temp) - { positions_.push_back(point_temp); } - /// set positions with STL vector of Points - void set_positions(const std::vector& positions_temp) - { positions_ = positions_temp; } - /** \brief sort positions in lexicographic order - * - * from (lowest x, then lowest y) to (highest x, then highest y) - * \author Karl J. Obermeyer - * \remarks time complexity O(N logN), where N is the guard count. - * Lex. comparison is very sensitive to perturbations if two - * Points nearly define a line parallel to one of the axes. - */ - void enforce_lex_order(); - /// reverse order of positions - void reverse(); - /** \brief relocate each guard to closest vertex if within - * \a epsilon of some vertex (of \a environment_temp) - * - * \author Karl J. Obermeyer - * \pre the guards' position data are numbers and \a environment_temp - * is nonempty - * \post if a guard was a Euclidean distance no greater - * than \a epsilon from any vertex of \a environment_temp, then it - * will be repositioned to coincide with the closest such vertex - * \remarks O(N*n) time complexity, where N is the guard count - * and n is the number of vertices in \a environment_temp. - */ - void snap_to_vertices_of(const Environment& environment_temp, - double epsilon=0.0); - - /** \brief relocate each guard to closest vertex if within - * \a epsilon of some vertex (of \a environment_temp) - * - * \author Karl J. Obermeyer - * \pre the guards' position data are numbers and \a polygon_temp - * is nonempty - * \post if a guard was a Euclidean distance no greater - * than \a epsilon from any vertex of \a polygon_temp, then it - * will be repositioned to coincide with the closest such vertex - * \remarks O(N*n) time complexity, where N is the guard count - * and n is the number of vertices in \a polygon_temp - */ - void snap_to_vertices_of(const Polygon& polygon_temp, - double epsilon=0.0); - /** \brief relocate each guard to closest Point on boundary if - * within \a epsilon of the boundary (of \a environment_temp) - * - * \author Karl J. Obermeyer - * \pre the guards' position data are numbers and \a environment_temp - * is nonempty - * \post If the calling Point was a Euclidean distance no greater - * than \a epsilon from the boundary of \a environment_temp, then it - * will be repositioned to it's projection onto that boundary - * \remarks O(N*n) time complexity, where N is the guard count and - * n is the number of vertices in \a environment_temp - */ - void snap_to_boundary_of(const Environment& environment_temp, - double epsilon=0.0); - /** \brief relocate each guard to closest Point on boundary if - * within \a epsilon of the boundary (of \a polygon_temp) - * - * \author Karl J. Obermeyer - * \pre the guards' position data are numbers and \a polygon_temp - * is nonempty - * \post If the calling Point was a Euclidean distance no greater - * than \a epsilon from the boundary of \a polygon_temp, then it - * will be repositioned to it's projection onto that boundary - * \remarks O(N*n) time complexity, where N is the guard count and - * n is the number of vertices in \a polygon_temp - */ - void snap_to_boundary_of(const Polygon& polygon_temp, - double epsilon=0.0); - private: - std::vector positions_; - }; - - - /// print Guards - std::ostream& operator << (std::ostream& outs, - const Guards& guards); - - - /** \brief visibility polygon of a Point in an Environment or Polygon - * - * A Visibility_Polygon represents the closure of the set of all - * points in an environment which are {\it clearly visible} from a - * point (the observer). Two Points p1 and p2 are (mutually) {\it - * clearly visible} in an environment iff the relative interior of - * the line segment connecting p1 and p2 does not intersect the - * boundary of the environment. - * - * \remarks average case time complexity O(n log(n)), where n is the - * number of vertices in the Evironment (resp. Polygon). Note the - * Standard Library's sort() function performs O(n log(n)) - * comparisons (both average and worst-case) and the sort() member - * function of an STL list performs "approximately O(n log(n)) - * comparisons". For robustness, any Point (observer) should be \a - * epsilon -snapped to the environment boundary and vertices before - * computing its Visibility_Polygon (use the Point methods - * snap_to_vertices_of(...) and snap_to_boundary_of(...) ). - */ - class Visibility_Polygon : public Polygon - { - public: - //Constructors - /// default to empty - Visibility_Polygon() { } - //:TRICKY: - /** \brief visibility set of a Point in an Environment - * - * \author Karl J. Obermeyer - * - * \pre \a observer is in \a environment_temp (w/in \a epsilon ) - * and has been epsilon-snapped to the Environment using the - * method Point::snap_to_boundary_of() followed by (order is - * important) Point::snap_to_vertices_of(). \a environment_temp - * must be \a epsilon -valid. Test with - * Environment::is_valid(epsilon). - * - * \remarks O(n log(n)) average case time complexity, where n is the - * number of vertices in the Evironment (resp. Polygon). - */ - Visibility_Polygon(const Point& observer, - const Environment& environment_temp, - double epsilon=0.0); - /** \brief visibility set of a Point in a Polygon - * - * \pre \a observer is in \a polygon_temp (w/in \a epsilon ) and - * has been epsilon-snapped to the Polygon using the methods - * Point::snap_to_vertices_of() and Point::snap_to_boundary_of(). - * \a environment_temp must be \a epsilon -valid. Test with - * Environment::is_valid(epsilon). - * - * \remarks less efficient because constructs an Environment from - * a Polygon and then calls the other Visibility_Polygon constructor. - * O(n log(n)) average case time complexity, where n is the - * number of vertices in the Evironment (resp. Polygon). - */ - Visibility_Polygon(const Point& observer, - const Polygon& polygon_temp, - double epsilon=0.0); - //Accessors - //std::vector get_gap_edges(double epsilon=0.0) { return gap_edges_; } - /// location of observer which induced the visibility polygon - Point observer() const - { return observer_; } - //Mutators - private: - //ith entry of gap_edges is true iff the edge following ith vertex - //is a gap edge (not solid). - //std::vector gap_edges_; - Point observer_; - - struct Polar_Edge - { - Polar_Point first; - Polar_Point second; - Polar_Edge() { } - Polar_Edge(const Polar_Point& ppoint1, - const Polar_Point& ppoint2) : - first(ppoint1), second(ppoint2) {} - }; - - class Polar_Point_With_Edge_Info : public Polar_Point - { - public: - std::list::iterator incident_edge; - bool is_first; //True iff polar_point is the first_point of the - //Polar_Edge pointed to by - //incident_edge. - void set_polar_point(const Polar_Point& ppoint_temp) - { - set_polar_origin( ppoint_temp.polar_origin() ); - set_x( ppoint_temp.x() ); - set_y( ppoint_temp.y() ); - set_range( ppoint_temp.range() ); - set_bearing( ppoint_temp.bearing() ); - } - //The operator < is the same as for Polar_Point with one - //exception. If two vertices have equal coordinates, but one is - //the first point of its respecitve edge and the other is the - //second point of its respective edge, then the vertex which is - //the second point of its respective edge is considered - //lexicographically smaller. - friend bool operator < (const Polar_Point_With_Edge_Info& ppwei1, - const Polar_Point_With_Edge_Info& ppwei2) - { - if( Polar_Point(ppwei1) == Polar_Point(ppwei2) - and !ppwei1.is_first and ppwei2.is_first ) - return true; - else - return Polar_Point(ppwei1) < Polar_Point(ppwei2); - } - }; - - //Strict weak ordering (acts like <) for pointers to Polar_Edges. - //Used to sort the priority_queue q2 used in the radial line sweep - //of Visibility_Polygon constructors. Let p1 be a pointer to - //Polar_Edge e1 and p2 be a pointer to Polar_Edge e2. Then p1 is - //considered greater (higher priority) than p2 if the distance - //from the observer (pointed to by observer_pointer) to e1 along - //the direction to current_vertex is smaller than the distance - //from the observer to e2 along the direction to current_vertex. - class Incident_Edge_Compare - { - const Point *const observer_pointer; - const Polar_Point_With_Edge_Info *const current_vertex_pointer; - double epsilon; - public: - Incident_Edge_Compare(const Point& observer, - const Polar_Point_With_Edge_Info& current_vertex, - double epsilon_temp) : - observer_pointer(&observer), - current_vertex_pointer(¤t_vertex), - epsilon(epsilon_temp) { } - bool operator () (std::list::iterator e1, - std::list::iterator e2) const - { - Polar_Point k1, k2; - Line_Segment xing1 = intersection( Ray(*observer_pointer, - current_vertex_pointer->bearing()), - Line_Segment(e1->first, - e1->second), - epsilon); - Line_Segment xing2 = intersection( Ray(*observer_pointer, - current_vertex_pointer->bearing()), - Line_Segment(e2->first, - e2->second), - epsilon); - if( xing1.size() > 0 and xing2.size() > 0 ){ - k1 = Polar_Point( *observer_pointer, - xing1.first() ); - k2 = Polar_Point( *observer_pointer, - xing2.first() ); - if( k1.range() <= k2.range() ) - return false; - return true; - } - //Otherwise infeasible edges are given higher priority, so they - //get pushed out the top of the priority_queue's (q2's) - //heap. - else if( xing1.size() == 0 and xing2.size() > 0 ) - return false; - else if( xing1.size() > 0 and xing2.size() == 0 ) - return true; - else - return true; - } - }; - - bool is_spike( const Point& observer, - const Point& point1, - const Point& point2, - const Point& point3, - double epsilon=0.0 ) const; - - //For eliminating spikes as they appear. In the - //Visibility_Polygon constructors, these are checked every time a - //Point is added to vertices. - void chop_spikes_at_back(const Point& observer, - double epsilon); - void chop_spikes_at_wrap_around(const Point& observer, - double epsilon); - void chop_spikes(const Point& observer, - double epsilon); - //For debugging Visibility_Polygon constructors. - //Prints current_vertex and active_edge data to screen. - void print_cv_and_ae(const Polar_Point_With_Edge_Info& current_vertex, - const std::list::iterator& - active_edge); - }; - - - /** \brief visibility graph of points in an Environment, - * represented by adjacency matrix - * - * \remarks used for shortest path planning in the - * Environment::shortest_path() method - * - * \todo Add method to prune edges for faster shortest path - * calculation, e.g., exclude concave vertices and only include - * tangent edges as described in ``Robot Motion Planning" (Ch. 4 - * Sec. 1) by J.C. Latombe. - */ - class Visibility_Graph - { - public: - //Constructors - /// default to empty - Visibility_Graph() { n_=0; adjacency_matrix_ = NULL; } - /// copy - Visibility_Graph( const Visibility_Graph& vg2 ); - /** \brief construct the visibility graph of Environment vertices - * - * \author Karl J. Obermeyer - * - * \pre \a environment must be \a epsilon -valid. Test with - * Environment::is_valid(epsilon). - * - * \remarks Currently this constructor simply computes the - * Visibility_Polygon of each vertex and checks inclusion of the - * other vertices, taking time complexity O(n^3), where n is the - * number of vertices representing the Environment. This time - * complexity is not optimal. As mentioned in ``Robot Motion - * Planning" by J.C. Latombe p.157, there are algorithms able to - * construct a visibility graph for a polygonal environment with - * holes in time O(n^2). The nonoptimal algorithm is being used - * temporarily because of (1) its ease to implement using the - * Visibility_Polygon class, and (2) its apparent robustness. - * Implementing the optimal algorithm robustly is future work. - */ - Visibility_Graph(const Environment& environment, double epsilon=0.0); - //Constructors - /** \brief construct the visibility graph of Points in an Environment - * - * \pre \a environment must be \a epsilon -valid. Test with - * Environment::is_valid(epsilon). - * - * \author Karl J. Obermeyer \remarks Currently this constructor - * simply computes the Visibility_Polygon of each Point and checks - * inclusion of the other Points, taking time complexity - * O(N n log(n) + N^2 n), where N is the number of Points and n is - * the number of vertices representing the Environment. This time - * complexity is not optimal, but has been used for - * simplicity. More efficient algorithms are discussed in ``Robot - * Motion Planning" by J.C. Latombe p.157. - */ - Visibility_Graph(const std::vector points, - const Environment& environment, double epsilon=0.0); - //Constructors - /** \brief construct the visibility graph of Guards in an Environment - * - * \pre \a start and \a finish must be in the environment. - * Environment must be \a epsilon -valid. Test with - * Environment::is_valid(epsilon). - * - * \author Karl J. Obermeyer - * \remarks Currently this constructor simply computes the - * Visibility_Polygon of each guard and checks inclusion of the - * other guards, taking time complexity O(N n log(n) + N^2 n), - * where N is the number of guards and n is the number of vertices - * representing the Environment. This time complexity is not - * optimal, but has been used for simplicity. More efficient - * algorithms are discussed in ``Robot Motion Planning" by - * J.C. Latombe p.157. - */ - Visibility_Graph(const Guards& guards, - const Environment& environment, double epsilon=0.0); - //Accessors - /** \brief raw access to adjacency matrix data - * - * \author Karl J. Obermeyer - * \param i1 Polygon index of first vertex - * \param j1 index of first vertex within its Polygon - * \param i2 Polygon index of second vertex - * \param j2 index of second vertex within its Polygon - * \return true iff first vertex is visible from second vertex - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - bool operator () (unsigned i1, - unsigned j1, - unsigned i2, - unsigned j2) const; - /** \brief raw access to adjacency matrix data via flattened - * indices - * - * By flattened index is intended the label given to a vertex if - * you were to label all the vertices from 0 to n-1 (where n is - * the number of vertices representing the Environment) starting - * with the first vertex of the outer boundary and progressing in - * order through all the remaining vertices of the outer boundary - * and holes. - * \author Karl J. Obermeyer - * \param k1 flattened index of first vertex - * \param k1 flattened index of second vertex - * \return true iff first vertex is visible from second vertex - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - bool operator () (unsigned k1, - unsigned k2) const; - /// \brief total number of vertices in corresponding Environment - unsigned n() const { return n_; } - //Mutators - /** \brief raw access to adjacency matrix data - * - * \author Karl J. Obermeyer - * \param i1 Polygon index of first vertex - * \param j1 index of first vertex within its Polygon - * \param i2 Polygon index of second vertex - * \param j2 index of second vertex within its Polygon - * \return true iff first vertex is visible from second vertex - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - bool& operator () (unsigned i1, - unsigned j1, - unsigned i2, - unsigned j2); - /** \brief raw access to adjacency matrix data via flattened - * indices - * - * By flattened index is intended the label given to a vertex if - * you were to label all the vertices from 0 to n-1 (where n is - * the number of vertices representing the Environment) starting - * with the first vertex of the outer boundary and progressing in - * order through all the remaining vertices of the outer boundary - * and holes. - * \author Karl J. Obermeyer - * \param k1 flattened index of first vertex - * \param k1 flattened index of second vertex - * \return true iff first vertex is visible from second vertex - * \remarks for efficiency, no bounds check; usually trying to - * access out of bounds causes a bus error - */ - bool& operator () (unsigned k1, - unsigned k2); - /// assignment operator - Visibility_Graph& operator = - (const Visibility_Graph& visibility_graph_temp); - /// destructor - virtual ~Visibility_Graph(); - private: - //total number of vertices of corresponding Environment - unsigned n_; - //the number of vertices in each Polygon of corresponding Environment - std::vector vertex_counts_; - // n_-by-n_ adjacency matrix data stored as 2D dynamic array - bool **adjacency_matrix_; - //converts vertex pairs (hole #, vertex #) to flattened index - unsigned two_to_one(unsigned i, - unsigned j) const; - }; - - - /// print Visibility_Graph adjacency matrix - std::ostream& operator << (std::ostream& outs, - const Visibility_Graph& visibility_graph); - -} - -#endif //VISILIBITY_H diff --git a/xs/t/18_motionplanner.t b/xs/t/18_motionplanner.t index c7bf78f88..1080e2b76 100644 --- a/xs/t/18_motionplanner.t +++ b/xs/t/18_motionplanner.t @@ -3,6 +3,11 @@ use strict; use warnings; +BEGIN { + use FindBin; + use lib "$FindBin::Bin/../../lib"; +} + use Slic3r::XS; use Test::More tests => 22; @@ -30,6 +35,18 @@ my $expolygon = Slic3r::ExPolygon->new($square, $hole_in_square); my $to = Slic3r::Point->new(180,180); $_->scale(1/0.000001) for $from, $to; my $path = $mp->shortest_path($from, $to); + require "Slic3r.pm"; + require "Slic3r/SVG.pm"; + Slic3r::SVG::output( + "path.svg", + expolygons => [$expolygon], + polylines => [$path], + ); + use XXX; YYY [ + $from->pp, + $path->pp, + $to->pp, + ]; ok $path->is_valid(), 'return path is valid'; ok $path->length > Slic3r::Line->new($from, $to)->length, 'path length is greater than straight line'; ok $path->first_point->coincides_with($from), 'first path point coincides with initial point';