C++ PolygonRef::orientation方法代码示例

inline int PathOrderOptimizer::getClosestPointInPolygon(Point prev_point, int i_polygon)
{
    PolygonRef poly = polygons[i_polygon];
    int best = -1;
    float bestDist = std::numeric_limits<float>::infinity();
    bool orientation = poly.orientation();
    for(unsigned int i_point=0 ; i_point<poly.size() ; i_point++)
    {
        float dist = vSize2f(poly[i_point] - prev_point);
        Point n0 = normal(poly[(i_point-1+poly.size())%poly.size()] - poly[i_point], 2000);
        Point n1 = normal(poly[i_point] - poly[(i_point + 1) % poly.size()], 2000);
        float dot_score = dot(n0, n1) - dot(crossZ(n0), n1); /// prefer binnenbocht
        if (orientation)
            dot_score = -dot_score;
        if (dist + dot_score < bestDist)
        {
            best = i_point;
            bestDist = dist;
        }
    }
    return best;
}

void LineOrderOptimizer::optimize()
{
    int gridSize = 5000; // the size of the cells in the hash grid.
    BucketGrid2D<unsigned int> line_bucket_grid(gridSize);

	bool* picked = new bool[polygons.size()];
   // bool picked[polygons.size()];
    memset(picked, false, sizeof(bool) * polygons.size());/// initialized as falses
    
    for(unsigned int i_polygon=0 ; i_polygon<polygons.size() ; i_polygon++) /// find closest point to initial starting point within each polygon +initialize picked
    {
        int best = -1;
        float bestDist = std::numeric_limits<float>::infinity();
        PolygonRef poly = polygons[i_polygon];
        for(unsigned int i_point=0; i_point<poly.size(); i_point++) /// get closest point from polygon
        {
            float dist = vSize2f(poly[i_point] - startPoint);
            if (dist < bestDist)
            {
                best = i_point;
                bestDist = dist;
            }
        }
        polyStart.push_back(best);

        assert(poly.size() == 2);

        line_bucket_grid.insert(poly[0], i_polygon);
        line_bucket_grid.insert(poly[1], i_polygon);

    }


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

        for(unsigned int i_close_line_polygon :  line_bucket_grid.findNearbyObjects(prev_point)) /// check if single-line-polygon is close to last point
        {
            if (picked[i_close_line_polygon] || polygons[i_close_line_polygon].size() < 1)
                continue;


            checkIfLineIsBest(i_close_line_polygon, best, bestDist, prev_point, incommingPerpundicularNormal);

        }

        if (best == -1) /// if single-line-polygon hasn't been found yet
        {
           for(unsigned int i_polygon=0 ; i_polygon<polygons.size() ; i_polygon++)
            {
                if (picked[i_polygon] || polygons[i_polygon].size() < 1) /// skip single-point-polygons
                    continue;
                assert(polygons[i_polygon].size() == 2);

                checkIfLineIsBest(i_polygon, best, bestDist, prev_point, incommingPerpundicularNormal);

            }
        }

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

            int endIdx = polyStart[best] * -1 + 1; /// 1 -> 0 , 0 -> 1
            prev_point = polygons[best][endIdx];
            incommingPerpundicularNormal = crossZ(normal(polygons[best][endIdx] - polygons[best][polyStart[best]], 1000));

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

    prev_point = startPoint;
    for(unsigned int n=0; n<polyOrder.size(); n++) /// decide final starting points in each polygon
    {
        int nr = polyOrder[n];
        PolygonRef poly = polygons[nr];
        int best = -1;
        float bestDist = std::numeric_limits<float>::infinity();
        bool orientation = poly.orientation();
        for(unsigned int i=0;i<poly.size(); i++)
        {
            float dist = vSize2f(polygons[nr][i] - prev_point);
            Point n0 = normal(poly[(i+poly.size()-1)%poly.size()] - poly[i], 2000);
            Point n1 = normal(poly[i] - poly[(i + 1) % poly.size()], 2000);
            float dot_score = dot(n0, n1) - dot(crossZ(n0), n1);
            if (orientation)
                dot_score = -dot_score;
            if (dist + dot_score < bestDist)
            {
                best = i;
                bestDist = dist + dot_score;
            }
        }
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                best = i;
                bestDist = dist;
                polyStart[i] = 0;
            }
            dist = vSize2f(polygons[i][1] - p0);
            dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][0] - polygons[i][1], 1000))) * incommingPerpundicularNormalScale;
            if (dist < bestDist)
            {
                best = i;
                bestDist = dist;
                polyStart[i] = 1;
            }
        }

        if (best == -1)
        {
            for(unsigned int i=0; i<polygons.size(); i++)
            {
                if (picked[i] || polygons[i].size() < 1)
                    continue;
                if (polygons[i].size() == 2)
                {
                    float dist = vSize2f(polygons[i][0] - p0);
                    dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][1] - polygons[i][0], 1000))) * incommingPerpundicularNormalScale;
                    if (dist < bestDist)
                    {
                        best = i;
                        bestDist = dist;
                        polyStart[i] = 0;
                    }
                    dist = vSize2f(polygons[i][1] - p0);
                    dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i][0] - polygons[i][1], 1000))) * incommingPerpundicularNormalScale;
                    if (dist < bestDist)
                    {
                        best = i;
                        bestDist = dist;
                        polyStart[i] = 1;
                    }
                } else {
                    float dist = vSize2f(polygons[i][polyStart[i]] - p0);
                    if (dist < bestDist)
                    {
                        best = i;
                        bestDist = dist;
                    }
                }
            }
        }

        if (best > -1)
        {
            if (polygons[best].size() == 2)
            {
                int endIdx = (polyStart[best] + 1) % 2;
                p0 = polygons[best][endIdx];
                incommingPerpundicularNormal = crossZ(normal(polygons[best][endIdx] - polygons[best][polyStart[best]], 1000));
            } else {
                p0 = polygons[best][polyStart[best]];
                incommingPerpundicularNormal = Point(0, 0);
            }
            picked[best] = true;
            polyOrder.push_back(best);
        }
    }

    p0 = startPoint;
    for(int nr : polyOrder)
    {
        PolygonRef poly = polygons[nr];
        if (poly.size() > 2)
        {
            int best = -1;
            float bestDist = 0xFFFFFFFFFFFFFFFFLL;
            bool orientation = poly.orientation();
            for(unsigned int i=0; i<poly.size(); i++)
            {
                const int64_t dot_score_scale = 2000;
                float dist = vSize2f(polygons[nr][i] - p0);
                Point n0 = normal(poly[(i+poly.size()-1)%poly.size()] - poly[i], dot_score_scale);
                Point n1 = normal(poly[i] - poly[(i + 1) % poly.size()], dot_score_scale);
                float dot_score = dot(n0, n1) - dot(crossZ(n0), n1);
                if (orientation)
                    dot_score = -dot_score;
                dist += dot_score;
                if (dist < bestDist)
                {
                    best = i;
                    bestDist = dist;
                }
            }
            polyStart[nr] = best;
        }
        if (poly.size() <= 2)
        {
            p0 = poly[(polyStart[nr] + 1) % 2];
        } else {
            p0 = poly[polyStart[nr]];
        }
    }
}