Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Circle-Circle.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Circle;
    »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".                                                   */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
/*  Algorithm taken from paper at: http://jgt.akpeters.com/papers/Vranek02/                                                                                               */
/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/* == Collision ========================================================================================================================================================= */

/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */
/*  File Local Functions                                                                                                                                                  */
/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */

TgTYPE_STRUCT(V(STg2_EqN_CI_CI),)
{
    TYPE                                fCF0, fCF1, fCF2, fCF3, fCF4, fCF5, fCF6, fCF7, fCF8, fCF9;
    TgBOOL                              bCoaxial, bSymmetric;
};

static TgVOID                               V(tgCO_FI_CI_Init_EqN_CI)(V(PC_STg2_EqN_CI_CI), V(CPC_TgCIRCLE), V(CPC_TgCIRCLE));
static TYPE                                 V(tgCO_FI_CI_Calc_Angle_EqN_CI)(PC_TgVOID, const TYPE, const TYPE, const TYPE);
static TYPE                                 V(tgCO_FI_CI_Solve2_EqN_CI)(PC_TgVOID, const TYPE, const TYPE);
static TYPE                                 V(tgCO_FI_CI_Solve1_EqN_CI)(PC_TgVOID, const TYPE);
static TYPE                                 V(tgCO_FI_CI_Solve_Diff1_EqN_CI)(PC_TgVOID, const TYPE);
static TYPE                                 V(tgCO_FI_CI_Solve_Diff2_EqN_CI)(PC_TgVOID, const TYPE);




/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */
/*  Public Functions                                                                                                                                                      */
/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */

