<commit_before>/** Copyright (C) 2013 David Braam - Released under terms of the AGPLv3 License */
#include "pathOrderOptimizer.h"

namespace cura {

void PathOrderOptimizer::optimize()
{
    std::vector<bool> picked;
    for(unsigned int i=0;i<polygons.size(); i++)
    {
        int best = -1;
        float bestDist = 0xFFFFFFFFFFFFFFFFLL;
        PolygonRef poly = polygons[i];
        for(unsigned int j=0; j<poly.size(); j++)
        {
            float dist = vSize2f(poly[j] - startPoint);
            if (dist < bestDist)
            {
                best = j;
                bestDist = dist;
            }
        }
        polyStart.push_back(best);
        picked.push_back(false);
    }

    Point incommingPerpundicularNormal(0, 0);
    Point p0 = startPoint;
    for(unsigned int n=0; n<polygons.size(); n++)
    {
        int best = -1;
        float bestDist = 0xFFFFFFFFFFFFFFFFLL;
        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))) * 0.0001f;
                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))) * 0.0001f;
                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(unsigned int n=0; n<polyOrder.size(); n++)
    {
        int nr = polyOrder[n];
        int best = -1;
        float bestDist = 0xFFFFFFFFFFFFFFFFLL;
        for(unsigned int i=0;i<polygons[nr].size(); i++)
        {
            float dist = vSize2f(polygons[nr][i] - p0);
            if (dist < bestDist)
            {
                best = i;
                bestDist = dist;
            }
        }
        polyStart[nr] = best;
        if (polygons[nr].size() <= 2)
        {
            p0 = polygons[nr][(best + 1) % 2];
        }else{
            p0 = polygons[nr][best];
        }
    }
}

}//namespace cura
<commit_msg>Try to put the Z seam in internal corners instead of anywhere on the external face.<commit_after>/** Copyright (C) 2013 David Braam - Released under terms of the AGPLv3 License */
#include "pathOrderOptimizer.h"
#include "utils/logOutput.h"

namespace cura {

void PathOrderOptimizer::optimize()
{
    std::vector<bool> picked;
    for(unsigned int i=0;i<polygons.size(); i++)
    {
        int best = -1;
        float bestDist = 0xFFFFFFFFFFFFFFFFLL;
        PolygonRef poly = polygons[i];
        for(unsigned int j=0; j<poly.size(); j++)
        {
            float dist = vSize2f(poly[j] - startPoint);
            if (dist < bestDist)
            {
                best = j;
                bestDist = dist;
            }
        }
        polyStart.push_back(best);
        picked.push_back(false);
    }

    Point incommingPerpundicularNormal(0, 0);
    Point p0 = startPoint;
    for(unsigned int n=0; n<polygons.size(); n++)
    {
        int best = -1;
        float bestDist = 0xFFFFFFFFFFFFFFFFLL;
        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))) * 0.0001f;
                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))) * 0.0001f;
                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(unsigned int n=0; n<polyOrder.size(); n++)
    {
        int nr = polyOrder[n];
        PolygonRef poly = polygons[nr];
        int best = -1;
        float bestDist = 0xFFFFFFFFFFFFFFFFLL;
        bool orientation = poly.orientation();
        for(unsigned int i=0;i<poly.size(); i++)
        {
            float dist = vSize2f(polygons[nr][i] - p0);
            Point n0 = normal(poly[(i+poly.size()-1)%poly.size()] - poly[i], 10000);
            Point n1 = normal(poly[i] - poly[(i + 1) % poly.size()], 10000);
            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;
            }
        }
        polyStart[nr] = best;
        if (poly.size() <= 2)
        {
            p0 = poly[(best + 1) % 2];
        }else{
            p0 = poly[best];
        }
    }
}

}//namespace cura
<|endoftext|>


<commit_before>/** Copyright (C) 2013 David Braam - Released under terms of the AGPLv3 License */
#include "pathOrderOptimizer.h"
#include "utils/logoutput.h"
#include "utils/BucketGrid2D.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 (PolygonRef poly : polygons) /// find closest point to initial starting point within each polygon +initialize picked
    {
        int best = -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 = point_idx;
                bestDist = dist;
            }
        }
        polyStart.push_back(best);
        //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");
        }
    }

    prev_point = startPoint;
    for (unsigned int poly_idx = 0; poly_idx < polyOrder.size(); poly_idx++) /// decide final starting points in each polygon
    {
        int ordered_poly_idx = polyOrder[poly_idx];
        int point_idx = getPolyStart(prev_point, ordered_poly_idx);
        polyStart[poly_idx] = point_idx;
        prev_point = polygons[ordered_poly_idx][point_idx];

    }
}

int PathOrderOptimizer::getPolyStart(Point prev_point, int poly_idx)
{
    switch (type)
    {
        case EZSeamType::BACK:      return getFarthestPointInPolygon(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)
{
    PolygonRef poly = polygons[poly_idx];
    int best_point_idx = -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_point_idx = i_point;
            bestDist = dist;
        }
    }
    return best_point_idx;
}

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

int PathOrderOptimizer::getFarthestPointInPolygon(int poly_idx)
{
    PolygonRef poly = polygons[poly_idx];
    int best_point_idx = -1;
    float best_y = std::numeric_limits<float>::min();
    for(unsigned int point_idx=0 ; point_idx<poly.size() ; point_idx++)
    {
        if (poly[point_idx].Y > best_y)
        {
            best_point_idx = point_idx;
            best_y = poly[point_idx].Y;
        }
    }
    return best_point_idx;
}

/**
*
*/
void LineOrderOptimizer::optimize()
{
    int gridSize = 5000; // the size of the cells in the hash grid.
    BucketGrid2D<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;
        float best_point_dist = std::numeric_limits<float>::infinity();
        PolygonRef poly = polygons[poly_idx];
        for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++) /// get closest point from polygon
        {
            float dist = vSize2f(poly[point_idx] - startPoint);
            if (dist < best_point_dist)
            {
                best_point_idx = point_idx;
                best_point_dist = dist;
            }
        }
        polyStart.push_back(best_point_idx);

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

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

    }

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

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

            checkIfLineIsBest(close_line_poly_idx, best_line_idx, bestDist, prev_point, incommingPerpundicularNormal);
        }

        if (best_line_idx == -1) /// if single-line-polygon hasn't been found yet
        {
            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);

                checkIfLineIsBest(poly_idx, best_line_idx, bestDist, prev_point, incommingPerpundicularNormal);

            }
        }

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

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

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

    prev_point = startPoint;
    for (int poly_idx : polyOrder)
    {
        PolygonRef poly = polygons[poly_idx];
        int best = -1;
        float bestDist = std::numeric_limits<float>::infinity();
        bool orientation = poly.orientation();
        for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++)
        {
            float dist = vSize2f(polygons[poly_idx][point_idx] - prev_point);
            Point n0 = normal(poly[(point_idx+poly.size()-1)%poly.size()] - poly[point_idx], 2000);
            Point n1 = normal(poly[point_idx] - poly[(point_idx + 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 = point_idx;
                bestDist = dist + dot_score;
            }
        }

        polyStart[poly_idx] = best;
        assert(poly.size() == 2);
        prev_point = poly[best *-1 + 1]; /// 1 -> 0 , 0 -> 1

    }
}

inline void LineOrderOptimizer::checkIfLineIsBest(unsigned int i_line_polygon, int& best, float& bestDist, Point& prev_point, Point& incommingPerpundicularNormal)
{
    { /// check distance to first point on line (0)
        float dist = vSize2f(polygons[i_line_polygon][0] - prev_point);
        dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i_line_polygon][1] - polygons[i_line_polygon][0], 1000))) * 0.0001f; /// penalize sharp corners
        if (dist < bestDist)
        {
            best = i_line_polygon;
            bestDist = dist;
            polyStart[i_line_polygon] = 0;
        }
    }
    { /// check distance to second point on line (1)
        float dist = vSize2f(polygons[i_line_polygon][1] - prev_point);
        dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i_line_polygon][0] - polygons[i_line_polygon][1], 1000) )) * 0.0001f; /// penalize sharp corners
        if (dist < bestDist)
        {
            best = i_line_polygon;
            bestDist = dist;
            polyStart[i_line_polygon] = 1;
        }
    }
}

}//namespace cura
<commit_msg>refactor: code cleanup helper functions of PathOrderOptimizer (CURA-1170)<commit_after>/** Copyright (C) 2013 David Braam - Released under terms of the AGPLv3 License */
#include "pathOrderOptimizer.h"
#include "utils/logoutput.h"
#include "utils/BucketGrid2D.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 (PolygonRef poly : polygons) /// find closest point to initial starting point within each polygon +initialize picked
    {
        int best = -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 = point_idx;
                bestDist = dist;
            }
        }
        polyStart.push_back(best);
        //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");
        }
    }

    prev_point = startPoint;
    for (unsigned int poly_idx = 0; poly_idx < polyOrder.size(); poly_idx++) /// decide final starting points in each polygon
    {
        int ordered_poly_idx = polyOrder[poly_idx];
        int point_idx = getPolyStart(prev_point, ordered_poly_idx);
        polyStart[poly_idx] = point_idx;
        prev_point = polygons[ordered_poly_idx][point_idx];

    }
}

