// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Cylinder-Box.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:  tgBX0: Box primitives
// Input:  tgCY0: Cylinder primitives
// Output: tgAxS: Structure holds the resulting axis separation information necessary to create a contact set.
// Output: bIsContained: A boolean array indicating which faces, if projected along their normal, fully contain the cylinder.
// Return: False if a separating axis exists, true otherwise
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgBOOL F_Axis_Seperation( PC_(AXIS_RESULT,DIM) ptgAxS, PC_TgBOOL bIsContained, CR_(BOX,DIM) tgBX0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgASSERT( tgCY0.Is_Valid() && tgBX0.Is_Valid() )

    TYPE                                tyTest;

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

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

    const TYPE                          tyDS_CYA = MATH::F_DOT( tvDS, tgCY0.Query_AxisUnit() );
    const TYPE                          tyDS_BX0 = MATH::F_DOT( tvDS, tgBX0.Query_AxisUnit0() );
    const TYPE                          tyDS_BX1 = MATH::F_DOT( tvDS, tgBX0.Query_AxisUnit1() );
    const TYPE                          tyDS_BX2 = MATH::F_DOT( tvDS, tgBX0.Query_AxisUnit2() );

    // == Containment Test ===================================================================

    //  To allow for quick selection of a planar contact surface, test to see if the cylinder's projection onto each of the
    // box's faces is entirely contained.  A similar test to see if the box is fully contained by the cylinder's cap surface.

    const TYPE                          tyABS_CYA_BX0 = P::ABS( MATH::F_DOT(tgCY0.Query_AxisUnit(),tgBX0.Query_AxisUnit0()) );
    const TYPE                          tyABS_CYA_BX1 = P::ABS( MATH::F_DOT(tgCY0.Query_AxisUnit(),tgBX0.Query_AxisUnit1()) );
    const TYPE                          tyABS_CYA_BX2 = P::ABS( MATH::F_DOT(tgCY0.Query_AxisUnit(),tgBX0.Query_AxisUnit2()) );

    const TYPE                          tyABS_CYC_BX0 = P::SQRT( tyABS_CYA_BX2*tyABS_CYA_BX2 + tyABS_CYA_BX1*tyABS_CYA_BX1 );
    const TYPE                          tyABS_CYC_BX1 = P::SQRT( tyABS_CYA_BX2*tyABS_CYA_BX2 + tyABS_CYA_BX0*tyABS_CYA_BX0 );
    const TYPE                          tyABS_CYC_BX2 = P::SQRT( tyABS_CYA_BX1*tyABS_CYA_BX1 + tyABS_CYA_BX0*tyABS_CYA_BX0 );

    const TYPE                          tyABS_CY_BX0 = tgCY0.Query_Radius()*tyABS_CYC_BX0 + tgCY0.Query_Extent()*tyABS_CYA_BX0;
    const TYPE                          tyABS_CY_BX1 = tgCY0.Query_Radius()*tyABS_CYC_BX1 + tgCY0.Query_Extent()*tyABS_CYA_BX1;
    const TYPE                          tyABS_CY_BX2 = tgCY0.Query_Radius()*tyABS_CYC_BX2 + tgCY0.Query_Extent()*tyABS_CYA_BX2;

    bIsContained[0] = (tgBX0.Query_Extent0() >= tyDS_BX0 + tyABS_CY_BX0) && (tyABS_CY_BX0 - tyDS_BX0 <= tgBX0.Query_Extent0());
    bIsContained[1] = (tgBX0.Query_Extent1() >= tyDS_BX1 + tyABS_CY_BX1) && (tyABS_CY_BX1 - tyDS_BX1 <= tgBX0.Query_Extent1());
    bIsContained[2] = (tgBX0.Query_Extent2() >= tyDS_BX2 + tyABS_CY_BX2) && (tyABS_CY_BX2 - tyDS_BX2 <= tgBX0.Query_Extent2());

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

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

    // -- Axis: Box Face/Plane Normals -------------------------

    // Box Axis #0

    tyTest = P::ABS( tyDS_BX0 ) - (tgBX0.Query_Extent0() + tyABS_CY_BX0);

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

    if (tyTest > ptgAxS->m_tyDepth)
    {
        ptgAxS->m_tvNormal = tyDS_BX0 > TYPE(0.0) ? MATH::F_NEG( tgBX0.Query_AxisUnit0() ) : tgBX0.Query_AxisUnit0();
        ptgAxS->m_tyDepth = tyTest;
        ptgAxS->m_iAxis = 0;

        if (bIsContained[1] && bIsContained[2])
        {
            return (TgTRUE);
        };
    };

    // Box Axis #1

    tyTest = P::ABS( tyDS_BX1 ) - (tgBX0.Query_Extent1() + tyABS_CY_BX1);

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

    if (tyTest > ptgAxS->m_tyDepth)
    {
        ptgAxS->m_tvNormal = tyDS_BX1 > TYPE(0.0) ? MATH::F_NEG( tgBX0.Query_AxisUnit1() ) : tgBX0.Query_AxisUnit1();
        ptgAxS->m_tyDepth = tyTest;
        ptgAxS->m_iAxis = 1;

        if (bIsContained[0] && bIsContained[2])
        {
            return (TgTRUE);
        };
    };

    // Box Axis #2

    tyTest = P::ABS( tyDS_BX2 ) - (tgBX0.Query_Extent2() + tyABS_CY_BX2);

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

    if (tyTest > ptgAxS->m_tyDepth)
    {
        ptgAxS->m_tvNormal = tyDS_BX2 > TYPE(0.0) ? MATH::F_NEG( tgBX0.Query_AxisUnit2() ) : tgBX0.Query_AxisUnit2();
        ptgAxS->m_tyDepth = tyTest;
        ptgAxS->m_iAxis = 2;

        if (bIsContained[0] && bIsContained[1])
        {
            return (TgTRUE);
        };
    };

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

    tyTest  = P::ABS( tyDS_CYA ) - tgCY0.Query_Extent();
    tyTest -= tgBX0.Query_Extent0()*tyABS_CYA_BX0;
    tyTest -= tgBX0.Query_Extent1()*tyABS_CYA_BX1;
    tyTest -= tgBX0.Query_Extent2()*tyABS_CYA_BX2;

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

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

    // -- Axis: Cylinder Radial Direction ----------------------

    //  The cross-product of the three box edges and the cylinder axis is by definition a radial direction as well.  Therefore, it
    // is not necessary to examine those axes outside of the radial test.  

    T_(VECTOR,DIM)                      tvCY0, tvBX1;
    T_(VECTOR,DIM)                      tvNM;

    tyTest = F_ClosestSq( &tvBX1, &tvCY0, tgBX0, tgCY0.Query_Segment() );

    if (tyTest >= TYPE(0.0))
    {
        const TYPE                          tyK0 = MATH::F_DOT( MATH::F_SUB( tvCY0, tvBX1 ), tgCY0.Query_AxisUnit() );

        tvNM = MATH::F_NORM( MATH::F_SUB( MATH::F_SUB( tvCY0, tvBX1 ), MATH::F_MUL( tyK0, tgCY0.Query_AxisUnit() ) ) );

        const TYPE                          tyDS_N = MATH::F_DOT( tvDS, tvNM );

        const TYPE                          tyBX0_N = tgBX0.Query_Extent0()*P::ABS( MATH::F_DOT( tgBX0.Query_AxisUnit0(), tvNM ) );
        const TYPE                          tyBX1_N = tgBX0.Query_Extent1()*P::ABS( MATH::F_DOT( tgBX0.Query_AxisUnit1(), tvNM ) );
        const TYPE                          tyBX2_N = tgBX0.Query_Extent2()*P::ABS( MATH::F_DOT( tgBX0.Query_AxisUnit2(), tvNM ) );

        tyTest = P::ABS( tyDS_N ) - (tgCY0.Query_Radius() + tyBX0_N + tyBX1_N + tyBX2_N);

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

        if (tyTest > ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvNormal = tyDS_N > TYPE(0.0) ? tvNM : MATH::F_NEG( tvNM );
            ptgAxS->m_tyDepth = tyTest;
            ptgAxS->m_iAxis = 4;
        };
    }
    else
    {
        //  The line segment must have intersected the box.  The cylinder is highly penetrated.  Find the minimal distance
        // between any box feature and the cylinder axis.  Use the difference vector as a separation axis.  Box features are
        // the twelve edges, and the eight vertices.

        TYPE                                tyTest, tyMinSq = LIMITS<TYPE>::MAX;
        T_(VECTOR,DIM)                      tvK1, tvT0,tvT1, tvMinAxis, tvMinEdge;
        TTgSEGMENT<TYPE,DIM>                tgEdge;

        for (TgINT iOrigin = 0; iOrigin < 4; ++iOrigin)
        {
            C_TgBOOL                            bAxis = ((0 != (iOrigin & 1)) ^ (0 != (iOrigin & 2)));

            tgEdge.Set_Origin(tgBX0.Calc_Point( 
                0 == (iOrigin&1) ? tgBX0.Query_Extent0() : -tgBX0.Query_Extent0(),
                0 == (iOrigin&2) ? tgBX0.Query_Extent1() : -tgBX0.Query_Extent1(),
                bAxis ? tgBX0.Query_Extent2() : -tgBX0.Query_Extent2()
            ));

            tvK1 = 0 != (iOrigin&1) ? tgBX0.Query_AxisUnit0() : MATH::F_NEG( tgBX0.Query_AxisUnit0() ) ;
            tgEdge.Set_DirN( MATH::F_MUL( tgBX0.Query_Extent0(), tvK1 ) );
            tyTest = F_ClosestSq( &tvT0,&tvT1, tgCY0.Query_Segment(), tgEdge );
            if (tyTest < tyMinSq)
            {
                tyMinSq = tyTest;
                tvMinAxis = tvT0;
                tvMinEdge = tvT1;
            };

            tvK1 = 0 != (iOrigin&2) ? tgBX0.Query_AxisUnit1() : MATH::F_NEG( tgBX0.Query_AxisUnit1() ) ;
            tgEdge.Set_DirN( MATH::F_MUL( tgBX0.Query_Extent1(), tvK1 ) );
            tyTest = F_ClosestSq( &tvT0,&tvT1, tgCY0.Query_Segment(), tgEdge );
            if (tyTest < tyMinSq)
            {
                tyMinSq = tyTest;
                tvMinAxis = tvT0;
                tvMinEdge = tvT1;
            };

            tvK1 = 0 != (iOrigin&1) ? tgBX0.Query_AxisUnit2() : MATH::F_NEG( tgBX0.Query_AxisUnit2() ) ;
            tgEdge.Set_DirN( MATH::F_MUL( tgBX0.Query_Extent2(), tvK1 ) );
            tyTest = F_ClosestSq( &tvT0,&tvT1, tgCY0.Query_Segment(), tgEdge );
            if (tyTest < tyMinSq)
            {
                tyMinSq = tyTest;
                tvMinAxis = tvT0;
                tvMinEdge = tvT1;
            };
        };

        C_(VECTOR,DIM)                      tvK2 = MATH::F_SUB( tvMinAxis, tvMinEdge );
        const TYPE                          tyK0 = MATH::F_DOT( tvK2, tgCY0.Query_AxisUnit() );
        C_(VECTOR,DIM)                      tvK3 = MATH::F_MUL( tyK0, tgCY0.Query_AxisUnit() );

        tvNM = MATH::F_NORM( MATH::F_SUB( tvK2, tvK3 ) );

        tvT0 = tgBX0.Calc_Support_Point( MATH::F_NEG( tvNM ) );
        tyTest = tgCY0.Query_Radius() + MATH::F_DOT( MATH::F_SUB( tgCY0.Query_Origin(), tvT0 ), tvNM );

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

        if (tyTest > ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvNormal = tvNM;
            ptgAxS->m_tyDepth = tyTest;
            ptgAxS->m_iAxis = 4;
        };
    };

    // -- Axis: Cylinder Rim -----------------------------------
    // Possible and if so how?

    return (TgTRUE);
};

