Exemplo n.º 1
0
int main()
{
	TPPLPoly poly;
	{
		float x = 2.0f;
		float buf[] = { -x, -x, x, -x, x, x, -x, x };
		make_poly(buf, 4, poly);
	}

	TPPLPoly hole;
	{
		float x = 1.0f;
		// float buf[] = { -x, -x, x, -x, x, x, 0, 0, -x, x };
		float buf[] = {-x,-x, -x,x, 0,0, x,x, x,-x};
		make_poly(buf, 5, hole);
	}

	std::list< TPPLPoly > list;
	list.push_back(poly);
	list.push_back(hole);

	std::list< TPPLPoly > triangles;

	TPPLPartition pp;
	if (!pp.Triangulate_EC(&list, &triangles))
	{
		printf("failed\n");
	}

	return 0;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
int main() {
	TPPLPartition pp;
	list<TPPLPoly> testpolys,result;

	ReadPolyList("failing_mono_clean - copy.txt",&testpolys);
	DrawPolyList("test.bmp", &testpolys);
	if(!pp.Triangulate_MONO(&testpolys,&result)) printf("Error\n");
	DrawPolyList("test2.bmp", &result);

}
Exemplo n.º 4
0
  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);
  }
Exemplo n.º 5
0
list<TPPLPoly> NavMesh::_processNavigationConvexTesselation(list<TPPLPoly> &polygons)
{
    TPPLPartition partition;

    list<TPPLPoly> navigationMeshPolygons;

    // convex tesselation with minimal convex elements
    int res = partition.ConvexPartition_HM(&polygons, &navigationMeshPolygons);

    if( res == 0 )
        std::cerr<< "NavMesh::processNavigationTesselation -> Error in the ConvexPartition_HM method"<<std::endl;

    return navigationMeshPolygons;
}
Exemplo n.º 6
0
void GeneralPolygon::_updateOptimalConvexTesselation()
{
	TPPLPartition partition;
	
	_optimalConvexTesselation.clear();

	list<TPPLPoly> polys = (list<TPPLPoly>)*this;
	
	// convex tesselation with minimal convex elements
	int res = partition.ConvexPartition_HM(&polys, &_optimalConvexTesselation);

	// _optimalConvexTesselation will be in local space

	if( res == 0 )
		std::cerr<< "GeneralPolygon::_updateOptimalConvexTesselation -> Error in the ConvexPartition_HM method"<<std::endl;

	polys.clear();
}
Exemplo n.º 7
0
static std::vector<Polygon> decompose(const Polygon& polygons) {
    TPPLPartition pp;
    TPPLPoly tp_poly;
    TPPLPolyList tp_parts;
    tp_poly.Init(polygons.size());
    for (size_t i = 0; i < polygons.size(); i++) {
        tp_poly[i] = {polygons[i].x, polygons[i].y, i};
    }
    pp.ConvexPartition_HM(&tp_poly, &tp_parts);
    std::vector<Polygon> ret;
    for (TPPLPoly& tp_poly : tp_parts) {
        Polygon polygon;
        for (size_t i = 0; i < tp_poly.GetNumPoints(); i++) {
            auto point = tp_poly.GetPoint(i);
            polygon.push_back({point.x, point.y});
        }
        ret.push_back(polygon);
    }
    return ret;
}
Exemplo n.º 8
0
 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));
     }
   }
 }
