transformations.h

Go to the documentation of this file.

/***************************************************************************
 *
 * Authors:     Carlos Oscar S. Sorzano (coss@cnb.csic.es)
 *              Sjors H.W. Scheres
 *
 * Unidad de  Bioinformatica of Centro Nacional de Biotecnologia , CSIC
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * 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, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
 * 02111-1307  USA
 *
 *  All comments concerning this program package may be sent to the
 *  e-mail address 'xmipp@cnb.csic.es'
 ***************************************************************************/

#ifndef CORE_TRANSFORMATIONS_H
#define CORE_TRANSFORMATIONS_H

#include "matrix2d.h"
#include "multidim_array.h"
#include "multidim_array_generic.h"
#include "transformations_defines.h"


class MDRow;



void geo2TransformationMatrix(const MDRow &imageHeader, Matrix2D<double> &A,
                              bool only_apply_shifts = false);

bool getLoopRange( double value, double min, double max, double delta, int loopLimit, int &minIter, int &maxIter);

void string2TransformationMatrix(const String &matrixStr, Matrix2D<double> &matrix, size_t dim=4);

template<typename T>
void transformationMatrix2Parameters2D(const Matrix2D<T> &A, bool &flip,
                                       T &scale, T &shiftX, T &shiftY,
                                       T &psi);

void transformationMatrix2Parameters3D(const Matrix2D<double> &A, bool &flip,
                                       double &scale, double &shiftX, double &shiftY,
                                       double &shiftZ, double &rot, double &tilt, double &psi);

void transformationMatrix2Geo(const Matrix2D<double> &A, MDRow & imageGeo);

template<typename T>
void rotation2DMatrix(T ang, Matrix2D<T> &m, bool homogeneous=true);

template<typename T>
void translation2DMatrix(const Matrix1D<T> &translation, Matrix2D<T> &resMatrix, bool inverse=false);

void rotation3DMatrix(double ang, char axis, Matrix2D< double > &m,
                      bool homogeneous=true);

void rotation3DMatrix(double ang, const Matrix1D< double >& axis, Matrix2D< double > &m,
                      bool homogeneous=true);

void alignWithZ(const Matrix1D< double >& axis, Matrix2D< double > &m, bool homogeneous=true);

template<typename T>
void translation3DMatrix(const Matrix1D<T> &translation, Matrix2D<T> &resMatrix, bool inverse=false);

void scale3DMatrix(const Matrix1D< double >& sc, Matrix2D< double > &m,
                   bool homogeneous=true);

void rotation3DMatrixFromIcoOrientations(const char* icoFrom, const char* icoTo, Matrix2D<double> &R);

namespace applyGeometryImpl {

template<typename T1,typename T, bool wrap>
void applyGeometry2DDegree1(
                   MultidimArray<T>& __restrict__ V2,
                   const MultidimArray<T1>& __restrict__ V1,
                   const Matrix2D<double> &Aref)
{
    // 2D transformation
    const double Aref00=MAT_ELEM(Aref,0,0);
    const double Aref10=MAT_ELEM(Aref,1,0);

    // Find center and limits of image
    const double cen_y  = (int)(YSIZE(V2) / 2);
    const double cen_x  = (int)(XSIZE(V2) / 2);
    const double cen_yp = (int)(YSIZE(V1) / 2);
    const double cen_xp = (int)(XSIZE(V1) / 2);
    const double minxp  = -cen_xp;
    const double minyp  = -cen_yp;
    const double minxpp = minxp-XMIPP_EQUAL_ACCURACY;
    const double minypp = minyp-XMIPP_EQUAL_ACCURACY;
    const double maxxp  = XSIZE(V1) - cen_xp - 1;
    const double maxyp  = YSIZE(V1) - cen_yp - 1;
    const double maxxpp = maxxp+XMIPP_EQUAL_ACCURACY;
    const double maxypp = maxyp+XMIPP_EQUAL_ACCURACY;
    const int Xdim   = XSIZE(V1);
    const int Ydim   = YSIZE(V1);

    // Now we go from the output image to the input image, ie, for any pixel
    // in the output image we calculate which are the corresponding ones in
    // the original image, make an interpolation with them and put this value
    // at the output pixel

#ifdef DEBUG_APPLYGEO
    std::cout << "A\n" << Aref << std::endl
    << "(cen_x ,cen_y )=(" << cen_x  << "," << cen_y  << ")\n"
    << "(cen_xp,cen_yp)=(" << cen_xp << "," << cen_yp << ")\n"
    << "(min_xp,min_yp)=(" << minxp  << "," << minyp  << ")\n"
    << "(max_xp,max_yp)=(" << maxxp  << "," << maxyp  << ")\n";
#endif
    // Calculate position of the beginning of the row in the output image
    const double x = -cen_x;
    double y = -cen_y;
    for (size_t i = 0; i < YSIZE(V2); i++)
    {
        // Calculate this position in the input image according to the
        // geometrical transformation
        // they are related by
        // coords_output(=x,y) = A * coords_input (=xp,yp)
        double xp = x * MAT_ELEM(Aref, 0, 0) + y * MAT_ELEM(Aref, 0, 1) + MAT_ELEM(Aref, 0, 2);
        double yp = x * MAT_ELEM(Aref, 1, 0) + y * MAT_ELEM(Aref, 1, 1) + MAT_ELEM(Aref, 1, 2);

        // Loop over j is splitted according to wrap (wrap==true is not
        // vectorizable) and also according to SplineDegree value
        // (I have not fully analyzed vector dependences for
        // SplineDegree==3 and else branch)
        if (wrap)
        {
            // This is original implementation
            for (int j=0; j<XSIZE(V2) ;j++)
            {
#ifdef DEBUG_APPLYGEO

                std::cout << "Computing (" << i << "," << j << ")\n";
                std::cout << "   (y, x) =(" << y << "," << x << ")\n"
                << "   before wrapping (y',x')=(" << yp << "," << xp << ") "
                << std::endl;
#endif
                bool x_isOut = XMIPP_RANGE_OUTSIDE_FAST(xp, minxpp, maxxpp);
                bool y_isOut = XMIPP_RANGE_OUTSIDE_FAST(yp, minypp, maxypp);

                if (x_isOut)
                {
                    xp = realWRAP(xp, minxp - 0.5, maxxp + 0.5);
                }

                if (y_isOut)
                {
                    yp = realWRAP(yp, minyp - 0.5, maxyp + 0.5);
                }

#ifdef DEBUG_APPLYGEO

                std::cout << "   after wrapping (y',x')=(" << yp << "," << xp << ") "
                << std::endl;
                std::cout << "   Interp = " << interp << std::endl;
                // The following line sounds dangerous...
                //x++;
#endif

                // Linear interpolation
                // Calculate the integer position in input image, be
                // careful that it is not the nearest but the one
                // at the top left corner of the interpolation square.
                // Ie, (0.7,0.7) would give (0,0)
                // Calculate also weights for point m1+1,n1+1
                double wx = xp + cen_xp;
                double wy = yp + cen_yp;
                int m1 = (int) wx;
                int n1 = (int) wy;
                int n2 = n1 + 1;
                int m2 = m1 + 1;
                wx = wx - m1;
                wy = wy - n1;
                double wx_1 = 1 - wx;
                double wy_1 = 1 - wy;

                // m2 and n2 can be out by 1 so wrap must be check here
                if (m2 >= Xdim)
                    m2 = 0;
                if (n2 >= Ydim)
                    n2 = 0;

#ifdef DEBUG_APPLYGEO
                std::cout << "   From (" << n1 << "," << m1 << ") and ("
                        << n2 << "," << m2 << ")\n";
                std::cout << "   wx= " << wx << " wy= " << wy
                        << std::endl;
#endif

                // Perform interpolation
                double v1 = wy_1 * wx_1 * DIRECT_A2D_ELEM(V1, n1, m1);
                double v2 = wy_1 * wx * DIRECT_A2D_ELEM(V1, n1, m2);
                double v3 = wx_1 * wy * DIRECT_A2D_ELEM(V1, n2, m1);
                double v4 = wx * wy * DIRECT_A2D_ELEM(V1, n2, m2);

                dAij(V2, i, j) = (T) (v1 + v2 + v3 + v4);
#ifdef DEBUG_APPYGEO
                std::cout << "   val= " << dAij(V2, i, j) << std::endl;
#endif

                // Compute new point inside input image
                xp += Aref00;
                yp += Aref10;
            }
        } /* wrap == true */
        else
        {

            // Inner loop boundaries.
            int globalMin=0, globalMax=XSIZE(V2);

            // First and last iteration with valid values for x and y coordinates.
            int minX, maxX, minY, maxY;

            // Compute valid iterations in x and y coordinates. If one of them is always out
            // of boundaries then the inner loop is not executed this iteration.
            if (!getLoopRange( xp, minxpp, maxxpp, Aref00, XSIZE(V2), minX, maxX) ||
                !getLoopRange( yp, minypp, maxypp, Aref10, XSIZE(V2), minY, maxY))
            {
                y++;
                continue;
            }
            else
            {
                // Compute initial iteration.
                globalMin = minX;
                if (minX < minY)
                {
                    globalMin = minY;
                }

                // Compute last iteration.
                globalMax = maxX;
                if (maxX > maxY)
                {
                    globalMax = maxY;
                }
                globalMax++;

                // Check max iteration is not higher than image.
                if ((globalMax >= 0) && ((size_t)globalMax > XSIZE(V2)))
                {
                    globalMax = XSIZE(V2);
                }

                xp += globalMin*Aref00;
                yp += globalMin*Aref10;
            }


            #pragma simd reduction (+:xp,yp)
            for (int j=globalMin; j<globalMax ;j++)
            {
#ifdef DEBUG_APPLYGEO

                std::cout << "Computing (" << i << "," << j << ")\n";
                std::cout << "   (y, x) =(" << y << "," << x << ")\n"
                << "   before wrapping (y',x')=(" << yp << "," << xp << ") "
                << std::endl;

                std::cout << "   after wrapping (y',x')=(" << yp << "," << xp << ") "
                << std::endl;
                std::cout << "   Interp = " << interp << std::endl;
                // The following line sounds dangerous...
                //x++;
#endif

                // Linear interpolation

                // Calculate the integer position in input image, be careful
                // that it is not the nearest but the one at the top left corner
                // of the interpolation square. Ie, (0.7,0.7) would give (0,0)
                // Calculate also weights for point m1+1,n1+1
                double wx = xp + cen_xp;
                int m1 = (int) wx;
                wx = wx - m1;
                int m2 = m1 + 1;
                double wy = yp + cen_yp;
                int n1 = (int) wy;
                wy = wy - n1;
                int n2 = n1 + 1;

#ifdef DEBUG_APPLYGEO
                std::cout << "   From (" << n1 << "," << m1 << ") and ("
                    << n2 << "," << m2 << ")\n";
                std::cout << "   wx= " << wx << " wy= " << wy << std::endl;
#endif

                // Perform interpolation
                // if wx == 0 means that the rightest point is useless for this
                // interpolation, and even it might not be defined if m1=xdim-1
                // The same can be said for wy.
                double wx_1 = (1-wx);
                double wy_1 = (1-wy);
                double aux2=wy_1* wx_1 ;
                double tmp  = aux2 * DIRECT_A2D_ELEM(V1, n1, m1);

                if ((wx != 0) && ((m2 < 0) || ((size_t)m2 < V1.xdim)))
                    tmp += (wy_1-aux2) * DIRECT_A2D_ELEM(V1, n1, m2);

                if ((wy != 0) && ((n2 < 0) || ((size_t)n2 < V1.ydim)))
                {
                    aux2=wy * wx_1;
                    tmp += aux2 * DIRECT_A2D_ELEM(V1, n2, m1);

                    if ((wx != 0) && ((m2 < 0) || ((size_t)m2 < V1.xdim)))
                        tmp += (wy-aux2) * DIRECT_A2D_ELEM(V1, n2, m2);
                }

                dAij(V2, i, j) = (T) tmp;

#ifdef DEBUG_APPYGEO
                std::cout << "   val= " << dAij(V2, i, j) << std::endl;
#endif

                // Compute new point inside input image
                xp += Aref00;
                yp += Aref10;
            }
        } /* wrap == false */

        y++;
    }
}


} // namespace applyGeometryImpl


