<commit_before>/** Copyright (C) 2013 <NAME> - Released under terms of the AGPLv3 License */
#include "pathOrderOptimizer.h"
#include "utils/logoutput.h"
#include "utils/SparsePointGridInclusive.h"
#include "utils/linearAlg2D.h"

#define INLINE static inline

namespace cura {

/**
*
*/
void PathOrderOptimizer::optimize()
{
    bool picked[polygons.size()];
    memset(picked, false, sizeof(bool) * polygons.size());/// initialized as falses
    
    for (unsigned poly_idx = 0; poly_idx < polygons.size(); ++poly_idx) /// find closest point to initial starting point within each polygon +initialize picked
    {
        const ConstPolygonRef poly = *polygons[poly_idx];
        switch (type)
        {
            case EZSeamType::USER_SPECIFIED:
                polyStart.push_back(getClosestPointInPolygon(z_seam_pos, poly_idx));
                break;
            case EZSeamType::RANDOM:
                polyStart.push_back(getRandomPointInPolygon(poly_idx));
                break;
            default:
            {
                int best_points_idx = -1;
                float bestDist = std::numeric_limits<float>::infinity();
                for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++) /// get closest point in polygon
                {
                    float dist = vSize2f(poly[point_idx] - startPoint);
                    if (dist < bestDist)
                    {
                        best_points_idx = point_idx;
                        bestDist = dist;
                    }
                }
                polyStart.push_back(best_points_idx);
                break;
            }
        }
        //picked.push_back(false); /// initialize all picked values as false
        assert(poly.size() != 2);
    }


    Point prev_point = startPoint;
    for (unsigned int poly_order_idx = 0; poly_order_idx < polygons.size(); poly_order_idx++) /// actual path order optimizer
    {
        int best_poly_idx = -1;
        float bestDist = std::numeric_limits<float>::infinity();


        for (unsigned int poly_idx = 0; poly_idx < polygons.size(); poly_idx++)
        {
            if (picked[poly_idx] || polygons[poly_idx]->size() < 1) /// skip single-point-polygons
            {
                continue;
            }

            assert (polygons[poly_idx]->size() != 2);

            float dist = vSize2f((*polygons[poly_idx])[polyStart[poly_idx]] - prev_point);
            if (dist < bestDist)
            {
                best_poly_idx = poly_idx;
                bestDist = dist;
            }

        }


        if (best_poly_idx > -1) /// should always be true; we should have been able to identify the best next polygon
        {
            assert(polygons[best_poly_idx]->size() != 2);

            prev_point = (*polygons[best_poly_idx])[polyStart[best_poly_idx]];

            picked[best_poly_idx] = true;
            polyOrder.push_back(best_poly_idx);
        }
        else
        {
            logError("Failed to find next closest polygon.\n");
        }
    }

    if (type == EZSeamType::SHORTEST)
    {
        prev_point = startPoint;
        for (unsigned int order_idx = 0; order_idx < polyOrder.size(); order_idx++) /// decide final starting points in each polygon
        {
            int poly_idx = polyOrder[order_idx];
            int point_idx = getPolyStart(prev_point, poly_idx);
            polyStart[poly_idx] = point_idx;
            prev_point = (*polygons[poly_idx])[point_idx];
        }
    }
}

int PathOrderOptimizer::getPolyStart(Point prev_point, int poly_idx)
{
    switch (type)
    {
        case EZSeamType::USER_SPECIFIED:    return getClosestPointInPolygon(z_seam_pos, poly_idx);
        case EZSeamType::RANDOM:            return getRandomPointInPolygon(poly_idx);
        case EZSeamType::SHORTEST:          return getClosestPointInPolygon(prev_point, poly_idx);
        default:                            return getClosestPointInPolygon(prev_point, poly_idx);
    }
}


int PathOrderOptimizer::getClosestPointInPolygon(Point prev_point, int poly_idx)
{
    ConstPolygonRef poly = *polygons[poly_idx];

    int best_point_idx = -1;
    float best_point_score = std::numeric_limits<float>::infinity();
    Point p0 = poly.back();
    for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++)
    {
        const Point& p1 = poly[point_idx];
        const Point& p2 = poly[(point_idx + 1) % poly.size()];
        int64_t dist = vSize2(p1 - prev_point);
        float is_on_inside_corner_score = -LinearAlg2D::getAngleLeft(p0, p1, p2) / M_PI * 5000 * 5000; // prefer inside corners
        // this score is in the order of 5 mm
        if (dist + is_on_inside_corner_score < best_point_score)
        {
            best_point_idx = point_idx;
            best_point_score = dist + is_on_inside_corner_score;
        }
        p0 = p1;
    }
    return best_point_idx;
}

int PathOrderOptimizer::getRandomPointInPolygon(int poly_idx)
{
    return rand() % polygons[poly_idx]->size();
}