Exemplo n.º 9
0
void GenerateTestData() {
	TPPLPartition pp;
	
	list<TPPLPoly> testpolys,result,expectedResult;

	ReadPolyList("test_input.txt",&testpolys);

	DrawPolyList("test_input.bmp",&testpolys);

	pp.Triangulate_EC(&testpolys,&result);
	//pp.Triangulate_EC(&(*testpolys.begin()),&result);
	DrawPolyList("test_triangulate_EC.bmp",&result);
	WritePolyList("test_triangulate_EC.txt",&result);

	result.clear(); expectedResult.clear();

	pp.Triangulate_OPT(&(*testpolys.begin()),&result);
	DrawPolyList("test_triangulate_OPT.bmp",&result);
	WritePolyList("test_triangulate_OPT.txt",&result);

	result.clear(); expectedResult.clear();

	pp.Triangulate_MONO(&testpolys,&result);
	//pp.Triangulate_MONO(&(*testpolys.begin()),&result);
	DrawPolyList("test_triangulate_MONO.bmp",&result);
	WritePolyList("test_triangulate_MONO.txt",&result);

	result.clear(); expectedResult.clear();

	pp.ConvexPartition_HM(&testpolys,&result);
	//pp.ConvexPartition_HM(&(*testpolys.begin()),&result);
	DrawPolyList("test_convexpartition_HM.bmp",&result);
	WritePolyList("test_convexpartition_HM.txt",&result);

	result.clear(); expectedResult.clear();

	pp.ConvexPartition_OPT(&(*testpolys.begin()),&result);
	DrawPolyList("test_convexpartition_OPT.bmp",&result);
	WritePolyList("test_convexpartition_OPT.txt",&result);
}
Exemplo n.º 10
0
// Triangulate a polygon, returning vertex indices to the triangles
//
// verts - 3D vertex positions
// outerRingInds  - Indices of outer boundary vertices in `verts` array;
//                  the j'th component of the i'th polygon vertex is
//                  verts[3*outerRingInds[i]+j].
// innerRingSizes - Number of vertices in each polygon hole
// innerRingInds  - Indices of vertices for all holes; the first hole has
//                  indices innerRingInds[i] for i in [0, innerRingSize[0])
// triangleInds   - Indices of triangular pieces are appended to this array.
//
// Return false if polygon couldn't be triangulated.
static bool triangulatePolygon(const std::vector<float>& verts,
                               const std::vector<GLuint>& outerRingInds,
                               const std::vector<GLuint>& innerRingSizes,
                               const std::vector<GLuint>& innerRingInds,
                               std::vector<GLuint>& triangleInds)
{
    // Figure out which dimension the bounding box is smallest along, and
    // discard it to reduce dimensionality to 2D.
    Imath::Box3d bbox;
    for (size_t i = 0; i < outerRingInds.size(); ++i)
    {
        GLuint j = 3*outerRingInds[i];
        assert(j + 2 < verts.size());
        bbox.extendBy(V3d(verts[j], verts[j+1], verts[j+2]));
    }
    V3d diag = bbox.size();
    int xind = 0;
    int yind = 1;
    if (diag.z > std::min(diag.x, diag.y))
    {
        if (diag.x < diag.y)
            xind = 2;
        else
            yind = 2;
    }
    std::list<TPPLPoly> triangles;
    if (innerRingSizes.empty())
    {
        TPPLPoly poly;
        // Simple case - no holes
        //
        // TODO: Use Triangulate_MONO, after figuring out why it's not always
        // working?
        initTPPLPoly(poly, verts, xind, yind,
                     outerRingInds.data(), (int)outerRingInds.size(), false);
        TPPLPartition polypartition;
        if (!polypartition.Triangulate_EC(&poly, &triangles))
        {
            g_logger.warning("Ignoring polygon which couldn't be triangulated.");
            return false;
        }
    }
    else
    {
        // Set up outer polygon boundary and holes.
        std::list<TPPLPoly> inputPolys;
        inputPolys.resize(1 + innerRingSizes.size());
        auto polyIter = inputPolys.begin();
        initTPPLPoly(*polyIter, verts, xind, yind,
                     outerRingInds.data(), (int)outerRingInds.size(), false);
        ++polyIter;
        for (size_t i = 0, j = 0; i < innerRingSizes.size(); ++i, ++polyIter)
        {
            size_t count = innerRingSizes[i];
            if (j + count > innerRingInds.size())
            {
                g_logger.warning("Ignoring polygon with inner ring %d of %d too large",
                                 i, innerRingSizes.size());
                return false;
            }
            initTPPLPoly(*polyIter, verts, xind, yind,
                         innerRingInds.data() + j, (int)count, true);
            j += count;
        }
        // Triangulate
        std::list<TPPLPoly> noHolePolys;
        TPPLPartition polypartition;
        polypartition.RemoveHoles(&inputPolys, &noHolePolys);
        if (!polypartition.Triangulate_EC(&noHolePolys, &triangles))
        {
            g_logger.warning("Ignoring polygon which couldn't be triangulated.");
            return false;
        }
    }
    for (auto tri = triangles.begin(); tri != triangles.end(); ++tri)
    {
        triangleInds.push_back((*tri)[0].id);
        triangleInds.push_back((*tri)[1].id);
        triangleInds.push_back((*tri)[2].id);
    }
    return true;
}
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
}
Exemplo n.º 12
0
  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;
  }
Exemplo n.º 13
0
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);
  }