// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Cylinder-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_Axis_Seperation ------------------------------------------------------------------------------------------------------- //
// Input:  tgST0: Space Triangle primitive
// Input:  tgCY0: Cylinder primitive
// Output: tgAxS: Structure holds the resulting axis separation information necessary to create a contact set.
// Return: False if a separating axis exists, true otherwise
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgBOOL F_Axis_Seperation( PC_(AXIS_RESULT,DIM) ptgAxS, CR_(STRI,DIM) tgST0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgASSERT( tgCY0.Is_Valid() && tgST0.Is_Valid() )

    TYPE                                tyMinCyl, tyMaxCyl, tyMinTri, tyMaxTri;
    TYPE                                tyTest, tyMinDepth, tyMaxDepth;

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

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

    tgCY0.Project( &tyMinCyl, &tyMaxCyl, tgST0.Query_Normal() );

    if (tyMaxCyl < tyTriD || tyMinCyl > tyTriD)
    {
        return (TgFALSE);
    };

    ptgAxS->m_tvNormal = MATH::F_NEG( tgST0.Query_Normal() );
    ptgAxS->m_tyDepth = tyTriD - tyMinCyl;
    ptgAxS->m_iAxis = 1;

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

    const TYPE                          tyNormTest = MATH::F_DOT(tgST0.Query_Normal(),tgCY0.Query_AxisUnit());

    if (!Near_One( tyNormTest ))
    {
        tyMinCyl = MATH::F_DOT(tgCY0.Query_Origin(),tgCY0.Query_AxisUnit());
        tyMaxCyl = tyMinCyl + tgCY0.Query_Extent();
        tyMinCyl -= tgCY0.Query_Extent();

        tgST0.Project( &tyMinTri, &tyMaxTri, tgCY0.Query_AxisUnit() );

        if (tyMaxCyl < tyMinTri || tyMinCyl > tyMaxTri)
        {
            return (TgFALSE);
        };

        tyMinDepth = tyMaxCyl - tyMinTri;
        tyMaxDepth = tyMaxTri - tyMinCyl;

        if (tyMinDepth > tyMaxDepth)
        {
            if (tyMaxDepth < ptgAxS->m_tyDepth)
            {
                ptgAxS->m_tvNormal = tgCY0.Query_AxisUnit();
                ptgAxS->m_tyDepth = tyMaxDepth;
                ptgAxS->m_iAxis = 2;
            };
        }
        else
        {
            if (tyMinDepth < ptgAxS->m_tyDepth)
            {
                ptgAxS->m_tvNormal = MATH::F_NEG( tgCY0.Query_AxisUnit() );
                ptgAxS->m_tyDepth = tyMinDepth;
                ptgAxS->m_iAxis = 3;
            };
        };
    };

    // -- Axis: Radial Cylindrical Sweep -----------------------

    TYPE                                tyET0,tyET1, tyCyl, tyDistSq;

    tyDistSq = F_ClosestSq( &tyET0,&tyET1, &tyCyl, tgST0, tgCY0.Query_Segment() );

    if (tyDistSq > tgCY0.Query_RadiusSq())
    {
        return (TgFALSE);
    };

    if (Is_Point_Culled( tgST0.Query_CT(), tyET0,tyET1 ))
    {
        return (TgTRUE);
    };

    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyET0, tgST0.Query_Edge0() );
    C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyET1, tgST0.Query_Edge2() );
    T_(VECTOR,DIM)                      tvProxCyl, tvProxTri;

    tvProxCyl = MATH::F_ADD( tgCY0.Query_Segment().Query_Origin(), MATH::F_MUL( tyCyl, tgCY0.Query_Segment().Query_DirN() ) );
    tvProxTri = MATH::F_ADD( tgST0.Query_Origin(), MATH::F_SUB( tvK0, tvK1 ) );

    if (Near_Zero( tyCyl ) || Near_One( tyCyl ) )
    {
        // -- Axis: Cap-Surface Component --------------------------

        const TYPE                          tyDet = TYPE(1.0) - tyNormTest*tyNormTest;

        if (tyDet <= LIMITS<TYPE>::ROOTEPSILON)
        {
            // The plane upon which the cylinder cap rests is parallel to that of the triangle.  Return the last best result.

            return (TgTRUE);
        };

        // Define the line of intersection between the two planar surfaces.

        const TYPE                          tyInvDet = TYPE(1.0) / tyDet;

        const TYPE                          tyCylD = MATH::F_DOT(tgCY0.Query_AxisUnit(),tvProxCyl);
        const TYPE                          tyTA = tyInvDet*( tyCylD - tyNormTest*tyTriD );
        const TYPE                          tyTB = tyInvDet*( tyTriD - tyNormTest*tyCylD );

        TTgLINE<TYPE,DIM>                   tgLN;

        tgLN.Set_Origin( MATH::F_ADD( MATH::F_MUL( tyTA, tgCY0.Query_AxisUnit() ), MATH::F_MUL( tyTB, tgST0.Query_Normal() ) ) );
        tgLN.Set_DirN( MATH::F_CX( tgCY0.Query_AxisUnit(), tgST0.Query_Normal() ) );

        TTgCLIP_LIST<TYPE,DIM>              ClipList;

        F_Clip( &ClipList, tgST0, tgLN );

        if (ClipList.m_niPoint != 2)
        {
            // No intersection can exist between the triangle and cylinder cap.  Error state.  Return the last best result.

            return (TgTRUE);
        };

        // Find the minimal distance to the cap rim.

        TTgSEGMENT<TYPE,DIM>                tgG0;
        TYPE                                tyG0;

        tgG0.Set_Origin( ClipList.m_ptvPoint[0] );
        tgG0.Set_DirN( MATH::F_SUB( ClipList.m_ptvPoint[1], ClipList.m_ptvPoint[0] ) );

        tyDistSq = F_ClosestSq( &tyG0, tgG0, tvProxCyl );

        if (tyDistSq > tgCY0.Query_RadiusSq())
        {
            // Error state.  Return the last best result.

            return (TgTRUE);
        };

        tvProxTri = MATH::F_ADD( tgG0.Query_Origin(), MATH::F_MUL( tyG0, tgG0.Query_DirN() ) );
    };

    T_(VECTOR,DIM)                      tvNormal = MATH::F_NORM( &tyTest, MATH::F_SUB( tvProxCyl, tvProxTri ) );

    if (Near_Zero( tyTest ))
    {
        return (TgTRUE); // Error state.  Return the last best result.
    };

    tyTest = P::FSEL( tgCY0.Query_Radius() - tyTest, tgCY0.Query_Radius() - tyTest, TYPE(0.0) );

    if (1 == ptgAxS->m_iAxis && Near_One( MATH::F_DOT( tgST0.Query_Normal(), tvNormal ) ))
    {
        return (TgTRUE); // Bias towards having the contacts created for a triangle contact.
    };

    if (tyTest < ptgAxS->m_tyDepth)
    {
        ptgAxS->m_tvNormal = tvNormal;
        ptgAxS->m_tvPoint = tvProxCyl;
        ptgAxS->m_tyDepth = tyTest;
        ptgAxS->m_iAxis = 4;
    };

    return (TgTRUE);
};

