Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Cylinder-Triangle.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Cylinder;Triangle;
    »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".                                                   */
/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/* == Collision ========================================================================================================================================================= */

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

static TgBOOL                               V(tgCO_F_ST_Axis_Seperation_CY)( V(PC_STg2_CO_Axis_Result), V(CPC_TgSTRI), V(CPC_TgTUBE));
static TgBOOL                               V(tgCO_F_ST_Internal_ClipTriToCyl_CY)( V(PC_STg2_CO_Clip_List), V(CPC_TgSTRI), V(CPC_TgTUBE));




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

/* ---- V(tgCO_F_ST_Penetrate_CY) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psST0: Space Triangle primitive                                                                                                                                */
/* Input:  psCY0: Cylinder primitive - contact points are generated on this primitive                                                                                     */
/* Output: tgPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_ST_Penetrate_CY)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgSTRI) psST0, V(CPC_TgTUBE) psCY0)
{
    V(P_STg2_CO_Contact)                psContact;
    V(STg2_CO_Axis_Result)              sAxS;

    C_TgSINT32                          niContact = psPacket->m_niContact;

    TgERROR(V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_ST_Is_Valid)(psST0));

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
    {
        return (KTgE_FAIL);
    };

    /*TgDEBUG_COLLISION_TRIANGLE_CREATEID( iDBG_TriID, psST0, etgDEBUG_COLLISION_ENTERFCN ); */

    {
        /* Primitive Culling - Set of criteria required for the primitive to be considered penetrating the triangle. */

        V(C_TgVEC)                          vK0 = V(F_SUB)(&psCY0->m.m.vOrigin, psST0->m_sCT.m_sET.m_sPT.m_avPoint);
        const TYPE                          fDS_N = V(F_DOT)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &vK0);
        const TYPE                          fEX_N = V(F_DOT)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &psCY0->m_vHAX);
        const TYPE                          fS0_N = fDS_N - fEX_N;
        const TYPE                          fS1_N = fDS_N + fEX_N;
        const TYPE                          fRadius = psCY0->m_fRadius;

        if ((fS0_N > fRadius && fS1_N > fRadius) || (fS0_N < MKL(0.0) && fS1_N < MKL(0.0)))
        {
            /* Either both of the capsule's end points are below the plane or more than radius above the plane. */

            return (KTgE_NO_INTERSECT);
        };

        /*TgDEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_PASSED_REJECT ); */

        /* Find the minimal axis of separation, or return if the primitives are not in contact. */

        if (!V(tgCO_F_ST_Axis_Seperation_CY)(&sAxS, psST0, psCY0))
        {
            return (KTgE_NO_INTERSECT);
        };

        TgERROR( F(tgCM_NR1)(V(F_LSQ)(&sAxS.m_vNormal)) && sAxS.m_fDepth >= MKL(0.0) );
    }

    /* == Contact Generation == */

    if (sAxS.m_iAxis >= 4)
    {
        /* -- Axis: Cap-Surface Component -- */

        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(psCY0->m_fRadius, &sAxS.m_vNormal);

        psContact = psPacket->m_psContact + psPacket->m_niContact;

        psContact->m_vS0 = V(F_SUB)(&sAxS.m_vPoint, &vK0);
        psContact->m_vN0 = sAxS.m_vNormal;
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = sAxS.m_fDepth;

        ++psPacket->m_niContact;

        return (KTgS_OK);
    };

    if (sAxS.m_iAxis >= 2)
    {
        /* -- Axis: Cylinder Primary Axis -- */

        V(PU_STg2_CO_Clip_List)             psCL;

        psCL = TgMALLOC_POOL( sizeof( V(STg2_CO_Clip_List) ) + 7 * sizeof( V(TgVEC) ) );
        psCL->m_niPoint = 0;
        psCL->m_niMax = 8;

        if (V(tgCO_F_ST_Internal_ClipTriToCyl_CY)(psCL, psST0, psCY0) && 0 == psCL->m_niPoint)
        {
            TgFREE_POOL( psCL );
            return (KTgE_NO_INTERSECT);
        }
        else
        {
            TgSINT32                            iIdx;

            C_TgSINT32 niMax = tgCM_MIN_S32( psCL->m_niPoint, psPacket->m_niMaxContact - psPacket->m_niContact );
            for (iIdx = 0; iIdx < niMax; ++iIdx)
            {
                V(C_TgVEC)                          vK0 = V(F_SUB)(psCL->m_avPoint + iIdx, &psCY0->m.m.vOrigin);
                const TYPE                          fP_AX = V(F_DOT)(&vK0, &psCY0->m.m.vU_HAX);
                TYPE                                fDepth = psCY0->m_fExtent - F(tgPM_ABS)(fP_AX);
                V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fDepth, &sAxS.m_vNormal);

                psContact = psPacket->m_psContact + psPacket->m_niContact;

                fDepth = F(tgPM_FSEL)(fDepth, fDepth, MKL(0.0));

                psContact->m_vS0 = V(F_SUB)(psCL->m_avPoint + iIdx, &vK2);
                psContact->m_vN0 = sAxS.m_vNormal;
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = fDepth;

                ++psPacket->m_niContact;
            };

            if (0 != psCL->m_niPoint)
            {
                TgFREE_POOL( psCL );
                return (niMax == psCL->m_niPoint ? KTgS_OK : KTgE_MAX_CONTACTS);
            };
        };

        TgFREE_POOL( psCL );

        /* Error catch - default to triangle normal for this error state. */
    };


    /* -- Triangle Normal -- */

    {
        const TYPE                          fAX_N = V(F_DOT)(&psCY0->m.m.vU_HAX, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
        const TYPE                          fUseBX = F(tgPM_ABS)(fAX_N) - (MKL(1.0) - F(KTgEPS));
        const TYPE                          fDN = F(tgPM_FSEL)(fAX_N, MKL(-1.0), MKL(1.0));
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fDN, &psCY0->m_vHAX);
        V(C_TgVEC)                          vC0 = V(F_ADD)(&psCY0->m.m.vOrigin, &vK0);
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fAX_N, &psCY0->m.m.vU_HAX);
        V(C_TgVEC)                          vT0 = V(F_SUB)(&vK1, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
        const TYPE                          fK0 = F(tgPM_FSEL)(fUseBX, psCY0->m.m.vU_Basis0.m.x, vT0.m.x);
        const TYPE                          fK1 = F(tgPM_FSEL)(fUseBX, psCY0->m.m.vU_Basis0.m.y, vT0.m.y);
        const TYPE                          fK2 = F(tgPM_FSEL)(fUseBX, psCY0->m.m.vU_Basis0.m.z, vT0.m.z);
        V(C_TgVEC)                          vK2 = V(FS_SETV)(fK0, fK1, fK2);
        V(C_TgVEC)                          vX0 = V(F_NORM)(&vK2);


        if (fAX_N > MKL(0.5))
        {
            /* Method: Produce three points along symmetrical points along the cylinder rim.  If the cylinder plane is not parallel to the triangle plane then one of */
            /* these points must be the point of deepest penetration. */

            TYPE                                fT0, fT1;
            TgSINT32                            iCode;

            V(C_TgVEC)                          vT1 = V(F_CX)(&vX0, &psCY0->m.m.vU_HAX);
            const TYPE                          fK3 = F(tgPM_FSEL)(fUseBX, psCY0->m.m.vU_Basis1.m.x, vT1.m.x);
            const TYPE                          fK4 = F(tgPM_FSEL)(fUseBX, psCY0->m.m.vU_Basis1.m.y, vT1.m.y);
            const TYPE                          fK5 = F(tgPM_FSEL)(fUseBX, psCY0->m.m.vU_Basis1.m.z, vT1.m.z);
            V(C_TgVEC)                          vK3 = V(FS_SETV)(fK3, fK4, fK5);
            V(C_TgVEC)                          vK4 = V(F_NORM)(&vK3);
            V(C_TgVEC)                          vX1 = V(F_MUL_VS)(&vK4, F(KTgF_SQRT3));

            /* Generate the line segment for each of these points from the middle of the cylinder's end cap - and then clip it against the triangle using feature */
            /* reduction information.  Chose the largest t value returned to create the contact point. */

            V(C_TgVEC)                          vT2 = V(F_MUL_SV)(psCY0->m_fRadius, &vX0);
            V(C_TgVEC)                          vK5 = V(F_ADD)(&vX0, &vX1);
            V(C_TgVEC)                          vT3 = V(F_MUL_SV)(MKL(-0.5)*psCY0->m_fRadius, &vK5);
            V(C_TgVEC)                          vK6 = V(F_SUB)(&vX0, &vX1);
            V(C_TgVEC)                          vT4 = V(F_MUL_SV)(MKL(-0.5)*psCY0->m_fRadius, &vK6);

            if (V(tgCO_FI_ST_Clip_Param_F_LR11)(&fT0, &fT1, &iCode, psST0, &vC0, &vT2) >= 0 && 0 != (iCode & 12))
            {
                const TYPE                          fL0 = 0 != (iCode & 4) ? fT0 : -fT1;
                const TYPE                          fL1 = 0 != (iCode & 8) ? fT1 : -fT0;
                const TYPE                          fLN = F(tgPM_FSEL)(fL1 - fL0, fL1, fL0);
                V(C_TgVEC)                          vK8 = V(F_MUL_SV)(fLN, &vT2);
                V(C_TgVEC)                          vK7 = V(F_ADD)(&vC0, &vK8);
                V(C_TgVEC)                          vK9 = V(F_SUB)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &vK7);
                const TYPE                          fK6 = V(F_DOT)(&vK9, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

                if (fK6 >= MKL(0.0))
                {
                    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                    {
                        return (KTgE_MAX_CONTACTS);
                    };

                    psContact = psPacket->m_psContact + psPacket->m_niContact;

                    psContact->m_vS0 = vK7;
                    psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fK6;

                    ++psPacket->m_niContact;
                };
            };

            if (V(tgCO_FI_ST_Clip_Param_F_LR11)(&fT0, &fT1, &iCode, psST0, &vC0, &vT3) >= 0 && 0 != (iCode & 12))
            {
                const TYPE                          fL0 = 0 != (iCode & 4) ? fT0 : -fT1;
                const TYPE                          fL1 = 0 != (iCode & 8) ? fT1 : -fT0;
                const TYPE                          fLN = F(tgPM_FSEL)(fL1 - fL0, fL1, fL0);
                V(C_TgVEC)                          vK8 = V(F_MUL_SV)(fLN, &vT3);
                V(C_TgVEC)                          vK7 = V(F_ADD)(&vC0, &vK8);
                V(C_TgVEC)                          vK9 = V(F_SUB)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &vK7);
                const TYPE                          fK6 = V(F_DOT)(&vK9, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

                if (fK6 >= MKL(0.0))
                {
                    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                    {
                        return (KTgE_MAX_CONTACTS);
                    };

                    psContact = psPacket->m_psContact + psPacket->m_niContact;

                    psContact->m_vS0 = vK7;
                    psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fK6;

                    ++psPacket->m_niContact;
                };
            };


            if (V(tgCO_FI_ST_Clip_Param_F_LR11)(&fT0, &fT1, &iCode, psST0, &vC0, &vT4) >= 0 && 0 != (iCode & 12))
            {
                const TYPE                          fL0 = 0 != (iCode & 4) ? fT0 : -fT1;
                const TYPE                          fL1 = 0 != (iCode & 8) ? fT1 : -fT0;
                const TYPE                          fLN = F(tgPM_FSEL)(fL1 - fL0, fL1, fL0);
                V(C_TgVEC)                          vK8 = V(F_MUL_SV)(fLN, &vT4);
                V(C_TgVEC)                          vK7 = V(F_ADD)(&vC0, &vK8);
                V(C_TgVEC)                          vK9 = V(F_SUB)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &vK7);
                const TYPE                          fK6 = V(F_DOT)(&vK9, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

                if (fK6 >= MKL(0.0))
                {
                    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                    {
                        return (KTgE_MAX_CONTACTS);
                    };

                    psContact = psPacket->m_psContact + psPacket->m_niContact;

                    psContact->m_vS0 = vK7;
                    psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fK6;

                    ++psPacket->m_niContact;
                };
            };

            if (niContact != psPacket->m_niContact)
            {
                return (KTgS_OK);
            }
            else
            {
                V(PU_STg2_CO_Clip_List)             psCL;

                TgSINT32                            iIdx, niMax;

                const TYPE                          fK6 = MKL(-2.0)*psCY0->m_fExtent;
                V(C_TgVEC)                          vT5 = V(F_MUL_SV)(fK6, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

                psCL = TgMALLOC_POOL( sizeof( V(STg2_CO_Clip_List) ) + 7 * sizeof( V(TgVEC) ) );
                psCL->m_niPoint = 0;
                psCL->m_niMax = 8;

                V(tgCO_F_ST_Internal_ClipTriToCyl_CY)(psCL, psST0, psCY0);

                if (0 == psCL->m_niPoint)
                {
                    TgFREE_POOL( psCL );
                    return (KTgE_NO_INTERSECT);
                }

                niMax = tgCM_MIN_S32( psCL->m_niPoint, psPacket->m_niMaxContact - psPacket->m_niContact );

                for (iIdx = 0; iIdx < niMax; ++iIdx)
                {
                    if (V(tgCO_FI_CY_Clip_Param_LR11)(&fT0, &fT1, psCY0, psCL->m_avPoint + iIdx, &vT5) < 0)
                    {
                        continue;
                    };

                    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                    {
                        TgFREE_POOL( psCL );
                        return (KTgE_MAX_CONTACTS);
                    }
                    else
                    {
                        const TYPE                          fLN = F(tgPM_FSEL)(fT1 - fT0, fT1, fT0);
                        V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fLN, &vT5);
                        V(C_TgVEC)                          vK8 = V(F_ADD)(psCL->m_avPoint + iIdx, &vK7);
                        V(C_TgVEC)                          vK9 = V(F_SUB)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &vK8);
                        const TYPE                          fK7 = V(F_DOT)(&vK9, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

                        psContact = psPacket->m_psContact + psPacket->m_niContact;

                        psContact->m_vS0 = vK8;
                        psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
                        psContact->m_fT0 = MKL(0.0);
                        psContact->m_fDepth = fK7;

                        ++psPacket->m_niContact;
                    };
                };

                TgFREE_POOL( psCL );
                return (niContact != psPacket->m_niContact ? KTgS_OK : KTgE_NO_INTERSECT);
            };
        }
        else
        {
            TYPE                                fT0, fT1;
            TgSINT32                            iCode;

            V(C_TgVEC)                          vD0 = V(F_MUL_SV)(MKL(-2.0)*fDN, &psCY0->m_vHAX);
            V(C_TgVEC)                          vS0 = V(F_ADD)(&vC0, &vX0);

            if (V(tgCO_FI_ST_Clip_Param_F_LR11)(&fT0, &fT1, &iCode, psST0, &vS0, &vD0) < 0 || 0 == (iCode & 12))
            {
                return (KTgE_NO_INTERSECT);
            }
            else
            {
                V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT0, &vD0);
                V(C_TgVEC)                          vK8 = V(F_ADD)(&vS0, &vK7);
                V(C_TgVEC)                          vK9 = V(F_SUB)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &vK8);
                const TYPE                          fK8 = V(F_DOT)(&vK9, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

                V(C_TgVEC)                          vKA = V(F_MUL_SV)(fT1, &vD0);
                V(C_TgVEC)                          vKB = V(F_ADD)(&vS0, &vKA);
                V(C_TgVEC)                          vKC = V(F_SUB)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &vKB);
                const TYPE                          fK7 = V(F_DOT)(&vKC, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

                if (0 == (iCode & 4) && fK8 >= MKL(0.0))
                {
                    psContact = psPacket->m_psContact + psPacket->m_niContact;

                    psContact->m_vS0 = vK8;
                    psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fK8;

                    ++psPacket->m_niContact;
                };

                if (0 == (iCode & 8) && fK7 >= MKL(0.0))
                {
                    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                    {
                        return (KTgE_MAX_CONTACTS);
                    };

                    psContact = psPacket->m_psContact + psPacket->m_niContact;

                    psContact->m_vS0 = vKB;
                    psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fK7;

                    ++psPacket->m_niContact;
                };
            };
        };
    };

    return (niContact != psPacket->m_niContact ? KTgS_OK : KTgE_NO_INTERSECT);
}




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

