C++ Clipper::AddPaths方法代码示例

ClipperLib::PolyTree
_clipper_do(const ClipperLib::ClipType clipType, const Polylines &subject, 
    const Polygons &clip, const ClipperLib::PolyFillType fillType,
    const bool safety_offset_)
{
    // read input
    ClipperLib::Paths input_subject = Slic3rMultiPoints_to_ClipperPaths(subject);
    ClipperLib::Paths input_clip    = Slic3rMultiPoints_to_ClipperPaths(clip);
    
    // perform safety offset
    if (safety_offset_) safety_offset(&input_clip);
    
    // init Clipper
    ClipperLib::Clipper clipper;
    clipper.Clear();
    
    // add polygons
    clipper.AddPaths(input_subject, ClipperLib::ptSubject, false);
    clipper.AddPaths(input_clip,    ClipperLib::ptClip,    true);
    
    // perform operation
    ClipperLib::PolyTree retval;
    clipper.Execute(clipType, retval, fillType, fillType);
    return retval;
}

void _clipper_do(const ClipperLib::ClipType clipType, const Slic3r::Polylines &subject, 
    const Slic3r::Polygons &clip, ClipperLib::PolyTree* retval, const ClipperLib::PolyFillType fillType,
    const bool safety_offset_)
{
    PROFILE_BLOCK(_clipper_do_polylines);

    // read input
    ClipperLib::Paths input_subject, input_clip;
    Slic3rMultiPoints_to_ClipperPaths(subject, &input_subject);
    Slic3rMultiPoints_to_ClipperPaths(clip,    &input_clip);
    
    // perform safety offset
    if (safety_offset_) safety_offset(&input_clip);
    
    // init Clipper
    ClipperLib::Clipper clipper;
    clipper.Clear();
    
    // add polygons
    {
        PROFILE_BLOCK(_clipper_do_polylines_AddPaths);
        clipper.AddPaths(input_subject, ClipperLib::ptSubject, false);
        clipper.AddPaths(input_clip,    ClipperLib::ptClip,    true);
    }
    
    // perform operation
    {
        PROFILE_BLOCK(_clipper_do_polylines_Execute);
        clipper.Execute(clipType, *retval, fillType, fillType);
    }
}

void _clipper_do(const ClipperLib::ClipType clipType, const Slic3r::Polygons &subject, 
    const Slic3r::Polygons &clip, T &retval, const ClipperLib::PolyFillType fillType, const bool safety_offset_)
{
    // read input
    ClipperLib::Paths* input_subject = new ClipperLib::Paths();
    ClipperLib::Paths* input_clip    = new ClipperLib::Paths();
    Slic3rMultiPoints_to_ClipperPaths(subject, *input_subject);
    Slic3rMultiPoints_to_ClipperPaths(clip,    *input_clip);
    
    // perform safety offset
    if (safety_offset_) {
        if (clipType == ClipperLib::ctUnion) {
            safety_offset(input_subject);
        } else {
            safety_offset(input_clip);
        }
    }
    
    // init Clipper
    ClipperLib::Clipper clipper;
    clipper.Clear();
    
    // add polygons
    clipper.AddPaths(*input_subject, ClipperLib::ptSubject, true);
    delete input_subject;
    clipper.AddPaths(*input_clip, ClipperLib::ptClip, true);
    delete input_clip;
    
    // perform operation
    clipper.Execute(clipType, retval, fillType, fillType);
}

