// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Triangle-Triangle.cpp
//  Author:    Andrew Aye (EMail: andrew.aye@gmail.com, Web: http://www.andrewaye.com) 
//  Version:   3.11
//
// ------------------------------------------------------------------------------------------------------------------------------ //
//
//  Copyright: © 2002-2008, 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: 
//    Redistributions of source code must retain this copyright notice, this list of conditions and the following disclaimers. 
//    Redistributions 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. 
//
//  Neither the names of the copyright owner nor the names of its contributors may 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".
//
// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //




namespace TGS { // START TGS ///////////////////////////////////////////////////////////////////////////////////////////////////////
namespace COL { // START COL ///////////////////////////////////////////////////////////////////////////////////////////////////////

// ============================================================================================================================== //

// ---- F_ClosestSq ------------------------------------------------------------------------------------------------------------- //
// Input:  tgST0, tgST1: Space triangle primitive
// Output: _tyST00, _tyST01: Parametric parameters to generate point of minimal distance on triangle #1
// Output: _tyST10, _tyST11: 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.
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TYPE F_ClosestSq( TYPE *ptyST00, TYPE *ptyST01, TYPE *ptyST10, TYPE *ptyST11, CR_(STRI,DIM) tgST0, CR_(STRI,DIM) tgST1 )
{
    TYPE                                tyDistSq, tyET00, tyET01, tyET10, tyET11, tyTest, tyT0, tyT1, tyG1;

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

    tyDistSq = TTgCSQ_STLN<TYPE,DIM,1,1>::DO( &tyET00,&tyET01, &tyET10, tgST0, tgST1.Query_Point0(), tgST1.Query_Edge0() );
    tyET11 = TYPE(0.0);

    tyTest = TTgCSQ_STLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, &tyG1, tgST0, tgST1.Query_Point1(), tgST1.Query_Edge1() );
    if (tyTest < tyDistSq)
    {
        tyDistSq = tyTest;
        tyET00 = tyT0;
        tyET01 = tyT1;
        tyET10 = TYPE(1.0) - tyG1;
        tyET11 = tyG1;
    };
    
    tyTest = TTgCSQ_STLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, &tyG1, tgST0, tgST1.Query_Point2(), tgST1.Query_Edge2() );
    if (tyTest < tyDistSq)
    {
        tyDistSq = tyTest;
        tyET00 = tyT0;
        tyET01 = tyT1;
        tyET10 = TYPE(0.0);
        tyET11 = TYPE(1.0) - tyG1;
    };

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

    tyTest = TTgCSQ_STLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, &tyG1, tgST1, tgST0.Query_Point0(), tgST0.Query_Edge0() );
    if (tyTest < tyDistSq)
    {
        tyDistSq = tyTest;
        tyET00 = tyG1;
        tyET01 = TYPE(0.0);
        tyET10 = tyT0;
        tyET11 = tyT1;
    };

    tyTest = TTgCSQ_STLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, &tyG1, tgST1, tgST0.Query_Point1(), tgST0.Query_Edge1() );
    if (tyTest < tyDistSq)
    {
        tyDistSq = tyTest;
        tyET00 = TYPE(1.0) - tyG1;
        tyET01 = tyG1;
        tyET10 = tyT0;
        tyET11 = tyT1;
    };
    
    tyTest = TTgCSQ_STLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, &tyG1, tgST1, tgST0.Query_Point2(), tgST0.Query_Edge2() );
    if (tyTest < tyDistSq)
    {
        tyDistSq = tyTest;
        tyET00 = TYPE(0.0);
        tyET01 = TYPE(1.0) - tyG1;
        tyET10 = tyT0;
        tyET11 = tyT1;
    };
    
    // == Return Values ========================================

    *ptyST00 = tyET00;
    *ptyST01 = tyET01;
    *ptyST10 = tyET10;
    *ptyST11 = tyET11;

    return (tyDistSq);
};

template TgFLOAT32 F_ClosestSq( P_TgFLOAT32, P_TgFLOAT32, P_TgFLOAT32, P_TgFLOAT32, CR_TgF4STRI, CR_TgF4STRI );


// ============================================================================================================================== //
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //

//                                              Contact Generation Utility Functions  

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //
// ============================================================================================================================== //

