Home

Resume

Blog

Teikitu


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

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

static TgRESULT                             V(tgCO_FI_ST_Penetrate_Parallel_CP)(V(PC_STg2_CO_Packet), V(CPC_TgSTRI), V(CPC_TgTUBE));
static TgRESULT                             V(tgCO_FI_ST_Penetrate_Parallel_NoClip_CP)(V(PC_STg2_CO_Packet), V(CPC_TgSTRI), V(CPC_TgTUBE));
static TgRESULT                             V(tgCO_FI_ST_Penetrate_Sphere_Cap_CP)( V(PC_STg2_CO_Packet), V(CPC_TgTUBE), V(CPC_TgSTRI), const TYPE, V(CPC_TgVEC) );




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

/* ---- V(tgCO_F_ST_Penetrate_CP) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psST0: Space Triangle primitive                                                                                                                                */
/* Input:  psCP0: Capsule primitive - contact points are generated on this primitive                                                                                      */
/* Output: psPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_ST_Penetrate_CP)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgSTRI) psST0, V(CPC_TgTUBE) psCP0)
{
    TgPARAM_CHECK(V(tgGM_TB_Is_Valid)(psCP0) && V(tgGM_ST_Is_Valid)(psST0));

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
    {
        return (KTgE_FAIL);
    }
    else
    {
        /* Primitive Culling - Set of criteria required for the primitive to be considered penetrating the triangle. */

        V(C_TgVEC)                          vK0 = V(F_SUB)(&psCP0->m.m.vOrigin, psST0->m_sCT.m_sET.m_sPT.m_avPoint);
        const TYPE                          fDS_N = V(F_DOT)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &vK0);
        const TYPE                          fEX_N = V(F_DOT)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &psCP0->m_vHAX);
        const TYPE                          fS0_N = fDS_N - fEX_N;
        const TYPE                          fS1_N = fDS_N + fEX_N;
        V(P_STg2_CO_Contact)                psContact;
        TYPE                                fDepth;
        TYPE                                fCT0, fCT1, fCP0;
        TgBOOL                              bResultCreated = TgFALSE;
        TgRESULT                            iResult;
        TYPE                                fDistSq;

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

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

            return (KTgE_NO_INTERSECT);
        };

        /*TgDEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_PASSED_REJECT ); */

        /* Check the projection of the capsule primary axis against the triangle normal. If its near zero (ie the two vectors are near perpendicular), the capsule must */
        /* be near-parallel to the triangle. In this case the method of closest proximity will not work, and the axis segment will instead be clipped to triangle space. */

        if (F(tgCM_NR0)(fEX_N))
        {
            /* Since the capsule is near-parallel to the triangle itself, in terms of the contact surface, it is no longer different than a regular tube.  Execute that */
            /* primitive's parallel case to generate the contact points. */
            if (TgFAILED( iResult = V(tgCO_FI_ST_Penetrate_Parallel_CP)(psPacket, psST0, psCP0) ))
            {
                return (iResult);
            };
        };

        /*TgDEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_CODE2 ); */

        /* Find the minimal distance and points of closest proximity for the capsule's axis and the triangle. */

        fDistSq = V(tgCO_F_ST_ParamSq_SG)(&fCT0, &fCT1, &fCP0, psST0, &psCP0->m_sAX);

        /* Intersection is impossible if the minimal distance is greater than the capsule's radius. */

        if (fDistSq > psCP0->m_fRadiusSq)
        {
            return (KTgE_NO_INTERSECT);
        };

        /* If the closest point on the triangle to the capsule's axis is not on a valid edge, then the generated axis is ignored and the system falls back to creating */
        /* contacts for the two spherical caps. */

        if (!F(tgCM_NR0)(fCP0) && !F(tgCM_NR1)(fCP0) && !V(tgGM_ST_Is_Point_Culled)(psST0, fCT0, fCT1))
        {
            /* Create contacts only for the tube portion of the capsule. */
            /* Create the two points of closest proximity and the corresponding vector difference between the two. */

            V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fCT0, psST0->m_sCT.m_sET.m_avEdge + 0);
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fCT1, psST0->m_sCT.m_sET.m_avEdge + 2);
            V(C_TgVEC)                          vK4 = V(F_MUL_SV)(fCP0, &psCP0->m_sAX.m_vDirN);
            V(C_TgVEC)                          vK5 = V(F_SUB)(&vK1, &vK2);
            V(C_TgVEC)                          vCP0 = V(F_ADD)(&psCP0->m_sAX.m_vOrigin, &vK4);
            V(C_TgVEC)                          vCT1 = V(F_ADD)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &vK5);
            V(C_TgVEC)                          vK3 = V(F_SUB)(&vCP0, psST0->m_sCT.m_sET.m_sPT.m_avPoint);
            const TYPE                          fTest = V(F_DOT)(&vK3, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
            V(C_TgVEC)                          vK6 = V(F_SUB)(&vCP0, &vCT1);
            V(C_TgVEC)                          vK7 = V(F_SUB)(&vCT1, &vCP0);
            V(TgVEC)                            vNormal = fTest > MKL(0.0) ? vK6 : vK7;

            /* Calculate the resultant penetration depth at this point. */

            const TYPE                          fDist = fTest > MKL(0.0) ? F(tgPM_SQRT)(fDistSq) : -F(tgPM_SQRT)(fDistSq);
            TgBOOL                              bUseNormal = fDistSq > F(KTgEPS);

            fDepth = fDist >= psCP0->m_fRadius ? MKL(0.0) : psCP0->m_fRadius - fDist;

            /*  Check to see if the normal of intersection should be replaced by the triangle's normal.  This is done to reduce floating point noise in the system where */
            /* near-normal results are returned.  By forcing it to the triangle's normal, extraneous rotations are minimized. The other possibility is that the capsule's */
            /* segment intersects the triangle itself, thus, requiring the selection of the triangle's normal for the intersection. */

            if (bUseNormal)
            {
                vNormal = V(F_NORM)(&vNormal);

                /* Check to see if the resultant normal is near that of the triangle's.  If they are close then use the triangle's normal to help further reduce */
                /* floating point noise. */
                bUseNormal = F(tgCM_NR0)(V(F_DOT)(&vNormal, &psST0->m_sCT.m_sET.m_sPT.m_vNormal) - MKL(1.0));
            };

            /* Create contact point. */

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

            {
                V(C_TgVEC)                          vK8 = bUseNormal ? vNormal : psST0->m_sCT.m_sET.m_sPT.m_vNormal;
                V(C_TgVEC)                          vK9 = V(F_MUL_SV)(psCP0->m_fRadius, &vK8);

                psContact->m_vS0 = V(F_SUB)(&vCT1, &vK9);
                psContact->m_vN0 = vK8;
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = fDepth;

                ++psPacket->m_niContact;
                bResultCreated = TgTRUE;
            };
        };

        {
            V(C_TgVEC)                          vKA = V(F_SUB)(&psCP0->m.m.vOrigin, &psCP0->m_vHAX);

            iResult = V(tgCO_FI_ST_Penetrate_Sphere_Cap_CP)(psPacket, psCP0, psST0, fS0_N, &vKA);
            switch (iResult)
            {
                case KTgE_MAX_CONTACTS:
                    return (KTgE_MAX_CONTACTS);
                default:
                    bResultCreated |= TgSUCCEEDED( iResult );
            };
        }

        {
            V(C_TgVEC)                          vKB = V(F_ADD)(&psCP0->m.m.vOrigin, &psCP0->m_vHAX);

            iResult = V(tgCO_FI_ST_Penetrate_Sphere_Cap_CP)(psPacket, psCP0, psST0, fS1_N, &vKB);
            switch (iResult)
            {
                case KTgE_MAX_CONTACTS:
                    return (KTgE_MAX_CONTACTS);
                default:
                    bResultCreated |= TgSUCCEEDED( iResult );
            };
        }

        /*TgDEBUG_COLLISION_TRIANGLE( iDBG_TriID, bResultCreated ? etgDEBUG_COLLISION_FINAL : etgDEBUG_COLLISION_CODE4 ); */

        return (bResultCreated ? KTgS_OK : KTgE_NO_INTERSECT);
    };
}