int PathOrderOptimizer::getPolyStart(Point prev_point, int poly_idx)
{
    switch (type)
    {
        case EZSeamType::BACK:      return getFarthestPointInPolygon(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)
{
    PolygonRef poly = polygons[poly_idx];
    int best_point_idx = -1;
    float bestDist = std::numeric_limits<float>::infinity();
    bool orientation = poly.orientation();
    for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++)
    {
        float dist = vSize2f(poly[point_idx] - prev_point);
        Point n0 = normal(poly[(point_idx-1+poly.size())%poly.size()] - poly[point_idx], 2000);
        Point n1 = normal(poly[point_idx] - poly[(point_idx + 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_point_idx = point_idx;
            bestDist = dist;
        }
    }
    return best_point_idx;
}

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

int PathOrderOptimizer::getFarthestPointInPolygon(int poly_idx)
{
    PolygonRef poly = polygons[poly_idx];
    int best_point_idx = -1;
    float best_y = std::numeric_limits<float>::min();
    for(unsigned int point_idx=0 ; point_idx<poly.size() ; point_idx++)
    {
        if (poly[point_idx].Y > best_y)
        {
            best_point_idx = point_idx;
            best_y = poly[point_idx].Y;
        }
    }
    return best_point_idx;
}

/**
*
*/
void LineOrderOptimizer::optimize()
{
    int gridSize = 5000; // the size of the cells in the hash grid.
    BucketGrid2D<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;
        float best_point_dist = std::numeric_limits<float>::infinity();
        PolygonRef poly = polygons[poly_idx];
        for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++) /// get closest point from polygon
        {
            float dist = vSize2f(poly[point_idx] - startPoint);
            if (dist < best_point_dist)
            {
                best_point_idx = point_idx;
                best_point_dist = dist;
            }
        }
        polyStart.push_back(best_point_idx);

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

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

    }

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

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

            checkIfLineIsBest(close_line_poly_idx, best_line_idx, bestDist, prev_point, incommingPerpundicularNormal);
        }

        if (best_line_idx == -1) /// if single-line-polygon hasn't been found yet
        {
            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);

                checkIfLineIsBest(poly_idx, best_line_idx, bestDist, prev_point, incommingPerpundicularNormal);

            }
        }

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

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

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

    prev_point = startPoint;
    for (int poly_idx : polyOrder)
    {
        PolygonRef poly = polygons[poly_idx];
        int best = -1;
        float bestDist = std::numeric_limits<float>::infinity();
        bool orientation = poly.orientation();
        for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++)
        {
            float dist = vSize2f(polygons[poly_idx][point_idx] - prev_point);
            Point n0 = normal(poly[(point_idx+poly.size()-1)%poly.size()] - poly[point_idx], 2000);
            Point n1 = normal(poly[point_idx] - poly[(point_idx + 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 = point_idx;
                bestDist = dist + dot_score;
            }
        }

        polyStart[poly_idx] = best;
        assert(poly.size() == 2);
        prev_point = poly[best *-1 + 1]; /// 1 -> 0 , 0 -> 1

    }
}

inline void LineOrderOptimizer::checkIfLineIsBest(unsigned int i_line_polygon, int& best, float& bestDist, Point& prev_point, Point& incommingPerpundicularNormal)
{
    { /// check distance to first point on line (0)
        float dist = vSize2f(polygons[i_line_polygon][0] - prev_point);
        dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i_line_polygon][1] - polygons[i_line_polygon][0], 1000))) * 0.0001f; /// penalize sharp corners
        if (dist < bestDist)
        {
            best = i_line_polygon;
            bestDist = dist;
            polyStart[i_line_polygon] = 0;
        }
    }
    { /// check distance to second point on line (1)
        float dist = vSize2f(polygons[i_line_polygon][1] - prev_point);
        dist += abs(dot(incommingPerpundicularNormal, normal(polygons[i_line_polygon][0] - polygons[i_line_polygon][1], 1000) )) * 0.0001f; /// penalize sharp corners
        if (dist < bestDist)
        {
            best = i_line_polygon;
            bestDist = dist;
            polyStart[i_line_polygon] = 1;
        }
    }
}

}//namespace cura
<|endoftext|>


<commit_before>// Copyright (c) 2014-2021 Dr. Colin Hirsch and Daniel Frey
// Please see LICENSE for license or visit https://github.com/taocpp/PEGTL/

#include "test.hpp"

#include "verify_meta.hpp"
#include "verify_rule.hpp"

namespace TAO_PEGTL_NAMESPACE
{
   template< typename Rule >
   struct my_action
   {};

   template<>
   struct my_action< eof >
   {
      static void apply0( bool& b )
      {
         b = true;
      }
   };

   void unit_test()
   {
      verify_meta< opt<>, internal::success >();
      verify_meta< opt< eof >, internal::opt< eof >, eof >();
      verify_meta< opt< eof, any >, internal::opt< internal::seq< eof, any > >, internal::seq< eof, any > >();

      verify_analyze< opt< any > >( __LINE__, __FILE__, false, false );
      verify_analyze< opt< eof > >( __LINE__, __FILE__, false, false );

      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "", result_type::success, 0 );
      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "a", result_type::success, 0 );
      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "aa", result_type::success, 1 );
      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "ab", result_type::success, 1 );
      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "ba", result_type::success, 2 );

      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "", result_type::success, 0 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "a", result_type::success, 1 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "ab", result_type::success, 0 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "aba", result_type::success, 1 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "abab", result_type::success, 2 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "bab", result_type::success, 3 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "cb", result_type::success, 2 );

      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "", result_type::success, 0 );
      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "a", result_type::success, 0 );
      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "aa", result_type::success, 1 );
      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "ab", result_type::success, 1 );
      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "ba", result_type::success, 2 );

      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "", result_type::success, 0 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "a", result_type::success, 1 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "ab", result_type::success, 0 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "aba", result_type::success, 1 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "abab", result_type::success, 2 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "bab", result_type::success, 3 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "cb", result_type::success, 2 );

      bool success = false;
      const bool result = parse< opt< eof >, my_action >( memory_input( "", __FUNCTION__ ), success );
      TAO_PEGTL_TEST_ASSERT( result );
      TAO_PEGTL_TEST_ASSERT( success );
   }

}  // namespace TAO_PEGTL_NAMESPACE

#include "main.hpp"
<commit_msg>Support for -fno-exception<commit_after>// Copyright (c) 2014-2021 Dr. Colin Hirsch and Daniel Frey
// Please see LICENSE for license or visit https://github.com/taocpp/PEGTL/

#include "test.hpp"

#include "verify_meta.hpp"
#include "verify_rule.hpp"

namespace TAO_PEGTL_NAMESPACE
{
   template< typename Rule >
   struct my_action
   {};

   template<>
   struct my_action< eof >
   {
      static void apply0( bool& b )
      {
         b = true;
      }
   };

   void unit_test()
   {
      verify_meta< opt<>, internal::success >();
      verify_meta< opt< eof >, internal::opt< eof >, eof >();
      verify_meta< opt< eof, any >, internal::opt< internal::seq< eof, any > >, internal::seq< eof, any > >();

      verify_analyze< opt< any > >( __LINE__, __FILE__, false, false );
      verify_analyze< opt< eof > >( __LINE__, __FILE__, false, false );

      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "", result_type::success, 0 );
      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "a", result_type::success, 0 );
      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "aa", result_type::success, 1 );
      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "ab", result_type::success, 1 );
      verify_rule< opt< one< 'a' > > >( __LINE__, __FILE__, "ba", result_type::success, 2 );

      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "", result_type::success, 0 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "a", result_type::success, 1 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "ab", result_type::success, 0 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "aba", result_type::success, 1 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "abab", result_type::success, 2 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "bab", result_type::success, 3 );
      verify_rule< opt< one< 'a' >, one< 'b' > > >( __LINE__, __FILE__, "cb", result_type::success, 2 );

#if defined( __cxx_exceptions )
      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "", result_type::success, 0 );
      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "a", result_type::success, 0 );
      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "aa", result_type::success, 1 );
      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "ab", result_type::success, 1 );
      verify_rule< must< opt< one< 'a' > > > >( __LINE__, __FILE__, "ba", result_type::success, 2 );

      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "", result_type::success, 0 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "a", result_type::success, 1 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "ab", result_type::success, 0 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "aba", result_type::success, 1 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "abab", result_type::success, 2 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "bab", result_type::success, 3 );
      verify_rule< must< opt< one< 'a' >, one< 'b' > > > >( __LINE__, __FILE__, "cb", result_type::success, 2 );
#endif

      bool success = false;
      const bool result = parse< opt< eof >, my_action >( memory_input( "", __FUNCTION__ ), success );
      TAO_PEGTL_TEST_ASSERT( result );
      TAO_PEGTL_TEST_ASSERT( success );
   }

}  // namespace TAO_PEGTL_NAMESPACE

#include "main.hpp"
<|endoftext|>


<commit_before>/**************************************************************************

 The MIT License (MIT)

 Copyright (c) 2015 Dmitry Sovetov

 https://github.com/dmsovetov

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:
 The above copyright notice and this permission notice shall be included in all
 copies or substantial portions of the Software.
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.

 **************************************************************************/

#include <gtest/gtest.h>
#include <io/Io.h>

DC_USE_DREEMCHEST

using namespace io;

char data[] = "Hello world!";

TEST(ByteBuffer, SizeAfterCreation)
{
    ByteBufferPtr buffer = ByteBuffer::create();
    EXPECT_EQ( 0, buffer->length() );
}

TEST(ByteBuffer, PositionAfterCreation)
{
    ByteBufferPtr buffer = ByteBuffer::create();
    EXPECT_EQ( 0, buffer->position() );
}

TEST(ByteBuffer, SizeAfterInitilization)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    EXPECT_EQ( strlen( data ), buffer->length() );
}

TEST(ByteBuffer, PiositionAfterInitilization)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    EXPECT_EQ( 0, buffer->position() );
}