// ---- F_Internal_CoP ---------------------------------------------------------------------------------------------------------- //
// Transfer the point generated by a clipping operation to the contact list.
// Input:  tgCL: F_Clip list of points
// Input:  tyT0: Time of contact
// Input:  tvNormal: The normal of contact
// Output: tgPacket: Contact points are added to it
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Internal_CoP( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(CLIP_LIST,DIM) tgData, const TYPE tyT0, M_(VECTOR,DIM) tvNormal )
{
    C_TgINT                             niPoint = MIN( tgData.m_niPoint, ptgPacket->m_niMaxContact - ptgPacket->m_niContact );
    P_(CONTACT,DIM)                     ptgContact;

    switch (niPoint)
    {
        case 6:
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
            ptgContact->m_tvPos = tgData.m_ptvPoint[5];
            ptgContact->m_tvNormal = tvNormal;
            ptgContact->m_tyT0 = tyT0;
            ptgContact->m_tyDepth = TYPE(0.0);

            ++ptgPacket->m_niContact;

        case 5:
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
            ptgContact->m_tvPos = tgData.m_ptvPoint[4];
            ptgContact->m_tvNormal = tvNormal;
            ptgContact->m_tyT0 = tyT0;
            ptgContact->m_tyDepth = TYPE(0.0);

            ++ptgPacket->m_niContact;

        case 4:
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
            ptgContact->m_tvPos = tgData.m_ptvPoint[3];
            ptgContact->m_tvNormal = tvNormal;
            ptgContact->m_tyT0 = tyT0;
            ptgContact->m_tyDepth = TYPE(0.0);

            ++ptgPacket->m_niContact;

        case 3:
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

            ptgContact->m_tvPos = tgData.m_ptvPoint[2];
            ptgContact->m_tvNormal = tvNormal;
            ptgContact->m_tyT0 = tyT0;
            ptgContact->m_tyDepth = TYPE(0.0);

            ++ptgPacket->m_niContact;

        case 2:
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
            ptgContact->m_tvPos = tgData.m_ptvPoint[1];
            ptgContact->m_tvNormal = tvNormal;
            ptgContact->m_tyT0 = tyT0;
            ptgContact->m_tyDepth = TYPE(0.0);

            ++ptgPacket->m_niContact;

        case 1:
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
            ptgContact->m_tvPos = tgData.m_ptvPoint[0];
            ptgContact->m_tvNormal = tvNormal;
            ptgContact->m_tyT0 = tyT0;
            ptgContact->m_tyDepth = TYPE(0.0);

            ++ptgPacket->m_niContact;

        case 0:
            return (niPoint < tgData.m_niPoint ? TgS_MAXCONTACTS : TgS_OK);

        default:
            break;
    };
    TgASSERT(TgFALSE);
    return (TgE_FAIL);
};

template TgRESULT F_Internal_CoP( PC_TgF4CONTACT_PACKET, CR_TgF4CLIP_LIST, C_TgFLOAT32, M_TgF4VECTOR );


// ============================================================================================================================== //
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //

//                                            Static Triangle - Triangle Intersection

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //
// ============================================================================================================================== //

// ---- F_Contact_Test ---------------------------------------------------------------------------------------------------------- //
// Input:  tgST0, tgST1: Space triangle primitive
// Return: True if the two triangles are in contact
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgBOOL F_Contact_Test( CR_(STRI,DIM) tgST0, CR_(STRI,DIM) tgST1 )
{
    const TYPE                          tyTriD0 = MATH::F_DOT(tgST0.Query_Normal(),tgST0.Query_Origin());
    const TYPE                          tyTriD1 = MATH::F_DOT(tgST1.Query_Normal(),tgST1.Query_Origin());
    TYPE                                tyMin,tyMax;

    // == Triangle Normal ====================================

    tgST1.Project( &tyMin,&tyMax, tgST0.Query_Normal() );
    if (tyMin > tyTriD0 || tyMax < tyTriD0)
    {
        return (TgFALSE);
    };

    tgST0.Project( &tyMin,&tyMax, tgST1.Query_Normal() );
    if (tyMin > tyTriD1 || tyMax < tyTriD1)
    {
        return (TgFALSE);
    };

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

    if (!Near_Zero( MATH::F_LSQ( MATH::F_CX( tgST0.Query_Normal(), tgST1.Query_Normal() ) ) )) 
    {
        T_(VECTOR,DIM)                      tvNormal;

        for (TgINT iE0 = 0; iE0 < 9; ++iE0)
        {
            tvNormal = MATH::F_CX( tgST0.Query_Edge( (iE0 & 3) ), tgST1.Query_Edge( (iE0 >> 3) ) );

            if (F_Is_Seperating_Axis( tvNormal, tgST0.Query_PT(), tgST1.Query_PT() ))
            {
                return (TgFALSE);
            };
        };
    }
    else
    {
        // Normal extruded space of triangle 0

        if (
            F_Sign_Dist( tgST0.Query_EdgePlane0(), tgST1.Query_Point0() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST0.Query_EdgePlane0(), tgST1.Query_Point1() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST0.Query_EdgePlane0(), tgST1.Query_Point2() ) < TYPE(0.0)
        ) {
            return (TgFALSE);
        };

        if (
            F_Sign_Dist( tgST0.Query_EdgePlane1(), tgST1.Query_Point0() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST0.Query_EdgePlane1(), tgST1.Query_Point1() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST0.Query_EdgePlane1(), tgST1.Query_Point2() ) < TYPE(0.0)
        ) {
            return (TgFALSE);
        };

        if (
            F_Sign_Dist( tgST0.Query_EdgePlane2(), tgST1.Query_Point0() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST0.Query_EdgePlane2(), tgST1.Query_Point1() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST0.Query_EdgePlane2(), tgST1.Query_Point2() ) < TYPE(0.0)
        ) {
            return (TgFALSE);
        };

        // Normal extruded space of triangle 1

        if (
            F_Sign_Dist( tgST1.Query_EdgePlane0(), tgST0.Query_Point0() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST1.Query_EdgePlane0(), tgST0.Query_Point1() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST1.Query_EdgePlane0(), tgST0.Query_Point2() ) < TYPE(0.0)
        ) {
            return (TgFALSE);
        };

        if (
            F_Sign_Dist( tgST1.Query_EdgePlane1(), tgST0.Query_Point0() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST1.Query_EdgePlane1(), tgST0.Query_Point1() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST1.Query_EdgePlane1(), tgST0.Query_Point2() ) < TYPE(0.0)
        ) {
            return (TgFALSE);
        };

        if (
            F_Sign_Dist( tgST1.Query_EdgePlane2(), tgST0.Query_Point0() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST1.Query_EdgePlane2(), tgST0.Query_Point1() ) < TYPE(0.0) &&
            F_Sign_Dist( tgST1.Query_EdgePlane2(), tgST0.Query_Point2() ) < TYPE(0.0)
        ) {
            return (TgFALSE);
        };
    };

    return (TgTRUE);
};
        