/* ---- V(tgCO_F_CI_ParamSq_CI) ----------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE V(tgCO_F_CI_ParamSq_CI)(TYPE *pfTB, V(CPC_TgCIRCLE) psCI0, V(CPC_TgCIRCLE) psCI1)
{
    V(STg2_EqN_CI_CI)                   sEqN;
    TYPE                                fMinDist, fTB = F(KTgMAX);

    TgERROR( V(tgGM_CI_Is_Valid)(psCI0) && V(tgGM_CI_Is_Valid)(psCI1) );
    V(tgCO_FI_CI_Init_EqN_CI)(&sEqN, psCI0, psCI1);

    /* Return if coaxial */
    if (sEqN.bCoaxial)
    {
        *pfTB = MKL(0.0);
        return (V(tgCO_FI_CI_Solve1_EqN_CI)(&sEqN, MKL(0.0)));
    }
    else
    {
        /* Create the bracketing triplet */

        TYPE                                fX0 = -F(KTgF_TWO_PI)*F(KTgF_THIRD);
        TYPE                                fX1 = sEqN.bSymmetric ? MKL(-0.5) : MKL(0.0);
        TYPE                                fX2 = sEqN.bSymmetric ? MKL(1.0) : F(KTgF_TWO_PI)*F(KTgF_THIRD);

        TYPE                                fF0 = V(tgCO_FI_CI_Solve1_EqN_CI)(&sEqN, fX0);
        TYPE                                fF1 = V(tgCO_FI_CI_Solve1_EqN_CI)(&sEqN, fX1);
        TYPE                                fF2 = V(tgCO_FI_CI_Solve1_EqN_CI)(&sEqN, fX2);

        TgSINT32                            i;

        i = (fF0 > fF1) && (fF1 < fF2) ? 0 : ((fF1 > fF2) && (fF2 < fF0) ? 1 : ((fF2 > fF0) && (fF0 < fF1) ? 2 : 3));

        if (i > 2) /* Two or more of the function values must be equivalent - process further. */
        {
            const TYPE                          fA = F(tgPM_FSEL)(-F(tgPM_ABS)(fF0 - fF2), fX1, fX0);
            const TYPE                          fC = F(tgPM_FSEL)(-F(tgPM_ABS)(fF0 - fF2), fX2, fX2);
            const TYPE                          fFA = F(tgPM_FSEL)(-F(tgPM_ABS)(fF0 - fF2), fF1, fF0);
            const TYPE                          fFB = F(tgPM_FSEL)(-F(tgPM_ABS)(fF0 - fF2), fF2, fF2);

            C_TgSINT32                          N = 10;
            TgSINT32                            j = 0;

            for (; j < N; j++)
            {
                const TYPE                          fB = -F(KTgF_PI) + MKL(2.0)*(j + 1)*F(KTgF_PI) / (N + 1);
                const TYPE                          fFC = V(tgCO_FI_CI_Solve1_EqN_CI)(&sEqN, fB);

                if ((fFA == fFC) || (fFB == fFC))
                {
                    continue;
                };

                fX0 = F(tgPM_FSEL)(fA - fB, fB, fA);
                fX1 = F(tgPM_FSEL)(fA - fB, fA, F(tgPM_FSEL)(fB - fC, fC, fB));
                fX2 = F(tgPM_FSEL)(fA - fB, fC, F(tgPM_FSEL)(fB - fC, fB, fC));

                fF0 = F(tgPM_FSEL)(fA - fB, fFC, fFA);
                fF1 = F(tgPM_FSEL)(fA - fB, fFA, F(tgPM_FSEL)(fB - fC, fFB, fFC));
                fF2 = F(tgPM_FSEL)(fA - fB, fFB, F(tgPM_FSEL)(fB - fC, fFC, fFB));

                i = (fF0 > fF1) && (fF1 < fF2) ? 0 : ((fF1 > fF2) && (fF2 < fF0) ? 1 : ((fF2 > fF0) && (fF0 < fF1) ? 2 : 3));

                break;
            };

            if (j >= N) /* Failure to create solvable conditions */
            {
                TgERROR(TgFALSE);
                return (-F(KTgMAX));
            };
        }

        /* Find the first minimum */

        switch (i)
        {
            case 0: F(F_BrentD)(&fTB, &fMinDist, &sEqN, V(tgCO_FI_CI_Solve1_EqN_CI), V(tgCO_FI_CI_Solve_Diff1_EqN_CI), fX0, fX1, fX2); break;
            case 1: F(F_BrentD)(&fTB, &fMinDist, &sEqN, V(tgCO_FI_CI_Solve1_EqN_CI), V(tgCO_FI_CI_Solve_Diff1_EqN_CI), fX1, fX2, fX0); break;
            case 2: F(F_BrentD)(&fTB, &fMinDist, &sEqN, V(tgCO_FI_CI_Solve1_EqN_CI), V(tgCO_FI_CI_Solve_Diff1_EqN_CI), fX2, fX0, fX1); break;
            default:
                TgS_NO_DEFAULT(break);
        };

        /* Polish up the root */

        fTB -= V(tgCO_FI_CI_Solve_Diff1_EqN_CI)(&sEqN, fTB) / V(tgCO_FI_CI_Solve_Diff2_EqN_CI)(&sEqN, fTB);
        fMinDist = V(tgCO_FI_CI_Solve1_EqN_CI)(&sEqN, fTB);

        /* In the case of the symmetric distance function the minimum has been already found */
        /* The circles are in touch and their distance is 0 */

        if (sEqN.bSymmetric || F(tgPM_ABS)(fMinDist) <= F(KTgROOT_EPS))
        {
            *pfTB = fTB;
            return (fMinDist);
        }
        else
        {
            /* Set up coefficients of a polynomial p4 */

            const TYPE                          fA0 = sEqN.fCF7 - fMinDist;
            const TYPE                          fA1 = sEqN.fCF8*sEqN.fCF8;
            const TYPE                          fA2 = sEqN.fCF6*sEqN.fCF6;

            const TYPE                          fC1 = MKL(2.0)*fA0*sEqN.fCF8 - fA2*sEqN.fCF2;
            const TYPE                          fC3 = fA0*fA0 - fA2*sEqN.fCF0 + fA1 - fA2*sEqN.fCF4;
            const TYPE                          fC4 = MKL(2.0)*fA0*sEqN.fCF9 - fA2*sEqN.fCF3;
            const TYPE                          fC5 = MKL(2.0)*sEqN.fCF9*sEqN.fCF8 - fA2*sEqN.fCF1;
            const TYPE                          fC6 = fA2*sEqN.fCF4 - fA1 + sEqN.fCF9*sEqN.fCF9 - fA2*sEqN.fCF5;

            const TYPE                          fP4 = fC6*fC6 + fC5*fC5;
            const TYPE                          fP3 = MKL(2.0)*fC1*fC5 + MKL(2.0)*fC4*fC6;
            TYPE                                fCos;

            /* Return in the case of the coefficient p4[4] = 0 */
            if (F(tgPM_ABS)(fP4) < F(KTgROOT_EPS))
            {
                *pfTB = fTB;
                return (fMinDist);
            }

            fTB = F(tgPM_FSEL)(fTB - F(KTgF_PI), fTB - F(KTgF_TWO_PI), fTB);

            /* Deflate polynomial p4->p2 */
            fCos = F(tgPM_COS)(fTB);

            {
                const TYPE                          fCF0 = fC1*fC1 - fC5*fC5 + MKL(2.0)*fC3*fC6 + fC4*fC4 + 3 * fP4*fCos*fCos + MKL(2.0)*fP3*fCos;
                const TYPE                          fCF1 = fP3 + MKL(2.0)*fP4*fCos;
                TYPE                                afRoot[2];
                TgSINT32                            niRoot;

                if (
                      !F(F_Calc_Root_2)(afRoot, &niRoot, fCF0, fCF1, fP4)
                    || F(tgPM_ABS)(afRoot[0]) > MKL(1.0)
                    || F(tgPM_ABS)(afRoot[1]) > MKL(1.0))
                {
                    *pfTB = fTB;
                    return (fMinDist);
                }
                else
                {
                    /* Calculate the new bracketing pair */

                    const TYPE                          fH1 = V(tgCO_FI_CI_Calc_Angle_EqN_CI)(&sEqN, afRoot[0], fMinDist, fTB);
                    const TYPE                          fH2 = V(tgCO_FI_CI_Calc_Angle_EqN_CI)(&sEqN, afRoot[1], fMinDist, fTB);
                    const TYPE                          fC01 = F(tgPM_FSEL)(fH1 - fH2, fH2, fH1);
                    const TYPE                          fC02 = F(tgPM_FSEL)(fH1 - fH2, fH1, fH2);
                    TYPE                                fLT, fLocalMin;

                    /* Return in the case of double root */
                    if (F(tgPM_ABS)(fH1 - fH2) <= F(KTgROOT_EPS))
                    {
                        *pfTB = fTB;
                        return (fMinDist);
                    };

                    /* Find the global minimum */
                    if ((fC01 < fTB) && (fTB < fC02))
                    {
                        F(F_BrentZ)(&fLT, &sEqN, V(tgCO_FI_CI_Solve_Diff1_EqN_CI), fC02, fC01 + F(KTgF_TWO_PI));
                    }
                    else
                    {
                        F(F_BrentZ)(&fLT, &sEqN, V(tgCO_FI_CI_Solve_Diff1_EqN_CI), fC01, fC02);
                    };

                    fLT -= V(tgCO_FI_CI_Solve_Diff1_EqN_CI)(&sEqN, fLT) / V(tgCO_FI_CI_Solve_Diff2_EqN_CI)(&sEqN, fLT);

                    fLocalMin = V(tgCO_FI_CI_Solve1_EqN_CI)(&sEqN, fLT);

                    if (fLocalMin < fMinDist)
                    {
                        fTB = fLT;
                        fMinDist = fLocalMin;
                    }

                    *pfTB = fTB;
                    return (fMinDist);
                };
            };
        };
    };
}


