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