Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Cylinder-Box.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Cylinder;Box;
    »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_BX_Axis_Seperation_CY)( V(PC_STg2_CO_Axis_Result), PC_TgBOOL, V(CPC_TgBOX), V(CPC_TgTUBE) );
static TgRESULT                             V(tgCO_F_BX_Penetrate_BoxAxis_CY)( V(PC_STg2_CO_Packet), V(CPC_STg2_CO_Axis_Result), PC_TgBOOL, V(CPC_TgBOX), V(CPC_TgTUBE) );
static TgRESULT                             V(tgCO_F_BX_Penetrate_CylAxis_CY)( V(PC_STg2_CO_Packet), V(CPC_STg2_CO_Axis_Result), V(CPC_TgBOX), V(CPC_TgTUBE) );
static TgRESULT                             V(tgCO_F_BX_Penetrate_CylRad_CY)( V(PC_STg2_CO_Packet), V(CPC_STg2_CO_Axis_Result), V(CPC_TgBOX), V(CPC_TgTUBE) );




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

/* ---- V(tgCO_F_BX_Penetrate_CY) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psBX0: Box 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_BX_Penetrate_CY)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgBOX) psBX0, V(CPC_TgTUBE) psCY0 )
{
    V(STg2_CO_Axis_Result)              sAxS;
    TgBOOL                              abIsContained[4];

    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_BX_Is_Valid)(psBX0) );

    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_BX_Axis_Seperation_CY)(&sAxS, abIsContained, psBX0, psCY0))
    {
        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:
        case 1:
        case 2: return (V(tgCO_F_BX_Penetrate_BoxAxis_CY)(psPacket, &sAxS, abIsContained, psBX0, psCY0));

        case 3: return (V(tgCO_F_BX_Penetrate_CylAxis_CY)(psPacket, &sAxS, psBX0, psCY0));

        case 4: return (V(tgCO_F_BX_Penetrate_CylRad_CY)(psPacket, &sAxS, psBX0, psCY0));
    };

    return (KTgE_FAIL);
}




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

/* ---- V(tgCO_F_BX_Axis_Seperation_CY) --------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psBX0: Box primitives                                                                                                                                          */
/* Input:  psCY0: Cylinder primitives                                                                                                                                     */
/* Output: sAxS: Structure holds the resulting axis separation information necessary to create a contact set.                                                             */
/* Output: bIsContained: A boolean array indicating which faces, if projected along their normal, fully contain the cylinder.                                             */
/* Return: False if a separating axis exists, true otherwise                                                                                                              */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgBOOL V(tgCO_F_BX_Axis_Seperation_CY)( V(PC_STg2_CO_Axis_Result) psAxS, PC_TgBOOL bIsContained, V(CPC_TgBOX) psBX0, V(CPC_TgTUBE) psCY0 )
{
    V(TgVEC)                            vCY0, vBX1;
    V(TgVEC)                            vNM;
    TYPE                                fTest;

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

    V(C_TgVEC)                          vDS = V(F_SUB)(&psBX0->m.m.vOrigin, &psCY0->m.m.vOrigin);

    const TYPE                          fDS_CYA = V(F_DOT)(&vDS, &psCY0->m.m.vU_HAX);
    const TYPE                          fDS_BX0 = V(F_DOT)(&vDS, psBX0->m.m.avAxis + 0);
    const TYPE                          fDS_BX1 = V(F_DOT)(&vDS, psBX0->m.m.avAxis + 1);
    const TYPE                          fDS_BX2 = V(F_DOT)(&vDS, psBX0->m.m.avAxis + 2);

    /* == Containment Test == */

    /* To allow for quick selection of a planar contact surface, test to see if the cylinder's projection onto each of the box's faces is entirely contained.  A similar */
    /* test to see if the box is fully contained by the cylinder's cap surface. */

    const TYPE                          fABS_CYA_BX0 = F(tgPM_ABS)(V(F_DOT)(&psCY0->m.m.vU_HAX, psBX0->m.m.avAxis + 0));
    const TYPE                          fABS_CYA_BX1 = F(tgPM_ABS)(V(F_DOT)(&psCY0->m.m.vU_HAX, psBX0->m.m.avAxis + 1));
    const TYPE                          fABS_CYA_BX2 = F(tgPM_ABS)(V(F_DOT)(&psCY0->m.m.vU_HAX, psBX0->m.m.avAxis + 2));

    const TYPE                          fABS_CYC_BX0 = F(tgPM_SQRT)(fABS_CYA_BX2*fABS_CYA_BX2 + fABS_CYA_BX1*fABS_CYA_BX1);
    const TYPE                          fABS_CYC_BX1 = F(tgPM_SQRT)(fABS_CYA_BX2*fABS_CYA_BX2 + fABS_CYA_BX0*fABS_CYA_BX0);
    const TYPE                          fABS_CYC_BX2 = F(tgPM_SQRT)(fABS_CYA_BX1*fABS_CYA_BX1 + fABS_CYA_BX0*fABS_CYA_BX0);

    const TYPE                          fABS_CY_BX0 = psCY0->m_fRadius*fABS_CYC_BX0 + psCY0->m_fExtent*fABS_CYA_BX0;
    const TYPE                          fABS_CY_BX1 = psCY0->m_fRadius*fABS_CYC_BX1 + psCY0->m_fExtent*fABS_CYA_BX1;
    const TYPE                          fABS_CY_BX2 = psCY0->m_fRadius*fABS_CYC_BX2 + psCY0->m_fExtent*fABS_CYA_BX2;

    TgERROR( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_BX_Is_Valid)(psBX0) );

    bIsContained[0] = (psBX0->m_vExtent.m.x >= fDS_BX0 + fABS_CY_BX0) && (fABS_CY_BX0 - fDS_BX0 <= psBX0->m_vExtent.m.x);
    bIsContained[1] = (psBX0->m_vExtent.m.y >= fDS_BX1 + fABS_CY_BX1) && (fABS_CY_BX1 - fDS_BX1 <= psBX0->m_vExtent.m.y);
    bIsContained[2] = (psBX0->m_vExtent.m.z >= fDS_BX2 + fABS_CY_BX2) && (fABS_CY_BX2 - fDS_BX2 <= psBX0->m_vExtent.m.z);

    /* == Axis Separation Tests == */

    psAxS->m_fDepth = -F(KTgMAX);

    /* -- Axis: Box Face/Plane Normals -- */

    /* Box Axis #0 */

    fTest = F(tgPM_ABS)(fDS_BX0) - (psBX0->m_vExtent.m.x + fABS_CY_BX0);

    if (fTest > MKL(0.0))
    {
        return (TgFALSE);
    };

    if (fTest > psAxS->m_fDepth)
    {
        psAxS->m_vNormal = fDS_BX0 > MKL(0.0) ? V(F_NEG)(psBX0->m.m.avAxis + 0) : psBX0->m.m.avAxis[0];
        psAxS->m_fDepth = fTest;
        psAxS->m_iAxis = 0;

        if (bIsContained[1] && bIsContained[2])
        {
            return (TgTRUE);
        };
    };

    /* Box Axis #1 */

    fTest = F(tgPM_ABS)(fDS_BX1) - (psBX0->m_vExtent.m.y + fABS_CY_BX1);

    if (fTest > MKL(0.0))
    {
        return (TgFALSE);
    };

    if (fTest > psAxS->m_fDepth)
    {
        psAxS->m_vNormal = fDS_BX1 > MKL(0.0) ? V(F_NEG)(psBX0->m.m.avAxis + 1) : psBX0->m.m.avAxis[1];
        psAxS->m_fDepth = fTest;
        psAxS->m_iAxis = 1;

        if (bIsContained[0] && bIsContained[2])
        {
            return (TgTRUE);
        };
    };

    /* Box Axis #2 */

    fTest = F(tgPM_ABS)(fDS_BX2) - (psBX0->m_vExtent.m.z + fABS_CY_BX2);

    if (fTest > MKL(0.0))
    {
        return (TgFALSE);
    };

    if (fTest > psAxS->m_fDepth)
    {
        psAxS->m_vNormal = fDS_BX2 > MKL(0.0) ? V(F_NEG)(psBX0->m.m.avAxis + 2) : psBX0->m.m.avAxis[2];
        psAxS->m_fDepth = fTest;
        psAxS->m_iAxis = 2;

        if (bIsContained[0] && bIsContained[1])
        {
            return (TgTRUE);
        };
    };

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

    fTest = F(tgPM_ABS)(fDS_CYA) - psCY0->m_fExtent;
    fTest -= psBX0->m_vExtent.m.x*fABS_CYA_BX0;
    fTest -= psBX0->m_vExtent.m.y*fABS_CYA_BX1;
    fTest -= psBX0->m_vExtent.m.z*fABS_CYA_BX2;

    if (fTest > MKL(0.0))
    {
        return (TgFALSE);
    };

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

    /* -- Axis: Cylinder Radial Direction -- */

    /* The cross-product of the three box edges and the cylinder axis is by definition a radial direction as well.  Therefore, it is not necessary to examine those axes */
    /* outside of the radial test. */

    fTest = V(tgCO_F_BX_ClosestSq_SG)(&vBX1, &vCY0, psBX0, &psCY0->m_sAX);

    if (fTest >= MKL(0.0))
    {
        V(C_TgVEC)                          vK0 = V(F_SUB)(&vCY0, &vBX1);
        const TYPE                          fK0 = V(F_DOT)(&vK0, &psCY0->m.m.vU_HAX);
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fK0, &psCY0->m.m.vU_HAX);
        V(C_TgVEC)                          vK2 = V(F_SUB)(&vCY0, &vBX1);
        V(C_TgVEC)                          vK3 = V(F_SUB)(&vK2, &vK1);
        V(C_TgVEC)                          vK4 = V(F_NORM)(&vK3);
        const TYPE                          fDS_N = V(F_DOT)(&vDS, &vK4);
        const TYPE                          fBX0_N = psBX0->m_vExtent.m.x*F(tgPM_ABS)(V(F_DOT)(psBX0->m.m.avAxis + 0, &vK4));
        const TYPE                          fBX1_N = psBX0->m_vExtent.m.y*F(tgPM_ABS)(V(F_DOT)(psBX0->m.m.avAxis + 1, &vK4));
        const TYPE                          fBX2_N = psBX0->m_vExtent.m.z*F(tgPM_ABS)(V(F_DOT)(psBX0->m.m.avAxis + 2, &vK4));

        fTest = F(tgPM_ABS)(fDS_N) - (psCY0->m_fRadius + fBX0_N + fBX1_N + fBX2_N);

        if (fTest > MKL(0.0))
        {
            return (TgFALSE);
        };

        if (fTest > psAxS->m_fDepth)
        {
            psAxS->m_vNormal = fDS_N > MKL(0.0) ? vK4 : V(F_NEG)(&vK4);
            psAxS->m_fDepth = fTest;
            psAxS->m_iAxis = 4;
        };
    }
    else
    {
        /* The line segment must have intersected the box.  The cylinder is highly penetrated.  Find the minimal distance between any box feature and the cylinder axis.  */
        /* Use the difference vector as a separation axis.  Box features are the twelve edges, and the eight vertices. */

        TYPE                                fMinSq = F(KTgMAX);
        V(TgVEC)                            vK1, vT0, vT1, vMinAxis, vMinEdge;
        V(TgSEGMENT)                        sEdge;
        TgSINT32                            iOrigin;

        for (iOrigin = 0; iOrigin < 4; ++iOrigin)
        {
            C_TgBOOL                            bAxis = ((0 != (iOrigin & 1)) ^ (0 != (iOrigin & 2)));

            sEdge.m_vOrigin = V(tgGM_BX_Calc_Point)(psBX0, 0 == (iOrigin & 1) ? psBX0->m_vExtent.m.x : -psBX0->m_vExtent.m.x,
                                                            0 == (iOrigin & 2) ? psBX0->m_vExtent.m.y : -psBX0->m_vExtent.m.y,
                                                                      bAxis ? psBX0->m_vExtent.m.z : -psBX0->m_vExtent.m.z);

            vK1 = 0 != (iOrigin & 1) ? psBX0->m.m.avAxis[0] : V(F_NEG)(psBX0->m.m.avAxis + 0);
            sEdge.m_vDirN = V(F_MUL_SV)(psBX0->m_vExtent.m.x, &vK1);

            fTest = V(tgCO_F_SG_ClosestSq_SG)(&vT0, &vT1, &psCY0->m_sAX, &sEdge);
            if (fTest < fMinSq)
            {
                fMinSq = fTest;
                vMinAxis = vT0;
                vMinEdge = vT1;
            };

            vK1 = 0 != (iOrigin & 2) ? psBX0->m.m.avAxis[1] : V(F_NEG)(psBX0->m.m.avAxis + 1);
            sEdge.m_vDirN = V(F_MUL_SV)(psBX0->m_vExtent.m.y, &vK1);

            fTest = V(tgCO_F_SG_ClosestSq_SG)(&vT0, &vT1, &psCY0->m_sAX, &sEdge);
            if (fTest < fMinSq)
            {
                fMinSq = fTest;
                vMinAxis = vT0;
                vMinEdge = vT1;
            };

            vK1 = 0 != (iOrigin & 1) ? psBX0->m.m.avAxis[2] : V(F_NEG)(psBX0->m.m.avAxis + 2);
            sEdge.m_vDirN = V(F_MUL_SV)(psBX0->m_vExtent.m.z, &vK1);

            fTest = V(tgCO_F_SG_ClosestSq_SG)(&vT0, &vT1, &psCY0->m_sAX, &sEdge);
            if (fTest < fMinSq)
            {
                fMinSq = fTest;
                vMinAxis = vT0;
                vMinEdge = vT1;
            };
        };

        {
            V(C_TgVEC)                          vK2 = V(F_SUB)(&vMinAxis, &vMinEdge);
            const TYPE                          fK0 = V(F_DOT)(&vK2, &psCY0->m.m.vU_HAX);
            V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK0, &psCY0->m.m.vU_HAX);
            V(C_TgVEC)                          vK4 = V(F_SUB)(&vK2, &vK3);
            V(C_TgVEC)                          vK5 = V(F_NORM)(&vK4);
            V(C_TgVEC)                          vK6 = V(F_NEG)(&vK5);
            V(C_TgVEC)                          vK7 = V(F_SUB)(&psCY0->m.m.vOrigin, &vT0);

            vNM = vK5;

            vT0 = V(tgGM_BX_Support_Point)(psBX0, &vK6);;
            fTest = psCY0->m_fRadius + V(F_DOT)(&vK7, &vK5);

            if (fTest > MKL(0.0))
            {
                return (TgFALSE);
            };

            if (fTest > psAxS->m_fDepth)
            {
                psAxS->m_vNormal = vNM;
                psAxS->m_fDepth = fTest;
                psAxS->m_iAxis = 4;
            };
        };
    };

    /* -- Axis: Cylinder Rim -- */
    /* Possible and if so how? */

    return (TgTRUE);
}


