MatrixSqrt.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __geometry_MatrixSqrt_h__
#define __geometry_MatrixSqrt_h__

#define FE_MSQ_DEBUG        FALSE
#define FE_MSQ_VERIFY       (FE_CODEGEN<=FE_DEBUG)
#define FE_MSQ_MAX_ERROR_R  1e-3
#define FE_MSQ_MAX_ERROR_T  1e-2

//* 0 = Denman-Beavers  two inverses per iteration
//* 1 = Meini           one inverse per iteration
//* 2 = Schulz          no inverses
#define FE_MSQ_METHOD   0

//* NOTE Meini and Shulz use the (3,3) element, so a 3x4 typename will fail

//* NOTE Schulz doesn't seem to converge very well

namespace fe
{
namespace ext
{

/**************************************************************************//**
    @brief solve A=B*B for B, given A

    @ingroup geometry

    Uses Denman-Beavers square root iteration.

    B will be set with best result even with a convergence exception,
    so the exception could be caught and ignored.
*//***************************************************************************/
template <typename MATRIX>
class MatrixSqrt
{
    public:
                MatrixSqrt(void):
                    m_iterations(8)
                {}

        void    solve(MATRIX& B, const MATRIX& A) const;

        void    setIterations(U32 iterations)   { m_iterations=iterations; }

