Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Cylinder-Cylinder.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Cylinder;
    »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_CY_Axis_Seperation_CY)( V(PC_STg2_CO_Axis_Result), V(CPC_TgTUBE), V(CPC_TgTUBE));
static TgRESULT                             V(tgCO_F_CY_Internal_CapCap_CY)( V(PU_STg2_CO_Contact), PC_TgSINT32, V(CPC_TgVEC), V(CPC_TgELLIPSE), V(CPC_TgTUBE), V(CPC_TgTUBE));
static TgRESULT                             V(tgCO_F_CY_Internal_CapContained_CY)( V(PU_STg2_CO_Contact), PC_TgSINT32, V(CPC_TgVEC), V(CPC_TgTUBE), V(CPC_TgTUBE));
static TgRESULT                             V(tgCO_F_CY_Internal_AxisCap_CY)( V(PU_STg2_CO_Contact), PC_TgSINT32, V(CPC_TgVEC), V(CPC_TgTUBE), V(CPC_TgTUBE));




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

/* ---- V(tgCO_F_CY_Penetrate_CY) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psCY0: Cylinder primitive                                                                                                                                      */
/* Input:  psCY1: 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_CY_Penetrate_CY)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgTUBE) psCY0, V(CPC_TgTUBE) psCY1)
{
    V(P_STg2_CO_Contact)                psContact;
    V(STg2_CO_Axis_Result)              sAxS;
    V(STg2_CO_Contact)                  sContact[4];
    TgSINT32                            niMax, iIdx, niPoint = 0;

    V(C_TgVEC)                          vDS = V(F_SUB)(&psCY1->m.m.vOrigin, &psCY0->m.m.vOrigin);
    const TYPE                          fDS_A0 = V(F_DOT)(&vDS, &psCY0->m.m.vU_HAX);
    const TYPE                          fDS_A1 = V(F_DOT)(&vDS, &psCY1->m.m.vU_HAX);

    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_TB_Is_Valid)(psCY1) );

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

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

    if (!V(tgCO_F_CY_Axis_Seperation_CY)(&sAxS, psCY0, psCY1))
    {
        return (KTgE_NO_INTERSECT);
    };

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

    /* == Contact Generation == */

    switch (sAxS.m_iAxis)
    {
        case 0: /* -- Axis: Cylinder #0 Axis -- */
        {
            V(TgELLIPSE)                        sEL1;

            if (V(tgGM_CY_Is_Cap_Contained)(&sEL1, psCY0, fDS_A0, psCY1, fDS_A1))
            {
                if (F(tgPM_ABS)(V(F_DOT)(&psCY0->m.m.vU_HAX, &psCY1->m.m.vU_HAX)) < F(KTgF_SQRT1_2))
                {
                    if (V(tgCO_F_CY_Internal_AxisCap_CY)(sContact, &niPoint, &sAxS.m_vNormal, psCY0, psCY1) >= 0)
                    {
                        break;
                    };
                }
                else if (V(tgCO_F_CY_Internal_CapContained_CY)(sContact, &niPoint, &sAxS.m_vNormal, psCY0, psCY1) >= 0)
                {
                    break;
                };
            }
            else
            {
                /* The contact normal is the primary axis of cylinder zero.  Now we need to determine if the case to be generated has the ends touching or an end and */
                /* side touching. */

                if (F(tgPM_ABS)(V(F_DOT)(&psCY0->m.m.vU_HAX, &psCY1->m.m.vU_HAX)) < F(KTgF_SQRT1_2))
                {
                    if (V(tgCO_F_CY_Internal_AxisCap_CY)(sContact, &niPoint, &sAxS.m_vNormal, psCY0, psCY1) >= 0)
                    {
                        break;
                    };
                }
                else if (V(tgCO_F_CY_Internal_CapCap_CY)(sContact, &niPoint, &sAxS.m_vNormal, &sEL1, psCY0, psCY1) >= 0)
                {
                    break;
                };
            };

            return (KTgE_NO_INTERSECT);
        };

        case 2: /* -- Axis: Cylinder #1 Axis -- */
        {
            TgRESULT                            iResult;
            V(TgELLIPSE)                        sEL0;
            TgSINT32                            iIndex;

            if (V(tgGM_CY_Is_Cap_Contained)(&sEL0, psCY1, -fDS_A1, psCY0, -fDS_A0))
            {
                if (F(tgPM_ABS)(V(F_DOT)(&psCY0->m.m.vU_HAX, &psCY1->m.m.vU_HAX)) < F(KTgF_SQRT1_2))
                {
                    iResult = V(tgCO_F_CY_Internal_AxisCap_CY)(sContact, &niPoint, &sAxS.m_vNormal, psCY1, psCY0);

                    if (iResult >= 0)
                    {
                        /* Since we swapped the primitives and normal during the contact generation we now need to take the points and reverse them back out so they are */
                        /* being generated on the correct geometry in the correct direction. */
                        for (iIndex = 0; iIndex < niPoint; ++iIndex)
                        {
                            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(sContact[iIndex].m_fDepth, &sAxS.m_vNormal);

                            sContact[iIndex].m_vS0 = V(F_SUB)(&sContact[iIndex].m_vS0, &vK0);
                        };
                        break;
                    };
                }
                else
                {
                    iResult = V(tgCO_F_CY_Internal_CapContained_CY)(sContact, &niPoint, &sAxS.m_vNormal, psCY1, psCY0);

                    if (iResult >= 0)
                    {
                        /* Since we swapped the primitives and normal during the contact generation we now need to take the points and reverse them back out so they are */
                        /* being generated on the correct geometry in the correct direction. */
                        for (iIndex = 0; iIndex < niPoint; ++iIndex)
                        {
                            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(sContact[iIndex].m_fDepth, &sAxS.m_vNormal);

                            sContact[iIndex].m_vS0 = V(F_ADD)(&sContact[iIndex].m_vS0, &vK0);
                        };
                        sAxS.m_vNormal = V(F_NEG)(&sAxS.m_vNormal);
                        break;
                    };
                };
            }
            else
            {
                if (F(tgPM_ABS)(V(F_DOT)(&psCY0->m.m.vU_HAX, &psCY1->m.m.vU_HAX)) < F(KTgF_SQRT1_2))
                {
                    iResult = V(tgCO_F_CY_Internal_AxisCap_CY)(sContact, &niPoint, &sAxS.m_vNormal, psCY0, psCY1);

                    if (iResult >= 0)
                    {
                        /* Since we swapped the primitives and normal during the contact generation we now need to take the points and reverse them back out so they are */
                        /* being generated on the correct geometry in the correct direction. */
                        for (iIndex = 0; iIndex < niPoint; ++iIndex)
                        {
                            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(sContact[iIndex].m_fDepth, &sAxS.m_vNormal);

                            sContact[iIndex].m_vS0 = V(F_SUB)(&sContact[iIndex].m_vS0, &vK0);
                        };
                        break;
                    };
                }

                iResult = V(tgCO_F_CY_Internal_CapCap_CY)(sContact, &niPoint, &sAxS.m_vNormal, &sEL0, psCY0, psCY1);

                if (iResult >= 0)
                {
                    /* Since we swapped the primitives and normal during the contact generation we now need to take the points and reverse them back out so they are */
                    /* being generated on the correct geometry in the correct direction. */
                    for (iIndex = 0; iIndex < niPoint; ++iIndex)
                    {
                        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(sContact[iIndex].m_fDepth, &sAxS.m_vNormal);

                        sContact[iIndex].m_vS0 = V(F_SUB)(&sContact[iIndex].m_vS0, &vK0);
                    };
                    break;
                };
            };

            return (KTgE_NO_INTERSECT);
        };

        case 4: /* -- Axis: Perpendicular to Common Cylinder Axis -- */
        {
            const TYPE                          fMinY = F(tgCM_MAX)(-psCY1->m_fExtent, fDS_A1 - psCY0->m_fExtent);
            const TYPE                          fMaxY = F(tgCM_MIN)(psCY1->m_fExtent, fDS_A1 + psCY0->m_fExtent);
            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fMaxY, &psCY1->m.m.vU_HAX);
            V(C_TgVEC)                          vK1 = V(F_MUL_SV)(psCY1->m_fRadius, &sAxS.m_vNormal);
            V(C_TgVEC)                          vK2 = V(F_ADD)(&psCY1->m.m.vOrigin, &vK0);

            if (!F(tgCM_NR0)(fMinY - fMaxY))
            {
                V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fMinY, &psCY1->m.m.vU_HAX);
                V(C_TgVEC)                          vK4 = V(F_MUL_SV)(psCY1->m_fRadius, &sAxS.m_vNormal);
                V(C_TgVEC)                          vK5 = V(F_ADD)(&psCY1->m.m.vOrigin, &vK3);

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

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

                ++psPacket->m_niContact;

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

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

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

            ++psPacket->m_niContact;

            return (KTgS_OK);
        };

        case 8: /* -- Axis: Contact Between the Two Cylinder Rims -- */
        case 5: /* -- Axis: Between Points of Closest Proximity -- */
        {
            /* Since this axis is only generated when the points of closest proximity do not lie at the termini of the respective cylinder axes, the resulting direction */
            /* vector is known to be orthogonal to both axes.  (Proof by Observation: the minimal distance would be along a mutually perpendicular vector, which is known */
            /* to exist since the lines are also known to be skew.) */

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

            psContact->m_vS0 = sAxS.m_vPoint;
            psContact->m_vN0 = sAxS.m_vNormal;
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = sAxS.m_fDepth;

            ++psPacket->m_niContact;

            return (KTgS_OK);
        };

        case 6: /* -- Axis: Perpendicular to Axis of Cylinder 0 -- */
        {
            V(C_TgVEC)                          vL0 = V(F_NEG)(&sAxS.m_vNormal);

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

            psContact->m_vS0 = V(tgGM_CY_Support_Point)(psCY1, &vL0);
            psContact->m_vN0 = sAxS.m_vNormal;
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = sAxS.m_fDepth;

            ++psPacket->m_niContact;

            return (KTgS_OK);
        };

        case 7: /* -- Axis: Perpendicular to Axis of Cylinder 1 -- */
        {
            V(C_TgVEC)                          vL0 = V(F_MUL_SV)(sAxS.m_fDepth, &sAxS.m_vNormal);
            V(C_TgVEC)                          vL1 = V(tgGM_CY_Support_Point)(psCY0, &sAxS.m_vNormal);

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

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

            ++psPacket->m_niContact;

            return (KTgS_OK);
        };

        default:
            TgS_NO_DEFAULT(break);
    };

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

    for (iIdx = 0; iIdx < niMax; ++iIdx)
    {
        psContact = psPacket->m_psContact + psPacket->m_niContact;

        psContact->m_vS0 = sContact[iIdx].m_vS0;
        psContact->m_vN0 = sAxS.m_vNormal;
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = sContact[iIdx].m_fDepth;

        ++psPacket->m_niContact;
    };

    return (niMax != niPoint ? KTgE_MAX_CONTACTS : KTgS_OK);
}




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

