Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Triangle-Triangle.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;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_F_ST_Internal_CoP_ST)( V(PC_STg2_CO_Packet), V(PC_STg2_CO_Clip_List), const TYPE, V(CPC_TgVEC) );




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

/* ---- V(tgCO_F_ST_ParamSq_ST) ----------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psST0, psST1: Space triangle primitive                                                                                                                         */
/* Output: _fST00, _fST01: Parametric parameters to generate point of minimal distance on triangle #1                                                                     */
/* Output: _fST10, _fST11: Parametric parameters to generate point of minimal distance on triangle #2                                                                     */
/* Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.                                                             */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE V(tgCO_F_ST_ParamSq_ST)(TYPE *pfST00, TYPE *pfST01, TYPE *pfST10, TYPE *pfST11, V(CPC_TgSTRI) psST0, V(CPC_TgSTRI) psST1)
{
    TYPE                                fDistSq, fET00, fET01, fET10, fET11, fTest, fT0, fT1, fG1;

    /* == Test Triangle 0 Edges against Triangle 1 == */

    fDistSq = V(tgCO_FI_ST_ParamSq_LR11)( &fET00, &fET01, &fET10, psST0, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 0, psST1->m_sCT.m_sET.m_avEdge + 0 );
    fET11 = MKL(0.0);

    fTest = V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fG1, psST0, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 1, psST1->m_sCT.m_sET.m_avEdge + 1 );
    if (fTest < fDistSq)
    {
        fDistSq = fTest;
        fET00 = fT0;
        fET01 = fT1;
        fET10 = MKL(1.0) - fG1;
        fET11 = fG1;
    };

    fTest = V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fG1, psST0, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 2, psST1->m_sCT.m_sET.m_avEdge + 2 );
    if (fTest < fDistSq)
    {
        fDistSq = fTest;
        fET00 = fT0;
        fET01 = fT1;
        fET10 = MKL(0.0);
        fET11 = MKL(1.0) - fG1;
    };

    /* == Test Triangle 1 Edges against Triangle 0 == */

    fTest = V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fG1, psST1, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 0, psST0->m_sCT.m_sET.m_avEdge + 0 );
    if (fTest < fDistSq)
    {
        fDistSq = fTest;
        fET00 = fG1;
        fET01 = MKL(0.0);
        fET10 = fT0;
        fET11 = fT1;
    };

    fTest = V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fG1, psST1, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 1, psST0->m_sCT.m_sET.m_avEdge + 1 );
    if (fTest < fDistSq)
    {
        fDistSq = fTest;
        fET00 = MKL(1.0) - fG1;
        fET01 = fG1;
        fET10 = fT0;
        fET11 = fT1;
    };

    fTest = V(tgCO_FI_ST_ParamSq_LR11)( &fT0, &fT1, &fG1, psST1, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 2, psST0->m_sCT.m_sET.m_avEdge + 2 );
    if (fTest < fDistSq)
    {
        fDistSq = fTest;
        fET00 = MKL(0.0);
        fET01 = MKL(1.0) - fG1;
        fET10 = fT0;
        fET11 = fT1;
    };

    /* == Return Values == */

    *pfST00 = fET00;
    *pfST01 = fET01;
    *pfST10 = fET10;
    *pfST11 = fET11;

    return (fDistSq);
}


