Polygon_2 remove_collinear(const Polygon_2& p, Number_type epsilon)
{
static log4cplus::Logger logger = log4cplus::Logger::getInstance("polygon_utils.remove_collinear");
LOG4CPLUS_TRACE(logger, pp(p));
if (!p.is_simple()) {
stringstream ss;
ss << "Polygon is not simple in remove_collinear: " << pp(p);
throw logic_error(ss.str());
}
Polygon_2::Vertex_circulator start = p.vertices_circulator();
Polygon_2::Vertex_circulator c = start;
Polygon_2::Vertex_circulator n = c;
Polygon_2::Vertex_circulator prev = c;
++n;
--prev;
Polygon_2 newp;
do
{
Triangle_2 t(*prev, *c, *n);
Number_type a = abs(t.area());
if (a > epsilon)
// if (!CGAL::collinear(*prev, *c, *n))
newp.push_back(*c);
else
LOG4CPLUS_TRACE(logger, "Removing collinearity at " << pp(*c) << " area = " << a);
++prev;
++c;
++n;
} while (c != start);
return newp;
}
Polygon_2 xz_swap_neg(const Polygon_2& p)
{
Polygon_2 ret;
for (Polygon_2::Vertex_iterator it = p.vertices_begin(); it != p.vertices_end(); ++it)
ret.push_back(xz_swap_neg(*it));
return ret;
}
void split_nonsimple(const Polygon_2& P, Out_iter out)
{
static log4cplus::Logger logger = log4cplus::Logger::getInstance("split_nonsimple");
typedef CGAL::Direction_2<Kernel> Direction_2;
typedef CGAL::Vector_2<Kernel> Vector_2;
std::set<Segment_2> segments;
segments.insert(P.edges_begin(), P.edges_end());
// Compute the subsegments, such that there are no interior pairwise
// intersections between subsegments.
std::list<Segment_2> subsegments_list;
CGAL::compute_subcurves(segments.begin(), segments.end(), back_inserter(subsegments_list));
// Index the subsegments by their source and target points.
std::map<Point_2, set<Segment_2> > source2sub, target2sub;
std::set<Segment_2> subsegments;
BOOST_FOREACH (Segment_2 s, subsegments_list) {
// if (segments.find(s.opposite()) != segments.end()) {
// s = s.opposite();
// }
subsegments.insert(s);
source2sub[s.source()].insert(s);
target2sub[s.target()].insert(s);
}
bool do_intersect(const Polygon_2& P, const Polygon_2& Q)
{
// return CGAL::do_intersect(P, Q);
if (boundaries_intersect(P, Q, true))
return true;
return P.has_on_positive_side(Q[0]) || Q.has_on_positive_side(P[0]);
}
/**
* @brief approximate an Offset_polygon_2 (filter null segments)
*/
Polygon_2 approximate( const Offset_polygon_2 & polygon, const int & n = 0 ){
std::list<std::pair<double, double> > pair_list;
/*
* iterate X_monotone_curve_2 components
*/
for ( Offset_polygon_2::Curve_const_iterator it = polygon.curves_begin();
it != polygon.curves_end(); ++it ){
it->approximate(std::back_inserter(pair_list), n ) ;
}
// remove duplicated last point
pair_list.pop_back() ;
/*
* convertr to polygon
*/
Polygon_2 result ;
bool isFirst = true ;
Kernel::Point_2 last ;
for ( std::list<std::pair<double, double> >::const_iterator it = pair_list.begin(); it != pair_list.end(); ++it ){
Kernel::Point_2 point( it->first, it->second ) ;
if ( isFirst ){
isFirst = false ;
}else if ( point == last ){
continue ;
}
result.push_back( point ) ;
last = point ;
}
return result ;
}
//Sill function to make a polygon type.There are more intricate checks and functions that can be used with the ploygon
//class but for now it is simply an intermediate or unecessary step.
Polygon_2 PathPlanner::makePoly(Points points){
Polygon_2 poly;
for (int i =0 ; i < points.size(); i++){
poly.push_back(points[i]);
}
return poly;
}
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 );
}
}
}
void mapdraw()
{
VisiLibity::Point pt1;
//pt1=VisiLibity::Point(160.0f,180.0f);
pt1=guest1.pos;
pt1.snap_to_boundary_of(mapEnv);
pt1.snap_to_vertices_of(mapEnv);
visiPoly=VisiLibity::Visibility_Polygon(pt1,mapEnv,0.00001);
Polygon_2 CGALvp;
for (int j=0;j<visiPoly.n();j++){
CGALvp.push_back(Point_2(visiPoly[j].x(),visiPoly[j].y()));
};
// CGAL::difference(mapEnvOB,holeCGAL,std::back_inserter(mapEnvOB));
Polygon_2 visRange=ngon(20,150.0);
Pwh_list_2 intR;
CGAL::intersection(CGALvp,visRange,std::back_inserter(intR));
visiBounded=intR.front();
tess = gluNewTess();
gluTessCallback (tess, GLU_TESS_BEGIN, (GLvoid (CALLBACK *)())tcbBegin);
gluTessCallback (tess, GLU_TESS_VERTEX, (GLvoid (CALLBACK *)())tcbVertex);
gluTessCallback (tess, GLU_TESS_END, (GLvoid (CALLBACK *)())tcbEnd);
};
bool boundaries_intersect(const Polygon_2& P, const Polygon_2& Q, bool proper)
{
std::list<Segment_2> segments0, segments1;
segments0.insert(segments0.end(), P.edges_begin(), P.edges_end());
segments1.insert(segments1.end(), Q.edges_begin(), Q.edges_end());
bool intersects = has_intersection(segments0.begin(), segments0.end(),
segments1.begin(), segments1.end(),
true, true);
if (!intersects || !proper)
return intersects;
if (has_intersection(segments0.begin(), segments0.end(),
segments1.begin(), segments1.end(),
false, false))
return true;
typedef Polygon_2::Vertex_const_iterator Iter;
for (Iter it = P.vertices_begin(); it != P.vertices_end(); ++it) {
if (Q.has_on_bounded_side(*it))
return true;
}
for (Iter it = Q.vertices_begin(); it != Q.vertices_end(); ++it) {
if (P.has_on_bounded_side(*it))
return true;
}
return false;
}
int main(int argc, char** argv)
{
//Reading polygon files
Polygon_2 poly;
QPolygonF poly_qt;
read_file("models/turtle_w.txt", poly, poly_qt);
//Compute straight skeleton
//*****This program now works for polygon WITHOUT HOLES.
SsPtr iss = CGAL::create_interior_straight_skeleton_2(poly.vertices_begin(), poly.vertices_end());
SsPtr ess = CGAL::create_exterior_straight_skeleton_2(EMaxOffset, poly);
//Constructing the graph combining the straight skeleton and polygon
std::vector<BridgingGraph> bg;
construct_bridging_graph(poly, *iss, *ess, bg);
//Compute perpendiculars
std::list<Perpendiculars> ppd;
generate_perpendiculars(*iss, *ess, bg, ppd);
deduplicate_perpendiculars<K>(ppd);
//Assign mountain and valley
std::list<Segment> mt;
std::list<Segment> vl;
MountainValley<K>(*iss, *ess, bg, ppd, mt, vl);
//Extracts skeleton from cgal and converts them to qt
std::list<QLineF> bis_qt;
convert_straight_skeleton(*iss, bis_qt);
convert_straight_skeleton(*ess, bis_qt);
std::list<QLineF> ppd_qt;
convert_perpendiculars<K>(ppd, ppd_qt);
std::list<QLineF> mt_qt, vl_qt;
convert_mountain_valley<K>(mt, mt_qt);
convert_mountain_valley<K>(vl, vl_qt);
//SETUP QT
//Create applicaiton
QApplication app(argc, argv);
//Create scene
QGraphicsScene scene;
createQTscene(scene, poly_qt, bis_qt, ppd_qt, mt_qt, vl_qt);
//Create view
QGraphicsView* view = new QGraphicsView(&scene);
CGAL::Qt::GraphicsViewNavigation nav;
view->installEventFilter(&nav);
view->viewport()->installEventFilter(&nav);
view->setRenderHint(QPainter::Antialiasing);
//view->keyPressEvent();
//Show view
view->show();
return app.exec();
}
Polygon_2 PathPlanner::convertPoints2Poly(Points in){
Polygon_2 poly;
for (int i = 0; i < in.size(); ++i)
{
poly.push_back(in[i]);
}
return poly;
}
Point_2 centroid(const Polygon_2& P)
{
Point_2 sum(0,0);
BOOST_FOREACH (const Point_2& p, make_pair(P.vertices_begin(), P.vertices_end())) {
sum = Point_2(sum.x() + p.x(), sum.y() + p.y());
}
return Point_2(sum.x()/P.size(), sum.y()/P.size());
}
vector<Polygon_2> CPEllipse::all_render_vertices() const
{
Polygon_2 points;
for (int i = 0; i < 64; ++i) {
points.push_back(_e(i/64.0));
}
vector<Polygon_2> ret;
ret.push_back(points);
return ret;
}
void insert_polygon( CDT& cdt, const Polygon_2& polygon ) {
if (polygon.is_empty())
return;
CDT::Vertex_handle v_prev = cdt.insert( *CGAL::cpp0x::prev( polygon.vertices_end() ) );
for (Polygon_2::Vertex_iterator vit = polygon.vertices_begin(); vit != polygon.vertices_end(); ++vit) {
CDT::Vertex_handle vh = cdt.insert(*vit);
cdt.insert_constraint(vh, v_prev);
v_prev = vh;
}
}
void gnuplot_print_polygon(std::ostream& out, const Polygon_2& P, int z)
{
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)
out << *vit << " " << z << std::endl;
out << *(P.vertices_begin()) << " " << z << std::endl;
out.precision(prec);
}
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);
}
bool intersects_boundary(const Polygon_2& P, const Segment_2& s, bool end_internal, bool end_end)
{
std::list<Segment_2> segments0, segments1;
segments0.insert(segments0.end(), P.edges_begin(), P.edges_end());
segments1.push_back(s);
return has_intersection(segments0.begin(), segments0.end(),
segments1.begin(), segments1.end(),
end_internal, end_end);
}
//Ohhh gosh. uhmm. gee what does this do?
Point_2 PathPlanner::getCentroid(Polygon_2 poly){
long double totalX =0;
long double totalY =0;
for (int i =0 ; i < poly.size(); i ++){
totalX += poly[i].x();
totalY += poly[i].y();
}
Point_2 centroid( (totalX/poly.size()) , (totalY/poly.size()) );
return centroid;
}
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());
}
Polygon_2 ngon(int n, float r)
{
Polygon_2 p;
for (int i=0;i<n;i++)
{ float PI=std::atan(1.0f)*4.0f;
p.push_back(Point_2(r*cos(2*PI/n*i)+guest1.pos.x(),r*sin(2*PI/n*i)+guest1.pos.y()));
}
return p;
}
/**
\brief Generates a vector of polygons from a vector of segments
\details First separes the segments in different adjacent groups (that would be the polygons). Then generates a polygon from each of the lists
\param segments an std::vector<Segment> with CGAL segment_2
\param &polygons INOUT A std::vector<Polygon_2> with CGAL Polygon_2
*/
void segments_to_polygons(std::vector<Segment> segments, std::vector< Polygon_2 > &polygons) {
Segment cs;
bool found = false;
int pid = 0; //polygon id (possible alpha shape with different polygons)
int count = 0;
std::vector<Segment> osegments [255]; //initialize at 255. not more
std::vector<Segment> segments_tmp;
for(int i = 0; i < segments.size(); i++) {
segments_tmp.push_back(segments[i]);
}
//push a first segment
osegments[pid].push_back(segments_tmp.back());
segments_tmp.pop_back(); //remove the element
while(segments_tmp.size() > 0) {
cs = osegments[pid].back();
found = false;
for(std::vector<Segment>::iterator it = segments_tmp.begin();
!found && it != segments_tmp.end();
++it) {
if (cs.target() == it->source()) {
found = true;
osegments[pid].push_back(*it);
segments_tmp.erase(it);
}
}
if (!found) {
/*if not found. I assume that there is another polygon.
Increase polygon id (pid) and keep going.
*/
++pid;
osegments[pid].push_back(segments_tmp.back());
segments_tmp.pop_back();
}
}
for(int i=0; i<255; i++) {
if(osegments[i].size()==0) break;
Polygon_2 P;
std::vector<Segment>::iterator it = osegments[i].begin();
P.push_back(it->source()); //first point
for(std::vector<Segment>::iterator it = osegments[i].begin(); it != osegments[i].end(); ++it) {
P.push_back(it->target());
}
P.push_back(P[0]);
polygons.push_back(P);
}
}
int main()
{
Polygon_2 outer ;
outer.push_back( Point(-1,-1) ) ;
outer.push_back( Point(0,-12) ) ;
outer.push_back( Point(1,-1) ) ;
outer.push_back( Point(12,0) ) ;
outer.push_back( Point(1,1) ) ;
outer.push_back( Point(0,12) ) ;
outer.push_back( Point(-1,1) ) ;
outer.push_back( Point(-12,0) ) ;
Polygon_2 hole ;
hole.push_back( Point(-1,0) ) ;
hole.push_back( Point(0,1 ) ) ;
hole.push_back( Point(1,0 ) ) ;
hole.push_back( Point(0,-1) ) ;
Polygon_with_holes poly( outer ) ;
poly.add_hole( hole ) ;
SsPtr iss = CGAL::create_interior_straight_skeleton_2(poly);
print_straight_skeleton(*iss);
return 0;
}
list<Polygon_with_holes_2> CPEllipse::render_vertices() const
{
Polygon_2 points;
for (int i = 0; i < 64; ++i) {
points.push_back(_e(i/64.0));
}
Polygon_with_holes_2 pwh(points);
list<Polygon_with_holes_2> ret;
ret.push_back(pwh);
return ret;
}
VisiLibity::Polygon ConvertPolygonCGAL2Vis(Polygon_2 pgn)
{
VisiLibity::Polygon cPoly;
Polygon_2::Vertex_iterator cv ;
for (cv=pgn.vertices_begin();cv!=pgn.vertices_end();++cv)
{
cPoly.push_back(VisiLibity::Point(cv->x(),cv->y()));
};
return cPoly;
}
std::string pp(const Polygon_2& P)
{
Polygon_2::Vertex_const_iterator vit;
std::stringstream out;
out << std::setprecision(pp_precision);
out << "[ " << P.size() << " vertices:";
for (vit = P.vertices_begin(); vit != P.vertices_end(); ++vit)
out << pp(*vit);
out << " ]";
return out.str();
}
void main()
{
if ( DEBUG )
{
debug_func();
return;
}
Polygon_2 poly;
int numOfPoints;
double x,y;
std::fstream myfile;
myfile.open ("out.txt");
myfile >> numOfPoints;
std::vector<int> rArray(numOfPoints);
int r;
for (int i=0; i<numOfPoints; ++i)
{
myfile >> x >> y >> r;
poly.push_back( Point_2(x,y) );
rArray [i] = r;
}
myfile.close();
Shor shor;
shor.load_polygon (poly);
shor.load_rArray (rArray);
shor.play();
shor.build_triangulation();
shor.simplify_triangulation();
std::ofstream out("outC.txt");
std::streambuf *coutbuf = std::cout.rdbuf(); //save old buf
std::cout.rdbuf(out.rdbuf()); //redirect std::cout to out.txt!
for ( int i = 0 , len = (int)shor.triangulated.size(); i < len ; ++i )
{
std::cout << shor.triangulated[i][0][0] << " " << shor.triangulated[i][0][1] <<std::endl;
std::cout << shor.triangulated[i][1][0] << " " << shor.triangulated[i][1][1] <<std::endl;
std::cout << shor.triangulated[i][2][0] << " " << shor.triangulated[i][2][1] <<std::endl;
}
//system("pause");
}
bool is_vertical(const Polygon_2& p)
{
for (int i = 0; i < p.size() - 1; ++i)
for (int j = i+1; j < p.size(); ++j)
if (xy_equal(p[i], p[j]))
return true;
// for (int i = 0; i < p.size() - 2; ++i)
// if (!collinear(p[i], p[i+1], p[i+2]))
// return false;
// return true;
return false;
}
/**
* Checks if plg2 is inside plg1
*/
bool is_inside(Polygon_2 plg1, Polygon_2 plg2) {
bool ishole = true;
for (std::vector<Point>::iterator v2 = plg2.vertices_begin();
v2 != plg2.vertices_end();
++v2) {
if(! check_inside(*v2, plg1, K())) {
ishole = false;
break; //stop checking
}
}
return ishole;
}
//-----------------------------------------------------------------------------
bool Polygon::inside(dolfin::Point p) const
{
typedef CGAL::Cartesian<double> K;
typedef CGAL::Point_2<K> Point_2;
typedef CGAL::Polygon_2<K> Polygon_2;
Polygon_2 polygon;
for (const dolfin::Point& vertex : _vertices)
polygon.push_back(Point_2(vertex.x(), vertex.y()));
return polygon.has_on_bounded_side(Point_2(p.x(), p.y()));
}
Polygon_set_2 join(Array polygons){
vector<Polygon_2> polygon_vector;
for (unsigned int i = 0; i < polygons.size(); i++) {
Polygon_2 polygon = from_ruby<Polygon_2>(polygons[i]);
if (polygon.is_simple()) {
polygon_vector.push_back(polygon);
}
}
Polygon_set_2 S;
S.join(polygon_vector.begin(), polygon_vector.end());
return S;
}