/* ---- V(tgCO_F_CY_Axis_Seperation_CY) --------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psCY0, psCY1: Cylinder primitives                                                                                                                              */
/* 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_CY_Axis_Seperation_CY)(V(PC_STg2_CO_Axis_Result) psAxS, V(CPC_TgTUBE) psCY0, V(CPC_TgTUBE) psCY1)
{
    /*  To figure out all the possible axes of separation, it is necessary only to imagine the infinite set of vectors that are orthogonal to the cylinder surface.  Once */
    /* conceived, it becomes obvious to separate the cylinder into three conceptual parts.  The body is the main tube section of the cylinder.  The caps are the planar */
    /* terminal part of the cylinder and the rims are where the planar terminal part of the cylinder and the tube intersect.  The body and the caps have obvious and well */
    /* defined orthogonal vectors sets.  In the case of the caps, the set has only one member - the normal defining the plane. For the body, the set is infinite, defined */
    /* as those vectors in the cylinder cross-sectional plane that are directed radially.  The rim set is composed of a locus forming a quarter torus.  Most of these */
    /* axes are invalid, and its a matter of choosing the right ones - and preferably a finite set in computational amount of time.  The correct axes can be found using */
    /* the following methods: */

    /* Cap-Rim      All cases should be found by the axial test */
    /* Cap-Cap      All cases should be found by the axial test */
    /* Cap-Body     All cases should be found by the axial test, if not the minimal distance vector will catch it. */

    /* Rim-Body     Minimal distance vector, projected onto the cylinder plane, will find all such cases. */
    /* Rim-Rim      The intersection of the two sets of normal rim vectors should be unique and is the test case. */

    /* Body-Body    Minimal distance vector will find all such cases. */


    TYPE                                fDistSq, fTest, fCA0, fCA1, fT0, fT1, fT2, fT3;

    /* Construct the difference vector between the two origins and calculate the reference frame projections. */

    V(C_TgVEC)                          vDS = V(F_SUB)(&psCY1->m.m.vOrigin, &psCY0->m.m.vOrigin);
    const TYPE                          fDS_A0 = V(F_DOT)(&vDS, &psCY0->m.m.vU_HAX);
    const TYPE                          fDS_A1 = V(F_DOT)(&vDS, &psCY1->m.m.vU_HAX);
    const TYPE                          fABS_A0_A1 = F(tgPM_ABS)(V(F_DOT)(&psCY0->m.m.vU_HAX, &psCY1->m.m.vU_HAX));
    const TYPE                          fABS_A0xA1 = F(tgPM_SQRT)(MKL(1.0) - F(tgCM_MIN)(MKL(1.0), fABS_A0_A1*fABS_A0_A1));
    const TYPE                          fRadSum = psCY0->m_fRadius + psCY1->m_fRadius;

    psAxS->m_fDepth = F(KTgMAX);

    TgERROR( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_TB_Is_Valid)(psCY1) );

    /* == Axis Separation Tests == */

    /* -- Axis: Cylinder #0 Axis -- */

    fTest = (psCY0->m_fExtent + psCY1->m_fRadius*fABS_A0xA1 + psCY1->m_fExtent*fABS_A0_A1) - F(tgPM_ABS)(fDS_A0);

    if (fTest <= MKL(0.0))
    {
        return (TgFALSE);
    };

    if (fTest < psAxS->m_fDepth)
    {
        psAxS->m_vNormal = fDS_A0 > MKL(0.0) ? psCY0->m.m.vU_HAX : V(F_NEG)(&psCY0->m.m.vU_HAX);
        psAxS->m_fDepth = fTest;
        psAxS->m_iAxis = 0;
    };

    /* == Cylinder Parallel Test == */

    if (F(tgCM_NR1)(fABS_A0_A1) || F(tgCM_NR0)(fABS_A0xA1))
    {
        /* -- Axis: Perpendicular to Common Cylinder Axis -- */

        fTest = V(F_LSQ)(&vDS) - fDS_A0*fDS_A0;

        if (fTest > fRadSum*fRadSum)
        {
            return (TgFALSE);
        };

        fTest = fRadSum - F(tgPM_SQRT)(fTest);

        if (fTest < psAxS->m_fDepth)
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fDS_A0, &psCY0->m.m.vU_HAX);

            psAxS->m_vNormal = V(F_SUB)(&vDS, &vK0);
            psAxS->m_fDepth = fTest;
            psAxS->m_iAxis = 4;

            psAxS->m_vNormal = V(F_NORM)(&psAxS->m_vNormal);
        };

        return (TgTRUE);
    };

    /* -- Axis: Cylinder #1 Axis -- */

    fTest = (psCY1->m_fExtent + psCY0->m_fRadius*fABS_A0xA1 + psCY0->m_fExtent*fABS_A0_A1) - F(tgPM_ABS)(fDS_A1);

    if (fTest <= MKL(0.0))
    {
        return (TgFALSE);
    };

    if (fTest < psAxS->m_fDepth)
    {
        psAxS->m_vNormal = fDS_A1 > MKL(0.0) ? V(F_NEG)(&psCY1->m.m.vU_HAX) : psCY1->m.m.vU_HAX;
        psAxS->m_fDepth = fTest;
        psAxS->m_iAxis = 2;
    };

    /* ======================================================================================= */

    fDistSq = V(tgCO_F_SG_ParamSq_SG)(&fCA0, &fCA1, &psCY0->m_sAX, &psCY1->m_sAX);

    if (fDistSq > fRadSum*fRadSum)
    {
        return (TgFALSE);
    };

    {
        V(TgVEC)                            vT0, vT1;
        V(TgLINE)                           sLN0;

        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fCA0, &psCY0->m_sAX.m_vDirN);
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fCA1, &psCY1->m_sAX.m_vDirN);
        V(C_TgVEC)                          vMin_CY0 = V(F_ADD)(&psCY0->m_sAX.m_vOrigin, &vK0);
        V(C_TgVEC)                          vMin_CY1 = V(F_ADD)(&psCY1->m_sAX.m_vOrigin, &vK1);
        V(C_TgVEC)                          vMinDist = V(F_SUB)(&vMin_CY1, &vMin_CY0);
        V(C_TgVEC)                          vMinDirN = V(F_NORM)(&vMinDist);
        C_TgBOOL                            bCapContact0 = F(tgCM_NR1)(fCA0) || F(tgCM_NR0)(fCA0);
        C_TgBOOL                            bCapContact1 = F(tgCM_NR1)(fCA1) || F(tgCM_NR0)(fCA1);

        /* -- Axis: Between Points of Closest Proximity -- */

        if (!bCapContact0 && !bCapContact1)
        {
            V(tgGM_CY_Project)(&fT0, &fT1, psCY0, &vMinDirN);
            V(tgGM_CY_Project)(&fT2, &fT3, psCY1, &vMinDirN);

            if (fT1 - fT2 < MKL(0.0) || fT3 - fT0 < MKL(0.0))
            {
                return (TgFALSE);
            };

            fTest = fT2 - fT1;

            if (fTest < psAxS->m_fDepth)
            {
                V(C_TgVEC)                          vK2 = V(F_MUL_SV)(psCY1->m_fRadius, &vMinDirN);

                psAxS->m_vPoint = V(F_SUB)(&vMin_CY1, &vK2);
                psAxS->m_vNormal = vMinDirN;
                psAxS->m_fDepth = fTest;
                psAxS->m_iAxis = 5;
            };
        };

        if (bCapContact0) /* If at a cylinder termini, a better axis may be the minimal direction projected on the cylinder plane. */
        {
            /* -- Axis: Perpendicular to Axis of Cylinder 0 -- */
            /* Example: Rim to Cylinder contact. */

            const TYPE                          fK0 = V(F_DOT)(&vMinDist, &psCY0->m.m.vU_Basis0);
            const TYPE                          fK1 = V(F_DOT)(&vMinDist, &psCY0->m.m.vU_Basis1);
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fK0, &psCY0->m.m.vU_Basis0);
            V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK1, &psCY0->m.m.vU_Basis1);
            V(C_TgVEC)                          vK4 = V(F_ADD)(&vK2, &vK3);
            V(C_TgVEC)                          vX0 = V(F_NORM)(&vK4);
            const TYPE                          fX0_A11_ABS = F(tgPM_ABS)(V(F_DOT)(&vX0, &psCY1->m.m.vU_HAX));
            const TYPE                          fX0_C10 = V(F_DOT)(&vX0, &psCY1->m.m.vU_Basis0);
            const TYPE                          fX0_C11 = V(F_DOT)(&vX0, &psCY1->m.m.vU_Basis1);
            const TYPE                          fK2 = F(tgPM_SQRT)(fX0_C10*fX0_C10 + fX0_C11*fX0_C11);
            const TYPE                          fK3 = psCY0->m_fRadius + fX0_A11_ABS*psCY1->m_fExtent;
            const TYPE                          fK4 = (fK3 + fK2*psCY1->m_fRadius) - F(tgPM_ABS)(V(F_DOT)(&vX0, &vDS));

            if (fK4 <= MKL(0.0))
            {
                return (TgFALSE);
            };

            if (fK4 < psAxS->m_fDepth)
            {
                psAxS->m_vNormal = vX0;
                psAxS->m_fDepth = fK4;
                psAxS->m_iAxis = 6;
            };
        };

        if (bCapContact1) /* If at a cylinder termini, a better axis may be the minimal direction projected on the cylinder plane. */
        {
            /* -- Axis: Perpendicular to Axis of Cylinder 1 -- */
            /* Example: Rim to Cylinder contact. */

            const TYPE                          fK0 = V(F_DOT)(&vMinDist, &psCY1->m.m.vU_Basis0);
            const TYPE                          fK1 = V(F_DOT)(&vMinDist, &psCY1->m.m.vU_Basis1);
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fK0, &psCY1->m.m.vU_Basis0);
            V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK1, &psCY1->m.m.vU_Basis1);
            V(C_TgVEC)                          vK4 = V(F_ADD)(&vK2, &vK3);
            V(C_TgVEC)                          vX0 = V(F_NORM)(&vK4);
            const TYPE                          fX0_A01_ABS = F(tgPM_ABS)(V(F_DOT)(&vX0, &psCY0->m.m.vU_HAX));
            const TYPE                          fX0_C00 = V(F_DOT)(&vX0, &psCY0->m.m.vU_Basis0);
            const TYPE                          fX0_C01 = V(F_DOT)(&vX0, &psCY0->m.m.vU_Basis1);
            const TYPE                          fK2 = F(tgPM_SQRT)(fX0_C00*fX0_C00 + fX0_C01*fX0_C01);
            const TYPE                          fK3 = psCY1->m_fRadius + fX0_A01_ABS*psCY0->m_fExtent;
            const TYPE                          fK4 = (fK3 + fK2*psCY1->m_fRadius) - F(tgPM_ABS)(V(F_DOT)(&vX0, &vDS));

            if (fK4 <= MKL(0.0))
            {
                return (TgFALSE);
            };

            if (fK4 < psAxS->m_fDepth)
            {
                psAxS->m_vNormal = vX0;
                psAxS->m_fDepth = fK4;
                psAxS->m_iAxis = 7;
            };
        };

        if (!bCapContact0 || !bCapContact1 || V(F_LSQ)(&vDS) < F(KTgROOT_EPS))
        {
            return (TgTRUE);
        }
        else
        {
            /* Construct the line of intersection between the two cylinder cap planes. */

            const TYPE                          fA0_A1 = V(F_DOT)(&psCY0->m.m.vU_HAX, &psCY1->m.m.vU_HAX);
            const TYPE                          fA = MKL(1.0) - fA0_A1*fA0_A1;
            const TYPE                          fInvA = MKL(1.0) / fA;
            const TYPE                          fD_CY0 = V(F_DOT)(&psCY0->m.m.vU_HAX, &vMin_CY0);
            const TYPE                          fD_CY1 = V(F_DOT)(&psCY1->m.m.vU_HAX, &vMin_CY1);
            const TYPE                          fK0 = (fD_CY0 - fD_CY1*fA0_A1) * fInvA;
            const TYPE                          fK1 = (fD_CY1 - fD_CY0*fA0_A1) * fInvA;
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fK1, &psCY1->m.m.vU_HAX);
            V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK0, &psCY0->m.m.vU_HAX);

            if (fA <= F(KTgROOT_EPS))
            {
                return (TgTRUE); /* Make sure the two planes are not co-planar. */
            };

            sLN0.m_vOrigin = V(F_ADD)(&vK3, &vK2);
            sLN0.m_vDirN = V(F_CX)(&psCY0->m.m.vU_HAX, &psCY1->m.m.vU_HAX);
        };

        /* F_Clip the line to cylinder 0 and find the contact point */

        V(tgCO_F_CY_Clip_Param_LN)(&fT0, &fT1, psCY0, &sLN0);

        {
            V(C_TgVEC)                          vK4 = V(F_MUL_SV)(fT0, &sLN0.m_vDirN);
            V(C_TgVEC)                          vK5 = V(F_ADD)(&sLN0.m_vOrigin, &vK4);
            V(C_TgVEC)                          vK6 = V(F_SUB)(&vK5, &vMin_CY1);

            if (V(F_LSQ)(&vK6) >= psCY1->m_fRadiusSq)
            {
                V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT1, &sLN0.m_vDirN);
                V(C_TgVEC)                          vK8 = V(F_ADD)(&sLN0.m_vOrigin, &vK7);
                V(C_TgVEC)                          vK9 = V(F_SUB)(&vK8, &vMin_CY1);

                vT0 = vK8;

                if (V(F_LSQ)(&vK9) >= psCY1->m_fRadiusSq)
                {
                    TgWARN_CO(TgT("Not 100% that this type of thing should happen - record for test case.\n" ));
                    return (TgFALSE);
                };
            }
            else
            {
                V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT1, &sLN0.m_vDirN);
                V(C_TgVEC)                          vK8 = V(F_ADD)(&sLN0.m_vOrigin, &vK7);
                V(C_TgVEC)                          vK9 = V(F_SUB)(&vK8, &vMin_CY1);

                vT0 = vK8;

                if (V(F_LSQ)(&vK9) <= psCY1->m_fRadiusSq)
                {
                    return (TgTRUE); /* If the entire segment is contained the best axis will be axial. */
                };
            };
        };

        /* F_Clip the line to cylinder 1 and find the contact point */

        V(tgCO_F_CY_Clip_Param_LN)(&fT0, &fT1, psCY1, &sLN0);

        {
            V(C_TgVEC)                          vK4 = V(F_MUL_SV)(fT0, &sLN0.m_vDirN);
            V(C_TgVEC)                          vK5 = V(F_ADD)(&sLN0.m_vOrigin, &vK4);
            V(C_TgVEC)                          vK6 = V(F_SUB)(&vK5, &vMin_CY1);

            if (V(F_LSQ)(&vK6) >= psCY0->m_fRadiusSq)
            {
                V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT1, &sLN0.m_vDirN);
                V(C_TgVEC)                          vK8 = V(F_ADD)(&sLN0.m_vOrigin, &vK7);
                V(C_TgVEC)                          vK9 = V(F_SUB)(&vK8, &vMin_CY1);

                vT1 = vK8;

                if (V(F_LSQ)(&vK9) >= psCY0->m_fRadiusSq)
                {
                    TgWARN_CO(TgT("Not 100% that this type of thing should happen - record for test case.\n" ));
                    return (TgFALSE);
                };
            }
            else
            {
                V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT1, &sLN0.m_vDirN);
                V(C_TgVEC)                          vK8 = V(F_ADD)(&sLN0.m_vOrigin, &vK7);
                V(C_TgVEC)                          vK9 = V(F_SUB)(&vK8, &vMin_CY1);

                vT1 = vK8;

                if (V(F_LSQ)(&vK9) <= psCY0->m_fRadiusSq)
                {
                    return (TgTRUE); /* If the entire segment is contained the best axis will be axial. */
                };
            };
        };

        /* Construct the tangent vectors, create a mutual orthogonal vector (cross-product) - this is the separating plane normal. Essentially the construction is */
        /* finding the (possibly unique) vector that exists in the intersection of the two sets of possible rim normals. */

        {
            TYPE                                fL0, fL1, fL2, fL3, fL4;

            V(C_TgVEC)                          vK2 = V(F_SUB)(&vT0, &vMin_CY0);
            V(C_TgVEC)                          vK3 = V(F_SUB)(&vT0, &vMin_CY1);
            V(C_TgVEC)                          vD0 = V(F_CX)(&vK2, &psCY0->m.m.vU_HAX); /* Tangent vector at rim on Cylinder 0 */
            V(C_TgVEC)                          vD1 = V(F_CX)(&vK3, &psCY1->m.m.vU_HAX); /* Tangent vector at rim on Cylinder 1 */
            V(C_TgVEC)                          vK4 = V(F_UCX_LEN)(&fL0, &vD0, &vD1);

            if (fL0 > F(KTgROOT_EPS))
            {
                V(tgGM_CY_Project)(&fL1, &fL2, psCY0, &vK4);
                V(tgGM_CY_Project)(&fL3, &fL4, psCY1, &vK4);

                fL2 -= fL3;
                fL4 -= fL1;

                if (fL2 < MKL(0.0) || fL4 < MKL(0.0))
                {
                    return (TgFALSE);
                };

                fTest = -F(tgCM_MIN)(fL2, fL4);

                if (fTest < psAxS->m_fDepth)
                {
                    psAxS->m_vPoint = vT0;
                    psAxS->m_vNormal = V(F_MUL_SV)(F(tgPM_FSEL)(fL2 - fL4, MKL(-1.0), MKL(1.0)), &vK4);
                    psAxS->m_fDepth = fTest;
                    psAxS->m_iAxis = 8;
                };
            };
        };

        {
            TYPE                                fL0, fL1, fL2, fL3, fL4;

            V(C_TgVEC)                          vK2 = V(F_SUB)(&vT1, &vMin_CY0);
            V(C_TgVEC)                          vK3 = V(F_SUB)(&vT1, &vMin_CY1);
            V(C_TgVEC)                          vD0 = V(F_CX)(&vK2, &psCY0->m.m.vU_HAX); /* Tangent vector at rim on Cylinder 0 */
            V(C_TgVEC)                          vD1 = V(F_CX)(&vK3, &psCY1->m.m.vU_HAX); /* Tangent vector at rim on Cylinder 1 */
            V(C_TgVEC)                          vK4 = V(F_UCX_LEN)(&fL0, &vD0, &vD1);

            if (fL0 > F(KTgROOT_EPS))
            {
                V(tgGM_CY_Project)(&fL1, &fL2, psCY0, &vK4);
                V(tgGM_CY_Project)(&fL3, &fL4, psCY1, &vK4);

                fL2 -= fL3;
                fL4 -= fL1;

                if (fL2 < MKL(0.0) || fL4 < MKL(0.0))
                {
                    return (TgFALSE);
                };

                fTest = -F(tgCM_MIN)(fL2, fL4);

                if (fTest < psAxS->m_fDepth)
                {
                    psAxS->m_vPoint = vT1;
                    psAxS->m_vNormal = V(F_MUL_SV)(F(tgPM_FSEL)(fL2 - fL4, MKL(-1.0), MKL(1.0)), &vK4);
                    psAxS->m_fDepth = fTest;
                    psAxS->m_iAxis = 8;
                };
            };
        };

        return (TgTRUE);
    }
}