/* ---- V(tgCO_F_CP_Test_Sweep_ST) -------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psST0: Space Triangle primitive                                                                                                                                */
/* Input:  psCP0: Capsule primitive                                                                                                                                       */
/* Input:  vUDT: Normalized direction of displacement for the swept primitive (capsule)                                                                                   */
/* Input:  fDT: Length of displacement for the swept primitive                                                                                                            */
/* Return: True if the two primitives are in contact at anytime during the sweep                                                                                          */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgBOOL V(tgCO_F_CP_Test_Sweep_ST)( V(CPC_TgSTRI) psST0, V(CPC_TgTUBE) psCP0, V(CPC_TgVEC) pvUDT, const TYPE fDT )
{
    const TYPE                          fUDT_CA = V(F_DOT)(pvUDT, &psCP0->m.m.vU_HAX);
    V(C_TgVEC)                          vDT = V(F_MUL_VS)(pvUDT, fDT);
    V(TgPARALLELOGRAM)                  sSweptCapsule;
    TYPE                                fT0, fT1, fT2;
    V(TgVEC)                            vNM;

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

    if (F(tgCM_NR0)(fUDT_CA - MKL(1.0))) /* Delta vector is parallel to the capsule axis. */
    {
        V(C_TgVEC)                          vK0 = V(F_ADD)( &psCP0->m_sAX.m_vDirN, &vDT );

        return (V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fT2, psST0, &psCP0->m_sAX.m_vOrigin, &vK0 ) < psCP0->m_fRadiusSq);
    };

    if (F(tgCM_NR0)(fUDT_CA + MKL(1.0))) /* Delta vector is parallel to the capsule axis. */
    {
        V(C_TgVEC)                          vK1 = V(F_ADD)( &psCP0->m_sAX.m_vOrigin, &vDT );
        V(C_TgVEC)                          vK2 = V(F_SUB)( &psCP0->m_sAX.m_vDirN, &vDT );

        return (V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fT2, psST0, &vK1, &vK2 ) < psCP0->m_fRadiusSq);
    }
    else
    {
        /* Test to see if the swept capsule (represented by the edges of a parallelogram) comes within range of the triangle. */
        V(C_TgVEC)                          vK3 = V(F_ADD)( &psCP0->m_sAX.m_vOrigin, &vDT );
        V(C_TgVEC)                          vK4 = V(F_ADD)( &psCP0->m_sAX.m_vOrigin, &psCP0->m_sAX.m_vDirN );
        if (
            V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fT2, psST0, &psCP0->m_sAX.m_vOrigin, &psCP0->m_sAX.m_vDirN ) < psCP0->m_fRadiusSq ||
            V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fT2, psST0, &psCP0->m_sAX.m_vOrigin, &vDT ) < psCP0->m_fRadiusSq ||
            V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fT2, psST0, &vK3, &psCP0->m_sAX.m_vDirN ) < psCP0->m_fRadiusSq ||
            V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fT2, psST0, &vK4, &vDT ) < psCP0->m_fRadiusSq
        )
        {
            return (TgTRUE);
        };
    };

    /* Test to see if the triangle ever comes within range of the swept capsule (represented by a parallelogram) */

    vNM = V(F_UCX)( &psCP0->m_sAX.m_vDirN, &vDT );
    V(tgGM_PE_Init)( &sSweptCapsule, &psCP0->m_sAX.m_vOrigin, &psCP0->m_sAX.m_vDirN, &vDT, &vNM );

    if (
        V(tgCO_FI_PE_Test_LR11)( &sSweptCapsule, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 0, psST0->m_sCT.m_sET.m_avEdge + 0 ) ||
        V(tgCO_FI_PE_Test_LR11)( &sSweptCapsule, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 0, psST0->m_sCT.m_sET.m_avEdge + 1 ) ||
        V(tgCO_FI_PE_Test_LR11)( &sSweptCapsule, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 1, psST0->m_sCT.m_sET.m_avEdge + 2 )
    )
    {
        return (TgTRUE);
    };

    return (TgFALSE);
}




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

