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

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

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

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