/* ---- V(tgCO_F_ST_Test_ST) -------------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psST0, psST1: Space triangle primitive                                                                                                                         */
/* Return: True if the two triangles are in contact                                                                                                                       */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgBOOL V(tgCO_F_ST_Test_ST)( V(CPC_TgSTRI) psST0, V(CPC_TgSTRI) psST1 )
{
    TYPE                                fMin, fMax;

    const TYPE fTriD0 = V(F_DOT)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, psST0->m_sCT.m_sET.m_sPT.m_avPoint);
    const TYPE fTriD1 = V(F_DOT)(&psST1->m_sCT.m_sET.m_sPT.m_vNormal, psST1->m_sCT.m_sET.m_sPT.m_avPoint);
    V(C_TgVEC) vK0 = V(F_CX)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);

    /* == Triangle Normal == */

    V(tgGM_ST_Project)(&fMin, &fMax, psST1, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
    if (fMin > fTriD0 || fMax < fTriD0)
    {
        return (TgFALSE);
    };

    V(tgGM_ST_Project)(&fMin, &fMax, psST0, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);
    if (fMin > fTriD1 || fMax < fTriD1)
    {
        return (TgFALSE);
    };

    /* == Cross-Product Normal == */

    if (!F(tgCM_NR0)(V(F_LSQ)(&vK0)))
    {
        V(TgVEC)                            vNormal;
        TgSINT32                            iE0;

        for (iE0 = 0; iE0 < 9; ++iE0)
        {
            vNormal = V(F_CX)(psST0->m_sCT.m_sET.m_avEdge + (iE0 & 3), psST1->m_sCT.m_sET.m_avEdge + (iE0 >> 3));

            if (V(tgCO_F_PT_Is_Seperating_Axis_PT)(&vNormal, &psST0->m_sCT.m_sET.m_sPT, &psST1->m_sCT.m_sET.m_sPT))
            {
                return (TgFALSE);
            };
        };
    }
    else
    {
        /* Normal extruded space of triangle 0 */

        if (
            V(tgCO_F_PN_Sign_Dist_VT)(psST0->m_avPlane + 0, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 0) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST0->m_avPlane + 0, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 1) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST0->m_avPlane + 0, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 2) < MKL(0.0)
        )
        {
            return (TgFALSE);
        };

        if (
            V(tgCO_F_PN_Sign_Dist_VT)(psST0->m_avPlane + 1, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 0) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST0->m_avPlane + 1, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 1) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST0->m_avPlane + 1, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 2) < MKL(0.0)
        )
        {
            return (TgFALSE);
        };

        if (
            V(tgCO_F_PN_Sign_Dist_VT)(psST0->m_avPlane + 2, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 0) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST0->m_avPlane + 2, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 1) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST0->m_avPlane + 2, psST1->m_sCT.m_sET.m_sPT.m_avPoint + 2) < MKL(0.0)
        )
        {
            return (TgFALSE);
        };

        /* Normal extruded space of triangle 1 */

        if (
            V(tgCO_F_PN_Sign_Dist_VT)(psST1->m_avPlane + 0, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 0) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST1->m_avPlane + 0, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 1) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST1->m_avPlane + 0, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 2) < MKL(0.0)
        )
        {
            return (TgFALSE);
        };

        if (
            V(tgCO_F_PN_Sign_Dist_VT)(psST1->m_avPlane + 1, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 0) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST1->m_avPlane + 1, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 1) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST1->m_avPlane + 1, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 2) < MKL(0.0)
        )
        {
            return (TgFALSE);
        };

        if (
            V(tgCO_F_PN_Sign_Dist_VT)(psST1->m_avPlane + 2, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 0) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST1->m_avPlane + 2, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 1) < MKL(0.0) &&
            V(tgCO_F_PN_Sign_Dist_VT)(psST1->m_avPlane + 2, psST0->m_sCT.m_sET.m_sPT.m_avPoint + 2) < MKL(0.0)
        )
        {
            return (TgFALSE);
        };
    };

    return (TgTRUE);
}


