// Initialize polypartition TPPLPoly from a list of indices and vertices
//
// verts - 3D polygon vertex vectors
// xind,yind - Indices of 3D vectors to extract as x and y coordinates for 2D
// triangulation computation.
// inds,size - Array of indices into the `verts` list
// isHole - Value for the hole flag, and to determine the orientation
static void initTPPLPoly(TPPLPoly& poly,
const std::vector<float>& verts,
int xind, int yind,
const GLuint* inds, int size,
bool isHole)
{
// Check for explicitly closed polygons (last and first vertices equal) and
// discard the last vertex in these cases. This is a pretty stupid
// convention, but the OGC have blessed it and now we've got a bunch of
// geospatial formats (kml, WKT, GeoJSON) which require it. Sigh.
// http://gis.stackexchange.com/questions/10308/why-do-valid-polygons-repeat-the-same-start-and-end-point/10309#10309
if (inds[0] == inds[size-1] ||
(verts[3*inds[0]+0] == verts[3*inds[size-1]+0] &&
verts[3*inds[0]+1] == verts[3*inds[size-1]+1] &&
verts[3*inds[0]+2] == verts[3*inds[size-1]+2]))
{
g_logger.warning_limited("Ignoring duplicate final vertex in explicitly closed polygon");
size -= 1;
}
// Copy into polypartition data structure
poly.Init(size);
for (int i = 0; i < size; ++i)
{
poly[i].x = verts[3*inds[i]+xind];
poly[i].y = verts[3*inds[i]+yind];
poly[i].id = inds[i];
}
int orientation = poly.GetOrientation();
// Invert so that outer = ccw, holes = cw
if ((orientation == TPPL_CW) ^ isHole)
poly.Invert();
poly.SetHole(isHole);
}
bool TriangulatorAdaptor::triangulate(int *triangleIndexes) {
int XX, YY;
from3DTo2D(XX, YY);
map<pair<double, double> , int> coorToPosMap; // coordinate to position map
for (int i = 0; i < polygon.size(); i++) {
coorToPosMap.insert(
pair<pair<double, double> , int>(
pair<double, double>(polygon[i][XX], polygon[i][YY]), i));
}
TPPLPoly poly;
poly.Init(polygon.size());
for (int i = 0; i < polygon.size(); i++) {
poly[i].x = polygon[i][XX];
poly[i].y = polygon[i][YY];
}
if (poly.GetOrientation() == TPPL_CW) {
isClockwise = true;
poly.Invert(); // polygon has to be counter-clockwise
} else if(poly.GetOrientation() == TPPL_CCW) {
isClockwise = false;
} else {
return false;
}
list<TPPLPoly> triangles;
TPPLPartition triangulator; // see http://code.google.com/p/polypartition/
int success = triangulator.Triangulate_OPT(&poly, &triangles);
if (success != 1) {
cout << "Error: polygon cannot be triangulated!" << endl;
cout << "Polygon:" << endl;
for (int i=0; i<polygon.size(); i++) {
cout << polygon[i][0] << " " << polygon[i][1] << " " << polygon[i][2] << endl;
}
return false;
}
//assert(success == 1);
assert(triangles.size() +2 == polygon.size());
int count = 0;
for (list<TPPLPoly>::iterator it = triangles.begin(); it != triangles.end(); it++) {
if (!isClockwise) {
for (int j = 0; j < 3; j++) {
triangleIndexes[count * 3 + j] = coorToPosMap[pair<double, double>((*it)[j].x,
(*it)[j].y)];
}
} else {
for (int j = 0; j < 3; j++) {
triangleIndexes[count * 3 + 2 - j] = coorToPosMap[pair<double, double>((*it)[j].x,
(*it)[j].y)];
}
}
count++;
}
assert(count == triangles.size());
return true;
}
TPPLPoly TPPLPoly_To_Polygon(const ClipperLib::Polygon& B)
{
TPPLPoly poly;
poly.Init(B.size());
for(unsigned int i=0; i < B.size() ; ++i)
{
poly[i].x = B[i].X;
poly[i].y = B[i].Y;
}
return poly;
}
std::vector<sf::Vector2f> _getEdges(TPPLPoly& A)
{
std::vector<sf::Vector2f> edges;
for(unsigned int i=0; i < (A.GetNumPoints()+1) ; ++i)
{
sf::Vector2f P1( A[i].x, A[i].y);
sf::Vector2f P2( A[(i+1)%A.GetNumPoints()].x, A[(i+1)%A.GetNumPoints()].y );
edges.push_back( P2 - P1 );
}
return edges;
}
void
Cylinder::triangulate(list<TPPLPoly>& tri_list) const
{
TPPLPartition pp;
list<TPPLPoly> polys;
TPPLPoly poly;
TPPLPoint pt;
double d_alpha = 0.5;
double alpha_max = 0, alpha_min = std::numeric_limits<double>::max();
for(size_t i = 0; i < contours_[0].size(); ++i)
{
double alpha = contours_[0][i](0) / r_;
if (alpha > alpha_max) alpha_max = alpha;
if (alpha < alpha_min) alpha_min = alpha;
}
std::cout << "r " << r_ << std::endl;
std::cout << "alpha " << alpha_min << "," << alpha_max << std::endl;
std::vector<std::vector<std::vector<Eigen::Vector2f> > > contours_split;
for(size_t j = 0; j < contours_.size(); j++)
{
for(double i = alpha_min + d_alpha; i <= alpha_max; i += d_alpha)
{
std::vector<Eigen::Vector2f> contour_segment;
for(size_t k = 0; k < contours_[j].size(); ++k)
{
double alpha = contours_[j][k](0) / r_;
if( alpha >= i - d_alpha - 0.25 && alpha < i + 0.25)
{
contour_segment.push_back(contours_[j][k]);
}
}
//std::cout << "c " << j << i << " has " << contour_segment.size() << " points" << std::endl;
if(contour_segment.size() < 3) continue;
poly.Init(contour_segment.size());
poly.SetHole(holes_[j]);
for( unsigned int l = 0; l < contour_segment.size(); l++)
{
pt.x = contour_segment[l](0);
pt.y = contour_segment[l](1);
poly[l] = pt;
}
if (holes_[j])
poly.SetOrientation(TPPL_CW);
else
poly.SetOrientation(TPPL_CCW);
polys.push_back(poly);
}
}
// triangulation into monotone triangles
pp.Triangulate_EC (&polys, &tri_list);
}
void make_poly(float* buf, int pnt_sz, TPPLPoly& poly)
{
poly.Init(pnt_sz);
for (int k(0); k < pnt_sz; k++)
{
poly[k].x = buf[2 * k];
poly[k].y = buf[2 * k + 1];
}
if (poly.GetOrientation() == TPPL_CW)
{
poly.SetHole(true);
}
}
std::vector<sf::Vector2f> _getPoints(TPPLPoly& A)
{
std::vector<sf::Vector2f> points;
for(unsigned int i=0; i < A.GetNumPoints() ; ++i)
{
sf::Vector2f P( A[i].x, A[i].y);
points.push_back( P );
}
return points;
}
void partition() {
TPPLPoly poly;
std::vector<Point> nodes = polygons_[0].nodes_;
poly.Init(nodes.size());
unsigned int i = 0;
for (std::vector<Point>::const_iterator p = nodes.begin(); p != nodes.end(); ++p, ++i) {
poly[i].x = p->lat;
poly[i].y = p->lon;
}
std::list<TPPLPoly> convex_polys;
TPPLPartition partitioner;
partitioner.Triangulate_OPT(&poly, &convex_polys);
//partitioner.ConvexPartition_HM(&poly, &convex_polys);
polygons_.clear();
for (std::list<TPPLPoly>::iterator p = convex_polys.begin(); p != convex_polys.end(); ++p) {
add_polygon();
for (long i = 0; i < p->GetNumPoints(); ++i) {
TPPLPoint point = p->GetPoint(i);
add_node(Point(point.x, point.y));
}
}
}
//triangulates a set of polygons by first partitioning them into monotone polygons
//O(n*log(n)) time complexity, O(n) space complexity
//the algorithm used here is outlined in the book
//"Computational Geometry: Algorithms and Applications"
//by Mark de Berg, Otfried Cheong, Marc van Kreveld and Mark Overmars
int TPPLPartition::MonotonePartition(list<TPPLPoly> *inpolys, list<TPPLPoly> *monotonePolys) {
list<TPPLPoly>::iterator iter;
MonotoneVertex *vertices;
long i,numvertices,vindex,vindex2,newnumvertices,maxnumvertices;
long polystartindex, polyendindex;
TPPLPoly *poly;
MonotoneVertex *v,*v2,*vprev,*vnext;
ScanLineEdge newedge;
bool error = false;
numvertices = 0;
for(iter = inpolys->begin(); iter != inpolys->end(); iter++) {
numvertices += iter->GetNumPoints();
}
maxnumvertices = numvertices*3;
vertices = new MonotoneVertex[maxnumvertices];
newnumvertices = numvertices;
polystartindex = 0;
for(iter = inpolys->begin(); iter != inpolys->end(); iter++) {
poly = &(*iter);
polyendindex = polystartindex + poly->GetNumPoints()-1;
for(i=0;i<poly->GetNumPoints();i++) {
vertices[i+polystartindex].p = poly->GetPoint(i);
if(i==0) vertices[i+polystartindex].previous = polyendindex;
else vertices[i+polystartindex].previous = i+polystartindex-1;
if(i==(poly->GetNumPoints()-1)) vertices[i+polystartindex].next = polystartindex;
else vertices[i+polystartindex].next = i+polystartindex+1;
}
polystartindex = polyendindex+1;
}
//construct the priority queue
long *priority = new long [numvertices];
for(i=0;i<numvertices;i++) priority[i] = i;
std::sort(priority,&(priority[numvertices]),VertexSorter(vertices));
//determine vertex types
char *vertextypes = new char[maxnumvertices];
for(i=0;i<numvertices;i++) {
v = &(vertices[i]);
vprev = &(vertices[v->previous]);
vnext = &(vertices[v->next]);
if(Below(vprev->p,v->p)&&Below(vnext->p,v->p)) {
if(IsConvex(vnext->p,vprev->p,v->p)) {
vertextypes[i] = TPPL_VERTEXTYPE_START;
} else {
vertextypes[i] = TPPL_VERTEXTYPE_SPLIT;
}
} else if(Below(v->p,vprev->p)&&Below(v->p,vnext->p)) {
if(IsConvex(vnext->p,vprev->p,v->p))
{
vertextypes[i] = TPPL_VERTEXTYPE_END;
} else {
vertextypes[i] = TPPL_VERTEXTYPE_MERGE;
}
} else {
vertextypes[i] = TPPL_VERTEXTYPE_REGULAR;
}
}
//helpers
long *helpers = new long[maxnumvertices];
//binary search tree that holds edges intersecting the scanline
//note that while set doesn't actually have to be implemented as a tree
//complexity requirements for operations are the same as for the balanced binary search tree
set<ScanLineEdge> edgeTree;
//store iterators to the edge tree elements
//this makes deleting existing edges much faster
set<ScanLineEdge>::iterator *edgeTreeIterators,edgeIter;
edgeTreeIterators = new set<ScanLineEdge>::iterator[maxnumvertices];
pair<set<ScanLineEdge>::iterator,bool> edgeTreeRet;
for(i = 0; i<numvertices; i++) edgeTreeIterators[i] = edgeTree.end();
//for each vertex
for(i=0;i<numvertices;i++) {
vindex = priority[i];
v = &(vertices[vindex]);
vindex2 = vindex;
v2 = v;
//depending on the vertex type, do the appropriate action
//comments in the following sections are copied from "Computational Geometry: Algorithms and Applications"
switch(vertextypes[vindex]) {
case TPPL_VERTEXTYPE_START:
//Insert ei in T and set helper(ei) to vi.
newedge.p1 = v->p;
newedge.p2 = vertices[v->next].p;
newedge.index = vindex;
edgeTreeRet = edgeTree.insert(newedge);
edgeTreeIterators[vindex] = edgeTreeRet.first;
helpers[vindex] = vindex;
break;
case TPPL_VERTEXTYPE_END:
//if helper(ei-1) is a merge vertex
if(vertextypes[helpers[v->previous]]==TPPL_VERTEXTYPE_MERGE) {
//Insert the diagonal connecting vi to helper(ei-1) in D.
AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous],
vertextypes, edgeTreeIterators, &edgeTree, helpers);
}
//Delete ei-1 from T
edgeTree.erase(edgeTreeIterators[v->previous]);
break;
case TPPL_VERTEXTYPE_SPLIT:
//Search in T to find the edge e j directly left of vi.
newedge.p1 = v->p;
newedge.p2 = v->p;
edgeIter = edgeTree.lower_bound(newedge);
if(edgeIter == edgeTree.begin()) {
error = true;
break;
}
edgeIter--;
//Insert the diagonal connecting vi to helper(ej) in D.
AddDiagonal(vertices,&newnumvertices,vindex,helpers[edgeIter->index],
vertextypes, edgeTreeIterators, &edgeTree, helpers);
vindex2 = newnumvertices-2;
v2 = &(vertices[vindex2]);
//helper(e j)?vi
helpers[edgeIter->index] = vindex;
//Insert ei in T and set helper(ei) to vi.
newedge.p1 = v2->p;
newedge.p2 = vertices[v2->next].p;
newedge.index = vindex2;
edgeTreeRet = edgeTree.insert(newedge);
edgeTreeIterators[vindex2] = edgeTreeRet.first;
helpers[vindex2] = vindex2;
break;
case TPPL_VERTEXTYPE_MERGE:
//if helper(ei-1) is a merge vertex
if(vertextypes[helpers[v->previous]]==TPPL_VERTEXTYPE_MERGE) {
//Insert the diagonal connecting vi to helper(ei-1) in D.
AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous],
vertextypes, edgeTreeIterators, &edgeTree, helpers);
vindex2 = newnumvertices-2;
v2 = &(vertices[vindex2]);
}
//Delete ei-1 from T.
edgeTree.erase(edgeTreeIterators[v->previous]);
//Search in T to find the edge e j directly left of vi.
newedge.p1 = v->p;
newedge.p2 = v->p;
edgeIter = edgeTree.lower_bound(newedge);
if(edgeIter == edgeTree.begin()) {
error = true;
break;
}
edgeIter--;
//if helper(ej) is a merge vertex
if(vertextypes[helpers[edgeIter->index]]==TPPL_VERTEXTYPE_MERGE) {
//Insert the diagonal connecting vi to helper(e j) in D.
AddDiagonal(vertices,&newnumvertices,vindex2,helpers[edgeIter->index],
vertextypes, edgeTreeIterators, &edgeTree, helpers);
}
//helper(e j)?vi
helpers[edgeIter->index] = vindex2;
break;
case TPPL_VERTEXTYPE_REGULAR:
//if the interior of P lies to the right of vi
if(Below(v->p,vertices[v->previous].p)) {
//if helper(ei-1) is a merge vertex
if(vertextypes[helpers[v->previous]]==TPPL_VERTEXTYPE_MERGE) {
//Insert the diagonal connecting vi to helper(ei-1) in D.
AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous],
vertextypes, edgeTreeIterators, &edgeTree, helpers);
vindex2 = newnumvertices-2;
v2 = &(vertices[vindex2]);
}
//Delete ei-1 from T.
edgeTree.erase(edgeTreeIterators[v->previous]);
//Insert ei in T and set helper(ei) to vi.
newedge.p1 = v2->p;
newedge.p2 = vertices[v2->next].p;
newedge.index = vindex2;
edgeTreeRet = edgeTree.insert(newedge);
edgeTreeIterators[vindex2] = edgeTreeRet.first;
helpers[vindex2] = vindex;
} else {
//Search in T to find the edge ej directly left of vi.
newedge.p1 = v->p;
newedge.p2 = v->p;
edgeIter = edgeTree.lower_bound(newedge);
if(edgeIter == edgeTree.begin()) {
error = true;
break;
}
edgeIter--;
//if helper(ej) is a merge vertex
if(vertextypes[helpers[edgeIter->index]]==TPPL_VERTEXTYPE_MERGE) {
//Insert the diagonal connecting vi to helper(e j) in D.
AddDiagonal(vertices,&newnumvertices,vindex,helpers[edgeIter->index],
vertextypes, edgeTreeIterators, &edgeTree, helpers);
}
//helper(e j)?vi
helpers[edgeIter->index] = vindex;
}
break;
}
if(error) break;
}
char *used = new char[newnumvertices];
memset(used,0,newnumvertices*sizeof(char));
if(!error) {
//return result
long size;
TPPLPoly mpoly;
for(i=0;i<newnumvertices;i++) {
if(used[i]) continue;
v = &(vertices[i]);
vnext = &(vertices[v->next]);
size = 1;
while(vnext!=v) {
vnext = &(vertices[vnext->next]);
size++;
}
mpoly.Init(size);
v = &(vertices[i]);
mpoly[0] = v->p;
vnext = &(vertices[v->next]);
size = 1;
used[i] = 1;
used[v->next] = 1;
while(vnext!=v) {
mpoly[size] = vnext->p;
used[vnext->next] = 1;
vnext = &(vertices[vnext->next]);
size++;
}
monotonePolys->push_back(mpoly);
}
}
//cleanup
delete [] vertices;
delete [] priority;
delete [] vertextypes;
delete [] edgeTreeIterators;
delete [] helpers;
delete [] used;
if(error) {
return 0;
} else {
return 1;
}
}
GeneralPolygon::operator list<TPPLPoly>()
{
auto isPolygonOutside = [&](const Contour &referencePolygon, const Contour &poly)
{
for(unsigned int p=0; p < referencePolygon.size() ; ++p)
if( !_evenOddRuleAlgorithm( referencePolygon[p], poly) )
return true;
return false;
};
list<TPPLPoly> polys;
for(unsigned int c=0; c < _contours.size() ; ++c)
{
const Contour &contour = _contours[c];
TPPLPoly poly;
poly.Init(contour.size());
for(unsigned int v=0; v < contour.size() ; ++v)
{
TPPLPoint point;
point.x = contour[v].x;
point.y = contour[v].y;
poly[v] = point;
}
// BE CAREFULL!!! not really correct because the polygon could be concave then
// the center of mass could not be inside the polygon
sf::Vector2f centerOfMass(0.0f, 0.0f);
for(unsigned int i=0; i < contour.size() ; ++i)
centerOfMass += contour[i];
centerOfMass *= (1.0f/contour.size());
std::vector<Contour> outsideContours;
for(unsigned int i=0; i < _contours.size() ; ++i)
{
if( i == c )
{
outsideContours.push_back(_contours[i]);
continue;
}
if( isPolygonOutside(_contours[i], contour) )
outsideContours.push_back(_contours[i]);
}
// first test if is a hole or not
if( !_evenOddRuleAlgorithm( centerOfMass, outsideContours ) )
poly.SetHole(true);
else
poly.SetHole(false);
// if it is a hole then it must be in CW order to the algorithm to recognize
// else must be in CCW
if( poly.IsHole() )
poly.SetOrientation(TPPL_CW);// Hole orientation (needed in the algorithm)
else
poly.SetOrientation(TPPL_CCW);// Not a hole orientation (needed in the algorithm)
polys.push_back(poly);
}
return polys;
}
int TPPLPartition::ConvexPartition_OPT(TPPLPoly *poly, list<TPPLPoly> *parts) {
TPPLPoint p1,p2,p3,p4;
PartitionVertex *vertices;
DPState2 **dpstates;
long i,j,k,n,gap;
list<Diagonal> diagonals,diagonals2;
Diagonal diagonal,newdiagonal;
list<Diagonal> *pairs,*pairs2;
list<Diagonal>::iterator iter,iter2;
int ret;
TPPLPoly newpoly;
list<long> indices;
list<long>::iterator iiter;
bool ijreal,jkreal;
n = poly->GetNumPoints();
vertices = new PartitionVertex[n];
dpstates = new DPState2 *[n];
for(i=0;i<n;i++) {
dpstates[i] = new DPState2[n];
}
//init vertex information
for(i=0;i<n;i++) {
vertices[i].p = poly->GetPoint(i);
vertices[i].isActive = true;
if(i==0) vertices[i].previous = &(vertices[n-1]);
else vertices[i].previous = &(vertices[i-1]);
if(i==(poly->GetNumPoints()-1)) vertices[i].next = &(vertices[0]);
else vertices[i].next = &(vertices[i+1]);
}
for(i=1;i<n;i++) {
UpdateVertexReflexity(&(vertices[i]));
}
//init states and visibility
for(i=0;i<(n-1);i++) {
p1 = poly->GetPoint(i);
for(j=i+1;j<n;j++) {
dpstates[i][j].visible = true;
if(j==i+1) {
dpstates[i][j].weight = 0;
} else {
dpstates[i][j].weight = 2147483647;
}
if(j!=(i+1)) {
p2 = poly->GetPoint(j);
//visibility check
if(!InCone(&vertices[i],p2)) {
dpstates[i][j].visible = false;
continue;
}
if(!InCone(&vertices[j],p1)) {
dpstates[i][j].visible = false;
continue;
}
for(k=0;k<n;k++) {
p3 = poly->GetPoint(k);
if(k==(n-1)) p4 = poly->GetPoint(0);
else p4 = poly->GetPoint(k+1);
if(Intersects(p1,p2,p3,p4)) {
dpstates[i][j].visible = false;
break;
}
}
}
}
}
for(i=0;i<(n-2);i++) {
j = i+2;
if(dpstates[i][j].visible) {
dpstates[i][j].weight = 0;
newdiagonal.index1 = i+1;
newdiagonal.index2 = i+1;
dpstates[i][j].pairs.push_back(newdiagonal);
}
}
dpstates[0][n-1].visible = true;
vertices[0].isConvex = false; //by convention
for(gap=3; gap<n; gap++) {
for(i=0;i<n-gap;i++) {
if(vertices[i].isConvex) continue;
k = i+gap;
if(dpstates[i][k].visible) {
if(!vertices[k].isConvex) {
for(j=i+1;j<k;j++) TypeA(i,j,k,vertices,dpstates);
} else {
for(j=i+1;j<(k-1);j++) {
if(vertices[j].isConvex) continue;
TypeA(i,j,k,vertices,dpstates);
}
TypeA(i,k-1,k,vertices,dpstates);
}
}
}
for(k=gap;k<n;k++) {
if(vertices[k].isConvex) continue;
i = k-gap;
if((vertices[i].isConvex)&&(dpstates[i][k].visible)) {
TypeB(i,i+1,k,vertices,dpstates);
for(j=i+2;j<k;j++) {
if(vertices[j].isConvex) continue;
TypeB(i,j,k,vertices,dpstates);
}
}
}
}
//recover solution
ret = 1;
newdiagonal.index1 = 0;
newdiagonal.index2 = n-1;
diagonals.push_front(newdiagonal);
while(!diagonals.empty()) {
diagonal = *(diagonals.begin());
diagonals.pop_front();
if((diagonal.index2 - diagonal.index1) <=1) continue;
pairs = &(dpstates[diagonal.index1][diagonal.index2].pairs);
if(pairs->empty()) {
ret = 0;
break;
}
if(!vertices[diagonal.index1].isConvex) {
iter = pairs->end();
iter--;
j = iter->index2;
newdiagonal.index1 = j;
newdiagonal.index2 = diagonal.index2;
diagonals.push_front(newdiagonal);
if((j - diagonal.index1)>1) {
if(iter->index1 != iter->index2) {
pairs2 = &(dpstates[diagonal.index1][j].pairs);
while(1) {
if(pairs2->empty()) {
ret = 0;
break;
}
iter2 = pairs2->end();
iter2--;
if(iter->index1 != iter2->index1) pairs2->pop_back();
else break;
}
if(ret == 0) break;
}
newdiagonal.index1 = diagonal.index1;
newdiagonal.index2 = j;
diagonals.push_front(newdiagonal);
}
} else {
iter = pairs->begin();
j = iter->index1;
newdiagonal.index1 = diagonal.index1;
newdiagonal.index2 = j;
diagonals.push_front(newdiagonal);
if((diagonal.index2 - j) > 1) {
if(iter->index1 != iter->index2) {
pairs2 = &(dpstates[j][diagonal.index2].pairs);
while(1) {
if(pairs2->empty()) {
ret = 0;
break;
}
iter2 = pairs2->begin();
if(iter->index2 != iter2->index2) pairs2->pop_front();
else break;
}
if(ret == 0) break;
}
newdiagonal.index1 = j;
newdiagonal.index2 = diagonal.index2;
diagonals.push_front(newdiagonal);
}
}
}
if(ret == 0) {
for(i=0;i<n;i++) {
delete [] dpstates[i];
}
delete [] dpstates;
delete [] vertices;
return ret;
}
newdiagonal.index1 = 0;
newdiagonal.index2 = n-1;
diagonals.push_front(newdiagonal);
while(!diagonals.empty()) {
diagonal = *(diagonals.begin());
diagonals.pop_front();
if((diagonal.index2 - diagonal.index1) <= 1) continue;
indices.clear();
diagonals2.clear();
indices.push_back(diagonal.index1);
indices.push_back(diagonal.index2);
diagonals2.push_front(diagonal);
while(!diagonals2.empty()) {
diagonal = *(diagonals2.begin());
diagonals2.pop_front();
if((diagonal.index2 - diagonal.index1) <= 1) continue;
ijreal = true;
jkreal = true;
pairs = &(dpstates[diagonal.index1][diagonal.index2].pairs);
if(!vertices[diagonal.index1].isConvex) {
iter = pairs->end();
iter--;
j = iter->index2;
if(iter->index1 != iter->index2) ijreal = false;
} else {
iter = pairs->begin();
j = iter->index1;
if(iter->index1 != iter->index2) jkreal = false;
}
newdiagonal.index1 = diagonal.index1;
newdiagonal.index2 = j;
if(ijreal) {
diagonals.push_back(newdiagonal);
} else {
diagonals2.push_back(newdiagonal);
}
newdiagonal.index1 = j;
newdiagonal.index2 = diagonal.index2;
if(jkreal) {
diagonals.push_back(newdiagonal);
} else {
diagonals2.push_back(newdiagonal);
}
indices.push_back(j);
}
indices.sort();
newpoly.Init((long)indices.size());
k=0;
for(iiter = indices.begin();iiter!=indices.end();iiter++) {
newpoly[k] = vertices[*iiter].p;
k++;
}
parts->push_back(newpoly);
}
for(i=0;i<n;i++) {
delete [] dpstates[i];
}
delete [] dpstates;
delete [] vertices;
return ret;
}
//minimum-weight polygon triangulation by dynamic programming
//O(n^3) time complexity
//O(n^2) space complexity
int TPPLPartition::Triangulate_OPT(TPPLPoly *poly, list<TPPLPoly> *triangles) {
long i,j,k,gap,n;
DPState **dpstates;
TPPLPoint p1,p2,p3,p4;
long bestvertex;
tppl_float weight,minweight,d1,d2;
Diagonal diagonal,newdiagonal;
list<Diagonal> diagonals;
TPPLPoly triangle;
int ret = 1;
n = poly->GetNumPoints();
dpstates = new DPState *[n];
for(i=1;i<n;i++) {
dpstates[i] = new DPState[i];
}
//init states and visibility
for(i=0;i<(n-1);i++) {
p1 = poly->GetPoint(i);
for(j=i+1;j<n;j++) {
dpstates[j][i].visible = true;
dpstates[j][i].weight = 0;
dpstates[j][i].bestvertex = -1;
if(j!=(i+1)) {
p2 = poly->GetPoint(j);
//visibility check
if(i==0) p3 = poly->GetPoint(n-1);
else p3 = poly->GetPoint(i-1);
if(i==(n-1)) p4 = poly->GetPoint(0);
else p4 = poly->GetPoint(i+1);
if(!InCone(p3,p1,p4,p2)) {
dpstates[j][i].visible = false;
continue;
}
if(j==0) p3 = poly->GetPoint(n-1);
else p3 = poly->GetPoint(j-1);
if(j==(n-1)) p4 = poly->GetPoint(0);
else p4 = poly->GetPoint(j+1);
if(!InCone(p3,p2,p4,p1)) {
dpstates[j][i].visible = false;
continue;
}
for(k=0;k<n;k++) {
p3 = poly->GetPoint(k);
if(k==(n-1)) p4 = poly->GetPoint(0);
else p4 = poly->GetPoint(k+1);
if(Intersects(p1,p2,p3,p4)) {
dpstates[j][i].visible = false;
break;
}
}
}
}
}
dpstates[n-1][0].visible = true;
dpstates[n-1][0].weight = 0;
dpstates[n-1][0].bestvertex = -1;
for(gap = 2; gap<n; gap++) {
for(i=0; i<(n-gap); i++) {
j = i+gap;
if(!dpstates[j][i].visible) continue;
bestvertex = -1;
for(k=(i+1);k<j;k++) {
if(!dpstates[k][i].visible) continue;
if(!dpstates[j][k].visible) continue;
if(k<=(i+1)) d1=0;
else d1 = Distance(poly->GetPoint(i),poly->GetPoint(k));
if(j<=(k+1)) d2=0;
else d2 = Distance(poly->GetPoint(k),poly->GetPoint(j));
weight = dpstates[k][i].weight + dpstates[j][k].weight + d1 + d2;
if((bestvertex == -1)||(weight<minweight)) {
bestvertex = k;
minweight = weight;
}
}
if(bestvertex == -1) {
for(i=1;i<n;i++) {
delete [] dpstates[i];
}
delete [] dpstates;
return 0;
}
dpstates[j][i].bestvertex = bestvertex;
dpstates[j][i].weight = minweight;
}
}
newdiagonal.index1 = 0;
newdiagonal.index2 = n-1;
diagonals.push_back(newdiagonal);
while(!diagonals.empty()) {
diagonal = *(diagonals.begin());
diagonals.pop_front();
bestvertex = dpstates[diagonal.index2][diagonal.index1].bestvertex;
if(bestvertex == -1) {
ret = 0;
break;
}
triangle.Triangle(poly->GetPoint(diagonal.index1),poly->GetPoint(bestvertex),poly->GetPoint(diagonal.index2));
triangles->push_back(triangle);
if(bestvertex > (diagonal.index1+1)) {
newdiagonal.index1 = diagonal.index1;
newdiagonal.index2 = bestvertex;
diagonals.push_back(newdiagonal);
}
if(diagonal.index2 > (bestvertex+1)) {
newdiagonal.index1 = bestvertex;
newdiagonal.index2 = diagonal.index2;
diagonals.push_back(newdiagonal);
}
}
for(i=1;i<n;i++) {
delete [] dpstates[i];
}
delete [] dpstates;
return ret;
}
int TPPLPartition::ConvexPartition_HM(TPPLPoly *poly, list<TPPLPoly> *parts) {
list<TPPLPoly> triangles;
list<TPPLPoly>::iterator iter1,iter2;
TPPLPoly *poly1 = 0, *poly2 = 0;
TPPLPoly newpoly;
TPPLPoint d1,d2,p1,p2,p3;
long i11,i12,i21,i22,i13,i23,j,k;
bool isdiagonal;
long numreflex;
//check if the poly is already convex
numreflex = 0;
for(i11=0;i11<poly->GetNumPoints();i11++) {
if(i11==0) i12 = poly->GetNumPoints()-1;
else i12=i11-1;
if(i11==(poly->GetNumPoints()-1)) i13=0;
else i13=i11+1;
if(IsReflex(poly->GetPoint(i12),poly->GetPoint(i11),poly->GetPoint(i13))) {
numreflex = 1;
break;
}
}
if(numreflex == 0) {
parts->push_back(*poly);
return 1;
}
if(!Triangulate_EC(poly,&triangles)) return 0;
for(iter1 = triangles.begin(); iter1 != triangles.end(); iter1++) {
poly1 = &(*iter1);
for(i11=0;i11<poly1->GetNumPoints();i11++) {
d1 = poly1->GetPoint(i11);
i12 = (i11+1)%(poly1->GetNumPoints());
d2 = poly1->GetPoint(i12);
isdiagonal = false;
for(iter2 = iter1; iter2 != triangles.end(); iter2++) {
if(iter1 == iter2) continue;
poly2 = &(*iter2);
for(i21=0;i21<poly2->GetNumPoints();i21++) {
if((d2.x != poly2->GetPoint(i21).x)||(d2.y != poly2->GetPoint(i21).y)) continue;
i22 = (i21+1)%(poly2->GetNumPoints());
if((d1.x != poly2->GetPoint(i22).x)||(d1.y != poly2->GetPoint(i22).y)) continue;
isdiagonal = true;
break;
}
if(isdiagonal) break;
}
if(!isdiagonal) continue;
p2 = poly1->GetPoint(i11);
if(i11 == 0) i13 = poly1->GetNumPoints()-1;
else i13 = i11-1;
p1 = poly1->GetPoint(i13);
if(i22 == (poly2->GetNumPoints()-1)) i23 = 0;
else i23 = i22+1;
p3 = poly2->GetPoint(i23);
if(!IsConvex(p1,p2,p3)) continue;
p2 = poly1->GetPoint(i12);
if(i12 == (poly1->GetNumPoints()-1)) i13 = 0;
else i13 = i12+1;
p3 = poly1->GetPoint(i13);
if(i21 == 0) i23 = poly2->GetNumPoints()-1;
else i23 = i21-1;
p1 = poly2->GetPoint(i23);
if(!IsConvex(p1,p2,p3)) continue;
newpoly.Init(poly1->GetNumPoints()+poly2->GetNumPoints()-2);
k = 0;
for(j=i12;j!=i11;j=(j+1)%(poly1->GetNumPoints())) {
newpoly[k] = poly1->GetPoint(j);
k++;
}
for(j=i22;j!=i21;j=(j+1)%(poly2->GetNumPoints())) {
newpoly[k] = poly2->GetPoint(j);
k++;
}
triangles.erase(iter2);
*iter1 = newpoly;
poly1 = &(*iter1);
i11 = -1;
continue;
}
}
for(iter1 = triangles.begin(); iter1 != triangles.end(); iter1++) {
parts->push_back(*iter1);
}
return 1;
}
//triangulation by ear removal
int TPPLPartition::Triangulate_EC(TPPLPoly *poly, list<TPPLPoly> *triangles) {
long numvertices;
PartitionVertex *vertices;
PartitionVertex *ear = 0;
TPPLPoly triangle;
long i,j;
bool earfound;
if(poly->GetNumPoints() < 3) return 0;
if(poly->GetNumPoints() == 3) {
triangles->push_back(*poly);
return 1;
}
numvertices = poly->GetNumPoints();
vertices = new PartitionVertex[numvertices];
for(i=0;i<numvertices;i++) {
vertices[i].isActive = true;
vertices[i].p = poly->GetPoint(i);
if(i==(numvertices-1)) vertices[i].next=&(vertices[0]);
else vertices[i].next=&(vertices[i+1]);
if(i==0) vertices[i].previous = &(vertices[numvertices-1]);
else vertices[i].previous = &(vertices[i-1]);
}
for(i=0;i<numvertices;i++) {
UpdateVertex(&vertices[i],vertices,numvertices);
}
for(i=0;i<numvertices-3;i++) {
earfound = false;
//find the most extruded ear
for(j=0;j<numvertices;j++) {
if(!vertices[j].isActive) continue;
if(!vertices[j].isEar) continue;
if(!earfound) {
earfound = true;
ear = &(vertices[j]);
} else {
if(vertices[j].angle > ear->angle) {
ear = &(vertices[j]);
}
}
}
if(!earfound) {
delete [] vertices;
return 0;
}
triangle.Triangle(ear->previous->p,ear->p,ear->next->p);
triangles->push_back(triangle);
ear->isActive = false;
ear->previous->next = ear->next;
ear->next->previous = ear->previous;
if(i==numvertices-4) break;
UpdateVertex(ear->previous,vertices,numvertices);
UpdateVertex(ear->next,vertices,numvertices);
}
for(i=0;i<numvertices;i++) {
if(vertices[i].isActive) {
triangle.Triangle(vertices[i].previous->p,vertices[i].p,vertices[i].next->p);
triangles->push_back(triangle);
break;
}
}
delete [] vertices;
return 1;
}
//removes holes from inpolys by merging them with non-holes
int TPPLPartition::RemoveHoles(list<TPPLPoly> *inpolys, list<TPPLPoly> *outpolys) {
list<TPPLPoly> polys;
list<TPPLPoly>::iterator holeiter,polyiter,iter,iter2;
long i,i2,holepointindex,polypointindex;
TPPLPoint holepoint,polypoint,bestpolypoint;
TPPLPoint linep1,linep2;
TPPLPoint v1,v2;
TPPLPoly newpoly;
bool hasholes;
bool pointvisible;
bool pointfound;
//check for trivial case (no holes)
hasholes = false;
for(iter = inpolys->begin(); iter!=inpolys->end(); iter++) {
if(iter->IsHole()) {
hasholes = true;
break;
}
}
if(!hasholes) {
for(iter = inpolys->begin(); iter!=inpolys->end(); iter++) {
outpolys->push_back(*iter);
}
return 1;
}
polys = *inpolys;
while(1) {
//find the hole point with the largest x
hasholes = false;
for(iter = polys.begin(); iter!=polys.end(); iter++) {
if(!iter->IsHole()) continue;
if(!hasholes) {
hasholes = true;
holeiter = iter;
holepointindex = 0;
}
for(i=0; i < iter->GetNumPoints(); i++) {
if(iter->GetPoint(i).x > holeiter->GetPoint(holepointindex).x) {
holeiter = iter;
holepointindex = i;
}
}
}
if(!hasholes) break;
holepoint = holeiter->GetPoint(holepointindex);
pointfound = false;
for(iter = polys.begin(); iter!=polys.end(); iter++) {
if(iter->IsHole()) continue;
for(i=0; i < iter->GetNumPoints(); i++) {
if(iter->GetPoint(i).x <= holepoint.x) continue;
if(!InCone(iter->GetPoint((i+iter->GetNumPoints()-1)%(iter->GetNumPoints())),
iter->GetPoint(i),
iter->GetPoint((i+1)%(iter->GetNumPoints())),
holepoint))
continue;
polypoint = iter->GetPoint(i);
if(pointfound) {
v1 = Normalize(polypoint-holepoint);
v2 = Normalize(bestpolypoint-holepoint);
if(v2.x > v1.x) continue;
}
pointvisible = true;
for(iter2 = polys.begin(); iter2!=polys.end(); iter2++) {
if(iter2->IsHole()) continue;
for(i2=0; i2 < iter2->GetNumPoints(); i2++) {
linep1 = iter2->GetPoint(i2);
linep2 = iter2->GetPoint((i2+1)%(iter2->GetNumPoints()));
if(Intersects(holepoint,polypoint,linep1,linep2)) {
pointvisible = false;
break;
}
}
if(!pointvisible) break;
}
if(pointvisible) {
pointfound = true;
bestpolypoint = polypoint;
polyiter = iter;
polypointindex = i;
}
}
}
if(!pointfound) return 0;
newpoly.Init(holeiter->GetNumPoints() + polyiter->GetNumPoints() + 2);
i2 = 0;
for(i=0;i<=polypointindex;i++) {
newpoly[i2] = polyiter->GetPoint(i);
i2++;
}
for(i=0;i<=holeiter->GetNumPoints();i++) {
newpoly[i2] = holeiter->GetPoint((i+holepointindex)%holeiter->GetNumPoints());
i2++;
}
for(i=polypointindex;i<polyiter->GetNumPoints();i++) {
newpoly[i2] = polyiter->GetPoint(i);
i2++;
}
polys.erase(holeiter);
polys.erase(polyiter);
polys.push_back(newpoly);
}
for(iter = polys.begin(); iter!=polys.end(); iter++) {
outpolys->push_back(*iter);
}
return 1;
}
//triangulates monotone polygon
//O(n) time, O(n) space complexity
int TPPLPartition::TriangulateMonotone(TPPLPoly *inPoly, list<TPPLPoly> *triangles) {
long i,i2,j,topindex,bottomindex,leftindex,rightindex,vindex;
TPPLPoint *points;
long numpoints;
TPPLPoly triangle;
numpoints = inPoly->GetNumPoints();
points = inPoly->GetPoints();
//trivial calses
if(numpoints < 3) return 0;
if(numpoints == 3) {
triangles->push_back(*inPoly);
}
topindex = 0; bottomindex=0;
for(i=1;i<numpoints;i++) {
if(Below(points[i],points[bottomindex])) bottomindex = i;
if(Below(points[topindex],points[i])) topindex = i;
}
//check if the poly is really monotone
i = topindex;
while(i!=bottomindex) {
i2 = i+1; if(i2>=numpoints) i2 = 0;
if(!Below(points[i2],points[i])) return 0;
i = i2;
}
i = bottomindex;
while(i!=topindex) {
i2 = i+1; if(i2>=numpoints) i2 = 0;
if(!Below(points[i],points[i2])) return 0;
i = i2;
}
char *vertextypes = new char[numpoints];
long *priority = new long[numpoints];
//merge left and right vertex chains
priority[0] = topindex;
vertextypes[topindex] = 0;
leftindex = topindex+1; if(leftindex>=numpoints) leftindex = 0;
rightindex = topindex-1; if(rightindex<0) rightindex = numpoints-1;
for(i=1;i<(numpoints-1);i++) {
if(leftindex==bottomindex) {
priority[i] = rightindex;
rightindex--; if(rightindex<0) rightindex = numpoints-1;
vertextypes[priority[i]] = -1;
} else if(rightindex==bottomindex) {
priority[i] = leftindex;
leftindex++; if(leftindex>=numpoints) leftindex = 0;
vertextypes[priority[i]] = 1;
} else {
if(Below(points[leftindex],points[rightindex])) {
priority[i] = rightindex;
rightindex--; if(rightindex<0) rightindex = numpoints-1;
vertextypes[priority[i]] = -1;
} else {
priority[i] = leftindex;
leftindex++; if(leftindex>=numpoints) leftindex = 0;
vertextypes[priority[i]] = 1;
}
}
}
priority[i] = bottomindex;
vertextypes[bottomindex] = 0;
long *stack = new long[numpoints];
long stackptr = 0;
stack[0] = priority[0];
stack[1] = priority[1];
stackptr = 2;
//for each vertex from top to bottom trim as many triangles as possible
for(i=2;i<(numpoints-1);i++) {
vindex = priority[i];
if(vertextypes[vindex]!=vertextypes[stack[stackptr-1]]) {
for(j=0;j<(stackptr-1);j++) {
if(vertextypes[vindex]==1) {
triangle.Triangle(points[stack[j+1]],points[stack[j]],points[vindex]);
} else {
triangle.Triangle(points[stack[j]],points[stack[j+1]],points[vindex]);
}
triangles->push_back(triangle);
}
stack[0] = priority[i-1];
stack[1] = priority[i];
stackptr = 2;
} else {
stackptr--;
while(stackptr>0) {
if(vertextypes[vindex]==1) {
if(IsConvex(points[vindex],points[stack[stackptr-1]],points[stack[stackptr]])) {
triangle.Triangle(points[vindex],points[stack[stackptr-1]],points[stack[stackptr]]);
triangles->push_back(triangle);
stackptr--;
} else {
break;
}
} else {
if(IsConvex(points[vindex],points[stack[stackptr]],points[stack[stackptr-1]])) {
triangle.Triangle(points[vindex],points[stack[stackptr]],points[stack[stackptr-1]]);
triangles->push_back(triangle);
stackptr--;
} else {
break;
}
}
}
stackptr++;
stack[stackptr] = vindex;
stackptr++;
}
}
vindex = priority[i];
for(j=0;j<(stackptr-1);j++) {
if(vertextypes[stack[j+1]]==1) {
triangle.Triangle(points[stack[j]],points[stack[j+1]],points[vindex]);
} else {
triangle.Triangle(points[stack[j+1]],points[stack[j]],points[vindex]);
}
triangles->push_back(triangle);
}
delete [] priority;
delete [] vertextypes;
delete [] stack;
return 1;
}
void ShapeMarker::onNewMessage( const MarkerConstPtr& old_message,
const MarkerConstPtr& new_message )
{
TPPLPartition pp;
list<TPPLPoly> polys,result;
//fill polys
for(size_t i=0; i<new_message->points.size(); i++) {
pcl::PointCloud<pcl::PointXYZ> pc;
TPPLPoly poly;
pcl::fromROSMsg(new_message->points[i],pc);
poly.Init(pc.size());
poly.SetHole(new_message->holes[i]);
for(size_t j=0; j<pc.size(); j++) {
poly[j] = MsgToPoint2D(pc[j], new_message);
}
if(new_message->holes[i])
poly.SetOrientation(TPPL_CW);
else
poly.SetOrientation(TPPL_CCW);
polys.push_back(poly);
}
pp.Triangulate_EC(&polys,&result);
polygon_->clear();
polygon_->begin(createMaterialIfNotExists(new_message->color.r,new_message->color.b,new_message->color.g,new_message->color.a), Ogre::RenderOperation::OT_TRIANGLE_LIST);
TPPLPoint p1,p2,p3,p4, p12,p23,p31;
for(std::list<TPPLPoly>::iterator it=result.begin(); it!=result.end(); it++) {
//draw each triangle
if(it->GetNumPoints()!=3) continue;
p1 = it->GetPoint(0);
p2 = it->GetPoint(1);
p3 = it->GetPoint(2);
p4.x = (p1.x+p2.x+p3.x)/3;
p4.y = (p1.y+p2.y+p3.y)/3;
p12.x = (p1.x+p2.x)/2;
p12.y = (p1.y+p2.y)/2;
p23.x = (p3.x+p2.x)/2;
p23.y = (p3.y+p2.y)/2;
p31.x = (p1.x+p3.x)/2;
p31.y = (p1.y+p3.y)/2;
triangle(new_message,polygon_,p1,p12,p4);
triangle(new_message,polygon_,p1,p31,p4);
triangle(new_message,polygon_,p3,p23,p4);
triangle(new_message,polygon_,p12,p2,p4);
triangle(new_message,polygon_,p31,p3,p4);
triangle(new_message,polygon_,p23,p2,p4);
}
polygon_->end();
vis_manager_->getSelectionManager()->removeObject(coll_);
/*coll_ = vis_manager_->getSelectionManager()->createCollisionForObject(
shape_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID(
"fake_ns", new_message->id))), coll_);*/
Ogre::Vector3 pos, scale, scale_correct;
Ogre::Quaternion orient;
//transform(new_message, pos, orient, scale);
/*if (owner_ && (new_message->scale.x * new_message->scale.y
* new_message->scale.z == 0.0f))
{
owner_->setMarkerStatus(getID(), status_levels::Warn,
"Scale of 0 in one of x/y/z");
}*/
Eigen::Vector3f origin=MsgToOrigin(new_message);
pos.x = origin(0);
pos.y = origin(1);
pos.z = origin(2);
//setPosition(pos);
return;
setOrientation( orient * Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) );
//scale_correct = Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) * scale;
//shape_->setScale(scale_correct);
}
void PlaneExt::TriangulatePlanePolygon()
{
// clear Marker and Shape messages
planeTriangles.points.clear();
planeTrianglesSRS.clear();
// for all polygons representing this plane
for (unsigned int polygon_i = 0; polygon_i < planePolygonsClipper.size(); ++polygon_i)
{
planeTrianglesSRS.push_back(cob_3d_mapping_msgs::Shape());
planeTrianglesSRS.back().type = cob_3d_mapping_msgs::Shape::POLYGON;
planeTrianglesSRS.back().params.push_back(a);
planeTrianglesSRS.back().params.push_back(b);
planeTrianglesSRS.back().params.push_back(c);
planeTrianglesSRS.back().params.push_back(d);
planeTrianglesSRS.back().holes.push_back(false);
// convert each polygon to polygonizer DS
TPPLPoly triPolygon;
triPolygon.Init(planePolygonsClipper[polygon_i].outer.size());
pcl::PointCloud<pcl::PointXYZ> current_point_cloud;
for (unsigned int i = 0; i < planePolygonsClipper[polygon_i].outer.size(); ++i)
{
triPolygon[i].x = CONVERT_FROM_LONG(planePolygonsClipper[polygon_i].outer[i].X);
triPolygon[i].y = CONVERT_FROM_LONG(planePolygonsClipper[polygon_i].outer[i].Y);
// additionaly, insert this point into shape message
pcl::PointXYZ point;
point.x = triPolygon[i].x;
point.y = triPolygon[i].y;
point.z = 0;
current_point_cloud.push_back(point);
}
// triangulate
TPPLPartition triangulation;
std::list<TPPLPoly> triangles;
triangulation.Triangulate_EC(&triPolygon, &triangles);
// create a message object
for (std::list<TPPLPoly>::iterator it = triangles.begin(); it != triangles.end(); ++it)
{
for (unsigned int j = 0; j < it->GetNumPoints(); ++j)
{
Eigen::Vector3f vec;
vec(0) = it->GetPoint(j).x;
vec(1) = it->GetPoint(j).y;
vec(2) = 0;
vec = planeTransXY.inverse() * vec;
geometry_msgs::Point p;
p.x = vec(0) + planeShift*a;
p.y = vec(1) + planeShift*b;
p.z = vec(2) + planeShift*c;
planeTriangles.points.push_back(p);
}
}
// insert polygon point cloud into Shape message
pcl::transformPointCloud(current_point_cloud, current_point_cloud, planeTransXY.inverse());
pcl::transformPointCloud(current_point_cloud, current_point_cloud, Eigen::Vector3f(a*planeShift, b*planeShift, c*planeShift), Eigen::Quaternion<float>(0,0,0,0));
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(current_point_cloud, cloud);
planeTrianglesSRS.back().points.push_back(cloud);
planeTrianglesSRS.back().centroid.x = planeShift*a;
planeTrianglesSRS.back().centroid.y = planeShift*b;
planeTrianglesSRS.back().centroid.z = planeShift*c;
planeTrianglesSRS.back().color = color;
planeTriangles.color = color;
}
// compute centroid
}
std::vector<std::vector<Point3d> > computeTriangulation(const Point3dVector& vertices, const std::vector<std::vector<Point3d> >& holes, double tol)
{
std::vector<std::vector<Point3d> > result;
// check input
if (vertices.size () < 3){
return result;
}
boost::optional<Vector3d> normal = getOutwardNormal(vertices);
if (!normal || normal->z() > -0.999){
return result;
}
for (const auto& hole : holes){
normal = getOutwardNormal(hole);
if (!normal || normal->z() > -0.999){
return result;
}
}
std::vector<Point3d> allPoints;
// PolyPartition does not support holes which intersect the polygon or share an edge
// if any hole is not fully contained we will use boost to remove all the holes
bool polyPartitionHoles = true;
for (const std::vector<Point3d>& hole : holes){
if (!within(hole, vertices, tol)){
// PolyPartition can't handle this
polyPartitionHoles = false;
break;
}
}
if (!polyPartitionHoles){
// use boost to do all the intersections
std::vector<std::vector<Point3d> > allFaces = subtract(vertices, holes, tol);
std::vector<std::vector<Point3d> > noHoles;
for (const std::vector<Point3d>& face : allFaces){
std::vector<std::vector<Point3d> > temp = computeTriangulation(face, noHoles);
result.insert(result.end(), temp.begin(), temp.end());
}
return result;
}
// convert input to vector of TPPLPoly
std::list<TPPLPoly> polys;
TPPLPoly outerPoly; // must be counter-clockwise, input vertices are clockwise
outerPoly.Init(vertices.size());
outerPoly.SetHole(false);
unsigned n = vertices.size();
for(unsigned i = 0; i < n; ++i){
// should all have zero z coordinate now
double z = vertices[n-i-1].z();
if (abs(z) > tol){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
return result;
}
Point3d point = getCombinedPoint(vertices[n-i-1], allPoints, tol);
outerPoly[i].x = point.x();
outerPoly[i].y = point.y();
}
outerPoly.SetOrientation(TPPL_CCW);
polys.push_back(outerPoly);
for (const std::vector<Point3d>& holeVertices : holes){
if (holeVertices.size () < 3){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Hole has fewer than 3 points, ignoring");
continue;
}
TPPLPoly innerPoly; // must be clockwise, input vertices are clockwise
innerPoly.Init(holeVertices.size());
innerPoly.SetHole(true);
//std::cout << "inner :";
for(unsigned i = 0; i < holeVertices.size(); ++i){
// should all have zero z coordinate now
double z = holeVertices[i].z();
if (abs(z) > tol){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods");
return result;
}
Point3d point = getCombinedPoint(holeVertices[i], allPoints, tol);
innerPoly[i].x = point.x();
innerPoly[i].y = point.y();
}
innerPoly.SetOrientation(TPPL_CW);
polys.push_back(innerPoly);
}
// do partitioning
TPPLPartition pp;
std::list<TPPLPoly> resultPolys;
int test = pp.Triangulate_EC(&polys,&resultPolys);
if (test == 0){
test = pp.Triangulate_MONO(&polys, &resultPolys);
}
if (test == 0){
LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Failed to partition polygon");
return result;
}
// convert back to vertices
std::list<TPPLPoly>::iterator it, itend;
//std::cout << "Start" << std::endl;
for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){
it->SetOrientation(TPPL_CW);
std::vector<Point3d> triangle;
for (long i = 0; i < it->GetNumPoints(); ++i){
TPPLPoint point = it->GetPoint(i);
triangle.push_back(Point3d(point.x, point.y, 0));
}
//std::cout << triangle << std::endl;
result.push_back(triangle);
}
//std::cout << "End" << std::endl;
return result;
}
void
ShapeMarker::createMarker (visualization_msgs::InteractiveMarkerControl& im_ctrl)
{
marker.id = shape_.id;
marker.header = shape_.header;
//std::cout << marker.header.frame_id << std::endl;
//marker.header.stamp = ros::Time::now() ;
marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
marker.ns = "shape visualization";
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration ();
//set color
marker.color.g = shape_.color.g;
marker.color.b = shape_.color.b;
marker.color.r = shape_.color.r;
if (arrows_ || deleted_){
marker.color.a = 0.5;
}
else
{
marker.color.a = shape_.color.a;
// marker.color.r = shape_.color.r;
}
//set scale
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 1;
/* transform shape points to 2d and store 2d point in triangle list */
TPPLPartition pp;
list<TPPLPoly> polys, tri_list;
Eigen::Vector3f v, normal, origin;
if(shape_.type== cob_3d_mapping_msgs::Shape::CYLINDER)
{
cob_3d_mapping::Cylinder c;
cob_3d_mapping::fromROSMsg (shape_, c);
c.ParamsFromShapeMsg();
// make trinagulated cylinder strip
//transform cylinder in local coordinate system
c.makeCyl2D();
c.TransformContours(c.transform_from_world_to_plane);
//c.transform2tf(c.transform_from_world_to_plane);
//TODO: WATCH OUT NO HANDLING FOR MULTY CONTOUR CYLINDERS AND HOLES
TPPLPoly poly;
TPPLPoint pt;
for(size_t j=0;j<c.contours.size();j++){
poly.Init(c.contours[j].size());
poly.SetHole (shape_.holes[j]);
for(size_t i=0;i<c.contours[j].size();++i){
pt.x=c.contours[j][i][0];
pt.y=c.contours[j][i][1];
poly[i]=pt;
}
if (shape_.holes[j])
poly.SetOrientation (TPPL_CW);
else
poly.SetOrientation (TPPL_CCW);
polys.push_back(poly);
}
// triangualtion itno monotone triangles
pp.Triangulate_EC (&polys, &tri_list);
transformation_inv_ = c.transform_from_world_to_plane.inverse();
// optional refinement step
list<TPPLPoly> refined_tri_list;
triangle_refinement(tri_list,refined_tri_list);
tri_list=refined_tri_list;
}
if(shape_.type== cob_3d_mapping_msgs::Shape::POLYGON)
{
cob_3d_mapping::Polygon p;
if (shape_.params.size () == 4)
{
cob_3d_mapping::fromROSMsg (shape_, p);
normal (0) = shape_.params[0];
normal (1) = shape_.params[1];
normal (2) = shape_.params[2];
origin (0) = shape_.centroid.x;
origin (1) = shape_.centroid.y;
origin (2) = shape_.centroid.z;
v = normal.unitOrthogonal ();
pcl::getTransformationFromTwoUnitVectorsAndOrigin (v, normal, origin, transformation_);
transformation_inv_ = transformation_.inverse ();
}
for (size_t i = 0; i < shape_.points.size (); i++)
{
pcl::PointCloud<pcl::PointXYZ> pc;
TPPLPoly poly;
pcl::fromROSMsg (shape_.points[i], pc);
poly.Init (pc.points.size ());
poly.SetHole (shape_.holes[i]);
for (size_t j = 0; j < pc.points.size (); j++)
{
poly[j] = msgToPoint2D (pc[j]);
}
if (shape_.holes[i])
poly.SetOrientation (TPPL_CW);
else
poly.SetOrientation (TPPL_CCW);
polys.push_back (poly);
}
pp.Triangulate_EC (&polys, &tri_list);
}//Polygon
if(tri_list.size()==0)
{
ROS_WARN("Could not triangulate, will not display this shape! (ID: %d)", shape_.id);
}
//ROS_INFO(" creating markers for this shape.....");
marker.points.resize (/*it->GetNumPoints ()*/tri_list.size()*3);
TPPLPoint pt;
int ctr=0;
for (std::list<TPPLPoly>::iterator it = tri_list.begin (); it != tri_list.end (); it++)
{
//draw each triangle
switch(shape_.type)
{
case(cob_3d_mapping_msgs::Shape::POLYGON):
{
for (long i = 0; i < it->GetNumPoints (); i++)
{
pt = it->GetPoint (i);
marker.points[3*ctr+i].x = pt.x;
marker.points[3*ctr+i].y = pt.y;
marker.points[3*ctr+i].z = 0;
//if(shape_.id == 39) std::cout << pt.x << "," << pt.y << std::endl;
}
//std::cout << marker.points.size() << std::endl;
}
case(cob_3d_mapping_msgs::Shape::CYLINDER):
{
for (long i = 0; i < it->GetNumPoints (); i++)
{
pt = it->GetPoint(i);
//apply rerolling of cylinder analogous to cylinder class
if(shape_.params.size()!=10){
break;
}
double alpha=pt.x/shape_.params[9];
marker.points[3*ctr+i].x = shape_.params[9]*sin(-alpha);
marker.points[3*ctr+i].y = pt.y;
marker.points[3*ctr+i].z = shape_.params[9]*cos(-alpha);
////Keep Cylinder flat - Debuging
//marker.points[i].x = pt.x;
//marker.points[i].y = pt.y;
//marker.points[i].z = 0;
}
}
}
ctr++;
}
//set pose
Eigen::Quaternionf quat (transformation_inv_.rotation ());
Eigen::Vector3f trans (transformation_inv_.translation ());
marker.pose.position.x = trans (0);
marker.pose.position.y = trans (1);
marker.pose.position.z = trans (2);
marker.pose.orientation.x = quat.x ();
marker.pose.orientation.y = quat.y ();
marker.pose.orientation.z = quat.z ();
marker.pose.orientation.w = quat.w ();
im_ctrl.markers.push_back (marker);
// if(!arrows_) {
// Added For displaying the arrows on Marker Position
marker_.pose.position.x = marker.pose.position.x ;
marker_.pose.position.y = marker.pose.position.y ;
marker_.pose.position.z = marker.pose.position.z ;
marker_.pose.orientation.x = marker.pose.orientation.x ;
marker_.pose.orientation.y = marker.pose.orientation.y ;
marker_.pose.orientation.z = marker.pose.orientation.z ;
// end
}
