Exemple #1
0
void SpatialStatistics(const PointSet &points, int npoints, Statistics *stats) {
#ifdef PSA_HAS_CGAL
    std::vector<Point_2> cgalPoints(npoints);
    for (int i = 0; i < npoints; ++i)
        cgalPoints[i] = Point_2(points[i].x, points[i].y);
    Delaunay dt(cgalPoints, true);
    dt.GetStatistics(stats);
#else
    Distances(points, npoints, &stats->mindist, &stats->avgmindist);
    stats->orientorder = 0;
#endif
}
Exemple #2
0
void test(){
    
    std::vector<Point_2> points;
    points.push_back(Point_2(0,0));
    points.push_back(Point_2(1,1));
    points.push_back(Point_2(0,1));
    
    DT dt2;
    //insert points into the triangulation
    dt2.insert(points.begin(),points.end());
    //construct a rectangle
    Iso_rectangle_2 bbox(-1,-1,2,2);
    Cropped_voronoi_from_delaunay vor(bbox);
    //extract the cropped Voronoi diagram
    dt2.draw_dual(vor);
    //print the cropped Voronoi diagram as segments
    
    
    std::copy(vor.m_cropped_vd.begin(),vor.m_cropped_vd.end(),
              std::ostream_iterator<Segment_2>(std::cout,"\n"));
    
}
void debug_func()
{
	Angle ang1( Point_2( 1 , 0 ) , Point_2( 0 , 0 ) , Point_2( -1 , 0 ) );
	Angle ang2( Point_2( -1 , 0 ) , Point_2( 0 , 0 ) , Point_2( 0 , -1 ) );
	Angle ang3( Point_2( 0 , -1 ) , Point_2( 0 , 0 ) , Point_2( 1 , 0 ) );
	Angle ang4 = ( ang1 + ang2 ) + ang3;
	std::cout << "ang1 = "<< ang1.getVal() << std::endl;
	std::cout << "ang2 = "<< ang2.getVal() << std::endl;
	std::cout << "ang3 = "<< ang3.getVal() << std::endl;
	std::cout << "ang4 = "<< ang4.getVal() << std::endl;
	std::cout << "rotation of ang4 is: R = "<< ang4.getR() << std::endl;
	system("pause");

						/*//debug
					if ( !tempL.isValid() )
					{
						std::cout<<"tempL problem\n";
						system ("pause");
					}*/
}
  bool is_selected (qglviewer::Vec& p)
  {
    if (domain_rectangle.xmin () < p.x &&
	p.x < domain_rectangle.xmax () &&
	domain_rectangle.ymin () < p.y &&
	p.y < domain_rectangle.ymax ())
      {
	if (rectangle)
	  return true;
	
	if (domain_freeform.has_on_bounded_side (Point_2 (p.x, p.y)))
	  return true;
      }
    return false;
  }