/* ---- V(tgCO_F_BX_Penetrate_BoxAxis_CY) ------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  sAxS: Structure holding the resulting axis separation information necessary to create a contact set.                                                           */
/* Input:  bIsContained: A boolean array indicating which faces, if projected along their normal, fully contain the cylinder.                                             */
/* Input:  psBX0: Box 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                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_F_BX_Penetrate_BoxAxis_CY)(
    V(PC_STg2_CO_Packet) psPacket, V(CPC_STg2_CO_Axis_Result) psAxS, PC_TgBOOL bIsContained, V(CPC_TgBOX) psBX0, V(CPC_TgTUBE) psCY0 )
{
    V(TgVEC)                            vCYB0, vCYB1, vT0;
    TYPE                                fLength;
    V(P_STg2_CO_Contact)                psContact;

    C_TgSINT32                          niContact = psPacket->m_niContact;

    /* The axis separation result normal is one of the box axes with the appropriate direction (sign). */

    C_TgSINT32                          i0 = (psAxS->m_iAxis + 1) % 3, i1 = (psAxS->m_iAxis + 2) % 3;
    const TYPE                          fKA = psBX0->m_vExtent.m_aData[psAxS->m_iAxis];
    const TYPE                          fPlnD = V(F_DOT)(&psBX0->m.m.vOrigin, &psAxS->m_vNormal) + fKA;
    C_TgSINT32                          iFlag = (bIsContained[i0] ? 0 : (1 << i0)) | (bIsContained[i1] ? 0 : (1 << i1));

    /* Create a new basis set for the cylinder such that the first axis is equal to the direction of greatest penetration. */

    const TYPE                          fCYA_N = V(F_DOT)(&psCY0->m.m.vU_HAX, &psAxS->m_vNormal);
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fCYA_N, &psCY0->m.m.vU_HAX);
    V(C_TgVEC)                          vK1 = V(F_SUB)(&vK0, &psAxS->m_vNormal);

    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_BX_Is_Valid)(psBX0) );

    vCYB0 = V(F_NORM_LEN)(&fLength, &vK1);

    if (F(tgCM_NR0)(fLength))
    {
        vCYB0 = psCY0->m.m.vU_Basis0;
        vCYB1 = psCY0->m.m.vU_Basis1;
    }
    else
    {
        vCYB1 = V(F_CX)(&vCYB0, &psCY0->m.m.vU_HAX);
    };

    /* The contact generator is split into two cases depending on the orientation of the cylinder to the separation axis. */
    /* 1. Cylinder is standing on the resulting plane (ie. the angle to the plane is greater than 45) */
    /* 2. Cylinder is resting/lying on the plane (ie. the angle to the plane is less than 45) */
    /* The reason for the separation is in how the extra contacts (outside of the deepest point of penetration) are created. In the first case the extra points are */
    /* created around the rim of the cylinder's penetrating cap.  The second case has a segment running parallel to the primary axis along the exterior of the cylinder */
    /* originating with the point of largest penetration. */

    if (F(tgPM_ABS)(fCYA_N) <= F(KTgF_SQRT1_2))
    {
        /* The cylinder is considered to be lying down on the surface of the box. */
        V(TgVEC)                            vP0, vP1;

        const TYPE                          fCYB0_N = V(F_DOT)(&vCYB0, &psAxS->m_vNormal);
        const TYPE                          fInvCYB0_N = MKL(1.0) / fCYB0_N;
        TYPE                                fT0 = MKL(0.0), fT1 = MKL(0.0), fDepth;

        /* Construct the termination points of the cylinder axis and project them onto the box face/surface. The box face is considered to be the control in determining */
        /* the contact surface because of the choice of axis. For instance if a segment was taken parallel to the axis, originating at the point of deepest penetration */
        /* it could have a zero contact solution with a thin box.  Instead, the intersecting volume is examined, and the surface on the box is used to create the contact */
        /* solution. */

        const TYPE                          fK0 = (V(F_DOT)(&vP0, &psAxS->m_vNormal) - fPlnD)*fInvCYB0_N;
        const TYPE                          fK1 = (V(F_DOT)(&vP0, &psAxS->m_vNormal) - fPlnD)*fInvCYB0_N;
        V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fK0, &vCYB0);
        V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK1, &vCYB0);
        V(C_TgVEC)                          vK4 = V(F_SUB)(&psCY0->m.m.vOrigin, &psCY0->m_vHAX);
        V(C_TgVEC)                          vK5 = V(F_ADD)(&psCY0->m.m.vOrigin, &psCY0->m_vHAX);
        V(C_TgVEC)                          vK6 = V(F_SUB)(&vP1, &vP0);

        vP0 = V(F_SUB)(&vK4, &vK2);
        vP1 = V(F_SUB)(&vK5, &vK3);

        if (0 == iFlag || V(tgCO_FI_BXF_Clip_Param_LR11)(&fT0, &fT1, psBX0, iFlag, &vP0, &vK6) >= 0)
        {
            {
                /* Point #0 */
                V(C_TgVEC)                          vKC = V(F_MUL_SV)(MKL(1.0) - fT0, &vP0);
                V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT0, &vP1);
                V(C_TgVEC)                          vK8 = V(F_ADD)(&vKC, &vK7);
                V(C_TgVEC)                          vK9 = V(F_SUB)(&vK8, &psCY0->m.m.vOrigin);
                const TYPE                          fK2 = V(F_DOT)(&vK9, &vCYB0);
                V(C_TgVEC)                          vKA = V(F_MUL_SV)(fK2, &vCYB0);
                V(C_TgVEC)                          vKB = V(F_SUB)(&vK8, &vKA);
                const TYPE                          fK3 = V(F_DOT)(&vKB, &psAxS->m_vNormal) - fPlnD;

                fDepth = psCY0->m_fRadius + F(tgPM_FSEL)(fK3, -fK2, fK2);

                if (fDepth > MKL(0.0))
                {
                    V(C_TgVEC)                          vKD = V(F_MUL_SV)(fDepth, &vCYB0);

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

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

                    psContact->m_vS0 = V(F_ADD)(&vK8, &vKD);
                    psContact->m_vN0 = psAxS->m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fCYB0_N*fDepth;

                    ++psPacket->m_niContact;
                };
            };

            {
                /* Point #1 */
                V(C_TgVEC)                          vKC = V(F_MUL_SV)(MKL(1.0) - fT1, &vP0);
                V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT1, &vP1);
                V(C_TgVEC)                          vK8 = V(F_ADD)(&vKC, &vK7);
                V(C_TgVEC)                          vK9 = V(F_SUB)(&vK8, &psCY0->m.m.vOrigin);
                const TYPE                          fK2 = V(F_DOT)(&vK9, &vCYB0);
                V(C_TgVEC)                          vKA = V(F_MUL_SV)(fK2, &vCYB0);
                V(C_TgVEC)                          vKB = V(F_SUB)(&vK8, &vKA);
                const TYPE                          fK3 = V(F_DOT)(&vKB, &psAxS->m_vNormal) - fPlnD;

                fDepth = psCY0->m_fRadius + F(tgPM_FSEL)(fK3, -fK2, fK2);

                if (fDepth > MKL(0.0))
                {
                    V(C_TgVEC)                          vKD = V(F_MUL_SV)(fDepth, &vCYB0);

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

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

                    psContact->m_vS0 = V(F_ADD)(&vK8, &vKD);
                    psContact->m_vN0 = psAxS->m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fCYB0_N*fDepth;

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

        return (niContact == psPacket->m_niContact ? KTgE_NO_INTERSECT : KTgS_OK);
    }
    else
    {
        TYPE                                fT0 = MKL(0.0), fT1 = MKL(0.0), fDepth;

        /* Example: A box is resting, unstable, on the edge of circular table (the centre of gravity when projected through the local force field does not rest on the */
        /* table). The desired reaction is the box slowly tilts and slides off the table surface, the box contact face remaining tangential to the table rim at all times */

        /* In this example the defining element of the contact surface is the box face itself.  The method used to find this surface is to take the non-separating box */
        /* axis, the resulting faces defined by them, and find how they intersect the cylinder's cap surface.  The cylinder cap is assumed to be the interacting surface */
        /* since other separation axis should be more dominant in the other cases. */

        /* Example: A roll of toilet paper is resting on top of one of those perpetually ever-present game crates. */

        /* In this case the non-contact box faces are completely independent of the cylinder volume/space. The defining element of the contact surface is the cylindrical */
        /* cap area.  In this case the previously mentioned method of using the direction of greatest penetration, followed by radially equi-distant rim points provides */
        /* the best solution. */

        V(TgVEC)                            vInsideDirN;
        TgBOOL                              bNewBasisSet;
        TgSINT32                            iIdx0, iIdx1, niCount;

        const TYPE                          fExtent = F(tgPM_FSEL)(fCYA_N, -psCY0->m_fExtent, psCY0->m_fExtent);
        V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fExtent, &psCY0->m.m.vU_HAX);
        V(C_TgVEC)                          vCentreW = V(F_ADD)(&psCY0->m.m.vOrigin, &vK2);
        V(C_TgVEC)                          vK3 = V(F_MUL_SV)(psCY0->m_fRadius, &vCYB0);
        V(C_TgVEC)                          vMaxPoint = V(F_ADD)(&vCentreW, &vK3);
        C_TgSINT32                          niCount_Max = (bIsContained[i0] ? 0 : 1) + (bIsContained[i1] ? 0 : 1);

        vInsideDirN = V(KTgV_ZERO);
        bNewBasisSet = TgFALSE;
        iIdx0 = bIsContained[i0] ? i1 : i0;
        iIdx1 = bIsContained[i0] ? i0 : i1;

        /* First test to see if those box faces not defined by the separation axis form a contact surface with the cylinder. */

        for (niCount = 0; niCount < niCount_Max; ++niCount, tgCM_SWP_S32( &iIdx0, &iIdx1 ))
        {
            TgSINT32                            niFace;

            /*  Find the line of intersection between the near cylinder cap plane and the two box faces defined by one axis. */
            /* The direction of the interface will remain the same for both faces, only the origin will be different. */

            V(C_TgVEC)                          vK4 = V(F_UCX)(&psCY0->m.m.vU_HAX, psBX0->m.m.avAxis + iIdx0);
            const TYPE                          fCYA_IX0 = V(F_DOT)(&psCY0->m.m.vU_HAX, psBX0->m.m.avAxis + iIdx0);
            const TYPE                          fDet = MKL(1.0) - fCYA_IX0*fCYA_IX0;
            const TYPE                          fBXO_IX0 = V(F_DOT)(&psBX0->m.m.vOrigin, psBX0->m.m.avAxis + iIdx0);
            const TYPE                          fPlnD_CYA = V(F_DOT)(&vCentreW, &psCY0->m.m.vU_HAX);
            const TYPE                          fInvDet = MKL(1.0) / fDet;
            const TYPE                          fDN_IX1 = V(F_DOT)(&vK4, psBX0->m.m.avAxis + iIdx1);

            if (F(tgCM_NR0)(fDet))
            {
                continue;
            };

            for (niFace = 0; niFace < 2; ++niFace)
            {
                V(TgVEC)                            vContact;
                TgBOOL                              bAddNormal;

                const TYPE                          fPlnD_IX0 = fBXO_IX0 + (niFace == 0 ? -psBX0->m_vExtent.m_aData[iIdx0] : psBX0->m_vExtent.m_aData[iIdx0]);
                const TYPE                          fL0 = (fPlnD_CYA - fCYA_IX0*fPlnD_IX0)*fInvDet;
                const TYPE                          fL1 = (fPlnD_IX0 - fCYA_IX0*fPlnD_CYA)*fInvDet;
                V(C_TgVEC)                          vK5 = V(F_MUL_SV)(fL0, &psCY0->m.m.vU_HAX);
                V(C_TgVEC)                          vK6 = V(F_MUL_SV)(fL1, psBX0->m.m.avAxis + iIdx0);
                V(C_TgVEC)                          vK7 = V(F_ADD)(&vK5, &vK6);

                bAddNormal = TgFALSE;

                /* F_Clip the resulting interface line to the cylindrical interior. */
                if (TgFAILED( V(tgCO_FI_TB_Clip_Param_LR11)(&fT0, &fT1, psCY0, &vK7, &vK4) ))
                {
                    continue;
                }
                else
                {
                    /* Find the min and max values along the face by examining the projection onto the opposing orthogonal axis. */

                    V(C_TgVEC)                          vK8 = V(F_SUB)(&psBX0->m.m.vOrigin, &vK7);
                    const TYPE                          fDist = V(F_DOT)(&vK8, psBX0->m.m.avAxis + iIdx1);

                    if (!F(tgCM_NR0)(fDN_IX1))
                    {
                        TYPE                                fMin, fMax;

                        if (fDN_IX1 > MKL(0.0))
                        {
                            fMin = (fDist - psBX0->m_vExtent.m_aData[iIdx1]) / fDN_IX1;
                            fMax = (fDist + psBX0->m_vExtent.m_aData[iIdx1]) / fDN_IX1;
                        }
                        else
                        {
                            fMin = (fDist + psBX0->m_vExtent.m_aData[iIdx1]) / fDN_IX1;
                            fMax = (fDist - psBX0->m_vExtent.m_aData[iIdx1]) / fDN_IX1;
                        };

                        fT0 = F(tgPM_FSEL)(fMin - fT0, fMin, F(tgPM_FSEL)(fT0 - fMax, fMax, fT0));
                        fT1 = F(tgPM_FSEL)(fMin - fT1, fMin, F(tgPM_FSEL)(fT1 - fMax, fMax, fT1));
                    };

                    vT0 = V(F_MUL_SV)(fT0, &vK4);
                    vContact = V(F_ADD)(&vK7, &vT0);
                    fDepth = fPlnD - V(F_DOT)(&vContact, &psAxS->m_vNormal);

                    /* Point #0 */

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

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

                        psContact->m_vS0 = vContact;
                        psContact->m_vN0 = psAxS->m_vNormal;
                        psContact->m_fT0 = MKL(0.0);
                        psContact->m_fDepth = fDepth;

                        vInsideDirN = V(F_ADD)(&vInsideDirN, &vContact);
                        bAddNormal = TgTRUE;

                        ++psPacket->m_niContact;
                    };

                    /* Point #1 - If the two clipped points are within tolerance of each other - skip the additional point. */
                    vT0 = V(F_MUL_SV)(fT1, &vK4);
                    vContact = V(F_ADD)(&vK7, &vT0);
                    fDepth = fPlnD - V(F_DOT)(&vContact, &psAxS->m_vNormal);

                    if (fDepth > MKL(0.0) && !F(tgCM_NR0)(fT0 - fT1))
                    {
                        if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                        {
                            return (KTgE_MAX_CONTACTS);
                        };

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

                        psContact->m_vS0 = vContact;
                        psContact->m_vN0 = psAxS->m_vNormal;
                        psContact->m_fT0 = MKL(0.0);
                        psContact->m_fDepth = fDepth;

                        vInsideDirN = V(F_ADD)(&vInsideDirN, &vContact);
                        bAddNormal = TgTRUE;

                        ++psPacket->m_niContact;
                    };

                    if (bAddNormal && !bNewBasisSet)
                    {
                        /*  If the point of maximum penetration lies outside of the box face, then the resulting clipped point will be along the same edge as one of the */
                        /* points already entered. The point's information could be made more useful. By reconstructing the basis so that a point in the interior of the */
                        /* box along the arc formed between the contact points added into the set will increase stability and prevent penetration into the box face. */

                        V(C_TgVEC)                          vK9 = V(F_SUB)(&vMaxPoint, &psBX0->m.m.vOrigin);

                        if (F(tgPM_ABS)(V(F_DOT)(&vK9, psBX0->m.m.avAxis + iIdx0)) > psBX0->m_vExtent.m_aData[iIdx0])
                        {
                            bNewBasisSet |= TgTRUE;
                        };
                    };
                };
            };
        };

        /* Second - attempt to build a contact surface through the cylinder cap. */

        if (bNewBasisSet && !F(tgCM_NR0)(V(F_LSQ)(&vInsideDirN)) && niContact != psPacket->m_niContact)
        {
            V(C_TgVEC)                          vK4 = V(F_DIV_VS)(&vInsideDirN, (TYPE)(psPacket->m_niContact - niContact));
            V(C_TgVEC)                          vK5 = V(F_SUB)(&vK4, &vCentreW);
            const TYPE                          fIDN_CYA = V(F_DOT)(&vK5, &psCY0->m.m.vU_HAX);
            V(C_TgVEC)                          vK6 = V(F_MUL_SV)(fIDN_CYA, &psCY0->m.m.vU_HAX);
            V(C_TgVEC)                          vK7 = V(F_SUB)(&vK5, &vK6);

            vInsideDirN = vK5;

            vCYB0 = V(F_NORM_LEN)(&fLength, &vK7);

            if (F(tgCM_NR0)(fLength))
            {
                TgWARN_CO(TgT("Counter Logical State\n" ));
                TgDIAG(TgFALSE);
                vCYB0 = psCY0->m.m.vU_Basis0;
                vCYB1 = psCY0->m.m.vU_Basis1;
            }
            else
            {
                vCYB1 = V(F_CX)(&vCYB0, &psCY0->m.m.vU_HAX);
            };
        };

        /* Create a contact point triple - equidistant along the cylinder's rim (ie. 120 between each arm ) */
        vT0 = V(F_MUL_VS)(&vCYB0, psCY0->m_fRadius);

        if (0 == iFlag || V(tgCO_FI_BXF_Clip_Param_LR11)(&fT0, &fT1, psBX0, iFlag, &vCentreW, &vT0) >= 0)
        {
            V(C_TgVEC)                          vK4 = V(F_MUL_VS)(&vCYB0, (0 == iFlag ? psCY0->m_fRadius : fT1));
            V(C_TgVEC)                          vPnt = V(F_ADD)(&vCentreW, &vK4);

            fDepth = fPlnD - V(F_DOT)(&vPnt, &psAxS->m_vNormal);

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

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

                psContact->m_vS0 = vPnt;
                psContact->m_vN0 = psAxS->m_vNormal;
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = fDepth;

                ++psPacket->m_niContact;
            };
        };

        if (psPacket->m_niContact - niContact >= 3)
        {
            /* If the contact surface is defined by the box planes - then we can end here.  It is necessary to allow the system to create the point of greatest */
            /* penetration as well since this will often by the point of tangential contact even when the surface is defined by the box. */

            return (KTgS_OK);
        }
        else
        {
            V(C_TgVEC)                          vK4 = V(F_MUL_SV)(F(KTgF_SQRT3), &vCYB1);
            V(C_TgVEC)                          vK5 = V(F_ADD)(&vCYB0, &vK4);
            V(C_TgVEC)                          vK6 = V(F_MUL_SV)(MKL(0.5), &vK5);
            V(C_TgVEC)                          vK7 = V(F_MUL_VS)(&vK6, psCY0->m_fRadius);
            V(C_TgVEC)                          vK8 = V(F_SUB)(&vCYB0, &vK4);
            V(C_TgVEC)                          vK9 = V(F_MUL_SV)(MKL(-0.5), &vK8);
            V(C_TgVEC)                          vKA = V(F_MUL_VS)(&vK9, psCY0->m_fRadius);

            if (0 == iFlag || V(tgCO_FI_BXF_Clip_Param_LR11)(&fT0, &fT1, psBX0, iFlag, &vCentreW, &vK7) >= 0)
            {
                V(C_TgVEC)                          vKB = V(F_MUL_VS)(&vK6, (0 == iFlag ? psCY0->m_fRadius : fT1));
                V(C_TgVEC)                          vPnt = V(F_ADD)(&vCentreW, &vKB);

                fDepth = fPlnD - V(F_DOT)(&vPnt, &psAxS->m_vNormal);

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

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

                    psContact->m_vS0 = vPnt;
                    psContact->m_vN0 = psAxS->m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fDepth;

                    ++psPacket->m_niContact;
                };
            };

            if (0 == iFlag || V(tgCO_FI_BXF_Clip_Param_LR11)(&fT0, &fT1, psBX0, iFlag, &vCentreW, &vKA) >= 0)
            {
                V(C_TgVEC)                          vKB = V(F_MUL_VS)(&vK9, (0 == iFlag ? psCY0->m_fRadius : fT1));
                V(C_TgVEC)                          vPnt = V(F_ADD)(&vCentreW, &vKB);

                fDepth = fPlnD - V(F_DOT)(&vPnt, &psAxS->m_vNormal);

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

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

                    psContact->m_vS0 = vPnt;
                    psContact->m_vN0 = psAxS->m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fDepth;

                    ++psPacket->m_niContact;
                };
            };

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