/* ---- V(tgCO_F_CY_Internal_CapCap_CY) --------------------------------------------------------------------------------------------------------------------------------- */
/*  -- Internal Function -- (Helper to create contact points between caps of two cylinders)                                                                               */
/* Input:  vNormal: The axis of contact being used.                                                                                                                       */
/* Input:  tgEL0: The Elliptical projection of the cylinder cap onto the plane defined by the axis of contact                                                             */
/* Input:  psCY0: Cylinder primitive                                                                                                                                      */
/* Input:  psCY1: Cylinder primitive - contact points are generated on this primitive                                                                                     */
/* Output: sContact: An array of contact points to return for processing that will project cylinder 1 out of cylinder 0.                                                  */
/* Output: niPoint: The number of valid points in the array                                                                                                               */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_F_CY_Internal_CapCap_CY)(
    V(PU_STg2_CO_Contact) psContact, PC_TgSINT32 pniPoint, V(CPC_TgVEC) pvNormal, V(CPC_TgELLIPSE) psEL0, V(CPC_TgTUBE) psCY0, V(CPC_TgTUBE) psCY1 )
{
    TgSINT32                            niPoint = 0;
    TYPE                                fT0, fT1;
    TYPE                                fTest;
    TgRESULT                            iResult = KTgE_NO_INTERSECT;
    V(TgVEC)                            vT0, vT1;

    const TYPE                          fK0 = F(tgPM_FSEL)(V(F_DOT)(pvNormal, &psCY0->m.m.vU_HAX), MKL(1.0), MKL(-1.0));
    const TYPE                          fK1 = F(tgPM_FSEL)(V(F_DOT)(pvNormal, &psCY1->m.m.vU_HAX), MKL(-1.0), MKL(1.0));
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fK0, &psCY0->m_vHAX);
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fK1, &psCY1->m_vHAX);
    V(C_TgVEC)                          vC0 = V(F_ADD)(&psCY0->m.m.vOrigin, &vK0);
    V(C_TgVEC)                          vC1 = V(F_ADD)(&psCY1->m.m.vOrigin, &vK1);

    /* Create a reference frame for cylinder one based on the direction of deepest penetration.  To determine this reference frame the primary axis of the second cylinder */
    /* is projected down onto the cross-sectional plane of the first cylinder. It is the angle of the second cylinder that will give us the direction of maximum */
    /* penetration.  Once that has been determined that direction now has to be taken back into the second cylinder's reference frame to determine the result basis. */

    const TYPE                          fK2 = -fK1*V(F_DOT)(&psCY0->m.m.vU_Basis0, &psCY1->m.m.vU_HAX);
    const TYPE                          fK3 = -fK1*V(F_DOT)(&psCY0->m.m.vU_Basis1, &psCY1->m.m.vU_HAX);
    V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fK2, &psCY0->m.m.vU_Basis0);
    V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK3, &psCY0->m.m.vU_Basis1);
    V(C_TgVEC)                          vK4 = V(F_ADD)(&vK2, &vK3);
    V(C_TgVEC)                          vK5 = V(F_NORM_LEN)(&fTest, &vK4);
    V(C_TgVEC)                          vK6 = V(F_UCX)(&vK5, &psCY1->m.m.vU_HAX);
    V(C_TgVEC)                          vK7 = V(F_UCX)(&vK6, &psCY1->m.m.vU_HAX);
    V(C_TgVEC)                          vX0 = fTest <= F(KTgEPS) ? psCY1->m.m.vU_Basis0 : vK7;
    V(C_TgVEC)                          vX1 = fTest <= F(KTgEPS) ? psCY1->m.m.vU_Basis1 : vK6;

    /* Use the reference frame of cylinder 1 to create the point of deepest penetration. */

    V(C_TgVEC)                          vT2 = V(F_MUL_SV)(psCY1->m_fRadius, &vX0);
    V(C_TgVEC)                          vK8 = V(F_SUB)(&vC1, &vT2);
    V(C_TgVEC)                          vK9 = V(F_MUL_SV)(MKL(2.0), &vT2);
    const TYPE                          fABS_A0_A1 = F(tgPM_ABS)(V(F_DOT)(&psCY0->m.m.vU_HAX, &psCY1->m.m.vU_HAX));
    const TYPE                          fFactor = -fK1 / fABS_A0_A1;

    if (V(tgCO_FI_CY_Clip_Param_LR11)(&fT0, &fT1, psCY0, &vK8, &vK9) >= 0)
    {
        V(C_TgVEC)                          vKA = V(F_MUL_SV)(MKL(2.0)*fT1 - MKL(1.0), &vT2);
        V(C_TgVEC)                          vT3 = V(F_ADD)(&vC1, &vKA);
        V(C_TgVEC)                          vKB = V(F_SUB)(&vC0, &vT3);
        const TYPE                          fK4 = V(F_DOT)(&vKB, pvNormal);

        if (fK4 > MKL(0.0))
        {
            psContact[niPoint++].m_vS0 = vT3;
            psContact[niPoint].m_fDepth = fK4;
        };
    };

    /* At issue is the fact that the elliptical intersection routine requires the solution of a quartic equation which are known to be very unstable.  Specifically, as */
    /* the ellipse ratios become closer to that of a full circle the intersection routine will quickly become degenerate.  Therefore, a heuristic value to determine when */
    /* to use the elliptical routine over assuming that both caps are co-planar has to be used. Keep in mind the the math will be distinctly less prone to these problems */
    /* on the 64bit Power architectures used in the Xbox360 and PS3.  However, the calculations will have to be kept in the floating point unit and not vectorized. */

    if (fABS_A0_A1 >= F(KTgF_SQRT1_2))
    {
        if (F(tgCM_NR0)(psEL0->m_fMajor - psEL0->m_fMinor))
        {
            /* Use the circle routine */

            V(TgCIRCLE)                         sCI0, sCI1;

            V(tgGM_CI_Init)(&sCI0, &psCY0->m.m.vU_Basis0, &psCY0->m.m.vU_HAX, &psCY0->m.m.vU_Basis1, &vC0, psCY0->m_fRadius);
            V(tgGM_CI_Init)(&sCI1, &psCY1->m.m.vU_Basis0, &psCY1->m.m.vU_HAX, &psCY1->m.m.vU_Basis1, &vC1, psCY1->m_fRadius);

            iResult = V(tgCO_F_CI_Intersect2D_CI)(&vT0, &vT1, &sCI0, &sCI1);
        }
        else
        {
            /* Use the elliptical routine */

            V(TgELLIPSE)                        sEL1;

            V(tgGM_EL_Init)( &sEL1, &psCY0->m.m.vU_Basis0, &psCY0->m.m.vU_HAX, &psCY0->m.m.vU_Basis1, &vC0, psCY0->m_fRadius, psCY0->m_fRadius);

            iResult = V(tgCO_F_EL_Intersect2D_EL)(&vT0, &vT1, &sEL1, psEL0);
        };
    };

    if (KTgE_NO_INTERSECT == iResult)
    {
        /* Used the reference frame of cylinder 0 to create three symmetrical contacts around the rim/base. */
        V(C_TgVEC)                          vKA = V(F_MUL_SV)(F(KTgF_SQRT3), &vX1);
        V(C_TgVEC)                          vKB = V(F_ADD)(&vX0, &vKA);
        V(C_TgVEC)                          vKC = V(F_MUL_SV)(psCY1->m_fRadius*MKL(-0.5), &vKB);
        V(C_TgVEC)                          vKD = V(F_MUL_SV)(F(KTgF_SQRT3), &vX1);
        V(C_TgVEC)                          vKE = V(F_SUB)(&vX0, &vKD);
        V(C_TgVEC)                          vKF = V(F_MUL_SV)(psCY1->m_fRadius*MKL(-0.5), &vKE);

        if (V(tgCO_FI_CY_Clip_Param_LR11)(&fT0, &fT1, psCY0, &vC1, &vKC) >= 0)
        {
            V(C_TgVEC)                          vKG = V(F_MUL_SV)(fT1, &vKC);
            V(C_TgVEC)                          vKH = V(F_ADD)(&vC1, &vKG);
            V(C_TgVEC)                          vKI = V(F_SUB)(&vC0, &vKH);
            const TYPE                          fK4 = V(F_DOT)(&vKI, pvNormal);

            if (fK4 > MKL(0.0))
            {
                psContact[niPoint++].m_vS0 = vKH;
                psContact[niPoint].m_fDepth = fK4;
            };
        };


        if (V(tgCO_FI_CY_Clip_Param_LR11)(&fT0, &fT1, psCY0, &vC1, &vKF) >= 0)
        {
            V(C_TgVEC)                          vKG = V(F_MUL_SV)(fT1, &vKF);
            V(C_TgVEC)                          vKH = V(F_ADD)(&vC1, &vKG);
            V(C_TgVEC)                          vKI = V(F_SUB)(&vC0, &vKH);
            const TYPE                          fK4 = V(F_DOT)(&vKI, pvNormal);

            if (fK4 > MKL(0.0))
            {
                psContact[niPoint++].m_vS0 = vKH;
                psContact[niPoint].m_fDepth = fK4;
            };
        };

        return (0 != niPoint ? KTgS_OK : KTgE_NO_INTERSECT);
    }

    if (iResult < 0)
    {
        TgERROR_MSG( TgFALSE, TgT("F_Internal_CapCap: No intersection points found between the two caps.") );
        *pniPoint = 0;
        return (KTgE_FAIL);
    }
    else
    {
        /* Use vTo,vT1, and bisect them and create points on other side of the intersection space. */
        V(C_TgVEC)                          vKC = V(F_SUB)(&vT0, &vC1);
        V(C_TgVEC)                          vKD = V(F_SUB)(&vT1, &vC1);
        V(C_TgVEC)                          vKE = V(F_ADD)(&vT0, &vT1);
        V(C_TgVEC)                          vKF = V(F_MUL_SV)(MKL(0.5), &vKE);
        V(C_TgVEC)                          vKG = V(F_SUB)(&vKF, &vC1);
        V(C_TgVEC)                          vT3 = V(F_NORM)(&vKG);
        V(C_TgVEC)                          vKH = V(F_MUL_VS)(&vT3, psCY1->m_fRadius);
        const TYPE                          fK4 = fFactor*(V(F_DOT)(&vKC, &psCY1->m.m.vU_HAX));
        const TYPE                          fK5 = fFactor*(V(F_DOT)(&vKD, &psCY1->m.m.vU_HAX));
        const TYPE                          fK6 = fFactor*(V(F_DOT)(&vKH, &psCY1->m.m.vU_HAX));

        if (fK4 > MKL(0.0))
        {
            V(C_TgVEC)                          vL2 = V(F_MUL_SV)(fK0*fK4, &psCY0->m.m.vU_HAX);

            psContact[niPoint++].m_vS0 = V(F_SUB)(&vT0, &vL2);
            psContact[niPoint].m_fDepth = fK4;
        };

        if (fK5 > MKL(0.0))
        {
            V(C_TgVEC)                          vL2 = V(F_MUL_SV)(fK0*fK5, &psCY0->m.m.vU_HAX);

            psContact[niPoint++].m_vS0 = V(F_SUB)(&vT1, &vL2);
            psContact[niPoint].m_fDepth = fK5;
        };

        if (fK6 > MKL(0.0))
        {
            V(C_TgVEC)                          vL0 = V(F_MUL_SV)(fK0*fK6, &psCY0->m.m.vU_HAX);
            V(C_TgVEC)                          vL1 = V(F_ADD)(&vKH, &vC1);

            psContact[niPoint++].m_vS0 = V(F_SUB)(&vL1, &vL0);
            psContact[niPoint].m_fDepth = fK6;
        };

        *pniPoint = niPoint;
        return (KTgS_OK);
    };
}


