void gnuplot_print_polygon(std::ostream& out, const Polygon_2& P)
{
int prec = out.precision();
out << std::setprecision(pp_precision);
Polygon_2::Vertex_const_iterator vit;
for (vit = P.vertices_begin(); vit != P.vertices_end(); ++vit)
vit->insert(out) << std::endl;//" " << vit->z() << std::endl;
P.vertices_begin()->insert(out) << std::endl;//" " << P.vertices_begin()->z() << std::endl;
out.precision(prec);
}
void print_ser(std::ostream& out, const Polygon_2& polygon, const string& component)
{
out << "<Contour name=\"" << component << "\" hidden=\"true\" closed=\"true\" simplified=\"true\" border=\"0 1 0\" fill=\"0 1 0\" mode=\"9\"" << endl;
out << "points=\"";
// insert points here
Polygon_2::Vertex_const_iterator vit;
for (vit = polygon.vertices_begin(); vit != polygon.vertices_end(); ++vit) {
out << "\t" << vit->x() << " " << vit->y() << "," << std::endl;
}
out << "\t\"/>" << endl;
}
void print_ser(std::ostream& out, const Slice& slice, Number_type thickness)
{
list<string> components;
slice.components(back_inserter(components));
out << "<?xml version=\"1.0\"?>" << endl;
out << "<!DOCTYPE Section SYSTEM \"section.dtd\">" << endl;
out << endl;
out << "<Section index=\"87\" thickness=\"" << thickness << "\" alignLocked=\"true\">" << endl;
for (list<string>::const_iterator it = components.begin(); it != components.end(); ++it) {
string component(*it);
out << "<Transform dim=\"0\"" << endl;
out << "xcoef=\" 0 1 0 0 0 0\"" << endl;
out << "ycoef=\" 0 0 1 0 0 0\">" << endl;
typedef Slice::Contour_const_iterator C_iter;
for (C_iter c_it = slice.begin(component); c_it != slice.end(component); ++c_it) {
Contour_handle contour = *c_it;
const Polygon_2& polygon = contour->polygon();
out << "<Contour name=\"" << component << "\" hidden=\"true\" closed=\"true\" simplified=\"true\" border=\"0 1 0\" fill=\"0 1 0\" mode=\"9\"" << endl;
out << "points=\"";
// insert points here
Polygon_2::Vertex_const_iterator vit;
for (vit = polygon.vertices_begin(); vit != polygon.vertices_end(); ++vit) {
out << "\t" << vit->x() << " " << vit->y() << "," << std::endl;
}
out << "\t\"/>" << endl;
}
out << "</Transform>" << endl;
out << endl;
}
out << "</Section>" << endl;
out << endl;
out << endl;
}
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;
}
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);
}