// Fix of #117: A large fractal pyramid takes ages to slice
// The Clipper library has difficulties processing overlapping polygons.
// Namely, the function Clipper::JoinCommonEdges() has potentially a terrible time complexity if the output
// of the operation is of the PolyTree type.
// This function implmenets a following workaround:
// 1) Peform the Clipper operation with the output to Paths. This method handles overlaps in a reasonable time.
// 2) Run Clipper Union once again to extract the PolyTree from the result of 1).
inline ClipperLib::PolyTree _clipper_do_polytree2(const ClipperLib::ClipType clipType, const Polygons &subject, 
    const Polygons &clip, const ClipperLib::PolyFillType fillType, const bool safety_offset_)
{
    // read input
    ClipperLib::Paths input_subject = Slic3rMultiPoints_to_ClipperPaths(subject);
    ClipperLib::Paths input_clip    = Slic3rMultiPoints_to_ClipperPaths(clip);
    
    // perform safety offset
    if (safety_offset_)
        safety_offset((clipType == ClipperLib::ctUnion) ? &input_subject : &input_clip);
    
    ClipperLib::Clipper clipper;
    clipper.AddPaths(input_subject, ClipperLib::ptSubject, true);
    clipper.AddPaths(input_clip,    ClipperLib::ptClip,    true);
    // Perform the operation with the output to input_subject.
    // This pass does not generate a PolyTree, which is a very expensive operation with the current Clipper library
    // if there are overapping edges.
    clipper.Execute(clipType, input_subject, fillType, fillType);
    // Perform an additional Union operation to generate the PolyTree ordering.
    clipper.Clear();
    clipper.AddPaths(input_subject, ClipperLib::ptSubject, true);
    ClipperLib::PolyTree retval;
    clipper.Execute(ClipperLib::ctUnion, retval, fillType, fillType);
    return retval;
}