/* ---- V(tgCO_F_ST_Test_Sweep_ST) -------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psST0, psST1: Space triangle primitive - Triangle #2 (psST1) is being swept.                                                                                   */
/* Input:  psDT: A structure holding the swept primitive displacement for the entire duration of the test period.                                                         */
/* Return: True if the primitives contact anywhere in 4D space.                                                                                                           */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgBOOL V(tgCO_F_ST_Test_Sweep_ST)( V(CPC_TgSTRI) psST0, V(CPC_TgSTRI) psST1, V(CPC_TgDELTA) psDT )
{
    V(STg2_CO_Axis_Test)                sAxTest; /* Parameter data output variable */

    const TYPE fS0_NM = V(F_DOT)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

    sAxTest.m_fLimitT = MKL(1.0); /* The maximum t-value by which a contact must happen resulting in non-contact. */
    sAxTest.m_fMinT = -F(KTgMAX);
    sAxTest.m_fMaxT = F(KTgMAX);

    /* -- Axis: Triangle 0 Normal -- */

    sAxTest.m_fMin0 = fS0_NM;
    sAxTest.m_fMax0 = fS0_NM;
    V(tgGM_ST_Project)(&sAxTest.m_fMin1, &sAxTest.m_fMax1, psST1, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
    sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

    if (0 >= V(tgCO_F_Test_Seperating_Axis)(&sAxTest))
    {
        return (TgFALSE);
    }
    else
    {
        /* Check to see if the triangle's are parallel. */
        TgSINT32                            iE0;

        V(C_TgVEC) vK0 = V(F_CX)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);

        if (V(F_LSQ)(&vK0) > F(KTgEPS))
        {
            /* -- Axis: Triangle 1 Normal -- */

            const TYPE fS1_NM = V(F_DOT)(psST1->m_sCT.m_sET.m_sPT.m_avPoint, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);

            V(tgGM_ST_Project)(&sAxTest.m_fMin0, &sAxTest.m_fMax0, psST0, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);
            sAxTest.m_fMin1 = fS1_NM;
            sAxTest.m_fMax1 = fS1_NM;
            sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);

            if (0 >= V(tgCO_F_Test_Seperating_Axis)(&sAxTest))
            {
                return (TgFALSE);
            };

            /* == Triangle Cross-Product == */

            for (iE0 = 0; iE0 < 9; ++iE0)
            {
                V(C_TgVEC) vK1 = V(F_CX)(psST0->m_sCT.m_sET.m_avEdge + (iE0 & 3), psST1->m_sCT.m_sET.m_avEdge + (iE0 >> 3));

                V(tgGM_ST_Project)(&sAxTest.m_fMin0, &sAxTest.m_fMax0, psST0, &vK1);
                V(tgGM_ST_Project)(&sAxTest.m_fMin1, &sAxTest.m_fMax1, psST1, &vK1);
                sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &vK1);

                if (0 >= V(tgCO_F_Test_Seperating_Axis)(&sAxTest))
                {
                    return (TgFALSE);
                };
            };
        }
        else
        {
            /* == Triangle 0 Space == */

            for (iE0 = 0; iE0 < 3; ++iE0)
            {
                V(C_TgVEC) vK1 = V(F_CX)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, psST0->m_sCT.m_sET.m_avEdge + iE0);

                V(tgGM_ST_Project)(&sAxTest.m_fMin0, &sAxTest.m_fMax0, psST0, &vK1);
                V(tgGM_ST_Project)(&sAxTest.m_fMin1, &sAxTest.m_fMax1, psST1, &vK1);
                sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &vK1);

                if (0 >= V(tgCO_F_Test_Seperating_Axis)(&sAxTest))
                {
                    return (TgFALSE);
                };
            };

            /* == Triangle 1 Space == */

            for (iE0 = 0; iE0 < 3; ++iE0)
            {
                V(C_TgVEC) vK1 = V(F_CX)(&psST1->m_sCT.m_sET.m_sPT.m_vNormal, psST1->m_sCT.m_sET.m_avEdge + iE0);

                V(tgGM_ST_Project)(&sAxTest.m_fMin0, &sAxTest.m_fMax0, psST0, &vK1);
                V(tgGM_ST_Project)(&sAxTest.m_fMin1, &sAxTest.m_fMax1, psST1, &vK1);
                sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &vK1);

                if (0 >= V(tgCO_F_Test_Seperating_Axis)(&sAxTest))
                {
                    return (TgFALSE);
                };
            };
        };

        return (TgTRUE);
    }
}


/* ---- V(tgCO_F_ST_Intersect_ST) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psST0, psST1: Space triangle primitive                                                                                                                         */
/* Output: tgPacket: Points of intersection between the two primitives are added to it                                                                                    */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_ST_Intersect_ST)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgSTRI) psST0, V(CPC_TgSTRI) psST1)
{
    if (!V(tgCO_F_ST_Test_ST)(psST0, psST1))
    {
        return (KTgE_NO_INTERSECT);
    }
    else
    {
        TYPE                                fK0;
        V(PU_STg2_CO_Clip_List)             psCL;

        const TYPE fN0_N1 = V(F_DOT)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);
        V(C_TgVEC) vK0 = V(F_CX)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);
        V(TgVEC) vN0xN1 = V(F_NORM_LEN)(&fK0, &vK0);
        const TYPE fDet = MKL(1.0) - fN0_N1*fN0_N1;

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

        if (F(tgCM_NR0)(fK0) || F(tgCM_NR0)(fDet)) /* Co-Planar Triangles */
        {
            V(tgCO_F_ST_Clip_PT)(psCL, psST0, &psST1->m_sCT.m_sET.m_sPT);
            return (V(tgCO_F_ST_Internal_CoP_ST)(psPacket, psCL, MKL(0.0), &psST0->m_sCT.m_sET.m_sPT.m_vNormal));
        }
        else
        {
            V(P_STg2_CO_Contact)                psContact;

            const TYPE fD0 = V(F_DOT)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
            const TYPE fD1 = V(F_DOT)(psST1->m_sCT.m_sET.m_sPT.m_avPoint, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);
            C_TgSINT32 niPoint = tgCM_MIN_S32( psCL->m_niPoint, psPacket->m_niMaxContact - psPacket->m_niContact );

            const TYPE fT0 = (fD0 - fD1*fN0_N1) / fDet;
            const TYPE fT1 = (fD1 - fD0*fN0_N1) / fDet;
            V(C_TgVEC) vK2 = V(F_MUL_SV)(fT0, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
            V(C_TgVEC) vK1 = V(F_MUL_SV)(fT1, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);
            V(C_TgVEC) vS0 = V(F_ADD)(&vK2, &vK1);

            V(tgCO_FI_ST_Clip_LR11)(psCL, psST0, &vS0, &vN0xN1);
            V(tgCO_F_Clip_ST)(psCL, psST1);

            switch (niPoint)
            {
                case 2:
                    psContact = psPacket->m_psContact + psPacket->m_niContact;
                    psContact->m_vS0 = psCL->m_avPoint[1];
                    psContact->m_vN0 = vN0xN1;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = MKL(0.0);

                    ++psPacket->m_niContact;

                case 1:
                    psContact = psPacket->m_psContact + psPacket->m_niContact;
                    psContact->m_vS0 = psCL->m_avPoint[0];
                    psContact->m_vN0 = vN0xN1;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = MKL(0.0);

                    ++psPacket->m_niContact;

                case 0:
                    return (niPoint < psCL->m_niPoint ? KTgE_MAX_CONTACTS : KTgS_OK);

                default:
                    TgS_NO_DEFAULT(break);
            };
            TgERROR(TgFALSE);
            return (KTgE_FAIL);
        };
    };
}


