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;
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;
};