C++ Polygon_2::clear方法代码示例

void decomposePolygonCgal(geometry_msgs::Polygon& input, std::vector<geometry_msgs::Polygon>& output)
{
    Polygon_list cgalOutputPolys;
    Traits partitionTraits;
    Validity_traits validityTraits;

    // Convert from ROS-polygon to CGAL polygon
    Polygon_2 cgalInput;

    for (int i = 0; i < input.points.size(); i++)
    {
        cgalInput.push_back(Point_2(input.points[i].x, input.points[i].y));
    }

    // Make sure we have a COUNTERCLOCKWISE polygon
    if (cgalInput.orientation() == CGAL::CLOCKWISE)
    {
        cgalInput.clear();
        for (int i = input.points.size() - 1; i >= 0; i--)
        {
            cgalInput.push_back(Point_2(input.points[i].x, input.points[i].y));
        }
    }

    // Do the decomposition using CGAL
    CGAL::optimal_convex_partition_2(
        cgalInput.vertices_begin(), cgalInput.vertices_end(), std::back_inserter(cgalOutputPolys), partitionTraits);

    std::cout << "CHECK output data!" << std::endl;
    assert(CGAL::partition_is_valid_2(cgalInput.vertices_begin(),
                                      cgalInput.vertices_end(),
                                      cgalOutputPolys.begin(),
                                      cgalOutputPolys.end(),
                                      validityTraits));

    // Now walk through the result and convert to ROS
    for (Polygon_iterator poly_it = cgalOutputPolys.begin(); poly_it != cgalOutputPolys.end(); poly_it++)
    {
        Polygon_2 p = *poly_it;

        geometry_msgs::Polygon outputPoly;

        // Iterate through the points for a polygon
        for (Vertex_iterator vi = p.vertices_begin(); vi != p.vertices_end(); ++vi)
        {
            Point_2 v = *vi;
            geometry_msgs::Point32 pt;
            pt.x = v.x();
            pt.y = v.y();

            outputPoly.points.push_back(pt);
        }

        output.push_back(outputPoly);
    }
}

//---------------
void cgalPolyFromRos(geometry_msgs::Polygon& input, Polygon_2& output)
{
    std::cout << "Converting to CGAL polygon... " << std::endl;
    for (int i = 0; i < input.points.size(); i++)
    {
        output.push_back(Point_2(input.points[i].x, input.points[i].y));
        //std::cout << i << " x=" <<input.points[i].x << ", y="  << input.points[i].y << std::endl;
    }

    
    // Make sure we have a COUNTERCLOCKWISE polygon
    if (output.orientation() == CGAL::CLOCKWISE)
    {
        //std::cout << "Reverse Direction" << std::endl;
        output.clear();
        for (int i = input.points.size() - 1; i >= 0; i--)
        {
            output.push_back(Point_2(input.points[i].x, input.points[i].y));
        }
    }
    //std::cout << "Done Converting to CGAL polygon... " << output.size() << " points" << std::endl;
}

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;
//.........这里部分代码省略.........