/* ---- V(tgCO_F_BX_Penetrate_CylAxis_CY) ------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  sAxS: Structure holding the resulting axis separation information necessary to create a contact set.                                                           */
/* Input:  psBX0: Box 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                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_F_BX_Penetrate_CylAxis_CY)( V(PC_STg2_CO_Packet) psPacket, V(CPC_STg2_CO_Axis_Result) psAxS, V(CPC_TgBOX) psBX0, V(CPC_TgTUBE) psCY0 )
{
    C_TgSINT32                          niContact = psPacket->m_niContact;
    V(P_STg2_CO_Contact)                psContact;

    const TYPE                          fBE0_N = psBX0->m_vExtent.m.x*V(F_DOT)(psBX0->m.m.avAxis + 0, &psAxS->m_vNormal);
    const TYPE                          fBE1_N = psBX0->m_vExtent.m.y*V(F_DOT)(psBX0->m.m.avAxis + 1, &psAxS->m_vNormal);
    const TYPE                          fBE2_N = psBX0->m_vExtent.m.z*V(F_DOT)(psBX0->m.m.avAxis + 2, &psAxS->m_vNormal);

    V(C_TgVEC)                          vL0 = V(F_MUL_SV)(psCY0->m_fExtent, &psAxS->m_vNormal);
    V(C_TgVEC)                          vCentreW = V(F_SUB)(&psCY0->m.m.vOrigin, &vL0);

    TgSINT32                            i0, iContained = 0;
    TYPE                                fE0, fE1, fE2, fDepth;

    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_BX_Is_Valid)(psBX0) );

    if (F(tgPM_ABS)(fBE0_N) > F(tgPM_ABS)(fBE1_N))
    {
        if (F(tgPM_ABS)(fBE1_N) > F(tgPM_ABS)(fBE2_N))
        {
            fE0 = F(tgPM_FSEL)(fBE0_N, psBX0->m_vExtent.m.x, -psBX0->m_vExtent.m.x);
            fE1 = F(tgPM_FSEL)(fBE1_N, psBX0->m_vExtent.m.y, -psBX0->m_vExtent.m.y);
            fE2 = F(tgPM_FSEL)(fBE2_N, psBX0->m_vExtent.m.z, -psBX0->m_vExtent.m.z);
        }
        else if (F(tgPM_ABS)(fBE0_N) > F(tgPM_ABS)(fBE2_N))
        {
            fE0 = F(tgPM_FSEL)(fBE0_N, psBX0->m_vExtent.m.x, -psBX0->m_vExtent.m.x);
            fE1 = F(tgPM_FSEL)(fBE2_N, psBX0->m_vExtent.m.z, -psBX0->m_vExtent.m.z);
            fE2 = F(tgPM_FSEL)(fBE1_N, psBX0->m_vExtent.m.y, -psBX0->m_vExtent.m.y);
        }
        else
        {
            fE0 = F(tgPM_FSEL)(fBE2_N, psBX0->m_vExtent.m.z, -psBX0->m_vExtent.m.z);
            fE1 = F(tgPM_FSEL)(fBE0_N, psBX0->m_vExtent.m.x, -psBX0->m_vExtent.m.x);
            fE2 = F(tgPM_FSEL)(fBE1_N, psBX0->m_vExtent.m.y, -psBX0->m_vExtent.m.y);
        };
    }
    else
    {
        if (F(tgPM_ABS)(fBE0_N) > F(tgPM_ABS)(fBE2_N))
        {
            fE0 = F(tgPM_FSEL)(fBE1_N, psBX0->m_vExtent.m.y, -psBX0->m_vExtent.m.y);
            fE1 = F(tgPM_FSEL)(fBE0_N, psBX0->m_vExtent.m.x, -psBX0->m_vExtent.m.x);
            fE2 = F(tgPM_FSEL)(fBE2_N, psBX0->m_vExtent.m.z, -psBX0->m_vExtent.m.z);
        }
        else if (F(tgPM_ABS)(fBE2_N) > F(tgPM_ABS)(fBE1_N))
        {
            fE0 = F(tgPM_FSEL)(fBE2_N, psBX0->m_vExtent.m.z, -psBX0->m_vExtent.m.z);
            fE1 = F(tgPM_FSEL)(fBE1_N, psBX0->m_vExtent.m.y, -psBX0->m_vExtent.m.y);
            fE2 = F(tgPM_FSEL)(fBE0_N, psBX0->m_vExtent.m.x, -psBX0->m_vExtent.m.x);
        }
        else
        {
            fE0 = F(tgPM_FSEL)(fBE1_N, psBX0->m_vExtent.m.y, -psBX0->m_vExtent.m.y);
            fE1 = F(tgPM_FSEL)(fBE2_N, psBX0->m_vExtent.m.z, -psBX0->m_vExtent.m.z);
            fE2 = F(tgPM_FSEL)(fBE0_N, psBX0->m_vExtent.m.x, -psBX0->m_vExtent.m.x);
        };
    };


    for (i0 = 0; i0 < 4; i0 += 3)
    {
        V(C_TgVEC)                          vK0 = V(tgGM_BX_Calc_Point)(psBX0, fE0, i0 == 0 ? fE1 : -fE1, i0 == 0 ? fE2 : -fE2);
        V(C_TgVEC)                          vK1 = V(F_SUB)(&vK0, &vCentreW);
        TgSINT32                            i1;

        /* Check to see if the edge/segment origin is a valid contact point. */

        fDepth = V(F_DOT)(&vK1, &psAxS->m_vNormal);

        if (fDepth >= MKL(0.0) && V(tgGM_CY_Is_Contained)(psCY0, &vK0))
        {
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fDepth, &psAxS->m_vNormal);

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

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

            psContact->m_vS0 = V(F_SUB)(&vK0, &vK2);
            psContact->m_vN0 = psAxS->m_vNormal;
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = fDepth;

            ++psPacket->m_niContact;
            iContained |= (1 << i0);
        };

        /* Build the two edges originating at the above vertex. */

        for (i1 = 1; i1 < 3; ++i1)
        {
            V(TgPLANE)                          sCapPlane;
            TYPE                                fT0, fT1;

            V(C_TgVEC)                          vEnd = V(tgGM_BX_Calc_Point)(psBX0, fE0, i1 == 1 ? fE1 : -fE1, i1 == 1 ? -fE2 : fE2);
            V(C_TgVEC)                          vK2 = V(F_SUB)(&vEnd, &vK0);

            /*  If the edge origin was contained then its possible the entire edge is completely contained.  Since a simple point containment check is fast and easy to */
            /* do - check to see if the other termination point is within the box - if so, then add the contact and continue onto the next edge. */

            if (0 != (iContained & (1 << i0)))
            {
                V(C_TgVEC)                          vK3 = V(F_SUB)(&vEnd, &vCentreW);

                fDepth = V(F_DOT)(&vK3, &psAxS->m_vNormal);

                if (fDepth >= MKL(0.0) && V(tgGM_CY_Is_Contained)(psCY0, &vEnd))
                {
                    V(C_TgVEC)                          vK4 = V(F_MUL_SV)(fDepth, &psAxS->m_vNormal);

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

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

                    psContact->m_vS0 = V(F_SUB)(&vEnd, &vK4);
                    psContact->m_vN0 = psAxS->m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fDepth;

                    ++psPacket->m_niContact;
                    iContained |= (1 << i1);

                    continue;
                };
            };

            /*  F_Clip the resulting edge by the cylinder. This will result with either zero or two points.  Check the contained bitfield to make sure that duplicate */
            /* points (the vertices) are not added into the system. */

            V(tgGM_PN_Set_Normal)(&sCapPlane, &psAxS->m_vNormal);
            V(tgGM_PN_Set_Constant)(&sCapPlane, V(F_DOT)(&psAxS->m_vNormal, &vCentreW));

            if (TgFAILED( V(tgCO_FI_PN_Clip_Param_LR11)(&fT0, &fT1, &sCapPlane, &vK0, &vK2) ))
            {
                /* The edge is entirely on the non-negative side of the cap plane.  No intersection.  Process next edge. */
                continue;
            }
            else
            {
                V(C_TgVEC)                          vK3 = V(F_MUL_SV)(MKL(1.0) - fT0, &vK0);
                V(C_TgVEC)                          vK4 = V(F_MUL_SV)(fT0, &vEnd);
                V(C_TgVEC)                          vK5 = V(F_ADD)(&vK3, &vK4);
                V(C_TgVEC)                          vK6 = V(F_MUL_SV)(fT1 - fT0, &vK2);

                if (F(tgCM_NR0)(fT1 - fT0) || TgFAILED( V(tgCO_FI_TB_Clip_Param_LR11)(&fT0, &fT1, psCY0, &vK5, &vK6) ))
                {
                    /* F_Clip was reduced to a point or lies outside of the infinite cylindrical space.  Process next edge. */
                    continue;
                }
                else
                {
                    /* Point #0 - */
                    V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT0, &vK6);
                    V(C_TgVEC)                          vP0 = V(F_ADD)(&vK5, &vK7);
                    V(C_TgVEC)                          vK8 = V(F_SUB)(&vP0, &vCentreW);
                    V(C_TgVEC)                          vK9 = V(F_MUL_SV)(fT1, &vK6);
                    V(C_TgVEC)                          vP1 = V(F_ADD)(&vK5, &vK9);
                    V(C_TgVEC)                          vKA = V(F_SUB)(&vP1, &vCentreW);

                    fDepth = V(F_DOT)(&vK8, &psAxS->m_vNormal);

                    if (fDepth > MKL(0.0) && !(F(tgCM_NR0)(fT0) && 0 != (iContained & !(1 << i0))))
                    {
                        V(C_TgVEC)                          vKB = V(F_MUL_SV)(fDepth, &psAxS->m_vNormal);

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

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

                        psContact->m_vS0 = V(F_SUB)(&vP0, &vKB);
                        psContact->m_vN0 = psAxS->m_vNormal;
                        psContact->m_fT0 = MKL(0.0);
                        psContact->m_fDepth = fDepth;

                        ++psPacket->m_niContact;
                    };

                    /* Point #1 - */

                    fDepth = V(F_DOT)(&vKA, &psAxS->m_vNormal);

                    if (fDepth > MKL(0.0) && !F(tgCM_NR0)(fT0 - fT1) && !(F(tgCM_NR1)(fT1) && 0 != (iContained & !(1 << i1))))
                    {
                        V(C_TgVEC)                          vKB = V(F_MUL_SV)(fDepth, &psAxS->m_vNormal);

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

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

                        psContact->m_vS0 = V(F_SUB)(&vP1, &vKB);
                        psContact->m_vN0 = psAxS->m_vNormal;
                        psContact->m_fT0 = MKL(0.0);
                        psContact->m_fDepth = fDepth;

                        ++psPacket->m_niContact;
                    };

                    /* Floating point error can produce some interesting artifacts.  In this case, it is possible for the two end points to be the result of the clipping */
                    /* process within a tolerance value.  So though the point check did not add them as contained - they will be added here and flagged. */

                    iContained |= F(tgCM_NR0)(fT0) ? (1 << i0) : 0;
                    iContained |= F(tgCM_NR1)(fT1) ? (1 << i1) : 0;
                };
            };
        };
    };

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