template<typename T1,typename T, typename T2>
void applyGeometry(int SplineDegree,
                   MultidimArray<T>& __restrict__ V2,
                   const MultidimArray<T1>& __restrict__ V1,
                   const Matrix2D< T2 > &At, bool inv,
                   bool wrap, T outside = T(0), MultidimArray<double> *BcoeffsPtr=NULL)
{
#ifndef RELEASE_MODE
    if (&V1 == (MultidimArray<T1>*)&V2)
        REPORT_ERROR(ERR_VALUE_INCORRECT,"ApplyGeometry: Input array cannot be the same as output array");

    if ( V1.getDim()==2 && ((MAT_XSIZE(At) != 3) || (MAT_YSIZE(At) != 3)) )
        REPORT_ERROR(ERR_MATRIX_SIZE,"ApplyGeometry: 2D transformation matrix is not 3x3");

    if ( V1.getDim()==3 && ((MAT_XSIZE(At) != 4) || (MAT_YSIZE(At) != 4)) )
        REPORT_ERROR(ERR_MATRIX_SIZE,"ApplyGeometry: 3D transformation matrix is not 4x4");
#endif

    if (At.isIdentity() && ( XSIZE(V2) == 0 || SAME_SHAPE3D(V1,V2) ) )
    {
        typeCast(V1,V2);
        return;
    }

    if (XSIZE(V1) == 0)
    {
        V2.clear();
        return;
    }

    MultidimArray<double> Bcoeffs;
    MultidimArray<double> *BcoeffsToUse=NULL;
    Matrix2D<double> A, Ainv;
    typeCast(At, A);
    const Matrix2D<double> * Aptr=&A;
    if (!inv)
    {
        Ainv = A.inv();
        Aptr=&Ainv;
    }
    const Matrix2D<double> &Aref=*Aptr;

    // For scalings the output matrix is resized outside to the final
    // size instead of being resized inside the routine with the
    // same size as the input matrix
    if (XSIZE(V2) == 0)
        V2.resizeNoCopy(V1);

    if (outside != 0.)
    {
        // Initialize output matrix with value=outside
        FOR_ALL_DIRECT_ELEMENTS_IN_MULTIDIMARRAY(V2)
        DIRECT_MULTIDIM_ELEM(V2, n) = outside;
    }
    else
        V2.initZeros();

    if (V1.getDim() == 2)
    {
        // this version should be slightly more optimized than the general one bellow
        if (1 == SplineDegree) {
            if (wrap) {
                applyGeometryImpl::applyGeometry2DDegree1<T1, T, true>(V2, V1, Aref);
            } else {
                applyGeometryImpl::applyGeometry2DDegree1<T1, T, false>(V2, V1, Aref);
            }
            return;
        }

        // 2D transformation
        double Aref00=MAT_ELEM(Aref,0,0);
        double Aref10=MAT_ELEM(Aref,1,0);

        // Find center and limits of image
        double cen_y  = (int)(YSIZE(V2) / 2);
        double cen_x  = (int)(XSIZE(V2) / 2);
        double cen_yp = (int)(YSIZE(V1) / 2);
        double cen_xp = (int)(XSIZE(V1) / 2);
        double minxp  = -cen_xp;
        double minyp  = -cen_yp;
        double minxpp = minxp-XMIPP_EQUAL_ACCURACY;
        double minypp = minyp-XMIPP_EQUAL_ACCURACY;
        double maxxp  = XSIZE(V1) - cen_xp - 1;
        double maxyp  = YSIZE(V1) - cen_yp - 1;
        double maxxpp = maxxp+XMIPP_EQUAL_ACCURACY;
        double maxypp = maxyp+XMIPP_EQUAL_ACCURACY;
        size_t Xdim   = XSIZE(V1);
        size_t Ydim   = YSIZE(V1);

        if (SplineDegree > 1)
        {
            // Build the B-spline coefficients
            if (BcoeffsPtr!=NULL)
                BcoeffsToUse=BcoeffsPtr;
            else
            {
                produceSplineCoefficients(SplineDegree, Bcoeffs, V1); //Bcoeffs is a single image
                BcoeffsToUse = &Bcoeffs;
            }
            STARTINGX(*BcoeffsToUse) = (int) minxp;
            STARTINGY(*BcoeffsToUse) = (int) minyp;
        }

        // Now we go from the output image to the input image, ie, for any pixel
        // in the output image we calculate which are the corresponding ones in
        // the original image, make an interpolation with them and put this value
        // at the output pixel

#ifdef DEBUG_APPLYGEO
        std::cout << "A\n" << Aref << std::endl
        << "(cen_x ,cen_y )=(" << cen_x  << "," << cen_y  << ")\n"
        << "(cen_xp,cen_yp)=(" << cen_xp << "," << cen_yp << ")\n"
        << "(min_xp,min_yp)=(" << minxp  << "," << minyp  << ")\n"
        << "(max_xp,max_yp)=(" << maxxp  << "," << maxyp  << ")\n";
#endif
        // Calculate position of the beginning of the row in the output image
        double x = -cen_x;
        double y = -cen_y;
        for (size_t i = 0; i < YSIZE(V2); i++)
        {
            // Calculate this position in the input image according to the
            // geometrical transformation
            // they are related by
            // coords_output(=x,y) = A * coords_input (=xp,yp)
            double xp = x * MAT_ELEM(Aref, 0, 0) + y * MAT_ELEM(Aref, 0, 1) + MAT_ELEM(Aref, 0, 2);
            double yp = x * MAT_ELEM(Aref, 1, 0) + y * MAT_ELEM(Aref, 1, 1) + MAT_ELEM(Aref, 1, 2);

            // Inner loop boundaries.
            int globalMin=0, globalMax=XSIZE(V2);

            if (!wrap)
            {
                // First and last iteration with valid values for x and y coordinates.
                int minX, maxX, minY, maxY;

                // Compute valid iterations in x and y coordinates. If one of them is always out
                // of boundaries then the inner loop is not executed this iteration.
                if (!getLoopRange( xp, minxpp, maxxpp, Aref00, XSIZE(V2), minX, maxX) ||
                    !getLoopRange( yp, minypp, maxypp, Aref10, XSIZE(V2), minY, maxY))
                {
                    y++;
                    continue;
                }
                else
                {
                    // Compute initial iteration.
                    globalMin = minX;
                    if (minX < minY)
                    {
                        globalMin = minY;
                    }

                    // Compute last iteration.
                    globalMax = maxX;
                    if (maxX > maxY)
                    {
                        globalMax = maxY;
                    }
                    globalMax++;

                    // Check max iteration is not higher than image.
                    if ((globalMax >= 0) && ((size_t)globalMax > XSIZE(V2)))
                    {
                        globalMax = XSIZE(V2);
                    }

                    xp += globalMin*Aref00;
                    yp += globalMin*Aref10;
                }
            }

            // Loop over j is splitted according to wrap (wrap==true is not
            // vectorizable) and also according to SplineDegree value
            // (I have not fully analyzed vector dependences for 
            // SplineDegree==3 and else branch)
            if (wrap) 
            {
                // This is original implementation
                for (int j=globalMin; j<globalMax ;j++)
                {
#ifdef DEBUG_APPLYGEO

                    std::cout << "Computing (" << i << "," << j << ")\n";
                    std::cout << "   (y, x) =(" << y << "," << x << ")\n"
                    << "   before wrapping (y',x')=(" << yp << "," << xp << ") "
                    << std::endl;
#endif
                    bool x_isOut = XMIPP_RANGE_OUTSIDE_FAST(xp, minxpp, maxxpp);
                    bool y_isOut = XMIPP_RANGE_OUTSIDE_FAST(yp, minypp, maxypp);

                    if (x_isOut)
                    {
                        xp = realWRAP(xp, minxp - 0.5, maxxp + 0.5);
                    }

                    if (y_isOut)
                    {
                        yp = realWRAP(yp, minyp - 0.5, maxyp + 0.5);
                    }

#ifdef DEBUG_APPLYGEO

                    std::cout << "   after wrapping (y',x')=(" << yp << "," << xp << ") "
                    << std::endl;
                    std::cout << "   Interp = " << interp << std::endl;
                    // The following line sounds dangerous...
                    //x++;
#endif

                    if (SplineDegree==1)
                    {
                        // Linear interpolation

                        // Calculate the integer position in input image, be 
                        // careful that it is not the nearest but the one 
                        // at the top left corner of the interpolation square. 
                        // Ie, (0.7,0.7) would give (0,0)
                        // Calculate also weights for point m1+1,n1+1
                        double wx = xp + cen_xp;
                        int m1 = (int) wx;
                        wx = wx - m1;
                        int m2 = m1 + 1;
                        double wy = yp + cen_yp;
                        int n1 = (int) wy;
                        wy = wy - n1;
                        int n2 = n1 + 1;

                        // m2 and n2 can be out by 1 so wrap must be check here
                        if ((m2 >= 0) && ((size_t)m2 >= Xdim))
                            m2 = 0;
                        if ((n2 >=0) && ((size_t)n2 >= Ydim))
                            n2 = 0;

#ifdef DEBUG_APPLYGEO
                        std::cout << "   From (" << n1 << "," << m1 << ") and ("
                                << n2 << "," << m2 << ")\n";
                        std::cout << "   wx= " << wx << " wy= " << wy 
                                << std::endl;
#endif

                        // Perform interpolation
                        // if wx == 0 means that the rightest point is useless 
                        // for this interpolation, and even it might not be 
                        // defined if m1=xdim-1
                        // The same can be said for wy.
                        double wx_1 = (1-wx);
                        double wy_1 = (1-wy);
                        double aux2=wy_1* wx_1 ;
                        double tmp  = aux2 * DIRECT_A2D_ELEM(V1, n1, m1);

                        if ((wx != 0) && ((m2 < 0) || ((size_t)m2 < V1.xdim)))
                            tmp += (wy_1-aux2) * DIRECT_A2D_ELEM(V1, n1, m2);

                        if ((wy != 0) && ((n2 < 0) || ((size_t)n2 < V1.ydim)))
                        {
                            aux2=wy * wx_1;
                            tmp += aux2 * DIRECT_A2D_ELEM(V1, n2, m1);

                            if ((wx != 0) && ((m2 < 0) || ((size_t)m2 < V1.xdim)))
                                tmp += (wy-aux2) * DIRECT_A2D_ELEM(V1, n2, m2);
                        }

                        dAij(V2, i, j) = (T) tmp;
                    }
                    else if (SplineDegree==0)
                    {
                        dAij(V2, i, j) = (T) A2D_ELEM(V1,(int)trunc(yp),(int)trunc(xp));
                    }
                    else if (SplineDegree==3)
                    {
                        // B-spline interpolation
                        dAij(V2, i, j) = (T) BcoeffsToUse->interpolatedElementBSpline2D_Degree3(xp, yp);
                    }
                    else
                    {
                        // B-spline interpolation
                        dAij(V2, i, j) = (T) BcoeffsToUse->interpolatedElementBSpline2D(xp, yp, SplineDegree);
                    }
#ifdef DEBUG_APPYGEO
                    std::cout << "   val= " << dAij(V2, i, j) << std::endl;
#endif

                    // Compute new point inside input image
                    xp += Aref00;
                    yp += Aref10;
                }
            } /* wrap == true */
            else
            {
                if (SplineDegree==1)
                {
                    #pragma simd reduction (+:xp,yp)
                    for (int j=globalMin; j<globalMax ;j++)
                    {
#ifdef DEBUG_APPLYGEO

                        std::cout << "Computing (" << i << "," << j << ")\n";
                        std::cout << "   (y, x) =(" << y << "," << x << ")\n"
                        << "   before wrapping (y',x')=(" << yp << "," << xp << ") "
                        << std::endl;

                        std::cout << "   after wrapping (y',x')=(" << yp << "," << xp << ") "
                        << std::endl;
                        std::cout << "   Interp = " << interp << std::endl;
                        // The following line sounds dangerous...
                        //x++;
#endif

                        // Linear interpolation

                        // Calculate the integer position in input image, be careful
                        // that it is not the nearest but the one at the top left corner
                        // of the interpolation square. Ie, (0.7,0.7) would give (0,0)
                        // Calculate also weights for point m1+1,n1+1
                        double wx = xp + cen_xp;
                        int m1 = (int) wx;
                        wx = wx - m1;
                        int m2 = m1 + 1;
                        double wy = yp + cen_yp;
                        int n1 = (int) wy;
                        wy = wy - n1;
                        int n2 = n1 + 1;

#ifdef DEBUG_APPLYGEO
                        std::cout << "   From (" << n1 << "," << m1 << ") and ("
                            << n2 << "," << m2 << ")\n";
                        std::cout << "   wx= " << wx << " wy= " << wy << std::endl;
#endif

                        // Perform interpolation
                        // if wx == 0 means that the rightest point is useless for this
                        // interpolation, and even it might not be defined if m1=xdim-1
                        // The same can be said for wy.
                        double wx_1 = (1-wx);
                        double wy_1 = (1-wy);
                        double aux2=wy_1* wx_1 ;
                        double tmp  = aux2 * DIRECT_A2D_ELEM(V1, n1, m1);

                        if ((wx != 0) && ((m2 < 0) || ((size_t)m2 < V1.xdim)))
                            tmp += (wy_1-aux2) * DIRECT_A2D_ELEM(V1, n1, m2);

                        if ((wy != 0) && ((n2 < 0) || ((size_t)n2 < V1.ydim)))
                        {
                            aux2=wy * wx_1;
                            tmp += aux2 * DIRECT_A2D_ELEM(V1, n2, m1);
    
                            if ((wx != 0) && ((m2 < 0) || ((size_t)m2 < V1.xdim)))
                                tmp += (wy-aux2) * DIRECT_A2D_ELEM(V1, n2, m2);
                        }

                        dAij(V2, i, j) = (T) tmp;

#ifdef DEBUG_APPYGEO
                        std::cout << "   val= " << dAij(V2, i, j) << std::endl;
#endif

                        // Compute new point inside input image
                        xp += Aref00;
                        yp += Aref10;
                    }
                }
                else if (SplineDegree==0)
                {
                    #pragma simd reduction (+:xp,yp)
                    for (int j=globalMin; j<globalMax ;j++) 
                    {
#ifdef DEBUG_APPLYGEO

                        std::cout << "Computing (" << i << "," << j << ")\n";
                        std::cout << "   (y, x) =(" << y << "," << x << ")\n"
                        << "   before wrapping (y',x')=(" << yp << "," << xp << ") "
                        << std::endl;

                        std::cout << "   after wrapping (y',x')=(" << yp << "," << xp << ") "
                        << std::endl;
                        std::cout << "   Interp = " << interp << std::endl;
                        // The following line sounds dangerous...
                        //x++;
#endif
                        dAij(V2, i, j) = (T) A2D_ELEM(V1,(int)trunc(yp),(int)trunc(xp));

#ifdef DEBUG_APPYGEO
                        std::cout << "   val= " << dAij(V2, i, j) << std::endl;
#endif

                        // Compute new point inside input image
                        xp += Aref00;
                        yp += Aref10;
                    }
                }
                else if (SplineDegree==3)
                {
                    for (int j=globalMin; j<globalMax ;j++)
                    {
#ifdef DEBUG_APPLYGEO

                        std::cout << "Computing (" << i << "," << j << ")\n";
                        std::cout << "   (y, x) =(" << y << "," << x << ")\n"
                        << "   before wrapping (y',x')=(" << yp << "," << xp << ") "
                        << std::endl;

                        std::cout << "   after wrapping (y',x')=(" << yp << "," << xp << ") "
                        << std::endl;
                        std::cout << "   Interp = " << interp << std::endl;
                        // The following line sounds dangerous...
                        //x++;
#endif

                        // B-spline interpolation
                        dAij(V2, i, j) = (T) BcoeffsToUse->interpolatedElementBSpline2D_Degree3(xp, yp);

#ifdef DEBUG_APPYGEO
                        std::cout << "   val= " << dAij(V2, i, j) << std::endl;
#endif

                        // Compute new point inside input image
                        xp += Aref00;
                        yp += Aref10;
                    }
                }
                else
                {
                    for (int j=globalMin; j<globalMax ;j++)
                    {
#ifdef DEBUG_APPLYGEO

                        std::cout << "Computing (" << i << "," << j << ")\n";
                        std::cout << "   (y, x) =(" << y << "," << x << ")\n"
                        << "   before wrapping (y',x')=(" << yp << "," << xp << ") "
                        << std::endl;

                        std::cout << "   after wrapping (y',x')=(" << yp << "," << xp << ") "
                        << std::endl;
                        std::cout << "   Interp = " << interp << std::endl;
                        // The following line sounds dangerous...
                        //x++;
#endif

                        // B-spline interpolation
                        dAij(V2, i, j) = (T) BcoeffsToUse->interpolatedElementBSpline2D(xp, yp, SplineDegree);
#ifdef DEBUG_APPYGEO
                        std::cout << "   val= " << dAij(V2, i, j) << std::endl;
#endif

                        // Compute new point inside input image
                        xp += Aref00;
                        yp += Aref10;
                    }
                }
            } /* wrap == false */

            y++;
        }
    }
    else
    {
        // 3D transformation
        size_t m1, n1, o1, m2, n2, o2;
        double x, y, z, xp, yp, zp;
        double minxp, minyp, maxxp, maxyp, minzp, maxzp;
        double cen_x, cen_y, cen_z, cen_xp, cen_yp, cen_zp;
        double wx, wy, wz;
        double Aref00=MAT_ELEM(Aref,0,0);
        double Aref10=MAT_ELEM(Aref,1,0);
        double Aref20=MAT_ELEM(Aref,2,0);

        // Find center of MultidimArray
        cen_z = (int)(V2.zdim / 2);
        cen_y = (int)(V2.ydim / 2);
        cen_x = (int)(V2.xdim / 2);
        cen_zp = (int)(V1.zdim / 2);
        cen_yp = (int)(V1.ydim / 2);
        cen_xp = (int)(V1.xdim / 2);
        minxp = -cen_xp;
        minyp = -cen_yp;
        minzp = -cen_zp;
        maxxp = V1.xdim - cen_xp - 1;
        maxyp = V1.ydim - cen_yp - 1;
        maxzp = V1.zdim - cen_zp - 1;

#ifdef DEBUG

        std::cout << "Geometry 2 center=("
        << cen_z  << "," << cen_y  << "," << cen_x  << ")\n"
        << "Geometry 1 center=("
        << cen_zp << "," << cen_yp << "," << cen_xp << ")\n"
        << "           min=("
        << minzp  << "," << minyp  << "," << minxp  << ")\n"
        << "           max=("
        << maxzp  << "," << maxyp  << "," << maxxp  << ")\n"
        ;
#endif

        if (SplineDegree > 1)
        {
            // Build the B-spline coefficients
            if (BcoeffsPtr!=NULL)
                BcoeffsToUse=BcoeffsPtr;
            else
            {
                produceSplineCoefficients(SplineDegree, Bcoeffs, V1); //Bcoeffs is a single image
                BcoeffsToUse = &Bcoeffs;
            }
            STARTINGX(*BcoeffsToUse) = (int) minxp;
            STARTINGY(*BcoeffsToUse) = (int) minyp;
            STARTINGZ(*BcoeffsToUse) = (int) minzp;
        }

        // Now we go from the output MultidimArray to the input MultidimArray, ie, for any
        // voxel in the output MultidimArray we calculate which are the corresponding
        // ones in the original MultidimArray, make an interpolation with them and put
        // this value at the output voxel

        // V2 is not initialised to 0 because all its pixels are rewritten
        for (size_t k = 0; k < V2.zdim; k++)
            for (size_t i = 0; i < V2.ydim; i++)
            {
                // Calculate position of the beginning of the row in the output
                // MultidimArray
                x = -cen_x;
                y = i - cen_y;
                z = k - cen_z;

                // Calculate this position in the input image according to the
                // geometrical transformation they are related by
                // coords_output(=x,y) = A * coords_input (=xp,yp)
                xp = x * MAT_ELEM(Aref, 0, 0) + y * MAT_ELEM(Aref, 0, 1) + z * MAT_ELEM(Aref, 0, 2) + MAT_ELEM(Aref, 0, 3);
                yp = x * MAT_ELEM(Aref, 1, 0) + y * MAT_ELEM(Aref, 1, 1) + z * MAT_ELEM(Aref, 1, 2) + MAT_ELEM(Aref, 1, 3);
                zp = x * MAT_ELEM(Aref, 2, 0) + y * MAT_ELEM(Aref, 2, 1) + z * MAT_ELEM(Aref, 2, 2) + MAT_ELEM(Aref, 2, 3);

                for (size_t j = 0; j < V2.xdim; j++)
                {
                    bool interp;
                    double tmp;

#ifdef DEBUG

                    bool show_debug = false;
                    if ((i == 0 && j == 0 && k == 0) ||
                        (i == V2.ydim - 1 && j == V2.xdim - 1 && k == V2.zdim - 1))
                        show_debug = true;

                    if (show_debug)
                        std::cout << "(x,y,z)-->(xp,yp,zp)= "
                        << "(" << x  << "," << y  << "," << z  << ") "
                        << "(" << xp << "," << yp << "," << zp << ")\n";
#endif

                    // If the point is outside the volume, apply a periodic
                    // extension of the volume, what exits by one side enters by
                    // the other
                    interp  = true;
                    bool x_isOut = XMIPP_RANGE_OUTSIDE(xp, minxp, maxxp);
                    bool y_isOut = XMIPP_RANGE_OUTSIDE(yp, minyp, maxyp);
                    bool z_isOut = XMIPP_RANGE_OUTSIDE(zp, minzp, maxzp);

                    if (wrap)
                    {
                        if (x_isOut)
                            xp = realWRAP(xp, minxp - 0.5, maxxp + 0.5);

                        if (y_isOut)
                            yp = realWRAP(yp, minyp - 0.5, maxyp + 0.5);

                        if (z_isOut)
                            zp = realWRAP(zp, minzp - 0.5, maxzp + 0.5);
                    }
                    else if (x_isOut || y_isOut || z_isOut)
                        interp = false;

                    if (interp)
                    {
                        if (SplineDegree == 1)
                        {
                            // Linear interpolation

                            // Calculate the integer position in input volume, be
                            // careful that it is not the nearest but the one at the
                            // top left corner of the interpolation square. Ie,
                            // (0.7,0.7) would give (0,0)
                            // Calculate also weights for point m1+1,n1+1
                            wx = xp + cen_xp;
                            m1 = (int) wx;
                            wx = wx - m1;
                            m2 = m1 + 1;
                            wy = yp + cen_yp;
                            n1 = (int) wy;
                            wy = wy - n1;
                            n2 = n1 + 1;
                            wz = zp + cen_zp;
                            o1 = (int) wz;
                            wz = wz - o1;
                            o2 = o1 + 1;

#ifdef DEBUG

                            if (show_debug)
                            {
                                std::cout << "After wrapping(xp,yp,zp)= "
                                << "(" << xp << "," << yp << "," << zp << ")\n";
                                std::cout << "(m1,n1,o1)-->(m2,n2,o2)="
                                << "(" << m1 << "," << n1 << "," << o1 << ") "
                                << "(" << m2 << "," << n2 << "," << o2 << ")\n";
                                std::cout << "(wx,wy,wz)="
                                << "(" << wx << "," << wy << "," << wz << ")\n";
                            }
#endif

                            // Perform interpolation
                            // if wx == 0 means that the rightest point is useless for
                            // this interpolation, and even it might not be defined if
                            // m1=xdim-1
                            // The same can be said for wy.
                            double wx_1=1-wx;
                            double wy_1=1-wy;
                            double wz_1=1-wz;

                            double aux1=wz_1 * wy_1;
                            double aux2=aux1*wx_1;
                            tmp  =  aux2 * DIRECT_A3D_ELEM(V1, o1, n1, m1);

                            if (wx != 0 && m2 < V1.xdim)
                                tmp += (aux1-aux2)* DIRECT_A3D_ELEM(V1, o1, n1, m2);

                            if (wy != 0 && n2 < V1.ydim)
                            {
                                aux1=wz_1 * wy;
                                aux2=aux1*wx_1;
                                tmp += aux2 * DIRECT_A3D_ELEM(V1, o1, n2, m1);
                                if (wx != 0 && m2 < V1.xdim)
                                    tmp += (aux1-aux2) * DIRECT_A3D_ELEM(V1, o1, n2, m2);
                            }

                            if (wz != 0 && o2 < V1.zdim)
                            {
                                aux1=wz * wy_1;
                                aux2=aux1*wx_1;
                                tmp += aux2 * DIRECT_A3D_ELEM(V1, o2, n1, m1);
                                if (wx != 0 && m2 < V1.xdim)
                                    tmp += (aux1-aux2) * DIRECT_A3D_ELEM(V1, o2, n1, m2);
                                if (wy != 0 && n2 < V1.ydim)
                                {
                                    aux1=wz * wy;
                                    aux2=aux1*wx_1;
                                    tmp += aux2 * DIRECT_A3D_ELEM(V1, o2, n2, m1);
                                    if (wx != 0 && m2 < V1.xdim)
                                        tmp += (aux1-aux2) * DIRECT_A3D_ELEM(V1, o2, n2, m2);
                                }
                            }

#ifdef DEBUG
                            if (show_debug)
                                std::cout <<
                                "tmp1=" << DIRECT_A3D_ELEM(V1, o1, n1, m1) << " "
                                << (T)(wz_1 *wy_1 *wx_1 * DIRECT_A3D_ELEM(V1, o1, n1, m1))
                                << std::endl <<
                                "tmp2=" << DIRECT_A3D_ELEM(V1, o1, n1, m2) << " "
                                << (T)(wz_1 *wy_1 * wx * DIRECT_A3D_ELEM(V1, o1, n1, m2))
                                << std::endl <<
                                "tmp3=" << DIRECT_A3D_ELEM(V1, o1, n2, m1) << " "
                                << (T)(wz_1 * wy *wx_1 * DIRECT_A3D_ELEM(V1, o1, n2, m1))
                                << std::endl <<
                                "tmp4=" << DIRECT_A3D_ELEM(V1, o1, n2, m2) << " "
                                << (T)(wz_1 * wy * wx * DIRECT_A3D_ELEM(V1, o2, n1, m1))
                                << std::endl <<
                                "tmp6=" << DIRECT_A3D_ELEM(V1, o2, n1, m2) << " "
                                << (T)(wz * wy_1 * wx * DIRECT_A3D_ELEM(V1, o2, n1, m2))
                                << std::endl <<
                                "tmp7=" << DIRECT_A3D_ELEM(V1, o2, n2, m1) << " "
                                << (T)(wz * wy *wx_1 * DIRECT_A3D_ELEM(V1, o2, n2, m1))
                                << std::endl <<
                                "tmp8=" << DIRECT_A3D_ELEM(V1, o2, n2, m2) << " "
                                << (T)(wz * wy * wx * DIRECT_A3D_ELEM(V1, o2, n2, m2))
                                << std::endl <<
                                "tmp= " << tmp << std::endl;
#endif

                            dAkij(V2 , k, i, j) = (T)tmp;
                        }
                        else if (SplineDegree==0)
                        {
                            dAkij(V2, k, i, j)=(T)A3D_ELEM(V1,(int)trunc(zp),(int)trunc(yp),(int)trunc(xp));
                        }
                        else
                        {
                            // B-spline interpolation
                            dAkij(V2, k, i, j) =
                                (T) BcoeffsToUse->interpolatedElementBSpline3D(xp, yp, zp, SplineDegree);
                        }
                    }
                    else
                        dAkij(V2, k, i, j) = outside;

                    // Compute new point inside input image
                    xp += Aref00;
                    yp += Aref10;
                    zp += Aref20;
                }
            }
    }
}