template TgBOOL F_Contact_Test( CR_TgF4STRI, CR_TgF4STRI );


// ---- F_Contact_Intersect ----------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgST0, tgST1: Space triangle primitive
// Output: tgPacket: Points of intersection between the two primitives are added to it
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Intersect( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(STRI,DIM) tgST0, CR_(STRI,DIM) tgST1 )
{
    if (!F_Contact_Test( tgST0, tgST1 ))
    {
        return (TgE_NOINTERSECT);
    };

    TYPE                                tyK0;
    T_(VECTOR,DIM)                      tvN0xN1 = MATH::F_NORM( &tyK0, MATH::F_CX( tgST0.Query_Normal(), tgST1.Query_Normal() ) );
    const TYPE                          tyN0_N1 = MATH::F_DOT(tgST0.Query_Normal(),tgST1.Query_Normal());
    const TYPE                          tyDet = TYPE(1.0) - tyN0_N1*tyN0_N1;

    if (Near_Zero( tyK0 ) || Near_Zero( tyDet ))
    {
        // Co-Planar Triangles

        TTgCLIP_LIST<TYPE,DIM>              tgCL;

        F_Clip( &tgCL, tgST0,tgST1.Query_PT() );
        return (F_Internal_CoP( ptgPacket, tgCL, TYPE(0.0), tgST0.Query_Normal() ));
    };

    const TYPE                          tyD0 = MATH::F_DOT(tgST0.Query_Origin(),tgST0.Query_Normal());
    const TYPE                          tyD1 = MATH::F_DOT(tgST1.Query_Origin(),tgST1.Query_Normal());

    const TYPE                          tyT0 = (tyD0 - tyD1*tyN0_N1) / tyDet;
    const TYPE                          tyT1 = (tyD1 - tyD0*tyN0_N1) / tyDet;

    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyT0, tgST0.Query_Normal() );
    C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyT1, tgST1.Query_Normal() );
    C_(VECTOR,DIM)                      tvS0 = MATH::F_ADD( tvK0, tvK1 );

    TTgCLIP_LIST<TYPE,DIM>              tgCL;

    TTgCLP_STLN<TYPE,DIM,1,1>::DO( &tgCL, tgST0, tvS0, tvN0xN1 );
    F_Clip( &tgCL, tgST1 );

    C_TgINT                             niPoint = MIN( tgCL.m_niPoint, ptgPacket->m_niMaxContact - ptgPacket->m_niContact );
    P_(CONTACT,DIM)                     ptgContact;

    switch (niPoint)
    {
        case 2:
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
            ptgContact->m_tvPos = tgCL.m_ptvPoint[1];
            ptgContact->m_tvNormal = tvN0xN1;
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = TYPE(0.0);

            ++ptgPacket->m_niContact;

        case 1:
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
            ptgContact->m_tvPos = tgCL.m_ptvPoint[0];
            ptgContact->m_tvNormal = tvN0xN1;
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = TYPE(0.0);

            ++ptgPacket->m_niContact;

        case 0:
            return (niPoint < tgCL.m_niPoint ? TgS_MAXCONTACTS : TgS_OK);

        default:
            break;
    };
    TgASSERT(TgFALSE);
    return (TgE_FAIL);
};