    private:
        U32     m_iterations;
};

template <typename MATRIX>
inline void MatrixSqrt<MATRIX>::solve(MATRIX& B, const MATRIX& A) const
{
#if FE_MSQ_DEBUG
    feLog("\nA\n%s\n",c_print(A));
#endif

    MATRIX AA=A;
    MATRIX correction;

    //* cancellation protection
    const BWORD fix0=(fabs(AA(0,0)+1)<1e-3);
    const BWORD fix1=(fabs(AA(1,1)+1)<1e-3);
    const BWORD fix2=(fabs(AA(2,2)+1)<1e-3);
    const BWORD fix=(fix0 || fix1 || fix2);

    if(fix)
    {
        Matrix<3,4,F64> doubleY=AA;

        Quaternion<F64> quat=doubleY;

        F64 radians;
        Vector<3,F64> axis;
        quat.computeAngleAxis(radians,axis);

        const F64 tiny=0.03;
        const F64 tinyAngle=tiny*radians;

        Quaternion<F64> forward(0.5*tinyAngle,axis);
        const Matrix<3,4,F64> correct64(forward);
        correction=correct64;
        const SpatialVector forwardT=0.5*tiny*doubleY.translation();
        translate(correction,forwardT);

        Quaternion<F64> reverse(-tinyAngle,axis);
        const Matrix<3,4,F64> tweak64(reverse);
        MATRIX tweak=tweak64;
        const SpatialVector reverseT= -tiny*doubleY.translation();
        translate(tweak,reverseT);

#if FE_MSQ_METHOD!=0
        correction(3,3)=1;
        tweak(3,3)=1;
#endif

//      feLog("ty\n%s\nyt\n%s\n",
//              c_print(tweak*AA),
//              c_print(AA*tweak));

        AA=tweak*AA;

#if FE_MSQ_DEBUG
        feLog("\ntweak\n%s\n",c_print(tweak));
        feLog("\ncorrection\n%s\n",c_print(correction));
#endif
    }

    MATRIX Y[2];
    MATRIX Z[2];
    U32 current=0;

    Y[0]=AA;
    setIdentity(Z[0]);

#if FE_MSQ_METHOD==0
    //* Denman-Beavers
    MATRIX invY;
    MATRIX invZ;
#elif FE_MSQ_METHOD==1
    //* Meini
    Y[1]=Y[0];
    Y[0]=Z[0]-Y[1];     //* I-A'
    Z[0]=2*(Z[0]+Y[1]); //* 2(I+A')

    MATRIX invZ;
#else
    //* Schulz

    MATRIX I3;
    setIdentity(I3);
    I3*=3;
#endif

    U32 iteration;
    for(iteration=0;iteration<m_iterations;iteration++)
    {
        U32 last=current;
        current= !current;

#if FE_MSQ_DEBUG
        feLog("\n>>>> iteration %d\nY\n%s\nZ\n%s\n",iteration,
                c_print(Y[last]),
                c_print(Z[last]));
        feLog("Y*Y\n%s\nZ*Z\n%s\n",
                c_print(Y[last]*Y[last]),
                c_print(Z[last]*Z[last]));
#endif

#if FE_MSQ_METHOD==0

        //* Denman-Beavers (1976)

        invert(invY,Y[last]);
        invert(invZ,Z[last]);

#if FE_MSQ_DEBUG
        feLog("invY\n%s\n",
                c_print(invY));
        feLog("invZ\n%s\n",
                c_print(invZ));
        feLog("Y+invZ\n%s\nZ+invY\n%s\n",
                c_print(Y[last]+invZ),
                c_print(Z[last]+invY));
#endif

        Y[current]=0.5*(Y[last]+invZ);
        Z[current]=0.5*(Z[last]+invY);

#elif FE_MSQ_METHOD==1

        //* Meini (2004)

        invert(invZ,Z[last]);

#if FE_MSQ_DEBUG
        feLog("invZ\n%s\n",
                c_print(invZ));
#endif

        Y[current]= -1*Y[last]*invZ*Y[last];
        Z[current]=Z[last]+2*Y[current];

#else

        //* Schulz

        MATRIX I3ZY=I3-Z[last]*Y[last];

        Y[current]=0.5*Y[last]*I3ZY;
        Z[current]=0.5*I3ZY*Z[last];

#endif
    }

#if FE_MSQ_METHOD==0
    //* Denman-Beavers
    MATRIX& R=Y[current];
#elif FE_MSQ_METHOD==1
    //* Meini
    MATRIX R=0.25*Z[current];
    R(3,3)=1;
#else
    //* Schulz
    MATRIX& R=Y[current];
    R(3,3)=1;
#endif

#if FE_MSQ_DEBUG
    feLog("\nA\n%s\n",c_print(A));
    if(fix)
    {
        feLog("\nA'\n%s\n",c_print(AA));
    }
    feLog("\nB'\n%s\nB'*B'\n%s\n",c_print(R),c_print(R*R));
#endif

    if(fix)
    {
        B=correction*R;

#if FE_MSQ_DEBUG
        feLog("\ncorrection\n%s\n",c_print(correction));
        feLog("\ncorrected\n%s\n",c_print(B));
        feLog("\nsquared\n%s\n",c_print(B*B));
#endif
    }
    else
    {
        B=R;
    }

#if FE_MSQ_VERIFY
    BWORD invalid=FALSE;
    MATRIX diff=AA-R*R;
    F32 sumR=0.0f;
    F32 sumT=0.0f;
    for(U32 m=0;m<width(diff);m++)
    {
        U32 n;
        for(n=0;n<height(diff)-1;n++)
        {
            if(FE_INVALID_SCALAR(diff(m,n)))
            {
                invalid=TRUE;
            }
            sumR+=fabs(diff(m,n));
        }
        if(FE_INVALID_SCALAR(diff(m,n)))
        {
            invalid=TRUE;
        }
        sumT+=fabs(diff(m,n));
    }
#endif
#if FE_MSQ_VERIFY && FE_MSQ_DEBUG
    feLog("\ndiff\n%s\ncomponent sumR=%.6G\n",c_print(diff),sumR);
#endif
#if FE_MSQ_VERIFY
    if(invalid || sumR>FE_MSQ_MAX_ERROR_R || sumT>FE_MSQ_MAX_ERROR_T)
    {
        feLog("MatrixSqrt< %s >::solve"
                " error of %.6G,%.6G exceeded limit of %.6G,%.6G\n",
                FE_TYPESTRING(MATRIX).c_str(),
                sumR,sumT,FE_MSQ_MAX_ERROR_R,FE_MSQ_MAX_ERROR_T);
        feLog("\nA'\n%s\n",c_print(AA));
        feLog("\nB'\n%s\nB'*B'\n%s\n",c_print(R),c_print(R*R));

        feX("MatrixSqrt<>::solve","failed to converge");
    }
#endif
}

} /* namespace ext */
} /* namespace fe */

#endif /* __geometry_MatrixSqrt_h__ */