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);
}
}
// The main program:
int main(int argc, char* argv[])
{
Polygon_2 polygon;
const char* filename = (argc > 1) ? argv[1] : "polygon.dat";
std::ifstream input_file(filename);
if (! input_file.is_open()) {
std::cerr << "Failed to open the " << filename << std::endl;
return -1;
}
input_file >> polygon;
input_file.close();
// Example for is_pullout_direction_single_mold_translational_casting_2 that
// accepts the edge
size_t index(0);
for (auto e_it = polygon.edges_begin(); e_it != polygon.edges_end(); ++e_it,
++index)
{
auto orientation = polygon.orientation();
auto segment_outer_circle =
SMS::internal::get_segment_outer_circle<Kernel>(*e_it, orientation);
auto d = segment_outer_circle.first;
d = d.perpendicular(CGAL::CLOCKWISE);
auto res = casting::is_pullout_direction(polygon, e_it, d);
std::cout << "The polygon is " << (res ? "" : "not ")
<< "castable using edge "
<< index << " in vartical translation (" << d << ")" << std::endl;
}
std::cout << "-----------------------------------"<< std::endl;
// Example for is_pullout_direction_single_mold_translational_casting_2 that
// do not accepts the edge
{
Vector_2 v (Point_2(0,0), Point_2(1,0));
Direction_2 d(v);
auto res = casting::is_pullout_direction(polygon, d);
if (res != polygon.edges_end()) {
std::cout << "The polygon is castable in direction d (" << d
<< ") using edge "<< *res << std::endl;
}
else {
std::cout << "The polygon is not castable in direction d (" << d << ")"
<< std::endl;
}
}
return 0;
}
//---------------
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;
}
void OffsetWorker::offsetPolygon(Polygon_2 poly, double offset) {
typedef CGAL::Gps_circle_segment_traits_2<SFCGAL::Kernel> Gps_traits_2;
typedef Gps_traits_2::Polygon_2 Offset_polygon_2;
if(!poly.is_simple()) {
DM::Logger(DM::Warning) << "Can't perform offset polygon is not simple";
}
CGAL::Orientation orient = poly.orientation();
if (orient == CGAL::CLOCKWISE) {
poly.reverse_orientation();
}
const double err_bound = 0.00001;
std::list<Offset_polygon_2> inset_polygons;
CGAL::approximated_inset_2 (poly, offset, err_bound, std::back_inserter (inset_polygons));
foreach (Offset_polygon_2 p, inset_polygons) {
SFCGAL::Polygon po(this->approximate(p));
QString wkt = QString(po.asText(9).c_str());
module->addToSystem(wkt);
//emit resultPolygon(wkt);
}
