void minkowskiSum( const LineString& gA, const Polygon_2& gB, Polygon_set_2& polygonSet )
{
if ( gA.isEmpty() ) {
return ;
}
int npt = gA.numPoints() ;
for ( int i = 0; i < npt - 1 ; i++ ) {
Polygon_2 P;
P.push_back( gA.pointN( i ).toPoint_2() );
P.push_back( gA.pointN( i+1 ).toPoint_2() );
//
// We want to compute the "minkowski sum" on each segment of the line string
// This is not very well defined. But it appears CGAL supports it.
// However we must use the explicit "full convolution" method for that particular case in CGAL >= 4.7
#if CGAL_VERSION_NR < 1040701000 // version 4.7
Polygon_with_holes_2 part = minkowski_sum_2( P, gB );
#else
Polygon_with_holes_2 part = minkowski_sum_by_full_convolution_2( P, gB );
#endif
// merge into a polygon set
if ( polygonSet.is_empty() ) {
polygonSet.insert( part );
}
else {
polygonSet.join( part );
}
}
}
int main ()
{
// Construct the first polygon (a triangle).
Polygon_2 P;
P.push_back (Point_2 (0, 0));
P.push_back (Point_2 (6, 0));
P.push_back (Point_2 (3, 5));
// Construct the second polygon (a triangle).
Polygon_2 Q;
Q.push_back (Point_2 (0, 0));
Q.push_back (Point_2 (2, -2));
Q.push_back (Point_2 (2, 2));
// Compute the Minkowski sum.
Polygon_with_holes_2 sum = minkowski_sum_2 (P, Q);
CGAL_assertion (sum.number_of_holes() == 0);
std::cout << "P = "; print_polygon (P);
std::cout << "Q = "; print_polygon (Q);
std::cout << "P (+) Q = "; print_polygon (sum.outer_boundary());
return (0);
}
void minkowskiSum( const Polygon& gA, const Polygon_2& gB, Polygon_set_2& polygonSet )
{
if ( gA.isEmpty() ) {
return ;
}
/*
* Invoke minkowski_sum_2 for exterior ring
*/
{
Polygon_with_holes_2 sum = minkowski_sum_2( gA.exteriorRing().toPolygon_2(), gB ) ;
if ( polygonSet.is_empty() ) {
polygonSet.insert( sum );
}
else {
polygonSet.join( sum );
}
}
/*
* Compute the Minkowski sum for each segment of the interior rings
* and perform the union of the result. The result is a polygon, and its holes
* correspond to the inset.
*
*/
if ( gA.hasInteriorRings() ) {
Polygon_set_2 sumInteriorRings ;
for ( size_t i = 0; i < gA.numInteriorRings(); i++ ) {
minkowskiSum( gA.interiorRingN( i ), gB, sumInteriorRings ) ;
}
/*
* compute the difference for each hole of the resulting polygons
*/
std::list<Polygon_with_holes_2> interiorPolygons ;
sumInteriorRings.polygons_with_holes( std::back_inserter( interiorPolygons ) ) ;
for ( std::list<Polygon_with_holes_2>::iterator it_p = interiorPolygons.begin();
it_p != interiorPolygons.end(); ++it_p ) {
for ( Polygon_with_holes_2::Hole_iterator it_hole = it_p->holes_begin();
it_hole != it_p->holes_end(); ++it_hole ) {
it_hole->reverse_orientation() ;
polygonSet.difference( *it_hole ) ;
} // foreach hole
}// foreach polygon
}
}
void
MainWindow::on_actionSelfMinkowskiSum_triggered()
{
if(poly.size()>0){
if(! poly.is_simple()){
return;
}
selfmink = minkowski_sum_2 (poly, poly);
minkgi = new CGAL::Qt::PolygonWithHolesGraphicsItem<Polygon_with_holes_2>(&selfmink);
minkgi->setVerticesPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
scene.addItem(minkgi);
minkgi->setEdgesPen(QPen(Qt::darkRed, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
}
}
void minkowskiSum( const LineString& gA, const Polygon_2 & gB, Polygon_set_2 & polygonSet ){
if ( gA.isEmpty() ){
return ;
}
int npt = gA.numPoints() ;
for ( int i = 0; i < npt - 1 ; i++ ){
Polygon_2 P;
P.push_back( gA.pointN(i).toPoint_2() );
P.push_back( gA.pointN(i+1).toPoint_2() );
Polygon_with_holes_2 part = minkowski_sum_2( P, gB );
// merge into a polygon set
if ( polygonSet.is_empty() ){
polygonSet.insert( part );
}else{
polygonSet.join( part );
}
}
}
int main ()
{
// Open the input file.
std::ifstream in_file ("rooms_star.dat");
if (! in_file.is_open())
{
std::cerr << "Failed to open the input file." << std::endl;
return (1);
}
// Read the two polygons from the file and compute their Minkowski sum.
Polygon_2 P, Q;
in_file >> P >> Q;
in_file.close();
// Compute and print the Minkowski sum.
Polygon_with_holes_2 sum = minkowski_sum_2 (P, Q);
std::cout << "P (+) Q = "; print_polygon_with_holes (sum);
return (0);
}
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);
}
void SubSelectIpelet::protected_run(int fn)
{
if (fn==2) {
show_help();
return;
}
std::list<Circle_2> cir_list;
std::list<Polygon_2> pol_list;
Iso_rectangle_2 bbox=
read_active_objects(
CGAL::dispatch_or_drop_output<Polygon_2,Circle_2>(
std::back_inserter(pol_list),
std::back_inserter(cir_list)
)
);
if (fn==0 && pol_list.size()!=2){
print_error_message("You must select exactly two polygons");
return;
}
std::list<double> r_offsets;
for (std::list<Circle_2>::iterator it=cir_list.begin();it!=cir_list.end();++it)
r_offsets.push_back(sqrt(CGAL::to_double(it->squared_radius())));
IpeMatrix tfm (1,0,0,1,-CGAL::to_double(bbox.min().x()),-CGAL::to_double(bbox.min().y()));
for (std::list<Polygon_2>::iterator it=pol_list.begin();it!=pol_list.end();++it)
if(!it->is_simple()){
print_error_message("Polygon(s) must be simple");
}
if (fn==0){
Polygon_2 polygon1=*pol_list.begin();
Polygon_2 polygon2=*++pol_list.begin();
Polygon_with_holes_2 sum = minkowski_sum_2 (polygon1, polygon2);
std::list<Point_2> LP;
for (Polygon_2::iterator it=sum.outer_boundary().vertices_begin();it!= sum.outer_boundary().vertices_end();++it)
LP.push_back(*it);
draw_polyline_in_ipe(LP.begin(),LP.end(),true,false,false);
for (Polygon_with_holes_2::Hole_const_iterator poly_it = sum.holes_begin(); poly_it != sum.holes_end();
++poly_it){
LP.clear();
for (Polygon_2::iterator it=poly_it->vertices_begin();it!= poly_it->vertices_end();++it)
LP.push_back(*it);
draw_polyline_in_ipe(LP.begin(),LP.end(),true,false,false);
}
create_polygon_with_holes(true);
transform_selected_objects_(tfm);
}
else{
if (r_offsets.size()==0)
r_offsets.push_back(10);
for (std::list<Polygon_2>::iterator it_pol=pol_list.begin();it_pol!=pol_list.end();++it_pol){
for(std::list<double>::iterator it=r_offsets.begin();it!=r_offsets.end();++it){
Offset_polygon_with_holes_2 offset=approximated_offset_2 (*it_pol, *it, 0.0001);
std::list<Segment_2> LS;
for( Offset_polygon_2::Curve_iterator itt=offset.outer_boundary().curves_begin();
itt!=offset.outer_boundary().curves_end();++itt){
Point_2 S=Point_2(CGAL::to_double(itt->source().x()),CGAL::to_double(itt->source().y()));
Point_2 T=Point_2(CGAL::to_double(itt->target().x()),CGAL::to_double(itt->target().y()));
if (itt->is_linear ())
LS.push_back(Segment_2(S,T));
if (itt->is_circular())
draw_in_ipe(Circular_arc_2(itt->supporting_circle(),S,T,itt->supporting_circle().orientation()));
}
draw_in_ipe(LS.begin(),LS.end());
}
}
}
}