/**
*
*/
void LineOrderOptimizer::optimize()
{
    int gridSize = 5000; // the size of the cells in the hash grid. TODO
    SparsePointGridInclusive<unsigned int> line_bucket_grid(gridSize);
    bool picked[polygons.size()];
    memset(picked, false, sizeof(bool) * polygons.size());/// initialized as falses
    
    for (unsigned int poly_idx = 0; poly_idx < polygons.size(); poly_idx++) /// find closest point to initial starting point within each polygon +initialize picked
    {
        int best_point_idx = -1;<commit_msg>Improve PathOrderOptimizer::getClosestPointInPolygon() handling of points on concave curves.<commit_after>/** Copyright (C) 2013 <NAME>am - Released under terms of the AGPLv3 License */
#include "pathOrderOptimizer.h"
#include "utils/logoutput.h"
#include "utils/SparsePointGridInclusive.h"
#include "utils/linearAlg2D.h"

#define INLINE static inline

namespace cura {

/**
*
*/
void PathOrderOptimizer::optimize()
{
    bool picked[polygons.size()];
    memset(picked, false, sizeof(bool) * polygons.size());/// initialized as falses
    
    for (unsigned poly_idx = 0; poly_idx < polygons.size(); ++poly_idx) /// find closest point to initial starting point within each polygon +initialize picked
    {
        const ConstPolygonRef poly = *polygons[poly_idx];
        switch (type)
        {
            case EZSeamType::USER_SPECIFIED:
                polyStart.push_back(getClosestPointInPolygon(z_seam_pos, poly_idx));
                break;
            case EZSeamType::RANDOM:
                polyStart.push_back(getRandomPointInPolygon(poly_idx));
                break;
            default:
            {
                int best_points_idx = -1;
                float bestDist = std::numeric_limits<float>::infinity();
                for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++) /// get closest point in polygon
                {
                    float dist = vSize2f(poly[point_idx] - startPoint);
                    if (dist < bestDist)
                    {
                        best_points_idx = point_idx;
                        bestDist = dist;
                    }
                }
                polyStart.push_back(best_points_idx);
                break;
            }
        }
        //picked.push_back(false); /// initialize all picked values as false
        assert(poly.size() != 2);
    }


    Point prev_point = startPoint;
    for (unsigned int poly_order_idx = 0; poly_order_idx < polygons.size(); poly_order_idx++) /// actual path order optimizer
    {
        int best_poly_idx = -1;
        float bestDist = std::numeric_limits<float>::infinity();


        for (unsigned int poly_idx = 0; poly_idx < polygons.size(); poly_idx++)
        {
            if (picked[poly_idx] || polygons[poly_idx]->size() < 1) /// skip single-point-polygons
            {
                continue;
            }

            assert (polygons[poly_idx]->size() != 2);

            float dist = vSize2f((*polygons[poly_idx])[polyStart[poly_idx]] - prev_point);
            if (dist < bestDist)
            {
                best_poly_idx = poly_idx;
                bestDist = dist;
            }

        }


        if (best_poly_idx > -1) /// should always be true; we should have been able to identify the best next polygon
        {
            assert(polygons[best_poly_idx]->size() != 2);

            prev_point = (*polygons[best_poly_idx])[polyStart[best_poly_idx]];

            picked[best_poly_idx] = true;
            polyOrder.push_back(best_poly_idx);
        }
        else
        {
            logError("Failed to find next closest polygon.\n");
        }
    }

    if (type == EZSeamType::SHORTEST)
    {
        prev_point = startPoint;
        for (unsigned int order_idx = 0; order_idx < polyOrder.size(); order_idx++) /// decide final starting points in each polygon
        {
            int poly_idx = polyOrder[order_idx];
            int point_idx = getPolyStart(prev_point, poly_idx);
            polyStart[poly_idx] = point_idx;
            prev_point = (*polygons[poly_idx])[point_idx];
        }
    }
}

int PathOrderOptimizer::getPolyStart(Point prev_point, int poly_idx)
{
    switch (type)
    {
        case EZSeamType::USER_SPECIFIED:    return getClosestPointInPolygon(z_seam_pos, poly_idx);
        case EZSeamType::RANDOM:            return getRandomPointInPolygon(poly_idx);
        case EZSeamType::SHORTEST:          return getClosestPointInPolygon(prev_point, poly_idx);
        default:                            return getClosestPointInPolygon(prev_point, poly_idx);
    }
}


int PathOrderOptimizer::getClosestPointInPolygon(Point prev_point, int poly_idx)
{
    ConstPolygonRef poly = *polygons[poly_idx];

    int best_point_idx = -1;
    float best_point_score = std::numeric_limits<float>::infinity();
    Point p0 = poly.back();
    for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++)
    {
        const Point& p1 = poly[point_idx];
        const Point& p2 = poly[(point_idx + 1) % poly.size()];
        int64_t dist = vSize2(p1 - prev_point);
        const float corner_angle = LinearAlg2D::getAngleLeft(p0, p1, p2) / M_PI; // 0 -> 2
        if (corner_angle > 1)
        {
            // p1 lies on a concave curve so reduce the distance to favour it
            // the more concave the curve, the more we reduce the distance
            dist -= (corner_angle - 1) * dist / 50;
        }
        if (dist < best_point_score)
        {
            best_point_idx = point_idx;
            best_point_score = dist;
        }
        p0 = p1;
    }
    return best_point_idx;
}

int PathOrderOptimizer::getRandomPointInPolygon(int poly_idx)
{
    return rand() % polygons[poly_idx]->size();
}

/**
*
*/
void LineOrderOptimizer::optimize()
{
    int gridSize = 5000; // the size of the cells in the hash grid. TODO
    SparsePointGridInclusive<unsigned int> line_bucket_grid(gridSize);
    bool picked[polygons.size()];
    memset(picked, false, sizeof(bool) * polygons.size());/// initialized as falses
    
    for (unsigned int poly_idx = 0; poly_idx < polygons.size(); poly_idx++) /// find closest point to initial starting point within each polygon +initialize picked
    {
        int best_point_idx = -1;
{'fraction_non_alphanumeric': 0.10554323725055433, 'fraction_numerical': 0.008869179600886918, 'mean_word_length': 2.1943342776203965, 'pattern_counts': {'":': 0, '<': 35, '<?xml version=': 0, '>': 25, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4482031', 'n_tokens_mistral': 3548, 'n_tokens_neox': 3374, 'n_words': 783}