// Special case for input MultidimArrayGeneric
template<typename T>
void applyGeometry(int SplineDegree,
                   MultidimArray<T>& V2,
                   const MultidimArrayGeneric& V1,
                   const Matrix2D< double > &A, bool inv,
                   bool wrap, T outside = 0)
{
#define APPLYGEO(type)  applyGeometry(SplineDegree,V2, (*(MultidimArray<type>*)(V1.im)), A, inv, wrap, outside);
    SWITCHDATATYPE(V1.datatype, APPLYGEO)
#undef APPLYGEO
}



template<typename T>
void selfApplyGeometry(int SplineDegree,
                       MultidimArray<T>& V1,
                       const Matrix2D< double > &A, bool inv,
                       bool wrap, T outside = 0)
{
    MultidimArray<T> aux = V1;
    V1.initZeros();
    applyGeometry(SplineDegree, V1, aux, A, inv, wrap, outside);
}

//Special cases for complex arrays
template<>
void applyGeometry(int SplineDegree,
                   MultidimArray< std::complex<double> >& V2,
                   const MultidimArray< std::complex<double> >& V1,
                   const Matrix2D< double > &A, bool inv,
                   bool wrap, std::complex<double> outside, MultidimArray<double> *BcoeffsPtr);

//Special cases for complex arrays
template<>
void selfApplyGeometry(int SplineDegree,
                       MultidimArray< std::complex<double> >& V1,
                       const Matrix2D< double > &A, bool inv,
                       bool wrap, std::complex<double> outside);