template TgRESULT F_Contact_Intersect( PC_TgF4CONTACT_PACKET, CR_TgF4STRI, CR_TgF4STRI );


// ============================================================================================================================== //
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //

//                                           Dynamic Triangle - Triangle Intersection

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //
// ============================================================================================================================== //

// ============================================================================================================================== //

// ---- F_Contact_Test ---------------------------------------------------------------------------------------------------------- //
// Input:  tgST0, tgST1: Space triangle primitive - Triangle #2 (tgST1) is being swept.
// Input:  tgDT: 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.
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgBOOL F_Contact_Test( CR_(STRI,DIM) tgST0, CR_(STRI,DIM) tgST1, CR_(DELTA,DIM) tgDT )
{
    T_(AXIS_TEST,DIM)                   tgAxTest; //« Parameter data output variable
    //TYPE                                tySkipT = -LIMITS<TYPE>::MAX; //« T-value for axes associated with reduced features.

    tgAxTest.m_tyLimitT = TYPE(1.0); // The maximum t-value by which a contact must happen resulting in non-contact.
    tgAxTest.m_tyMinT = -LIMITS<TYPE>::MAX;
    tgAxTest.m_tyMaxT =  LIMITS<TYPE>::MAX;

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

    const TYPE                          tyS0_NM = MATH::F_DOT(tgST0.Query_Origin(),tgST0.Query_Normal());

    tgAxTest.m_tyMin0 = tyS0_NM;
    tgAxTest.m_tyMax0 = tyS0_NM;
    tgST1.Project( &tgAxTest.m_tyMin1, &tgAxTest.m_tyMax1, tgST0.Query_Normal() );
    tgAxTest.m_tySpeed = MATH::F_DOT(tgDT.m_tvDT,tgST0.Query_Normal());

    if (0 >= F_Test_Seperating_Axis( &tgAxTest ))
    {
        return (TgFALSE);
    };

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

    if (MATH::F_LSQ( MATH::F_CX(tgST0.Query_Normal(),tgST1.Query_Normal()) ) > LIMITS<TYPE>::EPSILON)
    {
        // -- Axis: Triangle 1 Normal ------------------------------

        const TYPE                          tyS1_NM = MATH::F_DOT(tgST1.Query_Origin(),tgST1.Query_Normal());

        tgST0.Project( &tgAxTest.m_tyMin0, &tgAxTest.m_tyMax0, tgST1.Query_Normal() );
        tgAxTest.m_tyMin1 = tyS1_NM;
        tgAxTest.m_tyMax1 = tyS1_NM;
        tgAxTest.m_tySpeed = MATH::F_DOT( tgDT.m_tvDT, tgST1.Query_Normal() );

        if (0 >= F_Test_Seperating_Axis( &tgAxTest ))
        {
            return (TgFALSE);
        };

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

        T_(VECTOR,DIM)                      tvNormal;

        for (TgINT iE0 = 0; iE0 < 9; ++iE0)
        {
            tvNormal = MATH::F_CX( tgST0.Query_Edge( (iE0 & 3) ), tgST1.Query_Edge( (iE0 >> 3) ) );

            tgST0.Project( &tgAxTest.m_tyMin0, &tgAxTest.m_tyMax0, tvNormal );
            tgST1.Project( &tgAxTest.m_tyMin1, &tgAxTest.m_tyMax1, tvNormal );
            tgAxTest.m_tySpeed = MATH::F_DOT( tgDT.m_tvDT, tvNormal );

            if (0 >= F_Test_Seperating_Axis( &tgAxTest ))
            {
                return (TgFALSE);
            };
        };
    }
    else
    {
        T_(VECTOR,DIM)                      tvNormal;

        // == Triangle 0 Space ===================================

        for (TgINT iE0 = 0; iE0 < 3; ++iE0)
        {
            tvNormal = MATH::F_CX( tgST0.Query_Normal(), tgST0.Query_Edge( iE0 ) );

            tgST0.Project( &tgAxTest.m_tyMin0, &tgAxTest.m_tyMax0, tvNormal );
            tgST1.Project( &tgAxTest.m_tyMin1, &tgAxTest.m_tyMax1, tvNormal );
            tgAxTest.m_tySpeed = MATH::F_DOT( tgDT.m_tvDT, tvNormal );

            if (0 >= F_Test_Seperating_Axis( &tgAxTest ))
            {
                return (TgFALSE);
            };
        };

        // == Triangle 1 Space ===================================

        for (TgINT iE0 = 0; iE0 < 3; ++iE0)
        {
            tvNormal = MATH::F_CX( tgST1.Query_Normal(), tgST1.Query_Edge( iE0 ) );

            tgST0.Project( &tgAxTest.m_tyMin0, &tgAxTest.m_tyMax0, tvNormal );
            tgST1.Project( &tgAxTest.m_tyMin1, &tgAxTest.m_tyMax1, tvNormal );
            tgAxTest.m_tySpeed = MATH::F_DOT( tgDT.m_tvDT, tvNormal );

            if (0 >= F_Test_Seperating_Axis( &tgAxTest ))
            {
                return (TgFALSE);
            };
        };
    };

    return (TgTRUE);
}