/* ---- V(tgCO_F_CY_Internal_CapContained_CY) --------------------------------------------------------------------------------------------------------------------------- */
/*  -- Internal Function -- (Helper to create contact points between caps of two cylinders, given one is entirely contained)                                              */
/* Input:  vNormal: The axis of contact being used.                                                                                                                       */
/* Input:  psCY0: Cylinder primitive                                                                                                                                      */
/* Input:  psCY1: Cylinder primitive - contact points are generated on this primitive                                                                                     */
/* Output: sContact: An array of contact points to return for processing                                                                                                  */
/* Output: niPoint: The number of valid points in the array                                                                                                               */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_F_CY_Internal_CapContained_CY)(
    V(PU_STg2_CO_Contact) psContact, PC_TgSINT32 pniPoint, V(CPC_TgVEC) pvNormal, V(CPC_TgTUBE) psCY0, V(CPC_TgTUBE) psCY1 )
{
    TgSINT32                            niPoint = 0;
    TYPE                                fTest;

    const TYPE                          fK0 = F(tgPM_FSEL)(V(F_DOT)(pvNormal, &psCY0->m.m.vU_HAX), MKL(1.0), MKL(-1.0));
    const TYPE                          fK1 = F(tgPM_FSEL)(V(F_DOT)(pvNormal, &psCY1->m.m.vU_HAX), MKL(-1.0), MKL(1.0));
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fK0, &psCY0->m_vHAX);
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fK1, &psCY1->m_vHAX);
    V(C_TgVEC)                          vC0 = V(F_ADD)(&psCY0->m.m.vOrigin, &vK0);
    V(C_TgVEC)                          vC1 = V(F_ADD)(&psCY1->m.m.vOrigin, &vK1);

    /* Create a reference frame for cylinder one based on the direction of deepest penetration.  To determine this reference frame the primary axis of the first cylinder */
    /* is projected down onto the cross-sectional plane of the second cylinder. */

    const TYPE                          fK2 = -fK1*V(F_DOT)(&psCY0->m.m.vU_Basis0, &psCY1->m.m.vU_HAX);
    const TYPE                          fK3 = -fK1*V(F_DOT)(&psCY0->m.m.vU_Basis1, &psCY1->m.m.vU_HAX);
    V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fK2, &psCY0->m.m.vU_Basis0);
    V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK3, &psCY0->m.m.vU_Basis1);
    V(C_TgVEC)                          vK4 = V(F_ADD)(&vK2, &vK3);
    V(C_TgVEC)                          vK5 = V(F_NORM_LEN)(&fTest, &vK4);
    V(C_TgVEC)                          vX0 = fTest <= F(KTgEPS) ? psCY1->m.m.vU_Basis0 : vK5;
    V(C_TgVEC)                          vK6 = V(F_UCX)(&vX0, &psCY1->m.m.vU_HAX);
    V(C_TgVEC)                          vX1 = fTest <= F(KTgEPS) ? psCY1->m.m.vU_Basis1 : vK6;

    /* Used the reference frame of cylinder 1 to create three symmetrical contacts around the rim/base. */

    V(C_TgVEC)                          vKD = V(F_MUL_SV)(F(KTgF_SQRT3), &vX1);
    V(C_TgVEC)                          vKE = V(F_ADD)(&vX0, &vKD);
    V(C_TgVEC)                          vKF = V(F_SUB)(&vX0, &vKD);
    V(C_TgVEC)                          vL0 = V(F_MUL_SV)(psCY1->m_fRadius, &vX0);
    V(C_TgVEC)                          vL1 = V(F_ADD)(&vC1, &vL0);
    V(C_TgVEC)                          vL2 = V(F_SUB)(&vC0, &vL1);
    const TYPE                          fK4 = V(F_DOT)(&vL2, pvNormal);
    V(C_TgVEC)                          vL3 = V(F_MUL_SV)(psCY1->m_fRadius*MKL(-0.5), &vKE);
    V(C_TgVEC)                          vL4 = V(F_ADD)(&vC1, &vL3);
    V(C_TgVEC)                          vL5 = V(F_SUB)(&vC0, &vL4);
    const TYPE                          fK5 = V(F_DOT)(&vL5, pvNormal);
    V(C_TgVEC)                          vL6 = V(F_MUL_SV)(psCY1->m_fRadius*MKL(-0.5), &vKF);
    V(C_TgVEC)                          vL7 = V(F_ADD)(&vC1, &vL6);
    V(C_TgVEC)                          vL8 = V(F_SUB)(&vC0, &vL7);
    const TYPE                          fK6 = V(F_DOT)(&vL8, pvNormal);

    if (fK4 < MKL(0.0))
    {
        TgERROR_MSG( 0, TgT("F_Internal_CapContained: Axis determined a fully contained cylinder but found no contacts.") );
        *pniPoint = 0;
        return (KTgE_FAIL);
    };

    psContact[0].m_vS0 = vL1;
    psContact[0].m_fDepth = fK4;
    niPoint = 1;

    if (fK5 > MKL(0.0))
    {
        psContact[niPoint].m_vS0 = vL4;
        psContact[niPoint].m_fDepth = fK5;
        ++niPoint;
    };

    if (fK6 > MKL(0.0))
    {
        psContact[niPoint].m_vS0 = vL7;
        psContact[niPoint].m_fDepth = fK6;
        ++niPoint;
    };

    *pniPoint = niPoint;

    return (KTgS_OK);
}