template TgBOOL F_Axis_Seperation( PC_TgF4AXIS_RESULT, PC_TgBOOL, CR_TgF4BOX, CR_TgF4CYLINDER );


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

// ---- F_Contact_Penetrate_BoxAxis --------------------------------------------------------------------------------------------- //
//  -- 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:  bIsContained: A boolean array indicating which faces, if projected along their normal, fully contain the cylinder. 
// Input:  tgBX0: Box 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_BoxAxis(
    PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(AXIS_RESULT,DIM) tgAxS, PC_TgBOOL bIsContained, CR_(BOX,DIM) tgBX0, CR_(CYLINDER,DIM) tgCY0
) {
    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ));
    TgASSERT(tgCY0.Is_Valid() && tgBX0.Is_Valid());

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

    // The axis separation result normal is one of the box axes with the appropriate direction (sign).

    C_TgINT                             i0 = (tgAxS.m_iAxis + 1) % 3, i1 = (tgAxS.m_iAxis + 2) % 3;

    const TYPE                          tyPlnD = MATH::F_DOT(tgBX0.Query_Origin(),tgAxS.m_tvNormal) + tgBX0.Query_Extent(tgAxS.m_iAxis);
    C_TgINT                             iFlag = (bIsContained[i0] ? 0 : (1 << i0)) | (bIsContained[i1] ? 0 : (1 << i1));

    // Create a new basis set for the cylinder such that the first axis is equal to the direction of greatest penetration.

    const TYPE                          tyCYA_N = MATH::F_DOT(tgCY0.Query_AxisUnit(),tgAxS.m_tvNormal);
    T_(VECTOR,DIM)                      tvCYB0, tvCYB1;
    TYPE                                tyLength;

    tvCYB0 = MATH::F_NORM( &tyLength, MATH::F_SUB( MATH::F_MUL( tyCYA_N, tgCY0.Query_AxisUnit() ), tgAxS.m_tvNormal ) );

    if (Near_Zero( tyLength ))
    {
        tvCYB0 = tgCY0.Query_BasisUnit0();
        tvCYB1 = tgCY0.Query_BasisUnit1();
    }
    else
    {
        tvCYB1 = MATH::F_CX( tvCYB0, tgCY0.Query_AxisUnit() );
    };

    // The contact generator is split into two cases depending on the orientation of the cylinder to the separation axis.
    // 1. Cylinder is standing on the resulting plane (ie. the angle to the plane is greater than 45)
    // 2. Cylinder is resting/lying on the plane (ie. the angle to the plane is less than 45)
    //  The reason for the separation is in how the extra contacts (outside of the deepest point of penetration) are created. In
    // the first case the extra points are created around the rim of the cylinder's penetrating cap.  The second case has a
    // segment running parallel to the primary axis along the exterior of the cylinder originating with the point of largest
    // penetration.

    if (P::ABS( tyCYA_N ) <= KF32_SQRT1_2)
    {
        // The cylinder is considered to be lying down on the surface of the box.

        const TYPE                          tyCYB0_N = MATH::F_DOT(tvCYB0,tgAxS.m_tvNormal);
        const TYPE                          tyInvCYB0_N = TYPE(1.0) / tyCYB0_N;
        T_(VECTOR,DIM)                      tvP0, tvP1, tvContact;
        TYPE                                tyT0 = TYPE(0.0),tyT1 = TYPE(0.0), tyTMP, tyDepth;

        //  Construct the termination points of the cylinder axis and project them onto the box face/surface. The box face is
        // considered to be the control in determining the contact surface because of the choice of axis. For instance if a
        // segment was taken parallel to the axis, originating at the point of deepest penetration it could have a zero contact
        // solution with a thin box.  Instead, the intersecting volume is examined, and the surface on the box is used to create
        // the contact solution.

        const TYPE                          tyK0 = (MATH::F_DOT(tvP0,tgAxS.m_tvNormal) - tyPlnD)*tyInvCYB0_N;
        const TYPE                          tyK1 = (MATH::F_DOT(tvP0,tgAxS.m_tvNormal) - tyPlnD)*tyInvCYB0_N;

        tvP0 = MATH::F_SUB( MATH::F_SUB( tgCY0.Query_Origin(), tgCY0.Query_HalfAxis() ), MATH::F_MUL( tyK0, tvCYB0 ) );
        tvP1 = MATH::F_SUB( MATH::F_ADD( tgCY0.Query_Origin(), tgCY0.Query_HalfAxis() ), MATH::F_MUL( tyK1, tvCYB0 ) );

        if (0 == iFlag || TTgCLP_BXLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgBX0,iFlag, tvP0,MATH::F_SUB( tvP1, tvP0 ) ) >= 0)
        {
            // Point #0

            tvContact = MATH::F_ADD( MATH::F_MUL( TYPE(1.0) - tyT0, tvP0 ), MATH::F_MUL( tyT0, tvP1 ) );
            tyTMP = MATH::F_DOT( MATH::F_SUB( tvContact, tgCY0.Query_Origin() ), tvCYB0 );

            C_(VECTOR,DIM)                      tvK0 = MATH::F_SUB( tvContact, MATH::F_MUL( tyTMP, tvCYB0 ) );
            const TYPE                          tyK2 = MATH::F_DOT( tvK0, tgAxS.m_tvNormal ) - tyPlnD;

            tyDepth = tgCY0.Query_Radius() + P::FSEL( tyK2, -tyTMP, tyTMP );

            if (tyDepth> 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( tvContact, MATH::F_MUL( tyDepth, tvCYB0 ) );
                ptgContact->m_tvNormal = tgAxS.m_tvNormal;
                ptgContact->m_tyT0 = TYPE(0.0);
                ptgContact->m_tyDepth = tyCYB0_N*tyDepth;

                ++ptgPacket->m_niContact;
            };

            // Point #1

            tvContact = MATH::F_ADD( MATH::F_MUL( TYPE(1.0) - tyT1, tvP0 ), MATH::F_MUL( tyT1, tvP1 ) );
            tyTMP = MATH::F_DOT( MATH::F_SUB( tvContact, tgCY0.Query_Origin() ), tvCYB0 );

            C_(VECTOR,DIM)                      tvK1 = MATH::F_SUB( tvContact, MATH::F_MUL( tyTMP, tvCYB0 ) );
            const TYPE                          tyK3 = MATH::F_DOT( tvK1, tgAxS.m_tvNormal ) - tyPlnD;

            tyDepth = tgCY0.Query_Radius() + P::FSEL( tyK3, -tyTMP, tyTMP );

            if (tyDepth> 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( tvContact, MATH::F_MUL( tyDepth, tvCYB0 ) );
                ptgContact->m_tvNormal = tgAxS.m_tvNormal;
                ptgContact->m_tyT0 = TYPE(0.0);
                ptgContact->m_tyDepth = tyCYB0_N*tyDepth;

                ++ptgPacket->m_niContact;
            };
        };

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

    TYPE                                tyT0 = TYPE(0.0), tyT1 = TYPE(0.0), tyDepth;

    // Example: A box is resting, unstable, on the edge of circular table (the centre of gravity when projected through the local
    // force field does not rest on the table).  The desired reaction is for the box is slowly tilt and slide off the table
    // surface, the box contact face remaining tangential to the table rim at all times

    // In this example the defining element of the contact surface is the box face itself.  The method used to find this surface
    // is to take the non-seperating box axis, the resulting faces defined by them, and find how they intersect the cylinder's
    // cap surface.  The cylinder cap is assumed to be the interacting surface since other separation axis should be more
    // dominant in the other cases.


    // Example: A roll of toilet paper is resting on top of one of those perpetually ever-present game crates.
    
    //  In this case the non-contact box faces are completely independent of the cylinder volume/space.  The defining element of
    // the contact surface is the cylindrical cap area.  In this case the previously mentioned method of using the direction of
    // greatest penetration, followed by radially equi-distant rim points provides the best solution.



    const TYPE                          tyExtent = P::FSEL( tyCYA_N, -tgCY0.Query_Extent(), tgCY0.Query_Extent() );
    C_(VECTOR,DIM)                      tvK2 = MATH::F_MUL( tyExtent, tgCY0.Query_AxisUnit() );
    C_(VECTOR,DIM)                      tvCentreW = MATH::F_ADD( tgCY0.Query_Origin(), tvK2 );
    C_(VECTOR,DIM)                      tvMaxPoint = MATH::F_ADD( tvCentreW, MATH::F_MUL( tgCY0.Query_Radius(), tvCYB0 ) );

    TgINT                               iIdx0 = i0, iIdx1 = i1;
    T_(VECTOR,DIM)                      tvInsideDirN, tvX0;
    TgBOOL                              bNewBasisSet = TgFALSE;

    tvInsideDirN = MATH::F_0<TYPE,DIM>();

    // First test to see if those box faces not defined by the separation axis form a contact surface with the cylinder.

    for (TgINT niCount = 0; niCount < 2; ++niCount, SWAP( iIdx0, iIdx1 ))
    {
        if (bIsContained[iIdx0])
        {
            // If the cylinder is known to be entirely contained in the orthogonal face, then intersection is impossible.

            continue;
        };

        //  Find the line of intersection between the near cylinder cap plane and the two box faces defined by one axis.
        // The direction of the interface will remain the same for both faces, only the origin will be different.

        T_(LINE,DIM)                        tgL1;
        T_(VECTOR,DIM)                      tvK4;

        MATH::F_UCX( &tvK4, tgCY0.Query_AxisUnit(), tgBX0.Query_AxisUnit( iIdx0 ) );

        tgL1.Set_DirN( tvK4 );

        const TYPE                          tyCYA_IX0 = MATH::F_DOT( tgCY0.Query_AxisUnit(), tgBX0.Query_AxisUnit( iIdx0 ) );
        const TYPE                          tyDet = TYPE(1.0) - tyCYA_IX0*tyCYA_IX0;

        if (Near_Zero( tyDet ))
        {
            continue;
        };

        const TYPE                          tyBXO_IX0 = MATH::F_DOT( tgBX0.Query_Origin(), tgBX0.Query_AxisUnit( iIdx0 ) );
        const TYPE                          tyPlnD_CYA = MATH::F_DOT( tvCentreW, tgCY0.Query_AxisUnit() );
        const TYPE                          tyInvDet = TYPE(1.0) / tyDet;
        const TYPE                          tyDN_IX1 = MATH::F_DOT( tgL1.Query_DirN(), tgBX0.Query_AxisUnit( iIdx1 ) );

        for (TgINT niFace = 0; niFace < 2; ++niFace)
        {
            const TYPE                          tyPlnD_IX0 = tyBXO_IX0 + (niFace == 0 ? -tgBX0.Query_Extent( iIdx0 ) : tgBX0.Query_Extent( iIdx0 ));

            const TYPE                          tyL0 = (tyPlnD_CYA - tyCYA_IX0*tyPlnD_IX0)*tyInvDet;
            const TYPE                          tyL1 = (tyPlnD_IX0 - tyCYA_IX0*tyPlnD_CYA)*tyInvDet;
            C_(VECTOR,DIM)                      tvK5 = MATH::F_MUL( tyL0, tgCY0.Query_AxisUnit() );
            C_(VECTOR,DIM)                      tvK6 = MATH::F_MUL( tyL1, tgBX0.Query_AxisUnit( iIdx0 ) );

            // F_Clip the resulting interface line to the cylindrical interior.

            tgL1.Set_Origin( MATH::F_ADD( tvK5, tvK6 ) );

            if (TgFAILED(F_Clip( &tyT0,&tyT1, static_cast<CR_(TUBE,DIM)>(tgCY0), tgL1 )))
            {
                continue;
            };

            // Find the min and max values along the face by examining the projection onto the opposing orthogonal axis.

            C_(VECTOR,DIM)                      tvK3 = MATH::F_SUB( tgBX0.Query_Origin(), tgL1.Query_Origin() );
            const TYPE                          tyDist = MATH::F_DOT( tvK3, tgBX0.Query_AxisUnit( iIdx1 ) );

            if (!Near_Zero(tyDN_IX1))
            {
                TYPE                                tyMin,tyMax;

                if (tyDN_IX1 > TYPE(0.0))
                {
                    tyMin = (tyDist - tgBX0.Query_Extent( iIdx1 )) / tyDN_IX1;
                    tyMax = (tyDist + tgBX0.Query_Extent( iIdx1 )) / tyDN_IX1;
                }
                else
                {
                    tyMin = (tyDist + tgBX0.Query_Extent( iIdx1 )) / tyDN_IX1;
                    tyMax = (tyDist - tgBX0.Query_Extent( iIdx1 )) / tyDN_IX1;
                };

                tyT0 = P::FSEL( tyMin - tyT0, tyMin, P::FSEL( tyT0 - tyMax, tyMax, tyT0 ) );
                tyT1 = P::FSEL( tyMin - tyT1, tyMin, P::FSEL( tyT1 - tyMax, tyMax, tyT1 ) );
            };

            T_(VECTOR,DIM)                      tvContact;
            TgBOOL                              bAddNormal = TgFALSE;
            TYPE                                tyDepth;

            tvContact = MATH::F_ADD( tgL1.Query_Origin(), MATH::F_MUL( tyT0, tgL1.Query_DirN() ) );
            tyDepth = tyPlnD - MATH::F_DOT( tvContact, tgAxS.m_tvNormal );

            // Point #0

            if (tyDepth > 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 = tvContact;
                ptgContact->m_tvNormal = tgAxS.m_tvNormal;
                ptgContact->m_tyT0 = TYPE(0.0);
                ptgContact->m_tyDepth = tyDepth;

                tvInsideDirN = MATH::F_ADD( tvInsideDirN, tvContact );
                bAddNormal = TgTRUE;

                ++ptgPacket->m_niContact;
            };

            // Point #1 - If the two clipped points are within tolerance of each other - skip the additional point.

            tvContact = MATH::F_ADD( tgL1.Query_Origin(), MATH::F_MUL( tyT1, tgL1.Query_DirN() ) );
            tyDepth = tyPlnD - MATH::F_DOT( tvContact, tgAxS.m_tvNormal );

            if (tyDepth > TYPE(0.0) && !Near_Zero( tyT0 - tyT1 ))
            {
                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 = tvContact;
                ptgContact->m_tvNormal = tgAxS.m_tvNormal;
                ptgContact->m_tyT0 = TYPE(0.0);
                ptgContact->m_tyDepth = tyDepth;

                tvInsideDirN = MATH::F_ADD( tvInsideDirN, tvContact );
                bAddNormal = TgTRUE;

                ++ptgPacket->m_niContact;
            };

            if (bAddNormal && !bNewBasisSet)
            {
                //  If the point of maximum penetration lies outside of the box face, then the resulting clipped point will be
                // along the same edge as one of the points already entered. The point's information could be made more useful.
                // By reconstructing the basis so that a point in the interior of the box along the arc formed between the contact
                // points added into the system will increase stability and prevent undue penetration into the box face.

                C_(VECTOR,DIM)                      tvK4 = MATH::F_SUB( tvMaxPoint, tgBX0.Query_Origin() );

                if (P::ABS( MATH::F_DOT( tvK4,tgBX0.Query_AxisUnit( iIdx0 ) ) ) > tgBX0.Query_Extent( iIdx0 ))
                {
                    bNewBasisSet |= TgTRUE;
                };
            };
        };
    };

    // Second - attempt to build a contact surface through the cylinder cap.

    if (bNewBasisSet && !Near_Zero(MATH::F_LSQ( tvInsideDirN )) && niContact != ptgPacket->m_niContact)
    {
        tvInsideDirN = MATH::F_DIV( tvInsideDirN, static_cast<TYPE>(ptgPacket->m_niContact - niContact) );
        tvInsideDirN = MATH::F_SUB( tvInsideDirN, tvCentreW );

        const TYPE                          tyIDN_CYA = MATH::F_DOT( tvInsideDirN, tgCY0.Query_AxisUnit() );
        TYPE                                tyLength;

        tvCYB0 = MATH::F_NORM( &tyLength, MATH::F_SUB( tvInsideDirN, MATH::F_MUL( tyIDN_CYA, tgCY0.Query_AxisUnit() ) ) );

        if (Near_Zero( tyLength ))
        {
            COUT_NOOBJ(
                WARNING, TgT("%-16.16s(%-48.48s): [CY][BX] - Counter Logical State\n"), TgT("Collision"),
                TgT("F_Contact_Penetrate_BoxAxis")
            );

            TgCOLLISION_ASSERT( TgFALSE );
            tvCYB0 = tgCY0.Query_BasisUnit0();
            tvCYB1 = tgCY0.Query_BasisUnit1();
        }
        else
        {
            tvCYB1 = MATH::F_CX( tvCYB0, tgCY0.Query_AxisUnit() );
        };
    };

    // Create a contact point triple - equidistant along the cylinder's rim (ie. 120 between each arm )
    T_(VECTOR,DIM)                      tvK7;
        
    tvK7  = MATH::F_MUL( tvCYB0, tgCY0.Query_Radius() );

    if (0 == iFlag || TTgCLP_BXLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgBX0,iFlag, tvCentreW, tvK7 ) >=0)
    {
        T_(VECTOR,DIM)                      tvPnt;

        tvPnt = MATH::F_ADD( tvCentreW, MATH::F_MUL( tvCYB0, (0 == iFlag ? tgCY0.Query_Radius() : tyT1) ) );
        tyDepth = tyPlnD - MATH::F_DOT( tvPnt, tgAxS.m_tvNormal );

        if (tyDepth> 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 = tvPnt;
            ptgContact->m_tvNormal = tgAxS.m_tvNormal;
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tyDepth;

            ++ptgPacket->m_niContact;
        };
    };

    if (ptgPacket->m_niContact - niContact >= 3)
    {
        // If the contact surface is defined by the box planes - then we can end here.  It is necessary to allow the system to
        // create the point of greatest penetration as well since this will often by the point of tangential contact even when
        // the surface is defined by the box.

        return (TgS_OK);
    };

    tvCYB1 = MATH::F_MUL( KF32_SQRT3, tvCYB1 );

    tvX0 = MATH::F_MUL( -TYPE(0.5), MATH::F_ADD( tvCYB0, tvCYB1 ) );
    tvK7 = MATH::F_MUL( tvX0, tgCY0.Query_Radius() );

    if (0 == iFlag || TTgCLP_BXLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgBX0,iFlag, tvCentreW, tvK7 ) >=0 )
    {
        T_(VECTOR,DIM)                      tvPnt;

        tvPnt = MATH::F_ADD( tvCentreW, MATH::F_MUL( tvX0, (0 == iFlag ? tgCY0.Query_Radius() : tyT1) ) );
        tyDepth = tyPlnD - MATH::F_DOT(tvPnt,tgAxS.m_tvNormal);

        if (tyDepth > 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 = tvPnt;
            ptgContact->m_tvNormal = tgAxS.m_tvNormal;
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tyDepth;

            ++ptgPacket->m_niContact;
        };
    };

    tvX0 = MATH::F_MUL( -TYPE(0.5), MATH::F_SUB( tvCYB0, tvCYB1 ) );
    tvK7 = MATH::F_MUL( tvX0, tgCY0.Query_Radius() );

    if (0 == iFlag || TTgCLP_BXLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgBX0,iFlag, tvCentreW, tvK7 ) >= 0 )
    {
        T_(VECTOR,DIM)                      tvPnt;

        tvPnt = MATH::F_ADD( tvCentreW, MATH::F_MUL( tvX0, (0 == iFlag ? tgCY0.Query_Radius() : tyT1) ) );
        tyDepth = tyPlnD - MATH::F_DOT(tvPnt,tgAxS.m_tvNormal);

        if (tyDepth > 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 = tvPnt;
            ptgContact->m_tvNormal = tgAxS.m_tvNormal;
            ptgContact->m_tyT0 = TYPE(0.0);
            ptgContact->m_tyDepth = tyDepth;

            ++ptgPacket->m_niContact;
        };
    };

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