/* ---- V(tgCO_F_ST_Sweep_ST) ------------------------------------------------------------------------------------------------------------------------------------------- */
/*                                                                                                                                                                        */
/*  Axis Separation technique used to determine if two triangles are intersecting.                                                                                        */
/*                                                                                                                                                                        */
/* Input:  tgPacket:  Contact generation parameters                                                                                                                       */
/* Input:  fPM: Current normalized time of first contact for the contact query set.                                                                                       */
/* Input:  psST0, psST1: Space triangle primitive - Triangle #2 (psST1) is being swept.                                                                                   */
/* Input:  psDT: A structure holding the swept primitive displacement for the entire duration of the test period.                                                         */
/* Output: tgPacket: Contact points are added or replace the current set depending on the time comparison and given parameters                                            */
/* Output: fPM: New normalized time of first contact                                                                                                                      */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_ST_Sweep_ST)( V(PC_STg2_CO_Packet) psPacket, TYPE *pfPM, V(CPC_TgSTRI) psST0, V(CPC_TgSTRI) psST1, V(CPC_TgDELTA) psDT )
{
    V(STg2_CO_Axis_Project)             sP0, sP1;
    V(STg2_CO_Axis_Info)                sNFO;
    TgRESULT                            iResult;

    TgPARAM_CHECK( V(tgGM_ST_Is_Valid)(psST0) && V(tgGM_ST_Is_Valid)(psST1) );

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

    /*TgDEBUG_COLLISION_TRIANGLE_CREATEID(iDBG_TriID, psST1, ETgFEBUG_COLLISION_ENTERFCN); */

    sNFO.m_enSide = ETgCO_AXIS_CONTACT_DIRECTION__UNKNOWN;
    sNFO.m_fMinT = F(KTgMAX);

    iResult = V(tgCO_F_ST_Axis_Seperation_ST)(&sNFO, *pfPM + psPacket->m_fSweepTol, psST0, psST1, psDT);

    if (TgFAILED( iResult ))
    {
        TgERROR( KTgE_NO_INTERSECT == iResult );
        return (iResult);
    };

    /* == Contact Generation == */

    TgERROR( (sNFO.m_enSide != ETgCO_AXIS_CONTACT_DIRECTION__UNKNOWN) );

    if (sNFO.m_enSide == ETgCO_AXIS_CONTACT_DIRECTION__PENETRATED || sNFO.m_fMinT < MKL(0.0))
    {
        psPacket->m_niContact = 0;
        *pfPM = MKL(0.0);
        return (KTgE_PREPENETRATION);
    };

    /* Make sure this contact has not occurred more than tolerance later than the current sweep time. */

    if (sNFO.m_fMinT > *pfPM + psPacket->m_fSweepTol)
    {
        return (KTgE_NO_INTERSECT);
    };

    if (sNFO.m_fMinT < *pfPM - psPacket->m_fSweepTol)
    {
        psPacket->m_niContact = 0;
        *pfPM = sNFO.m_fMinT;
    };

    V(tgCO_F_PT_Axis_ProjInfo)( &sP0, &sNFO.m_vNormal, &psST0->m_sCT.m_sET.m_sPT );
    V(tgCO_F_PT_Axis_ProjInfo)( &sP1, &sNFO.m_vNormal, &psST1->m_sCT.m_sET.m_sPT );

    {
        V(P_STg2_CO_Contact)                psContact;
        V(PU_STg2_CO_Clip_List)             psCL;
        TgSINT32                            niPoint;

        /* Contact generation - interpolate the position of the triangle's at the given time, and compute the contact points. */

        C_TgBOOL                            bNegSide = sNFO.m_enSide == ETgCO_AXIS_CONTACT_DIRECTION__NEGATIVE;
        V(C_TgVEC)                          vOffset = V(F_MUL_SV)(sNFO.m_fMinT, &psDT->m_vDT);

        /*  The two triangles are situated so that they are just touching each other.  Examine how the triangle vertices were projected onto the axis to determine the */
        /* geometric relation of the contact - and then create the contact points. */

        C_TgSINT32                          niVertD0 = bNegSide ? sP0.m_iMinID : sP0.m_iMaxID;
        V(PC_TgVEC)                         avVert0 = bNegSide ? sP0.m_avMinVert : sP0.m_avMaxVert;
        C_TgSINT32                          niVertD1 = bNegSide ? sP1.m_iMaxID : sP1.m_iMinID;
        V(PC_TgVEC)                         avVert1 = bNegSide ? sP1.m_avMaxVert : sP1.m_avMinVert;
        V(C_TgVEC)                          vK0 = V(F_NEG)(&sNFO.m_vNormal);
        V(C_TgVEC)                          vNormal = bNegSide ? vK0 : sNFO.m_vNormal;

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

        if (1 == niVertD0) /* Triangle 0 vertex touching a feature of Triangle 1 */
        {
            psContact = psPacket->m_psContact + psPacket->m_niContact;
            psContact->m_vS0 = avVert0[0];
            psContact->m_vN0 = vNormal;
            psContact->m_fT0 = sNFO.m_fMinT;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

            return (KTgS_OK);
        };

        if (1 == niVertD1) /* Triangle 1 vertex touching a feature of Triangle 0 */
        {
            psContact = psPacket->m_psContact + psPacket->m_niContact;
            psContact->m_vS0 = V(F_ADD)(&avVert1[0], &vOffset);
            psContact->m_vN0 = vNormal;
            psContact->m_fT0 = sNFO.m_fMinT;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

            return (KTgS_OK);
        };

        if (2 == niVertD0 && 2 == niVertD1) /* Edge-Edge Contact */
        {
            V(TgVEC)                            vS0, vS1;
            TgSINT32                            niCode;

            V(C_TgVEC)                          vK1 = V(F_SUB)(avVert0 + 1, avVert0);
            V(C_TgVEC)                          vK2 = V(F_ADD)(&vOffset, avVert1);
            V(C_TgVEC)                          vK3 = V(F_SUB)(avVert1 + 1, avVert1);

            niCode = V(tgCO_F_LN_Internal_Intersect_LN)(&vS0, &vS1, avVert0, &vK1, &vK2, &vK3);

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

            switch (niPoint)
            {
                case 2:
                    psContact = psPacket->m_psContact + psPacket->m_niContact;
                    psContact->m_vS0 = vS1;
                    psContact->m_vN0 = vNormal;
                    psContact->m_fT0 = sNFO.m_fMinT;
                    psContact->m_fDepth = MKL(0.0);

                    ++psPacket->m_niContact;

                case 1:
                    psContact = psPacket->m_psContact + psPacket->m_niContact;
                    psContact->m_vS0 = vS0;
                    psContact->m_vN0 = vNormal;
                    psContact->m_fT0 = sNFO.m_fMinT;
                    psContact->m_fDepth = MKL(0.0);

                    ++psPacket->m_niContact;

                    return (niCode != niPoint ? KTgE_MAX_CONTACTS : KTgS_OK);

                default:
                    return (KTgE_NO_INTERSECT);
            };
        };

        if (2 == niVertD0 && 3 == niVertD1) /* Edge-Face Contact */
        {
            V(C_TgVEC)                          vK1 = V(F_SUB)(avVert0, &vOffset);
            V(C_TgVEC)                          vK2 = V(F_SUB)(avVert0 + 1, avVert0);

            V(tgCO_FI_ST_Clip_LR11)(psCL, psST1, &vK1, &vK2);
            niPoint = tgCM_MIN_S32( psCL->m_niPoint, psPacket->m_niMaxContact - psPacket->m_niContact );

            switch (niPoint)
            {
                case 2:
                    psContact = psPacket->m_psContact + psPacket->m_niContact;
                    psContact->m_vS0 = V(F_ADD)(&psCL->m_avPoint[1], &vOffset);
                    psContact->m_vN0 = vNormal;
                    psContact->m_fT0 = sNFO.m_fMinT;
                    psContact->m_fDepth = MKL(0.0);

                    ++psPacket->m_niContact;

                case 1:
                    psContact = psPacket->m_psContact + psPacket->m_niContact;
                    psContact->m_vS0 = V(F_ADD)(&psCL->m_avPoint[0], &vOffset);
                    psContact->m_vN0 = vNormal;
                    psContact->m_fT0 = sNFO.m_fMinT;
                    psContact->m_fDepth = MKL(0.0);

                    ++psPacket->m_niContact;

                    return (psCL->m_niPoint != niPoint ? KTgE_MAX_CONTACTS : KTgS_OK);

                default:
                    return (KTgE_NO_INTERSECT);
            };
        };

        if (3 == niVertD0 && 2 == niVertD1) /* Face-Edge Contact */
        {
            V(C_TgVEC)                          vK1 = V(F_ADD)(&vOffset, avVert1);
            V(C_TgVEC)                          vK2 = V(F_SUB)(avVert1 + 1, avVert1);

            V(tgCO_FI_ST_Clip_LR11)(psCL, psST0, &vK1, &vK2);
            niPoint = tgCM_MIN_S32( psCL->m_niPoint, psPacket->m_niMaxContact - psPacket->m_niContact );

            switch (niPoint)
            {
                case 2:
                    psContact = psPacket->m_psContact + psPacket->m_niContact;
                    psContact->m_vS0 = psCL->m_avPoint[1];
                    psContact->m_vN0 = vNormal;
                    psContact->m_fT0 = sNFO.m_fMinT;
                    psContact->m_fDepth = MKL(0.0);

                    ++psPacket->m_niContact;

                case 1:
                    psContact = psPacket->m_psContact + psPacket->m_niContact;
                    psContact->m_vS0 = psCL->m_avPoint[0];
                    psContact->m_vN0 = vNormal;
                    psContact->m_fT0 = sNFO.m_fMinT;
                    psContact->m_fDepth = MKL(0.0);

                    ++psPacket->m_niContact;

                    return (psCL->m_niPoint != niPoint ? KTgE_MAX_CONTACTS : KTgS_OK);

                default:
                    return (KTgE_NO_INTERSECT);
            };
        };

        TgERROR( 3 == niVertD0 && 3 == niVertD1 );

        V(tgCO_F_ST_Clip_PT)(psCL, psST0, &psST1->m_sCT.m_sET.m_sPT);

        return (V(tgCO_F_ST_Internal_CoP_ST)(psPacket, psCL, sNFO.m_fMinT, &vNormal));
    }
}




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