// Special cases for MultidimArrayGeneric
void applyGeometry(int SplineDegree,
                   MultidimArrayGeneric &V2,
                   const MultidimArrayGeneric &V1,
                   const Matrix2D< double > &A, bool inv,
                   bool wrap, double outside);


template<typename T>
void produceSplineCoefficients(int SplineDegree,
                               MultidimArray< double > &coeffs,
                               const MultidimArray< T > &V1);

// Special case for complex arrays
void produceSplineCoefficients(int SplineDegree,
                               MultidimArray< double > &coeffs,
                               const MultidimArray< std::complex<double> > &V1);

template <typename T>
void produceImageFromSplineCoefficients(int SplineDegree,
                                        MultidimArray< T >& img,
                                        const MultidimArray< double > &coeffs);

template<typename T>
void rotate(int SplineDegree,
            MultidimArray<T>& V2,
            const MultidimArray<T>& V1,
            double ang, char axis = 'Z',
            bool wrap = xmipp_transformation::DONT_WRAP, T outside = 0)
{
    Matrix2D< double > tmp;
    if (V1.getDim()==2)
    {
        rotation2DMatrix(ang,tmp);
    }
    else if (V1.getDim()==3)
    {
        rotation3DMatrix(ang, axis, tmp);
    }
    else
        REPORT_ERROR(ERR_MULTIDIM_DIM,"rotate ERROR: rotate only valid for 2D or 3D arrays");

    applyGeometry(SplineDegree, V2, V1, tmp, xmipp_transformation::IS_NOT_INV, wrap, outside);
}