template TgRESULT F_Contact_Penetrate_BoxAxis( PC_TgF4CONTACT_PACKET, CR_TgF4AXIS_RESULT, PC_TgBOOL, CR_TgF4BOX, 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:  tgBX0: Box 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_(BOX,DIM) tgBX0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ))
    TgASSERT(tgCY0.Is_Valid() && tgBX0.Is_Valid())

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

    const TYPE                          tyBE0_N = tgBX0.Query_Extent0()*MATH::F_DOT( tgBX0.Query_AxisUnit0(), tgAxS.m_tvNormal );
    const TYPE                          tyBE1_N = tgBX0.Query_Extent1()*MATH::F_DOT( tgBX0.Query_AxisUnit1(), tgAxS.m_tvNormal );
    const TYPE                          tyBE2_N = tgBX0.Query_Extent2()*MATH::F_DOT( tgBX0.Query_AxisUnit2(), tgAxS.m_tvNormal );

    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tgCY0.Query_Extent(), tgAxS.m_tvNormal );
    C_(VECTOR,DIM)                      tvCentreW = MATH::F_SUB( tgCY0.Query_Origin(), tvK0 );

    TYPE                                tyE0,tyE1,tyE2, tyDepth;

    if (P::ABS( tyBE0_N ) > P::ABS( tyBE1_N ))
    {
        if (P::ABS( tyBE1_N ) > P::ABS( tyBE2_N ))
        {
            tyE0 = P::FSEL( tyBE0_N, tgBX0.Query_Extent0(), -tgBX0.Query_Extent0() );
            tyE1 = P::FSEL( tyBE1_N, tgBX0.Query_Extent1(), -tgBX0.Query_Extent1() );
            tyE2 = P::FSEL( tyBE2_N, tgBX0.Query_Extent2(), -tgBX0.Query_Extent2() );
        }
        else if (P::ABS( tyBE0_N ) > P::ABS( tyBE2_N ))
        {
            tyE0 = P::FSEL( tyBE0_N, tgBX0.Query_Extent0(), -tgBX0.Query_Extent0() );
            tyE1 = P::FSEL( tyBE2_N, tgBX0.Query_Extent2(), -tgBX0.Query_Extent2() );
            tyE2 = P::FSEL( tyBE1_N, tgBX0.Query_Extent1(), -tgBX0.Query_Extent1() );
        }
        else
        {
            tyE0 = P::FSEL( tyBE2_N, tgBX0.Query_Extent2(), -tgBX0.Query_Extent2() );
            tyE1 = P::FSEL( tyBE0_N, tgBX0.Query_Extent0(), -tgBX0.Query_Extent0() );
            tyE2 = P::FSEL( tyBE1_N, tgBX0.Query_Extent1(), -tgBX0.Query_Extent1() );
        };
    }
    else
    {
        if (P::ABS( tyBE0_N ) > P::ABS( tyBE2_N ))
        {
            tyE0 = P::FSEL( tyBE1_N, tgBX0.Query_Extent1(), -tgBX0.Query_Extent1() );
            tyE1 = P::FSEL( tyBE0_N, tgBX0.Query_Extent0(), -tgBX0.Query_Extent0() );
            tyE2 = P::FSEL( tyBE2_N, tgBX0.Query_Extent2(), -tgBX0.Query_Extent2() );
        }
        else if (P::ABS( tyBE2_N ) > P::ABS( tyBE1_N ))
        {
            tyE0 = P::FSEL( tyBE2_N, tgBX0.Query_Extent2(), -tgBX0.Query_Extent2() );
            tyE1 = P::FSEL( tyBE1_N, tgBX0.Query_Extent1(), -tgBX0.Query_Extent1() );
            tyE2 = P::FSEL( tyBE0_N, tgBX0.Query_Extent0(), -tgBX0.Query_Extent0() );
        }
        else
        {
            tyE0 = P::FSEL( tyBE1_N, tgBX0.Query_Extent1(), -tgBX0.Query_Extent1() );
            tyE1 = P::FSEL( tyBE2_N, tgBX0.Query_Extent2(), -tgBX0.Query_Extent2() );
            tyE2 = P::FSEL( tyBE0_N, tgBX0.Query_Extent0(), -tgBX0.Query_Extent0() );
        };
    };

    TgINT                               iContained = 0;

    for (TgINT i0 = 0; i0 < 4; i0 += 3)
    {
        TTgSEGMENT<TYPE,DIM>                tgBE;

        // Check to see if the edge/segment origin is a valid contact point.

        tgBE.Set_Origin(tgBX0.Calc_Point( tyE0, i0 == 0 ? tyE1 : -tyE1, i0 == 0 ? tyE2 : -tyE2 ));
        tyDepth = MATH::F_DOT( MATH::F_SUB( tgBE.Query_Origin(), tvCentreW ), tgAxS.m_tvNormal );

        if (tyDepth >= TYPE(0.0) && tgCY0.Is_Contained( tgBE.Query_Origin() ))
        {
            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_SUB( tgBE.Query_Origin(), 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;
            iContained |= (1 << i0);
        };

        // Build the two edges originating at the above vertex.

        for (TgINT i1 = 1; i1 < 3; ++i1)
        {
            T_(VECTOR,DIM)                      tvEnd;

            tvEnd = tgBX0.Calc_Point( tyE0, i1 == 1 ? tyE1 : -tyE1, i1 == 1 ? -tyE2 : tyE2 );

            //  If the edge origin was contained then its possible the entire edge is completely contained.  Since a simple point
            // containment check is fast and easy to do - check to see if the other termination point is within the box - if so,
            // then add the contact and continue onto the next edge.

            if (0 != (iContained & (1 << i0)))
            {
                tyDepth = MATH::F_DOT( MATH::F_SUB( tvEnd, tvCentreW ), tgAxS.m_tvNormal );

                if (tyDepth >= TYPE(0.0) && tgCY0.Is_Contained( tvEnd ))
                {
                    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_SUB( tvEnd, 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;
                    iContained |= (1 << i1);

                    continue;
                };
            };

            //  F_Clip the resulting edge by the cylinder. This will result with either zero or two points.  Check the contained
            // bitfield to make sure that duplicate points (the vertices) are not added into the system.

            CR_(TUBE,DIM)                       tgTB0 = static_cast<CR_(TUBE,DIM)>(tgCY0);
            TTgPLANE<TYPE,DIM>                  tgCapPlane;
            TYPE                                tyT0,tyT1;
            T_(VECTOR,DIM)                      tvPnt;

            tgCapPlane.Set_Normal( tgAxS.m_tvNormal );
            tgCapPlane.Set_Constant( MATH::F_DOT(tgAxS.m_tvNormal,tvCentreW) );

            tgBE.Set_DirN( MATH::F_SUB( tvEnd, tgBE.Query_Origin() ) );

            if (TgFAILED(F_Clip( &tyT0,&tyT1, tgCapPlane, tgBE )))
            {
                // The edge is entirely on the non-negative side of the cap plane.  No intersection.  Process next edge.

                continue;
            };

            tgBE.Set_Origin( MATH::F_ADD( MATH::F_MUL( TYPE(1.0) - tyT0, tgBE.Query_Origin() ), MATH::F_MUL( tyT0, tvEnd ) ) );
            tgBE.Set_DirN( MATH::F_MUL( tyT1 - tyT0, tgBE.Query_DirN() ) );

            if (Near_Zero( tyT1 - tyT0 ) || TgFAILED(F_Clip( &tyT0,&tyT1, tgTB0, tgBE )))
            {
                // F_Clip was reduced to a point or lies outside of the infinite cylindrical space.  Process next edge.

                continue;
            };

            // Point #0 - 

            tvPnt = MATH::F_ADD( tgBE.Query_Origin(), MATH::F_MUL( tyT0, tgBE.Query_DirN() ) );
            tyDepth = MATH::F_DOT( MATH::F_SUB( tvPnt, tvCentreW ), tgAxS.m_tvNormal );

            if (tyDepth > TYPE(0.0) && !(Near_Zero( tyT0 ) && 0 != (iContained & !(1 << i0))))
            {
                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_SUB( tvPnt, 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;
            };

            // Point #1 - 

            tvPnt = MATH::F_ADD( tgBE.Query_Origin(), MATH::F_MUL( tyT1, tgBE.Query_DirN() ) );
            tyDepth = MATH::F_DOT( MATH::F_SUB( tvPnt, tvCentreW ), tgAxS.m_tvNormal );

            if (tyDepth > TYPE(0.0) && !Near_Zero( tyT0 - tyT1 ) && !(Near_One( tyT1 ) && 0 != (iContained & !(1 << i1))))
            {
                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_SUB( tvPnt, 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;
            };

            //  Floating point error can produce some interesting artifacts.  In this case, it is possible for the two end
            // points to be the result of the clipping process within a tolerance value.  So though the point check did not
            // add them as contained - they will be added here and flagged.

            iContained |= Near_Zero( tyT0 ) ? (1 << i0) : 0;
            iContained |= Near_One( tyT1 ) ? (1 << i1) : 0;
        };
    };

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

template TgRESULT F_Contact_Penetrate_CylAxis( PC_TgF4CONTACT_PACKET, CR_TgF4AXIS_RESULT, CR_TgF4BOX, CR_TgF4CYLINDER );


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

// ---- F_Contact_Penetrate_CylRad ---------------------------------------------------------------------------------------------- //
//  -- 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:  tgBX0: Box 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_CylRad( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(AXIS_RESULT,DIM) tgAxS, CR_(BOX,DIM) tgBX0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ));
    TgASSERT(tgCY0.Is_Valid() && tgBX0.Is_Valid());
    TgCOLLISION_ASSERT( !Near_Zero(TgCOLLISION_ASSERT*tgCYA.Query_AxisUnit()) );

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

    // Make sure there is no axial component in the normal direction to be used.

    const TYPE                          tyCY0_N = MATH::F_DOT( tgAxS.m_tvNormal, tgCY0.Query_BasisUnit0() );
    const TYPE                          tyCY1_N = MATH::F_DOT( tgAxS.m_tvNormal, tgCY0.Query_BasisUnit1() );
    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyCY0_N, tgCY0.Query_BasisUnit0() );
    C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyCY1_N, tgCY0.Query_BasisUnit1() );
    C_(VECTOR,DIM)                      tvRad = MATH::F_NORM( &tyLength, MATH::F_ADD( tvK0, tvK1 ) );

    if (Near_Zero( tyLength ))
    {
        return (TgE_FAIL);
    };

    C_(VECTOR,DIM)                      tvK2 = MATH::F_MUL( tgCY0.Query_Radius(), tvRad );
    C_(VECTOR,DIM)                      tvOrg = MATH::F_ADD( tgCY0.Query_Segment().Query_Origin(), tvK2 );
    TYPE                                tyT0,tyT1, tyDepth;
    T_(VECTOR,DIM)                      tvPlane_Point, tvPnt;

    if (TTgCLP_BXLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgBX0, tvOrg,tgCY0.Query_Segment().Query_DirN() ) < 0)
    {
        return (TgE_NOINTERSECT);
    };

    tvPlane_Point = tgBX0.Calc_Support_Point( MATH::F_NEG( tgAxS.m_tvNormal ) );

    tvPnt = MATH::F_ADD( tvOrg, MATH::F_MUL( tyT0, tgCY0.Query_Segment().Query_DirN() ) );
    tyDepth = MATH::F_DOT( MATH::F_SUB( tvPnt, tvPlane_Point ), tgAxS.m_tvNormal );

    if (tyDepth> TYPE(0.0))
    {
        ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

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

        ++ptgPacket->m_niContact;
    };

    tvPnt = MATH::F_ADD( tvOrg, MATH::F_MUL( tyT1, tgCY0.Query_Segment().Query_DirN() ) );
    tyDepth = MATH::F_DOT( MATH::F_SUB( tvPnt, tvPlane_Point ), tgAxS.m_tvNormal );

    if (tyDepth> 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 = tvPnt;
        ptgContact->m_tvNormal = tgAxS.m_tvNormal;
        ptgContact->m_tyT0 = TYPE(0.0);
        ptgContact->m_tyDepth = tyDepth;

        ++ptgPacket->m_niContact;
    };

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

template TgRESULT F_Contact_Penetrate_CylRad( PC_TgF4CONTACT_PACKET, CR_TgF4AXIS_RESULT, CR_TgF4BOX, CR_TgF4CYLINDER );


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

// ---- F_Contact_Penetrate ----------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgBX0: Box 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_(BOX,DIM) tgBX0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgBOX<<16)|(TgUINT)ETgCYLINDER));
    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ));
    TgASSERT(tgCY0.Is_Valid() && tgBX0.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.

    TgBOOL                              bIsContained[4];
    TTgAXIS_RESULT<TYPE,DIM>            tgAxS;

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

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

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

    switch (tgAxS.m_iAxis)
    {
        case 0:
        case 1: 
        case 2: return (F_Contact_Penetrate_BoxAxis( ptgPacket, tgAxS, bIsContained, tgBX0, tgCY0 ));

        case 3: return (F_Contact_Penetrate_CylAxis( ptgPacket, tgAxS, tgBX0, tgCY0 ));

        case 4: return (F_Contact_Penetrate_CylRad( ptgPacket, tgAxS, tgBX0, tgCY0 ));
    };

    return (TgE_FAIL);
};

template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4BOX, CR_TgF4CYLINDER );


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

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