GteIntpAkimaUniform2.h

Go to the documentation of this file.

// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2017
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)

#pragma once

#include <LowLevel/GteArray2.h>
#include <LowLevel/GteLogger.h>
#include <algorithm>
#include <cmath>
#include <cstring>

// The interpolator is for uniformly spaced (x,y)-values.  The input samples
// F must be stored in row-major order to represent f(x,y); that is,
// F[c + xBound*r] corresponds to f(x,y), where c is the index corresponding
// to x and r is the index corresponding to y.

namespace gte
{

template <typename Real>
class IntpAkimaUniform2
{
public:
    // Construction and destruction.
    ~IntpAkimaUniform2();
    IntpAkimaUniform2(int xBound, int yBound, Real xMin, Real xSpacing,
        Real yMin, Real ySpacing, Real const* F);

    // Member access.
    inline int GetXBound() const;
    inline int GetYBound() const;
    inline int GetQuantity() const;
    inline Real const* GetF() const;
    inline Real GetXMin() const;
    inline Real GetXMax() const;
    inline Real GetXSpacing() const;
    inline Real GetYMin() const;
    inline Real GetYMax() const;
    inline Real GetYSpacing() const;

    // Evaluate the function and its derivatives.  The functions clamp the
    // inputs to xmin <= x <= xmax and ymin <= y <= ymax.  The first operator
    // is for function evaluation.  The second operator is for function or
    // derivative evaluations.  The xOrder argument is the order of the
    // x-derivative and the yOrder argument is the order of the y-derivative.
    // Both orders are zero to get the function value itself.
    Real operator()(Real x, Real y) const;
    Real operator()(int xOrder, int yOrder, Real x, Real y) const;

private:
    class Polynomial
    {
    public:
        Polynomial();

        // P(x,y) = (1,x,x^2,x^3)*A*(1,y,y^2,y^3).  The matrix term A[ix][iy]
        // corresponds to the polynomial term x^{ix} y^{iy}.
        Real& A(int ix, int iy);
        Real operator()(Real x, Real y) const;
        Real operator()(int xOrder, int yOrder, Real x, Real y) const;

    private:
        Real mCoeff[4][4];
    };

    // Support for construction.
    void GetFX(Array2<Real> const& F, Array2<Real>& FX);
    void GetFY(Array2<Real> const& F, Array2<Real>& FY);
    void GetFXY(Array2<Real> const& F, Array2<Real>& FXY);
    void GetPolynomials(Array2<Real> const& F, Array2<Real> const& FX,
        Array2<Real> const& FY, Array2<Real> const& FXY);
    Real ComputeDerivative(Real const* slope) const;
    void Construct(Polynomial& poly, Real const F[2][2], Real const FX[2][2],
        Real const FY[2][2], Real const FXY[2][2]);

    // Support for evaluation.
    void XLookup(Real x, int& xIndex, Real& dx) const;
    void YLookup(Real y, int& yIndex, Real& dy) const;