/* ---- V(tgCO_F_CI_Intersect2D_CI) ------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_CI_Intersect2D_CI)(V(PC_TgVEC) pvR0, V(PC_TgVEC) pvR1, V(CPC_TgCIRCLE) psCI0, V(CPC_TgCIRCLE) psCI1)
{
    /* The intersection routine requires that the circles be in parallel planes and that the normal direction is not important to the calculation.  Specifically it will  */
    /* return points of intersection between the two circles as if the second circle was projected down onto the plane of the first circle. */

    if (!F(tgCM_NR1)(V(F_DOT)(&psCI0->m.m.vNormal, &psCI1->m.m.vNormal)))
    {
        TgWARN_CO(TgT("Intersection between circles is only valid if co-planar.\n") );
        return (KTgE_FAIL);
    }
    else
    {
        V(C_TgVEC)                          vDS = V(F_SUB)(&psCI1->m.m.vOrigin, &psCI0->m.m.vOrigin);
        const TYPE                          fDS_N = V(F_DOT)(&vDS, &psCI0->m.m.vNormal);
        V(C_TgVEC)                          vL0 = V(F_MUL_SV)(fDS_N, &psCI0->m.m.vNormal);
        V(TgVEC)                            vU = V(F_SUB)(&vDS, &vL0);
        const TYPE                          fU = V(F_LEN)(&vU);

        const TYPE                          fABS_RS = F(tgPM_ABS)(psCI0->m_fRadius + psCI1->m_fRadius);
        const TYPE                          fABS_RD = F(tgPM_ABS)(psCI0->m_fRadius - psCI1->m_fRadius);

        if (fABS_RD >= fU || fABS_RS <= fU)
        {
            return (KTgE_NO_INTERSECT);
        }
        else
        {
            const TYPE                          fU_U0 = V(F_DOT)(&vU, &psCI0->m.m.vU);
            const TYPE                          fU_V0 = V(F_DOT)(&vU, &psCI0->m.m.vV);
            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fU_V0, &psCI0->m.m.vU);
            V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fU_U0, &psCI0->m.m.vV);
            V(C_TgVEC)                          vV = V(F_SUB)(&vK0, &vK1);
            const TYPE                          fM0 = MKL(1.0) / (fU*fU);
            const TYPE                          fRSQD = psCI0->m_fRadiusSq - psCI1->m_fRadiusSq;
            const TYPE                          fT0 = MKL(0.5)*(MKL(1.0) + fRSQD*fM0);
            const TYPE                          fT1 = F(tgPM_SQRT)(psCI0->m_fRadiusSq*fM0 - fT0*fT0);
            V(C_TgVEC)                          vK4 = V(F_MUL_SV)(fT1, &vV);
            V(C_TgVEC)                          vK5 = V(F_MUL_SV)(fT1, &vV);
            V(C_TgVEC)                          vK6 = V(F_MUL_SV)(fT0, &vU);
            V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT0, &vU);
            V(C_TgVEC)                          vK2 = V(F_ADD)(&vK6, &vK4);
            V(C_TgVEC)                          vK3 = V(F_SUB)(&vK7, &vK5);

            *pvR0 = V(F_ADD)(&psCI0->m.m.vOrigin, &vK2);
            *pvR1 = V(F_ADD)(&psCI0->m.m.vOrigin, &vK3);

            return (KTgS_OK);
        };
    };
}