template TgBOOL F_Axis_Seperation( PC_TgF4AXIS_RESULT, CR_TgF4STRI, CR_TgF4CYLINDER );


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

// ---- F_Internal_ClipTriToCyl ------------------------------------------------------------------------------------------------- //
//  -- Internal Function -- 
// Input:  tgST0: Space Triangle primitive
// Input:  tgCY0: Cylinder primitive
// Output: tgCL: The resulting points of the clipping operation
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM> TgFORCEINLINE
TgBOOL F_Internal_ClipTriToCyl( PC_(CLIP_LIST,DIM) ptgCL, CR_(STRI,DIM) tgST0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgBOOL                              bC0,bC1, bTest0,bTest1, bRet = TgFALSE;
    TYPE                                tyT0,tyT1;

    // Triangle Edge 0 

    bRet |= TTgCLP_CYLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgCY0, tgST0.Query_Point0(),tgST0.Query_Edge0() );

    bC0 = Near_Zero( tyT0 );
    bC1 = Near_One( tyT0 );
    bTest0 = (bC0 && !tgST0.Test_Point0()) || (bC1 && !tgST0.Test_Point1()) || (!bC0 && !bC1 && !tgST0.Test_Edge0());

    bC0 = Near_Zero( tyT1 );
    bC1 = Near_One( tyT1 );
    bTest1 = (bC0 && !tgST0.Test_Point0()) || (bC1 && !tgST0.Test_Point1()) || (!bC0 && !bC1 && !tgST0.Test_Edge0());

    if (!bTest0)
    {
        ptgCL->m_ptvPoint[ptgCL->m_niPoint++] = MATH::F_ADD( tgST0.Query_Point0(), MATH::F_MUL( tyT0, tgST0.Query_Edge0() ) );
    };

    if (!bTest1)
    {
        ptgCL->m_ptvPoint[ptgCL->m_niPoint++] = MATH::F_ADD( tgST0.Query_Point0(), MATH::F_MUL( tyT1, tgST0.Query_Edge0() ) );
    };


    // Triangle Edge 1 

    bRet |= TTgCLP_CYLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgCY0, tgST0.Query_Point1(),tgST0.Query_Edge1() );

    bC0 = Near_Zero( tyT0 );
    bC1 = Near_One( tyT0 );
    bTest0 = (bC0 && !tgST0.Test_Point1()) || (bC1 && !tgST0.Test_Point2()) || (!bC0 && !bC1 && !tgST0.Test_Edge1());

    bC0 = Near_Zero( tyT1 );
    bC1 = Near_One( tyT1 );
    bTest1 = (bC0 && !tgST0.Test_Point1()) || (bC1 && !tgST0.Test_Point2()) || (!bC0 && !bC1 && !tgST0.Test_Edge1());

    if (!bTest0)
    {
        ptgCL->m_ptvPoint[ptgCL->m_niPoint++] = MATH::F_ADD( tgST0.Query_Point1(), MATH::F_MUL( tyT0, tgST0.Query_Edge1() ) );
    };

    if (!bTest1)
    {
        ptgCL->m_ptvPoint[ptgCL->m_niPoint++] = MATH::F_ADD( tgST0.Query_Point1(), MATH::F_MUL( tyT1, tgST0.Query_Edge1() ) );
    };


    // Triangle Edge 2 

    bRet |= TTgCLP_CYLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgCY0, tgST0.Query_Point2(),tgST0.Query_Edge2() );

    bC0 = Near_Zero( tyT0 );
    bC1 = Near_One( tyT0 );
    bTest0 = (bC0 && !tgST0.Test_Point2()) || (bC1 && !tgST0.Test_Point0()) || (!bC0 && !bC1 && !tgST0.Test_Edge2());

    bC0 = Near_Zero( tyT1 );
    bC1 = Near_One( tyT1 );
    bTest1 = (bC0 && !tgST0.Test_Point2()) || (bC1 && !tgST0.Test_Point0()) || (!bC0 && !bC1 && !tgST0.Test_Edge2());

    if (!bTest0)
    {
        ptgCL->m_ptvPoint[ptgCL->m_niPoint++] = MATH::F_ADD( tgST0.Query_Point2(), MATH::F_MUL( tyT0, tgST0.Query_Edge2() ) );
    };

    if (!bTest1)
    {
        ptgCL->m_ptvPoint[ptgCL->m_niPoint++] = MATH::F_ADD( tgST0.Query_Point2(), MATH::F_MUL( tyT1, tgST0.Query_Edge2() ) );
    };


    return (bRet);
};


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