/* ---- V(tgCO_F_ST_Internal_CoP_ST) ------------------------------------------------------------------------------------------------------------------------------------ */
/* Transfer the point generated by a clipping operation to the contact list.                                                                                              */
/* Input:  tgCL: F_Clip list of points                                                                                                                                    */
/* Input:  fT0: Time of contact                                                                                                                                           */
/* Input:  vNormal: The normal of contact                                                                                                                                 */
/* Output: tgPacket: Contact points are added to it                                                                                                                       */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_F_ST_Internal_CoP_ST)( V(PC_STg2_CO_Packet) psPacket, V(PC_STg2_CO_Clip_List) psCL, const TYPE fT0, V(CPC_TgVEC) pvNormal )
{
    C_TgSINT32                          niPoint = tgCM_MIN_S32( psCL->m_niPoint, psPacket->m_niMaxContact - psPacket->m_niContact );
    V(P_STg2_CO_Contact)                psContact;

    switch (niPoint)
    {
        case 6:
            psContact = psPacket->m_psContact + psPacket->m_niContact;
            psContact->m_vS0 = psCL->m_avPoint[5];
            psContact->m_vN0 = *pvNormal;
            psContact->m_fT0 = fT0;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

        case 5:
            psContact = psPacket->m_psContact + psPacket->m_niContact;
            psContact->m_vS0 = psCL->m_avPoint[4];
            psContact->m_vN0 = *pvNormal;
            psContact->m_fT0 = fT0;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

        case 4:
            psContact = psPacket->m_psContact + psPacket->m_niContact;
            psContact->m_vS0 = psCL->m_avPoint[3];
            psContact->m_vN0 = *pvNormal;
            psContact->m_fT0 = fT0;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

        case 3:
            psContact = psPacket->m_psContact + psPacket->m_niContact;

            psContact->m_vS0 = psCL->m_avPoint[2];
            psContact->m_vN0 = *pvNormal;
            psContact->m_fT0 = fT0;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

        case 2:
            psContact = psPacket->m_psContact + psPacket->m_niContact;
            psContact->m_vS0 = psCL->m_avPoint[1];
            psContact->m_vN0 = *pvNormal;
            psContact->m_fT0 = fT0;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

        case 1:
            psContact = psPacket->m_psContact + psPacket->m_niContact;
            psContact->m_vS0 = psCL->m_avPoint[0];
            psContact->m_vN0 = *pvNormal;
            psContact->m_fT0 = fT0;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

        case 0:
            return (niPoint < psCL->m_niPoint ? KTgE_MAX_CONTACTS : KTgS_OK);

        default:
            TgS_NO_DEFAULT(return (KTgE_FAIL));
    };
}


