// 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);
}

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;
//.........这里部分代码省略.........

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);
				}
			}
		}
//.........这里部分代码省略.........

//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;
		}
	}

//.........这里部分代码省略.........

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());
//.........这里部分代码省略.........

//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 {
//.........这里部分代码省略.........

  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;
//.........这里部分代码省略.........

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_