template TgBOOL F_Contact_Test( CR_TgF4STRI, CR_TgF4STRI, CR_TgF4DELTA );


// ============================================================================================================================== //

// ---- F_Axis_Seperation --------------------------------------------------------------------------------------------------------- //
// Input:  tgST0, tgST1: Space triangle primitive - Triangle #2 (tgST1) is being swept.
// Input:  tyLimitT: Current normalized time of contact for the contact query set.
// Input:  tgDT: A structure holding the swept primitive displacement for the entire duration of the test period.
// Output: tgNFO: Structure holds the resulting axis separation information necessary to create a contact set.
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Axis_Seperation( PC_(AXIS_INFO,DIM) ptgNFO, const TYPE tyLimitT, CR_(STRI,DIM) tgST0, CR_(STRI,DIM) tgST1, CR_(DELTA,DIM) tgDT )
{
    T_(AXIS_TEST,DIM)                   tgAxTest; //« Parameter data output variable
    //TYPE                                tySkipT = -LIMITS<TYPE>::MAX; //« T-value for axes associated with reduced features.
    TgINT                               iResult;

    tgAxTest.m_tyLimitT = tyLimitT; // The maximum t-value by which a contact must happen resulting in non-contact.
    tgAxTest.m_tyMinT = -LIMITS<TYPE>::MAX;
    tgAxTest.m_tyMaxT =  LIMITS<TYPE>::MAX;


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

    const TYPE                          tyS0_NM = MATH::F_DOT( tgST0.Query_Origin(), tgST0.Query_Normal() );

    tgAxTest.m_tyMin0 = tyS0_NM;
    tgAxTest.m_tyMax0 = tyS0_NM;
    tgST1.Project( &tgAxTest.m_tyMin1, &tgAxTest.m_tyMax1, tgST0.Query_Normal() );
    tgAxTest.m_tySpeed = MATH::F_DOT( tgDT.m_tvDT, tgST0.Query_Normal() );

    iResult = F_Test_Seperating_Axis( &tgAxTest );
    switch (iResult) {
        case  1: //« Update has occurred
            ptgNFO->m_eSide = (tgAxTest.m_tyMax1 < tgAxTest.m_tyMin0) ? T_(AXIS_INFO,DIM)::NEGATIVE : T_(AXIS_INFO,DIM)::POSITIVE;
            ptgNFO->m_tvNormal = tgST0.Query_Normal();
            ptgNFO->m_tyMinT = tgAxTest.m_tyMinT;
        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 (TgE_NOINTERSECT);
    };

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

    if (MATH::F_LSQ( MATH::F_CX( tgST0.Query_Normal(), tgST1.Query_Normal() ) ) > LIMITS<TYPE>::EPSILON)
    {
        // -- Axis: Triangle 1 Normal ------------------------------

        const TYPE                          tyS1_NM = MATH::F_DOT(tgST1.Query_Origin(),tgST1.Query_Normal());

        tgST0.Project( &tgAxTest.m_tyMin0, &tgAxTest.m_tyMax0, tgST1.Query_Normal() );
        tgAxTest.m_tyMin1 = tyS1_NM;
        tgAxTest.m_tyMax1 = tyS1_NM;
        tgAxTest.m_tySpeed = MATH::F_DOT( tgDT.m_tvDT, tgST1.Query_Normal() );

        iResult = F_Test_Seperating_Axis( &tgAxTest );
        switch (iResult) {
            case  1: //« Update has occurred
                ptgNFO->m_eSide = (tgAxTest.m_tyMax1 < tgAxTest.m_tyMin0) ? T_(AXIS_INFO,DIM)::NEGATIVE : T_(AXIS_INFO,DIM)::POSITIVE;
                ptgNFO->m_tvNormal = tgST1.Query_Normal();
                ptgNFO->m_tyMinT = tgAxTest.m_tyMinT;
            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 (TgE_NOINTERSECT);
        };

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

        T_(VECTOR,DIM)                      tvNormal;

        for (TgINT iE0 = 0; iE0 < 9; ++iE0)
        {
            tvNormal = MATH::F_CX( tgST0.Query_Edge( (iE0 & 3) ), tgST1.Query_Edge( (iE0 >> 3) ) );

            tgST0.Project( &tgAxTest.m_tyMin0, &tgAxTest.m_tyMax0, tvNormal );
            tgST1.Project( &tgAxTest.m_tyMin1, &tgAxTest.m_tyMax1, tvNormal );
            tgAxTest.m_tySpeed = MATH::F_DOT( tgDT.m_tvDT, tvNormal );

            iResult = F_Test_Seperating_Axis( &tgAxTest );
            switch (iResult) {
                case  1: //« Update has occurred
                    ptgNFO->m_eSide = (tgAxTest.m_tyMax1 < tgAxTest.m_tyMin0) ? T_(AXIS_INFO,DIM)::NEGATIVE : T_(AXIS_INFO,DIM)::POSITIVE;
                    ptgNFO->m_tvNormal = tvNormal;
                    ptgNFO->m_tyMinT = tgAxTest.m_tyMinT;
                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 (TgE_NOINTERSECT);
            };
        };
    }
    else
    {
        T_(VECTOR,DIM)                      tvNormal;

        // == Triangle 0 Space ===================================

        for (TgINT iE0 = 0; iE0 < 3; ++iE0)
        {
            tvNormal = MATH::F_CX( tgST0.Query_Normal(), tgST0.Query_Edge( iE0 ) );

            tgST0.Project( &tgAxTest.m_tyMin0, &tgAxTest.m_tyMax0, tvNormal );
            tgST1.Project( &tgAxTest.m_tyMin1, &tgAxTest.m_tyMax1, tvNormal );
            tgAxTest.m_tySpeed = MATH::F_DOT( tgDT.m_tvDT, tvNormal );

            iResult = F_Test_Seperating_Axis( &tgAxTest );
            switch (iResult) {
                case  1: //« Update has occurred
                    ptgNFO->m_eSide = (tgAxTest.m_tyMax1 < tgAxTest.m_tyMin0) ? T_(AXIS_INFO,DIM)::NEGATIVE : T_(AXIS_INFO,DIM)::POSITIVE;
                    ptgNFO->m_tvNormal = tvNormal;
                    ptgNFO->m_tyMinT = tgAxTest.m_tyMinT;
                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 (TgE_NOINTERSECT);
            };
        };

        // == Triangle 1 Space ===================================

        for (TgINT iE0 = 0; iE0 < 3; ++iE0)
        {
            tvNormal = MATH::F_CX( tgST1.Query_Normal(), tgST1.Query_Edge( iE0 ) );

            tgST0.Project( &tgAxTest.m_tyMin0, &tgAxTest.m_tyMax0, tvNormal );
            tgST1.Project( &tgAxTest.m_tyMin1, &tgAxTest.m_tyMax1, tvNormal );
            tgAxTest.m_tySpeed = MATH::F_DOT( tgDT.m_tvDT, tvNormal );

            iResult = F_Test_Seperating_Axis( &tgAxTest );
            switch (iResult) {
                case  1: //« Update has occurred
                    ptgNFO->m_eSide = (tgAxTest.m_tyMax1 < tgAxTest.m_tyMin0) ? T_(AXIS_INFO,DIM)::NEGATIVE : T_(AXIS_INFO,DIM)::POSITIVE;
                    ptgNFO->m_tvNormal = tvNormal;
                    ptgNFO->m_tyMinT = tgAxTest.m_tyMinT;
                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 (TgE_NOINTERSECT);
            };
        };
    };

    return (TgS_OK);
};