// ---- F_Contact_Penetrate ----------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgST0: Space Triangle primitive
// Input:  tgCY0: Cylinder primitive - contact points are generated on this primitive
// Output: tgPacket: Points of penetration between the two primitives are added to it
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Penetrate( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(STRI,DIM) tgST0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgCAPSULE<<16)|(TgUINT)ETgTRIANGLE))

    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof(T_(CONTACT,DIM)) && tgCY0.Is_Valid() && tgST0.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, tgST0, etgDEBUG_COLLISION_ENTERFCN );

    // Primitive Culling - Set of criteria required for the primitive to be considered penetrating the triangle.

    C_(VECTOR,DIM)                      tvK0 = MATH::F_SUB( tgCY0.Query_Origin(), tgST0.Query_Origin() );
    const TYPE                          tyDS_N = MATH::F_DOT( tgST0.Query_Normal(), tvK0 );
    const TYPE                          tyEX_N = MATH::F_DOT( tgST0.Query_Normal(), tgCY0.Query_HalfAxis() );
    const TYPE                          tyS0_N = tyDS_N - tyEX_N;
    const TYPE                          tyS1_N = tyDS_N + tyEX_N;
    const TYPE                          tyRadius = tgCY0.Query_Radius();

    if ((tyS0_N > tyRadius && tyS1_N > tyRadius) || (tyS0_N < TYPE(0.0) && tyS1_N < TYPE(0.0)))
    {
        // Either both of the capsule's end points are below the plane or more than radius above the plane.

        return (TgE_NOINTERSECT);
    };

    TgFEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_PASSED_REJECT );

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

    TTgAXIS_RESULT<TYPE,DIM>            tgAxS;

    if (!F_Axis_Seperation( &tgAxS, tgST0,tgCY0 ))
    {
        return (TgE_NOINTERSECT);
    };

    TgASSERT( Near_One( MATH::F_LSQ(tgAxS.m_tvNormal) ) && tgAxS.m_tyDepth >= TYPE(0.0) )

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

    //C_TgINT                             niContact = ptgPacket->m_niContact;
    P_(CONTACT,DIM)                     ptgContact;

    if (tgAxS.m_iAxis >= 4)
    {
        // -- Axis: Cap-Surface Component --------------------------------------------------------------------------------------

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

        ptgContact->m_tvPos = MATH::F_SUB( tgAxS.m_tvPoint, MATH::F_MUL( tgCY0.Query_Radius(), tgAxS.m_tvNormal ) );
        ptgContact->m_tvNormal = tgAxS.m_tvNormal;
        ptgContact->m_tyT0 = TYPE(0.0);
        ptgContact->m_tyDepth = tgAxS.m_tyDepth;

        ++ptgPacket->m_niContact;

        return (TgS_OK);
    };

    if (tgAxS.m_iAxis >= 2)
    {
        // -- Axis: Cylinder Primary Axis --------------------------------------------------------------------------------------

        T_(CLIP_LIST,DIM)                   tgCL;

        if (F_Internal_ClipTriToCyl( &tgCL, tgST0,tgCY0 ) && 0 == tgCL.m_niPoint)
        {
            return (TgE_NOINTERSECT);
        };

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

        for (TgINT iIdx = 0; iIdx < niMax; ++iIdx)
        {
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

            C_(VECTOR,DIM)                      tvK1 = MATH::F_SUB( tgCL.m_ptvPoint[iIdx], tgCY0.Query_Origin() );
            const TYPE                          tyP_AX = MATH::F_DOT( tvK1, tgCY0.Query_AxisUnit() );
            TYPE                                tyDepth = tgCY0.Query_Extent() - P::ABS( tyP_AX );

            tyDepth = P::FSEL( tyDepth, tyDepth, TYPE(0.0) );

            ptgContact->m_tvPos = MATH::F_SUB( tgCL.m_ptvPoint[iIdx], MATH::F_MUL( tyDepth, tgAxS.m_tvNormal ) );
            ptgContact->m_tvNormal = tgAxS.m_tvNormal;
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tyDepth;

            ++ptgPacket->m_niContact;
        };

        if (0 != tgCL.m_niPoint)
        {
            return (niMax == tgCL.m_niPoint ? TgS_OK : TgS_MAXCONTACTS);
        };

        // Error catch - default to triangle normal for this error state.
    };

    // -- Triangle Normal ------------------------------------------------------------------------------------------------------

    const TYPE                          tyAX_N = MATH::F_DOT(tgCY0.Query_AxisUnit(),tgST0.Query_Normal());

    M_(VECTOR,DIM)                      tvBX0 = tgCY0.Query_BasisUnit0();
    M_(VECTOR,DIM)                      tvBX1 = tgCY0.Query_BasisUnit1();

    T_(VECTOR,DIM)                      tvT0, tvX0;
    TYPE                                tyX,tyY,tyZ;
    const TYPE                          tyUseBX = P::ABS( tyAX_N ) - (TYPE(1.0) - LIMITS<TYPE>::EPSILON);

    tvT0 = MATH::F_SUB( MATH::F_MUL( tyAX_N, tgCY0.Query_AxisUnit() ), tgST0.Query_Normal() );
    tyX = P::FSEL( tyUseBX, tvBX0.X(), tvT0.X() );
    tyY = P::FSEL( tyUseBX, tvBX0.Y(), tvT0.Y() );
    tyZ = P::FSEL( tyUseBX, tvBX0.Z(), tvT0.Z() );
    tvX0 = MATH::F_NORM( MATH::F_SETV<TYPE,DIM>( tyX,tyY,tyZ ) );

    const TYPE                          tyDN = P::FSEL( tyAX_N, TYPE(-1.0), TYPE(1.0) );
    C_(VECTOR,DIM)                      tvC0 = MATH::F_ADD( tgCY0.Query_Origin(), MATH::F_MUL( tyDN, tgCY0.Query_HalfAxis() ) );

    if (tyAX_N > TYPE(0.5))
    {
        // Method: Produce three points along symmetrical points along the cylinder rim.  If the cylinder plane is not parallel
        // to the triangle plane then one of these points must be the point of deepest penetration.

        T_(VECTOR,DIM)                      tvX1;

        tvT0 = MATH::F_CX( tvX0, tgCY0.Query_AxisUnit() );
        tyX = P::FSEL( tyUseBX, tvBX1.X(), tvT0.X() );
        tyY = P::FSEL( tyUseBX, tvBX1.Y(), tvT0.Y() );
        tyZ = P::FSEL( tyUseBX, tvBX1.Z(), tvT0.Z() );
        tvX1 = MATH::F_SETV<TYPE,DIM>( tyX,tyY,tyZ );
        MATH::F_NORM( tvX1 );
        tvX1 = MATH::F_MUL( tvX1, KF32_SQRT3 );

        // Generate the line segment for each of these points from the middle of the cylinder's end cap - and then clip it against
        // the triangle using feature reduction information.  Chose the largest t value returned to create the contact point.

        TYPE                                tyT0,tyT1, tyTest;
        TgINT                               iCode;
        C_TgINT                             niContact = ptgPacket->m_niContact;

        tvT0 = MATH::F_MUL( tgCY0.Query_Radius(), tvX0 );

        if (TTgCLF_STLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, &iCode, tgST0, tvC0,tvT0 ) >= 0 && 0 != (iCode & 12))
        {
            const TYPE                          tyL0 = 0 != (iCode & 4) ? tyT0 : -tyT1;
            const TYPE                          tyL1 = 0 != (iCode & 8) ? tyT1 : -tyT0;
            const TYPE                          tyLN = P::FSEL( tyL1 - tyL0, tyL1, tyL0 );
            C_(VECTOR,DIM)                      tvK1 = MATH::F_ADD( tvC0, MATH::F_MUL( tyLN, tvT0 ) );

            tyTest = MATH::F_DOT( MATH::F_SUB( tgST0.Query_Origin(), tvK1 ), tgST0.Query_Normal() );

            if (tyTest >= TYPE(0.0))
            {
                if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
                {
                    return (TgS_MAXCONTACTS);
                };

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

                ptgContact->m_tvPos = MATH::F_ADD( tvC0, MATH::F_MUL( tyLN, tvT0 ) );
                ptgContact->m_tvNormal = tgST0.Query_Normal();
                ptgContact->m_tyT0 = TYPE(0.0);
                ptgContact->m_tyDepth = tyTest;

                ++ptgPacket->m_niContact;
            };
        };

        tvT0 = MATH::F_MUL( TYPE(-0.5)*tgCY0.Query_Radius(), MATH::F_ADD( tvX0, tvX1 ) );

        if (TTgCLF_STLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, &iCode, tgST0, tvC0,tvT0 ) >= 0 && 0 != (iCode & 12))
        {
            const TYPE                          tyL0 = 0 != (iCode & 4) ? tyT0 : -tyT1;
            const TYPE                          tyL1 = 0 != (iCode & 8) ? tyT1 : -tyT0;
            const TYPE                          tyLN = P::FSEL( tyL1 - tyL0, tyL1, tyL0 );
            C_(VECTOR,DIM)                      tvK1 = MATH::F_ADD( tvC0, MATH::F_MUL( tyLN, tvT0 ) );

            tyTest = MATH::F_DOT( MATH::F_SUB( tgST0.Query_Origin(), tvK1 ), tgST0.Query_Normal() );

            if (tyTest >= TYPE(0.0))
            {
                if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
                {
                    return (TgS_MAXCONTACTS);
                };

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

                ptgContact->m_tvPos = MATH::F_ADD( tvC0, MATH::F_MUL( tyLN, tvT0 ) );
                ptgContact->m_tvNormal = tgST0.Query_Normal();
                ptgContact->m_tyT0 = TYPE(0.0);
                ptgContact->m_tyDepth = tyTest;

                ++ptgPacket->m_niContact;
            };
        };

        tvT0 = MATH::F_MUL( TYPE(-0.5)*tgCY0.Query_Radius(), MATH::F_SUB( tvX0, tvX1 ) );

        if (TTgCLF_STLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, &iCode, tgST0, tvC0,tvT0 ) >= 0 && 0 != (iCode & 12))
        {
            const TYPE                          tyL0 = 0 != (iCode & 4) ? tyT0 : -tyT1;
            const TYPE                          tyL1 = 0 != (iCode & 8) ? tyT1 : -tyT0;
            const TYPE                          tyLN = P::FSEL( tyL1 - tyL0, tyL1, tyL0 );
            C_(VECTOR,DIM)                      tvK1 = MATH::F_ADD( tvC0, MATH::F_MUL( tyLN, tvT0 ) );

            tyTest = MATH::F_DOT( MATH::F_SUB( tgST0.Query_Origin(), tvK1 ), tgST0.Query_Normal() );

            if (tyTest >= TYPE(0.0))
            {
                if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
                {
                    return (TgS_MAXCONTACTS);
                };

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

                ptgContact->m_tvPos = MATH::F_ADD( tvC0, MATH::F_MUL( tyLN, tvT0 ) );
                ptgContact->m_tvNormal = tgST0.Query_Normal();
                ptgContact->m_tyT0 = TYPE(0.0);
                ptgContact->m_tyDepth = tyTest;

                ++ptgPacket->m_niContact;
            };
        };

        if (niContact != ptgPacket->m_niContact)
        {
            return (TgS_OK);
        };

        T_(CLIP_LIST,DIM)                   tgCL;

        F_Internal_ClipTriToCyl( &tgCL, tgST0,tgCY0 );

        if (0 == tgCL.m_niPoint)
        {
            return (TgE_NOINTERSECT);
        };

        C_TgINT                             niMax = MIN( tgCL.m_niPoint, ptgPacket->m_niMaxContact - ptgPacket->m_niContact );

        tvT0 = MATH::F_MUL( TYPE(-2.0)*tgCY0.Query_Extent(), tgST0.Query_Normal() );

        for (TgINT iIdx = 0; iIdx < niMax; ++iIdx)
        {
            if (TTgCLP_CYLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgCY0, tgCL.m_ptvPoint[iIdx],tvT0 ) < 0)
            {
                continue;
            };

            if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
            {
                return (TgS_MAXCONTACTS);
            };

            const TYPE                          tyLN = P::FSEL( tyT1 - tyT0, tyT1, tyT0 );
            C_(VECTOR,DIM)                      tvK2 = MATH::F_ADD( tgCL.m_ptvPoint[iIdx], MATH::F_MUL( tyLN, tvT0 ) );

            tyTest = MATH::F_DOT( MATH::F_SUB( tgST0.Query_Origin(), tvK2 ), tgST0.Query_Normal() );

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

            ptgContact->m_tvPos = MATH::F_ADD( tgCL.m_ptvPoint[iIdx], MATH::F_MUL( tyLN, tvT0 ) );
            ptgContact->m_tvNormal = tgST0.Query_Normal();
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tyTest;

            ++ptgPacket->m_niContact;
        };

        return (niContact != ptgPacket->m_niContact ? TgS_OK : TgE_NOINTERSECT);
    }
    else
    {
        C_(VECTOR,DIM)                      tvD0 = MATH::F_MUL( TYPE(-2.0)*tyDN, tgCY0.Query_HalfAxis() );
        C_(VECTOR,DIM)                      tvS0 = MATH::F_ADD( tvC0, tvX0 );
        TYPE                                tyTest, tyT0,tyT1;
        C_TgINT                             niContact = ptgPacket->m_niContact;
        TgINT                               iCode;

        if (TTgCLF_STLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, &iCode,tgST0, tvS0,tvD0 ) < 0 || 0 == (iCode & 12))
        {
            return (TgE_NOINTERSECT);
        };

        C_(VECTOR,DIM)                      tvK1 = MATH::F_ADD( tvS0, MATH::F_MUL( tyT0, tvD0 ) );

        tyTest = MATH::F_DOT( MATH::F_SUB( tgST0.Query_Origin(), tvK1 ), tgST0.Query_Normal() );

        if (0 == (iCode & 4) && tyTest >= TYPE(0.0))
        {
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

            ptgContact->m_tvPos = MATH::F_ADD( tvS0, MATH::F_MUL( tyT0, tvD0 ) );
            ptgContact->m_tvNormal = tgST0.Query_Normal();
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tyTest;

            ++ptgPacket->m_niContact;
        };

        C_(VECTOR,DIM)                      tvK2 = MATH::F_ADD( tvS0, MATH::F_MUL( tyT1, tvD0 ) );

        tyTest = MATH::F_DOT( MATH::F_SUB( tgST0.Query_Origin(), tvK2 ), tgST0.Query_Normal() );

        if (0 == (iCode & 8) && tyTest >= TYPE(0.0))
        {
            if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
            {
                return (TgS_MAXCONTACTS);
            };

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

            ptgContact->m_tvPos = MATH::F_ADD( tvS0, MATH::F_MUL( tyT1, tvD0 ) );
            ptgContact->m_tvNormal = tgST0.Query_Normal();
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tyTest;

            ++ptgPacket->m_niContact;
        };

        return (niContact != ptgPacket->m_niContact ? TgS_OK : TgE_NOINTERSECT);
    };
};

template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4STRI, CR_TgF4CYLINDER );


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

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