Interpolator2D.h

Go to the documentation of this file.

// **************************************************************************
// **************************************************************************
#ifndef FACT_Interpolator2D
#define FACT_Interpolator2D

#include <float.h>
#include <math.h>
#include <vector>
#include <fstream>

class Interpolator2D
{
public:
    struct vec
    {
        double x;
        double y;

        vec(double _x=0, double _y=0) : x(_x), y(_y) { }

        vec orto() const { return vec(-y, x); }

        double dist(const vec &v) const { return hypot(x-v.x, y-v.y); }
        double operator^(const vec &v) const { return x*v.y - y*v.x; }
        vec operator-(const vec &v) const { return vec(x-v.x, y-v.y); }
        vec operator+(const vec &v) const { return vec(x+v.x, y+v.y); }
        vec operator/(double b)     const { return vec(x/b, y/b); }
    };


    struct point : vec
    {
        unsigned int i;
        point(unsigned int _i, const vec &_v) : vec(_v), i(_i) { }
        point(unsigned int _i=0, double _x=0, double _y=0) : vec(_x, _y), i(_i) { }
    };

    struct circle : point
    {
        point p[3];
        double r;

        static bool sameSide(const vec &p1, const vec &p2, const vec &a, const vec &b)
        {
            return ((b-a)^(p1-a))*((b-a)^(p2-a)) > 0;
        }

        bool isInsideTriangle(const vec &v) const
        {
            return sameSide(v, p[0], p[1], p[2]) && sameSide(v, p[1], p[0], p[2]) && sameSide(v, p[2], p[0], p[1]);
        }

        bool isInsideCircle(const vec &v) const
        {
            return dist(v) < r;
        }
    };

    struct weight : point
    {
        circle c;
        double w[3];
    };

private:
    std::vector<point>  inputGrid;   
    std::vector<point>  outputGrid;  
    std::vector<circle> circles;     
    std::vector<weight> weights;     

    // --------------------------------------------------------------------------
    //
    //
    void CalculateGrid()
    {
        circles.reserve(2*inputGrid.size());

        // Loop over all triplets of points
        for (auto it0=inputGrid.cbegin(); it0<inputGrid.cend(); it0++)
        {
            for (auto it1=inputGrid.cbegin(); it1<it0; it1++)
            {
                for (auto it2=inputGrid.cbegin(); it2<it1; it2++)
                {
                    // Calculate the circle through the three points

                    // Vectors along the side of the corresponding triangle
                    const vec v1 = *it1 - *it0;
                    const vec v2 = *it2 - *it1;

                    // Orthogonal vectors on the sides
                    const vec n1 = v1.orto();
                    const vec n2 = v2.orto();

                    // Center point of two of the three sides
                    const vec p1 = (*it0 + *it1)/2;
                    const vec p2 = (*it1 + *it2)/2;

                    // Calculate the crossing point of the two
                    // orthogonal vectors originating in the
                    // center of the sides.
                    const double denom = n1^n2;
                    if (denom==0)
                        continue;

                    const vec x(n1.x, n2.x);
                    const vec y(n1.y, n2.y);

                    const vec w(p1^(p1+n1), p2^(p2+n2));

                    circle c;

                    // This is the x and y coordinate of the circle
                    // through the three points and the circle's radius.
                    c.x = (x^w)/denom;
                    c.y = (y^w)/denom;
                    c.r = c.dist(*it1);

                    // Check if any other grid point lays within this circle
                    auto it3 = inputGrid.cbegin();
                    for (; it3<inputGrid.cend(); it3++)
                    {
                        if (it3==it0 || it3==it1 || it3==it2)
                            continue;

                        if (c.isInsideCircle(*it3))
                            break;
                    }

                    // If a point was found inside, reject the circle
                    if (it3!=inputGrid.cend())
                        continue;

                    // Store the three points of the triangle
                    c.p[0] = *it0;
                    c.p[1] = *it1;
                    c.p[2] = *it2;

                    // Keep in list
                    circles.push_back(c);
                }
            }
        }
    }

    // --------------------------------------------------------------------------
    //
    //
    bool CalculateWeights()
    {
        weights.reserve(outputGrid.size());

        // Loop over all points in the output grid
        for (auto ip=outputGrid.cbegin(); ip<outputGrid.cend(); ip++)
        {
            double mindd = DBL_MAX;

            auto mint = circles.cend();
            auto minc = circles.cend();
            auto mind = circles.cend();

            for (auto ic=circles.cbegin(); ic<circles.cend(); ic++)
            {
                // Check if point is inside the triangle
                if (ic->isInsideTriangle(*ip))
                {
                    if (mint==circles.cend() || ic->r<mint->r)
                        mint = ic;
                }

                // If we have found such a triangle, no need to check for more
                if (mint!=circles.cend())
                    continue;

                // maybe at least inside the circle
                const double dd = ic->dist(*ip);
                if (dd<ic->r)
                {
                    if (minc==circles.cend() || ic->r<minc->r)
                        minc = ic;
                }

                // If we found such a circle, no need to check for more
                if (minc!=circles.cend())
                    continue;

                // then look for the closest circle center
                if (dd<mindd)
                {
                    mindd = dd;
                    mind  = ic;
                }
            }

            // Choose the best of the three options
            const auto it = mint==circles.cend() ? (minc==circles.cend() ? mind : minc) : mint;
            if (it==circles.cend())
                return false;

            // Calculate the bi-linear interpolation
            const vec &p1 = it->p[0];
            const vec &p2 = it->p[1];
            const vec &p3 = it->p[2];

            const double dy23 = p2.y - p3.y;
            const double dy31 = p3.y - p1.y;
            const double dy12 = p1.y - p2.y;

            const double dx32 = p3.x - p2.x;
            const double dx13 = p1.x - p3.x;
            const double dx21 = p2.x - p1.x;

            const double dxy23 = p2^p3;
            const double dxy31 = p3^p1;
            const double dxy12 = p1^p2;

            const double det = dxy12 + dxy23 + dxy31;

            const double w1 = (dy23*ip->x + dx32*ip->y + dxy23)/det;
            const double w2 = (dy31*ip->x + dx13*ip->y + dxy31)/det;
            const double w3 = (dy12*ip->x + dx21*ip->y + dxy12)/det;

            // Store the original grid-point, the circle's parameters
            // and the calculate weights
            weight w;
            w.x = ip->x;
            w.y = ip->y;
            w.c = *it;
            w.w[0] = w1;
            w.w[1] = w2;
            w.w[2] = w3;

            weights.push_back(w);
        }

        return true;
    }

public:
    // --------------------------------------------------------------------------
    //
    //
    Interpolator2D()
    {
    }

