From 5fe5021fd7ee295e7be2788100206422ebcd70e0 Mon Sep 17 00:00:00 2001 From: Alessandro Ranellucci Date: Tue, 13 May 2014 20:06:01 +0200 Subject: [PATCH] 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 907a0048..36212894 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 cdf26c9e..af52517a 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 d8334bcd..184998f8 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 9975eb02..c498f45a 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 92c2d063..39af4bf3 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 e2ee364a..9d4c5809 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 e7c56d8d..be606591 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 67a22682..3de86e7c 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 75f2f505..f6a27284 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 00000000..2df76efd --- /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 00000000..b0fd29ee --- /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 63778d5a..d9dcb720 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 00000000..5f1f5bb9 --- /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 00000000..2a4af899 --- /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 df0f203d..aefbd1ae 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 00000000..57abef93 --- /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 b122d95d..46c3e271 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 945b104b..1b060bb3 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};