template<typename T>
void selfRotate(int SplineDegree,
                MultidimArray<T>& V1,
                double ang, char axis = 'Z',
                bool wrap = xmipp_transformation::DONT_WRAP, T outside = 0)
{
    MultidimArray<T> aux = V1;
    V1.initZeros();
    rotate(SplineDegree, V1, aux, ang, axis, wrap, outside);
}

template<typename T>
void translate(int SplineDegree,
               MultidimArray<T> &V2,
               const MultidimArray<T> &V1,
               const Matrix1D< double >& v,
               bool wrap = xmipp_transformation::WRAP, T outside = 0)
{
    Matrix2D< double > tmp;
    if (V1.getDim()==2)
        translation2DMatrix(v, tmp,true);
    else if (V1.getDim()==3)
        translation3DMatrix(v, tmp,true);
    else
        REPORT_ERROR(ERR_MULTIDIM_DIM,"translate ERROR: translate only valid for 2D or 3D arrays");
    applyGeometry(SplineDegree, V2, V1, tmp, xmipp_transformation::IS_INV, wrap, outside);
}

template<typename T>
void selfTranslate(int SplineDegree,
                   MultidimArray<T>& V1,
                   const Matrix1D< double >& v,
                   bool wrap = xmipp_transformation::WRAP, T outside = 0)
{
    MultidimArray<T> aux = V1;
    V1.initZeros();
    translate(SplineDegree, V1, aux, v, wrap, outside);
}