TEST(ByteBuffer, DataAfterInitialization)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        EXPECT_EQ( data[i], buffer->buffer()[i] );
    }
}

TEST(ByteBuffer, SeekSet)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->setPosition( i, SeekSet );
        EXPECT_EQ( i, buffer->position() );
    }
}

TEST(ByteBuffer, SeekCur)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->setPosition( 1, SeekCur );
        EXPECT_EQ( i + 1, buffer->position() );
    }
}

TEST(ByteBuffer, SeekEnd)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->setPosition( i, SeekEnd );
        EXPECT_EQ( buffer->length() - i, buffer->position() );
    }
}

TEST(ByteBuffer, DataAfterSeek)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->setPosition( i );
        EXPECT_EQ( data[i], buffer->current()[0] );
    }
}

TEST(ByteBuffer, Fill)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    buffer->fill( 1 );

    for( int i = 0; i < buffer->length(); i++ ) {
        EXPECT_EQ( 1, buffer->buffer()[i] );
    }
}

TEST(ByteBuffer, TrimLeft)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    buffer->trimFromLeft( 2 );
    ASSERT_EQ( strlen( data ) - 2, buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        ASSERT_EQ( data[i + 2], buffer->current()[i] );
    }
}

TEST(ByteBuffer, TrimRight)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    buffer->trimFromRight( 2 );
    ASSERT_EQ( strlen( data ) - 2, buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        ASSERT_EQ( data[i], buffer->current()[i] );
    }
}

TEST(ByteBuffer, Read)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char in[20] = { 0 };
    buffer->read( in, strlen( data ) );

    ASSERT_STREQ( data, in );
}

TEST(ByteBuffer, PositionAfterRead)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char in[20] = { 0 };
    buffer->read( in, strlen( data ) );

    ASSERT_EQ( strlen( data ), buffer->position() );
}

TEST(ByteBuffer, SequentialRead)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char c;

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->read( &c, 1 );
        ASSERT_EQ( i + 1, buffer->position() );
        ASSERT_EQ( data[i], c );
    }
}

TEST(ByteBuffer, Write)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char* text = "written";

    buffer->write( text, strlen( text ) );
    for( int i = 0; i < strlen( text ); i++ ) {
         ASSERT_EQ( text[i], buffer->buffer()[i] );
    }
}

TEST(ByteBuffer, PositionAfterWrite)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char* text = "written";

    buffer->write( text, strlen( text ) );
    ASSERT_EQ( strlen( text ), buffer->position() );
}

TEST(ByteBuffer, SequentialWrite)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char c = 'x';

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->write( &c, 1 );
        ASSERT_EQ( i + 1, buffer->position() );
    }

    for( int i = 0; i < buffer->length(); i++ ) {
        ASSERT_EQ( c, buffer->buffer()[i] );
    }
}

TEST(ByteBuffer, BytesAbvailableAfterInit)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );
    EXPECT_EQ( strlen( data ), buffer->bytesAvailable() );
    EXPECT_EQ( buffer->length(), buffer->bytesAvailable() );
}

TEST(ByteBuffer, BytesAbvailableAfterRead)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char c;

    for( int i = 0; i < buffer->length(); i++ ) {
        ASSERT_EQ( buffer->length() - i, buffer->bytesAvailable() );
        buffer->read( &c, 1 );
        ASSERT_EQ( buffer->length() - (i + 1), buffer->bytesAvailable() );
    }
}

TEST(ByteBuffer, WriteExpandsTheBuffer)
{
    ByteBufferPtr buffer = ByteBuffer::createWithData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    buffer->setPosition( 0, SeekEnd );
    ASSERT_EQ( buffer->length(), buffer->position() );

    char* msg     = "message";
    int   before = buffer->length();

    buffer->write( msg, strlen( msg ) );
    ASSERT_EQ( buffer->length(), before + strlen( msg ) );
}<commit_msg>Fixed ByteBuffer unit tests compilation issue<commit_after>/**************************************************************************

 The MIT License (MIT)

 Copyright (c) 2015 Dmitry Sovetov

 https://github.com/dmsovetov

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:
 The above copyright notice and this permission notice shall be included in all
 copies or substantial portions of the Software.
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.

 **************************************************************************/

#include <gtest/gtest.h>
#include <io/Io.h>

DC_USE_DREEMCHEST

using namespace io;

char data[] = "Hello world!";

TEST(ByteBuffer, SizeAfterCreation)
{
    ByteBufferPtr buffer = ByteBuffer::create();
    EXPECT_EQ( 0, buffer->length() );
}

TEST(ByteBuffer, PositionAfterCreation)
{
    ByteBufferPtr buffer = ByteBuffer::create();
    EXPECT_EQ( 0, buffer->position() );
}

TEST(ByteBuffer, SizeAfterInitilization)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    EXPECT_EQ( strlen( data ), buffer->length() );
}

TEST(ByteBuffer, PiositionAfterInitilization)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    EXPECT_EQ( 0, buffer->position() );
}

TEST(ByteBuffer, DataAfterInitialization)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        EXPECT_EQ( data[i], buffer->buffer()[i] );
    }
}

TEST(ByteBuffer, SeekSet)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->setPosition( i, SeekSet );
        EXPECT_EQ( i, buffer->position() );
    }
}

TEST(ByteBuffer, SeekCur)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->setPosition( 1, SeekCur );
        EXPECT_EQ( i + 1, buffer->position() );
    }
}

TEST(ByteBuffer, SeekEnd)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->setPosition( i, SeekEnd );
        EXPECT_EQ( buffer->length() - i, buffer->position() );
    }
}

TEST(ByteBuffer, DataAfterSeek)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->setPosition( i );
        EXPECT_EQ( data[i], buffer->current()[0] );
    }
}

TEST(ByteBuffer, Fill)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    buffer->fill( 1 );

    for( int i = 0; i < buffer->length(); i++ ) {
        EXPECT_EQ( 1, buffer->buffer()[i] );
    }
}

TEST(ByteBuffer, TrimLeft)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    buffer->trimFromLeft( 2 );
    ASSERT_EQ( strlen( data ) - 2, buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        ASSERT_EQ( data[i + 2], buffer->current()[i] );
    }
}

TEST(ByteBuffer, TrimRight)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    buffer->trimFromRight( 2 );
    ASSERT_EQ( strlen( data ) - 2, buffer->length() );

    for( int i = 0; i < buffer->length(); i++ ) {
        ASSERT_EQ( data[i], buffer->current()[i] );
    }
}

TEST(ByteBuffer, Read)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char in[20] = { 0 };
    buffer->read( in, strlen( data ) );

    ASSERT_STREQ( data, in );
}

TEST(ByteBuffer, PositionAfterRead)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char in[20] = { 0 };
    buffer->read( in, strlen( data ) );

    ASSERT_EQ( strlen( data ), buffer->position() );
}

TEST(ByteBuffer, SequentialRead)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char c;

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->read( &c, 1 );
        ASSERT_EQ( i + 1, buffer->position() );
        ASSERT_EQ( data[i], c );
    }
}

TEST(ByteBuffer, Write)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char* text = "written";

    buffer->write( text, strlen( text ) );
    for( int i = 0; i < strlen( text ); i++ ) {
         ASSERT_EQ( text[i], buffer->buffer()[i] );
    }
}

TEST(ByteBuffer, PositionAfterWrite)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char* text = "written";

    buffer->write( text, strlen( text ) );
    ASSERT_EQ( strlen( text ), buffer->position() );
}

TEST(ByteBuffer, SequentialWrite)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char c = 'x';

    for( int i = 0; i < buffer->length(); i++ ) {
        buffer->write( &c, 1 );
        ASSERT_EQ( i + 1, buffer->position() );
    }

    for( int i = 0; i < buffer->length(); i++ ) {
        ASSERT_EQ( c, buffer->buffer()[i] );
    }
}

TEST(ByteBuffer, BytesAbvailableAfterInit)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );
    EXPECT_EQ( strlen( data ), buffer->bytesAvailable() );
    EXPECT_EQ( buffer->length(), buffer->bytesAvailable() );
}

TEST(ByteBuffer, BytesAbvailableAfterRead)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    char c;

    for( int i = 0; i < buffer->length(); i++ ) {
        ASSERT_EQ( buffer->length() - i, buffer->bytesAvailable() );
        buffer->read( &c, 1 );
        ASSERT_EQ( buffer->length() - (i + 1), buffer->bytesAvailable() );
    }
}

TEST(ByteBuffer, WriteExpandsTheBuffer)
{
    ByteBufferPtr buffer = ByteBuffer::createFromData( ( u8* )data, strlen( data ) );
    ASSERT_EQ( strlen( data ), buffer->length() );

    buffer->setPosition( 0, SeekEnd );
    ASSERT_EQ( buffer->length(), buffer->position() );

    char* msg     = "message";
    int   before = buffer->length();

    buffer->write( msg, strlen( msg ) );
    ASSERT_EQ( buffer->length(), before + strlen( msg ) );
}<|endoftext|>


<commit_before>/* Deathray - An Avisynth plug-in filter for spatial/temporal non-local means de-noising.
 *
 * version 1.00
 *
 * Copyright 2011, Jawed Ashraf - Deathray@cupidity.f9.co.uk
 */

#include <math.h>
#include "buffer_map.h"
#include "CLutil.h"

int buffer_map::NewIndex() {

	if (buffer_map_.size() != 0)
		return buffer_map_.rbegin()->first + 1;
	else
		return 1;
}

