Esempio n. 1
0
bool Comb::calc(Point startPoint, Point endPoint, CombPaths& combPaths, bool _startInside, bool _endInside, int64_t max_comb_distance_ignored, bool via_outside_makes_combing_fail, bool fail_on_unavoidable_obstacles)
{
    if (shorterThen(endPoint - startPoint, max_comb_distance_ignored))
    {
        return true;
    }

    //Move start and end point inside the comb boundary
    unsigned int start_inside_poly = NO_INDEX;
    const bool startInside = moveInside(_startInside, startPoint, start_inside_poly);

    unsigned int end_inside_poly = NO_INDEX;
    const bool endInside = moveInside(_endInside, endPoint, end_inside_poly);

    unsigned int start_part_boundary_poly_idx;
    unsigned int end_part_boundary_poly_idx;
    unsigned int start_part_idx =   (start_inside_poly == NO_INDEX)?    NO_INDEX : partsView_inside.getPartContaining(start_inside_poly, &start_part_boundary_poly_idx);
    unsigned int end_part_idx =     (end_inside_poly == NO_INDEX)?      NO_INDEX : partsView_inside.getPartContaining(end_inside_poly, &end_part_boundary_poly_idx);
    
    if (startInside && endInside && start_part_idx == end_part_idx)
    { // normal combing within part
        PolygonsPart part = partsView_inside.assemblePart(start_part_idx);
        combPaths.emplace_back();
        return LinePolygonsCrossings::comb(part, *inside_loc_to_line, startPoint, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored, fail_on_unavoidable_obstacles);
    }
    else 
    { // comb inside part to edge (if needed) >> move through air avoiding other parts >> comb inside end part upto the endpoint (if needed) 
        //  INSIDE  |          in_between            |            OUTSIDE     |              in_between         |     INSIDE
        //        ^crossing_1_in     ^crossing_1_mid  ^crossing_1_out        ^crossing_2_out    ^crossing_2_mid   ^crossing_2_in
        //
        // when startPoint is inside crossing_1_in is of interest
        // when it is in between inside and outside it is equal to crossing_1_mid

        if (via_outside_makes_combing_fail)
        {
            return false;
        }

        Crossing start_crossing(startPoint, startInside, start_part_idx, start_part_boundary_poly_idx, boundary_inside, inside_loc_to_line);
        Crossing end_crossing(endPoint, endInside, end_part_idx, end_part_boundary_poly_idx, boundary_inside, inside_loc_to_line);

        { // find crossing over the in-between area between inside and outside
            start_crossing.findCrossingInOrMid(partsView_inside, endPoint);
            end_crossing.findCrossingInOrMid(partsView_inside, start_crossing.in_or_mid);
        }

        bool skip_avoid_other_parts_path = false;
        if (skip_avoid_other_parts_path && vSize2(start_crossing.in_or_mid - end_crossing.in_or_mid) < offset_from_inside_to_outside * offset_from_inside_to_outside * 4)
        { // parts are next to eachother, i.e. the direct crossing will always be smaller than two crossings via outside
            skip_avoid_other_parts_path = true;
        }

        if (avoid_other_parts && !skip_avoid_other_parts_path)
        { // compute the crossing points when moving through air
            // comb through all air, since generally the outside consists of a single part

            bool success = start_crossing.findOutside(*boundary_outside, end_crossing.in_or_mid, fail_on_unavoidable_obstacles, *this);
            if (!success)
            {
                return false;
            }

            success = end_crossing.findOutside(*boundary_outside, start_crossing.out, fail_on_unavoidable_obstacles, *this);
            if (!success)
            {
                return false;
            }
        }

        // generate the actual comb paths
        if (startInside)
        {
            // start to boundary
            assert(start_crossing.dest_part.size() > 0 && "The part we start inside when combing should have been computed already!");
            combPaths.emplace_back();
            bool combing_succeeded = LinePolygonsCrossings::comb(start_crossing.dest_part, *inside_loc_to_line, startPoint, start_crossing.in_or_mid, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored, fail_on_unavoidable_obstacles);
            if (!combing_succeeded)
            { // Couldn't comb between start point and computed crossing from the start part! Happens for very thin parts when the offset_to_get_off_boundary moves points to outside the polygon
                return false;
            }
        }
        
        // throught air from boundary to boundary
        if (avoid_other_parts && !skip_avoid_other_parts_path)
        {
            combPaths.emplace_back();
            combPaths.throughAir = true;
            if ( vSize(start_crossing.in_or_mid - end_crossing.in_or_mid) < vSize(start_crossing.in_or_mid - start_crossing.out) + vSize(end_crossing.in_or_mid - end_crossing.out) )
            { // via outside is moving more over the in-between zone
                combPaths.back().push_back(start_crossing.in_or_mid);
                combPaths.back().push_back(end_crossing.in_or_mid);
            }
            else
            {
                bool combing_succeeded = LinePolygonsCrossings::comb(*boundary_outside, *outside_loc_to_line, start_crossing.out, end_crossing.out, combPaths.back(), offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored, fail_on_unavoidable_obstacles);
                if (!combing_succeeded)
                {
                    return false;
                }
            }
        }
        else 
        { // directly through air (not avoiding other parts)
            combPaths.emplace_back();
            combPaths.throughAir = true;
            combPaths.back().cross_boundary = true; // note: we don't actually know whether this is cross boundary, but it might very well be
            combPaths.back().push_back(start_crossing.in_or_mid);
            combPaths.back().push_back(end_crossing.in_or_mid);
        }
        if (skip_avoid_other_parts_path)
        {
            if (startInside == endInside && start_part_idx == end_part_idx)
            {
                if (startInside)
                { // both start and end are inside
                    combPaths.back().cross_boundary = PolygonUtils::polygonCollidesWithLineSegment(startPoint, endPoint, *inside_loc_to_line);
                }
                else
                { // both start and end are outside
                    combPaths.back().cross_boundary = PolygonUtils::polygonCollidesWithLineSegment(startPoint, endPoint, *outside_loc_to_line);
                }
            }
            else
            {
                combPaths.back().cross_boundary = true;
            }
        }
        
        if (endInside)
        {
            // boundary to end
            assert(end_crossing.dest_part.size() > 0 && "The part we end up inside when combing should have been computed already!");
            combPaths.emplace_back();
            
            bool combing_succeeded = LinePolygonsCrossings::comb(end_crossing.dest_part, *inside_loc_to_line, end_crossing.in_or_mid, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored, fail_on_unavoidable_obstacles);
            if (!combing_succeeded)
            { // Couldn't comb between end point and computed crossing to the end part! Happens for very thin parts when the offset_to_get_off_boundary moves points to outside the polygon
                return false;
            }
        }
        
        return true;
    }
}
Esempio n. 2
0
bool Comb::calc(Point startPoint, Point endPoint, CombPaths& combPaths, bool startInside, bool endInside, int64_t max_comb_distance_ignored)
{
    if (shorterThen(endPoint - startPoint, max_comb_distance_ignored))
    {
        return true;
    }

    //Move start and end point inside the comb boundary
    unsigned int start_inside_poly = NO_INDEX;
    if (startInside) 
    {
        start_inside_poly = PolygonUtils::moveInside(boundary_inside, startPoint, offset_extra_start_end, max_moveInside_distance2);
        if (!boundary_inside.inside(start_inside_poly) || start_inside_poly == NO_INDEX)
        {
            if (start_inside_poly != NO_INDEX)
            { // if not yet inside because of overshoot, try again
                start_inside_poly = PolygonUtils::moveInside(boundary_inside, startPoint, offset_extra_start_end, max_moveInside_distance2);
            }
            if (start_inside_poly == NO_INDEX)    //If we fail to move the point inside the comb boundary we need to retract.
            {
                startInside = false;
            }
        }
    }
    unsigned int end_inside_poly = NO_INDEX;
    if (endInside)
    {
        end_inside_poly = PolygonUtils::moveInside(boundary_inside, endPoint, offset_extra_start_end, max_moveInside_distance2);
        if (!boundary_inside.inside(endPoint) || end_inside_poly == NO_INDEX)
        {
            if (end_inside_poly != NO_INDEX)
            { // if not yet inside because of overshoot, try again
                end_inside_poly = PolygonUtils::moveInside(boundary_inside, endPoint, offset_extra_start_end, max_moveInside_distance2);
            }
            if (end_inside_poly == NO_INDEX)    //If we fail to move the point inside the comb boundary we need to retract.
            {
                endInside = false;
            }
        }
    }

    
    unsigned int start_part_boundary_poly_idx;
    unsigned int end_part_boundary_poly_idx;
    unsigned int start_part_idx =   (start_inside_poly == NO_INDEX)?    NO_INDEX : partsView_inside.getPartContaining(start_inside_poly, &start_part_boundary_poly_idx);
    unsigned int end_part_idx =     (end_inside_poly == NO_INDEX)?      NO_INDEX : partsView_inside.getPartContaining(end_inside_poly, &end_part_boundary_poly_idx);
    
    if (startInside && endInside && start_part_idx == end_part_idx)
    { // normal combing within part
        PolygonsPart part = partsView_inside.assemblePart(start_part_idx);
        combPaths.emplace_back();
        LinePolygonsCrossings::comb(part, startPoint, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored);
        return true;
    }
    else 
    { // comb inside part to edge (if needed) >> move through air avoiding other parts >> comb inside end part upto the endpoint (if needed) 
        //  INSIDE  |          in_between            |            OUTSIDE     |              in_between         |     INSIDE
        //        ^crossing_1_in     ^crossing_1_mid  ^crossing_1_out        ^crossing_2_out    ^crossing_2_mid   ^crossing_2_in
        //
        // when startPoint is inside crossing_1_in is of interest
        // when it is in between inside and outside it is equal to crossing_1_mid
        Point crossing_1_in_or_mid; // the point inside the starting polygon if startPoint is inside or the startPoint itself if it is not inside
        Point crossing_1_out;
        Point crossing_2_in_or_mid; // the point inside the ending polygon if endPoint is inside or the endPoint itself if it is not inside
        Point crossing_2_out;
        
        { // find crossing over the in-between area between inside and outside
            if (startInside)
            {
                ClosestPolygonPoint crossing_1_in_cp = PolygonUtils::findClosest(endPoint, boundary_inside[start_part_boundary_poly_idx]);
                crossing_1_in_or_mid = PolygonUtils::moveInside(crossing_1_in_cp, offset_dist_to_get_from_on_the_polygon_to_outside); // in-case
            }
            else 
            {
                crossing_1_in_or_mid = startPoint; // mid-case
            }

            if (endInside)
            {
                ClosestPolygonPoint crossing_2_in_cp = PolygonUtils::findClosest(crossing_1_in_or_mid, boundary_inside[end_part_boundary_poly_idx]);
                crossing_2_in_or_mid = PolygonUtils::moveInside(crossing_2_in_cp, offset_dist_to_get_from_on_the_polygon_to_outside); // in-case
            }
            else 
            {
                crossing_2_in_or_mid = endPoint; // mid-case
            }
        }
        
        bool avoid_other_parts_now = avoid_other_parts;
        if (avoid_other_parts_now && vSize2(crossing_1_in_or_mid - crossing_2_in_or_mid) < offset_from_outlines_outside * offset_from_outlines_outside * 4)
        { // parts are next to eachother, i.e. the direct crossing will always be smaller than two crossings via outside
            avoid_other_parts_now = false;
        }
        
        if (avoid_other_parts_now)
        { // compute the crossing points when moving through air
            Polygons& outside = getBoundaryOutside(); // comb through all air, since generally the outside consists of a single part
            
            
            crossing_1_out = crossing_1_in_or_mid;
            if (startInside || outside.inside(crossing_1_in_or_mid, true)) // start in_between
            { // move outside
                ClosestPolygonPoint* crossing_1_out_cpp = PolygonUtils::findClose(crossing_1_in_or_mid, outside, getOutsideLocToLine());
                if (crossing_1_out_cpp)
                {
                    crossing_1_out = PolygonUtils::moveOutside(*crossing_1_out_cpp, offset_dist_to_get_from_on_the_polygon_to_outside);
                }
                else 
                {
                    PolygonUtils::moveOutside(outside, crossing_1_out, offset_dist_to_get_from_on_the_polygon_to_outside);
                }
            }
            int64_t in_out_dist2_1 = vSize2(crossing_1_out - crossing_1_in_or_mid); 
            if (startInside && in_out_dist2_1 > max_crossing_dist2) // moveInside moved too far
            { // if move is to far over in_between
                // find crossing closer by
                std::shared_ptr<std::pair<ClosestPolygonPoint, ClosestPolygonPoint>> best = findBestCrossing(boundary_inside[start_part_boundary_poly_idx], startPoint, endPoint);
                if (best)
                {
                    crossing_1_in_or_mid = PolygonUtils::moveInside(best->first, offset_dist_to_get_from_on_the_polygon_to_outside);
                    crossing_1_out = PolygonUtils::moveOutside(best->second, offset_dist_to_get_from_on_the_polygon_to_outside);
                }
            }


            crossing_2_out = crossing_2_in_or_mid;
            if (endInside || outside.inside(crossing_2_in_or_mid, true))
            { // move outside
                ClosestPolygonPoint* crossing_2_out_cpp = PolygonUtils::findClose(crossing_2_in_or_mid, outside, getOutsideLocToLine());
                if (crossing_2_out_cpp)
                {
                    crossing_2_out = PolygonUtils::moveOutside(*crossing_2_out_cpp, offset_dist_to_get_from_on_the_polygon_to_outside);
                }
                else 
                {
                    PolygonUtils::moveOutside(outside, crossing_2_out, offset_dist_to_get_from_on_the_polygon_to_outside);
                }
            }
            int64_t in_out_dist2_2 = vSize2(crossing_2_out - crossing_2_in_or_mid); 
            if (endInside && in_out_dist2_2 > max_crossing_dist2) // moveInside moved too far
            { // if move is to far over in_between
                // find crossing closer by
                std::shared_ptr<std::pair<ClosestPolygonPoint, ClosestPolygonPoint>> best = findBestCrossing(boundary_inside[end_part_boundary_poly_idx], endPoint, crossing_1_out);
                if (best)
                {
                    crossing_2_in_or_mid = PolygonUtils::moveInside(best->first, offset_dist_to_get_from_on_the_polygon_to_outside);
                    crossing_2_out = PolygonUtils::moveOutside(best->second, offset_dist_to_get_from_on_the_polygon_to_outside);
                }
            }
        }

        if (startInside)
        {
            // start to boundary
            PolygonsPart part_begin = partsView_inside.assemblePart(start_part_idx); // comb through the starting part only
            combPaths.emplace_back();
            LinePolygonsCrossings::comb(part_begin, startPoint, crossing_1_in_or_mid, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored);
        }
        
        // throught air from boundary to boundary
        if (avoid_other_parts_now)
        {
            combPaths.emplace_back();
            combPaths.throughAir = true;
            if ( vSize(crossing_1_in_or_mid - crossing_2_in_or_mid) < vSize(crossing_1_in_or_mid - crossing_1_out) + vSize(crossing_2_in_or_mid - crossing_2_out) )
            { // via outside is moving more over the in-between zone
                combPaths.back().push_back(crossing_1_in_or_mid);
                combPaths.back().push_back(crossing_2_in_or_mid);
            }
            else
            {
                LinePolygonsCrossings::comb(getBoundaryOutside(), crossing_1_out, crossing_2_out, combPaths.back(), offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored);
            }
        }
        else 
        { // directly through air (not avoiding other parts)
            combPaths.emplace_back();
            combPaths.throughAir = true;
            combPaths.back().cross_boundary = true; // TODO: calculate whether we cross a boundary!
            combPaths.back().push_back(crossing_1_in_or_mid);
            combPaths.back().push_back(crossing_2_in_or_mid);
        }
        
        if (endInside)
        {
            // boundary to end
            PolygonsPart part_end = partsView_inside.assemblePart(end_part_idx); // comb through end part only
            combPaths.emplace_back();
            LinePolygonsCrossings::comb(part_end, crossing_2_in_or_mid, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside, max_comb_distance_ignored);
        }
        
        return true;
    }
}
Esempio n. 3
0
bool Comb::calc(Point startPoint, Point endPoint, CombPaths& combPaths)
{
    if (shorterThen(endPoint - startPoint, max_comb_distance_ignored))
    {
        return true;
    }
    
    
    bool startInside = true;
    bool endInside = true;
    
    //Move start and end point inside the comb boundary
    unsigned int start_inside_poly = boundary_inside.findInside(startPoint, true);
    if (start_inside_poly == NO_INDEX)
    {
        start_inside_poly = moveInside(boundary_inside, startPoint, offset_extra_start_end, max_moveInside_distance2);
        if (start_inside_poly == NO_INDEX)    //If we fail to move the point inside the comb boundary we need to retract.
        {   
            startInside = false;
        }
    }
    unsigned int end_inside_poly = boundary_inside.findInside(endPoint, true);
    if (end_inside_poly == NO_INDEX)
    {
        end_inside_poly = moveInside(boundary_inside, endPoint, offset_extra_start_end, max_moveInside_distance2);
        if (end_inside_poly == NO_INDEX)    //If we fail to move the point inside the comb boundary we need to retract.
        {
            endInside = false;
        }
    }
    

    
    unsigned int start_part_boundary_poly_idx;
    unsigned int end_part_boundary_poly_idx;
    unsigned int start_part_idx = partsView_inside.getPartContaining(start_inside_poly, &start_part_boundary_poly_idx);
    unsigned int end_part_idx = partsView_inside.getPartContaining(end_inside_poly, &end_part_boundary_poly_idx);
    
    if (startInside && endInside && start_part_idx == end_part_idx)
    { // normal combing within part
        PolygonsPart part = partsView_inside.assemblePart(start_part_idx);
        combPaths.emplace_back();
        LinePolygonsCrossings::comb(part, startPoint, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside);
        return true;
    }
    else 
    { // comb inside part to edge (if needed) >> move through air avoiding other parts >> comb inside end part upto the endpoint (if needed) 
        Point middle_from;
        Point middle_to;
        
        if (startInside && endInside)
        {
            ClosestPolygonPoint middle_from_cp = findClosest(endPoint, boundary_inside[start_part_boundary_poly_idx]);
            ClosestPolygonPoint middle_to_cp = findClosest(middle_from_cp.location, boundary_inside[end_part_boundary_poly_idx]);
//             walkToNearestSmallestConnection(middle_from_cp, middle_to_cp); // TODO: perform this optimization?
            middle_from = middle_from_cp.location;
            middle_to = middle_to_cp.location;
        }
        else 
        {
            if (!startInside && !endInside) 
            { 
                middle_from = startPoint; 
                middle_to = endPoint; 
            }
            else if (!startInside && endInside)
            {
                middle_from = startPoint;
                ClosestPolygonPoint middle_to_cp = findClosest(middle_from, boundary_inside[end_part_boundary_poly_idx]);
                middle_to = middle_to_cp.location;
            }
            else if (startInside && !endInside)
            {
                middle_to = endPoint;
                ClosestPolygonPoint middle_from_cp = findClosest(middle_to, boundary_inside[start_part_boundary_poly_idx]);
                middle_from = middle_from_cp.location;
            }
        }
        
        if (startInside)
        {
            // start to boundary
            PolygonsPart part_begin = partsView_inside.assemblePart(start_part_idx); // comb through the starting part only
            combPaths.emplace_back();
            LinePolygonsCrossings::comb(part_begin, startPoint, middle_from, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside);
        }
        
        // throught air from boundary to boundary
        if (avoid_other_parts)
        {
            Polygons& middle = *getBoundaryOutside(); // comb through all air, since generally the outside consists of a single part
            Point from_outside = middle_from;
            if (startInside || middle.inside(from_outside, true))
            { // move outside
                moveInside(middle, from_outside, -offset_extra_start_end, max_moveOutside_distance2);
            }
            Point to_outside = middle_to;
            if (endInside || middle.inside(to_outside, true))
            { // move outside
                moveInside(middle, to_outside, -offset_extra_start_end, max_moveOutside_distance2);
            }
            combPaths.emplace_back();
            combPaths.back().throughAir = true;
            if ( vSize(middle_from - middle_to) < vSize(middle_from - from_outside) + vSize(middle_to - to_outside) )
            { // via outside is a detour
                combPaths.back().push_back(middle_from);
                combPaths.back().push_back(middle_to);
            }
            else
            {
                LinePolygonsCrossings::comb(middle, from_outside, to_outside, combPaths.back(), offset_dist_to_get_from_on_the_polygon_to_outside);
            }
        }
        else 
        { // directly through air (not avoiding other parts)
            combPaths.emplace_back();
            combPaths.back().throughAir = true;
            combPaths.back().cross_boundary = true; // TODO: calculate whether we cross a boundary!
            combPaths.back().push_back(middle_from);
            combPaths.back().push_back(middle_to);
        }
        
        if (endInside)
        {
            // boundary to end
            PolygonsPart part_end = partsView_inside.assemblePart(end_part_idx); // comb through end part only
            combPaths.emplace_back();
            LinePolygonsCrossings::comb(part_end, middle_to, endPoint, combPaths.back(), -offset_dist_to_get_from_on_the_polygon_to_outside);
        }
        
        return true;
    }
}