// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Cylinder-Capsule.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:  tgCP0: Capsule 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_(CAPSULE,DIM) tgCP0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgASSERT( tgCP0.Is_Valid() && tgCY0.Is_Valid() )

    TYPE                                tyTest;
    T_(VECTOR,DIM)                      tvTest;

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

    C_(VECTOR,DIM)                      tvDS = MATH::F_SUB( tgCY0.Query_Origin(), tgCP0.Query_Origin() );

    //const TYPE                          tyDS_A0 = MATH::F_DOT( tvDS, tgCP0.Query_AxisUnit() );
    const TYPE                          tyDS_A1 = MATH::F_DOT( tvDS, tgCY0.Query_AxisUnit() );

    // == Axis Separation Tests ==============================================================

    ptgAxS->m_tyDepth = -LIMITS<TYPE>::MAX;

    const TYPE                          tyABS_A0_A1 = P::ABS( MATH::F_DOT( tgCP0.Query_AxisUnit(), tgCY0.Query_AxisUnit() ) );

    // -- Axis: Cylinder Axes ----------------------------------

    // Cylinder #0 Axis

    tyTest = P::ABS( tyDS_A1 ) - (tgCY0.Query_Extent() + tgCP0.Query_Radius() + tgCP0.Query_Extent()*tyABS_A0_A1);

    if (tyTest > TYPE(0.0))
    {
        return (TgFALSE);
    };

    if (tyTest > ptgAxS->m_tyDepth)
    {
        ptgAxS->m_tvNormal = tyDS_A1 > TYPE(0.0) ? tgCY0.Query_AxisUnit() : MATH::F_NEG( tgCY0.Query_AxisUnit() );
        ptgAxS->m_tyDepth = tyTest;
        ptgAxS->m_iAxis = 1;
    };

    // ---------------------------------------------------------

    const TYPE                          tyRadSum = tgCP0.Query_Radius() + tgCY0.Query_Radius();

    CR_(SEGMENT,DIM)                    tgSG_CP0 = tgCP0.Query_Segment();
    CR_(SEGMENT,DIM)                    tgSG_CY0 = tgCY0.Query_Segment();

    TYPE                                tyCA0,tyCA1, tyC0,tyC1, tyTMPA;

    const TYPE                          tyDistSq = F_ClosestSq( &tyCA0, &tyCA1, tgSG_CP0, tgSG_CY0 );
    
    if (tyDistSq > tyRadSum*tyRadSum)
    {
        return (TgFALSE);
    };
    
    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyCA0, tgSG_CP0.Query_DirN() );
    C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyCA1, tgSG_CY0.Query_DirN() );

    C_(VECTOR,DIM)                      tvMin_CP0 = MATH::F_ADD( tgSG_CP0.Query_Origin(), tvK0 );
    C_(VECTOR,DIM)                      tvMin_CY0 = MATH::F_ADD( tgSG_CY0.Query_Origin(), tvK1 );
    T_(VECTOR,DIM)                      tvMinDirN;
    TgBOOL                              bIntersecting = TgTRUE;

    if (tyDistSq > LIMITS<TYPE>::EPSILON)
    {
        C_(VECTOR,DIM)                      tvMinDist = MATH::F_SUB( tvMin_CY0, tvMin_CP0 );

        tvMinDirN = MATH::F_SUB( tvMin_CY0, tvMin_CP0 );
        MATH::F_NORM( tvMinDirN );

        // -- Axis: Direction Between Points of Closest Proximity --

        TYPE                                tyT0,tyT1, tyT2,tyT3;

        tgCP0.Project( &tyT0,&tyT1, tvMinDirN );
        tgCY0.Project( &tyT2,&tyT3, tvMinDirN );

        if (tyT1 - tyT2 < TYPE(0.0) || tyT3 - tyT0 < TYPE(0.0))
        {
            return (TgFALSE);
        };

        tyTest = tyT2 - tyT1;

        if (tyTest > ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvPoint = tvMin_CY0;
            ptgAxS->m_tvNormal = tvMinDirN;
            ptgAxS->m_tyDepth = tyTest;
            ptgAxS->m_iAxis = 2;
        };
    }
    else
    {
        TYPE                                tyTest;

        bIntersecting = TgFALSE;
        tvMinDirN = MATH::F_NORM( &tyTest, tvDS );

        if (Near_Zero( tyTest ))
        {
            // That's it - throw in the towel, I give up - these primitives are just too penetrated to care which axis is used.
            return (TgTRUE);
        };
    };

    // -- Axis: Perpendicular to Axis of Cylinder --------------

    tyC0 = MATH::F_DOT( tvMinDirN, tgCY0.Query_BasisUnit0() );
    tyC1 = MATH::F_DOT( tvMinDirN, tgCY0.Query_BasisUnit1() );

    C_(VECTOR,DIM)                      tvK2 = MATH::F_MUL( tyC0, tgCY0.Query_BasisUnit0() );
    C_(VECTOR,DIM)                      tvK3 = MATH::F_MUL( tyC1, tgCY0.Query_BasisUnit1() );

    tvTest = MATH::F_NORM( MATH::F_ADD( tvK2, tvK3 ) );

    const TYPE                          tyABS_Ax_A0 = P::ABS( MATH::F_DOT(tvTest,tgCP0.Query_AxisUnit()) );

    tyTest = P::ABS( MATH::F_DOT(tvTest,tvDS) ) - (tgCY0.Query_Radius() + tyABS_Ax_A0*tgCP0.Query_Extent() + tgCP0.Query_Radius());

    if (tyTest > TYPE(0.0))
    {
        return (TgFALSE);
    };

    if (tyTest > ptgAxS->m_tyDepth)
    {
        ptgAxS->m_tvPoint = tvMin_CP0;
        ptgAxS->m_tvNormal = tvTest;
        ptgAxS->m_tyDepth = tyTest;
        ptgAxS->m_iAxis = 2;
    };

    const TYPE                          tyABS_A0xA1 = P::SQRT( TYPE(1.0) - MIN( TYPE(1.0), tyABS_A0_A1*tyABS_A0_A1 ) );

    if (!Near_One( tyABS_A0_A1 ) && !Near_Zero( tyABS_A0xA1 ))
    {
        // -- Axis: Perpendicular to Axis of Capsule ---------------

        tyC0 = MATH::F_DOT(tvMinDirN,tgCP0.Query_BasisUnit0());
        tyC1 = MATH::F_DOT(tvMinDirN,tgCP0.Query_BasisUnit1());

        C_(VECTOR,DIM)                      tvK4 = MATH::F_MUL( tyC0, tgCP0.Query_BasisUnit0() );
        C_(VECTOR,DIM)                      tvK5 = MATH::F_MUL( tyC1, tgCP0.Query_BasisUnit1() );

        tvTest = MATH::F_NORM( MATH::F_ADD( tvK4, tvK5 ) );

        const TYPE                          tyABS_Ax_A1 = P::ABS( MATH::F_DOT(tvTest,tgCY0.Query_AxisUnit()) );
        const TYPE                          tyAx_C10 = MATH::F_DOT(tvTest,tgCY0.Query_BasisUnit0());
        const TYPE                          tyAx_C11 = MATH::F_DOT(tvTest,tgCY0.Query_BasisUnit1());
        const TYPE                          tyK0 = tgCP0.Query_Radius() + tyABS_Ax_A1*tgCY0.Query_Extent();

        tyTMPA = P::SQRT( tyAx_C10*tyAx_C10 + tyAx_C11*tyAx_C11 );
        tyTest = P::ABS( MATH::F_DOT( tvTest, tvDS ) ) - (tyK0 + tyTMPA*tgCY0.Query_Radius());

        if (tyTest > TYPE(0.0))
        {
            return (TgFALSE);
        };

        if (tyTest > ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvPoint = tvMin_CY0;
            ptgAxS->m_tvNormal = tvTest;
            ptgAxS->m_tyDepth = tyTest;
            ptgAxS->m_iAxis = 2;
        };
    };

    // !! VOODOO MAGIC TIME !! - Red Red Wine, Drink a lot of it before proceeding, I'm telling you man - get snookered.
    // Need to try to test the rim cases - <shot>

    if (Near_One( tyCA1 ) || Near_Zero( tyCA1 ))
    {
        TYPE                                tyT0,tyT1, tyT2,tyT3;
        T_(VECTOR,DIM)                      tvCI0, tvSG0;
        CR_(SEGMENT,DIM)                    tgSG_CP0 = tgCP0.Query_Segment();

        const TYPE tyDistSq = TTgCSQ_CILN<TYPE,DIM,1,1>::DO(
            &tvCI0,&tvSG0, tvMin_CY0,tgCY0.Query_AxisUnit(), tgCY0.Query_Radius(), tgSG_CP0.Query_Origin(), tgSG_CP0.Query_DirN()
        );

        if (tyDistSq < TYPE(0.0))
        {
            return (TgTRUE);
        };

        tvTest = MATH::F_NORM( MATH::F_SUB( tvCI0, tvSG0 ) );

        tgCP0.Project( &tyT0,&tyT1, tvTest );
        tgCY0.Project( &tyT2,&tyT3, tvTest );

        if (tyT1 - tyT2 < TYPE(0.0) || tyT3 - tyT0 < TYPE(0.0))
        {
            return (TgFALSE);
        };

        tyTest = tyT2 - tyT1;

        if (tyTest > ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvPoint = tvCI0;
            ptgAxS->m_tvNormal = tvTest;
            ptgAxS->m_tyDepth = tyTest;
            ptgAxS->m_iAxis = 3;
        };
    };

    return (TgTRUE);
};

