Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Common - Math API [F].c_inc
    »Author«    Andrew Aye (EMail: mailto:andrew.aye@gmail.com, Web: http://www.andrewaye.com)
    »Version«   4.51 / »GUID« A9981407-3EC9-42AF-8B6F-8BE6DD919615                                                                                                        */
/*   -------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
/*  Copyright: © 2002-2017, Andrew Aye.  All Rights Reserved.
    This software is free for non-commercial use.  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
      following conditions are met:
        Redistribution of source code must retain this copyright notice, this list of conditions and the following disclaimers.
        Redistribution in binary form must reproduce this copyright notice, this list of conditions and the following disclaimers in the documentation and other materials
          provided with the distribution.
    The name of the author may not be used to endorse or promote products derived from this software without specific prior written permission.
    The intellectual property rights of the algorithms used reside with Andrew Aye.
    You may not use this software, in whole or in part, in support of any commercial product without the express written consent of the author.
    There is no warranty or other guarantee of fitness of this software for any purpose. It is provided solely "as is".                                                   */
/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/* == Common ============================================================================================================================================================ */

/* ---- Van Wijngaarden-Dekker-Brent Root Finder ------------------------------------------------------------------------------------------------------------------------ */
/*   Guaranteed to find a root to the equation given that the two initial parameters straddle a solution point (ie. one positive and one negative value) - IVT guarantees at */
/* least one theoretical solution.  Iterative technique will refine solution until its within the passed in tolerance value.  Implementation from Numerical Recipes. */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgBOOL F(F_BrentZ)(PCU_TYPE pfT0, PCU_TgVOID pParam, TYPE( *pfnFunc ) (PCU_TgVOID, const TYPE), TYPE fTA, TYPE fTC)
{
    TYPE                                fValA = (*pfnFunc)(pParam, fTA);
    TYPE                                fValC = (*pfnFunc)(pParam, fTC);
    TYPE                                fTB = fTC;
    TYPE                                fValB, p, q, d, e;
    TgSINT32                            iCount;

    if (fValA*fValC > MKL(0.0))
    {
        /* Root must be bracketed for IVT to be valid. */
        return (TgFALSE);
    };

    fValB = fValC;
    fValC = fValA;
    fTC = fTA;
    e = d = fTB - fTA;

    for (iCount = 1; iCount <= 100; ++iCount)
    {
        TYPE                                fTol1, fTM;

        if (F(tgPM_ABS)(fValC) < F(tgPM_ABS)(fValB))
        {
            fTA = fTB;
            fTB = fTC;
            fTC = fTA;

            fValA = fValB;
            fValB = fValC;
            fValC = fValA;
        }

        /* Convergence check. */

        fTol1 = MKL(2.0)*F(KTgROOT_EPS)*F(tgPM_ABS)(fTB) + MKL(0.5)*F(KTgROOT_EPS);
        fTM = MKL(0.5)*(fTC - fTB);

        if (F(tgPM_ABS)(fTM) <= fTol1 || fValB == MKL(0.0))
        {
            *pfT0 = fTB;
            return (TgTRUE);
        };

        if (F(tgPM_ABS)(e) >= fTol1 && F(tgPM_ABS)(fValA) > F(tgPM_ABS)(fValB))
        {
            /* Attempt inverse quadratic interpolation. */

            const TYPE                          fBA = fValB / fValA, fBC = fValB / fValC, fAC = fValA / fValC;

            p = F(tgPM_FSEL)(-F(tgPM_ABS)(fTA - fTC), MKL(2.0)*fTM*fBA,
                fBA*(MKL(2.0)*fTM*fAC*(fAC - fBC) - (fTB - fTA)*(fBC - MKL(1.0)))
            );
            q = F(tgPM_FSEL)(-F(tgPM_ABS)(fTA - fTC), MKL(1.0) - fBA, (fAC - MKL(1.0))*(fBC - MKL(1.0))*(fBA - MKL(1.0)));

            /* Check whether in bounds. */

            q = F(tgPM_FSEL)(p, -q, q);
            p = F(tgPM_ABS)(p);

            {
                const TYPE                          fMin1 = MKL(3.0)*fTM*q - F(tgPM_ABS)(fTol1*q);
                const TYPE                          fMin2 = F(tgPM_ABS)(e*q);
                const TYPE                          fInt = F(tgPM_FSEL)(fMin2 - fMin1, fMin1, fMin2) - MKL(2.0)*p;

                e = F(tgPM_FSEL)(fInt, d, fTM);
                d = F(tgPM_FSEL)(fInt, p / q, fTM);
            }
/*          F(tgPM_FSEL)( F(tgPM_FSEL)( fMin2 - fMin1, fMin1, fMin2 ) - TYPE(2.0)*p, Accept interpolation, Use bisection ); */
        }
        else
        {
            /* Bounds decreasing too slowly, use bisection. */

            d = fTM;
            e = fTM;
        }

        /* Move last best guess to fTA. */

        fTA = fTB;
        fValA = fValB;

        /* Evaluate new trial root. */

        fTB += F(tgPM_FSEL)(F(tgPM_ABS)(d) - fTol1, d, F(tgPM_FSEL)(fTM, fTol1, -fTol1));
        fValB = (*pfnFunc)(pParam, fTB);

        if (fValB*fValC > MKL(0.0))
        {
            /* Rename fTA, fTB, fTC and adjust bounding interval */

            fTC = fTA;
            fValC = fValA;
            e = d = fTB - fTA;
        }
    }

    return (TgFALSE);
}


/* ---- Van Wijngaarden-Dekker-Brent Root Finder ------------------------------------------------------------------------------------------------------------------------ */
/*  Given a function fn, and given a bracketing triplet of abscissas t1, t2, t3 (such that t2 is between t1 and t3, and f(t1) is less than both f(t1) and f(t3)), this */
/* routine isolates the minimum to a fractional precision of about tol using Brent's method.  See Numerical Recipes in C, section 10.2 */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
CLANG_WARN_DISABLE_PUSH(float-equal)
TgBOOL F(F_BrentD)(
    PCU_TYPE pfT0, PCU_TYPE ptyV0, PCU_TgVOID pParam, TYPE( *pfnFunc0 )(PCU_TgVOID, const TYPE),
    TYPE( *pfnFunc1 )(PCU_TgVOID, const TYPE), TYPE fT1, TYPE fT2, TYPE fT3
)
{
    TYPE                                fTM, fD = MKL(0.0), fE = MKL(0.0);
    TYPE                                fU, fFU, fDU;
    TYPE                                fV, fFV, fDV;
    TYPE                                fW, fFW, fDW;
    TYPE                                fX, fFX, fDX;
    TYPE                                fA = F(tgPM_FSEL)(fT3 - fT1, fT1, fT3);
    TYPE                                fB = F(tgPM_FSEL)(fT1 - fT3, fT1, fT3);
    TYPE                                fPrevD, fPrevE;
    TgSINT32                            niCount;

    fD = MKL( 0.0 );
    fE = MKL( 0.0 );
    fPrevD = MKL( 0.0 );
    fPrevE = MKL( 0.0 );
    fX = fW = fV = fT2;
    fFX = fFW = fFV = (*pfnFunc0) (pParam, fX);
    fDX = fDW = fDV = (*pfnFunc1) (pParam, fX);

    for (niCount = 1; niCount <= 100; ++niCount)
    {
        const TYPE                          fTol1 = F(KTgROOT_EPS) * F(tgPM_ABS)(fX) + F(KTgEPS);
        const TYPE                          fTol2 = MKL(2.0) * fTol1;

        TgDIAG( fTol1 > MKL(0.0) );

        fTM = MKL(0.5)*(fA + fB);

        if (F(tgPM_ABS)(fX - fTM) <= (fTol2 - MKL(0.5)*(fB - fA)))
        {
            *pfT0 = fX;
            *ptyV0 = fFX;
            return (TgTRUE);
        };

        fPrevD = fD;

        fD = MKL(0.5) * (fE = F(tgPM_FSEL)(fDX, fA - fX, fB - fX));

        if (F(tgPM_ABS)(fPrevE) - fTol1 > MKL(0.0))
        {
            const TYPE fK0 = fDW - fDX;
            const TYPE fK1 = fK0 <= F( KTgEPS ) ? MKL(0.0) : MKL(1.0) / fK0;
            const TYPE fD1 = F(tgPM_FSEL)( -F(tgPM_ABS)(fK0), (fX - fW)*fDX *fK1, MKL(2.0)*(fB - fA) );
            const TYPE fK2 = fDV - fDX;
            const TYPE fK3 = fK2 <= F( KTgEPS ) ? MKL( 0.0 ) : MKL( 1.0 ) / fK0;
            const TYPE fD2 = F(tgPM_FSEL)( -F(tgPM_ABS)(fK2), (fX - fV)*fDX * fK3, fD1 );
            const TYPE fOK1 = F(tgPM_FSEL)( (fA - fX - fD1)*(fX + fD1 - fB), F(tgPM_FSEL)(-fDX*fD1, MKL(1.0), MKL(-1.0)), MKL(-1.0) );
            const TYPE fOK2 = F(tgPM_FSEL)( (fA - fX - fD2)*(fX + fD2 - fB), F(tgPM_FSEL)(-fDX*fD2, MKL(1.0), MKL(-1.0)), MKL(-1.0) );

            const TYPE fTMP = F(tgPM_FSEL)( fOK1, F(tgPM_FSEL)(fOK2, F(tgPM_FSEL)(F(tgPM_ABS)(fD2) - F(tgPM_ABS)(fD1), fD1, fD2), fD1), fD2 );

            if ((fOK1 + fOK2 >= MKL(0.0)) && (F(tgPM_ABS)(MKL(0.5) * fPrevE) - F(tgPM_ABS)(fTMP) >= MKL(0.0)))
            {
                const TYPE                      fNewD = F(tgPM_COPY_SIGN)(fTol1, fTM - fX);

                fPrevE = fE;
                fE = fPrevD;
                fD = F(tgPM_FSEL)(fA - fX - fTMP + fTol2, fNewD, F(tgPM_FSEL)(fA - fX - fTMP + fTol2, fNewD, fTMP));
            }
        }

        fU = F(tgPM_FSEL)(F(tgPM_ABS)(fD) - fTol1, fX + fD, fX + F(tgPM_COPY_SIGN)(fTol1, fD));
        fFU = (*pfnFunc0) (pParam, fU);

        if (fFU <= fFX)
        {
            fDU = (*pfnFunc1) (pParam, fU);

            fA = F(tgPM_FSEL)(fU - fX, fX, fA);
            fB = F(tgPM_FSEL)(fU - fX, fB, fX);

            fV = fW;
            fFV = fFW;
            fDV = fDW;

            fW = fX;
            fFW = fFX;
            fDW = fDX;

            fX = fU;
            fFX = fFU;
            fDX = fDU;
        }
        else
        {
            if (F(tgPM_ABS)(fD) < fTol1)
            {
                *pfT0 = fX;
                *ptyV0 = fFX;
                return (TgTRUE);
            }

            fA = F(tgPM_FSEL)(fX - fU, fU, fA);
            fB = F(tgPM_FSEL)(fX - fU, fB, fU);

            fDU = (*pfnFunc1) (pParam, fU);

            if (fFU <= fFW || fW == fX)
            {
                fV = fW;
                fFV = fFW;
                fDV = fDW;

                fW = fU;
                fFW = fFU;
                fDW = fDU;
            }
            else if (fFU < fFV || fV == fX || fV == fW)
            {
                fV = fU;
                fFV = fFU;
                fDV = fDU;
            }
        }
    }

    return (TgFALSE);
}
CLANG_WARN_DISABLE_POP(float-equal)


/* ---- Degree 1 Algebraic Solver --------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgBOOL F(F_Calc_Root_1)(PCU_TYPE atyRoot, PCU_TgSINT32 piCount, TYPE fC0, TYPE fC1)
{
    if (F(tgPM_ABS)(fC1) >= F(KTgROOT_EPS))
    {
        atyRoot[0] = -(fC0 / fC1);
        *piCount = 1;
        return (TgTRUE);
    }
    else
    {
        *piCount = 0;
        return (TgFALSE);
    };
}


/* ---- Degree 2 Algebraic Solver --------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgBOOL F(F_Calc_Root_2)(PCU_TYPE atyRoot, PCU_TgSINT32 piCount, TYPE fC0, TYPE fC1, TYPE fC2)
{
    if (F(tgPM_ABS)(fC2) <= F(KTgROOT_EPS))
    {
        return (F(F_Calc_Root_1)(atyRoot, piCount, fC0, fC1));
    }
    else
    {
        TYPE                                fDSC = fC1*fC1 - MKL(4.0)*fC0*fC2;
        TYPE                                fTMP00;

        if (F(tgPM_ABS)(fDSC) <= F(KTgROOT_EPS))
        {
            fDSC = MKL(0.0);
        };

        if (fDSC < MKL(0.0))
        {
            *piCount = 0;
            return (TgFALSE);
        };

        fTMP00 = MKL(0.5) / fC2;

        if (fDSC > MKL(0.0))
        {
            fDSC = F(tgPM_SQRT)(fDSC);

            atyRoot[0] = fTMP00*(-fC1 - fDSC);
            atyRoot[1] = fTMP00*(-fC1 + fDSC);

            *piCount = 2;
        }
        else
        {
            atyRoot[0] = -fTMP00*fC1;
            atyRoot[1] = -fTMP00*fC1;

            *piCount = 1;
        }

        return (TgTRUE);
    };
}


/* ---- Degree 3 Algebraic Solver --------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgBOOL F(F_Calc_Root_3)(PCU_TYPE atyRoot, PCU_TgSINT32 piCount, TYPE fC0, TYPE fC1, TYPE fC2, TYPE fC3)
{
    if (F(tgPM_ABS)(fC3) <= F(KTgEPS))
    {
        return (F(F_Calc_Root_2)(atyRoot, piCount, fC0, fC1, fC2));
    };

    /* Make polynomial monic, x^3+c2*x^2+c1*x+c0 */

    if (fC3 != MKL(1.0))
    {
        const TYPE                          fInvC3 = MKL(1.0) / fC3;

        fC0 *= fInvC3;
        fC1 *= fInvC3;
        fC2 *= fInvC3;
    };

    /* Convert to y^3+a*y+b = 0 by x = y-c2/3 */

    {
        const TYPE                          fOffset = fC2 / MKL(3.0);
        const TYPE                          fA = fC1 - fC2*fOffset;
        const TYPE                          fTMP00 = fC2*fC2;
        const TYPE                          fB = fC0 + fC2*(fTMP00 + fTMP00 - MKL(9.0)*fC1)*(MKL(1.0) / MKL(27.0));
        const TYPE                          fHalfB = MKL(0.5)*fB;

        TYPE                                fDSC = fHalfB*fHalfB + fA*fA*fA*(MKL(1.0) / MKL(27.0));

        if (F(tgPM_ABS)(fDSC) <= F(KTgROOT_EPS))
        {
            fDSC = MKL(0.0);
        };

        if (fDSC > MKL(0.0))
        {
            const TYPE                          fDSC_SQRT = F(tgPM_SQRT)(fDSC);
            const TYPE                          fTMP01 = -fHalfB + fDSC_SQRT;
            const TYPE                          fTMP02 = -fHalfB - fDSC_SQRT;

            if (fTMP01 >= MKL(0.0))
            {
                atyRoot[0] = F(tgPM_POW)(fTMP01, MKL(1.0) / MKL(3.0));
            }
            else
            {
                atyRoot[0] = -F(tgPM_POW)(-fTMP01, MKL(1.0) / MKL(3.0));
            }

            if (fTMP02 >= MKL(0.0))
            {
                atyRoot[0] += F(tgPM_POW)(fTMP02, MKL(1.0) / MKL(3.0));
            }
            else
            {
                atyRoot[0] -= F(tgPM_POW)(-fTMP02, MKL(1.0) / MKL(3.0));
            }

            atyRoot[0] -= fOffset;

            *piCount = 1;
        }
        else if (fDSC < MKL(0.0))
        {

            const TYPE                          fDist = F(tgPM_SQRT)(-fA / MKL(3.0));
            const TYPE                          fAngle = F(tgPM_ATAN2)(F(tgPM_SQRT)(-fDSC), -fHalfB) / MKL(3.0);
            const TYPE                          fCos = F(tgPM_COS)(fAngle);
            const TYPE                          fSin = F(tgPM_SIN)(fAngle);
            const TYPE                          fTMP01 = fDist*fCos;

            atyRoot[0] = fTMP01 + fTMP01 - fOffset;
            atyRoot[1] = -fDist*(fCos + F(KTgF_SQRT3)*fSin) - fOffset;
            atyRoot[2] = -fDist*(fCos - F(KTgF_SQRT3)*fSin) - fOffset;

            *piCount = 3;
        }
        else
        {
            TYPE                                fTMP01;

            if (fHalfB >= MKL(0.0))
            {
                fTMP01 = -F(tgPM_POW)(fHalfB, MKL(1.0) / MKL(3.0));
            }
            else
            {
                fTMP01 = F(tgPM_POW)(-fHalfB, MKL(1.0) / MKL(3.0));
            }

            atyRoot[0] = fTMP01 + fTMP01 - fOffset;
            atyRoot[1] = -fTMP01 - fOffset;
            atyRoot[2] = atyRoot[1];

            *piCount = 3;
        }
    }

    return (TgTRUE);
}


