C++ TPPLPoly::Init方法代码示例

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

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

}

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

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

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

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

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

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

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

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

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