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());
}
//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;
}
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;
}
static void CollectEdges(Block_2& io_dst, edge_collector_t<Block_2> * collector, const Polygon_2& src, Block_locator * loc)
{
DebugAssert(src.size() >= 3);
DebugAssert(src.is_simple());
for(int n = 0; n < src.size(); ++n)
{
collector->input = Pmwx::X_monotone_curve_2(src.edge(n),0);
collector->ctr = 0;
DebugAssert(collector->input.source() != collector->input.target());
if(loc) CGAL::insert(io_dst, collector->input,*loc);
else CGAL::insert(io_dst, collector->input);
DebugAssert(collector->ctr > 0);
}
}
/*
* append gA+gB into the polygonSet
*/
void minkowskiSum( const Point& gA, const Polygon_2& gB, Polygon_set_2& polygonSet )
{
BOOST_ASSERT( gB.size() );
CGAL::Aff_transformation_2< Kernel > translate(
CGAL::TRANSLATION,
gA.toVector_2()
);
Polygon_2 sum ;
for ( Polygon_2::Vertex_const_iterator it = gB.vertices_begin();
it != gB.vertices_end(); ++it ) {
sum.push_back( translate.transform( *it ) );
}
if ( sum.is_clockwise_oriented() ) {
sum.reverse_orientation() ;
}
if ( polygonSet.is_empty() ) {
polygonSet.insert( sum );
}
else {
polygonSet.join( sum );
}
}
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());
}
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 printCgalPoly(Polygon_2 poly)
{
int i = 0;
std::cout <<"Printing polygon: size=" << poly.size();
for (Vertex_iterator vi = poly.vertices_begin(); vi != poly.vertices_end(); ++vi)
{
Point_2 v = *vi;
geometry_msgs::Point32 pt;
pt.x = v.x();
pt.y = v.y();
std::cout << i++ << " x=" <<pt.x << ", y=" << pt.y << std::endl;
}
}
void build_skeleton(const char* fname)
{
typedef typename Kernel::Point_2 Point_2;
typedef CGAL::Polygon_2<Kernel> Polygon_2;
typedef CGAL::Straight_skeleton_builder_traits_2<Kernel> SsBuilderTraits;
typedef CGAL::Straight_skeleton_2<Kernel> Ss;
typedef CGAL::Straight_skeleton_builder_2<SsBuilderTraits,Ss> SsBuilder;
Polygon_2 pgn;
std::ifstream input(fname);
FT x,y;
while(input)
{
input >> x;
if (!input) break;
input >> y;
if (!input) break;
pgn.push_back( Point_2( typename Kernel::FT(x), typename Kernel::FT(y) ) );
}
input.close();
std::cout << "Polygon has " << pgn.size() << " points\n";
if(!pgn.is_counterclockwise_oriented()) {
std::cerr << "Polygon is not CCW Oriented" << std::endl;
}
if(!pgn.is_simple()) {
std::cerr << "Polygon is not simple" << std::endl;
}
CGAL::Timer time;
time.start();
SsBuilder ssb;
ssb.enter_contour(pgn.vertices_begin(), pgn.vertices_end());
boost::shared_ptr<Ss> straight_ske = ssb.construct_skeleton();
time.stop();
std::cout << "Time spent to build skeleton " << time.time() << "\n";
if(!straight_ske->is_valid()) {
std::cerr << "Straight skeleton is not valid" << std::endl;
}
std::cerr.precision(60);
print_straight_skeleton(*straight_ske);
}
void _buildingWall( const Polygon_2& ring, const Kernel::FT& wallHeight, PolyhedralSurface& shell )
{
size_t npt = ring.size() ;
for ( size_t i = 0; i < npt; i++ ) {
const Point_2& a = ring.vertex( i ) ;
const Point_2& b = ring.vertex( ( i+1 ) % npt ) ;
LineString wallRing ;
wallRing.addPoint( new Point( a.x(), a.y(), Kernel::FT( 0 ) ) );
wallRing.addPoint( new Point( b.x(), b.y(), Kernel::FT( 0 ) ) );
wallRing.addPoint( new Point( b.x(), b.y(), wallHeight ) );
wallRing.addPoint( new Point( a.x(), a.y(), wallHeight ) );
wallRing.addPoint( new Point( a.x(), a.y(), Kernel::FT( 0 ) ) );
shell.addPolygon( Polygon( wallRing ) );
}
}
int main ()
{
// Open the input file.
std::ifstream in_file ("tight.dat");
if (! in_file.is_open())
{
std::cerr << "Failed to open the input file." << std::endl;
return (1);
}
// Read the input polygon.
Polygon_2 P;
in_file >> P;
in_file.close();
std::cout << "Read an input polygon with "
<< P.size() << " vertices." << std::endl;
// Approximate the offset polygon.
const Number_type radius = 1;
const double err_bound = 0.00001;
std::list<Offset_polygon_2> inset_polygons;
std::list<Offset_polygon_2>::iterator iit;
CGAL::Timer timer;
timer.start();
approximated_inset_2 (P, radius, err_bound,
std::back_inserter (inset_polygons));
timer.stop();
std::cout << "The inset comprises "
<< inset_polygons.size() << " polygon(s)." << std::endl;
for (iit = inset_polygons.begin(); iit != inset_polygons.end(); ++iit)
{
std::cout << " Polygon with "
<< iit->size() << " vertices." << std::endl;
}
std::cout << "Inset computation took "
<< timer.time() << " seconds." << std::endl;
return (0);
}
static Point_2 centroid(const Polygon_2& poly)
{
assert(poly.size() >= 3);
Polygon_2::Vertex_circulator vcir = poly.vertices_circulator();
Polygon_2::Vertex_circulator vend = vcir;
Polygon_2::Vertex_circulator vnext = vcir; ++vnext;
Vector_2 centre(0, 0);
NT a(0), asum(0);
do {
a = (vcir->x() * vnext->y()) - (vnext->x() * vcir->y());
centre = centre + a * ((*vcir - CGAL::ORIGIN) + (*vnext - CGAL::ORIGIN)); // slow...
asum += a;
vcir = vnext;
++vnext;
} while(vcir != vend);
centre = centre / (asum * 3);
return CGAL::ORIGIN + centre;
}
int main ()
{
// Open the input file.
std::ifstream in_file ("spiked.dat");
if (! in_file.is_open())
{
std::cerr << "Failed to open the input file." << std::endl;
return (1);
}
// Read the input polygon.
Polygon_2 P;
in_file >> P;
in_file.close();
std::cout << "Read an input polygon with "
<< P.size() << " vertices." << std::endl;
// Compute the offset polygon.
Conic_traits_2 traits;
const Rational radius = 5;
Offset_polygon_with_holes_2 offset;
CGAL::Timer timer;
timer.start();
offset = offset_polygon_2 (P, radius, traits);
timer.stop();
std::cout << "The offset polygon has "
<< offset.outer_boundary().size() << " vertices, "
<< offset.number_of_holes() << " holes." << std::endl;
std::cout << "Offset computation took "
<< timer.time() << " seconds." << std::endl;
return (0);
}
int alpha_shape(vertex_t *vertices, size_t count, double alpha,
vertex_t **res, size_t *res_count, char **err_msg)
{
try {
std::list<Point> points;
{
std::vector<Point> pv;
for (std::size_t j = 0; j < count; ++j) {
Point p(vertices[j].x, vertices[j].y);
pv.push_back(p);
}
std::sort(pv.begin(), pv.end(),
[](const Point &e1, const Point &e2)->bool {
return e2.y() < e1.y();
});
std::stable_sort(pv.begin(), pv.end(),
[](const Point &e1, const Point &e2)->bool {
return e2.x() < e1.x();
});
pv.erase(std::unique(pv.begin(), pv.end()), pv.end());
if (pv.size() != count && pv.size() < 3) {
*err_msg = strdup("After eliminating duplicated points, less than 3 points remain!!. Alpha shape calculation needs at least 3 vertices.");
return -1;
}
points.insert(points.begin(), pv.begin(), pv.end());
}
Alpha_shape_2 A(points.begin(), points.end(),
coord_type(10000),
Alpha_shape_2::REGULARIZED);
std::vector<Segment> segments;
// std::vector<Segment> result;
// Alpha_shape_2::Alpha_shape_vertices_iterator vit;
// Alpha_shape_2::Vertex_handle vertex;
// Alpha_shape_2::Alpha_shape_edges_iterator eit;
// Alpha_shape_2::Edge edge;
// Alpha_shape_2::Face_iterator fit;
// Alpha_shape_2::Face_handle face;
if (alpha <= 0.0)
{
alpha = *A.find_optimal_alpha(1);
}
A.set_alpha(alpha);
alpha_edges( A, std::back_inserter(segments));
// Segment s = segments.at(0);
// find_next_edge(s, segments, result);
if (segments.empty())
{
*res = NULL;
*res_count = 0;
}
else
{
std::set<int> unusedIndexes;
for (unsigned int i = 0; i < segments.size(); i++)
{
unusedIndexes.insert(i);
}
std::vector<Polygon_2> rings;
Polygon_2 ring;
ring.push_back(segments.at(0).source());
rings.push_back(ring);
unusedIndexes.erase(0);
find_next_edge(segments.at(0), segments, unusedIndexes, rings);
size_t result_count = 0;
for (unsigned int i = 0; i < rings.size(); i++)
{
Polygon_2 ring = rings.at(i);
result_count += ring.size();
}
result_count += rings.size() - 1;
*res = (vertex_t *) malloc(sizeof(vertex_t) * result_count);
*res_count = result_count;
int idx = 0;
for (unsigned int i = 0; i < rings.size(); i++)
{
if (i > 0)
{
(*res)[idx].x = DBL_MAX;
(*res)[idx].y = DBL_MAX;
idx++;
}
Polygon_2 ring = rings.at(i);
for(unsigned int j = 0; j < ring.size(); j++)
{
Point point = ring.vertex(j);
(*res)[idx].x = point.x();
(*res)[idx].y = point.y();
idx++;
}
}
}
*err_msg = NULL;
return EXIT_SUCCESS;
} catch ( ... ) {
*err_msg = strdup("Caught unknown expection!");
}
return -1;
}
void PathPlanner::printPoly(Polygon_2 poly){
for ( int i =0 ; i < poly.size(); i++){
cout<<std::fixed;
cout<<"Point: "<<i<<" "<<poly[i].x()<<" , "<<poly[i].y()<<'\n';
}
}
vector<Polygon_2> PathPlanner::linearShrink(){
// Points dist2center; //really lazy hack to store xcomp and ycomp in the point class
// for(int i =0 ; i < BPs.size(); i++){
// long double distance = getDistance(this->centroid,BPs[i]);
// cout<<"Dist: "<<distance<<'\n';
// cout<<"distBetweenPoints "<< this->distBetweenPoints<<'\n';
// //BUG - dividing by any decimal severly skews the results
// long double increment = distance - distBetweenPoints;
// cout<<"increment: "<<increment<<'\n';
// long double angle = getAnglebetweenPoints(BPs[i], this->centroid);
// cout<<"angle: "<<angle<<'\n';
// long double xcomp = cos(angle*M_PI/180.0)*increment;
// cout<<"xcomp: "<<xcomp<<'\n';
// long double ycomp = sin(angle*M_PI/180.0)*increment;
// cout<<"ycomp: "<<ycomp<<'\n';
// dist2center.push_back(Point_2(xcomp,ycomp));
// }
// printPoints(dist2center);
vector<Polygon_2> allTraces;
Polygon_2 polyIterator = convertPoints2Poly(this->BPs);
bool centroidFlag = true;
while(centroidFlag){
Polygon_2 holder;
for (int i = 0; i < polyIterator.size(); ++i)
{
// long double x,y;
// if(polyIterator[i].x() <centroid.x() ){
// x = polyIterator[i].x()+dist2center[i].x();
// y = polyIterator[i].y()+dist2center[i].y();
// }
// else{
// x = polyIterator[i].x()-dist2center[i].x();
// y = polyIterator[i].y()-dist2center[i].y();
// }
cout<<"centroid: "<<this->centroid<<"\tpoint: "<<polyIterator[i]<<endl;
long double distance = getDistance(this->centroid,polyIterator[i]);
cout<<"Dist: "<<distance<<'\n';
cout<<"distBetweenPoints "<< this->distBetweenPoints<<'\n';
long double increment = distance - this->distBetweenPoints;
cout<<"increment: "<<increment<<'\n';
long double angle = getAnglebetweenPoints(polyIterator[i], this->centroid);
cout<<"angle: "<<angle<<'\n';
long double xcomp = cos(angle*M_PI/180.0)*increment;
cout<<"xcomp: "<<xcomp<<'\n';
long double ycomp = sin(angle*M_PI/180.0)*increment;
cout<<"ycomp: "<<ycomp<<'\n';
long double x, y;
x =this->centroid.x();
y =this->centroid.y();
//top right quadrant from centroid
if((polyIterator[i].x()>centroid.x()) && (polyIterator[i].y()>centroid.y()) ){
Point_2 newPoint(x+xcomp,y+ycomp);
if(getDistance(newPoint, getCentroid() ) > this->distBetweenPoints){
holder.push_back(newPoint);
}
else{
cout<<"Point too close to center"<<endl;
}
}
//top left quadrant from centroid
else if((polyIterator[i].x()<centroid.x()) &&(polyIterator[i].y()>centroid.y())){
Point_2 newPoint(x-xcomp,y+ycomp);
if(getDistance(newPoint, getCentroid() ) > this->distBetweenPoints){
holder.push_back(newPoint);
}
else{
cout<<"Point too close to center"<<endl;
}
}
//bottom right quadrant of centroid
else if((polyIterator[i].x()>centroid.x()) && polyIterator[i].y()<centroid.y()){
Point_2 newPoint(x+xcomp,y-ycomp);
if(getDistance(newPoint, getCentroid() ) > this->distBetweenPoints){
holder.push_back(newPoint);
}
else{
cout<<"Point too close to center"<<endl;
}
}
//Bottom left quadrant of centriod
else{
Point_2 newPoint(x-xcomp,y-ycomp);
if(getDistance(newPoint, getCentroid() ) > this->distBetweenPoints){
holder.push_back(newPoint);
}
else{
cout<<"Point too close to center"<<endl;
}
}
cout<<xcomp<<" , "<<ycomp<<'\n';
}
if(holder.size() == 0){
centroidFlag = false;
holder.push_back(getCentroid());
allTraces.push_back(holder);
}
else{
allTraces.push_back(holder);
polyIterator = holder;
}
}
return allTraces;
}