// This is a safe variant of the polygon offset, tailored for a single ExPolygon:
// a single polygon with multiple non-overlapping holes.
// Each contour and hole is offsetted separately, then the holes are subtracted from the outer contours.
ClipperLib::Paths _offset(const Slic3r::ExPolygon &expolygon, const float delta,
    ClipperLib::JoinType joinType, double miterLimit)
{
//    printf("new ExPolygon offset\n");
    // 1) Offset the outer contour.
    const float delta_scaled = delta * float(CLIPPER_OFFSET_SCALE);
    ClipperLib::Paths contours;
    {
        ClipperLib::Path input = Slic3rMultiPoint_to_ClipperPath(expolygon.contour);
        scaleClipperPolygon(input);
        ClipperLib::ClipperOffset co;
        if (joinType == jtRound)
            co.ArcTolerance = miterLimit * double(CLIPPER_OFFSET_SCALE);
        else
            co.MiterLimit = miterLimit;
        co.ShortestEdgeLength = double(std::abs(delta_scaled * CLIPPER_OFFSET_SHORTEST_EDGE_FACTOR));
        co.AddPath(input, joinType, ClipperLib::etClosedPolygon);
        co.Execute(contours, delta_scaled);
    }

    // 2) Offset the holes one by one, collect the results.
    ClipperLib::Paths holes;
    {
        holes.reserve(expolygon.holes.size());
        for (Polygons::const_iterator it_hole = expolygon.holes.begin(); it_hole != expolygon.holes.end(); ++ it_hole) {
            ClipperLib::Path input = Slic3rMultiPoint_to_ClipperPath_reversed(*it_hole);
            scaleClipperPolygon(input);
            ClipperLib::ClipperOffset co;
            if (joinType == jtRound)
                co.ArcTolerance = miterLimit * double(CLIPPER_OFFSET_SCALE);
            else
                co.MiterLimit = miterLimit;
            co.ShortestEdgeLength = double(std::abs(delta_scaled * CLIPPER_OFFSET_SHORTEST_EDGE_FACTOR));
            co.AddPath(input, joinType, ClipperLib::etClosedPolygon);
            ClipperLib::Paths out;
            co.Execute(out, - delta_scaled);
            holes.insert(holes.end(), out.begin(), out.end());
        }
    }

    // 3) Subtract holes from the contours.
    ClipperLib::Paths output;
    if (holes.empty()) {
        output = std::move(contours);
    } else {
        ClipperLib::Clipper clipper;
        clipper.Clear();
        clipper.AddPaths(contours, ClipperLib::ptSubject, true);
        clipper.AddPaths(holes, ClipperLib::ptClip, true);
        clipper.Execute(ClipperLib::ctDifference, output, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
    }
    
    // 4) Unscale the output.
    unscaleClipperPolygons(output);
    return output;
}

bool BooleanTool::executeForObjects(PathObject* subject, PathObjects& in_objects, PathObjects& out_objects)
{
	// Convert the objects to Clipper polygons and
	// create a hash map, mapping point positions to the PathCoords.
	// These paths are to be regarded as closed.
	PolyMap polymap;
	
	ClipperLib::Paths subject_polygons;
	pathObjectToPolygons(subject, subject_polygons, polymap);
	
	ClipperLib::Paths clip_polygons;
	for (PathObject* object : in_objects)
	{
		if (object != subject)
		{
			pathObjectToPolygons(object, clip_polygons, polymap);
		}
	}
	
	// Do the operation.
	ClipperLib::Clipper clipper;
	clipper.AddPaths(subject_polygons, ClipperLib::ptSubject, true);
	clipper.AddPaths(clip_polygons, ClipperLib::ptClip, true);
	
	ClipperLib::ClipType clip_type;
	ClipperLib::PolyFillType fill_type = ClipperLib::pftNonZero;
	switch (op)
	{
	case Union:         clip_type = ClipperLib::ctUnion;
	                    break;
	case Intersection:  clip_type = ClipperLib::ctIntersection;
	                    break;
	case Difference:    clip_type = ClipperLib::ctDifference;
	                    break;
	case XOr:           clip_type = ClipperLib::ctXor;
	                    break;
	case MergeHoles:    clip_type = ClipperLib::ctUnion;
	                    fill_type = ClipperLib::pftPositive;
	                    break;
	default:            Q_ASSERT(false && "Undefined operation");
	                    return false;
	}

	ClipperLib::PolyTree solution;
	bool success = clipper.Execute(clip_type, solution, fill_type, fill_type);
	if (success)
	{
		// Try to convert the solution polygons to objects again
		polyTreeToPathObjects(solution, out_objects, subject, polymap);
	}
	
	return success;
}

void simplify_polygons(const Slic3r::Polygons &subject, Slic3r::ExPolygons* retval, bool preserve_collinear)
{
    PROFILE_FUNC();

    if (!preserve_collinear) {
        Polygons polygons;
        simplify_polygons(subject, &polygons, preserve_collinear);
        union_(polygons, retval);
        return;
    }
    
    // convert into Clipper polygons
    ClipperLib::Paths input_subject;
    Slic3rMultiPoints_to_ClipperPaths(subject, &input_subject);
    
    ClipperLib::PolyTree polytree;
    
    ClipperLib::Clipper c;
    c.PreserveCollinear(true);
    c.StrictlySimple(true);
    c.AddPaths(input_subject, ClipperLib::ptSubject, true);
    c.Execute(ClipperLib::ctUnion, polytree, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
    
    // convert into ExPolygons
    PolyTreeToExPolygons(polytree, retval);
}

ExPolygons
ClipperPaths_to_Slic3rExPolygons(const ClipperLib::Paths &input)
{
    // init Clipper
    ClipperLib::Clipper clipper;
    clipper.Clear();
    
    // perform union
    clipper.AddPaths(input, ClipperLib::ptSubject, true);
    ClipperLib::PolyTree polytree;
    clipper.Execute(ClipperLib::ctUnion, polytree, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);  // offset results work with both EvenOdd and NonZero
    
    // write to ExPolygons object
    return PolyTreeToExPolygons(polytree);
}

Polygons top_level_islands(const Slic3r::Polygons &polygons)
{
    // init Clipper
    ClipperLib::Clipper clipper;
    clipper.Clear();
    // perform union
    clipper.AddPaths(Slic3rMultiPoints_to_ClipperPaths(polygons), ClipperLib::ptSubject, true);
    ClipperLib::PolyTree polytree;
    clipper.Execute(ClipperLib::ctUnion, polytree, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd); 
    // Convert only the top level islands to the output.
    Polygons out;
    out.reserve(polytree.ChildCount());
    for (int i = 0; i < polytree.ChildCount(); ++i)
        out.emplace_back(ClipperPath_to_Slic3rPolygon(polytree.Childs[i]->Contour));
    return out;
}

void
ClipperPaths_to_Slic3rExPolygons(const ClipperLib::Paths &input, Slic3r::ExPolygons &output)
{
    // init Clipper
    ClipperLib::Clipper clipper;
    clipper.Clear();
    
    // perform union
    clipper.AddPaths(input, ClipperLib::ptSubject, true);
    ClipperLib::PolyTree* polytree = new ClipperLib::PolyTree();
    clipper.Execute(ClipperLib::ctUnion, *polytree, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);  // offset results work with both EvenOdd and NonZero
    
    // write to ExPolygons object
    output.clear();
    PolyTreeToExPolygons(*polytree, output);
    
    delete polytree;
}

void simplify_polygons(const Slic3r::Polygons &subject, Slic3r::Polygons* retval, bool preserve_collinear)
{
    // convert into Clipper polygons
    ClipperLib::Paths input_subject, output;
    Slic3rMultiPoints_to_ClipperPaths(subject, &input_subject);
    
    if (preserve_collinear) {
        ClipperLib::Clipper c;
        c.PreserveCollinear(true);
        c.StrictlySimple(true);
        c.AddPaths(input_subject, ClipperLib::ptSubject, true);
        c.Execute(ClipperLib::ctUnion, output, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
    } else {
        ClipperLib::SimplifyPolygons(input_subject, output, ClipperLib::pftNonZero);
    }
    
    // convert into Slic3r polygons
    ClipperPaths_to_Slic3rMultiPoints(output, retval);
}

ExPolygons simplify_polygons_ex(const Polygons &subject, bool preserve_collinear)
{
    if (! preserve_collinear)
        return union_ex(simplify_polygons(subject, false));

    // convert into Clipper polygons
    ClipperLib::Paths input_subject = Slic3rMultiPoints_to_ClipperPaths(subject);
    
    ClipperLib::PolyTree polytree;
    
    ClipperLib::Clipper c;
    c.PreserveCollinear(true);
    c.StrictlySimple(true);
    c.AddPaths(input_subject, ClipperLib::ptSubject, true);
    c.Execute(ClipperLib::ctUnion, polytree, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
    
    // convert into ExPolygons
    return PolyTreeToExPolygons(polytree);
}

 double poly_intersection(const ContContainer& poly1, const ContContainer& poly2)
 {
     /* ************* TEMPORAL ************
      * Conversion, we should remove junctions from container
      * or define it for our containers */
     ClipperLib::Paths paths1(poly1.begin(),poly1.end());
     ClipperLib::Paths paths2(poly2.begin(),poly2.end());
     
     /* Get the intersection polygon */
     ClipperLib::Clipper clpr;
     clpr.AddPaths(paths1, ClipperLib::ptSubject, true);
     clpr.AddPaths(paths2, ClipperLib::ptClip   , true);
     ClipperLib::Paths solution;
     clpr.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
     
     /* Get its area */
     double int_area = 0;
     for(std::size_t ii=0; ii<solution.size(); ++ii)
         int_area += std::abs(ClipperLib::Area(solution[ii]));
     
     return int_area;
 }

void World::update(cv::Mat &homography)
{
    this->m_world->Step(dt, 10, 10);

    //check contacts
    std::vector<MyContact>::iterator pos;
    std::map<b2Body*, ClipperLib::Paths*> newBodyMap;
    std::vector<b2Body*> removeBarrierList;

    for(pos = this->m_contactListener->m_contacts.begin();
        pos != this->m_contactListener->m_contacts.end();
        ++pos)
    {
        MyContact contact = *pos;

        if ((contact.fixtureA == this->m_ballFixture || contact.fixtureB == this->m_ballFixture) 
            && (contact.fixtureA->GetBody() != m_groundBody && contact.fixtureB->GetBody() != m_groundBody)
            && (contact.fixtureA->GetBody() != m_paddlesBody && contact.fixtureB->GetBody() != m_paddlesBody))
        {
            b2Fixture* objectFixture = contact.fixtureA == this->m_ballFixture ? contact.fixtureB : contact.fixtureA;
            b2Body *objectBody = objectFixture->GetBody();

            if (objectFixture->GetType() == b2Shape::e_edge)
            {
                cv::Point2f hitPoint = CVUtils::transformPoint(cv::Point2f(contact.contactPoint->x * PTM_RATIO, contact.contactPoint->y * PTM_RATIO), homography);
                this->notifyBallHitObservers(hitPoint.x, hitPoint.y);

                // change the shape of the fixture
                // only go into processing if this body was not processed yet (possible ball hit two fixture of same body)
                if (newBodyMap.find(objectBody) == newBodyMap.end())
                {
                    ClipperLib::Paths* bodyPolygons = (ClipperLib::Paths*)objectBody->GetUserData();

                    b2Vec2* impactVelocity = contact.fixtureA == m_ballFixture ? contact.impactVelocityA : contact.impactVelocityB;
                    float ballAngle = atan2(impactVelocity->y, impactVelocity->x); // get the angle (in radians) the ball is moving to
                    float ballPower = impactVelocity->Length() * 0.5;    // get the "power" of the ball movement vector
                    float openingStepInRadians = 10 * CV_PI / 180;  // calculate the opening in radians

                    // create the clipping object/shape - a wedge from ball's center with 30 degree opening over ball direction (angle)
                    ClipperLib::Path clip;
                    clip.push_back(ClipperLib::IntPoint(contact.contactPoint->x * PTM_RATIO, contact.contactPoint->y * PTM_RATIO));

                    for(int step = 9; step > -10; step--)
                    {
                        float dx = cos(ballAngle + step * openingStepInRadians) * ballPower;
                        float dy = sin(ballAngle + step * openingStepInRadians) * ballPower;

                        clip.push_back(ClipperLib::IntPoint(contact.contactPoint->x * PTM_RATIO + dx, contact.contactPoint->y * PTM_RATIO + dy));
                    }

                    ClipperLib::Clipper clipper;
                    clipper.AddPaths((*bodyPolygons), ClipperLib::ptSubject, true);
                    clipper.AddPath(clip, ClipperLib::ptClip, true);

                    // the difference is the new polygon formed by the clipping (collision)
                    ClipperLib::Paths* newPolygons = new ClipperLib::Paths();
                    clipper.Execute(ClipperLib::ctDifference, (*newPolygons), ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);

                    // Save the new polygons of this body
                    objectBody->SetUserData(newPolygons);
                    newBodyMap[objectBody] = newPolygons;

                    // now, find the intersection regions - these should be inpainted to the scene
                    ClipperLib::Paths destroyedParts;
                    clipper.Execute(ClipperLib::ctIntersection, destroyedParts, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);

                    // paint the required areas to be coppied
                    for (size_t i = 0; i < destroyedParts.size(); i++)
                    {
                        cv::Point* points = new cv::Point[destroyedParts[i].size()];

                        for (size_t j = 0; j < destroyedParts[i].size(); j++)
                        {
                            points[j].x = (int)destroyedParts[i][j].X;
                            points[j].y = (int)destroyedParts[i][j].Y;
                        }

                        m_destroyedPolygons.push_back(points);
                        m_destroyedPolygonsPointCount.push_back((int)destroyedParts[i].size());
                    }
                }
            }
            else if (objectFixture->GetType() == b2Shape::e_circle)
            {
                // this is a barrier - add it to the remove list
                removeBarrierList.push_back(objectBody);
            }
        }
    }

    std::map<b2Body*, ClipperLib::Paths*>::iterator iter;

    for(iter = newBodyMap.begin(); iter != newBodyMap.end(); iter++)
    {
        b2Body* objectBody = iter->first;
        ClipperLib::Paths* newPolygons = iter->second;

        // remove all the current fixtures from this body
        for (b2Fixture* f = objectBody->GetFixtureList(); f; )
        {
//.........这里部分代码省略.........

int Grasp_Calculator::calcMeshGraspPosition(moveit_msgs::CollisionObject co, std::vector<geometry_msgs::PoseStamped> &poses,
        std::vector<geometry_msgs::PoseStamped> &pre_poses,
        double gripper_depth)
{
    ros::Time t = ros::Time::now();
    string gripper_group = gripper_depth == Gripper::R_GRIPPER_PALM_LENGTH ?
                           Gripper::get_r_group_name() : Gripper::get_l_group_name();


    std::vector<geometry_msgs::Point> c;
    suturo_manipulation::Mesh meshi(co);

    // 2. search for cluster with opposite normal - O(|Cluster|²)
    std::vector< std::pair<uint, uint> > opposite_cluster = meshi.get_opposite_cluster();
    int h = 0;
    ROS_INFO_STREAM("opposite cluster size: " << opposite_cluster.size());
    for (int i = 0; i < opposite_cluster.size(); i++)
    {
        // 3. project both cluster on a plain - o(|vertices|)?
        suturo_manipulation::Plane plane = meshi.get_plane(opposite_cluster[i].first, opposite_cluster[i].second);
        plane.orthonormalize();

        std::vector<geometry_msgs::Point> p1 = meshi.get_polygon(opposite_cluster[i].first);
        std::vector<geometry_msgs::Point> p2 = meshi.get_polygon(opposite_cluster[i].second);
        if (i == 0)
        {
            pi_->publishMarkerLine(co.id, p1, 0);
            pi_->publishMarkerLine(co.id, p2, 1000);

        }

        DPolygon2D dpolygon1 = project_polygon_to_plane(plane, p1);
        DPolygon2D dpolygon2 = project_polygon_to_plane(plane, p2);

        ClipperLib::Paths polygon1(1);
        ClipperLib::Paths polygon2(1);
        double_polygon_to_path(dpolygon1, polygon1);
        double_polygon_to_path(dpolygon2, polygon2);


        // 4. calc polygon intersection - O(|vertices|²)?
        //     http://www.integis.ch/documents/ISem_Opprecht_Overlay_2002-02-28.pdf
        //     http://www.cs.man.ac.uk/~toby/alan/software/gpc.html
        ClipperLib::Clipper clpr;
        clpr.AddPaths(polygon1, ClipperLib::ptSubject, true);
        clpr.AddPaths(polygon2, ClipperLib::ptClip, true);
        ClipperLib::Paths solution;
        clpr.Execute(ClipperLib::ctIntersection, solution, ClipperLib::pftNonZero, ClipperLib::pftNonZero);


        // 5. calc centriod - O(|vertices|)
        Path int_centroids = calc_poly_centroid(solution);

        DPolygon2D double_centroids;
        path_to_double_polygon(double_centroids, int_centroids);
        std::vector<geometry_msgs::Point> centroids = d2d_points_to_d3d_points(plane, double_centroids);
        for (std::vector<geometry_msgs::Point>::iterator i = centroids.begin(); i != centroids.end(); ++i)
        {
            c.push_back(*i);
        }

        //ggf 5.2 check dist to cluster
        // http://www.uninformativ.de/bin/RaytracingSchnitttests-76a577a-CC-BY.pdf


        // 6. add grasppose pointing towards centriod with differen angles - const

        for (std::vector<geometry_msgs::Point>::iterator i = centroids.begin(); i != centroids.end(); ++i)
        {
            for (double a = 0; a < 2 * M_PI; a += (M_PI / 2))
            {
                geometry_msgs::PoseStamped temp_grasp_pose;
                geometry_msgs::PoseStamped temp_pre_grasp_pose;
                temp_grasp_pose.header.frame_id = co.id;
                temp_pre_grasp_pose.header.frame_id = co.id;

                if (!get_grasp_point(plane, *i, gripper_depth , a, temp_grasp_pose, temp_pre_grasp_pose, meshi))
                    continue;

                // h++;
                // 7. use moveit to test poses - O(much)???
                // if (!pi_->check_group_object_collision(gripper_group, temp_grasp_pose, co))
                // {
                // ROS_INFO_STREAM("collision!" );
                // ros::WallDuration(0.5).sleep();
                // ROS_WARN_STREAM("centroid " << *i);
                poses.push_back(temp_grasp_pose);
                pre_poses.push_back(temp_pre_grasp_pose);


                // }
            }
        }
    }
    pi_->publishMarkerPoints(co.id, c);
    if (poses.empty())
    {
        ROS_WARN_STREAM("No Grasppositions found for: " << co.id);
        return 0;
    }
//.........这里部分代码省略.........