/* ---- V(tgCO_F_BX_Penetrate_CylRad_CY) -------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  sAxS: Structure holding the resulting axis separation information necessary to create a contact set.                                                           */
/* Input:  psBX0: Box 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                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_F_BX_Penetrate_CylRad_CY)( V(PC_STg2_CO_Packet) psPacket, V(CPC_STg2_CO_Axis_Result) psAxS, V(CPC_TgBOX) psBX0, V(CPC_TgTUBE) psCY0 )
{
    C_TgSINT32                          niContact = psPacket->m_niContact;
    V(P_STg2_CO_Contact)                     psContact;
    TYPE                                fLength;

    /* Make sure there is no axial component in the normal direction to be used. */

    const TYPE                          fCY0_N = V(F_DOT)(&psAxS->m_vNormal, &psCY0->m.m.vU_Basis0);
    const TYPE                          fCY1_N = V(F_DOT)(&psAxS->m_vNormal, &psCY0->m.m.vU_Basis1);
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fCY0_N, &psCY0->m.m.vU_Basis0);
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fCY1_N, &psCY0->m.m.vU_Basis1);
    V(C_TgVEC)                          vK3 = V(F_ADD)(&vK0, &vK1);
    V(C_TgVEC)                          vRad = V(F_NORM_LEN)(&fLength, &vK3);

    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_BX_Is_Valid)(psBX0) );

    if (F(tgCM_NR0)(fLength))
    {
        return (KTgE_FAIL);
    }
    else
    {
        V(C_TgVEC)                          vK2 = V(F_MUL_SV)(psCY0->m_fRadius, &vRad);
        V(C_TgVEC)                          vOrg = V(F_ADD)(&psCY0->m_sAX.m_vOrigin, &vK2);
        V(C_TgVEC)                          vK4 = V(F_NEG)(&psAxS->m_vNormal);
        TYPE                                fT0, fT1, fDepth;
        V(TgVEC)                            vPlane_Point, vPnt, vT0, vT1;

        if (V(tgCO_FI_BX_Clip_Param_LR11)(&fT0, &fT1, psBX0, &vOrg, &psCY0->m_sAX.m_vDirN) < 0)
        {
            return (KTgE_NO_INTERSECT);
        };

        vPlane_Point = V(tgGM_BX_Support_Point)(psBX0, &vK4);
        vT1 = V(F_MUL_SV)(fT0, &psCY0->m_sAX.m_vDirN);
        vPnt = V(F_ADD)(&vOrg, &vT1);
        vT0 = V(F_SUB)(&vPnt, &vPlane_Point);
        fDepth = V(F_DOT)(&vT0, &psAxS->m_vNormal);

        if (fDepth > MKL(0.0))
        {
            psContact = psPacket->m_psContact + psPacket->m_niContact;

            psContact->m_vS0 = vPnt;
            psContact->m_vN0 = psAxS->m_vNormal;
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = fDepth;

            ++psPacket->m_niContact;
        };

        vT1 = V(F_MUL_SV)(fT1, &psCY0->m_sAX.m_vDirN);
        vPnt = V(F_ADD)(&vOrg, &vT1);
        vT0 = V(F_SUB)(&vPnt, &vPlane_Point);
        fDepth = V(F_DOT)(&vT0, &psAxS->m_vNormal);

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

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

            psContact->m_vS0 = vPnt;
            psContact->m_vN0 = psAxS->m_vNormal;
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = fDepth;

            ++psPacket->m_niContact;
        };

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