/* ---- V(tgCO_F_ST_Axis_Seperation_ST) --------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psST0, psST1: Space triangle primitive - Triangle #2 (psST1) is being swept.                                                                                   */
/* Input:  fLimitT: Current normalized time of contact for the contact query set.                                                                                         */
/* Input:  psDT: A structure holding the swept primitive displacement for the entire duration of the test period.                                                         */
/* Output: sNFO: Structure holds the resulting axis separation information necessary to create a contact set.                                                             */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_ST_Axis_Seperation_ST)( V(PC_STg2_CO_Axis_Info) psNFO, const TYPE fLimitT, V(CPC_TgSTRI) psST0, V(CPC_TgSTRI) psST1, V(CPC_TgDELTA) psDT )
{
    V(STg2_CO_Axis_Test)                sAxTest; /* Parameter data output variable */
    TgSINT32                            iResult;
    TgSINT32                            iE0;

    const TYPE fS0_NM = V(F_DOT)(psST0->m_sCT.m_sET.m_sPT.m_avPoint, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
    V(C_TgVEC) vK0 = V(F_CX)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);

    sAxTest.m_fLimitT = fLimitT; /* The maximum t-value by which a contact must happen resulting in non-contact. */
    sAxTest.m_fMinT = -F(KTgMAX);
    sAxTest.m_fMaxT = F(KTgMAX);

    /* -- Axis: Triangle 0 Normal -- */

    sAxTest.m_fMin0 = fS0_NM;
    sAxTest.m_fMax0 = fS0_NM;
    V(tgGM_ST_Project)(&sAxTest.m_fMin1, &sAxTest.m_fMax1, psST1, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
    sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

    iResult = V(tgCO_F_Test_Seperating_Axis)(&sAxTest);
    switch (iResult)
    {
        case  1: /* Update has occurred */
            psNFO->m_enSide = (sAxTest.m_fMax1 < sAxTest.m_fMin0) ?
                ETgCO_AXIS_CONTACT_DIRECTION__NEGATIVE : ETgCO_AXIS_CONTACT_DIRECTION__POSITIVE;
            psNFO->m_vNormal = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
            psNFO->m_fMinT = sAxTest.m_fMinT;
        case  0: /* Contact occurred in valid interval, but earlier contact already recorded. */
            break;
        case -1: /* No contact occurred during valid interval, thus primitives are separated on this axis. */
            return (KTgE_NO_INTERSECT);
    };

    /* Check to see if the triangle's are parallel. */

    if (V(F_LSQ)(&vK0) > F(KTgEPS))
    {
        /* -- Axis: Triangle 1 Normal -- */

        const TYPE fS1_NM = V(F_DOT)(psST1->m_sCT.m_sET.m_sPT.m_avPoint, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);

        V(tgGM_ST_Project)(&sAxTest.m_fMin0, &sAxTest.m_fMax0, psST0, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);
        sAxTest.m_fMin1 = fS1_NM;
        sAxTest.m_fMax1 = fS1_NM;
        sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &psST1->m_sCT.m_sET.m_sPT.m_vNormal);

        iResult = V(tgCO_F_Test_Seperating_Axis)(&sAxTest);
        switch (iResult)
        {
            case  1: /* Update has occurred */
                psNFO->m_enSide = (sAxTest.m_fMax1 < sAxTest.m_fMin0) ?
                    ETgCO_AXIS_CONTACT_DIRECTION__NEGATIVE : ETgCO_AXIS_CONTACT_DIRECTION__POSITIVE;
                psNFO->m_vNormal = psST1->m_sCT.m_sET.m_sPT.m_vNormal;
                psNFO->m_fMinT = sAxTest.m_fMinT;
            case  0: /* Contact occurred in valid interval, but earlier contact already recorded. */
                break;
            case -1: /* No contact occurred during valid interval, thus primitives are separated on this axis. */
                return (KTgE_NO_INTERSECT);
        };

        /* == Triangle Cross-Product == */

        for (iE0 = 0; iE0 < 9; ++iE0)
        {
            V(C_TgVEC) vK1 = V(F_CX)(psST0->m_sCT.m_sET.m_avEdge + (iE0 & 3), psST1->m_sCT.m_sET.m_avEdge + (iE0 >> 3));

            V(tgGM_ST_Project)(&sAxTest.m_fMin0, &sAxTest.m_fMax0, psST0, &vK1);
            V(tgGM_ST_Project)(&sAxTest.m_fMin1, &sAxTest.m_fMax1, psST1, &vK1);
            sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &vK1);

            iResult = V(tgCO_F_Test_Seperating_Axis)(&sAxTest);
            switch (iResult)
            {
                case  1: /* Update has occurred */
                    psNFO->m_enSide = (sAxTest.m_fMax1 < sAxTest.m_fMin0) ?
                        ETgCO_AXIS_CONTACT_DIRECTION__NEGATIVE : ETgCO_AXIS_CONTACT_DIRECTION__POSITIVE;
                    psNFO->m_vNormal = vK1;
                    psNFO->m_fMinT = sAxTest.m_fMinT;
                case  0: /* Contact occurred in valid interval, but earlier contact already recorded. */
                    break;
                case -1: /* No contact occurred during valid interval, thus primitives are separated on this axis. */
                    return (KTgE_NO_INTERSECT);
            };
        };
    }
    else
    {
        /* == Triangle 0 Space == */

        for (iE0 = 0; iE0 < 3; ++iE0)
        {
            V(C_TgVEC) vK1 = V(F_CX)(&psST0->m_sCT.m_sET.m_sPT.m_vNormal, psST0->m_sCT.m_sET.m_avEdge + iE0);

            V(tgGM_ST_Project)(&sAxTest.m_fMin0, &sAxTest.m_fMax0, psST0, &vK1);
            V(tgGM_ST_Project)(&sAxTest.m_fMin1, &sAxTest.m_fMax1, psST1, &vK1);
            sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &vK1);

            iResult = V(tgCO_F_Test_Seperating_Axis)(&sAxTest);
            switch (iResult)
            {
                case  1: /* Update has occurred */
                    psNFO->m_enSide = (sAxTest.m_fMax1 < sAxTest.m_fMin0) ?
                        ETgCO_AXIS_CONTACT_DIRECTION__NEGATIVE : ETgCO_AXIS_CONTACT_DIRECTION__POSITIVE;
                    psNFO->m_vNormal = vK1;
                    psNFO->m_fMinT = sAxTest.m_fMinT;
                case  0: /* Contact occurred in valid interval, but earlier contact already recorded. */
                    break;
                case -1: /* No contact occurred during valid interval, thus primitives are separated on this axis. */
                    return (KTgE_NO_INTERSECT);
            };
        };

        /* == Triangle 1 Space == */

        for (iE0 = 0; iE0 < 3; ++iE0)
        {
            V(C_TgVEC) vK1 = V(F_CX)(&psST1->m_sCT.m_sET.m_sPT.m_vNormal, psST1->m_sCT.m_sET.m_avEdge + iE0);

            V(tgGM_ST_Project)(&sAxTest.m_fMin0, &sAxTest.m_fMax0, psST0, &vK1);
            V(tgGM_ST_Project)(&sAxTest.m_fMin1, &sAxTest.m_fMax1, psST1, &vK1);
            sAxTest.m_fSpeed = V(F_DOT)(&psDT->m_vDT, &vK1);

            iResult = V(tgCO_F_Test_Seperating_Axis)(&sAxTest);
            switch (iResult)
            {
                case  1: /* Update has occurred */
                    psNFO->m_enSide = (sAxTest.m_fMax1 < sAxTest.m_fMin0) ?
                        ETgCO_AXIS_CONTACT_DIRECTION__NEGATIVE : ETgCO_AXIS_CONTACT_DIRECTION__POSITIVE;
                    psNFO->m_vNormal = vK1;
                    psNFO->m_fMinT = sAxTest.m_fMinT;
                case  0: /* Contact occurred in valid interval, but earlier contact already recorded. */
                    break;
                case -1: /* No contact occurred during valid interval, thus primitives are separated on this axis. */
                    return (KTgE_NO_INTERSECT);
            };
        };
    };

    return (KTgS_OK);
}