template TgRESULT F_Axis_Seperation( PC_TgF4AXIS_INFO, C_TgFLOAT32, CR_TgF4STRI, CR_TgF4STRI, CR_TgF4DELTA );


// ============================================================================================================================== //

// ---- F_Contact_Sweep --------------------------------------------------------------------------------------------------------- //
//
//  Axis Separation technique used to determine if two triangles are intersecting.
//
// Input:  tgPacket:  Contact generation parameters
// Input:  tyPM: Current normalized time of first contact for the contact query set.
// Input:  tgST0, tgST1: Space triangle primitive - Triangle #2 (tgST1) is being swept.
// Input:  tgDT: 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: tyPM: New normalized time of first contact
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Sweep( PC_(CONTACT_PACKET,DIM) ptgPacket, TYPE *ptyPM, CR_(STRI,DIM) tgST0, CR_(STRI,DIM) tgST1, CR_(DELTA,DIM) tgDT )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_SWEEP, (((TgUINT)ETgTRIANGLE<<16)|(TgUINT)ETgBOX))

    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ))
    TgASSERT(tgST0.Is_Valid() && tgST1.Is_Valid())

    if (0 == ptgPacket->m_niMaxContact || ptgPacket->m_niContact >= ptgPacket->m_niMaxContact || NULL == ptgPacket->m_ptgContact)
    {
        return (TgE_FAIL);
    };

    TgFEBUG_COLLISION_TRIANGLE_CREATEID(iDBG_TriID, tgST1, ETgFEBUG_COLLISION_ENTERFCN);

    TTgAXIS_INFO<TYPE,DIM>              tgNFO;

    tgNFO.m_eSide = TTgAXIS_INFO<TYPE,DIM>::UNKNOWN;
    tgNFO.m_tyMinT =  LIMITS<TYPE>::MAX;

    C_TgRESULT tgResult = F_Axis_Seperation( &tgNFO, *ptyPM + ptgPacket->m_tySweepTol, tgST0, tgST1, tgDT );

    if (TgFAILED(tgResult))
    {
        TgASSERT(TgE_NOINTERSECT == tgResult)
        return (tgResult);
    };

    // == Contact Generation ==================================================================================================== //

    TgASSERT((tgNFO.m_eSide != TTgAXIS_INFO<TYPE,DIM>::UNKNOWN))

    if (tgNFO.m_eSide == TTgAXIS_INFO<TYPE,DIM>::PENETRATED || tgNFO.m_tyMinT < TYPE(0.0))
    {
        ptgPacket->m_niContact = 0;
        *ptyPM = TYPE(0.0);
        return (TgE_PREPENETRATION);
    };

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

    if (tgNFO.m_tyMinT > *ptyPM + ptgPacket->m_tySweepTol)
    {
        return (TgE_NOINTERSECT);
    };

    if (tgNFO.m_tyMinT < *ptyPM - ptgPacket->m_tySweepTol)
    {
        ptgPacket->m_niContact = 0;
        *ptyPM = tgNFO.m_tyMinT;
    };

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

    C_(VECTOR,DIM)                      tvOffset = MATH::F_MUL( tgNFO.m_tyMinT, tgDT.m_tvDT );
    T_(AXIS_PROJECT,DIM)                tgP0, tgP1;

    F_Axis_ProjInfo( &tgP0, tgNFO.m_tvNormal, tgST0.Query_PT() );
    F_Axis_ProjInfo( &tgP1, tgNFO.m_tvNormal, tgST1.Query_PT() );

    //  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_TgINT                             niVertD0 = (tgNFO.m_eSide == T_(AXIS_INFO,DIM)::NEGATIVE) ? tgP0.m_niMinDepth : tgP0.m_niMaxDepth;
    PC_(VECTOR,DIM)                     atvVert0 = (tgNFO.m_eSide == T_(AXIS_INFO,DIM)::NEGATIVE) ? tgP0.m_atvMinVert : tgP0.m_atvMaxVert;
    C_TgINT                             niVertD1 = (tgNFO.m_eSide == T_(AXIS_INFO,DIM)::NEGATIVE) ? tgP1.m_niMaxDepth : tgP1.m_niMinDepth;
    PC_(VECTOR,DIM)                     atvVert1 = (tgNFO.m_eSide == T_(AXIS_INFO,DIM)::NEGATIVE) ? tgP1.m_atvMaxVert : tgP1.m_atvMinVert;
    C_(VECTOR,DIM)                      tvK0 = MATH::F_NEG( tgNFO.m_tvNormal );
    C_(VECTOR,DIM)                      tvNormal = (tgNFO.m_eSide == T_(AXIS_INFO,DIM)::NEGATIVE) ? tvK0 : tgNFO.m_tvNormal;
    P_(CONTACT,DIM)                     ptgContact;

    if (1 == niVertD0) //« Triangle 0 vertex touching a feature of Triangle 1
    {
        ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
        ptgContact->m_tvPos = atvVert0[0];
        ptgContact->m_tvNormal = tvNormal;
        ptgContact->m_tyT0 = tgNFO.m_tyMinT;
        ptgContact->m_tyDepth = TYPE(0.0);

        ++ptgPacket->m_niContact;

        return (TgS_OK);
    };

    if (1 == niVertD1) //« Triangle 1 vertex touching a feature of Triangle 0
    {
        ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
        ptgContact->m_tvPos = MATH::F_ADD( atvVert1[0], tvOffset );
        ptgContact->m_tvNormal = tvNormal;
        ptgContact->m_tyT0 = tgNFO.m_tyMinT;
        ptgContact->m_tyDepth = TYPE(0.0);

        ++ptgPacket->m_niContact;

        return (TgS_OK);
    };

    if (2 == niVertD0 && 2 == niVertD1) //« Edge-Edge Contact
    {
        TgINT                               niCode, niPoint;
        T_(VECTOR,DIM)                      tvS0,tvS1;
        
        niCode = F_Internal_Intersect(
            &tvS0,&tvS1, atvVert0[0],
            MATH::F_SUB( atvVert0[1], atvVert0[0] ),
            MATH::F_ADD( tvOffset,    atvVert1[0] ),
            MATH::F_SUB( atvVert1[1], atvVert1[0] )
        );

        niPoint = MIN( niCode, ptgPacket->m_niMaxContact - ptgPacket->m_niContact );

        switch (niPoint) {
            case 2:
                ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
                ptgContact->m_tvPos = tvS1;
                ptgContact->m_tvNormal = tvNormal;
                ptgContact->m_tyT0 = tgNFO.m_tyMinT;
                ptgContact->m_tyDepth = TYPE(0.0);

                ++ptgPacket->m_niContact;

            case 1:
                ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
                ptgContact->m_tvPos = tvS0;
                ptgContact->m_tvNormal = tvNormal;
                ptgContact->m_tyT0 = tgNFO.m_tyMinT;
                ptgContact->m_tyDepth = TYPE(0.0);

                ++ptgPacket->m_niContact;

                return (niCode != niPoint ? TgS_MAXCONTACTS : TgS_OK);

            default:
                return (TgE_NOINTERSECT);
        };
    };

    TTgCLIP_LIST<TYPE,DIM>              tgCL;

    if (2 == niVertD0 && 3 == niVertD1) //« Edge-Face Contact
    {
        TTgCLF_STLN<TYPE,DIM,1,1>::DO( &tgCL, tgST1, MATH::F_SUB( atvVert0[0], tvOffset ), MATH::F_SUB( atvVert0[1], atvVert0[0] ) );
        C_TgINT niPoint = MIN( tgCL.m_niPoint, ptgPacket->m_niMaxContact - ptgPacket->m_niContact );

        switch (niPoint) {
            case 2:
                ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
                ptgContact->m_tvPos = MATH::F_ADD( tgCL.m_ptvPoint[1], tvOffset );
                ptgContact->m_tvNormal = tvNormal;
                ptgContact->m_tyT0 = tgNFO.m_tyMinT;
                ptgContact->m_tyDepth = TYPE(0.0);

                ++ptgPacket->m_niContact;

            case 1:
                ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
                ptgContact->m_tvPos = MATH::F_ADD( tgCL.m_ptvPoint[0], tvOffset );
                ptgContact->m_tvNormal = tvNormal;
                ptgContact->m_tyT0 = tgNFO.m_tyMinT;
                ptgContact->m_tyDepth = TYPE(0.0);

                ++ptgPacket->m_niContact;

                return (tgCL.m_niPoint != niPoint ? TgS_MAXCONTACTS : TgS_OK);

            default:
                return (TgE_NOINTERSECT);
        };
    };

    if (3 == niVertD0 && 2 == niVertD1) //« Face-Edge Contact
    {
        TTgCLF_STLN<TYPE,DIM,1,1>::DO( &tgCL, tgST0, MATH::F_ADD( tvOffset, atvVert1[0] ), MATH::F_SUB( atvVert1[1], atvVert1[0] ) );
        C_TgINT niPoint = MIN( tgCL.m_niPoint, ptgPacket->m_niMaxContact - ptgPacket->m_niContact );

        switch (niPoint) {
            case 2:
                ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
                ptgContact->m_tvPos = tgCL.m_ptvPoint[1];
                ptgContact->m_tvNormal = tvNormal;
                ptgContact->m_tyT0 = tgNFO.m_tyMinT;
                ptgContact->m_tyDepth = TYPE(0.0);

                ++ptgPacket->m_niContact;

            case 1:
                ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);
                ptgContact->m_tvPos = tgCL.m_ptvPoint[0];
                ptgContact->m_tvNormal = tvNormal;
                ptgContact->m_tyT0 = tgNFO.m_tyMinT;
                ptgContact->m_tyDepth = TYPE(0.0);

                ++ptgPacket->m_niContact;

                return (tgCL.m_niPoint != niPoint ? TgS_MAXCONTACTS : TgS_OK);

            default:
                return (TgE_NOINTERSECT);
        };
    };

    TgASSERT(3 == niVertD0 && 3 == niVertD1)

    F_Clip( &tgCL, tgST0,tgST1.Query_PT() );

    return (F_Internal_CoP( ptgPacket, tgCL, tgNFO.m_tyMinT, tvNormal ));
};

template TgRESULT F_Contact_Sweep( PC_TgF4CONTACT_PACKET, P_TgFLOAT32, CR_TgF4STRI, CR_TgF4STRI, CR_TgF4DELTA );


// ============================================================================================================================== //

}; // END COL //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}; // END TGS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////