template<typename T>
void translateCenterOfMassToCenter(int SplineDegree,
                                   MultidimArray<T> &V2,
                                   const MultidimArray<T> &V1,
                                   bool wrap = xmipp_transformation::WRAP)
{
    V2 = V1;
    V2.setXmippOrigin();
    Matrix1D< double > center;
    V2.centerOfMass(center);
    center *= -1;
    translate(SplineDegree, V2, V1, center, wrap, 0);
}

template<typename T>
void selfTranslateCenterOfMassToCenter(int SplineDegree,
                                       MultidimArray<T> &V1,
                                       bool wrap = xmipp_transformation::WRAP)
{
    MultidimArray<T> aux = V1;
    V1.initZeros();
    translateCenterOfMassToCenter(SplineDegree, V1, aux, wrap);
}

template<typename T1, typename T>
void scaleToSize(int SplineDegree,
                 MultidimArray<T> &V2,
                 const MultidimArray<T1> &V1,
                 size_t Xdim, size_t Ydim, size_t Zdim = 1)
{
    if (Xdim == XSIZE(V1) && Ydim == YSIZE(V1) && \
        (!(V1.getDim()==3) || (Zdim == ZSIZE(V1))) )
    {
        typeCast(V1,V2);
        return;
    }

    Matrix2D< double > tmp;
    if (V1.getDim()==2)
    {
        tmp.initIdentity(3);
        tmp(0, 0) = (double) Xdim / (double) XSIZE(V1);
        tmp(1, 1) = (double) Ydim / (double) YSIZE(V1);
        V2.initZeros(1, 1, Ydim, Xdim);
    }
    else if (V1.getDim()==3)
    {
        tmp.initIdentity(4);
        tmp(0, 0) = (double) Xdim / (double) XSIZE(V1);
        tmp(1, 1) = (double) Ydim / (double) YSIZE(V1);
        tmp(2, 2) = (double) Zdim / (double) ZSIZE(V1);
        V2.initZeros(1, Zdim, Ydim, Xdim);
    }
    else
        REPORT_ERROR(ERR_MULTIDIM_DIM,"scaleToSize ERROR: scaleToSize only valid for 2D or 3D arrays");

    V2.setXmippOrigin();
    applyGeometry(SplineDegree, V2, V1, tmp, xmipp_transformation::IS_NOT_INV, xmipp_transformation::WRAP, (T)0);
}