/* ---- V(tgCO_F_ST_Axis_Seperation_CY) --------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psST0: Space Triangle primitive                                                                                                                                */
/* Input:  psCY0: Cylinder primitive                                                                                                                                      */
/* Output: sAxS: Structure holds the resulting axis separation information necessary to create a contact set.                                                             */
/* Return: False if a separating axis exists, true otherwise                                                                                                              */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgBOOL V(tgCO_F_ST_Axis_Seperation_CY)(V(PC_STg2_CO_Axis_Result) psAxS, V(CPC_TgSTRI) psST0, V(CPC_TgTUBE) psCY0)
{
    TYPE                                fMinDepth, fMaxDepth, fCyl;
    V(TgVEC)                            vProxCyl, vProxTri;

    const TYPE                          fTriD = V(F_DOT)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
    const TYPE                          fNormTest = V(F_DOT)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &psCY0->m.m.vU_HAX);

    {
        /* -- Axis: Triangle Normal -- */

        TYPE                                fMinCyl, fMaxCyl;

        V(tgGM_CY_Project)(&fMinCyl, &fMaxCyl, psCY0, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

        TgERROR( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_ST_Is_Valid)(psST0) );

        if (fMaxCyl < fTriD || fMinCyl > fTriD)
        {
            return (TgFALSE);
        };

        psAxS->m_vNormal = V(F_NEG)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal);
        psAxS->m_fDepth = fTriD - fMinCyl;
        psAxS->m_iAxis = 1;
    };

    {
        /* -- Axis: Cylinder Primary Axis -- */

        TYPE                                fMinCyl, fMaxCyl, fMinTri, fMaxTri;

        if (!F(tgCM_NR1)(fNormTest))
        {
            fMinCyl = V(F_DOT)(&psCY0->m.m.vOrigin, &psCY0->m.m.vU_HAX);
            fMaxCyl = fMinCyl + psCY0->m_fExtent;
            fMinCyl -= psCY0->m_fExtent;

            V(tgGM_ST_Project)(&fMinTri, &fMaxTri, psST0, &psCY0->m.m.vU_HAX);

            if (fMaxCyl < fMinTri || fMinCyl > fMaxTri)
            {
                return (TgFALSE);
            };

            fMinDepth = fMaxCyl - fMinTri;
            fMaxDepth = fMaxTri - fMinCyl;

            if (fMinDepth > fMaxDepth)
            {
                if (fMaxDepth < psAxS->m_fDepth)
                {
                    psAxS->m_vNormal = psCY0->m.m.vU_HAX;
                    psAxS->m_fDepth = fMaxDepth;
                    psAxS->m_iAxis = 2;
                };
            }
            else
            {
                if (fMinDepth < psAxS->m_fDepth)
                {
                    psAxS->m_vNormal = V(F_NEG)(&psCY0->m.m.vU_HAX);
                    psAxS->m_fDepth = fMinDepth;
                    psAxS->m_iAxis = 3;
                };
            };
        };
    };

    {
        /* -- Axis: Radial Cylindrical Sweep -- */

        TYPE                                fET0, fET1;

        const TYPE fDistSq = V(tgCO_F_ST_ParamSq_SG)(&fET0, &fET1, &fCyl, psST0, &psCY0->m_sAX);

        if (fDistSq > psCY0->m_fRadiusSq)
        {
            return (TgFALSE);
        };

        if (V(tgGM_ST_Is_Point_Culled)(psST0, fET0, fET1))
        {
            return (TgTRUE);
        }
        else
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fET0, psST0->m_sCT.m_sET.m_avEdge + 0);
            V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fET1, psST0->m_sCT.m_sET.m_avEdge + 2);
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fCyl, &psCY0->m_sAX.m_vDirN);
            V(C_TgVEC)                          vK3 = V(F_SUB)(&vK0, &vK1);

            vProxCyl = V(F_ADD)(&psCY0->m_sAX.m_vOrigin, &vK2);
            vProxTri = V(F_ADD)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &vK3);
        };
    }

    if (F(tgCM_NR0)(fCyl) || F(tgCM_NR1)(fCyl))
    {
        /* -- Axis: Cap-Surface Component -- */

        const TYPE                          fDet = MKL(1.0) - fNormTest*fNormTest;

        if (fDet <= F(KTgROOT_EPS))
        {
            /* The plane upon which the cylinder cap rests is parallel to that of the triangle.  Return the last best result. */

            return (TgTRUE);
        }
        else
        {
            /* Define the line of intersection between the two planar surfaces. */

            V(TgLINE)                           sLN0;
            TYPE                                fG0;
            V(PU_STg2_CO_Clip_List)             psCL;

            const TYPE                          fInvDet = MKL(1.0) / fDet;
            const TYPE                          fCylD = V(F_DOT)(&psCY0->m.m.vU_HAX, &vProxCyl);
            const TYPE                          fTA = fInvDet*(fCylD - fNormTest*fTriD);
            const TYPE                          fTB = fInvDet*(fTriD - fNormTest*fCylD);
            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fTA, &psCY0->m.m.vU_HAX);
            V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fTB, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

            psCL = TgMALLOC_POOL( sizeof( V(STg2_CO_Clip_List) ) + 7 * sizeof( V(TgVEC) ) );
            psCL->m_niPoint = 0;
            psCL->m_niMax = 8;

            sLN0.m_vOrigin = V(F_ADD)(&vK0, &vK1);
            sLN0.m_vDirN = V(F_CX)(&psCY0->m.m.vU_HAX, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

            V(tgCO_F_ST_Clip_LN)(psCL, psST0, &sLN0);

            if (psCL->m_niPoint != 2)
            {
                /* No intersection can exist between the triangle and cylinder cap.  Error state.  Return the last best result. */
                TgFREE_POOL( psCL );
                return (TgTRUE);
            }
            else
            {
                /* Find the minimal distance to the cap rim. */

                V(C_TgVEC)                          vK2 = V(F_SUB)(psCL->m_avPoint + 1, psCL->m_avPoint);
                const TYPE                          fK0 = V(tgCO_F_LR11_ParamSq_VT)(&fG0, psCL->m_avPoint, &vK2, &vProxCyl);
                V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fG0, &vK2);

                TgFREE_POOL( psCL );

                if (fK0 > psCY0->m_fRadiusSq)
                {
                    /* Error state.  Return the last best result. */
                    return (TgTRUE);
                };

                vProxTri = V(F_ADD)(psCL->m_avPoint, &vK3);
            };
        };
    };

    {
        TYPE                                fTest;

        V(C_TgVEC)                          vT0 = V(F_SUB)(&vProxCyl, &vProxTri);
        V(C_TgVEC)                          vNormal = V(F_NORM_LEN)(&fTest, &vT0);

        if (F(tgCM_NR0)(fTest))
        {
            return (TgTRUE); /* Error state.  Return the last best result. */
        };

        fTest = F(tgPM_FSEL)(psCY0->m_fRadius - fTest, psCY0->m_fRadius - fTest, MKL(0.0));

        if (1 == psAxS->m_iAxis && F(tgCM_NR1)(V(F_DOT)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &vNormal)))
        {
            return (TgTRUE); /* Bias towards having the contacts created for a triangle contact. */
        };

        if (fTest < psAxS->m_fDepth)
        {
            psAxS->m_vNormal = vNormal;
            psAxS->m_vPoint = vProxCyl;
            psAxS->m_fDepth = fTest;
            psAxS->m_iAxis = 4;
        };
    };

    return (TgTRUE);
}