    int mXBound, mYBound, mQuantity;
    Real mXMin, mXMax, mXSpacing;
    Real mYMin, mYMax, mYSpacing;
    Real const* mF;
    Array2<Polynomial> mPoly;
};


template <typename Real>
IntpAkimaUniform2<Real>::~IntpAkimaUniform2()
{
}

template <typename Real>
IntpAkimaUniform2<Real>::IntpAkimaUniform2(int xBound, int yBound, Real xMin,
    Real xSpacing, Real yMin, Real ySpacing, Real const* F)
    :
    mXBound(xBound),
    mYBound(yBound),
    mQuantity(xBound * yBound),
    mXMin(xMin),
    mXSpacing(xSpacing),
    mYMin(yMin),
    mYSpacing(ySpacing),
    mF(F),
    mPoly(xBound - 1, mYBound - 1)
{
    // At least a 3x3 block of data points is needed to construct the
    // estimates of the boundary derivatives.
    LogAssert(mXBound >= 3 && mYBound >= 3 && mF, "Invalid input.");
    LogAssert(mXSpacing > (Real)0 && mYSpacing > (Real)0, "Invalid input.");

    mXMax = mXMin + mXSpacing * static_cast<Real>(mXBound - 1);
    mYMax = mYMin + mYSpacing * static_cast<Real>(mYBound - 1);

    // Create a 2D wrapper for the 1D samples.
    Array2<Real> Fmap(mXBound, mYBound, const_cast<Real*>(mF));

    // Construct first-order derivatives.
    Array2<Real> FX(mXBound, mYBound), FY(mXBound, mYBound);
    GetFX(Fmap, FX);
    GetFY(Fmap, FY);

    // Construct second-order derivatives.
    Array2<Real> FXY(mXBound, mYBound);
    GetFXY(Fmap, FXY);

    // Construct polynomials.
    GetPolynomials(Fmap, FX, FY, FXY);
}

template <typename Real> inline
int IntpAkimaUniform2<Real>::GetXBound() const
{
    return mXBound;
}

template <typename Real> inline
int IntpAkimaUniform2<Real>::GetYBound() const
{
    return mYBound;
}

template <typename Real> inline
int IntpAkimaUniform2<Real>::GetQuantity() const
{
    return mQuantity;
}

template <typename Real> inline
Real const* IntpAkimaUniform2<Real>::GetF() const
{
    return mF;
}

template <typename Real> inline
Real IntpAkimaUniform2<Real>::GetXMin() const
{
    return mXMin;
}

template <typename Real> inline
Real IntpAkimaUniform2<Real>::GetXMax() const
{
    return mXMax;
}

template <typename Real> inline
Real IntpAkimaUniform2<Real>::GetXSpacing() const
{
    return mXSpacing;
}

template <typename Real> inline
Real IntpAkimaUniform2<Real>::GetYMin() const
{
    return mYMin;
}

template <typename Real> inline
Real IntpAkimaUniform2<Real>::GetYMax() const
{
    return mYMax;
}

template <typename Real> inline
Real IntpAkimaUniform2<Real>::GetYSpacing() const
{
    return mYSpacing;
}

template <typename Real>
Real IntpAkimaUniform2<Real>::operator()(Real x, Real y) const
{
    x = std::min(std::max(x, mXMin), mXMax);
    y = std::min(std::max(y, mYMin), mYMax);
    int ix, iy;
    Real dx, dy;
    XLookup(x, ix, dx);
    YLookup(y, iy, dy);
    return mPoly[iy][ix](dx, dy);
}

template <typename Real>
Real IntpAkimaUniform2<Real>::operator()(int xOrder, int yOrder, Real x,
    Real y) const
{
    x = std::min(std::max(x, mXMin), mXMax);
    y = std::min(std::max(y, mYMin), mYMax);
    int ix, iy;
    Real dx, dy;
    XLookup(x, ix, dx);
    YLookup(y, iy, dy);
    return mPoly[iy][ix](xOrder, yOrder, dx, dy);
}

template <typename Real>
void IntpAkimaUniform2<Real>::GetFX(Array2<Real> const& F, Array2<Real>& FX)
{
    Array2<Real> slope(mXBound + 3, mYBound);
    Real invDX = ((Real)1) / mXSpacing;
    int ix, iy;
    for (iy = 0; iy < mYBound; ++iy)
    {
        for (ix = 0; ix < mXBound - 1; ++ix)
        {
            slope[iy][ix + 2] = (F[iy][ix + 1] - F[iy][ix]) * invDX;
        }

        slope[iy][1] = ((Real)2) * slope[iy][2] - slope[iy][3];
        slope[iy][0] = ((Real)2) * slope[iy][1] - slope[iy][2];
        slope[iy][mXBound + 1] = ((Real)2) * slope[iy][mXBound] - slope[iy][mXBound - 1];
        slope[iy][mXBound + 2] = ((Real)2) * slope[iy][mXBound + 1] - slope[iy][mXBound];
    }

    for (iy = 0; iy < mYBound; ++iy)
    {
        for (ix = 0; ix < mXBound; ++ix)
        {
            FX[iy][ix] = ComputeDerivative(slope[iy] + ix);
        }
    }
}

template <typename Real>
void IntpAkimaUniform2<Real>::GetFY(Array2<Real> const& F, Array2<Real>& FY)
{
    Array2<Real> slope(mYBound + 3, mXBound);
    Real invDY = ((Real)1) / mYSpacing;
    int ix, iy;
    for (ix = 0; ix < mXBound; ++ix)
    {
        for (iy = 0; iy < mYBound - 1; ++iy)
        {
            slope[ix][iy + 2] = (F[iy + 1][ix] - F[iy][ix]) * invDY;
        }

        slope[ix][1] = ((Real)2) * slope[ix][2] - slope[ix][3];
        slope[ix][0] = ((Real)2) * slope[ix][1] - slope[ix][2];
        slope[ix][mYBound + 1] = ((Real)2) * slope[ix][mYBound] - slope[ix][mYBound - 1];
        slope[ix][mYBound + 2] = ((Real)2) * slope[ix][mYBound + 1] - slope[ix][mYBound];
    }

    for (ix = 0; ix < mXBound; ++ix)
    {
        for (iy = 0; iy < mYBound; ++iy)
        {
            FY[iy][ix] = ComputeDerivative(slope[ix] + iy);
        }
    }
}

template <typename Real>
void IntpAkimaUniform2<Real>::GetFXY(Array2<Real> const& F, Array2<Real>& FXY)
{
    int xBoundM1 = mXBound - 1;
    int yBoundM1 = mYBound - 1;
    int ix0 = xBoundM1, ix1 = ix0 - 1, ix2 = ix1 - 1;
    int iy0 = yBoundM1, iy1 = iy0 - 1, iy2 = iy1 - 1;
    int ix, iy;

    Real invDXDY = ((Real)1) / (mXSpacing * mYSpacing);

    // corners
    FXY[0][0] = ((Real)0.25)*invDXDY*(
        ((Real)9)*F[0][0]
        - ((Real)12)*F[0][1]
        + ((Real)3)*F[0][2]
        - ((Real)12)*F[1][0]
        + ((Real)16)*F[1][1]
        - ((Real)4)*F[1][2]
        + ((Real)3)*F[2][0]
        - ((Real)4)*F[2][1]
        + F[2][2]);

    FXY[0][xBoundM1] = ((Real)0.25)*invDXDY*(
        ((Real)9)*F[0][ix0]
        - ((Real)12)*F[0][ix1]
        + ((Real)3)*F[0][ix2]
        - ((Real)12)*F[1][ix0]
        + ((Real)16)*F[1][ix1]
        - ((Real)4)*F[1][ix2]
        + ((Real)3)*F[2][ix0]
        - ((Real)4)*F[2][ix1]
        + F[2][ix2]);

    FXY[yBoundM1][0] = ((Real)0.25)*invDXDY*(
        ((Real)9)*F[iy0][0]
        - ((Real)12)*F[iy0][1]
        + ((Real)3)*F[iy0][2]
        - ((Real)12)*F[iy1][0]
        + ((Real)16)*F[iy1][1]
        - ((Real)4)*F[iy1][2]
        + ((Real)3)*F[iy2][0]
        - ((Real)4)*F[iy2][1]
        + F[iy2][2]);

    FXY[yBoundM1][xBoundM1] = ((Real)0.25)*invDXDY*(
        ((Real)9)*F[iy0][ix0]
        - ((Real)12)*F[iy0][ix1]
        + ((Real)3)*F[iy0][ix2]
        - ((Real)12)*F[iy1][ix0]
        + ((Real)16)*F[iy1][ix1]
        - ((Real)4)*F[iy1][ix2]
        + ((Real)3)*F[iy2][ix0]
        - ((Real)4)*F[iy2][ix1]
        + F[iy2][ix2]);

    // x-edges
    for (ix = 1; ix < xBoundM1; ++ix)
    {
        FXY[0][ix] = ((Real)0.25)*invDXDY*(
            ((Real)3)*(F[0][ix - 1] - F[0][ix + 1])
            - ((Real)4)*(F[1][ix - 1] - F[1][ix + 1])
            + (F[2][ix - 1] - F[2][ix + 1]));

        FXY[yBoundM1][ix] = ((Real)0.25)*invDXDY*(
            ((Real)3)*(F[iy0][ix - 1] - F[iy0][ix + 1])
            - ((Real)4)*(F[iy1][ix - 1] - F[iy1][ix + 1])
            + (F[iy2][ix - 1] - F[iy2][ix + 1]));
    }

    // y-edges
    for (iy = 1; iy < yBoundM1; ++iy)
    {
        FXY[iy][0] = ((Real)0.25)*invDXDY*(
            ((Real)3)*(F[iy - 1][0] - F[iy + 1][0])
            - ((Real)4)*(F[iy - 1][1] - F[iy + 1][1])
            + (F[iy - 1][2] - F[iy + 1][2]));

        FXY[iy][xBoundM1] = ((Real)0.25)*invDXDY*(
            ((Real)3)*(F[iy - 1][ix0] - F[iy + 1][ix0])
            - ((Real)4)*(F[iy - 1][ix1] - F[iy + 1][ix1])
            + (F[iy - 1][ix2] - F[iy + 1][ix2]));
    }

    // interior
    for (iy = 1; iy < yBoundM1; ++iy)
    {
        for (ix = 1; ix < xBoundM1; ++ix)
        {
            FXY[iy][ix] = ((Real)0.25)*invDXDY*(F[iy - 1][ix - 1] -
                F[iy - 1][ix + 1] - F[iy + 1][ix - 1] + F[iy + 1][ix + 1]);
        }
    }
}

template <typename Real>
void IntpAkimaUniform2<Real>::GetPolynomials(Array2<Real> const& F,
    Array2<Real> const& FX, Array2<Real> const& FY, Array2<Real> const& FXY)
{
    int xBoundM1 = mXBound - 1;
    int yBoundM1 = mYBound - 1;
    for (int iy = 0; iy < yBoundM1; ++iy)
    {
        for (int ix = 0; ix < xBoundM1; ++ix)
        {
            // Note the 'transposing' of the 2x2 blocks (to match notation
            // used in the polynomial definition).
            Real G[2][2] =
            {
                { F[iy][ix], F[iy + 1][ix] },
                { F[iy][ix + 1], F[iy + 1][ix + 1] }
            };

            Real GX[2][2] =
            {
                { FX[iy][ix], FX[iy + 1][ix] },
                { FX[iy][ix + 1], FX[iy + 1][ix + 1] }
            };

            Real GY[2][2] =
            {
                { FY[iy][ix], FY[iy + 1][ix] },
                { FY[iy][ix + 1], FY[iy + 1][ix + 1] }
            };

            Real GXY[2][2] =
            {
                { FXY[iy][ix], FXY[iy + 1][ix] },
                { FXY[iy][ix + 1], FXY[iy + 1][ix + 1] }
            };

            Construct(mPoly[iy][ix], G, GX, GY, GXY);
        }
    }
}

template <typename Real>
Real IntpAkimaUniform2<Real>::ComputeDerivative(Real const* slope) const
{
    if (slope[1] != slope[2])
    {
        if (slope[0] != slope[1])
        {
            if (slope[2] != slope[3])
            {
                Real ad0 = fabs(slope[3] - slope[2]);
                Real ad1 = fabs(slope[0] - slope[1]);
                return (ad0 * slope[1] + ad1 * slope[2]) / (ad0 + ad1);
            }
            else
            {
                return slope[2];
            }
        }
        else
        {
            if (slope[2] != slope[3])
            {
                return slope[1];
            }
            else
            {
                return ((Real)0.5) * (slope[1] + slope[2]);
            }
        }
    }
    else
    {
        return slope[1];
    }
}

template <typename Real>
void IntpAkimaUniform2<Real>::Construct(Polynomial& poly, Real const F[2][2],
    Real const FX[2][2], Real const FY[2][2], Real const FXY[2][2])
{
    Real dx = mXSpacing;
    Real dy = mYSpacing;
    Real invDX = ((Real)1) / dx, invDX2 = invDX*invDX;
    Real invDY = ((Real)1) / dy, invDY2 = invDY*invDY;
    Real b0, b1, b2, b3;

    poly.A(0, 0) = F[0][0];
    poly.A(1, 0) = FX[0][0];
    poly.A(0, 1) = FY[0][0];
    poly.A(1, 1) = FXY[0][0];

    b0 = (F[1][0] - poly(0, 0, dx, (Real)0))*invDX2;
    b1 = (FX[1][0] - poly(1, 0, dx, (Real)0))*invDX;
    poly.A(2, 0) = ((Real)3)*b0 - b1;
    poly.A(3, 0) = (-((Real)2)*b0 + b1)*invDX;

    b0 = (F[0][1] - poly(0, 0, (Real)0, dy))*invDY2;
    b1 = (FY[0][1] - poly(0, 1, (Real)0, dy))*invDY;
    poly.A(0, 2) = ((Real)3)*b0 - b1;
    poly.A(0, 3) = (-((Real)2)*b0 + b1)*invDY;

    b0 = (FY[1][0] - poly(0, 1, dx, (Real)0))*invDX2;
    b1 = (FXY[1][0] - poly(1, 1, dx, (Real)0))*invDX;
    poly.A(2, 1) = ((Real)3)*b0 - b1;
    poly.A(3, 1) = (-((Real)2)*b0 + b1)*invDX;

    b0 = (FX[0][1] - poly(1, 0, (Real)0, dy))*invDY2;
    b1 = (FXY[0][1] - poly(1, 1, (Real)0, dy))*invDY;
    poly.A(1, 2) = ((Real)3)*b0 - b1;
    poly.A(1, 3) = (-((Real)2)*b0 + b1)*invDY;

    b0 = (F[1][1] - poly(0, 0, dx, dy))*invDX2*invDY2;
    b1 = (FX[1][1] - poly(1, 0, dx, dy))*invDX*invDY2;
    b2 = (FY[1][1] - poly(0, 1, dx, dy))*invDX2*invDY;
    b3 = (FXY[1][1] - poly(1, 1, dx, dy))*invDX*invDY;
    poly.A(2, 2) = ((Real)9)*b0 - ((Real)3)*b1 - ((Real)3)*b2 + b3;
    poly.A(3, 2) = (-((Real)6)*b0 + ((Real)3)*b1 + ((Real)2)*b2 - b3)*invDX;
    poly.A(2, 3) = (-((Real)6)*b0 + ((Real)2)*b1 + ((Real)3)*b2 - b3)*invDY;
    poly.A(3, 3) = (((Real)4)*b0 - ((Real)2)*b1 - ((Real)2)*b2 + b3)*invDX*invDY;
}

template <typename Real>
void IntpAkimaUniform2<Real>::XLookup(Real x, int& xIndex, Real& dx) const
{
    for (xIndex = 0; xIndex + 1 < mXBound; ++xIndex)
    {
        if (x < mXMin + mXSpacing*(xIndex + 1))
        {
            dx = x - (mXMin + mXSpacing*xIndex);
            return;
        }
    }

    --xIndex;
    dx = x - (mXMin + mXSpacing*xIndex);
}

template <typename Real>
void IntpAkimaUniform2<Real>::YLookup(Real y, int& yIndex, Real& dy) const
{
    for (yIndex = 0; yIndex + 1 < mYBound; ++yIndex)
    {
        if (y < mYMin + mYSpacing*(yIndex + 1))
        {
            dy = y - (mYMin + mYSpacing*yIndex);
            return;
        }
    }

    yIndex--;
    dy = y - (mYMin + mYSpacing*yIndex);
}

template <typename Real>
IntpAkimaUniform2<Real>::Polynomial::Polynomial()
{
    memset(&mCoeff[0][0], 0, 16 * sizeof(Real));
}

template <typename Real>
Real& IntpAkimaUniform2<Real>::Polynomial::A(int ix, int iy)
{
    return mCoeff[ix][iy];
}

template <typename Real>
Real IntpAkimaUniform2<Real>::Polynomial::operator()(Real x, Real y) const
{
    Real B[4];
    for (int i = 0; i <= 3; ++i)
    {
        B[i] = mCoeff[i][0] + y*(mCoeff[i][1] + y*(mCoeff[i][2] + y*mCoeff[i][3]));
    }

    return B[0] + x*(B[1] + x*(B[2] + x*B[3]));
}

template <typename Real>
Real IntpAkimaUniform2<Real>::Polynomial::operator()(int xOrder, int yOrder,
    Real x, Real y) const
{
    Real xPow[4];
    switch (xOrder)
    {
    case 0:
        xPow[0] = (Real)1;
        xPow[1] = x;
        xPow[2] = x * x;
        xPow[3] = x * x * x;
        break;
    case 1:
        xPow[0] = (Real)0;
        xPow[1] = (Real)1;
        xPow[2] = ((Real)2) * x;
        xPow[3] = ((Real)3) * x * x;
        break;
    case 2:
        xPow[0] = (Real)0;
        xPow[1] = (Real)0;
        xPow[2] = (Real)2;
        xPow[3] = ((Real)6) * x;
        break;
    case 3:
        xPow[0] = (Real)0;
        xPow[1] = (Real)0;
        xPow[2] = (Real)0;
        xPow[3] = (Real)6;
        break;
    default:
        return (Real)0;
    }

    Real yPow[4];
    switch (yOrder)
    {
    case 0:
        yPow[0] = (Real)1;
        yPow[1] = y;
        yPow[2] = y * y;
        yPow[3] = y * y * y;
        break;
    case 1:
        yPow[0] = (Real)0;
        yPow[1] = (Real)1;
        yPow[2] = ((Real)2) * y;
        yPow[3] = ((Real)3) * y * y;
        break;
    case 2:
        yPow[0] = (Real)0;
        yPow[1] = (Real)0;
        yPow[2] = (Real)2;
        yPow[3] = ((Real)6) * y;
        break;
    case 3:
        yPow[0] = (Real)0;
        yPow[1] = (Real)0;
        yPow[2] = (Real)0;
        yPow[3] = (Real)6;
        break;
    default:
        return (Real)0;
    }

    Real p = (Real)0;
    for (int iy = 0; iy <= 3; ++iy)
    {
        for (int ix = 0; ix <= 3; ++ix)
        {
            p += mCoeff[ix][iy] * xPow[ix] * yPow[iy];
        }
    }

    return p;
}

}