QList<Vector2D> Voronoi_Diagram::calculate(QList<Vector2D> in_points)
{
    //consider some points
    std::vector<Point_2> points;
    for(int i=0;i<in_points.size();i++)
    {
        points.push_back(Point_2(in_points.at(i).x,in_points.at(i).y));
    }

    Delaunay_triangulation_2 dt2;
    //insert points into the triangulation
    dt2.insert(points.begin(),points.end());
    //construct a rectangle
    Iso_rectangle_2 bbox(Field::MinX,Field::MinY,Field::MaxX,Field::MaxY);
    Cropped_voronoi_from_delaunay vor(bbox);
    //extract the cropped Voronoi diagram
    dt2.draw_dual(vor);
    //print the cropped Voronoi diagram as segments
    std::stringstream ss;

    QList<Vector2D> out;
    wm->voronoi.clear();

    for(int i=0;i<vor.m_cropped_vd.size();i++)
    {
        Segment_2 tmp = vor.m_cropped_vd.at(i);
        Point_2 start = tmp.vertex(0) , end = tmp.vertex(1);
        Vector2D first(floor(start.x()),floor(start.y())) , second(floor(end.x()),floor(end.y()));

        if( !out.contains(first) )
        {
            if( wm->kn->IsInsideField(first) )
                 out.append(first);
        }

        if( !out.contains(second) )
        {
            if( wm->kn->IsInsideField(second) )
               out.append(second);
        }

        Segment2D seg(first,second);
        wm->voronoi.append(seg);
    }

    return out;
}
TEST_F(ch_chanFixture, CheckSubHulls){
    Point_2 points[8];
    int i =0;
    points[i++] = Point_2(0,0);
    points[i++] = Point_2(0,1);
    points[i++] = Point_2(0,0.1);
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,1);
    points[i++] = Point_2(1,0.1);
    points[i++] = Point_2(2,0);
    vector<vector< Point_2 > > hulls = ch_chan::GetSubHulls(points,points+8,4);
    int actual = hulls[0].size();
    int expected = 4;
    EXPECT_EQ(actual,expected);
    actual = hulls[1].size();
    expected = 4;
    EXPECT_EQ(actual,expected);
}
Exemple #7
0
void ApplyObjects(Pmwx& ioMap)
{
	if (gFAAObs.empty()) return;

	Point_2	sw, ne;
	CalcBoundingBox(ioMap, sw, ne);
// 	ioMap.Index();

	int	placed = 0;

//	CGAL::Arr_landmarks_point_location<Arrangement_2>	locator(gMap);
	CGAL::Arr_walk_along_line_point_location<Arrangement_2>	locator(gMap);

	for (FAAObsTable::iterator i = gFAAObs.begin(); i != gFAAObs.end(); ++i)
	{
		if (i->second.kind != NO_VALUE)
		{
			Point_2 loc = Point_2(i->second.lon, i->second.lat);

			DebugAssert(CGAL::is_valid(gMap));
			CGAL::Object obj = locator.locate(loc);
			Face_const_handle ff;
			if(CGAL::assign(ff,obj))
			{
				Face_handle f = ioMap.non_const_handle(ff);
				GISPointFeature_t	feat;
					feat.mFeatType = i->second.kind;
					feat.mLocation = loc;
					if (i->second.agl != DEM_NO_DATA)
						feat.mParams[pf_Height] = i->second.agl;
					feat.mInstantiated = false;
					f->data().mPointFeatures.push_back(feat);
					++placed;
	#if 0
					printf("Placed %s at %lf, %lf\n",
						FetchTokenString(i->second.kind), i->second.lon, i->second.lat);
	#endif
//				if (v.size() > 1)
//					fprintf(stderr,"WARNING (%d,%d): Point feature %lf, %lf matches multiple areas.\n",gMapWest, gMapSouth, CGAL::to_double(loc.x()), CGAL::to_double(loc.y()));

			}
		}
	}
	printf("Placed %d objects.\n", placed);
}
Exemple #8
0
Obstacle
SSLPathPlanner::getGoalAsObstacle (ssl::FIELD_SIDES left_or_right)
{
  Obstacle obstacle;
  obstacle.type = ssl::GOAL;
  Rectancle_2 rectangle_upper;
  Rectancle_2 rectangle_bottom;

  if (left_or_right == ssl::LEFT)
  {
    rectangle_upper = Rectancle_2 (Point_2 (-3.050, 0.35), Point_2 (-3.025, 0.37));
    rectangle_bottom = Rectancle_2 (Point_2 (-3.050, -0.37), Point_2 (-3.025, -0.35));
  }
  else
  {
    rectangle_upper = Rectancle_2 (Point_2 (3.025, 0.35), Point_2 (3.050, 0.37));
    rectangle_bottom = Rectancle_2 (Point_2 (3.025, -0.37), Point_2 (3.050, -0.35));
  }

  Point upper_points[] = {rectangle_upper.vertex (0), rectangle_upper.vertex (1), rectangle_upper.vertex (2),
                          rectangle_upper.vertex (3)};

  Point lower_points[] = {rectangle_bottom.vertex (0), rectangle_bottom.vertex (1), rectangle_bottom.vertex (2),
                          rectangle_bottom.vertex (3)};

  //  Point upper_points[] = {Point (ssl::math::sign ((int8_t)left_or_right) * 3.050, 0.35),
  //                          Point (ssl::math::sign ((int8_t)left_or_right) * 3.050, 0.37),
  //                          Point (ssl::math::sign ((int8_t)left_or_right) * 3.025, 0.37),
  //                          Point (ssl::math::sign ((int8_t)left_or_right) * 3.025, 0.35)};
  //
  //  Point lower_points[] = {Point (ssl::math::sign ((int8_t)left_or_right) * 3.025, -0.35),
  //                          Point (ssl::math::sign ((int8_t)left_or_right) * 3.025, -0.37),
  //                          Point (ssl::math::sign ((int8_t)left_or_right) * 3.050, -0.35),
  //                          Point (ssl::math::sign ((int8_t)left_or_right) * 3.050, -0.37)};

  obstacle.polygons.push_back (Polygon_2 (upper_points, upper_points + 4));
  obstacle.polygons.push_back (Polygon_2 (lower_points, lower_points + 4));

  return obstacle;
}
Polygon_2 CollisionDetector::translate_polygon_to(const Polygon_2& poly, const Point_2& new_ref_pt) const
{
	m_translate_helper.resize(0);
	const Point_2 &ref = *poly.left_vertex();
	std::pair<double,double> diff(
	//Vector_2 diff(
		CGAL::to_double(ref.x().exact()) - CGAL::to_double(new_ref_pt.x().exact()), 
		CGAL::to_double(ref.y().exact()) - CGAL::to_double(new_ref_pt.y().exact())
	);

  	for (Polygon_2::Vertex_const_iterator it = poly.vertices_begin(), it_end = poly.vertices_end(); it != it_end; ++it)
  	{
		m_translate_helper.push_back( Point_2(CGAL::to_double(it->x()) + diff.first, CGAL::to_double(it->y()) + diff.second ) );
		//translated.push_back( (*it) + diff );
  	}



  	Polygon_2 new_poly(m_translate_helper.begin(),m_translate_helper.end());
  	return new_poly;
}
Exemple #10
0
Obstacle
SSLPathPlanner::getRobotAsObstacle (uint8_t team, uint8_t id, const geometry_msgs::Vector3& vel,
                                    const geometry_msgs::Vector3& acc)
{
  Obstacle obstacle = getRobotAsObstacle (team, id);
  //assuming linear velocity in one control cycle which is about 20ms.
  tf::Vector3 x_curr;
  tf::vector3MsgToTF (getRobotPosition (team, id), x_curr);

  tf::Vector3 v_curr;
  tf::vector3MsgToTF (vel, v_curr);

  tf::Vector3 acc_cmd;
  tf::vector3MsgToTF (acc, acc_cmd);

  tf::Vector3 x_next = x_curr + v_curr * ssl::config::TIME_STEP_SEC + 0.5 * acc_cmd * ssl::config::TIME_STEP_SEC
      * ssl::config::TIME_STEP_SEC;

  obstacle.circles.push_back (Circle_2 (Point_2 (x_next.x (), x_next.y ()), ssl::config::ROBOT_BOUNDING_RADIUS));

  //TODO fill in between these circles with a rectangular polygon
  // skipping this for now since, a robot in 20ms can move at most 6cm while moving with 3m/s
  return obstacle;
}
Point_2 Origin::operator-(const Vector_2& v) const {return Point_2( CGAL::ORIGIN-v.get_data() ); }
Exemple #12
0
PwhPtr CstmCGAL::applyOffset(double offset, const Polygon_with_holes_2& poly) {

    // This code is inspired from the CGAL example Straight_skeleton_2/Low_level_API
    // As the offset can only produce an interior polygon, we need to produce a frame
    // that encloses the polygon and is big enough so that the offset of the contour
    // does not interfere with the one ot the polygon. See CGAL doc page for more info
    boost::optional<double> margin = CGAL::compute_outer_frame_margin(
                                         poly.outer_boundary().vertices_begin(),poly.outer_boundary().vertices_end(),offset);

    if ( margin ) {

        CGAL::Bbox_2 bbox = CGAL::bbox_2(poly.outer_boundary().vertices_begin(),poly.outer_boundary().vertices_end());

        double fxmin = bbox.xmin() - *margin ;
        double fxmax = bbox.xmax() + *margin ;
        double fymin = bbox.ymin() - *margin ;
        double fymax = bbox.ymax() + *margin ;

        // Create the rectangular frame
        Point_2 frame[4]= { Point_2(fxmin,fymin)
                            , Point_2(fxmax,fymin)
                            , Point_2(fxmax,fymax)
                            , Point_2(fxmin,fymax)
                          } ;

        SsBuilder ssb ;

        ssb.enter_contour(frame,frame+4);

        // We have to revert the orientation of the polygon
        std::vector<Point_2> outerBoundary = std::vector<Point_2>(
                poly.outer_boundary().vertices_begin(),poly.outer_boundary().vertices_end());

        ssb.enter_contour(outerBoundary.rbegin(), outerBoundary.rend());

        SsPtr ss = ssb.construct_skeleton();

        if ( ss ) {
            std::vector<Polygon_2Ptr> offset_contours ;

            OffsetBuilder ob(*ss);

            ob.construct_offset_contours(offset, std::back_inserter(offset_contours));

            // Locate the offset contour that corresponds to the frame
            // That must be the outmost offset contour, which in turn must be the one
            // with the largest unsigned area.
            std::vector<Polygon_2Ptr>::iterator f = offset_contours.end();
            double lLargestArea = 0.0 ;
            for (std::vector<Polygon_2Ptr>::iterator i = offset_contours.begin(); i != offset_contours.end(); ++ i) {
                double lArea = CGAL_NTS abs( (*i)->area() ) ; //Take abs() as  Polygon_2::area() is signed.
                if ( lArea > lLargestArea ) {
                    f = i ;
                    lLargestArea = lArea ;
                }
            }

            offset_contours.erase(f);

            // Construct result polygon

            std::vector<Point_2> newOuterBoundary = std::vector<Point_2>(
                    offset_contours.front()->vertices_begin(), offset_contours.front()->vertices_end());

            Polygon_with_holes_2 result = Polygon_with_holes_2(Polygon_2(newOuterBoundary.rbegin(), newOuterBoundary.rend()));

            // We have to handle the holes separately

            for (auto it = poly.holes_begin() ; it != poly.holes_end() ; it++) {
                std::vector<Point_2> hole = std::vector<Point_2>(it->vertices_begin(),it->vertices_end());

                std::vector<PwhPtr> holeOffsets =
                    CGAL::create_interior_skeleton_and_offset_polygons_with_holes_2(offset,
                            Polygon_with_holes_2(Polygon_2(hole.begin(), hole.end())));

                for (auto it2 = holeOffsets.begin() ; it2 != holeOffsets.end() ; it++) {
                    std::vector<Point_2> revertNewHoles = std::vector<Point_2>(
                            (*it2)->outer_boundary().vertices_begin(),(*it2)->outer_boundary().vertices_end());

                    result.add_hole(Polygon_2(revertNewHoles.rbegin(), revertNewHoles.rend()));
                }
            }

            return boost::make_shared<Polygon_with_holes_2>(result);
        }
    }

    return NULL;
}
TEST_F(ch_chanFixture, FindLeftmostHull){
    Point_2 points[16];
    int i =0;
    //Single Point
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,0);
    //Two Points
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,1);
    points[i++] = Point_2(1,1);
    //Square
    points[i++] = Point_2(0,0);
    points[i++] = Point_2(0,-1);
    points[i++] = Point_2(-1,0.32); // This guy is the leftmost!!!
    points[i++] = Point_2(-0.9,-1);
    //Triangle with point in middle
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,1);
    points[i++] = Point_2(1,0.1);
    points[i++] = Point_2(2,0);
    vector<vector< Point_2 > > hulls = ch_chan::GetSubHulls(points,points+i,4);
    int hIndex, pIndex;
    ch_chan::FindLeftmostHull(hulls, hIndex, pIndex);
    int expected = 2;
    int actual = hIndex;
    EXPECT_EQ(expected, actual)<<"hIndex";
    expected = 0.32;
    actual = hulls[hIndex][pIndex].y();
    EXPECT_EQ(expected, actual)<<"pIndex";
}
TEST_F(ch_chanFixture, NextPair){
    vector<Point_2> square;
    //square
    square.push_back(Point_2(0,0));
    square.push_back(Point_2(1,-0.1));
    square.push_back(Point_2(1,1.2));
    square.push_back(Point_2(0.1,1));
    square.push_back(square[0]);

    vector<Point_2> triangle;
    //Triangle
    triangle.push_back(Point_2(2,-0.2));
    triangle.push_back(Point_2(3,0));
    triangle.push_back(Point_2(2,1.1));
    triangle.push_back(triangle[0]);

    vector<vector<Point_2> > hulls;
    hulls.push_back(square);
    hulls.push_back(triangle);
    int hIndex = 0;
    int pIndex = 0;
    int counter = 1;
    ch_chan::NextPair(hulls,hIndex,pIndex);
    int actualH = hIndex;
    int expectedH = 0;
    int actualP = pIndex;
    int expectedP = 1;
    EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
    EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
    counter++;
    ch_chan::NextPair(hulls,hIndex,pIndex);
    actualH = hIndex;
    expectedH = 1;
    actualP = pIndex;
    expectedP = 0;
    EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
    EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
    counter++;
    ch_chan::NextPair(hulls,hIndex,pIndex);
    actualH = hIndex;
    expectedH = 1;
    actualP = pIndex;
    expectedP = 1;
    EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
    EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
    counter++;
    ch_chan::NextPair(hulls,hIndex,pIndex);
    actualH = hIndex;
    expectedH = 1;
    actualP = pIndex;
    expectedP = 2;
    EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
    EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
    counter++;
    ch_chan::NextPair(hulls,hIndex,pIndex);
    actualH = hIndex;
    expectedH = 0;
    actualP = pIndex;
    expectedP = 2;
    EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
    EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
    counter++;
    ch_chan::NextPair(hulls,hIndex,pIndex);
    actualH = hIndex;
    expectedH = 0;
    actualP = pIndex;
    expectedP = 3;
    EXPECT_EQ(expectedH,actualH)<<"Point("<<counter<<"): h";
    EXPECT_EQ(expectedP,actualP)<<"Point("<<counter<<"): p";
    counter++;
}
bool LocalPlanner::local_planner_two_robot(const Point_2& start_r1,
		                                         const Point_2& target_r1,
		                                         const Point_2& start_r2,
		                                         const Point_2& target_r2,
		                                         double eps ) const
{
	//robot 1 calc
	double x1_r1 = CGAL::to_double(start_r1.x());
	double y1_r1 = CGAL::to_double(start_r1.y());
	double x2_r1 = CGAL::to_double(target_r1.x());
	double y2_r1 = CGAL::to_double(target_r1.y()) ;

	double distance_r1 = sqrt(pow(x1_r1 - x2_r1, 2) + pow(y1_r1 - y2_r1,2));
	double step_size = eps;

	// calculate how many steps are required for first robot
	int step_num_r1 = floor((distance_r1 - step_size) / step_size);
	double vx_r1 = x2_r1 - x1_r1;
	double vy_r1 = y2_r1 - y1_r1;


	//robot 2 calc
	double x1_r2 = CGAL::to_double(start_r2.x());
	double y1_r2 = CGAL::to_double(start_r2.y());
	double x2_r2 = CGAL::to_double(target_r2.x());
	double y2_r2 = CGAL::to_double(target_r2.y()) ;

	double distance_r2 = sqrt(pow(x1_r2 - x2_r2, 2) + pow(y1_r2 - y2_r2,2));

	// calculate how many steps are required for second robot
	int step_num_r2 = floor((distance_r2 - step_size) / step_size);
	double vx_r2 = x2_r2 - x1_r2;
	double vy_r2 = y2_r2 - y1_r2;

	//select the largest number of steps of both robot for loop iteration check
	double step_num =  step_num_r1 > step_num_r2 ? step_num_r1 : step_num_r2;

	double step_size_r1 = distance_r1/step_num;
	double step_size_r2 = distance_r2/step_num;

	std::vector<double> coords;

	for (int i = 1; i <= step_num; ++i)
	{
		// generate a configuration for every step
		double offset_r1 =  (i * step_size_r1) / (distance_r1 - step_size_r1);
		double currx_r1 = x1_r1 + vx_r1 * offset_r1;
		double curry_r1 = y1_r1 + vy_r1 * offset_r1;

		double offset_r2 =  (i * step_size_r2) / (distance_r2 - step_size_r2);
		double currx_r2 = x1_r2 + vx_r2 * offset_r2;
		double curry_r2 = y1_r2 + vy_r2 * offset_r2;

		if( !m_cd.valid_conf( Point_2(currx_r1,curry_r1),Point_2(currx_r2,curry_r2) ) ) return false;

		// If an intermidiate configuration is invalid, return false
		//if (!m_cd.one_robot_valid_conf(currentPos,obstacles)) return false;
	}

	// GREAT SUCCESS!
	return true;
}
CollisionDetector::CollisionDetector(Polygon_2 robot1, Polygon_2 robot2, Obstacles* obs,double eps)
: approx_robot1(robot1)
, approx_robot2(robot2)
, m_obs(obs)
, m_translate_helper()
, m_minus_r1(flip(robot1))
, m_minus_r2(flip(robot2))
, m_epsilon(eps)
{
	Polygon_2 enlarger;
	int nOfEdges = 8;
	double radius = eps/2;
	double dAlpha = (360.0/nOfEdges)*CGAL_PI/180;
	for (int i = nOfEdges; i>0; i--) {
		double alpha = (i + 0.5)*dAlpha;
		double x = (radius/cos(dAlpha/2)) * cos(alpha) * 1.05;
		double y = (radius/cos(dAlpha/2)) * sin(alpha) * 1.05;
		enlarger.push_back(Point_2(x, y));
	}
	
	// Compute the Minkowski sum using the decomposition approach.
	CGAL::Small_side_angle_bisector_decomposition_2<Kernel>  ssab_decomp;

	Polygon_with_holes_2  pwh1 = minkowski_sum_2 (robot1, enlarger, ssab_decomp);
	Polygon_with_holes_2  pwh2 = minkowski_sum_2 (robot2, enlarger, ssab_decomp);
		
	approx_robot1 = pwh1.outer_boundary();
	approx_robot2 = pwh2.outer_boundary();

	Polygon_with_holes_2  pwhF1 = minkowski_sum_2 (m_minus_r1, enlarger, ssab_decomp);
	Polygon_with_holes_2  pwhF2 = minkowski_sum_2 (m_minus_r2, enlarger, ssab_decomp);

	m_minus_r1_en = pwhF1.outer_boundary();
	m_minus_r2_en = pwhF2.outer_boundary();


	Polygon_set_2 ps;
	if (!m_obs->empty())
	{
		for (Obstacles::iterator Oiter = m_obs->begin(); Oiter != m_obs->end(); Oiter++)
		{		
			  // For every obstacle calculate its Minkowski sum with a "robot"
			Polygon_with_holes_2  poly_wh1 = minkowski_sum_2 (*Oiter, m_minus_r1_en, ssab_decomp);		
			Polygon_with_holes_2  poly_wh2 = minkowski_sum_2 (*Oiter, m_minus_r2_en, ssab_decomp);		
			// Add the result to the polygon set
			m_r1_poly_set.join(poly_wh1);
			m_r2_poly_set.join(poly_wh2);
		}
	}
	
	Polygon_with_holes_2  r1_r2 = minkowski_sum_2 (approx_robot1, m_minus_r2_en, ssab_decomp);
	Polygon_2 for_print = r1_r2.outer_boundary();
	for(Polygon_2::Vertex_const_iterator it = for_print.vertices_begin(); it != for_print.vertices_end(); ++it)
	{
		std::cout << "(" << it->x() << "," << it->y() << "),";
	}
	std::cout << std::endl;

	m_r1_min_r2.join(r1_r2);
	
	Polygon_with_holes_2  r2_r1 = minkowski_sum_2 (approx_robot2, m_minus_r1_en, ssab_decomp);
	m_r2_min_r1.join(r2_r1);
}
Exemple #17
0
geoData mapload(char* path,VisiLibity::Environment & mapEnv,VisiLibity::Visibility_Graph & visGraph,float clearDist )
{


    std::ifstream in(path);
    std::string s;
    std::string s1="";

    while(getline(in, s)) { // Discards newline char
    s1=s1+s+"\n";
    }

    std::vector<char> xml_copy(s1.begin(), s1.end());
    xml_copy.push_back('\0');

    rapidxml::xml_document<> doc;    // character type defaults to char
    doc.parse<0>(&xml_copy[0]);    // 0 means default parse flags

    //std::cout << "Name of my first node is: " << doc.first_node()->name() << "\n";

    rapidxml::xml_node<char> *n1;

    n1=doc.first_node("svg");
    n1=n1->first_node("g")->first_node("path");

    //std::vector < VisiLibity::Point > poly_temp;

    //geoData vertices_temp(10,poly_temp);
    geoData vertices_temp;

    int i=0;

    while (n1)
    {
        vertices_temp.push_back(loadPath(n1->first_attribute("d")->value()));
        n1=n1->next_sibling("path");
        i++;
    }


   Polygon_2 mapEnvOB;

    for (int j=0;j<vertices_temp[0].size();j++)
        {
            Point_2 nextVert= Point_2(vertices_temp[0][j][0],vertices_temp[0][j][1]);
            mapEnvOB.push_back(nextVert);
        };


        Polygon_set_2 S;
    mapEnvOB.reverse_orientation();

        S.insert(mapEnvOB);



    for (int k=1;k<vertices_temp.size();k++)
    {
        Polygon_2 holeCGAL;
        for (int j=0;j<vertices_temp[k].size();j++)
        {
            holeCGAL.push_back(Point_2(vertices_temp[k][j][0],vertices_temp[k][j][1]));
        };

        //holeCGAL.reverse_orientation();

        S.difference(holeCGAL);

        holeCGAL.clear();
    };

  std::list<Polygon_with_holes_2> res;
  std::list<Polygon_with_holes_2>::const_iterator it;


   S.polygons_with_holes (std::back_inserter (res));

    std::vector<VisiLibity::Polygon> envPolys;

   for (it = res.begin(); it != res.end(); ++it)
   {
      if(CGAL::ON_BOUNDED_SIDE==CGAL::bounded_side_2(it->outer_boundary().vertices_begin(),it->outer_boundary().vertices_end(),Point_2(guest1.pos.x(),guest1.pos.y()),Kernel()))
      {
        envPolys.push_back(ConvertPolygonCGAL2Vis(it->outer_boundary()));

        Polygon_with_holes_2::Hole_const_iterator hi;
        for (hi=it->holes_begin();hi!=it->holes_end();++hi)
        {
            envPolys.push_back(ConvertPolygonCGAL2Vis(*hi));
        };

        break;
      };

  }


    for (int i=0;i<envPolys.size();i++){
        //envPolys.push_back(VisiLibity::Polygon(vertices_temp[i]));
        //i=0;
        envPolys[i].eliminate_redundant_vertices(0.0001);
        VisiLibity::Point cm=envPolys[i].centroid();
            for (int j=0;j<envPolys[i].n();j++)
            {
                    if (j<envPolys[i].n()-1){
                        VisiLibity::Point n1=clearDist*normal(envPolys[i][j+1]-envPolys[i][j]);
                        envPolys[i][j]=envPolys[i][j]+n1;
                        envPolys[i][j+1]=envPolys[i][j+1]+n1;
                    }
            }
            VisiLibity::Point norm1=clearDist*normal(envPolys[i][0]-envPolys[i][envPolys[i].n()-1]);
            envPolys[i][0]=envPolys[i][0]+norm1;
            envPolys[i][envPolys[i].n()-1]=envPolys[i][envPolys[i].n()-1]+norm1;
    };

    mapEnv = *(new VisiLibity::Environment(envPolys));
    mapEnv.enforce_standard_form();

    visGraph = *(new VisiLibity::Visibility_Graph(mapEnv,0.00000001));
    return vertices_temp;

};
void CKWResearchWorkDoc::OnHelpTest()
{
	// TODO: Add your command handler code here
	vector<Point_3> SamplePoints;
	GeometryAlgorithm::SampleCircle(Point_3(0,0,0),0.3,20,SamplePoints);
	FILE* pfile=fopen("circle0.contour","w");
	for (unsigned int i=0;i<SamplePoints.size();i++)
	{
		fprintf(pfile,"%.3f %.3f %.3f\n",SamplePoints.at(i).x(),SamplePoints.at(i).y(),SamplePoints.at(i).z());
	}
	fclose(pfile);




	Polygon_2 PolyTest;
	PolyTest.push_back(Point_2(0,0));
	PolyTest.push_back(Point_2(1,1));
	PolyTest.push_back(Point_2(2,0));
	PolyTest.push_back(Point_2(2,2));
	PolyTest.push_back(Point_2(0,2));

	Point_2 ResultPoint;
	bool bResult=GeometryAlgorithm::GetArbiPointInPolygon(PolyTest,ResultPoint);
	DBWindowWrite("result point: %f %f\n",ResultPoint.x(),ResultPoint.y());



	SparseMatrix LHMatrix(2);
	LHMatrix.m = 3;
	LHMatrix[0][0] = 2;
	LHMatrix[0][1] = -1;
	LHMatrix[0][2] = 1;
	LHMatrix[1][1] = 1;
	LHMatrix[1][2] = 1;

	SparseMatrix LHMatrixAT(LHMatrix.NCols());
	CMath MathCompute;
	MathCompute.TAUCSFactorize(LHMatrix,LHMatrixAT);

	vector<vector<double>> RightMatB,ResultMat;
	vector<double> BRow;
	BRow.push_back(5);BRow.push_back(3);
	RightMatB.push_back(BRow);
	BRow.clear();
	BRow.push_back(10);BRow.push_back(1);
	RightMatB.push_back(BRow);
	BRow.clear();


	MathCompute.TAUCSComputeLSE(LHMatrixAT,RightMatB,ResultMat);

	return;

	int iVer=this->Mesh.size_of_vertices();
	int iEdge=this->Mesh.size_of_halfedges();
	int iFacet=this->Mesh.size_of_facets();

	Polygon_2 BoundingPolygon0;
	Polygon_2 BoundingPolygon1;

	bool bSimple0=BoundingPolygon0.is_simple();
	bool bSimple1=BoundingPolygon1.is_simple();
	bool bConvex0=BoundingPolygon0.is_convex();
	bool bConvex1=BoundingPolygon1.is_convex();
	bool bOrien0=BoundingPolygon0.is_clockwise_oriented();
	bool bOrien1=BoundingPolygon1.is_clockwise_oriented();

	BoundingPolygon0.reverse_orientation();
	BoundingPolygon1.reverse_orientation();
	//float?
	bool bIntersect=CGAL::do_intersect(BoundingPolygon0,BoundingPolygon1);

	bool bCW=BoundingPolygon0.is_clockwise_oriented();
	bool bConvex=BoundingPolygon0.is_convex();


	Plane_3 plane(1,1,1,0);
	vector<vector<Point_3>> IntersectCurves;
	int iNum=GeometryAlgorithm::GetMeshPlaneIntersection(this->Mesh,plane,IntersectCurves);



	CMath CMathTest;
	CMathTest.testTAUCS();



//	Vector_3 vec0(Point_3(0,0,1),Point_3(0,0,0));
	Vector_3 vec0(Point_3(0,0,0),Point_3(0,0,1));
//	Vector_3 vec0(0,0,1);
//	Vector_3 vec1(0,-1,-1);
	Vector_3 vec1(Point_3(0,0,0),Point_3(0,-1,-1));
	double dAngle=GeometryAlgorithm::GetAngleBetweenTwoVectors3d(vec0,vec1);


	vector<double> number;
	number.push_back(2);
	number.push_back(4);
	number.push_back(4);
	number.push_back(4);
	number.push_back(5);
	number.push_back(5);
	number.push_back(7);
	number.push_back(9);
	double dderi=GeometryAlgorithm::GetDerivation(number);





	Point_3 center(0,0,0);
	Sphere_3 sphere(center,1);
	Line_3 line(Point_3(0,2,1),Point_3(0,2,-1));
	vector<Point_3> points;
	GeometryAlgorithm::GetLineSphereIntersection(line,sphere,points);



	vector<Point_3> Group0,Group1;
	vector<Int_Int_Pair> GroupResult;

	Group0.push_back(Point_3(3,3,2));
	Group0.push_back(Point_3(5,3,2));
	Group0.push_back(Point_3(2,3,2));
	Group0.push_back(Point_3(6,3,2));
	Group0.push_back(Point_3(4,3,2));
	Group0.push_back(Point_3(1,3,2));

	Group1.push_back(Point_3(3,4,2));
	Group1.push_back(Point_3(1,4,2));
	Group1.push_back(Point_3(6,4,2));
	Group1.push_back(Point_3(2,4,2));
	Group1.push_back(Point_3(5,4,2));
	Group1.push_back(Point_3(4,4,2));

	vector<Point_3> vecMidPoint;
	GeometryAlgorithm::GroupNearestPoints(Group0,Group1,GroupResult,vecMidPoint);//

	vector<Point_3> OriginalCurve;
	OriginalCurve.push_back(Point_3(-1,0,2));
	OriginalCurve.push_back(Point_3(1,0,2));
	OriginalCurve.push_back(Point_3(1,0,0));
	OriginalCurve.push_back(Point_3(-1,0,0));
	vector<Point_3> NewCurve=OriginalCurve;
	vector<int> HandleInd;
	HandleInd.push_back(0);
	HandleInd.push_back(1);
	vector<Point_3> NewPos;
	NewPos.push_back(Point_3(-1,0,3));
	NewPos.push_back(Point_3(1,0,3));
//	CCurveDeform::ClosedCurveNaiveLaplacianDeform(NewCurve,HandleInd,NewPos,1);




	vector<vector<float>> MatrixA,MatrixB,Result;
	vector<float> CurrentRow;
	CurrentRow.push_back(2);CurrentRow.push_back(0);CurrentRow.push_back(1);
	MatrixA.push_back(CurrentRow);
	CurrentRow.clear();
	CurrentRow.push_back(3);CurrentRow.push_back(1);CurrentRow.push_back(2);
	MatrixA.push_back(CurrentRow);
	CurrentRow.clear();
	CurrentRow.push_back(0);CurrentRow.push_back(1);CurrentRow.push_back(0);
	MatrixA.push_back(CurrentRow);
	CurrentRow.clear();

	CurrentRow.push_back(1);CurrentRow.push_back(3);CurrentRow.push_back(0);
	MatrixB.push_back(CurrentRow);
	CurrentRow.clear();
	CurrentRow.push_back(2);CurrentRow.push_back(0);CurrentRow.push_back(2);
	MatrixB.push_back(CurrentRow);
	CurrentRow.clear();
	CurrentRow.push_back(1);CurrentRow.push_back(0);CurrentRow.push_back(1);
	MatrixB.push_back(CurrentRow);
	CurrentRow.clear();



	int iANonZeroSize=0;
	for (unsigned int i=0;i<MatrixA.size();i++)
	{
		for (unsigned int j=0;j<MatrixA.front().size();j++)
		{
			if (MatrixA.at(i).at(j)!=0)
			{
				iANonZeroSize++;
			}
		}
	}
	KW_SparseMatrix A(MatrixA.size(),MatrixA.front().size(),iANonZeroSize);
	for (unsigned int i=0;i<MatrixA.size();i++)
	{
		for (unsigned int j=0;j<MatrixA.front().size();j++)
		{
			if (MatrixA.at(i).at(j)!=0)
			{
				A.fput(i,j,MatrixA.at(i).at(j));
			}
		}
	}

	int iBNonZeroSize=0;
	for (unsigned int i=0;i<MatrixB.size();i++)
	{
		for (unsigned int j=0;j<MatrixB.front().size();j++)
		{
			if (MatrixB.at(i).at(j)!=0)
			{
				iBNonZeroSize++;
			}
		}
	}
	KW_SparseMatrix B(MatrixB.size(),MatrixB.front().size(),iBNonZeroSize);
	for (unsigned int i=0;i<MatrixB.size();i++)
	{
		for (unsigned int j=0;j<MatrixB.front().size();j++)
		{
			if (MatrixB.at(i).at(j)!=0)
			{
				B.fput(i,j,MatrixB.at(i).at(j));
			}
		}
	}


	KW_SparseMatrix C(A.m,B.n,0);
	KW_SparseMatrix::KW_multiply(A,B,C);

	int iIndex=0;
	Result.clear();
	for (int i=0;i<C.m;i++)
	{
		vector<float> CurrentRow;
		for (int j=0;j<C.n;j++)
		{
			iIndex=0;
			while (iIndex<C.vol)
			{
				if (C.indx[iIndex]==i&&C.jndx[iIndex]==j)
				{
					break;
				}
				iIndex++;
			}
			if (iIndex!=C.vol)
			{
				CurrentRow.push_back(C.array[iIndex]);
			}
			else
			{
				CurrentRow.push_back(0);
			}
		}
		Result.push_back(CurrentRow);
	}
}
Exemple #19
0
// this method overrides the virtual method 'draw()' of Qt_widget_layer
void Qt_layer_show_ch::draw()
{
  widget->lock(); // widget has to be locked before drawing
  RasterOp old_rasterop = widget->rasterOp();
  widget->get_painter().setRasterOp(XorROP);
  widget->setFilled (true);
  widget->setFillColor (CGAL::RED);  
  *widget <<  CGAL::BLACK; 
  std::list<Polygon_with_holes> red_pgns_list;
  myWin->red_set.polygons_with_holes(std::back_inserter(red_pgns_list));
  std::list<Polygon_with_holes>::iterator itpgn1 = red_pgns_list.begin();

  while (itpgn1 != red_pgns_list.end())
    {
      const Polygon_with_holes& pgn_with_hole = *itpgn1;
      const Polygon_2& outer_boundary = pgn_with_hole.outer_boundary();
      if (outer_boundary.is_empty()) 
	{
	  // no boundary -> unbounded polygon
	  Iso_rectangle rect(Point_2(widget->x_min(), widget->y_min()),
			     Point_2(widget->x_max(), widget->y_max()));
	  *widget << rect;
	}
      else
	*widget << outer_boundary;

      for(Hole_const_iterator hit = pgn_with_hole.holes_begin();
          hit != pgn_with_hole.holes_end();
          ++hit)
	{
	  *widget << *hit;
	}
      ++itpgn1;
    }


  widget->setFilled (true);
  widget->setFillColor (CGAL::BLUE);

  std::list<Polygon_with_holes> blue_pgns_list;
  myWin->blue_set.polygons_with_holes(std::back_inserter(blue_pgns_list));
  std::list<Polygon_with_holes>::iterator itpgn2 = blue_pgns_list.begin();

  while (itpgn2 != blue_pgns_list.end())
    {
      const Polygon_with_holes& pgn_with_hole = *itpgn2;
      const Polygon_2& outer_boundary = pgn_with_hole.outer_boundary();
      if (outer_boundary.is_empty())
	{
	  // no boundary -> unbounded polygon
	  Iso_rectangle rect(Point_2(widget->x_min(), widget->y_min()),
			     Point_2(widget->x_max(), widget->y_max()));
	  *widget << rect;
	}
      else
	{
	  *widget << outer_boundary;
	}

      for(Hole_const_iterator hit = pgn_with_hole.holes_begin();
          hit != pgn_with_hole.holes_end();
          ++hit)
	{
	  *widget << *hit;
	}
      ++itpgn2;
    }
  widget->get_painter().setRasterOp(old_rasterop);
  widget->setFilled (false);
  widget->unlock(); // widget have to be unlocked when finished drawing
}
Exemple #20
0
void SubSelectIpelet::protected_run(int fn)
{
  if (fn==2) {
    show_help();
    return;
  }
  
  std::list<Circle_2> cir_list;
  std::list<Polygon_2> pol_list;
  
  Iso_rectangle_2 bbox=
    read_active_objects(
      CGAL::dispatch_or_drop_output<Polygon_2,Circle_2>(
        std::back_inserter(pol_list),
        std::back_inserter(cir_list)
      )
    );  
  
  
  if (fn==0 && pol_list.size()!=2){
    print_error_message("You must select exactly two polygons");
    return;
  }  
  
  
  std::list<double> r_offsets;
  for (std::list<Circle_2>::iterator it=cir_list.begin();it!=cir_list.end();++it)
    r_offsets.push_back(sqrt(CGAL::to_double(it->squared_radius())));
  
  IpeMatrix tfm (1,0,0,1,-CGAL::to_double(bbox.min().x()),-CGAL::to_double(bbox.min().y()));
  
  for (std::list<Polygon_2>::iterator it=pol_list.begin();it!=pol_list.end();++it)
    if(!it->is_simple()){
      print_error_message("Polygon(s) must be simple");
    }
  
  
  if (fn==0){
    Polygon_2 polygon1=*pol_list.begin();
    Polygon_2 polygon2=*++pol_list.begin();
    Polygon_with_holes_2  sum = minkowski_sum_2 (polygon1, polygon2);
    std::list<Point_2> LP;
    for (Polygon_2::iterator it=sum.outer_boundary().vertices_begin();it!= sum.outer_boundary().vertices_end();++it)
      LP.push_back(*it);
    draw_polyline_in_ipe(LP.begin(),LP.end(),true,false,false);
    
    for (Polygon_with_holes_2::Hole_const_iterator poly_it = sum.holes_begin(); poly_it != sum.holes_end();
          ++poly_it){
      LP.clear();
      for (Polygon_2::iterator it=poly_it->vertices_begin();it!= poly_it->vertices_end();++it)
        LP.push_back(*it);
      draw_polyline_in_ipe(LP.begin(),LP.end(),true,false,false);
    }
    
    create_polygon_with_holes(true);
    transform_selected_objects_(tfm);
  }
  else{
    if (r_offsets.size()==0)
      r_offsets.push_back(10);
    for (std::list<Polygon_2>::iterator it_pol=pol_list.begin();it_pol!=pol_list.end();++it_pol){
      for(std::list<double>::iterator it=r_offsets.begin();it!=r_offsets.end();++it){
        Offset_polygon_with_holes_2  offset=approximated_offset_2 (*it_pol, *it, 0.0001);
        std::list<Segment_2> LS;
        for( Offset_polygon_2::Curve_iterator itt=offset.outer_boundary().curves_begin();
          itt!=offset.outer_boundary().curves_end();++itt){
          Point_2 S=Point_2(CGAL::to_double(itt->source().x()),CGAL::to_double(itt->source().y()));
          Point_2 T=Point_2(CGAL::to_double(itt->target().x()),CGAL::to_double(itt->target().y()));
          if (itt->is_linear ())
            LS.push_back(Segment_2(S,T));
          if (itt->is_circular())
            draw_in_ipe(Circular_arc_2(itt->supporting_circle(),S,T,itt->supporting_circle().orientation()));
        }
        draw_in_ipe(LS.begin(),LS.end());
      }
    }
  }
}
TEST_F(ch_chanFixture, CheckSubHullsOddNumberOfPoints){
    Point_2 points[16];
    int i =0;
    //Single Point
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,0);
    //Two Points
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,1);
    points[i++] = Point_2(1,1);
    //Triangle with point in middle
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,1);
    points[i++] = Point_2(1,0.1);
    points[i++] = Point_2(2,0);
    //Square
    points[i++] = Point_2(0,0);
    points[i++] = Point_2(0,1);
    points[i++] = Point_2(1,0);
    points[i++] = Point_2(1,1);
    vector<vector< Point_2 > > hulls = ch_chan::GetSubHulls(points,points+i,4);
    for(int i =0;i<4;i++){
        int expected = i+2;
        int actual = hulls[i].size();
        EXPECT_EQ(expected, actual);
    }
}
inline	Point_2 RawCoordToCoord(const RawCoordPair& p) { return Point_2(atof(p.second.c_str())/* / 1000000.0*/, atof(p.first.c_str())/* / 1000000.0*/); }
Exemple #23
0
int main(int argc , char* argv[])
{
  if (argc < 7) {
    std::cerr << "usage: test <input file> <output folder> "
      "<format (wkt | geojson | sql)> <weight city> <weight town> <weight village>" << std::endl;
    exit(1);
  }

  char* input = argv[1];
  char* outdir = argv[2];
  char* format = argv[3];

  char* swc = argv[4];
  char* swt = argv[5];
  char* swv = argv[6];

  std::ifstream ifs(input);
  assert( ifs );

  double wc, wt, wv;
  std::istringstream stmc, stmt, stmv;
  stmc.str(swc);
  stmc >> wc;
  stmt.str(swt);
  stmt >> wt;
  stmv.str(swv);
  stmv >> wv;

  std::cerr << "using weights: city: " << wc << ", town: " << wt << ", village: " << wv << std::endl;

  /*
   * prepare data
   */

  // we use a ScalingFactor(SF) here to stretch input values at the
  // beginning, and divide by SF in the end. This is used because the
  // point-generation of the hyperbola class is using some arbitrary
  // internal decision thresholds to decide how many points to generate for
  // a certain part of the curve. Rule of thumb is: the higher SF the more
  // detail is used in approximation of the hyperbolas.
  double SF = 4000;

  // read in sites from input file
  SiteList sites = readSites(ifs, SF, wc, wt, wv);

  printSites(sites);

  // calculate bounding box of all input sites (and extend it a little).
  // Extension is important, because we later add artificial sites which are
  // actually mirrored on the bounds of this rectangle. If we did not extend
  // some points would lie on the boundary of the bounding box and so would
  // their artificial clones. This would complicate the whole stuff a lot :)
  Iso_rectangle_2 crect = extend(boundingBox(sites), 0.1*SF);
  std::cerr << "rect: " << crect << std::endl;

  // a number of artificial sites
  SiteList artificialSites = createArtificialSites(sites, crect);

  /*
   * create Apollonius graph
   */

  Apollonius_graph ag;

  SiteList::iterator itr;
  // add all original sites to the apollonius graph
  for (itr = sites.begin(); itr != sites.end(); ++itr) {
    Site_2 site = *itr;
    ag.insert(site);
  }
  // add all artificial sites to the apollonius graph
  for (itr = artificialSites.begin(); itr != artificialSites.end(); ++itr) {
    Site_2 site = *itr;
    ag.insert(site);
  }

  // validate the Apollonius graph
  assert( ag.is_valid(true, 1) );
  std::cerr << std::endl;

  /*
   * create polygons from cells
   */

  // we want an identifier for each vertex within the iteration.
  // this is a loop iteration counter
  int vertexIndex = 0;

  // for each vertex in the apollonius graph (this are the sites)
  for (All_vertices_iterator viter = ag.all_vertices_begin ();
      viter != ag.all_vertices_end(); ++viter) {
    // get the corresponding site
    Site_2 site = viter->site();
    Point_2 point = site.point();
    // ignore artifical sites, detect them by their position
    if (!CGAL::do_intersect(crect, point)) {
      continue;
    }
    std::cerr << "vertex " << ++vertexIndex << std::endl;

    // we than circulate all incident edges. By obtaining the respective
    // dual of each edge, we get access to the objects forming the boundary
    // of each voronoi cell in a proper order.
    Edge_circulator ecirc = ag.incident_edges(viter), done(ecirc);
    // this is where we store the polylines
    std::vector<PointList> polylines;
    // for each incident edge
    do {
      // the program may fail in certain situations without this test.
      // acutally !is_infinite(edge) is a precondition in ag.dual(edge).
      if (ag.is_infinite(*ecirc)) {
        continue;
      }
      // NOTE: for this to work, we had to make public the dual function in ApolloniusGraph
      // change line 542 in "Apollonius_graph_2.h" from "private:" to "public:"
      Object_2 o = ag.dual(*ecirc);
      handleDual(o, crect, polylines);
    } while(++ecirc != done);

    PointList polygon = buildPolygon(site, polylines);
    for (int i = 0; i < polygon.size(); i++) {
      Point_2& p = polygon.at(i);
      p = Point_2(p.x()/SF, p.y()/SF);
    }
    if(std::string(format) == "geojson") {
      writeGeoJSON(site, polygon, outdir);
    } else if(std::string(format) == "sql") {
      writeSQL(site, polygon, outdir);
    } else {
      writeWKT(site, polygon, outdir);
    }

    // check each point
    for (int i = 0; i < polygon.size(); i++) {
      Point_2 p = polygon.at(i);
      if (p.x() > crect.xmax()/SF || p.x() < crect.xmin()/SF || p.y() > crect.ymax()/SF || p.y() < crect.ymin()/SF) {
        std::cerr << "out of bounds" << std::endl;
      }
    }
  }
}