result buffer_map::Append(mem **new_mem, int *new_index) {

	pair<map<int, mem*>::iterator, bool> insertionStatus;

	*new_index = NewIndex();
	insertionStatus = buffer_map_.insert (pair<int, mem*> (*new_index, *new_mem));

	if (insertionStatus.second)
		return FILTER_OK;
	else
		return FILTER_ERROR;
}

result buffer_map::AllocBuffer(
	const	cl_command_queue	&cq,
	const	size_t				&bytes,
			int					*new_index) {

	result status = FILTER_OK;

	buffer *new_float_buffer = new buffer;
	new_float_buffer->Init(cq, bytes);
	if (new_float_buffer->valid()) {
		mem* new_mem = new_float_buffer;
		status = Append(&new_mem, new_index);
		return status;
	} else {
		return FILTER_ERROR;
	}
}

result buffer_map::CopyToBuffer(
	const	int		&index,
	const	void	*host_buffer,
	const	size_t	&bytes) {

	buffer *destination;
	destination = static_cast<buffer*>(buffer_map_[index]);
	return destination->CopyTo(host_buffer, bytes);
}

result buffer_map::CopyFromBuffer(
	const	int		&index,
	const	size_t	&bytes,
			void	*host_buffer) {

	buffer* source;
	source = static_cast<buffer*>(buffer_map_[index]);
	return source->CopyFrom(bytes, host_buffer);
}

result buffer_map::AllocPlane(
	const	cl_command_queue	&cq,
	const	int					&width,
	const	int					&height,
			int					*new_index) {

	result status = FILTER_OK;

	plane* new_plane = new plane;
	new_plane->Init(cq, width, height, 2, 0);
	if (new_plane->valid()) {
		mem* new_mem = new_plane;
		status = Append(&new_mem, new_index);
		return status;
	} else {
		return FILTER_ERROR;
	}
}

result buffer_map::CopyToPlane(
	const	int		&index,
	const	byte	&host_buffer,
	const	int		&host_cols,
	const	int		&host_rows,
	const	int		&host_pitch) {

	plane* destination;
	destination = static_cast<plane*>(buffer_map_[index]);
	return destination->CopyTo(host_buffer, host_cols, host_rows, host_pitch);
}

result buffer_map::CopyToPlaneAsynch(
	const	int			&index,
	const	byte		&host_buffer,
	const	int			&host_cols,
	const	int			&host_rows,
	const	int			&host_pitch,
			cl_event	*event) {

	plane* destination;
	destination = static_cast<plane*>(buffer_map_[index]);
	return destination->CopyToAsynch(host_buffer, host_cols, host_rows, host_pitch, event);
}

result buffer_map::CopyFromPlane(
	const	int		&index,
	const	int		&host_cols,
	const	int		&host_rows,
	const	int		&host_pitch,
			byte	*host_buffer) {

	plane* source;
	source = static_cast<plane*>(buffer_map_[index]);
	return source->CopyFrom(host_cols, host_rows, host_pitch, host_buffer);
}

result buffer_map::CopyFromPlaneAsynch(
	const	int			&index,
	const	int			&host_cols,
	const	int			&host_rows,
	const	int			&host_pitch,
	const	cl_event	*antecedent,
			cl_event	*event,
			byte		*host_buffer) {

	plane* source;
	source = static_cast<plane*>(buffer_map_[index]);
	return source->CopyFromAsynch(host_cols, host_rows, host_pitch, antecedent, event, host_buffer);
}

void buffer_map::Destroy(const int &index) {
	if (! ValidIndex(index)) return;

	// Check reference count
	cl_uint reference_count = 0;
	clGetMemObjectInfo(buffer_map_[index]->obj(),
					   CL_MEM_REFERENCE_COUNT,
					   sizeof(cl_uint),
					   &reference_count,
					   0);

	// Repeatedly release it to ensure it will be destroyed
	for (cl_uint i = 0; i < reference_count; ++i) {
		clReleaseMemObject(buffer_map_[index]->obj());
	}

	buffer_map_.erase(index);
}

void buffer_map::DestroyAll() {
	map<int, mem*>::iterator each_buffer;
	map<int, mem*>::iterator next_buffer;
	each_buffer = buffer_map_.begin();
	while (each_buffer != buffer_map_.end()) {
		next_buffer = each_buffer;
		++next_buffer;
		Destroy(each_buffer->first);
		each_buffer = next_buffer;
	}
	buffer_map_.clear();
}

bool buffer_map::ValidIndex(const int &index) {
	return buffer_map_.count(index) > 0;
}

cl_mem* buffer_map::ptr(const int &index) {
	return buffer_map_[index]->ptr();
}
<commit_msg>Extra buffer error statuses<commit_after>/* Deathray - An Avisynth plug-in filter for spatial/temporal non-local means de-noising.
 *
 * version 1.00
 *
 * Copyright 2011, Jawed Ashraf - Deathray@cupidity.f9.co.uk
 */

#include <math.h>
#include "buffer_map.h"
#include "CLutil.h"

int buffer_map::NewIndex() {

	if (buffer_map_.size() != 0)
		return buffer_map_.rbegin()->first + 1;
	else
		return 1;
}

result buffer_map::Append(mem **new_mem, int *new_index) {

	pair<map<int, mem*>::iterator, bool> insertionStatus;

	*new_index = NewIndex();
	insertionStatus = buffer_map_.insert(pair<int, mem*>(*new_index, *new_mem));

	if (insertionStatus.second)
		return FILTER_OK;
	else
		return FILTER_ERROR;
}

result buffer_map::AllocBuffer(
	const	cl_command_queue	&cq,
	const	size_t				&bytes,
			int					*new_index) {

	result status = FILTER_OK;

	buffer *new_float_buffer = new buffer;
	new_float_buffer->Init(cq, bytes);
	if (new_float_buffer->valid()) {
		mem *new_mem = new_float_buffer;
		status = Append(&new_mem, new_index);
		return status;
	} else {
		return FILTER_BUFFER_ALLOCATION_FAILED;
	}
}

result buffer_map::CopyToBuffer(
	const	int		&index,
	const	void	*host_buffer,
	const	size_t	&bytes) {

	buffer *destination;
	destination = static_cast<buffer*>(buffer_map_[index]);
	return destination->CopyTo(host_buffer, bytes);
}

result buffer_map::CopyFromBuffer(
	const	int		&index,
	const	size_t	&bytes,
			void	*host_buffer) {

	buffer *source;
	source = static_cast<buffer*>(buffer_map_[index]);
	return source->CopyFrom(bytes, host_buffer);
}

result buffer_map::AllocPlane(
	const	cl_command_queue	&cq,
	const	int					&width,
	const	int					&height,
			int					*new_index) {

	result status = FILTER_OK;

	plane *new_plane = new plane;
	new_plane->Init(cq, width, height, 2, 0);
	if (new_plane->valid()) {
		mem *new_mem = new_plane;
		status = Append(&new_mem, new_index);
		return status;
	} else {
		return FILTER_PLANE_ALLOCATION_FAILED;
	}
}

result buffer_map::CopyToPlane(
	const	int		&index,
	const	byte	&host_buffer,
	const	int		&host_cols,
	const	int		&host_rows,
	const	int		&host_pitch) {

	plane *destination;
	destination = static_cast<plane*>(buffer_map_[index]);
	return destination->CopyTo(host_buffer, host_cols, host_rows, host_pitch);
}

result buffer_map::CopyToPlaneAsynch(
	const	int			&index,
	const	byte		&host_buffer,
	const	int			&host_cols,
	const	int			&host_rows,
	const	int			&host_pitch,
			cl_event	*event) {

	plane *destination;
	destination = static_cast<plane*>(buffer_map_[index]);
	return destination->CopyToAsynch(host_buffer, host_cols, host_rows, host_pitch, event);
}

result buffer_map::CopyFromPlane(
	const	int		&index,
	const	int		&host_cols,
	const	int		&host_rows,
	const	int		&host_pitch,
			byte	*host_buffer) {

	plane *source;
	source = static_cast<plane*>(buffer_map_[index]);
	return source->CopyFrom(host_cols, host_rows, host_pitch, host_buffer);
}

result buffer_map::CopyFromPlaneAsynch(
	const	int			&index,
	const	int			&host_cols,
	const	int			&host_rows,
	const	int			&host_pitch,
	const	cl_event	*antecedent,
			cl_event	*event,
			byte		*host_buffer) {

	plane *source;
	source = static_cast<plane*>(buffer_map_[index]);
	return source->CopyFromAsynch(host_cols, host_rows, host_pitch, antecedent, event, host_buffer);
}

void buffer_map::Destroy(const int &index) {
	if (! ValidIndex(index)) return;

	// Check reference count
	cl_uint reference_count = 0;
	clGetMemObjectInfo(buffer_map_[index]->obj(),
					   CL_MEM_REFERENCE_COUNT,
					   sizeof(cl_uint),
					   &reference_count,
					   0);

	// Repeatedly release it to ensure it will be destroyed
	for (cl_uint i = 0; i < reference_count; ++i) {
		clReleaseMemObject(buffer_map_[index]->obj());
	}

	buffer_map_.erase(index);
}

void buffer_map::DestroyAll() {
	map<int, mem*>::iterator each_buffer;
	map<int, mem*>::iterator next_buffer;
	each_buffer = buffer_map_.begin();
	while (each_buffer != buffer_map_.end()) {
		next_buffer = each_buffer;
		++next_buffer;
		Destroy(each_buffer->first);
		each_buffer = next_buffer;
	}
	buffer_map_.clear();
}

bool buffer_map::ValidIndex(const int &index) {
	return buffer_map_.count(index) > 0;
}

cl_mem* buffer_map::ptr(const int &index) {
	return buffer_map_[index]->ptr();
}
<|endoftext|>