/* ---- V(tgCO_FI_ST_Penetrate_Parallel_CP) ----------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psST0: Space Triangle primitive                                                                                                                                */
/* Input:  psCP0: Capsule primitive - contact points are generated on this primitive                                                                                      */
/* Output: psPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_FI_ST_Penetrate_Parallel_CP)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgSTRI) psST0, V(CPC_TgTUBE) psCP0 )
{
    /* The capsule is, within tolerance, parallel to the triangle plane.  The format of the surface of contact is therefore definitively a longitudinal surface - */
    /* stretching along the capsule.  It is necessary to trap this specific case since the non-parallel case will use a closest point algorithm derivative.  Using that */
    /* system will cause a varying and random selection of one of the capsule extremities - which would then introduce a time varying reaction force resulting in */
    /* an unrealistic rocking motion or could potentially feed existing rotations.  Keep in mind that since the selection process would be at the extremities, an */
    /* unbalanced moment would be added into the simulation which could be very problematic. */

    TYPE                                fT0, fT1;

    if (TgFAILED( V(tgCO_F_ST_Clip_Param_SG)(&fT0, &fT1, psST0, &psCP0->m_sAX) ))
    {
        /* The capsule axis does not exist anywhere in the triangle's normal extruded space.  A specific contact routine will create the contacts between the triangle */
        /* edge and the capsule. */

        return (V(tgCO_FI_ST_Penetrate_Parallel_NoClip_CP)(psPacket, psST0, psCP0));
    }
    else
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT0, &psCP0->m_sAX.m_vDirN);
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fT1, &psCP0->m_sAX.m_vDirN);
        V(C_TgVEC)                          vP0 = V(F_ADD)(&psCP0->m_sAX.m_vOrigin, &vK0);
        V(C_TgVEC)                          vP1 = V(F_ADD)(&psCP0->m_sAX.m_vOrigin, &vK1);

        V(C_TgVEC)                          vK2 = V(F_SUB)(&vP0, psST0->m_sCT.m_sET.m_sPT.m_avPoint);
        V(C_TgVEC)                          vK3 = V(F_SUB)(&vP1, psST0->m_sCT.m_sET.m_sPT.m_avPoint);
        const TYPE                          fDist0 = psCP0->m_fRadius - V(F_DOT)(&vK2, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
        const TYPE                          fDist1 = psCP0->m_fRadius - V(F_DOT)(&vK3, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

        V(P_STg2_CO_Contact)                psContact;

        if (fDist0 >= MKL(0.0))
        {
            if (psPacket->m_niContact >= psPacket->m_niMaxContact)
            {
                return (KTgE_MAX_CONTACTS);
            }
            else
            {
                V(C_TgVEC) vK4 = V(F_MUL_SV)(psCP0->m_fRadius, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

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

                psContact->m_vS0 = V(F_SUB)(&vP0, &vK4);
                psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = fDist0;

                ++psPacket->m_niContact;
            };
        };

        if (fDist1 >= MKL(0.0))
        {
            if (psPacket->m_niContact >= psPacket->m_niMaxContact)
            {
                return (KTgE_MAX_CONTACTS);
            }
            else
            {
                V(C_TgVEC) vK5 = V(F_MUL_SV)(psCP0->m_fRadius, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

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

                psContact->m_vS0 = V(F_SUB)(&vP1, &vK5);
                psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = fDist1;

                ++psPacket->m_niContact;
            };
        };

        return (fDist0 >= MKL(0.0) || fDist1 >= MKL(0.0) ? KTgS_OK : KTgE_NO_INTERSECT);
    }
}


/* ---- V(tgCO_FI_ST_Penetrate_Parallel_NoClip_CP) ---------------------------------------------------------------------------------------------------------------------- */
/*  -- Internal Function --                                                                                                                                               */
/* Input:  psPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psST0: Space Triangle primitive                                                                                                                                */
/* Input:  psCP0: Capsule primitive - contact points are generated on this primitive                                                                                      */
/* Output: psPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/*                                                                                                                                                                        */
/* Used when the capsule is parallel to the triangle plane, and the axis lies outside of the normal extruded triangle space.                                              */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_FI_ST_Penetrate_Parallel_NoClip_CP)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgSTRI) psST0, V(CPC_TgTUBE) psCP0 )
{
    /* It is now known that the capsule axis does not pass through the triangle normal extruded space.  However, contact can still occur because the triangle's edge */
    /* and/or vertices can penetrate the capsule resulting in a non-planar directed normal.  Special care must be taken into account if the resulting penetrating feature */
    /* is parallel to the capsule. */

    /* Method: Find the nearest triangle feature to the capsule.  If the feature is not enabled then its assumed that the resulting adjoined primitives will produce the */
    /* required contacts and the feature can be ignored and the procedure terminated.  No matter the type of feature found the resulting edge or connected edges should */
    /* be tested to see if they are parallel to the capsule axis so that multiple points of contact can be created.  The secondary contact point should also be tested */
    /* for feature reduction. */

    V(C_TgVEC)                          vS0 = V(F_SUB)(&psCP0->m.m.vOrigin, &psCP0->m_vHAX);
    V(C_TgVEC)                          vS1 = V(F_ADD)(&psCP0->m.m.vOrigin, &psCP0->m_vHAX);
    V(C_TgVEC)                          vAX = psCP0->m_sAX.m_vDirN;
    const TYPE                          fAX_AX = V(F_LSQ)(&vAX);

    C_TgSINT32                          niContact = psPacket->m_niContact;
    V(P_STg2_CO_Contact)                psContact;
    TgSINT32                            iEdge;

    for (iEdge = 0; iEdge < 3; ++iEdge)
    {
        /* Test each vertex individually to prevent the creation of duplicate contacts from the edge routines (shared vertices). */
        if (V(tgGM_ST_Test_Point)(psST0, iEdge))
        {
            V(tgCO_F_VT_Penetrate_CP)(psPacket, psST0->m_sCT.m_sET.m_sPT.m_avPoint + iEdge, psCP0);
        };

        if (!V(tgGM_ST_Test_Edge)(psST0, iEdge)) /* Only collide with those edges marked as valid */
        {
            continue;
        }
        else
        {
            /* Contacts are created only for those edges where the capsule exists entirely in the positive half-space and at least minimally exists in the feature space. */

            V(C_TgVEC)                          vEP = psST0->m_sCT.m_sET.m_sPT.m_avPoint[iEdge];
            V(C_TgVEC)                          vEN = psST0->m_avPlane[iEdge].m_vNormal;
            V(C_TgVEC)                          vET = psST0->m_sCT.m_sET.m_avEdge[iEdge];

            V(C_TgVEC)                          vK0 = V(F_SUB)(&vS0, &vEP);
            V(C_TgVEC)                          vK1 = V(F_SUB)(&vS1, &vEP);
            V(C_TgVEC)                          vK2 = V(F_SUB)(&vS0, &vEP);
            V(C_TgVEC)                          vK3 = V(F_SUB)(&vS1, &vEP);

            const TYPE                          fDist0 = V(F_DOT)(&vK0, &vET);
            const TYPE                          fDist1 = V(F_DOT)(&vK1, &vET);
            const TYPE                          fTest0 = V(F_DOT)(&vK2, &vEN);
            const TYPE                          fTest1 = V(F_DOT)(&vK3, &vEN);

            const TYPE                          fET_ET = V(F_LSQ)(&vET);

            if (
                (fDist0 < MKL(0.0) && fDist1 < MKL(0.0)) ||
                (fDist0 > fET_ET   && fDist1 > fET_ET) ||
                (fTest0 < MKL(0.0) && fTest1 < MKL(0.0))
            )
            {
                continue;
            };

            /* The capsule is known to be on the positive side of the edge normal half-space and is captured passing through it. */

            if (F(tgCM_NR0)(V(F_DOT)(&psCP0->m.m.vU_HAX, &vEN)))
            {
                /* The capsule axis is parallel to the edge */

                V(C_TgVEC)                          vDS = V(F_SUB)(&vEP, &vS0);

                /*                                  Projection Values */
                const TYPE                          fAX_ET = V(F_DOT)(&vAX, &vET);
                const TYPE                          fDS_AX = V(F_DOT)(&vDS, &vAX);
                const TYPE                          fDS_ET = -V(F_DOT)(&vDS, &vET);

                const TYPE                          fDE_AX = fDS_AX + fAX_ET; /* vDE = vEP+vET, fDE_ET = (vEP+vET - vS0)•vAX */
                const TYPE                          fDF_ET = fAX_ET - fDS_ET; /* vDF = vS0+vAX, fDF_ET = (vS0+vAX - vEP)•vET */

                if (
                    (fAX_ET >= MKL(0.0) && (fDE_AX < MKL(0.0) || fDS_AX > MKL(1.0))) ||
                    (fAX_ET <= MKL(0.0) && (fDS_AX < MKL(0.0) || fDE_AX > MKL(1.0)))
                )
                {
                    continue;
                }
                else
                {
                    const TYPE                          fTA = fDS_AX / fAX_AX;
                    const TYPE                          fTC = fDE_AX / fAX_AX;

                    /* Point 0 of segment 0 if contained in segment 1, otherwise if segments are mutually directed point 0 of segment 1, else point 1 of segment 1. */
                    const TYPE                          fK0 = F(tgPM_FSEL)(fET_ET - fDS_ET, MKL(1.0), MKL(-1.0));
                    const TYPE                          fF0 = F(tgPM_FSEL)(fDS_ET, fK0, MKL(-1.0));
                    const TYPE                          fT0 = F(tgPM_FSEL)(fF0, MKL(0.0), F(tgPM_FSEL)(fAX_ET, fTA, fTC));
                    const TYPE                          fK1 = F(tgPM_FSEL)(fAX_ET, MKL(0.0), MKL(1.0));
                    const TYPE                          fT1 = F(tgPM_FSEL)(fF0, (fDS_ET / fET_ET), fK1);

                    /* Point 1 of segment 0 if contained in segment 1, otherwise if segments are mutually directed point 1 of segment 1, else point 0 of segment 1. */
                    const TYPE                          fK2 = F(tgPM_FSEL)(fET_ET - fDF_ET, MKL(1.0), MKL(-1.0));
                    const TYPE                          fF1 = F(tgPM_FSEL)(fDF_ET, fK2, MKL(-1.0));
                    const TYPE                          fT2 = F(tgPM_FSEL)(fF1, MKL(1.0), F(tgPM_FSEL)(fAX_ET, fTC, fTA));
                    const TYPE                          fK3 = F(tgPM_FSEL)(fAX_ET, MKL(1.0), MKL(0.0));
                    const TYPE                          fT3 = F(tgPM_FSEL)(fF1, (fDF_ET / fET_ET), fK3);

                    V(C_TgVEC)                          vK4 = V(F_MUL_SV)(fT0, &vAX);
                    V(C_TgVEC)                          vK5 = V(F_MUL_SV)(fT1, &vET);
                    V(C_TgVEC)                          vK6 = V(F_MUL_SV)(fT2, &vAX);
                    V(C_TgVEC)                          vK7 = V(F_MUL_SV)(fT3, &vET);
                    V(C_TgVEC)                          vP0 = V(F_ADD)(&vS0, &vK4);
                    V(C_TgVEC)                          vP1 = V(F_ADD)(&vEP, &vK5);
                    V(C_TgVEC)                          vK8 = V(F_SUB)(&vP0, &vP1);
                    V(C_TgVEC)                          vP2 = V(F_ADD)(&vS0, &vK6);
                    V(C_TgVEC)                          vP3 = V(F_ADD)(&vEP, &vK7);
                    V(C_TgVEC)                          vK9 = V(F_SUB)(&vP2, &vP3);

                    V(TgVEC)                            vNM;
                    TYPE                                fTM;

                    TgERROR( fT0 >= MKL(0.0) && fT0 <= MKL(1.0) );
                    TgERROR( fT1 >= MKL(0.0) && fT1 <= MKL(1.0) );
                    TgERROR( fT2 >= MKL(0.0) && fT2 <= MKL(1.0) );
                    TgERROR( fT3 >= MKL(0.0) && fT3 <= MKL(1.0) );

                    vNM = V(F_NORM_LEN)(&fTM, &vK8);

                    if (fTM < psCP0->m_fRadius && !F(tgCM_NR0)(fT1) && !F(tgCM_NR1)(fT1))
                    {
                        if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                        {
                            return (KTgE_MAX_CONTACTS);
                        }
                        else
                        {
                            V(C_TgVEC)                          vKA = V(F_MUL_SV)(psCP0->m_fRadius, &vNM);

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

                            psContact->m_vS0 = V(F_SUB)(&vP0, &vKA);
                            psContact->m_vN0 = vNM;
                            psContact->m_fT0 = MKL(0.0);
                            psContact->m_fDepth = psCP0->m_fRadius - fTM;

                            ++psPacket->m_niContact;
                        };
                    };

                    vNM = V(F_NORM_LEN)(&fTM, &vK9);

                    if (fTM < psCP0->m_fRadius && !F(tgCM_NR0)(fT3) && !F(tgCM_NR1)(fT3))
                    {
                        if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                        {
                            return (KTgE_MAX_CONTACTS);
                        }
                        else
                        {
                            V(C_TgVEC)                          vKA = V(F_MUL_SV)(psCP0->m_fRadius, &vNM);

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

                            psContact->m_vS0 = V(F_SUB)(&vP2, &vKA);
                            psContact->m_vN0 = vNM;
                            psContact->m_fT0 = MKL(0.0);
                            psContact->m_fDepth = psCP0->m_fRadius - fTM;

                            ++psPacket->m_niContact;
                        };
                    };
                };
            }
            else
            {
                V(C_TgVEC)                          vK4 = V(F_SUB)(&vS1, &vS0);
                TYPE                                fT0, fT1;
                const TYPE                          fDistSq = V(tgCO_F_LR11_ParamSq_LR11)(&fT0, &fT1, &vS0, &vK4, &vEP, &vET);

                if (fDistSq < psCP0->m_fRadiusSq && !F(tgCM_NR0)(fT1) && !F(tgCM_NR1)(fT1))
                {
                    /* The closest point on the triangle is not a vertex - and the edge is valid. */

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

                        V(C_TgVEC)                          vKA = V(F_MUL_SV)(MKL(1.0) - fT1, &vS0);
                        V(C_TgVEC)                          vKE = V(F_MUL_SV)(fT1, &vS1);
                        V(C_TgVEC)                          vP0 = V(F_ADD)(&vKA, &vKE);
                        V(C_TgVEC)                          vKB = V(F_SUB)(&vP0, &vEP);
                        V(C_TgVEC)                          vKC = V(F_MUL_SV)(fT1, &vET);
                        V(C_TgVEC)                          vKD = V(F_ADD)(&vKB, &vKC);
                        V(C_TgVEC)                          vNM = V(F_NORM_LEN)(&fDist, &vKD);
                        V(C_TgVEC)                          vK5 = V(F_MUL_SV)(psCP0->m_fRadius, &vNM);

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

                        psContact->m_vS0 = V(F_SUB)(&vP0, &vK5);
                        psContact->m_vN0 = vNM;
                        psContact->m_fT0 = MKL(0.0);
                        psContact->m_fDepth = psCP0->m_fRadius - fDist;

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

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


/* ---- V(tgCO_FI_ST_Penetrate_Sphere_Cap_CP) --------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psCP0: Capsule primitive - contact points are generated on this primitive                                                                                      */
/* Input:  psST0: Space Triangle primitive                                                                                                                                */
/* Input:  fDist: The minimal distance between the capsule axis (segment) and the triangle                                                                                */
/* Input:  vP0: Point of closest proximity on the capsule axis between it and the triangle                                                                                */
/* Output: psPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_FI_ST_Penetrate_Sphere_Cap_CP)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgTUBE) psCP0, V(CPC_TgSTRI) psST0, const TYPE fDist, V(CPC_TgVEC) pvP0 )
{
    V(P_STg2_CO_Contact)                     psContact;

    /* Check the start cap (origin - axis) for contact generation. */

    if (fDist >= MKL(0.0))
    {
        if (fDist >= psCP0->m_fRadius)
        {
            return (KTgE_NO_INTERSECT);
        }
        else
        {
            V(STg2_CO_Packet)                   sCap_Packet;
            V(STg2_CO_Contact)                  sCap_Contact;
            V(TgSPHERE)                         sCap;
            TgRESULT                            iResult;
            V(TgVEC)                            vK0, vK1;

            /* Capsule cap is penetrating the triangle plane with the origin above the plane. Execute the sphere penetration code to generate the contact point for the */
            /* capsule cap.  However, since the caps are hemi-spherical in reality, it is necessary to examine the resultant contact point and make sure that the point */
            /* was not created on the illegal space of the sphere.  This is done by culling points out based on the their contact normal. */

            /* Create contact points for the two end caps. */
            sCap_Packet.m_psContact = &sCap_Contact;
            sCap_Packet.m_fSweepTol = MKL(0.0);
            sCap_Packet.m_niContact = 0;
            sCap_Packet.m_niMaxContact = 1;

            V(tgGM_SP_Init)(&sCap, pvP0, psCP0->m_fRadius);

            iResult = V(tgCO_F_ST_Penetrate_SP)(&sCap_Packet, psST0, &sCap);

            vK0 = V(F_SUB)(&sCap_Contact.m_vS0, pvP0);
            vK1 = V(F_SUB)(pvP0, &psCP0->m.m.vOrigin);

            if (TgFAILED( iResult ) || V(F_DOT)(&vK0, &vK1) <= MKL(0.0))
            {
                return (KTgE_NO_INTERSECT);
            };

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

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

            psContact->m_vS0 = sCap_Contact.m_vS0;
            psContact->m_vN0 = sCap_Contact.m_vN0;
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = sCap_Contact.m_fDepth;

            ++psPacket->m_niContact;
            return (KTgS_OK);
        };
    }
    else
    {
        /* Capsule cap lies below the plane.  Thus, the sphere penetration code would have ignored it, so for the capsule it is necessary to deal with this separately. */
        /* In this case only generate a contact point if the cap position is contained inside of the normal extruded space of the triangle.  For this case, the contact */
        /* will only have a normal equal to the triangle's normal. */

        if (!V(tgGM_ST_Is_Contained)(psST0, pvP0))
        {
            return (KTgE_NO_INTERSECT);
        };

        if (psPacket->m_niContact >= psPacket->m_niMaxContact)
        {
            return (KTgE_MAX_CONTACTS);
        }
        else
        {
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(psCP0->m_fRadius, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

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

            psContact->m_vS0 = V(F_SUB)(pvP0, &vK2);
            psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = psCP0->m_fRadius - fDist;

            ++psPacket->m_niContact;
            return (KTgS_OK);
        };
    };
}