/* ---- Degree 4 Algebraic Solver --------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgBOOL F(F_Calc_Root_4)(PCU_TYPE atyRoot, PCU_TgSINT32 piCount, TYPE fC0, TYPE fC1, TYPE fC2, TYPE fC3, TYPE fC4)
{
    const TYPE                          fInvC4 = MKL(1.0) / fC4;

    if (F(tgPM_ABS)(fC4) <= F(KTgROOT_EPS))
    {
        return (F(F_Calc_Root_3)(atyRoot, piCount, fC0, fC1, fC2, fC3));
    }

    /* Make polynomial monic, x^4+c3*x^3+c2*x^2+c1*x+c0 */

    if (fInvC4 != MKL(1.0))
    {
        fC0 *= fInvC4;
        fC1 *= fInvC4;
        fC2 *= fInvC4;
        fC3 *= fInvC4;
    };

    {
        /* Reduction to Resolvent Cubic Polynomial y^3+r2*y^2+r1*y+r0 = 0 */

        TYPE                                fR0 = -fC3*fC3*fC0 + MKL(4.0)*fC0*fC2 - fC1*fC1;
        TYPE                                fR1 = fC3*fC1 - MKL(4.0)*fC0;
        TYPE                                fR2 = -fC2;
        TYPE                                fDSC;

        F(F_Calc_Root_3)(atyRoot, piCount, fR0, fR1, fR2, MKL(1.0));

        fDSC = MKL(0.25)*fC3*fC3 - fC2 + atyRoot[0];

        *piCount = 0;

        if (F(tgPM_ABS)(fDSC) <= F(KTgROOT_EPS))
        {
            fDSC = MKL(0.0);
        };

        if (fDSC > MKL(0.0))
        {
            const TYPE                          fTMP00 = F(tgPM_SQRT)(fDSC);
            const TYPE                          fTMP01 = MKL(0.75)*fC3*fC3 - fTMP00*fTMP00 - fC2 - fC2;
            const TYPE                          fTMP02 = MKL(4.0)*(fC3*fC2 - fC1 - fC1);
            const TYPE                          fTMP03 = MKL(0.25)*(fTMP02 - fC3*fC3*fC3) / (fTMP00);

            TYPE                                fTMP04 = fTMP01 + fTMP03;
            TYPE                                fTMP05 = fTMP01 - fTMP03;

            if (F(tgPM_ABS)(fTMP04) <= F(KTgROOT_EPS))
            {
                fTMP04 = MKL(0.0);
            }

            if (F(tgPM_ABS)(fTMP05) <= F(KTgROOT_EPS))
            {
                fTMP05 = MKL(0.0);
            }

            if (fTMP04 >= MKL(0.0))
            {
                const TYPE                          fTMP06 = F(tgPM_SQRT)(fTMP04);

                atyRoot[0] = MKL(-0.25)*fC3 + MKL(0.5)*(fTMP00 + fTMP06);
                atyRoot[1] = MKL(-0.25)*fC3 + MKL(0.5)*(fTMP00 - fTMP06);
                *piCount += 2;
            }

            if (fTMP05 >= MKL(0.0))
            {
                const TYPE                          fTMP06 = F(tgPM_SQRT)(fTMP05);

                atyRoot[(*piCount)++] = MKL(-0.25)*fC3 + MKL(0.5)*(fTMP06 - fTMP00);
                atyRoot[(*piCount)++] = MKL(-0.25)*fC3 - MKL(0.5)*(fTMP06 + fTMP00);
            }

        }
        else if (fDSC < MKL(0.0))
        {
            *piCount = 0;
        }
        else
        {
            TYPE                                fTMP01 = atyRoot[0] * atyRoot[0] - MKL(4.0)*fC0;

            if (fTMP01 >= -F(KTgROOT_EPS))
            {
                TYPE                                fTMP00 = MKL(0.75)*fC3*fC3 - fC2 - fC2;

                fTMP01 = fTMP01 < MKL(0.0) ? MKL(0.0) : MKL(2.0)*F(tgPM_SQRT)(fTMP01);

                if (fTMP00 + fTMP01 >= F(KTgROOT_EPS))
                {
                    TYPE                                fTMP02 = F(tgPM_SQRT)(fTMP00 + fTMP01);

                    atyRoot[0] = MKL(-0.25)*fC3 + MKL(0.5)*fTMP02;
                    atyRoot[1] = MKL(-0.25)*fC3 - MKL(0.5)*fTMP02;
                    *piCount += 2;
                }

                if (fTMP00 - fTMP01 >= F(KTgROOT_EPS))
                {
                    const TYPE                          fTMP02 = F(tgPM_SQRT)(fTMP00 - fTMP01);

                    atyRoot[(*piCount)++] = MKL(-0.25)*fC3 + MKL(0.5)*fTMP02;
                    atyRoot[(*piCount)++] = MKL(-0.25)*fC3 - MKL(0.5)*fTMP02;
                }
            }
        }
    }

    return *piCount > 0 ? TgTRUE : TgFALSE;
}