<commit_before>#include <v8.h>
#include <node.h>
#include <node_buffer.h>

#include <cstring>

using namespace v8;
using namespace node;

namespace {

// this is an application of the Curiously Recurring Template Pattern
template <class T> struct UnaryAction {
	Handle<Value> apply(Buffer& buffer, const Arguments& args);

	Handle<Value> operator()(const Arguments& args) {
		if (args[0]->IsObject()) {
			Local<Object> object = args[0]->ToObject();
			if (Buffer::HasInstance(object)) {
				Buffer& buffer = *ObjectWrap::Unwrap<Buffer>(object);
				return static_cast<T*>(this)->apply(buffer, args);
			}
		}
		static Persistent<String> illegalArgumentException = Persistent<String>::New(String::New("First argument should be a buffer."));
		return ThrowException(Exception::TypeError(illegalArgumentException));
	}
};

template <class T> struct BinaryAction {
	Handle<Value> apply(Buffer& a, Buffer& b, const Arguments& args);

	Handle<Value> operator()(const Arguments& args) {
		if (args[0]->IsObject() && args[1]->IsObject()) {
			Local<Object> arg0 = args[0]->ToObject();
			Local<Object> arg1 = args[0]->ToObject();
			if (Buffer::HasInstance(arg0) && Buffer::HasInstance(arg1)) {
				Buffer& a = *ObjectWrap::Unwrap<Buffer>(arg0);
				Buffer& b = *ObjectWrap::Unwrap<Buffer>(arg1);
				return static_cast<T*>(this)->apply(a, b, args);
			}
		}
		static Persistent<String> illegalArgumentException = Persistent<String>::New(String::New("First and second argument should be a buffer."));
		return ThrowException(Exception::TypeError(illegalArgumentException));
	}
};

//
// helper functions
//
Handle<Value> clear(Buffer& buffer, int c) {
	memset(buffer.data(), c, buffer.length());
	return buffer.handle_;
}

Handle<Value> fill(Buffer& buffer, void* pattern, size_t size) {
	if (size >= buffer.length()) {
		memcpy(buffer.data(), pattern, buffer.length());
	} else {
		const int n_copies = buffer.length() / size;
		const int remainder = buffer.length() % size;
		for (int i = 0; i < n_copies; i++) {
			memcpy(buffer.data() + size * i, pattern, size);
		}
		memcpy(buffer.data() + size * n_copies, pattern, remainder);
	}
	return buffer.handle_;
}

int compare(Buffer& a, Buffer& b) {
	if (a.length() != b.length()) {
		return a.length() > b.length() ? 1 : -1;
	}
	return memcmp(a.data(), b.data(), a.length());
}

//
// actions
//
struct ClearAction: UnaryAction<ClearAction> {
	Handle<Value> apply(Buffer& buffer, const Arguments& args) {
		const int c = args[1]->IsInt32() ? args[1]->ToInt32()->Int32Value() : 0;
		return clear(buffer, c);
	}
};

struct FillAction: UnaryAction<FillAction> {
	Handle<Value> apply(Buffer& buffer, const Arguments& args) {
		if (args[1]->IsInt32()) {
			int c = args[1]->ToInt32()->Int32Value();
			return clear(buffer, c);
		}
		if (args[1]->IsString()) {
			String::AsciiValue s(args[1]->ToString());
			return fill(buffer, *s, s.length());
		}
		if (args[1]->IsObject()) {
			Local<Object> o = args[1]->ToObject();
			if (Buffer::HasInstance(o)) {
				Buffer& src = *Buffer::Unwrap<Buffer>(o);
				return fill(buffer, src.data(), src.length());
			}
		}
		static Persistent<String> illegalArgumentException = Persistent<String>::New(String::New(
				"Second argument should be either a string, a buffer or an integer."));
		return ThrowException(Exception::TypeError(illegalArgumentException));
	}
};

struct EqualsAction: BinaryAction<EqualsAction> {
	Handle<Value> apply(Buffer& a, Buffer& b, const Arguments& args) {
		return compare(a, b) == 0 ? True() : False();
	}
};

struct CompareAction: BinaryAction<CompareAction> {
	Handle<Value> apply(Buffer& a, Buffer& b, const Arguments& args) {
		HandleScope scope;
		return scope.Close(Integer::New(compare(a, b)));
	}
};

//
// V8 function callbacks
//
Handle<Value> Clear(const Arguments& args) {
	return ClearAction()(args);
}

Handle<Value> Fill(const Arguments& args) {
	return FillAction()(args);
}

Handle<Value> Equals(const Arguments& args) {
	return EqualsAction()(args);
}

Handle<Value> Compare(const Arguments& args) {
	return CompareAction()(args);
}

extern "C" void init(Handle<Object> target) {
	HandleScope scope;
	target->Set(String::New("fill"), FunctionTemplate::New(Fill)->GetFunction());
	target->Set(String::New("clear"), FunctionTemplate::New(Clear)->GetFunction());
	target->Set(String::New("equals"), FunctionTemplate::New(Equals)->GetFunction());
	target->Set(String::New("compare"), FunctionTemplate::New(Compare)->GetFunction());
}

}
<commit_msg>Bug fix: wrong array item referenced.<commit_after>#include <v8.h>
#include <node.h>
#include <node_buffer.h>

#include <cstring>

using namespace v8;
using namespace node;

namespace {

// this is an application of the Curiously Recurring Template Pattern
template <class T> struct UnaryAction {
	Handle<Value> apply(Buffer& buffer, const Arguments& args);

	Handle<Value> operator()(const Arguments& args) {
		if (args[0]->IsObject()) {
			Local<Object> object = args[0]->ToObject();
			if (Buffer::HasInstance(object)) {
				Buffer& buffer = *ObjectWrap::Unwrap<Buffer>(object);
				return static_cast<T*>(this)->apply(buffer, args);
			}
		}
		static Persistent<String> illegalArgumentException = Persistent<String>::New(String::New("First argument should be a buffer."));
		return ThrowException(Exception::TypeError(illegalArgumentException));
	}
};

template <class T> struct BinaryAction {
	Handle<Value> apply(Buffer& a, Buffer& b, const Arguments& args);

	Handle<Value> operator()(const Arguments& args) {
		if (args[0]->IsObject() && args[1]->IsObject()) {
			Local<Object> arg0 = args[0]->ToObject();
			Local<Object> arg1 = args[1]->ToObject();
			if (Buffer::HasInstance(arg0) && Buffer::HasInstance(arg1)) {
				Buffer& a = *ObjectWrap::Unwrap<Buffer>(arg0);
				Buffer& b = *ObjectWrap::Unwrap<Buffer>(arg1);
				return static_cast<T*>(this)->apply(a, b, args);
			}
		}
		static Persistent<String> illegalArgumentException = Persistent<String>::New(String::New("First and second argument should be a buffer."));
		return ThrowException(Exception::TypeError(illegalArgumentException));
	}
};

//
// helper functions
//
Handle<Value> clear(Buffer& buffer, int c) {
	memset(buffer.data(), c, buffer.length());
	return buffer.handle_;
}

Handle<Value> fill(Buffer& buffer, void* pattern, size_t size) {
	if (size >= buffer.length()) {
		memcpy(buffer.data(), pattern, buffer.length());
	} else {
		const int n_copies = buffer.length() / size;
		const int remainder = buffer.length() % size;
		for (int i = 0; i < n_copies; i++) {
			memcpy(buffer.data() + size * i, pattern, size);
		}
		memcpy(buffer.data() + size * n_copies, pattern, remainder);
	}
	return buffer.handle_;
}

int compare(Buffer& a, Buffer& b) {
	if (a.length() != b.length()) {
		return a.length() > b.length() ? 1 : -1;
	}
	return memcmp(a.data(), b.data(), a.length());
}

//
// actions
//
struct ClearAction: UnaryAction<ClearAction> {
	Handle<Value> apply(Buffer& buffer, const Arguments& args) {
		const int c = args[1]->IsInt32() ? args[1]->ToInt32()->Int32Value() : 0;
		return clear(buffer, c);
	}
};

struct FillAction: UnaryAction<FillAction> {
	Handle<Value> apply(Buffer& buffer, const Arguments& args) {
		if (args[1]->IsInt32()) {
			int c = args[1]->ToInt32()->Int32Value();
			return clear(buffer, c);
		}
		if (args[1]->IsString()) {
			String::AsciiValue s(args[1]->ToString());
			return fill(buffer, *s, s.length());
		}
		if (args[1]->IsObject()) {
			Local<Object> o = args[1]->ToObject();
			if (Buffer::HasInstance(o)) {
				Buffer& src = *Buffer::Unwrap<Buffer>(o);
				return fill(buffer, src.data(), src.length());
			}
		}
		static Persistent<String> illegalArgumentException = Persistent<String>::New(String::New(
				"Second argument should be either a string, a buffer or an integer."));
		return ThrowException(Exception::TypeError(illegalArgumentException));
	}
};

struct EqualsAction: BinaryAction<EqualsAction> {
	Handle<Value> apply(Buffer& a, Buffer& b, const Arguments& args) {
		return compare(a, b) == 0 ? True() : False();
	}
};

struct CompareAction: BinaryAction<CompareAction> {
	Handle<Value> apply(Buffer& a, Buffer& b, const Arguments& args) {
		HandleScope scope;
		return scope.Close(Integer::New(compare(a, b)));
	}
};

//
// V8 function callbacks
//
Handle<Value> Clear(const Arguments& args) {
	return ClearAction()(args);
}

Handle<Value> Fill(const Arguments& args) {
	return FillAction()(args);
}

Handle<Value> Equals(const Arguments& args) {
	return EqualsAction()(args);
}

Handle<Value> Compare(const Arguments& args) {
	return CompareAction()(args);
}

extern "C" void init(Handle<Object> target) {
	HandleScope scope;
	target->Set(String::New("fill"), FunctionTemplate::New(Fill)->GetFunction());
	target->Set(String::New("clear"), FunctionTemplate::New(Clear)->GetFunction());
	target->Set(String::New("equals"), FunctionTemplate::New(Equals)->GetFunction());
	target->Set(String::New("compare"), FunctionTemplate::New(Compare)->GetFunction());
}

}
<|endoftext|>


