visilibity 0.0.1

data/ext/VisiLibity/extconf.rb

@@ -0,0 +1,19 @@
+require 'mkmf'
+
+# need C++ support for visilibity
+$libs = append_library($libs, "stdc++")
+
+swig_dir = with_config('swig-dir', File.dirname(__FILE__))
+
+create_makefile('VisiLibity_native')
+
+# eww eww eww
+original_makefile = File.read('Makefile')
+File.open('Makefile', 'w') do |make|
+  make.print %(
+#{original_makefile}
+
+#{swig_dir}/VisiLibity_wrap.cxx: #{swig_dir}/VisiLibity.i
+\tswig -c++ -ruby #{swig_dir}/VisiLibity.i
+  )
+end

data/ext/VisiLibity/visilibity.cpp

@@ -0,0 +1,3659 @@
+/**
+ * \file visilibity.cpp
+ * \author Karl J. Obermeyer
+ * \date March 20, 2008
+ *
+\remarks
+VisiLibity:  A Floating-Point Visibility Algorithms Library,
+Copyright (C) 2008  Karl J. Obermeyer (karl.obermeyer [ at ] gmail.com)
+
+This file is part of VisiLibity.
+
+VisiLibity is free software: you can redistribute it and/or modify it under
+the terms of the GNU Lesser General Public License as published by the
+Free Software Foundation, either version 3 of the License, or (at your
+option) any later version.
+
+VisiLibity is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
+License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with VisiLibity.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "visilibity.hpp"  //VisiLibity header
+#include <cmath>         //math functions in std namespace
+#include <vector>
+#include <queue>         //queue and priority_queue
+#include <set>           //priority queues with iteration,
+                         //integrated keys
+#include <list>
+#include <algorithm>     //sorting, min, max, reverse
+#include <cstdlib>       //rand and srand
+#include <ctime>         //Unix time
+#include <fstream>       //file I/O
+#include <iostream>
+#include <cstring>       //gives C-string manipulation
+#include <string>        //string class
+#include <cassert>       //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<double>( std::rand() )
+                   / static_cast<double>( 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<environment_temp.h(); i++){
+      point_temp = projection_onto_vertices_of(environment_temp.holes_[i]);
+      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 Polygon& polygon_temp) const
+  {
+    assert( *this == *this
+	    and polygon_temp.n() > 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<environment_temp.h(); i++){
+      point_temp = projection_onto_boundary_of(environment_temp.holes_[i]);
+      if( distance(*this, point_temp) < running_min ){
+	running_projection = point_temp;
+	running_min = distance(*this, running_projection);
+      }
+    }
+    return running_projection;
+  }
+
+  
+  bool Point::on_boundary_of(const Polygon& polygon_temp,
+			     double epsilon) const
+  {
+    assert( *this == *this
+	    and polygon_temp.vertices_.size() > 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<environment_temp.h(); i++)
+      if( in(environment_temp.holes_[i]) )
+	return false;
+    //Must be in interior.
+    return true;
+  }
+
+
+  bool Point::is_endpoint_of(const Line_Segment& line_segment_temp,
+			     double epsilon) const
+  {
+    assert( *this == *this
+	    and line_segment_temp.size() > 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<polyline_temp.size()-1; i++){
+      distance_temp = distance(point_temp, Line_Segment(polyline_temp[i],
+							polyline_temp[i+1]) );
+      if(distance_temp < running_min)
+	running_min = distance_temp;
+    }
+    return running_min;
+  }
+  double distance(const Polyline& polyline_temp,
+		  const Point& point_temp)
+  {
+    return distance(point_temp, polyline_temp);
+  }
+
+
+  double boundary_distance(const Point& point_temp,
+			   const Polygon& polygon_temp)
+  {
+    assert( point_temp == point_temp
+	    and polygon_temp.n() > 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<running_min)
+      running_min = distance_temp;
+    distance_temp = distance(line_segment2.first(), line_segment1);
+    if(distance_temp<running_min)
+      running_min = distance_temp;
+    distance_temp = distance(line_segment2.second(), line_segment1);
+    if(distance_temp<running_min)
+      return distance_temp;
+    return running_min;
+  }
+
+
+  double boundary_distance(const Line_Segment& line_segment,
+			   const Polygon& polygon)
+  {
+    assert( line_segment.size() > 0 and polygon.n() > 0 );
+
+    double running_min = distance( line_segment , polygon[0] );
+    if( polygon.n() > 1 )
+      for(unsigned i=0; i<polygon.n(); i++){
+	double d = distance(  line_segment, 
+			      Line_Segment( polygon[i] , polygon[i+1] )  );
+	if( running_min > 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<running_min)
+      running_min = distance_temp;
+    distance_temp = distance(c, line_segment1);
+    if(distance_temp<running_min)
+      running_min = distance_temp;
+    distance_temp = distance(d, line_segment1);
+    if(distance_temp<running_min)
+      running_min = distance_temp;
+    //If an endpoint is close enough to the other segment, the
+    //intersection is not considered proper.
+    if(running_min <= epsilon)
+      return false; 
+    //This test is from O'Rourke's "Computational Geometry in C",
+    //p.30.  Checks left and right turns.
+    if(  cross(b-a, c-b) * cross(b-a, d-b) < 0   
+	 and   cross(d-c, b-d) * cross(d-c, a-d) < 0  )
+      return true;
+    return false;
+  }
+
+  
+  Line_Segment intersection(const Line_Segment& line_segment1,
+			    const Line_Segment& line_segment2, double epsilon)
+  {
+    //Initially empty.
+    Line_Segment line_segment_temp;
+
+    if( line_segment1.size() == 0
+	or line_segment2.size() == 0 )
+      return line_segment_temp;
+
+    //No intersection => 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_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;
+
+    return !(polar_point1>polar_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<size()-1; i++){
+    for(unsigned j=i+1; j<size(); j++){
+      if( distance( (*this)[i] , (*this)[j] ) > 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<Point> 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<polyline_temp.size(); i++)
+      outs << polyline_temp[i] << std::endl;
+    return outs;
+  }
+
+
+  void Polyline::append( const Polyline& polyline ){
+    vertices_.reserve( vertices_.size() + polyline.vertices_.size() );
+    for(unsigned i=0; i<polyline.vertices_.size(); i++){
+      vertices_.push_back( polyline.vertices_[i] );
+    }
+  }
+
+
+  //Polygon
+
+
+  Polygon::Polygon (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() );
+
+    Point point_temp;
+    double x_temp, y_temp;
+    while (fin >> 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<Point>& 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<n()-2; i++)
+      for(unsigned j=i+2; j<=n()-1; j++)
+	if( 0!=(j+1)%vertices_.size()  
+	    and distance( Line_Segment((*this)[i],(*this)[i+1]) , 
+			  Line_Segment((*this)[j],(*this)[j+1]) ) <= epsilon  )
+	  return false;
+    
+    return true;
+  }
+
+
+  bool Polygon::is_in_standard_form() const
+  {
+    if(vertices_.size() > 1)  //if more than one point in the polygon.
+      for(unsigned i=1; i<vertices_.size(); i++)
+	if(vertices_[0] > 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<n()-1; i++)
+      length_temp += distance( vertices_[i] , vertices_[i+1] );
+    length_temp += distance( vertices_[n()-1] , 
+			     vertices_[0] );
+    return length_temp;
+  }
+
+
+  double Polygon::area() const
+  {
+    double area_temp = 0;
+    if(n()==0)
+      return 0;
+    for(unsigned i=0; i<=n()-1; i++)
+      area_temp += (*this)[i].x()*(*this)[i+1].y() 
+	- (*this)[i+1].x()*(*this)[i].y();
+    return area_temp/2.0;
+  }
+
+
+  Point Polygon::centroid() const
+  {
+    assert( vertices_.size() > 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<n()-1; i++){
+    for(unsigned j=i+1; j<n(); j++){
+      if( distance( (*this)[i] , (*this)[j] ) > 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<Point> Polygon::random_points(const unsigned& count,
+					    double epsilon) const
+  {
+    //Precondition:  nonempty Polygon.
+    assert( vertices_.size() > 0 );
+
+    Bounding_Box bounding_box = bbox();
+    std::vector<Point> 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<n(); i++)
+      fout << vertices_[i] << std::endl;
+    fout.close();
+  }
+
+
+  void Polygon::enforce_standard_form()
+  {
+    int point_count=vertices_.size();
+    if(point_count > 1){ //if more than one point in the polygon.
+      std::vector<Point> 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<point_count; i++)
+	if(vertices_[i]<vertices_[index_of_smallest])
+	  index_of_smallest=i;
+      //Fill vertices_temp starting with lex. smallest.
+      for(i=index_of_smallest; i<point_count; i++)
+	vertices_temp.push_back(vertices_[i]);
+      for(i=0; i<index_of_smallest; i++)
+	vertices_temp.push_back(vertices_[i]);
+      vertices_=vertices_temp;
+    }
+  }
+
+
+  void Polygon::eliminate_redundant_vertices(double epsilon)
+  {
+    //Degenerate case.
+    if( vertices_.size() < 4 )
+      return;
+
+    //Store new minimal length list of vertices.
+    std::vector<Point> 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<polygon1.n(); i++)
+      if(  !(polygon1[i] == polygon2[i])  )
+	return false;
+    return true;
+  }
+  bool operator != (Polygon polygon1, Polygon polygon2)
+  {
+    return !( polygon1 == polygon2 );
+  }
+  bool equivalent(Polygon polygon1, Polygon polygon2, double epsilon)
+  {
+    if( polygon1.n() == 0 or polygon2.n() == 0 )
+      return false;
+    if( polygon1.n() != polygon2.n() )
+      return false;
+    //Try all cyclic matches
+    unsigned n = polygon1.n();//=polygon2.n()
+    for( unsigned offset = 0 ; offset < n ; offset++ ){
+      bool successful_match = true;
+      for(unsigned i=0; i<n; i++){
+	if(  distance( polygon1[ i ] , polygon2[ i + offset ] ) > 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<polygon_temp.n(); i++)
+      outs << polygon_temp[i] << std::endl;
+    return outs;
+  }
+
+
+  //Environment
+  
+  
+  Environment::Environment(const std::vector<Polygon>& polygons)
+  {
+    outer_boundary_ = polygons[0];
+    for(unsigned i=1; i<polygons.size(); i++)
+      holes_.push_back( polygons[i] );
+    update_flattened_index_key();
+  }
+  Environment::Environment(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() );
+
+    //Temporary vars for numbers to be read from file.
+    double x_temp, y_temp;  
+    std::vector<Point> 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<unsigned,unsigned> ij(one_to_two(k));
+    std::pair<unsigned,unsigned> 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<h(); i++)
+      n_count += holes_[i].n();
+    return n_count;
+  }
+
+
+  unsigned Environment::r() const
+  {
+    int r_count = 0;
+    Polygon polygon_temp;
+    r_count = outer_boundary_.r();
+    for(unsigned i=0; i<h(); i++){
+      r_count +=  holes_[i].n() - holes_[i].r();
+    }
+    return r_count;
+  }
+
+
+  bool Environment::is_in_standard_form() const
+  {
+    if( outer_boundary_.is_in_standard_form() == false 
+	or outer_boundary_.area() < 0 )
+      return false;
+    for(unsigned i=0; i<holes_.size(); i++)
+      if( holes_[i].is_in_standard_form() == false 
+	  or holes_[i].area() > 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<h(); i++)
+      if( !holes_[i].is_simple(epsilon) ){
+	std::cerr << std::endl << "\x1b[31m" 
+		  << "Hole " << i << " is not simple." 
+		  <<  "\x1b[0m" << std::endl;
+	return false; 
+      }
+
+    //Check none of the Polygons' boundaries intersect w/in epsilon.
+    for(unsigned i=0; i<h(); i++)
+      if( boundary_distance(outer_boundary_, holes_[i]) <= epsilon ){
+	std::cerr << std::endl << "\x1b[31m" 
+	  << "The outer boundary intersects the boundary of hole " << i << "." 
+	  <<  "\x1b[0m" << std::endl;
+	return false; 
+      }
+    for(unsigned i=0; i<h(); i++)
+      for(unsigned j=i+1; j<h(); j++)
+	if( boundary_distance(holes_[i], holes_[j]) <= epsilon ){
+	  std::cerr << std::endl << "\x1b[31m" 
+		    << "The boundary of hole " << i 
+		    << " intersects the boundary of hole " << j << "." 
+		    <<  "\x1b[0m" << std::endl;
+	  return false;
+	}
+
+    //Check that the vertices of each hole are in the outside_boundary
+    //and not in any other holes.
+    //Loop over holes.
+    for(unsigned i=0; i<h(); i++){
+      //Loop over vertices of a hole
+      for(unsigned j=0; j<holes_[i].n(); j++){
+	if( !holes_[i][j].in(outer_boundary_, epsilon) ){
+	  std::cerr << std::endl << "\x1b[31m" 
+		    << "Vertex " << j << " of hole " << i 
+		    << " is not within the outer boundary." 
+		    <<  "\x1b[0m" << std::endl;
+	  return false; 
+	}
+	//Second loop over holes.
+	for(unsigned k=0; k<h(); k++)
+	  if( i!=k and holes_[i][j].in(holes_[k], epsilon) ){
+	    std::cerr << std::endl << "\x1b[31m" 
+		      << "Vertex " << j 
+		      << " of hole " << i 
+		      << " is in hole " << k << "." 
+		      <<  "\x1b[0m" << std::endl;
+	    return false;
+	  }
+      }
+    }
+
+    //Check outer_boundary is ccw and holes are cw.
+    if( outer_boundary_.area() <= 0 ){
+      std::cerr << std::endl << "\x1b[31m" 
+		<< "The outer boundary vertices are not listed ccw." 
+		<<  "\x1b[0m" << std::endl;
+      return false; 
+    }
+    for(unsigned i=0; i<h(); i++)
+      if( holes_[i].area() >= 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<h(); i++)
+      length_temp += holes_[i].boundary_length();
+    return length_temp;
+  }
+
+
+  double Environment::area() const
+  {
+    double area_temp = outer_boundary_.area();
+    for(unsigned i=0; i<h(); i++)
+      area_temp += holes_[i].area();
+    return area_temp;
+  }
+
+
+  std::vector<Point> Environment::random_points(const unsigned& count,
+						double epsilon) const
+  {
+    assert( area() > 0 );
+
+    Bounding_Box bounding_box = bbox();
+    std::vector<Point> 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<n(); k++){
+      if(  (*this)(k).in( start_visibility_polygon , epsilon )  )
+	start_visible[k] = true;
+      else
+	start_visible[k] = false;
+      if(  (*this)(k).in( finish_visibility_polygon , epsilon )  )
+	finish_visible[k] = true;
+      else
+	finish_visible[k] = false;
+    }
+
+    //Initialize search tree of visited nodes
+    std::list<Shortest_Path_Node> 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<Shortest_Path_Node> 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<Shortest_Path_Node> 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<Shortest_Path_Node>::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<Shortest_Path_Node>::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<Shortest_Path_Node>::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<Shortest_Path_Node>::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<h(); i++)
+      {
+	fout << "//Hole" << std::endl << holes_[i];
+      }
+    //fout << "//EOF marker";
+    fout.close();
+  }
+
+
+  Point& Environment::operator () (unsigned k)
+  {
+    //std::pair<unsigned,unsigned> ij( one_to_two(k) );
+    std::pair<unsigned,unsigned> 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<h(); i++){
+      if( holes_[i].area() > 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<holes_.size(); i++)
+      holes_[i].eliminate_redundant_vertices(epsilon);
+
+    update_flattened_index_key();
+  }
+
+
+  void Environment::reverse_holes()
+  {
+    for(unsigned i=0; i < holes_.size(); i++)
+      holes_[i].reverse();
+  }
+
+
+  void Environment::update_flattened_index_key()
+  {
+    flattened_index_key_.clear();
+    std::pair<unsigned, unsigned> 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<unsigned,unsigned> Environment::one_to_two(unsigned k) const
+  {
+    std::pair<unsigned,unsigned> 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_.size()-1; i++)
+	if(positions_[i] > positions_[i+1])
+	  return false;
+    return true;
+  }
+
+
+  bool Guards::noncolocated(double epsilon) const
+  {
+    for(unsigned i=0; i<positions_.size(); i++)
+      for(unsigned j=i+1; j<positions_.size(); j++)
+	if( distance(positions_[i], positions_[j]) <= epsilon )
+	  return false;
+    return true;
+  }
+
+
+  bool Guards::in(const Polygon& polygon_temp, double epsilon) const
+  {
+    for(unsigned i=0; i<positions_.size(); i++)
+      if(!positions_[i].in(polygon_temp, epsilon))
+	return false;
+    return true;
+  }
+
+
+  bool Guards::in(const Environment& environment_temp, double epsilon) const
+  {
+    for(unsigned i=0; i<positions_.size(); i++)
+      if(!positions_[i].in(environment_temp, epsilon))
+	return false;
+    return true;
+  }
+
+
+  double Guards::diameter() const
+  {
+    //Precondition:  more than 0 guards
+    assert( N() > 0 );
+
+    double running_max=0;
+    for(unsigned i=0; i<N()-1; i++){
+    for(unsigned j=i+1; j<N(); j++){
+      if( distance( (*this)[i] , (*this)[j] ) > 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<positions_.size(); i++)
+      fout << positions_[i].x() << "  " << positions_[i].y() << std::endl;
+    //fout << "//EOF marker";
+    fout.close();
+  }
+
+
+  void Guards::enforce_lex_order()
+  {
+    //std::stable_sort(positions_.begin(), positions_.end());
+    std::sort(positions_.begin(), positions_.end());
+  }
+
+
+  void Guards::reverse()
+  {
+    std::reverse( positions_.begin() , positions_.end() );
+  }
+
+
+  void Guards::snap_to_vertices_of(const Environment& environment_temp,
+				   double epsilon)
+  {
+    for(unsigned i=0; i<positions_.size(); i++)
+      positions_[i].snap_to_vertices_of(environment_temp);
+  }
+
+
+  void Guards::snap_to_vertices_of(const Polygon& polygon_temp,
+				   double epsilon)
+  {
+    for(unsigned i=0; i<positions_.size(); i++)
+      positions_[i].snap_to_vertices_of(polygon_temp);
+  }
+
+
+  void Guards::snap_to_boundary_of(const Environment& environment_temp,
+				   double epsilon)
+  {
+    for(unsigned i=0; i<positions_.size(); i++)
+      positions_[i].snap_to_boundary_of(environment_temp);
+  }
+
+
+  void Guards::snap_to_boundary_of(const Polygon& polygon_temp,
+				   double epsilon)
+  {
+    for(unsigned i=0; i<positions_.size(); i++)
+      positions_[i].snap_to_boundary_of(polygon_temp);
+  }
+
+
+  std::ostream& operator << (std::ostream& outs, const Guards& guards)
+  {
+    outs << "//Guard Positions" << std::endl; 
+    for(unsigned i=0; i<guards.N(); i++)
+      outs << guards[i].x() << "  " << guards[i].y() << std::endl;
+    //outs << "//EOF marker";
+    return outs;
+  }
+
+
+  //Visibility_Polygon
+
+
+  bool Visibility_Polygon::is_spike( const Point& observer,
+				     const Point& point1,
+				     const Point& point2,
+				     const Point& point3, 
+				     double epsilon) const
+  {
+
+    return(  
+         //Make sure observer not colocated with any of the points.
+	   distance( observer , point1 ) > 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<Point> spike_tips;
+    std::vector<Point> vertices_temp;
+    //Middle point is potentially the tip of a spike
+    for(unsigned i=0; i<vertices_.size(); i++)
+      if(   distance(  (*this)[i+2], 
+		       Line_Segment( (*this)[i], (*this)[i+1] )  )
+	    <= epsilon
+	    or
+	    distance(  (*this)[i], 
+		       Line_Segment( (*this)[i+1], (*this)[i+2] )  )
+	    <= epsilon   )
+	spike_tips.insert( (*this)[i+1] );
+    
+    for(unsigned i=0; i<vertices_.size(); i++)
+      if( spike_tips.find(vertices_[i]) == spike_tips.end() )
+	vertices_temp.push_back( vertices_[i] );
+    vertices_.swap( vertices_temp );
+  }
+
+
+  void Visibility_Polygon::
+  print_cv_and_ae(const Polar_Point_With_Edge_Info& current_vertex,
+		  const std::list<Polar_Edge>::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<Polar_Edge> 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<environment_temp[i].n(); j++){      
+      ppoint1 = Polar_Point( observer, environment_temp[i][j] );
+      ppoint2 = Polar_Point( observer, environment_temp[i][j+1] );
+
+      //If the observer is in the relative interior of the edge.
+      if(  observer.in_relative_interior_of( Line_Segment(ppoint1, ppoint2),
+					     epsilon )  ){
+	//Split the edge at the observer and add the resulting two
+	//edges to elp (the polar edge list).
+	split_bottom = Polar_Point(observer, observer);
+	split_top = Polar_Point(observer, observer);
+
+	if( ppoint2.bearing() == Angle(0.0) )
+	  ppoint2.set_bearing_to_2pi();
+	
+	left_wall_bearing = ppoint1.bearing();
+	right_wall_bearing = ppoint2.bearing();
+
+	elp.push_back(  Polar_Edge( ppoint1   , split_bottom )  );
+	elp.push_back(  Polar_Edge( split_top , ppoint2      )  );
+	continue;
+      }
+
+      //Else if the observer is on first vertex of edge.
+      else if( distance(observer, ppoint1) <= epsilon ){
+	if( ppoint2.bearing() == Angle(0.0) )
+	  ppoint2.set_bearing_to_2pi();
+	//Get right wall bearing.
+	right_wall_bearing = ppoint2.bearing();
+	elp.push_back(  Polar_Edge( Polar_Point(observer, observer),
+				    ppoint2 )  );
+	continue;
+      }
+      //Else if the observer is on second vertex of edge.
+      else if( distance(observer, ppoint2) <= epsilon ){
+	//Get left wall bearing.
+	left_wall_bearing = ppoint1.bearing();
+	elp.push_back(  Polar_Edge( ppoint1,
+				    Polar_Point(observer, observer) )  );	
+	continue;
+      }
+
+      //Otherwise the observer is not on the edge.
+
+      //If edge not horizontal (w/in epsilon).
+      else if(  std::fabs( ppoint1.y() - ppoint2.y() ) > 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<Polar_Point_With_Edge_Info> q1;
+    Polar_Point_With_Edge_Info ppoint_wei1, ppoint_wei2;
+    std::list<Polar_Edge>::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<Polar_Point_With_Edge_Info>::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<Polar_Edge>::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<std::list<Polar_Edge>::iterator,
+                        std::vector<std::list<Polar_Edge>::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<n_; i++)
+      adjacency_matrix_[i] = adjacency_matrix_[i-1] + n_;
+
+    //copy each entry
+    for(unsigned i=0; i<n_; i++){
+    for(unsigned j=0; j<n_; j++){
+      adjacency_matrix_[i][j] 
+	= vg2.adjacency_matrix_[i][j];
+    }}
+  }
+
+
+  Visibility_Graph::Visibility_Graph(const Environment& environment,
+				     double epsilon)
+  {
+    n_ = environment.n();
+
+    //fill vertex_counts_
+    vertex_counts_.reserve( environment.h() );
+    for(unsigned i=0; i<environment.h(); i++)
+      vertex_counts_.push_back( environment[i].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<n_; i++)
+      adjacency_matrix_[i] = adjacency_matrix_[i-1] + n_;
+    
+    // fill adjacency matrix by checking for inclusion in the
+    // visibility polygons
+    Polygon polygon_temp;
+    for(unsigned k1=0; k1<n_; k1++){
+      polygon_temp = Visibility_Polygon( environment(k1),
+					 environment,
+					 epsilon );
+      for(unsigned k2=0; k2<n_; k2++){
+	if( k1 == k2 )
+	  adjacency_matrix_[ k1 ][ k1 ] = true;
+	else
+	  adjacency_matrix_[ k1 ][ k2 ] =
+	    adjacency_matrix_[ k2 ][ k1 ] =    
+	    environment(k2).in( polygon_temp , epsilon ); 
+      }
+    }
+  }
+
+
+  Visibility_Graph::Visibility_Graph(const std::vector<Point> 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<n_; i++)
+      adjacency_matrix_[i] = adjacency_matrix_[i-1] + n_;
+    
+    // fill adjacency matrix by checking for inclusion in the
+    // visibility polygons
+    Polygon polygon_temp;
+    for(unsigned k1=0; k1<n_; k1++){
+      polygon_temp = Visibility_Polygon( points[k1],
+					 environment,
+					 epsilon );
+      for(unsigned k2=0; k2<n_; k2++){
+	if( k1 == k2 )
+	  adjacency_matrix_[ k1 ][ k1 ] = true;
+	else
+	  adjacency_matrix_[ k1 ][ k2 ] =
+	    adjacency_matrix_[ k2 ][ k1 ] =    
+	    points[k2].in( polygon_temp , epsilon ); 
+      }
+    }
+  }
+
+  
+  Visibility_Graph::Visibility_Graph(const Guards& guards,
+				     const Environment& environment, 
+				     double epsilon)
+  {
+    *this = Visibility_Graph( guards.positions_,
+			      environment, 
+			      epsilon );
+  }
+  
+  
+  bool Visibility_Graph::operator () (unsigned i1,
+				      unsigned j1,
+				      unsigned i2,
+				      unsigned j2) const 
+  {
+    return adjacency_matrix_[ two_to_one(i1,j1) ][ two_to_one(i2,j2) ];
+  }
+  bool Visibility_Graph::operator () (unsigned k1,
+				      unsigned k2) const 
+  {
+    return adjacency_matrix_[ k1 ][ k2 ];
+  }
+  bool& Visibility_Graph::operator () (unsigned i1,
+				       unsigned j1,
+				       unsigned i2,
+				       unsigned j2)
+  {
+    return adjacency_matrix_[ two_to_one(i1,j1) ][ two_to_one(i2,j2) ];
+  }
+  bool& Visibility_Graph::operator () (unsigned k1,
+				       unsigned k2)
+  {
+    return adjacency_matrix_[ k1 ][ k2 ];
+  }
+
+
+  Visibility_Graph& Visibility_Graph::operator = 
+  (const Visibility_Graph& visibility_graph_temp)
+  {
+    if( this == &visibility_graph_temp )
+      return *this;
+    
+    n_ = visibility_graph_temp.n_;
+    vertex_counts_ = visibility_graph_temp.vertex_counts_;
+
+    //resize adjacency_matrix_
+    if( adjacency_matrix_ != NULL ){
+      delete [] adjacency_matrix_[0];
+      delete [] adjacency_matrix_; 
+    }
+    adjacency_matrix_ = new bool*[n_];
+    adjacency_matrix_[0] = new bool[n_*n_];
+    for(unsigned i=1; i<n_; i++)
+      adjacency_matrix_[i] = adjacency_matrix_[i-1] + n_;
+
+    //copy each entry
+    for(unsigned i=0; i<n_; i++){
+    for(unsigned j=0; j<n_; j++){
+      adjacency_matrix_[i][j] 
+	= visibility_graph_temp.adjacency_matrix_[i][j];
+    }}
+    
+    return *this;
+  }
+  
+  
+  unsigned Visibility_Graph::two_to_one(unsigned i,
+					unsigned j) const
+  {
+    unsigned k=0;
+
+    for(unsigned counter=0; counter<i; counter++)
+      k += vertex_counts_[counter];
+    k += j;
+
+    return k;
+  }
+
+
+  Visibility_Graph::~Visibility_Graph()    
+  { 
+    if( adjacency_matrix_ != NULL ){
+      delete [] adjacency_matrix_[0];
+      delete [] adjacency_matrix_; 
+    }
+  }
+
+
+  std::ostream& operator << (std::ostream& outs,
+			     const Visibility_Graph& visibility_graph)
+  {
+    for(unsigned k1=0; k1<visibility_graph.n(); k1++){
+      for(unsigned k2=0; k2<visibility_graph.n(); k2++){
+	outs << visibility_graph( k1, k2 );
+	if( k2 < visibility_graph.n()-1 )
+	  outs << "  ";
+	else
+	  outs << std::endl;
+      }
+    }
+
+    return outs;
+  }
+
+}