/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */
/*  File Local Functions                                                                                                                                                  */
/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */

/* ---- V(tgCO_FI_CI_Init_EqN_CI) --------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgVOID V(tgCO_FI_CI_Init_EqN_CI)(V(PC_STg2_EqN_CI_CI) psEQN, V(CPC_TgCIRCLE) psCI0, V(CPC_TgCIRCLE) psCI1)
{
    /* Prepare calculation of coefficients */

    V(C_TgVEC)                          vDS = V(F_SUB)(&psCI1->m.m.vOrigin, &psCI0->m.m.vOrigin);
    const TYPE                          fDS_DS = V(F_LSQ)(&vDS);

    const TYPE                          fI0_U1 = V(F_DOT)(&psCI0->m.m.vOrigin, &psCI1->m.m.vU);
    const TYPE                          fI0_V1 = V(F_DOT)(&psCI0->m.m.vOrigin, &psCI1->m.m.vV);
    const TYPE                          fI1_U1 = V(F_DOT)(&psCI1->m.m.vOrigin, &psCI1->m.m.vU);
    const TYPE                          fI1_V1 = V(F_DOT)(&psCI1->m.m.vOrigin, &psCI1->m.m.vV);

    const TYPE                          fN0_U1 = V(F_DOT)(&psCI0->m.m.vNormal, &psCI1->m.m.vU);
    const TYPE                          fN0_V1 = V(F_DOT)(&psCI0->m.m.vNormal, &psCI1->m.m.vV);
    const TYPE                          fN0_I0 = V(F_DOT)(&psCI0->m.m.vNormal, &psCI0->m.m.vOrigin);
    const TYPE                          fN0_I1 = V(F_DOT)(&psCI0->m.m.vNormal, &psCI1->m.m.vOrigin);

    const TYPE                          fDiff = fN0_I1 - fN0_I0;

    /* Calculate coefficients of the trigonometric function */

    psEQN->fCF0 = psCI1->m_fRadiusSq + fDS_DS - fDiff*fDiff;
    psEQN->fCF1 = -psCI1->m_fRadiusSq*fN0_U1*fN0_V1*MKL(2.0);
    psEQN->fCF2 = psCI1->m_fRadius*MKL(2.0)*(fI1_V1 - fI0_V1 - fDiff*fN0_V1);
    psEQN->fCF3 = psCI1->m_fRadius*MKL(2.0)*(fI1_U1 - fI0_U1 - fDiff*fN0_U1);
    psEQN->fCF4 = -psCI1->m_fRadiusSq*fN0_V1*fN0_V1;
    psEQN->fCF5 = -psCI1->m_fRadiusSq*fN0_U1*fN0_U1;
    psEQN->fCF6 = -psCI0->m_fRadius*MKL(2.0);
    psEQN->fCF7 = psCI1->m_fRadiusSq + psCI0->m_fRadiusSq + fDS_DS;
    psEQN->fCF8 = psCI1->m_fRadius*MKL(2.0)*(fI1_V1 - fI0_V1);
    psEQN->fCF9 = psCI1->m_fRadius*MKL(2.0)*(fI1_U1 - fI0_U1);

    /* Eliminate a case of coaxial circles */

    psEQN->bCoaxial = F(tgCM_NR0)(fI1_U1 - fI0_U1) && F(tgCM_NR0)(fI1_V1 - fI0_V1) && F(tgCM_NR0)(fN0_U1) && F(tgCM_NR0)(fN0_V1);

    /* Eliminate a case of a symmetric distance function */

    psEQN->bSymmetric = F(tgCM_NR0)(psEQN->fCF8) && F(tgCM_NR0)(psEQN->fCF4) && F(tgCM_NR0)(psEQN->fCF1);
}