    // --------------------------------------------------------------------------
    //
    //
    Interpolator2D(int n, double *x, double *y)
    {
        SetInputGrid(n, x, y);
    }

    Interpolator2D(const std::vector<Interpolator2D::vec> &v)
    {
        SetInputGrid(v);
    }

    const std::vector<Interpolator2D::weight> getWeights() const { return weights; }
    const std::vector<Interpolator2D::point>  getInputGrid() const { return inputGrid; }
    const std::vector<Interpolator2D::point>  getOutputGrid() const { return outputGrid; }

    // --------------------------------------------------------------------------
    //
    //
    static std::vector<Interpolator2D::vec> ReadGrid(const std::string &filename)
    {
        std::vector<Interpolator2D::vec> grid;

        std::ifstream fin(filename);
        if (!fin.is_open())
            return std::vector<Interpolator2D::vec>();

        while (1)
        {
            double x, y;
            fin >> x;
            fin >> y;
            if (!fin)
                break;

            grid.emplace_back(x, y);
        }

        return fin.bad() ? std::vector<Interpolator2D::vec>() : grid;
    }

    // --------------------------------------------------------------------------
    //
    //
    void SetInputGrid(unsigned int n, double *x, double *y)
    {
        circles.clear();
        weights.clear();
        outputGrid.clear();

        inputGrid.clear();
        inputGrid.reserve(n);
        for (unsigned int i=0; i<n; i++)
            inputGrid.emplace_back(i, x[i], y[i]);

        CalculateGrid();
    }

    void SetInputGrid(const std::vector<Interpolator2D::vec> &v)
    {
        circles.clear();
        weights.clear();
        outputGrid.clear();

        inputGrid.clear();
        inputGrid.reserve(v.size());
        for (unsigned int i=0; i<v.size(); i++)
            inputGrid.emplace_back(i, v[i]);

        CalculateGrid();
    }

    /*
    void SetInputGrid(const std::vector<Interpolator2D::point> &v)
    {
        circles.clear();
        weights.clear();
        outputGrid.clear();

        inputGrid.clear();
        inputGrid.reserve(v.size());
        for (unsigned int i=0; i<v.size(); i++)
            inputGrid.emplace_back(v[i], i);

        CalculateGrid();
    }*/

    bool ReadInputGrid(const std::string &filename)
    {
        const auto grid = ReadGrid(filename);
        if (grid.empty())
            return false;

        SetInputGrid(grid);
        return true;
    }


    // --------------------------------------------------------------------------
    //
    //
    bool SetOutputGrid(std::size_t n, double *x, double *y)
    {
        if (inputGrid.empty() && n==0)
            return false;

        weights.clear();

        outputGrid.clear();
        outputGrid.reserve(n);
        for (std::size_t i=0; i<n; i++)
            outputGrid.emplace_back(i, x[i], y[i]);

        return CalculateWeights();
    }

    /*
    bool SetOutputGrid(const std::vector<std::pair<double,double>> &v)
    {
        if (inputGrid.empty() || v.empty())
            return false;

        weights.clear();

        outputGrid.clear();
        outputGrid.reserve(v.size());
        for (std::size_t i=0; i<v.size(); i++)
            outputGrid.emplace_back(i, v[i].first, v[i].second);

        return CalculateWeights();
    }*/

    bool SetOutputGrid(const std::vector<Interpolator2D::vec> &v)
    {
        if (inputGrid.empty())
            return false;

        weights.clear();

        outputGrid.clear();
        outputGrid.reserve(v.size());
        for (std::size_t i=0; i<v.size(); i++)
            outputGrid.emplace_back(i, v[i]);

        return CalculateWeights();
    }

    bool ReadOutputGrid(const std::string &filename)
    {
        const auto grid = ReadGrid(filename);
        if (grid.empty())
            return false;

        return SetOutputGrid(grid);
    }


    // --------------------------------------------------------------------------
    //
    //
    std::vector<double> Interpolate(const std::vector<double> &z) const
    {
        if (z.size()!=inputGrid.size())
            return std::vector<double>();

        std::vector<double> rc;
        rc.reserve(z.size());

        for (auto it=weights.cbegin(); it<weights.cend(); it++)
            rc.push_back(z[it->c.p[0].i] * it->w[0] + z[it->c.p[1].i] * it->w[1] + z[it->c.p[2].i] * it->w[2]);

        return rc;
    }
};
#endif