<commit_before>/*
 * ---------------------------------------------------------------------
 * Numenta Platform for Intelligent Computing (NuPIC)
 * Copyright (C) 2013, Numenta, Inc.  Unless you have purchased from
 * Numenta, Inc. a separate commercial license for this software code, the
 * following terms and conditions apply:
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 3 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see http://www.gnu.org/licenses.
 *
 * http://numenta.org/licenses/
 * ---------------------------------------------------------------------
 */

#include <nta/types/BasicType.hpp>
#include <nta/engine/YAMLUtils.hpp>
#include <nta/ntypes/Value.hpp>
#include <nta/ntypes/Collection.hpp>
#include <nta/ntypes/MemStream.hpp>
#include <nta/engine/Spec.hpp>
#include <string.h> // strlen
#include <yaml-cpp/yaml.h>

#include <sstream>

namespace nta
{
namespace YAMLUtils
{

/*
 * These functions are used internally by toValue and toValueMap
 */
static void _toScalar(const YAML::Node& node, boost::shared_ptr<Scalar>& s);
static void _toArray(const YAML::Node& node, boost::shared_ptr<Array>& a);
static Value toValue(const YAML::Node& node, NTA_BasicType dataType);

static void _toScalar(const YAML::Node& node, boost::shared_ptr<Scalar>& s)
{
  NTA_CHECK(node.Type() == YAML::NodeType::Scalar);
  switch(s->getType())
  {
  case NTA_BasicType_Byte:
    // We should have already detected this and gone down the string path
    NTA_THROW << "Internal error: attempting to convert YAML string to scalar of type Byte";
    break;
  case NTA_BasicType_UInt16:
    node >> s->value.uint16;
    break;
  case NTA_BasicType_Int16:
    node >> s->value.int16;
    break;
  case NTA_BasicType_UInt32:
    node >> s->value.uint32;
    break;
  case NTA_BasicType_Int32:
    node >> s->value.int32;
    break;
  case NTA_BasicType_UInt64:
    node >> s->value.uint64;
    break;
  case NTA_BasicType_Int64:
    node >> s->value.int64;
    break;
  case NTA_BasicType_Real32:
    node >> s->value.real32;
    break;
  case NTA_BasicType_Real64:
    node >> s->value.real64;
    break;
  case NTA_BasicType_Handle:
    NTA_THROW << "Attempt to specify a YAML value for a scalar of type Handle";
    break;
  default:
    // should not happen
    std::string val;
    node >> val;
    NTA_THROW << "Unknown data type " << s->getType() << " for yaml node '" << val << "'";
  }
}

static void _toArray(const YAML::Node& node, boost::shared_ptr<Array>& a)
{
  NTA_CHECK(node.Type() == YAML::NodeType::Sequence);

  a->allocateBuffer(node.size());
  void* buffer = a->getBuffer();

  for (size_t i = 0; i < node.size(); i++)
  {
    const YAML::Node& item = node[i];
    NTA_CHECK(item.Type() == YAML::NodeType::Scalar);
    switch(a->getType())
    {
    case NTA_BasicType_Byte:
      // We should have already detected this and gone down the string path
      NTA_THROW << "Internal error: attempting to convert YAML string to array of type Byte";
      break;
    case NTA_BasicType_UInt16:
      item.to<UInt16>();
      break;
    case NTA_BasicType_Int16:
      item.to<Int16>();
      break;
    case NTA_BasicType_UInt32:
      item.to<UInt32>();
      break;
    case NTA_BasicType_Int32:
      item.to<Int32>();
      break;
    case NTA_BasicType_UInt64:
      item.to<UInt64>();
      break;
    case NTA_BasicType_Int64:
      item.to<Int64>();
      break;
    case NTA_BasicType_Real32:
      item.to<Real32>();
      break;
    case NTA_BasicType_Real64:
      item.to<Real64>();
      break;
    default:
      // should not happen
      NTA_THROW << "Unknown data type " << a->getType();
    }
  }
}

static Value toValue(const YAML::Node& node, NTA_BasicType dataType)
{
  if (node.Type() == YAML::NodeType::Map || node.Type() == YAML::NodeType::Null)
  {
    NTA_THROW << "YAML string does not not represent a value.";
  }
  if (node.Type() == YAML::NodeType::Scalar)
  {
    if (dataType == NTA_BasicType_Byte)
    {
      // node >> *str;
      std::string val;
      node.Read(val);
      boost::shared_ptr<std::string> str(new std::string(val));
      Value v(str);
      return v;
    } else {
      boost::shared_ptr<Scalar> s(new Scalar(dataType));
      _toScalar(node, s);
      Value v(s);
      return v;
    }
  } else {
    // array
    boost::shared_ptr<Array> a(new Array(dataType));
    _toArray(node, a);
    Value v(a);
    return v;
  }
}

/*
 * For converting default values specified in nodespec
 */
Value toValue(const std::string& yamlstring, NTA_BasicType dataType)
{
  // IMemStream s(yamlstring, ::strlen(yamlstring));

  // yaml-cpp bug: append a space if it is only one character
  // This is very inefficient, but should be ok since it is
  // just used at construction time for short strings
  std::string paddedstring(yamlstring);
  if (paddedstring.size() < 2)
    paddedstring = paddedstring + " ";
  std::stringstream s(paddedstring);

  // TODO -- return value? exceptions?
  bool success = false;
  YAML::Node doc;
  try
  {
    YAML::Parser parser(s);
    success = parser.GetNextDocument(doc);
    // } catch(YAML::ParserException& e) {
  } catch(...) {
    success = false;
  }
  if (!success)
  {
    std::string ys(paddedstring);
    if (ys.size() > 30)
    {
      ys = ys.substr(0, 30) + "...";
    }
    NTA_THROW << "Unable to parse YAML string '" << ys << "' for a scalar value";
  }
  Value v = toValue(doc, dataType);
  return v;
}

/*
 * For converting param specs for Regions and LinkPolicies
 */
ValueMap toValueMap(const char* yamlstring,
                               Collection<ParameterSpec>& parameters,
                               const std::string & nodeType,
                               const std::string & regionName)
{

  ValueMap vm;

  // yaml-cpp bug: append a space if it is only one character
  // This is very inefficient, but should be ok since it is
  // just used at construction time for short strings
  std::string paddedstring(yamlstring);
  // TODO: strip white space to determine if empty
  bool empty = (paddedstring.size() == 0);

  if (paddedstring.size() < 2)
    paddedstring = paddedstring + " ";
  std::stringstream s(paddedstring);
  // IMemStream s(yamlstring, ::strlen(yamlstring));

  // TODO: utf-8 compatible?
  YAML::Node doc;
  if (!empty)
  {
    YAML::Parser parser(s);
    bool success = parser.GetNextDocument(doc);

    if (!success)
      NTA_THROW << "Unable to find document in YAML string";

    // A ValueMap is specified as a dictionary
    if (doc.Type() != YAML::NodeType::Map)
    {
      std::string ys(yamlstring);
      if (ys.size() > 30)
      {
        ys = ys.substr(0, 30) + "...";
      }
      NTA_THROW << "YAML string '" << ys
                << "' does not not specify a dictionary of key-value pairs. "
                << "Region and Link parameters must be specified at a dictionary";
    }
  }

  // Grab each value out of the YAML dictionary and put into the ValueMap
  // if it is allowed by the nodespec.
  YAML::Iterator i;
  for (i = doc.begin(); i != doc.end(); i++)
  {
    const std::string key = i.first().to<std::string>();
    if (!parameters.contains(key))
    {
      std::stringstream ss;
      for (UInt j = 0; j < parameters.getCount(); j++)
      {
        ss << "   " << parameters.getByIndex(j).first << "\n";
      }

      if (nodeType == std::string(""))
      {
        NTA_THROW << "Unknown parameter '" << key << "'\n"
                  << "Valid parameters are:\n" << ss.str();
      }
      else
      {
        NTA_CHECK(regionName != std::string(""));
        NTA_THROW << "Unknown parameter '" << key << "' for region '"
                  << regionName << "' of type '" << nodeType << "'\n"
                  << "Valid parameters are:\n" << ss.str();
      }
    }
    if (vm.contains(key))
      NTA_THROW << "Parameter '" << key << "' specified more than once in YAML document";
    ParameterSpec spec = parameters.getByName(key);
    try
    {
      Value v = toValue(i.second(), spec.dataType);
      if (v.isScalar() && spec.count != 1)
      {
        throw std::runtime_error("Expected array value but got scalar value");
      }
      if (!v.isScalar() && spec.count == 1)
      {
        throw std::runtime_error("Expected scalar value but got array value");
      }
      vm.add(key, v);
    } catch (std::runtime_error& e) {
      NTA_THROW << "Unable to set parameter '" << key << "'. " << e.what();
    }
  }

  // Populate ValueMap with default values if they were not specified in the YAML dictionary.
  for (size_t i = 0; i < parameters.getCount(); i++)
  {
    std::pair<std::string, ParameterSpec>& item = parameters.getByIndex(i);
    if (!vm.contains(item.first))
    {
      ParameterSpec & ps = item.second;
      if (ps.defaultValue != "")
      {
        // TODO: This check should be uncommented after dropping NuPIC 1.x nodes (which don't comply)
        // if (ps.accessMode != ParameterSpec::CreateAccess)
        // {
        //   NTA_THROW << "Default value for non-create parameter: " << item.first;
        // }

        try {
#ifdef YAMLDEBUG
          NTA_DEBUG << "Adding default value '" << ps.defaultValue
                    << "' to parameter " << item.first
                    << " of type " << BasicType::getName(ps.dataType)
                    << " count " << ps.count;
#endif
          Value v = toValue(ps.defaultValue, ps.dataType);
          vm.add(item.first, v);
        } catch (...) {
          NTA_THROW << "Unable to set default value for item '"
                    << item.first << "' of datatype "
                    << BasicType::getName(ps.dataType)
                    <<" with value '" << ps.defaultValue << "'";
        }
      }
    }
  }

  return vm;
}

} // end of YAMLUtils namespace

} // end of nta namespace