/* ---- V(tgCO_FI_CI_Calc_Angle_EqN_CI) --------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TYPE V(tgCO_FI_CI_Calc_Angle_EqN_CI)(PC_TgVOID psEQN, const TYPE fCos, const TYPE fOffset, const TYPE fAngOrig)
{
    TYPE                                fSin = F(tgPM_SQRT)(MKL(1.0) - fCos*fCos);
    TYPE                                fVal1 = V(tgCO_FI_CI_Solve2_EqN_CI)(((V(PC_STg2_EqN_CI_CI))(psEQN)), fSin, fCos) - fOffset;
    TYPE                                fVal2 = V(tgCO_FI_CI_Solve2_EqN_CI)(((V(PC_STg2_EqN_CI_CI))(psEQN)), -fSin, fCos) - fOffset;
    TYPE                                fAng = F(tgPM_ACOS)(fCos);

    if (MKL(0.01)*F(tgPM_ABS)(fVal1) < F(KTgROOT_EPS) && MKL(0.01)*F(tgPM_ABS)(fVal2) < F(KTgROOT_EPS))
    {
        return (-fAngOrig);
    };

    return (F(tgPM_FSEL)(F(tgPM_ABS)(fVal1) - F(tgPM_ABS)(fVal2), -fAng, fAng));
}


/* ---- V(tgCO_FI_CI_Solve2_EqN_CI) ------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TYPE V(tgCO_FI_CI_Solve2_EqN_CI)(PC_TgVOID psEQN, const TYPE fSin, const TYPE fCos)
{
    return (
          ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF9*fCos
        + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF8*fSin
        + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF7
        + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF6*F(tgPM_SQRT)
            (
                ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF5*fCos*fCos
                + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF4*fSin*fSin
                + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF3*fCos
                + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF2*fSin
                + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF1*fSin*fCos
                + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF0
                )
    );
}


/* ---- V(tgCO_FI_CI_Solve1_EqN_CI) ------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TYPE V(tgCO_FI_CI_Solve1_EqN_CI)(PC_TgVOID psEQN, const TYPE fT0)
{
    TYPE                                fSin, fCos;

    F(tgPM_SINCOS)(&fSin, &fCos, fT0);
    return (V(tgCO_FI_CI_Solve2_EqN_CI)(((V(PC_STg2_EqN_CI_CI))(psEQN)), fSin, fCos));
}


/* ---- V(tgCO_FI_CI_Solve_Diff1_EqN_CI) -------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TYPE V(tgCO_FI_CI_Solve_Diff1_EqN_CI)(PC_TgVOID psEQN, const TYPE fT0)
{
    TYPE                                fSin, fCos;

    F(tgPM_SINCOS)(&fSin, &fCos, fT0);

    return (
          ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF8*fCos
        - ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF9*fSin
        + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF6
        / F(tgPM_SQRT)(
            ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF5*fCos*fCos
            + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF3*fCos
            + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF4*fSin*fSin
            + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF2*fSin
            + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF1*fSin*fCos
            + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF0
            )
        *
        (
            ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF5*fCos*fSin*MKL(-2.0)
            - ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF3*fSin
            + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF4*fSin*fCos*MKL(2.0)
            + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF2*fCos
            + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF1*fCos*fCos
            - ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF1*fSin*fSin
            )
        / MKL(2.0)
    );
}


/* ---- V(tgCO_FI_CI_Solve_Diff2_EqN_CI) -------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TYPE V(tgCO_FI_CI_Solve_Diff2_EqN_CI)(PC_TgVOID psEQN, const TYPE fT0)
{
    TYPE                                fSin, fCos;

    F(tgPM_SINCOS)(&fSin, &fCos, fT0);

    {
        const TYPE                          t6 = ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF5*fCos*fCos;
        const TYPE                          t7 = ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF3*fCos;
        const TYPE                          t9 = ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF4*fSin*fSin;
        const TYPE                          t10 = ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF2*fSin;
        const TYPE                          t12 = ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF1*fSin*fCos;
        const TYPE                          t13 = t6 + t7 + t9 + t10 + t12 + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF0;
        const TYPE                          t14 = F(tgPM_SQRT)(t13);
        const TYPE                          fT15 = -MKL(2.0)*((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF5*fCos*fSin
                                                 - ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF3*fSin
                                                 + MKL(2.0)*((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF4*fSin*fCos
                                                 + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF2*fCos
                                                 + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF1*fCos*fCos
                                                 - ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF1*fSin*fSin;

        return (
            MKL(2.0)*((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF4*fCos*fCos
            - ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF9*fCos
            - ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF8*fSin
            - ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF6 / t14 / t13*fT15*fT15 / MKL(4.0)
            + ((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF6 / t14*(MKL(2.0)*((V(PC_STg2_EqN_CI_CI))(psEQN))->fCF5*fSin*fSin
            - MKL(2.0)*t6 - t7 - MKL(2.0)*t9 - t10 - MKL(4.0)*t12) / MKL(2.0)
        );
    };
}