/* ---- V(tgCO_F_CY_Internal_AxisCap_CY) -------------------------------------------------------------------------------------------------------------------------------- */
/*  -- Internal Function -- (Helper to create contact points between the tube and cap of two cylinders)                                                                   */
/* Input:  vNormal: The axis of contact being used.                                                                                                                       */
/* Input:  psCY0: Cylinder primitive                                                                                                                                      */
/* Input:  psCY1: Cylinder primitive - contact points are generated on this primitive                                                                                     */
/* Output: sContact: An array of contact points to return for processing                                                                                                  */
/* Output: niPoint: The number of valid points in the array                                                                                                               */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_F_CY_Internal_AxisCap_CY)( V(PU_STg2_CO_Contact) psContact, PC_TgSINT32 pniPoint, V(CPC_TgVEC) pvNormal, V(CPC_TgTUBE) psCY0, V(CPC_TgTUBE) psCY1 )
{
    TYPE                                fT0, fT1;

    const TYPE                          fK1 = F(tgPM_FSEL)(V(F_DOT)(pvNormal, &psCY1->m.m.vU_HAX), MKL(-1.0), MKL(1.0));
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fK1, &psCY1->m_vHAX);
    V(C_TgVEC)                          vC1 = V(F_ADD)(&psCY1->m.m.vOrigin, &vK1);

    /* Create a reference frame for cylinder one based on the direction of deepest penetration. */

    const TYPE                          fK2 = -fK1*V(F_DOT)(&psCY0->m.m.vU_Basis0, &psCY1->m.m.vU_HAX);
    const TYPE                          fK3 = -fK1*V(F_DOT)(&psCY0->m.m.vU_Basis1, &psCY1->m.m.vU_HAX);
    V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fK2, &psCY0->m.m.vU_Basis0);
    V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK3, &psCY0->m.m.vU_Basis1);
    V(C_TgVEC)                          vK4 = V(F_ADD)(&vK2, &vK3);
    V(C_TgVEC)                          vX0 = V(F_NORM)(&vK4);
    V(C_TgVEC)                          vK5 = V(F_MUL_SV)(psCY0->m_fRadius, &vX0);
    V(C_TgVEC)                          vS0 = V(F_ADD)(&psCY0->m_sAX.m_vOrigin, &vK5);

    TgERROR_MSG( F(tgCM_NR1)(V(F_LSQ)(&vX0)), TgT("F_Internal_AxisCap: Degenerate basis set formed.") );

    /* Create a segment along the length of the cylinder, in the direction of deepest penetration. */

    if (V(tgCO_FI_TB_Clip_Param_LR11)(&fT0, &fT1, psCY1, &vS0, &psCY0->m_sAX.m_vDirN) < 0)
    {
        *pniPoint = 0;
        return (KTgE_NO_INTERSECT);
    }
    else
    {
        V(TgPLANE)                          sPN0;

        V(C_TgVEC)                          vS1 = V(F_MUL_VS)(&vS0, fT0);
        V(C_TgVEC)                          vD1 = V(F_MUL_VS)(&psCY0->m_sAX.m_vDirN, fT1 - fT0);

        V(tgGM_PN_Init_NP)(&sPN0, pvNormal, &vC1);

        if (V(tgCO_FI_PN_Clip_Param_LR11)(&fT0, &fT1, &sPN0, &vS1, &vD1) < 0)
        {
            *pniPoint = 0;
            return (KTgE_NO_INTERSECT);
        }
        else
        {
            /* Segment was clipped to the cylinder surface.  Thus, the resulting point must be contained inside or on the cylinder. */

            V(C_TgVEC)                          vK6 = V(F_MUL_SV)(fT0, &psCY0->m_sAX.m_vDirN);
            V(C_TgVEC)                          vT0 = V(F_ADD)(&vS0, &vK6);
            V(C_TgVEC)                          vK7 = V(F_SUB)(&vT0, &vC1);
            const TYPE                          fK4 = V(F_DOT)(&vK7, pvNormal);
            V(C_TgVEC)                          vK8 = V(F_MUL_SV)(fT1, &psCY0->m_sAX.m_vDirN);
            V(C_TgVEC)                          vT1 = V(F_ADD)(&vS0, &vK8);
            V(C_TgVEC)                          vK9 = V(F_SUB)(&vT1, &vC1);
            const TYPE                          fK5 = V(F_DOT)(&vK9, pvNormal);

            psContact[0].m_vS0 = fK4 - fK5 > MKL(0.0) ? vT0 : vT1;
            psContact[0].m_fDepth = F(tgPM_FSEL)(fK4 - fK5, fK4, fK5);
            psContact[1].m_vS0 = fK4 - fK5 > MKL(0.0) ? vT1 : vT0;
            psContact[1].m_fDepth = F(tgPM_FSEL)(fK4 - fK5, fK5, fK4);

            /* Always create the point of largest penetration but only use the second point if it would generate a counter-moment. */

            *pniPoint = !F(tgCM_NR0)(fT0 - fT1) && ((fT0 - MKL(0.5) < MKL(0.0)) ^ (fT1 - MKL(0.5) < MKL(0.0))) ? 2 : 1;

            return (KTgS_OK);
        };
    };
}