<commit_msg>yaml-cpp: partially revert : API - use Read()<commit_after>/*
 * ---------------------------------------------------------------------
 * Numenta Platform for Intelligent Computing (NuPIC)
 * Copyright (C) 2013, Numenta, Inc.  Unless you have purchased from
 * Numenta, Inc. a separate commercial license for this software code, the
 * following terms and conditions apply:
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 3 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see http://www.gnu.org/licenses.
 *
 * http://numenta.org/licenses/
 * ---------------------------------------------------------------------
 */

#include <nta/types/BasicType.hpp>
#include <nta/engine/YAMLUtils.hpp>
#include <nta/ntypes/Value.hpp>
#include <nta/ntypes/Collection.hpp>
#include <nta/ntypes/MemStream.hpp>
#include <nta/engine/Spec.hpp>
#include <string.h> // strlen
#include <yaml-cpp/yaml.h>

#include <sstream>

namespace nta
{
namespace YAMLUtils
{

/*
 * These functions are used internally by toValue and toValueMap
 */
static void _toScalar(const YAML::Node& node, boost::shared_ptr<Scalar>& s);
static void _toArray(const YAML::Node& node, boost::shared_ptr<Array>& a);
static Value toValue(const YAML::Node& node, NTA_BasicType dataType);

static void _toScalar(const YAML::Node& node, boost::shared_ptr<Scalar>& s)
{
  NTA_CHECK(node.Type() == YAML::NodeType::Scalar);
  switch(s->getType())
  {
  case NTA_BasicType_Byte:
    // We should have already detected this and gone down the string path
    NTA_THROW << "Internal error: attempting to convert YAML string to scalar of type Byte";
    break;
  case NTA_BasicType_UInt16:
    node >> s->value.uint16;
    break;
  case NTA_BasicType_Int16:
    node >> s->value.int16;
    break;
  case NTA_BasicType_UInt32:
    node >> s->value.uint32;
    break;
  case NTA_BasicType_Int32:
    node >> s->value.int32;
    break;
  case NTA_BasicType_UInt64:
    node >> s->value.uint64;
    break;
  case NTA_BasicType_Int64:
    node >> s->value.int64;
    break;
  case NTA_BasicType_Real32:
    node >> s->value.real32;
    break;
  case NTA_BasicType_Real64:
    node >> s->value.real64;
    break;
  case NTA_BasicType_Handle:
    NTA_THROW << "Attempt to specify a YAML value for a scalar of type Handle";
    break;
  default:
    // should not happen
    std::string val;
    node >> val;
    NTA_THROW << "Unknown data type " << s->getType() << " for yaml node '" << val << "'";
  }
}

static void _toArray(const YAML::Node& node, boost::shared_ptr<Array>& a)
{
  NTA_CHECK(node.Type() == YAML::NodeType::Sequence);

  a->allocateBuffer(node.size());
  void* buffer = a->getBuffer();

  for (size_t i = 0; i < node.size(); i++)
  {
    const YAML::Node& item = node[i];
    NTA_CHECK(item.Type() == YAML::NodeType::Scalar);
    switch(a->getType())
    {
    case NTA_BasicType_Byte:
      // We should have already detected this and gone down the string path
      NTA_THROW << "Internal error: attempting to convert YAML string to array of type Byte";
      break;
    case NTA_BasicType_UInt16:
      item.Read<UInt16>(((UInt16*)buffer)[i]);
      break;
    case NTA_BasicType_Int16:
      item.Read<Int16>(((Int16*)buffer)[i]);
      break;
    case NTA_BasicType_UInt32:
      item.Read<UInt32>(((UInt32*)buffer)[i]);
      break;
    case NTA_BasicType_Int32:
      item.Read<Int32>(((Int32*)buffer)[i]);
      break;
    case NTA_BasicType_UInt64:
      item.Read<UInt64>(((UInt64*)buffer)[i]);
      break;
    case NTA_BasicType_Int64:
      item.Read<Int64>(((Int64*)buffer)[i]);
      break;
    case NTA_BasicType_Real32:
      item.Read<Real32>(((Real32*)buffer)[i]);
      break;
    case NTA_BasicType_Real64:
      item.Read<Real64>(((Real64*)buffer)[i]);
      break;
    default:
      // should not happen
      NTA_THROW << "Unknown data type " << a->getType();
    }
  }
}

static Value toValue(const YAML::Node& node, NTA_BasicType dataType)
{
  if (node.Type() == YAML::NodeType::Map || node.Type() == YAML::NodeType::Null)
  {
    NTA_THROW << "YAML string does not not represent a value.";
  }
  if (node.Type() == YAML::NodeType::Scalar)
  {
    if (dataType == NTA_BasicType_Byte)
    {
      // node >> *str;
      std::string val;
      node.Read(val);
      boost::shared_ptr<std::string> str(new std::string(val));
      Value v(str);
      return v;
    } else {
      boost::shared_ptr<Scalar> s(new Scalar(dataType));
      _toScalar(node, s);
      Value v(s);
      return v;
    }
  } else {
    // array
    boost::shared_ptr<Array> a(new Array(dataType));
    _toArray(node, a);
    Value v(a);
    return v;
  }
}

/*
 * For converting default values specified in nodespec
 */
Value toValue(const std::string& yamlstring, NTA_BasicType dataType)
{
  // IMemStream s(yamlstring, ::strlen(yamlstring));

  // yaml-cpp bug: append a space if it is only one character
  // This is very inefficient, but should be ok since it is
  // just used at construction time for short strings
  std::string paddedstring(yamlstring);
  if (paddedstring.size() < 2)
    paddedstring = paddedstring + " ";
  std::stringstream s(paddedstring);

  // TODO -- return value? exceptions?
  bool success = false;
  YAML::Node doc;
  try
  {
    YAML::Parser parser(s);
    success = parser.GetNextDocument(doc);
    // } catch(YAML::ParserException& e) {
  } catch(...) {
    success = false;
  }
  if (!success)
  {
    std::string ys(paddedstring);
    if (ys.size() > 30)
    {
      ys = ys.substr(0, 30) + "...";
    }
    NTA_THROW << "Unable to parse YAML string '" << ys << "' for a scalar value";
  }
  Value v = toValue(doc, dataType);
  return v;
}

/*
 * For converting param specs for Regions and LinkPolicies
 */
ValueMap toValueMap(const char* yamlstring,
                               Collection<ParameterSpec>& parameters,
                               const std::string & nodeType,
                               const std::string & regionName)
{

  ValueMap vm;

  // yaml-cpp bug: append a space if it is only one character
  // This is very inefficient, but should be ok since it is
  // just used at construction time for short strings
  std::string paddedstring(yamlstring);
  // TODO: strip white space to determine if empty
  bool empty = (paddedstring.size() == 0);

  if (paddedstring.size() < 2)
    paddedstring = paddedstring + " ";
  std::stringstream s(paddedstring);
  // IMemStream s(yamlstring, ::strlen(yamlstring));

  // TODO: utf-8 compatible?
  YAML::Node doc;
  if (!empty)
  {
    YAML::Parser parser(s);
    bool success = parser.GetNextDocument(doc);

    if (!success)
      NTA_THROW << "Unable to find document in YAML string";

    // A ValueMap is specified as a dictionary
    if (doc.Type() != YAML::NodeType::Map)
    {
      std::string ys(yamlstring);
      if (ys.size() > 30)
      {
        ys = ys.substr(0, 30) + "...";
      }
      NTA_THROW << "YAML string '" << ys
                << "' does not not specify a dictionary of key-value pairs. "
                << "Region and Link parameters must be specified at a dictionary";
    }
  }

  // Grab each value out of the YAML dictionary and put into the ValueMap
  // if it is allowed by the nodespec.
  YAML::Iterator i;
  for (i = doc.begin(); i != doc.end(); i++)
  {
    const std::string key = i.first().to<std::string>();
    if (!parameters.contains(key))
    {
      std::stringstream ss;
      for (UInt j = 0; j < parameters.getCount(); j++)
      {
        ss << "   " << parameters.getByIndex(j).first << "\n";
      }

      if (nodeType == std::string(""))
      {
        NTA_THROW << "Unknown parameter '" << key << "'\n"
                  << "Valid parameters are:\n" << ss.str();
      }
      else
      {
        NTA_CHECK(regionName != std::string(""));
        NTA_THROW << "Unknown parameter '" << key << "' for region '"
                  << regionName << "' of type '" << nodeType << "'\n"
                  << "Valid parameters are:\n" << ss.str();
      }
    }
    if (vm.contains(key))
      NTA_THROW << "Parameter '" << key << "' specified more than once in YAML document";
    ParameterSpec spec = parameters.getByName(key);
    try
    {
      Value v = toValue(i.second(), spec.dataType);
      if (v.isScalar() && spec.count != 1)
      {
        throw std::runtime_error("Expected array value but got scalar value");
      }
      if (!v.isScalar() && spec.count == 1)
      {
        throw std::runtime_error("Expected scalar value but got array value");
      }
      vm.add(key, v);
    } catch (std::runtime_error& e) {
      NTA_THROW << "Unable to set parameter '" << key << "'. " << e.what();
    }
  }

  // Populate ValueMap with default values if they were not specified in the YAML dictionary.
  for (size_t i = 0; i < parameters.getCount(); i++)
  {
    std::pair<std::string, ParameterSpec>& item = parameters.getByIndex(i);
    if (!vm.contains(item.first))
    {
      ParameterSpec & ps = item.second;
      if (ps.defaultValue != "")
      {
        // TODO: This check should be uncommented after dropping NuPIC 1.x nodes (which don't comply)
        // if (ps.accessMode != ParameterSpec::CreateAccess)
        // {
        //   NTA_THROW << "Default value for non-create parameter: " << item.first;
        // }

        try {
#ifdef YAMLDEBUG
          NTA_DEBUG << "Adding default value '" << ps.defaultValue
                    << "' to parameter " << item.first
                    << " of type " << BasicType::getName(ps.dataType)
                    << " count " << ps.count;
#endif
          Value v = toValue(ps.defaultValue, ps.dataType);
          vm.add(item.first, v);
        } catch (...) {
          NTA_THROW << "Unable to set default value for item '"
                    << item.first << "' of datatype "
                    << BasicType::getName(ps.dataType)
                    <<" with value '" << ps.defaultValue << "'";
        }
      }
    }
  }

  return vm;
}

} // end of YAMLUtils namespace

} // end of nta namespace