/* ---- V(tgCO_F_ST_Internal_ClipTriToCyl_CY) --------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psST0: Space Triangle primitive                                                                                                                                */
/* Input:  psCY0: Cylinder primitive                                                                                                                                      */
/* Output: sCL: The resulting points of the clipping operation                                                                                                            */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgBOOL V(tgCO_F_ST_Internal_ClipTriToCyl_CY)(V(PC_STg2_CO_Clip_List) psCL, V(CPC_TgSTRI) psST0, V(CPC_TgTUBE) psCY0)
{
    TgBOOL                              bC0, bC1, bTest0, bTest1, bRet = TgFALSE;
    TYPE                                fT0, fT1;

    /* Triangle Edge 0 */

    bRet |= V(tgCO_FI_CY_Clip_Param_LR11)( &fT0, &fT1, psCY0, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 0, psST0->m_sCT.m_sET.m_avEdge + 0);

    bC0 = F(tgCM_NR0)(fT0);
    bC1 = F(tgCM_NR1)(fT0);
    bTest0 = (bC0 && !V(tgGM_ST_Test_Point_0)(psST0)) || (bC1 && !V(tgGM_ST_Test_Point_1)(psST0)) || (!bC0 && !bC1 && !V(tgGM_ST_Test_Edge_0)(psST0));

    bC0 = F(tgCM_NR0)(fT1);
    bC1 = F(tgCM_NR1)(fT1);
    bTest1 = (bC0 && !V(tgGM_ST_Test_Point_0)(psST0)) || (bC1 && !V(tgGM_ST_Test_Point_1)(psST0)) || (!bC0 && !bC1 && !V(tgGM_ST_Test_Edge_0)(psST0));

    if (!bTest0)
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT0, psST0->m_sCT.m_sET.m_avEdge + 0);

        psCL->m_avPoint[psCL->m_niPoint++] = V(F_ADD)(psST0->m_sCT.m_sET.m_sPT.m_avPoint + 0, &vK0);
    };

    if (!bTest1)
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT1, psST0->m_sCT.m_sET.m_avEdge + 0);

        psCL->m_avPoint[psCL->m_niPoint++] = V(F_ADD)(psST0->m_sCT.m_sET.m_sPT.m_avPoint + 0, &vK0);
    };


    /* Triangle Edge 1 */

    bRet |= V(tgCO_FI_CY_Clip_Param_LR11)( &fT0, &fT1, psCY0, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 1, psST0->m_sCT.m_sET.m_avEdge + 1);

    bC0 = F(tgCM_NR0)(fT0);
    bC1 = F(tgCM_NR1)(fT0);
    bTest0 = (bC0 && !V(tgGM_ST_Test_Point_1)(psST0)) || (bC1 && !V(tgGM_ST_Test_Point_2)(psST0)) || (!bC0 && !bC1 && !V(tgGM_ST_Test_Edge_1)(psST0));

    bC0 = F(tgCM_NR0)(fT1);
    bC1 = F(tgCM_NR1)(fT1);
    bTest1 = (bC0 && !V(tgGM_ST_Test_Point_1)(psST0)) || (bC1 && !V(tgGM_ST_Test_Point_2)(psST0)) || (!bC0 && !bC1 && !V(tgGM_ST_Test_Edge_1)(psST0));

    if (!bTest0)
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT0, psST0->m_sCT.m_sET.m_avEdge + 1);

        psCL->m_avPoint[psCL->m_niPoint++] = V(F_ADD)(psST0->m_sCT.m_sET.m_sPT.m_avPoint + 1, &vK0);
    };

    if (!bTest1)
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT1, psST0->m_sCT.m_sET.m_avEdge + 1);

        psCL->m_avPoint[psCL->m_niPoint++] = V(F_ADD)(psST0->m_sCT.m_sET.m_sPT.m_avPoint + 1, &vK0);
    };


    /* Triangle Edge 2 */

    bRet |= V(tgCO_FI_CY_Clip_Param_LR11)( &fT0, &fT1, psCY0, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 2, psST0->m_sCT.m_sET.m_avEdge + 2 );

    bC0 = F(tgCM_NR0)(fT0);
    bC1 = F(tgCM_NR1)(fT0);
    bTest0 = (bC0 && !V(tgGM_ST_Test_Point_2)(psST0)) || (bC1 && !V(tgGM_ST_Test_Point_0)(psST0)) || (!bC0 && !bC1 && !V(tgGM_ST_Test_Edge_2)(psST0));

    bC0 = F(tgCM_NR0)(fT1);
    bC1 = F(tgCM_NR1)(fT1);
    bTest1 = (bC0 && !V(tgGM_ST_Test_Point_2)(psST0)) || (bC1 && !V(tgGM_ST_Test_Point_0)(psST0)) || (!bC0 && !bC1 && !V(tgGM_ST_Test_Edge_2)(psST0));

    if (!bTest0)
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT0, psST0->m_sCT.m_sET.m_avEdge + 2);

        psCL->m_avPoint[psCL->m_niPoint++] = V(F_ADD)(psST0->m_sCT.m_sET.m_sPT.m_avPoint + 2, &vK0);
    };

    if (!bTest1)
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT1, psST0->m_sCT.m_sET.m_avEdge + 2);

        psCL->m_avPoint[psCL->m_niPoint++] = V(F_ADD)(psST0->m_sCT.m_sET.m_sPT.m_avPoint + 2, &vK0);
    };


    return (bRet);
}