template TgBOOL F_Axis_Seperation( PC_TgF4AXIS_RESULT, CR_TgF4CAPSULE, CR_TgF4CYLINDER );


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

// ---- F_Contact_Penetrate_CylAxis --------------------------------------------------------------------------------------------- //
//  -- Internal Function --
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgAxS: Structure holding the resulting axis separation information necessary to create a contact set.
// Input:  tgCP0: Capsule 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_CylAxis(
    PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(AXIS_RESULT,DIM) tgAxS, CR_(CAPSULE,DIM) tgCP0, CR_(CYLINDER,DIM) tgCY0
) {
    // (1) Capsule is contained within the infinite projection of the cylinder (tube)
    // (2) Capsule axis termination is contained but also passes through the cylinder (tube).
    // (3) Capsule passes through-and-through the cylinder (tube)
    // (4) Capsule axis is entirely outside of the cylinder - the only intersecting part is the capsule cap.

    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tgCY0.Query_Extent(), tgAxS.m_tvNormal );
    C_(VECTOR,DIM)                      tvCYC = MATH::F_SUB( tgCY0.Query_Origin(), tvK0 );
    const TYPE                          tyAX_N = MATH::F_DOT( tgCP0.Query_AxisUnit(), tgAxS.m_tvNormal );
    C_TgINT                             niContact = ptgPacket->m_niContact;
    P_(CONTACT,DIM)                     ptgContact;
    T_(VECTOR,DIM)                      tvCPC, tvTest;
    TYPE                                tyTest;

    // Attempt to create a contact point at the lowest point on the capsule.

    C_(VECTOR,DIM)                      tvK2 = MATH::F_MUL( P::FSEL( tyAX_N, TYPE(1.0), TYPE(-1.0) ), tgCP0.Query_HalfAxis() );

    tvCPC = MATH::F_ADD( tgCP0.Query_Origin(), tvK2 );
    tyTest = MATH::F_DOT( MATH::F_SUB( tvCPC, tvCYC ), tgAxS.m_tvNormal );

    if (tyTest > TYPE(0.0) && tgCY0.Is_Contained( tvCPC ))
    {
        // Capsule termination is a contact point ([Case: 1,2])

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

        ptgContact->m_tvPos = MATH::F_ADD( tvCPC, MATH::F_MUL( tgCP0.Query_Radius(), tgAxS.m_tvNormal ) );
        ptgContact->m_tvNormal = tgAxS.m_tvNormal;
        ptgContact->m_tyT0 = TYPE(0.0);
        ptgContact->m_tyDepth = tyTest;

        ++ptgPacket->m_niContact;
    };

    // Construct a capsule axis that is directed for maximum penetration into the cylinder.

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

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

    tvT0 = MATH::F_SUB( tgAxS.m_tvNormal, MATH::F_MUL( tyAX_N, tgCP0.Query_AxisUnit() ) );

    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 ) );

    // Translate the capsule axis to the line of deepest penetration along the capsule, and then clip it to the cylinder.

    T_(SEGMENT,DIM)                     tgCPAX = tgCP0.Query_Segment();

    tgCPAX.Set_Origin( MATH::F_ADD( tgCPAX.Query_Origin(), MATH::F_MUL( tgCP0.Query_Radius(), tvX0 ) ) );

    if (F_Clip( &tyT0,&tyT1, tgCY0,tgCPAX ) >= 0)
    {
        tvTest = MATH::F_ADD( tgCPAX.Query_Origin(), MATH::F_MUL( tyT0, tgCPAX.Query_DirN() ) );
        tyTest = MATH::F_DOT( MATH::F_SUB( tvTest, tvCYC ), tgAxS.m_tvNormal );

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

            // Capsule termination is a contact point ([Case: 3])

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

            ptgContact->m_tvPos = tvTest;
            ptgContact->m_tvNormal = tgAxS.m_tvNormal;
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tyTest;

            ++ptgPacket->m_niContact;
        };

        // Similar to above, and it must not be sufficiently divergent from the first point (prevent two-point tangents)

        tvTest = MATH::F_ADD( tgCPAX.Query_Origin(), MATH::F_MUL( tyT1, tgCPAX.Query_DirN() ) );
        tyTest = MATH::F_DOT( MATH::F_SUB( tvTest, tvCYC ), tgAxS.m_tvNormal );

        if (tyTest > TYPE(0.0) && !Near_Zero( tyT0 - tyT1 ))
        {
            if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
            {
                return (TgS_MAXCONTACTS);
            };

            // Capsule termination is a contact point ([Case: 1,2,3])

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

            ptgContact->m_tvPos = tvTest;
            ptgContact->m_tvNormal = tgAxS.m_tvNormal;
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tyTest;

            ++ptgPacket->m_niContact;
        };

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

    // The capsule contact line is outside of the cylinder.  Attempt to intersect the cylinder with the capsule. ([Case: 4])

    // One of the extremes of the cylinder rim must be penetrated into the capsule.  Therefore use math.

    C_(VECTOR,DIM)                      tvK1 = MATH::F_SUB( tgCP0.Query_AxisUnit(), MATH::F_MUL( tyAX_N, tgAxS.m_tvNormal ) );

    tvX0 = MATH::F_NORM( MATH::F_MUL( P::FSEL( tyAX_N, TYPE(-1.0), TYPE(-1.0) ), tvK1 ) );
    tvT0 = MATH::F_ADD( tvCYC, MATH::F_MUL( tgCY0.Query_Radius(), tvX0 ) );
    tvTest = MATH::F_NORM( &tyTest, MATH::F_SUB( tvT0, tvCPC ) );
    tyTest = MATH::F_DOT( tvTest, tgAxS.m_tvNormal ) * (tgCP0.Query_Radius() - tyTest);

    if (tyTest > TYPE(0.0))
    {
        // Capsule termination is a contact point ([Case: 3])

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

        ptgContact->m_tvPos = tvT0;
        ptgContact->m_tvNormal = tgAxS.m_tvNormal;
        ptgContact->m_tyT0 = TYPE(0.0);
        ptgContact->m_tyDepth = tyTest;

        ++ptgPacket->m_niContact;
    };

    tvT0 = MATH::F_SUB( tvCYC, MATH::F_MUL( tgCY0.Query_Radius(), tvX0 ) );
    tvTest = MATH::F_NORM( &tyTest, MATH::F_SUB( tvT0, tvCPC ) );
    tyTest = MATH::F_DOT( tvTest, tgAxS.m_tvNormal ) * (tgCP0.Query_Radius() - tyTest);

    if (tyTest > TYPE(0.0))
    {
        // Capsule termination is a contact point ([Case: 3])

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

        ptgContact->m_tvPos = tvT0;
        ptgContact->m_tvNormal = tgAxS.m_tvNormal;
        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_CylAxis( PC_TgF4CONTACT_PACKET, CR_TgF4AXIS_RESULT, CR_TgF4CAPSULE, CR_TgF4CYLINDER );


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

// ---- F_Contact_Penetrate ----------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgCP0: Capsule 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_(CAPSULE,DIM) tgCP0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgCAPSULE<<16)|(TgUINT)ETgCYLINDER))

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

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

    // 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, tgCP0,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;

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

    switch (tgAxS.m_iAxis) {
        case 7: // -- Axis: Rim --------------------------------------------

            ptgContact->m_tvPos = tgAxS.m_tvPoint;
            ptgContact->m_tvNormal = tgAxS.m_tvNormal;
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tgAxS.m_tyDepth;

            ++ptgPacket->m_niContact;

            return (TgS_OK);

        case 2: { // -- Axis: Perpendicular to Axis of Cylinder --------------

            T_(VECTOR,DIM)                      tvTest;
            TYPE                                tyTest;

            tvTest = tgCP0.Calc_Support_Point( tgAxS.m_tvNormal );
            tyTest = MATH::F_DOT( MATH::F_SUB( tvTest, tgCY0.Query_Origin() ), tgAxS.m_tvNormal ) + tgCY0.Query_Radius();

            if (tyTest <= TYPE(0.0))
            {
                COUT_NOOBJ(
                    WARNING, TgT("%-16.16s(%-48.48s): [CY][CP] Algorithm: Separation Axis found with no contacts generated.\n"),
                    TgT("Collision"), TgT("F_Contact_Penetrate")
                );;

                return (TgE_NOINTERSECT);
            };

            ptgContact->m_tvPos = MATH::F_SUB( tvTest, MATH::F_MUL( tyTest, 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);

        };

        case 1: { // -- Axis: Cylinder Axis ----------------------------------

            return (F_Contact_Penetrate_CylAxis( ptgPacket, tgAxS, tgCP0, tgCY0 ));

        };

        default:
            COUT_NOOBJ(
                CRITICAL, TgT("%-16.16s(%-48.48s): [CY][CP] Algorithmic - Should not be able to reach this location.\n"),
                TgT("Collision"), TgT("F_Contact_Penetrate")
            );

            return (TgE_FAIL);
    };

    COUT_NOOBJ(
        CRITICAL, TgT("%-16.16s(%-48.48s): [CY][CP] Algorithmic - Should not be able to reach this location.\n"), TgT("Collision"),
        TgT("F_Contact_Penetrate")
    );

    return (TgE_FAIL);
};

template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4CAPSULE, CR_TgF4CYLINDER );


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

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