<|endoftext|>


<commit_before>#include "positiontracker.h"
#include <cmath>
#include <common/config/configmanager.h>

PositionTracker::PositionTracker()
    : LOnGPSData(this),
      LOnMotionCommand(this),
      LOnIMUData(this)
{
}

Position PositionTracker::GetPosition()
{
    return _current_estimate;
}

Position PositionTracker::UpdateWithMeasurement(Position S, Position Measurement)
{
    Position result;
    result.Latitude.SetValue( ( S.Latitude.Value()*Measurement.Latitude.Variance() + Measurement.Latitude.Value()*S.Latitude.Variance() ) / (S.Latitude.Variance()+Measurement.Latitude.Variance()) );
    result.Longitude.SetValue( ( S.Longitude.Value()*Measurement.Longitude.Variance() + Measurement.Longitude.Value()*S.Longitude.Variance() ) / (S.Longitude.Variance()+Measurement.Longitude.Variance()) );
    result.Heading.SetValue( ( S.Heading.Value()*Measurement.Heading.Variance() + Measurement.Heading.Value()*S.Heading.Variance() ) / (S.Heading.Variance()+Measurement.Heading.Variance()) );

    result.Latitude.SetVariance( 1.0 / (1.0/S.Latitude.Variance() + 1.0 / Measurement.Latitude.Variance()) );
    result.Longitude.SetVariance( 1.0 / (1.0/S.Longitude.Variance() + 1.0 / Measurement.Longitude.Variance()) );
    result.Heading.SetVariance( 1.0 / (1.0/S.Heading.Variance() + 1.0 / Measurement.Heading.Variance()) );
    return result;
}

Position PositionTracker::UpdateWithMotion(Position S, Position Delta)
{
    Position result;
    result.Latitude.SetValue(S.Latitude.Value() + Delta.Latitude.Value());
    result.Longitude.SetValue(S.Longitude.Value() + Delta.Longitude.Value());
    result.Heading.SetValue(S.Heading.Value() + Delta.Heading.Value());

    result.Latitude.SetVariance(S.Latitude.Variance() + Delta.Latitude.Variance());
    result.Longitude.SetVariance(S.Longitude.Variance() + Delta.Longitude.Variance());
    result.Heading.SetVariance(S.Heading.Variance() + Delta.Heading.Variance());
    return result;
}

Position PositionTracker::DeltaFromMotionCommand(MotorCommand cmd)
{
    using namespace std;
    double V = ( cmd.rightVel + cmd.leftVel ) / 2.0;
    double W = ( cmd.rightVel - cmd.leftVel ) / ConfigManager::getValue("Robot", "Baseline", 1.0);
    double t = cmd.millis / 1000.0;
    double R = V/W;
    Position delta;
    double theta = _current_estimate.Heading.Value() * M_PI/180.0;
    double thetaPrime = (_current_estimate.Heading.Value() + W*t) * M_PI/180.0;
    delta.Heading.SetValue(W*t);
    delta.Latitude.SetValue(R*(cos(theta) - cos(thetaPrime)));
    delta.Longitude.SetValue(R*(sin(thetaPrime) - sin(theta)));
    // TODO - handle variances
    return delta;
}

void PositionTracker::OnGPSData(GPSData data)
{
    Position measurement;
    measurement.Latitude.SetValue(data.Lat());
    measurement.Longitude.SetValue(data.Long());
    // TODO - make sure the GPS actually outputs heading data
    measurement.Heading.SetValue(data.Heading());
    // TODO - handle variances
    _current_estimate = UpdateWithMeasurement(_current_estimate, measurement);
}

void PositionTracker::OnIMUData(IMUData data)
{

}

void PositionTracker::OnMotionCommand(MotorCommand cmd)
{
    _current_estimate = UpdateWithMotion(_current_estimate, DeltaFromMotionCommand(cmd));
}
<commit_msg>Fixes bug in omega calculation.<commit_after>#include "positiontracker.h"
#include <cmath>
#include <common/config/configmanager.h>

PositionTracker::PositionTracker()
    : LOnGPSData(this),
      LOnMotionCommand(this),
      LOnIMUData(this)
{
}

Position PositionTracker::GetPosition()
{
    return _current_estimate;
}

Position PositionTracker::UpdateWithMeasurement(Position S, Position Measurement)
{
    Position result;
    result.Latitude.SetValue( ( S.Latitude.Value()*Measurement.Latitude.Variance() + Measurement.Latitude.Value()*S.Latitude.Variance() ) / (S.Latitude.Variance()+Measurement.Latitude.Variance()) );
    result.Longitude.SetValue( ( S.Longitude.Value()*Measurement.Longitude.Variance() + Measurement.Longitude.Value()*S.Longitude.Variance() ) / (S.Longitude.Variance()+Measurement.Longitude.Variance()) );
    result.Heading.SetValue( ( S.Heading.Value()*Measurement.Heading.Variance() + Measurement.Heading.Value()*S.Heading.Variance() ) / (S.Heading.Variance()+Measurement.Heading.Variance()) );

    result.Latitude.SetVariance( 1.0 / (1.0/S.Latitude.Variance() + 1.0 / Measurement.Latitude.Variance()) );
    result.Longitude.SetVariance( 1.0 / (1.0/S.Longitude.Variance() + 1.0 / Measurement.Longitude.Variance()) );
    result.Heading.SetVariance( 1.0 / (1.0/S.Heading.Variance() + 1.0 / Measurement.Heading.Variance()) );
    return result;
}

Position PositionTracker::UpdateWithMotion(Position S, Position Delta)
{
    Position result;
    result.Latitude.SetValue(S.Latitude.Value() + Delta.Latitude.Value());
    result.Longitude.SetValue(S.Longitude.Value() + Delta.Longitude.Value());
    result.Heading.SetValue(S.Heading.Value() + Delta.Heading.Value());

    result.Latitude.SetVariance(S.Latitude.Variance() + Delta.Latitude.Variance());
    result.Longitude.SetVariance(S.Longitude.Variance() + Delta.Longitude.Variance());
    result.Heading.SetVariance(S.Heading.Variance() + Delta.Heading.Variance());
    return result;
}

Position PositionTracker::DeltaFromMotionCommand(MotorCommand cmd)
{
    using namespace std;
    double V = ( cmd.rightVel + cmd.leftVel ) / 2.0;
    double W = ( cmd.rightVel - cmd.leftVel ) / ConfigManager::Instance().getValue("Robot", "Baseline", 1.0);
    double t = cmd.millis / 1000.0;
    double R = V/W;
    Position delta;
    double theta = _current_estimate.Heading.Value() * M_PI/180.0;
    double thetaPrime = (_current_estimate.Heading.Value() + W*t) * M_PI/180.0;
    delta.Heading.SetValue(W*t);
    delta.Latitude.SetValue(R*(cos(theta) - cos(thetaPrime)));
    delta.Longitude.SetValue(R*(sin(thetaPrime) - sin(theta)));
    // TODO - handle variances
    return delta;
}

void PositionTracker::OnGPSData(GPSData data)
{
    Position measurement;
    measurement.Latitude.SetValue(data.Lat());
    measurement.Longitude.SetValue(data.Long());
    // TODO - make sure the GPS actually outputs heading data
    measurement.Heading.SetValue(data.Heading());
    // TODO - handle variances
    _current_estimate = UpdateWithMeasurement(_current_estimate, measurement);
}

void PositionTracker::OnIMUData(IMUData data)
{

}

void PositionTracker::OnMotionCommand(MotorCommand cmd)
{
    _current_estimate = UpdateWithMotion(_current_estimate, DeltaFromMotionCommand(cmd));
}
<|endoftext|>