std::list<Polygon_with_holes_2> CstmCGAL::splitPoly(const Polygon_with_holes_2& poly) {
std::vector<Point_2> outerBoundary = std::vector<Point_2>(
poly.outer_boundary().vertices_begin(),poly.outer_boundary().vertices_end());
std::list<Polygon_with_holes_2> result;
for (unsigned int i = 0 ; i < outerBoundary.size() ; i++) {
for (unsigned int j = i+1 ; j < outerBoundary.size() ; j++) {
if (outerBoundary[i] == outerBoundary[j]) {
result.splice(result.end(), splitPoly(Polygon_with_holes_2(
Polygon_2(outerBoundary.begin() + i, outerBoundary.begin() + j)
)));
for (unsigned int k = i+1 ; k < outerBoundary.size() ; k++) {
outerBoundary[k] = outerBoundary[k + j - i];
}
outerBoundary.resize(outerBoundary.size() - j + i);
break;
}
}
}
result.push_back(Polygon_with_holes_2(
Polygon_2(outerBoundary.begin(), outerBoundary.end())));
return result;
}
void
arrangement::newProblem()
{
// reset polygon data
env = Polygon_2();
obs.clear();
inset.clear();
offsets.clear();
admissible.clear();
inset_o.clear();
offsets_o.clear();
admissible_o.clear();
env_points.clear();
// reset arrangement data
convolutions.clear();
convolutions_o.clear();
//ccI.clear();
//ccII.clear();
// reset for graphs
nonCriticalRegions.clear();
//convolution_r_all.clear();
point_in_faces.clear();
neighbours.clear();
ACScells.clear();
GRASPcells.clear();
GRASPManipCells.clear();
source.clear();
target.clear();
}
void apply_path()
{
update_polyline ();
domain_rectangle = CGAL::bbox_2 (contour_2d.begin (), contour_2d.end ());
if (!rectangle)
domain_freeform = Polygon_2 (contour_2d.begin (), contour_2d.end ());
}
Polygon_2 CollisionDetector::flip(const Polygon_2& robot)
{
m_translate_helper.resize(0);
for(int i = 0; i < robot.size(); ++i)
{
Vector_2 minus_p = CGAL::ORIGIN - robot.vertex(i);
m_translate_helper.push_back(Point_2(minus_p.x(),minus_p.y()));
}
return Polygon_2(m_translate_helper.begin(), m_translate_helper.end());
}
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;
}
void perturb(Polygon_2& p, Number_type epsilon) {
Polygon_2::Vertex_iterator vit;
for (vit = p.vertices_begin(); vit != p.vertices_end(); ++vit) {
Point_2 pnt = *vit;
// Make sure we get a consistent perturbation with each point across runs
const static Number_type prime = 827;
srand((int)(pnt.x() + prime*(pnt.y() + prime*pnt.z())));
pnt = Point_25_<Kernel>(perturb(pnt.x(), epsilon), perturb(pnt.y(), epsilon), pnt.z(), pnt.id());
p.set(vit, pnt);
}
p = Polygon_2(p.vertices_begin(), unique(p.vertices_begin(), p.vertices_end()));
}
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;
}