template<typename T>
inline void scaleToSize(int SplineDegree,
                        MultidimArray<T> &V2,
                        const MultidimArrayGeneric &V1,int Xdim, int Ydim, int Zdim = 1)
{
#define SCALETOSIZE(type) scaleToSize(SplineDegree,V2,*((MultidimArray<type>*)(V1.im)),Xdim,Ydim,Zdim);
    SWITCHDATATYPE(V1.datatype,SCALETOSIZE)
#undef SCALETOSIZE
}

template<typename T>
inline void scaleToSize(int SplineDegree,
                        MultidimArrayGeneric &V2,
                        const MultidimArray<T> &V1,int Xdim, int Ydim, int Zdim = 1)
{
#define SCALETOSIZE(type) scaleToSize(SplineDegree,MULTIDIM_ARRAY_TYPE(V2,type),V1,Xdim,Ydim,Zdim);
    SWITCHDATATYPE(V1.datatype,SCALETOSIZE)
#undef SCALETOSIZE
}

void scaleToSize(int SplineDegree,
                        MultidimArrayGeneric &V2,
                        const MultidimArrayGeneric &V1,int Xdim, int Ydim, int Zdim = 1);

template<typename T>
void selfScaleToSize(int SplineDegree,
                     MultidimArray<T> &V1,
                     int Xdim, int Ydim, int Zdim = 1)
{
    MultidimArray<T> aux = V1;
    scaleToSize(SplineDegree, V1, aux, Xdim, Ydim, Zdim);
}

void selfScaleToSize(int SplineDegree,
                     MultidimArrayGeneric &V1,
                     int Xdim, int Ydim, int Zdim = 1);


// Special case for complex arrays
void scaleToSize(int SplineDegree,
                 MultidimArray< std::complex<double> > &V2,
                 const MultidimArray< std::complex<double> > &V1,
                 int Xdim, int Ydim, int Zdim = 1);

// Special case for complex arrays
void selfScaleToSize(int SplineDegree,
                     MultidimArray< std::complex<double> > &V1,
                     int Xdim, int Ydim, int Zdim = 1);


template<typename T>
void reduceBSpline(int SplineDegree,
                   MultidimArray< double >& V2,
                   const MultidimArray<T> &V1);

template<typename T>
void expandBSpline(int SplineDegree,
                   MultidimArray< double >& V2,
                   const MultidimArray<T> &V1);

template<typename T>
void pyramidReduce(int SplineDegree,
                   MultidimArray<T> &V2,
                   const MultidimArray<T> &V1,
                   int levels = 1)
{
    MultidimArray< double > coeffs, coeffs2;

    produceSplineCoefficients(SplineDegree, coeffs, V1);

    for (int i = 0; i < levels; i++)
    {
        reduceBSpline(SplineDegree, coeffs2, coeffs);
        coeffs = coeffs2;
    }

    produceImageFromSplineCoefficients(SplineDegree, V2, coeffs);
}

template<typename T>
void selfPyramidReduce(int SplineDegree,
                       MultidimArray<T> &V1,
                       int levels = 1)
{
    MultidimArray<T> aux = V1;
    V1.initZeros();
    pyramidReduce(SplineDegree, V1, aux, levels);
}

void selfPyramidReduce(int SplineDegree,
                       MultidimArrayGeneric &V1,
                       int levels = 1);


template<typename T>
void pyramidExpand(int SplineDegree,
                   MultidimArray<T> &V2,
                   const MultidimArray<T> &V1,
                   int levels = 1)
{
    MultidimArray< double > coeffs, coeffs2;

    produceSplineCoefficients(SplineDegree, coeffs, V1);

    for (int i = 0; i < levels; i++)
    {
        expandBSpline(SplineDegree, coeffs2, coeffs);
        coeffs = coeffs2;
    }

    produceImageFromSplineCoefficients(SplineDegree, V2, coeffs);

}

void pyramidExpand(int SplineDegree,
                   MultidimArrayGeneric &V2,
                   const MultidimArrayGeneric &V1,
                   int levels = 1);

void pyramidReduce(int SplineDegree,
                   MultidimArrayGeneric &V2,
                   const MultidimArrayGeneric &V1,
                   int levels = 1);

template<typename T>
void selfPyramidExpand(int SplineDegree,
                       MultidimArray<T> &V1,
                       int levels = 1)
{
    MultidimArray<T> aux = V1;
    V1.initZeros();
    pyramidExpand(SplineDegree, V1, aux, levels);
}

void selfPyramidExpand(int SplineDegree,
                       MultidimArrayGeneric &V1,
                       int levels = 1);


template<typename T>
void radialAverage(const MultidimArray< T >& m,
                   Matrix1D< int >& center_of_rot,
                   MultidimArray< T >& radial_mean,
                   MultidimArray< int >& radial_count,
                   const bool& rounding = false)
{
    Matrix1D< double > idx(3);

    // If center_of_rot was written for 2D image
    if (center_of_rot.size() < 3)
        center_of_rot.resize(3);

    // First determine the maximum distance that one should expect, to set the
    // dimension of the radial average vector
    MultidimArray< int > distances(8);

    double z = STARTINGZ(m) - ZZ(center_of_rot);
    double y = STARTINGY(m) - YY(center_of_rot);
    double x = STARTINGX(m) - XX(center_of_rot);

    distances(0) = (int) floor(sqrt(x * x + y * y + z * z));
    x = FINISHINGX(m) - XX(center_of_rot);

    distances(1) = (int) floor(sqrt(x * x + y * y + z * z));
    y = FINISHINGY(m) - YY(center_of_rot);

    distances(2) = (int) floor(sqrt(x * x + y * y + z * z));
    x = STARTINGX(m) - XX(center_of_rot);

    distances(3) = (int) floor(sqrt(x * x + y * y + z * z));
    z = FINISHINGZ(m) - ZZ(center_of_rot);

    distances(4) = (int) floor(sqrt(x * x + y * y + z * z));
    x = FINISHINGX(m) - XX(center_of_rot);

    distances(5) = (int) floor(sqrt(x * x + y * y + z * z));
    y = STARTINGY(m) - YY(center_of_rot);

    distances(6) = (int) floor(sqrt(x * x + y * y + z * z));
    x = STARTINGX(m) - XX(center_of_rot);

    distances(7) = (int) floor(sqrt(x * x + y * y + z * z));

    int dim = (int) CEIL(distances.computeMax()) + 1;
    if (rounding)
        dim++;

    // Define the vectors
    radial_mean.initZeros(dim);
    radial_count.initZeros(dim);

    // Perform the radial sum and count pixels that contribute to every
    // distance
    FOR_ALL_ELEMENTS_IN_ARRAY3D(m)
    {
        ZZ(idx) = k - ZZ(center_of_rot);
        YY(idx) = i - YY(center_of_rot);
        XX(idx) = j - XX(center_of_rot);

        // Determine distance to the center
        ;
        double module = sqrt(ZZ(idx)*ZZ(idx)+YY(idx)*YY(idx)+XX(idx)*XX(idx));
        int distance = (rounding) ? (int) round(module) : (int) floor(module);

        // Sum the value to the pixels with the same distance
        DIRECT_MULTIDIM_ELEM(radial_mean,distance) += A3D_ELEM(m, k, i, j);

        // Count the pixel
        DIRECT_MULTIDIM_ELEM(radial_count,distance)++;
    }

    // Perform the mean
    FOR_ALL_DIRECT_ELEMENTS_IN_ARRAY1D(radial_mean)
      if (DIRECT_MULTIDIM_ELEM(radial_count,i) > 0)
        DIRECT_MULTIDIM_ELEM(radial_mean,i) /= DIRECT_MULTIDIM_ELEM(radial_count,i);
}

template<typename T>
void radialAveragePrecomputeDistance(const MultidimArray< T >& m,
                                     Matrix1D< int >& center_of_rot,
                                     MultidimArray< int >& distance,
                                     int &dim,
                                     const bool& rounding = false)
{
    Matrix1D< double > idx(3);

    // If center_of_rot was written for 2D image
    if (center_of_rot.size() < 3)
        center_of_rot.resize(3);

    // First determine the maximum distance that one should expect, to set the
    // dimension of the radial average vector
    MultidimArray< int > distances(8);

    double z = STARTINGZ(m) - ZZ(center_of_rot);
    double y = STARTINGY(m) - YY(center_of_rot);
    double x = STARTINGX(m) - XX(center_of_rot);

    distances(0) = (int) floor(sqrt(x * x + y * y + z * z));
    x = FINISHINGX(m) - XX(center_of_rot);

    distances(1) = (int) floor(sqrt(x * x + y * y + z * z));
    y = FINISHINGY(m) - YY(center_of_rot);

    distances(2) = (int) floor(sqrt(x * x + y * y + z * z));
    x = STARTINGX(m) - XX(center_of_rot);

    distances(3) = (int) floor(sqrt(x * x + y * y + z * z));
    z = FINISHINGZ(m) - ZZ(center_of_rot);

    distances(4) = (int) floor(sqrt(x * x + y * y + z * z));
    x = FINISHINGX(m) - XX(center_of_rot);

    distances(5) = (int) floor(sqrt(x * x + y * y + z * z));
    y = STARTINGY(m) - YY(center_of_rot);

    distances(6) = (int) floor(sqrt(x * x + y * y + z * z));
    x = STARTINGX(m) - XX(center_of_rot);

    distances(7) = (int) floor(sqrt(x * x + y * y + z * z));

    dim = (int) CEIL(distances.computeMax()) + 1;
    if (rounding)
        dim++;

    // Perform the radial sum and count pixels that contribute to every
    // distance
    distance.initZeros(m);
    FOR_ALL_ELEMENTS_IN_ARRAY3D(m)
    {
        ZZ(idx) = k - ZZ(center_of_rot);
        YY(idx) = i - YY(center_of_rot);
        XX(idx) = j - XX(center_of_rot);

        // Determine distance to the center
        if (rounding)
            A3D_ELEM(distance,k,i,j) = (int) round(idx.module());
        else
            A3D_ELEM(distance,k,i,j) = (int) floor(idx.module());
    }
}

template<typename T>
void fastRadialAverage(const MultidimArray< T >& m,
                       const MultidimArray< int >& distance,
                       int dim,
                       MultidimArray< T >& radial_mean,
                       MultidimArray< int >& radial_count)
{
    // Define the vectors
    radial_mean.initZeros(dim);
    radial_count.initZeros(dim);

    // Perform the radial sum and count pixels that contribute to every
    // distance
    FOR_ALL_DIRECT_ELEMENTS_IN_MULTIDIMARRAY(m)
    {
        int d=DIRECT_MULTIDIM_ELEM(distance,n);
        A1D_ELEM(radial_mean,d) += DIRECT_MULTIDIM_ELEM(m,n);
        ++A1D_ELEM(radial_count,d);
    }

    // Perform the mean
    FOR_ALL_DIRECT_ELEMENTS_IN_ARRAY1D(radial_mean)
      if (DIRECT_MULTIDIM_ELEM(radial_count,i) > 0)
        DIRECT_MULTIDIM_ELEM(radial_mean,i) /= DIRECT_MULTIDIM_ELEM(radial_count,i);
}

template<typename T>
void radialAverageAxis(const MultidimArray< T >& in, char axis, MultidimArray< double >& out)
{
    MultidimArray<double> inCentered;
    inCentered.alias(in);
    inCentered.setXmippOrigin();
    if (axis=='z')
    {
        out.initZeros(ZSIZE(in),XSIZE(in));
        out.setXmippOrigin();
        for (int i=STARTINGY(inCentered); i<=FINISHINGY(inCentered); ++i)
        {
            double z=i;
            for (int j=0; j<XSIZE(out)/2; ++j)
            {
                for (double ang=0; ang<TWOPI; ang+=TWOPI/72)
                {
                    double x=j*cos(ang);
                    double y=j*sin(ang);
                    double val=inCentered.interpolatedElement3D(x,y,z);
                    A2D_ELEM(out,i,j)+=val;
                }
                A2D_ELEM(out,i,j)/=73;
                A2D_ELEM(out,i,-j)=A2D_ELEM(out,i,j);
            }
        }
    }

    else
        REPORT_ERROR(ERR_ARG_INCORRECT,"Not implemented yet");
}

template<typename T>
void radialAverageNonCubic(const MultidimArray< T >& m,
        Matrix1D< int >& center_of_rot,
        MultidimArray< T >& radial_mean,
        MultidimArray< int >& radial_count,
        bool rounding = false);

void radiallySymmetrize(const MultidimArray< double >& img, MultidimArray<double> &radialImg);

double interpolatedElementBSplineDiffX(MultidimArray<double> &vol, double x, double y, double z,
                                       int SplineDegree = 3);

double interpolatedElementBSplineDiffY(MultidimArray<double> &vol, double x, double y, double z,
                                       int SplineDegree = 3);
double interpolatedElementBSplineDiffZ(MultidimArray<double> &vol, double x, double y, double z,
                                       int SplineDegree = 3);
#endif