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

    //  To figure out all the possible axes of separation, it is necessary only to imagine the infinite set of vectors that are
    // orthogonal to the cylinder surface.  Once conceived, it becomes obvious to separate the cylinder into three conceptual
    // parts.  The body is the main tube section of the cylinder.  The caps are the planar terminal part of the cylinder and the
    // rims are where the planar terminal part of the cylinder and the tube intersect.  The body and the caps have obvious and
    // well defined orthogonal vectors sets.  In the case of the caps, the set has only one member - the normal defining the plane.
    // For the body, the set is infinite - defined as those vectors in the cylinder cross-sectional plane that are directed
    // radially.  The rim set is composed of a locus forming a quarter torus.  Most of these axes are invalid, and its a matter
    // of choosing the right ones - and preferably a finite set in computational amount of time.  The correct axes can be found
    // using the following methods:
    // 
    // Cap-Rim      All cases should be found by the axial test
    // Cap-Cap      All cases should be found by the axial test
    // Cap-Body     All cases should be found by the axial test, if not the minimal distance vector will catch it.
    // 
    // Rim-Body     Minimal distance vector, projected onto the cylinder plane, will find all such cases.
    // Rim-Rim      The intersection of the two sets of normal rim vectors should be unique and is the test case.
    // 
    // Body-Body    Minimal distance vector will find all such cases.
    // 

    TYPE                                tyTest;

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

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

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

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

    const TYPE                          tyABS_A0_A1 = P::ABS( MATH::F_DOT(tgCY0.Query_AxisUnit(),tgCY1.Query_AxisUnit()) );
    const TYPE                          tyABS_A0xA1 = P::SQRT( TYPE(1.0) - MIN( TYPE(1.0), tyABS_A0_A1*tyABS_A0_A1 ) );

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

    // -- Axis: Cylinder #0 Axis -------------------------------

    tyTest = (tgCY0.Query_Extent() + tgCY1.Query_Radius()*tyABS_A0xA1 + tgCY1.Query_Extent()*tyABS_A0_A1) - P::ABS( tyDS_A0 );

    if (tyTest <= TYPE(0.0))
    {
        return (TgFALSE);
    };

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

    // == Cylinder Parallel Test =============================================================

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

    if (Near_One( tyABS_A0_A1 ) || Near_Zero( tyABS_A0xA1 ))
    {
        // -- Axis: Perpendicular to Common Cylinder Axis ----------

        tyTest = MATH::F_LSQ( tvDS ) - tyDS_A0*tyDS_A0;
        
        if (tyTest > tyRadSum*tyRadSum)
        {
            return (TgFALSE);
        };
        
        tyTest = tyRadSum - P::SQRT( tyTest );
        
        if (tyTest < ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvNormal = MATH::F_SUB( tvDS, MATH::F_MUL( tyDS_A0, tgCY0.Query_AxisUnit() ) );
            ptgAxS->m_tyDepth = tyTest;
            ptgAxS->m_iAxis = 4;

            MATH::F_NORM( ptgAxS->m_tvNormal );
        };

        return (TgTRUE);
    };

    // -- Axis: Cylinder #1 Axis -------------------------------

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

    if (tyTest <= TYPE(0.0))
    {
        return (TgFALSE);
    };

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

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

    CR_(SEGMENT,DIM)                    tgSG_CY0 = tgCY0.Query_Segment();
    CR_(SEGMENT,DIM)                    tgSG_CY1 = tgCY1.Query_Segment();

    TYPE                                tyCA0,tyCA1;
    TYPE                                tyT0,tyT1, tyT2,tyT3;

    const TYPE                          tyDistSq = F_ClosestSq( &tyCA0,&tyCA1, tgSG_CY0, tgSG_CY1 );
    
    if (tyDistSq > tyRadSum*tyRadSum)
    {
        return (TgFALSE);
    };
    
    C_(VECTOR,DIM)                      tvK14 = MATH::F_MUL( tyCA0, tgSG_CY0.Query_DirN() );
    C_(VECTOR,DIM)                      tvK15 = MATH::F_MUL( tyCA1, tgSG_CY1.Query_DirN() );
    C_(VECTOR,DIM)                      tvMin_CY0 = MATH::F_ADD( tgSG_CY0.Query_Origin(), tvK14 );
    C_(VECTOR,DIM)                      tvMin_CY1 = MATH::F_ADD( tgSG_CY1.Query_Origin(), tvK15 );
    C_(VECTOR,DIM)                      tvMinDist = MATH::F_SUB( tvMin_CY1, tvMin_CY0 );
    C_(VECTOR,DIM)                      tvMinDirN = MATH::F_SUB( tvMin_CY1, tvMin_CY0 );

    MATH::F_NORM( tvMinDirN );

    C_TgBOOL                            bCapContact0 = Near_One( tyCA0 ) || Near_Zero( tyCA0 );
    C_TgBOOL                            bCapContact1 = Near_One( tyCA1 ) || Near_Zero( tyCA1 );

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

    if (!bCapContact0 && !bCapContact1)
    {
        tgCY0.Project( &tyT0,&tyT1, tvMinDirN );
        tgCY1.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 = MATH::F_SUB( tvMin_CY1, MATH::F_MUL( tgCY1.Query_Radius(), tvMinDirN ) );
            ptgAxS->m_tvNormal = tvMinDirN;
            ptgAxS->m_tyDepth = tyTest;
            ptgAxS->m_iAxis = 5;
        };
    };

    if (bCapContact0) // If at a cylinder termini, a better axis may be the minimal direction projected on the cylinder plane.
    {
        // -- Axis: Perpendicular to Axis of Cylinder 0 ------------
        // Example: Rim to Cylinder contact.

        const TYPE                          tyK00 = MATH::F_DOT( tvMinDist, tgCY0.Query_BasisUnit0() );
        const TYPE                          tyK01 = MATH::F_DOT( tvMinDist, tgCY0.Query_BasisUnit1() );
        C_(VECTOR,DIM)                      tvK00 = MATH::F_MUL( tyK00, tgCY0.Query_BasisUnit0() );
        C_(VECTOR,DIM)                      tvK01 = MATH::F_MUL( tyK01, tgCY0.Query_BasisUnit1() );
        C_(VECTOR,DIM)                      tvX00 = MATH::F_NORM( MATH::F_ADD( tvK00, tvK01 ) );

        const TYPE                          tyABS_Ax_A1 = P::ABS( MATH::F_DOT( tvX00, tgCY1.Query_AxisUnit() ) );
        const TYPE                          tyAx_C10 = MATH::F_DOT( tvX00, tgCY1.Query_BasisUnit0() );
        const TYPE                          tyAx_C11 = MATH::F_DOT( tvX00, tgCY1.Query_BasisUnit1() );

        const TYPE                          tyK02 = P::SQRT( tyAx_C10*tyAx_C10 + tyAx_C11*tyAx_C11 );
        const TYPE                          tyK03 = tgCY0.Query_Radius() + tyABS_Ax_A1*tgCY1.Query_Extent();
        const TYPE                          tyK04 = (tyK03 + tyK02*tgCY1.Query_Radius()) - P::ABS( MATH::F_DOT( tvX00, tvDS ) );

        if (tyK04 <= TYPE(0.0))
        {
            return (TgFALSE);
        };

        if (tyK04 < ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvNormal = tvX00;
            ptgAxS->m_tyDepth = tyK04;
            ptgAxS->m_iAxis = 6;
        };
    };

    if (bCapContact1) // If at a cylinder termini, a better axis may be the minimal direction projected on the cylinder plane.
    {
        // -- Axis: Perpendicular to Axis of Cylinder 1 ------------
        // Example: Rim to Cylinder contact.

        const TYPE                          tyK06 = MATH::F_DOT( tvMinDist, tgCY1.Query_BasisUnit0() );
        const TYPE                          tyK07 = MATH::F_DOT( tvMinDist, tgCY1.Query_BasisUnit1() );
        C_(VECTOR,DIM)                      tvK06 = MATH::F_MUL( tyK06, tgCY1.Query_BasisUnit0() );
        C_(VECTOR,DIM)                      tvK07 = MATH::F_MUL( tyK07, tgCY1.Query_BasisUnit1() );
        C_(VECTOR,DIM)                      tvX10 = MATH::F_NORM( MATH::F_ADD( tvK06, tvK07 ) );

        const TYPE                          tyABS_Ax_A0 = P::ABS( MATH::F_DOT( tvX10, tgCY0.Query_AxisUnit() ) );
        const TYPE                          tyAx_C00 = MATH::F_DOT( tvX10, tgCY0.Query_BasisUnit0() );
        const TYPE                          tyAx_C01 = MATH::F_DOT( tvX10, tgCY0.Query_BasisUnit1() );

        const TYPE                          tyK08 = P::SQRT( tyAx_C00*tyAx_C00 + tyAx_C01*tyAx_C01 );
        const TYPE                          tyK09 = tgCY1.Query_Radius() + tyABS_Ax_A0*tgCY0.Query_Extent();
        const TYPE                          tyK10 = (tyK09 + tyK08*tgCY1.Query_Radius()) - P::ABS( MATH::F_DOT( tvX10, tvDS ) );

        if (tyK10 <= TYPE(0.0))
        {
            return (TgFALSE);
        };

        if (tyK10 < ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvNormal = tvX10;
            ptgAxS->m_tyDepth = tyK10;
            ptgAxS->m_iAxis = 7;
        };
    };

    if (!bCapContact0 || !bCapContact1 || MATH::F_LSQ( tvDS ) < LIMITS<TYPE>::ROOTEPSILON)
    {
        return (TgTRUE);
    };

    // Construct the line of intersection between the two cylinder cap planes.

    const TYPE                          tyA0_A1 = MATH::F_DOT( tgCY0.Query_AxisUnit(), tgCY1.Query_AxisUnit() );
    const TYPE                          tyA = TYPE(1.0) - tyA0_A1*tyA0_A1;

    if (tyA <= LIMITS<TYPE>::ROOTEPSILON)
    {
        return (TgTRUE); //« Make sure the two planes are not co-planar.
    };

    const TYPE                          tyInvA = TYPE(1.0) / tyA;
    const TYPE                          tyD_CY0 = MATH::F_DOT( tgCY0.Query_AxisUnit(), tvMin_CY0 );
    const TYPE                          tyD_CY1 = MATH::F_DOT( tgCY1.Query_AxisUnit(), tvMin_CY1 );

    tyT0 = (tyD_CY0 - tyD_CY1*tyA0_A1) * tyInvA;
    tyT1 = (tyD_CY1 - tyD_CY0*tyA0_A1) * tyInvA;

    T_(VECTOR,DIM)                      tvD0, tvTest, tvT0,tvT1;

    // F_Clip the common line of intersection to the interior of the cylinder tube space.

    T_(LINE,DIM)                        tgLN0;

    tgLN0.Set_Origin( MATH::F_ADD( MATH::F_MUL( tyT0, tgCY0.Query_AxisUnit() ), MATH::F_MUL( tyT1, tgCY1.Query_AxisUnit() ) ) );
    tgLN0.Set_DirN( MATH::F_CX( tgCY0.Query_AxisUnit(), tgCY1.Query_AxisUnit() ) );

    // F_Clip the line to cylinder 0 and find the contact point

    F_Clip( &tyT0,&tyT1, static_cast<CR_(TUBE,DIM)>(tgCY0), tgLN0 );

    tvT0 = MATH::F_ADD( tgLN0.Query_Origin(), MATH::F_MUL( tyT0, tgLN0.Query_DirN() ) );

    if (MATH::F_LSQ( MATH::F_SUB( tvT0, tvMin_CY1 ) ) >= tgCY1.Query_RadiusSq())
    {
        tvT0 = MATH::F_ADD( tgLN0.Query_Origin(), MATH::F_MUL( tyT1, tgLN0.Query_DirN() ) );

        if (MATH::F_LSQ( MATH::F_SUB( tvT0, tvMin_CY1 ) ) >= tgCY1.Query_RadiusSq())
        {
            COUT_NOOBJ(
                WARNING, TgT("%-16.16s(%-48.48s): [CY][CY] Not 100% that this type of thing should happen - record for test case.\n"),
                TgT("Collision"), TgT("F_Axis_Seperation")
            );
            return (TgFALSE);
        };
    }
    else
    {
        tvTest = MATH::F_ADD( tgLN0.Query_Origin(), MATH::F_MUL( tyT1, tgLN0.Query_DirN() ) );

        if (MATH::F_LSQ( MATH::F_SUB( tvTest, tvMin_CY1 ) ) <= tgCY1.Query_RadiusSq())
        {
            // If the entire segment is contained the best axis will be axial.
            return (TgTRUE);
        };
    };

    // F_Clip the line to cylinder 1 and find the contact point

    F_Clip( &tyT0,&tyT1, static_cast<CR_(TUBE,DIM)>(tgCY1), tgLN0 );

    tvT1 = MATH::F_ADD( tgLN0.Query_Origin(), MATH::F_MUL( tyT0, tgLN0.Query_DirN() ) );

    if (MATH::F_LSQ( MATH::F_SUB( tvT1, tvMin_CY1 ) ) >= tgCY0.Query_RadiusSq())
    {
        tvT1 = MATH::F_ADD( tgLN0.Query_Origin(), MATH::F_MUL( tyT1, tgLN0.Query_DirN() ) );

        if (MATH::F_LSQ( MATH::F_SUB( tvT1, tvMin_CY1 ) ) >= tgCY0.Query_RadiusSq())
        {
            COUT_NOOBJ(
                WARNING, TgT("%-16.16s(%-48.48s): [CY][CY] Not 100% that this type of thing should happen - record for test case.\n"),
                TgT("Collision"), TgT("F_Axis_Seperation")
            );
            return (TgFALSE);
        };
    }
    else
    {
        tvTest = MATH::F_ADD( tgLN0.Query_Origin(), MATH::F_MUL( tyT1, tgLN0.Query_DirN() ) );

        if (MATH::F_LSQ( MATH::F_SUB( tvTest, tvMin_CY1 ) ) <= tgCY0.Query_RadiusSq())
        {
            // If the entire segment is contained the best axis will be axial.
            return (TgTRUE);
        };
    };

    T_(VECTOR,DIM)                      tvD1;

    // Construct the tangent vectors, create a mutual orthogonal vector (cross-product) - this is the separating plane normal.
    // Essentially the construction is finding the (possibly unique) vector that exists in the intersection of the two sets
    // of possible rim normals.

    tvD0 = MATH::F_CX( MATH::F_SUB( tvT0, tvMin_CY0 ), tgCY0.Query_AxisUnit() ); //« Tangent vector at rim on Cylinder 0
    tvD1 = MATH::F_CX( MATH::F_SUB( tvT0, tvMin_CY1 ), tgCY1.Query_AxisUnit() ); //« Tangent vector at rim on Cylinder 1

    if (MATH::F_UCX( &tvTest, tvD0,tvD1 ) > LIMITS<TYPE>::ROOTEPSILON)
    {
        tgCY0.Project( &tyT0,&tyT1, tvTest );
        tgCY1.Project( &tyT2,&tyT3, tvTest );

        tyT1 -= tyT2;
        tyT3 -= tyT0;

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

        tyTest = -TgMIN( tyT1, tyT3 );

        if (tyTest < ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvPoint = tvT0;
            ptgAxS->m_tvNormal = MATH::F_MUL( P::FSEL( tyT1 - tyT3, TYPE(-1.0), TYPE(1.0) ), tvTest );
            ptgAxS->m_tyDepth = tyTest;
            ptgAxS->m_iAxis = 8;
        };
    };

    tvD0 = MATH::F_CX( MATH::F_SUB( tvT1, tvMin_CY0 ), tgCY0.Query_AxisUnit() ); //« Tangent vector at rim on Cylinder 0
    tvD1 = MATH::F_CX( MATH::F_SUB( tvT1, tvMin_CY1 ), tgCY1.Query_AxisUnit() ); //« Tangent vector at rim on Cylinder 1

    if (MATH::F_UCX( &tvTest, tvD0,tvD1 ) > LIMITS<TYPE>::ROOTEPSILON)
    {
        tgCY0.Project( &tyT0,&tyT1, tvTest );
        tgCY1.Project( &tyT2,&tyT3, tvTest );

        tyT1 -= tyT2;
        tyT3 -= tyT0;

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

        tyTest = -TgMIN( tyT1, tyT3 );

        if (tyTest < ptgAxS->m_tyDepth)
        {
            ptgAxS->m_tvPoint = tvT1;
            ptgAxS->m_tvNormal = MATH::F_MUL( P::FSEL( tyT1 - tyT3, TYPE(-1.0), TYPE(1.0) ), tvTest );
            ptgAxS->m_tyDepth = tyTest;
            ptgAxS->m_iAxis = 8;
        };
    };

    return (TgTRUE);
};

template TgBOOL F_Axis_Seperation( PC_TgF4AXIS_RESULT, CR_TgF4CYLINDER, CR_TgF4CYLINDER );


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

// ---- F_Internal_CapCap ------------------------------------------------------------------------------------------------------- //
//  -- Internal Function -- (Helper to create contact points between caps of two cylinders)
// Input:  tvNormal: The axis of contact being used.
// Input:  tgEL0: The Elliptical projection of the cylinder cap onto the plane defined by the axis of contact
// Input:  tgCY0: Cylinder primitive
// Input:  tgCY1: Cylinder primitive - contact points are generated on this primitive
// Output: aF_Contact: An array of contact points to return for processing that will project cylinder 1 out of cylinder 0.
// Output: nuiPoint: The number of valid points in the array
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM> TgFORCEINLINE
TgRESULT F_Internal_CapCap(
    T_(CONTACT,DIM) aF_Contact[4], TgUINT32& nuiPoint, M_(VECTOR,DIM) tvNormal, CR_(ELLIPSE,DIM) tgEL1, CR_(CYLINDER,DIM) tgCY0, CR_(CYLINDER,DIM) tgCY1
) {
    const TYPE                          tyDN0 = P::FSEL( MATH::F_DOT(tvNormal,tgCY0.Query_AxisUnit()), TYPE(1.0), TYPE(-1.0) );
    const TYPE                          tyDN1 = P::FSEL( MATH::F_DOT(tvNormal,tgCY1.Query_AxisUnit()), TYPE(-1.0), TYPE(1.0) );
    C_(VECTOR,DIM)                      tvC0 = MATH::F_ADD( tgCY0.Query_Origin(), MATH::F_MUL( tyDN0, tgCY0.Query_HalfAxis() ) );
    C_(VECTOR,DIM)                      tvC1 = MATH::F_ADD( tgCY1.Query_Origin(), MATH::F_MUL( tyDN1, tgCY1.Query_HalfAxis() ) );

    T_(VECTOR,DIM)                      tvX10,tvX11, tvTest;
    TYPE                                tyT0,tyT1, tyTest;

    nuiPoint = 0;

    // Create a reference frame for cylinder one based on the direction of deepest penetration.  To determine this reference frame
    // the primary axis of the second cylinder is projectd down onto the cross-sectional plane of the first cylinder. It is the
    // angle of the second cylinder that will give us the direction of maximum penetration.  Once that has been determined that
    // direction now has to be taken back into the second cylinder's reference frame to determine the result basis.

    const TYPE                          tyK0 = -tyDN1*MATH::F_DOT( tgCY0.Query_BasisUnit0(), tgCY1.Query_AxisUnit() );
    const TYPE                          tyK1 = -tyDN1*MATH::F_DOT( tgCY0.Query_BasisUnit1(), tgCY1.Query_AxisUnit() );
    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyK0, tgCY0.Query_BasisUnit0() );
    C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyK1, tgCY0.Query_BasisUnit1() );

    tvX10 = MATH::F_NORM( &tyTest, MATH::F_ADD( tvK0, tvK1 ) );

    if (tyTest <= LIMITS<TYPE>::EPSILON)
    {
        tvX10 = tgCY1.Query_BasisUnit0();
        tvX11 = tgCY1.Query_BasisUnit1();
    }
    else
    {
        MATH::F_UCX( &tvX11, tvX10, tgCY1.Query_AxisUnit() );
        MATH::F_UCX( &tvX10, tvX11, tgCY1.Query_AxisUnit() );
    };

    // Use the reference frame of cylinder 1 to create the point of deepest penetration.

    tvTest = MATH::F_MUL( tgCY1.Query_Radius(), tvX10 );

    if (TTgCLP_CYLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgCY0, MATH::F_SUB( tvC1, tvTest ), MATH::F_MUL( TYPE(2.0), tvTest ) ) >= 0)
    {
        tvTest = MATH::F_ADD( tvC1, MATH::F_MUL( TYPE(2.0)*tyT1 - TYPE(1.0), tvTest ) );
        tyTest = MATH::F_DOT( MATH::F_SUB( tvC0, tvTest ), tvNormal );

        if (tyTest > TYPE(0.0))
        {
            aF_Contact[nuiPoint].m_tyDepth = tyTest;
            aF_Contact[nuiPoint++].m_tvPos = tvTest;
        };
    };

    //  At issue is the fact that the elliptical intersection routine requires the solution of a quartic equation which are known
    // to be very unstable.  Specifically, as the ellipse ratios become closer to that of a full circle the intersection routine
    // will quickly become degenerate.  Therefore, a heuristic value to determine when to use the elliptical routine over assuming
    // that both caps are co-planar has to be used.  Keep in mind the the math will be distinctly less prone to these problems on
    // the 64bit Power architectures used in the Xbox360 and PS3.  However, the calculations will have to be kept in the floating
    // point unit and not vectorized.

    T_(VECTOR,DIM)                      tvT0, tvT1;
    const TYPE                          tyABS_A0_A1 = P::ABS( MATH::F_DOT( tgCY0.Query_AxisUnit(), tgCY1.Query_AxisUnit() ) );
    TgRESULT                            tgResult = TgE_NOINTERSECT;

    if (tyABS_A0_A1 >= KF32_SQRT1_2)
    {
        if (Near_Zero( tgEL1.Query_Major_Radius() - tgEL1.Query_Minor_Radius() ))
        {
            // Use the circle routine

            T_(CIRCLE,DIM)                      tgCI0, tgCI1;

            tgCI0.Set( tgCY0.Query_BasisUnit0(), tgCY0.Query_AxisUnit(), tgCY0.Query_BasisUnit1(), tvC0, tgCY0.Query_Radius() );
            tgCI1.Set( tgCY1.Query_BasisUnit0(), tgCY1.Query_AxisUnit(), tgCY1.Query_BasisUnit1(), tvC1, tgCY1.Query_Radius() );

            tgResult = F_Contact_Intersect2D( &tvT0,&tvT1, tgCI0,tgCI1 );
        }
        else
        {
            // Use the elliptical routine

            T_(ELLIPSE,DIM)                     tgEL0;

            tgEL0.Set(
                tvC0, tgCY0.Query_BasisUnit0(), tgCY0.Query_AxisUnit(), tgCY0.Query_BasisUnit1(),
                tgCY0.Query_Radius(), tgCY0.Query_Radius()
            );

            tgResult = F_Contact_Intersect2D( &tvT0,&tvT1, tgEL0,tgEL1 );
        };
    };

    if (TgE_NOINTERSECT == tgResult)
    {
        // Used the reference frame of cylinder 0 to create three symmetrical contacts around the rim/base.

        C_(VECTOR,DIM)                      tvK0 = MATH::F_ADD( tvX10, MATH::F_MUL( KF32_SQRT3, tvX11 ) );

        tvTest = MATH::F_MUL( tgCY1.Query_Radius()*TYPE(-0.5), tvK0 );

        if (TTgCLP_CYLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgCY0, tvC1,tvTest ) >= 0)
        {
            tvTest = MATH::F_ADD( tvC1, MATH::F_MUL( tyT1, tvTest ) );
            tyTest = MATH::F_DOT( MATH::F_SUB( tvC0, tvTest ), tvNormal );

            if (tyTest > TYPE(0.0))
            {
                aF_Contact[nuiPoint].m_tyDepth = tyTest;
                aF_Contact[nuiPoint++].m_tvPos = tvTest;
            };
        };

        C_(VECTOR,DIM)                      tvK1 = MATH::F_SUB( tvX10, MATH::F_MUL( KF32_SQRT3, tvX11 ) );

        tvTest = MATH::F_MUL( tgCY1.Query_Radius()*TYPE(-0.5), tvK1 );

        if (TTgCLP_CYLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgCY0, tvC1,tvTest ) >= 0)
        {
            tvTest = MATH::F_ADD( tvC1, MATH::F_MUL( tyT1, tvTest ) );
            tyTest = MATH::F_DOT( MATH::F_SUB( tvC0, tvTest ), tvNormal );

            if (tyTest > TYPE(0.0))
            {
                aF_Contact[nuiPoint].m_tyDepth = tyTest;
                aF_Contact[nuiPoint++].m_tvPos = tvTest;
            };
        };

        return (0 != nuiPoint ? TgS_OK : TgE_NOINTERSECT);
    }

    if (tgResult < 0)
    {
        TgASSERT_MSG( TgFALSE, TgT("F_Internal_CapCap: No intersection points found between the two caps.") );
        return (TgE_FAIL);
    };

    // Use tvTo,tvT1, and bisect them and create points on other side of the intersection space.

    const TYPE                          tyFactor = -tyDN1 / tyABS_A0_A1;

    tyTest = tyFactor*(MATH::F_DOT( MATH::F_SUB( tvT0, tvC1 ), tgCY1.Query_AxisUnit() ));

    if (tyTest > TYPE(0.0))
    {
        aF_Contact[nuiPoint].m_tyDepth = tyTest;
        aF_Contact[nuiPoint++].m_tvPos = MATH::F_SUB( tvT0, MATH::F_MUL( tyDN0*tyTest, tgCY0.Query_AxisUnit() ) );
    };

    tyTest = tyFactor*(MATH::F_DOT( MATH::F_SUB( tvT1, tvC1 ), tgCY1.Query_AxisUnit() ));

    if (tyTest > TYPE(0.0))
    {
        aF_Contact[nuiPoint].m_tyDepth = tyTest;
        aF_Contact[nuiPoint++].m_tvPos = MATH::F_SUB( tvT1, MATH::F_MUL( tyDN0*tyTest, tgCY0.Query_AxisUnit() ) );
    };

    tvTest = MATH::F_NORM( MATH::F_SUB( MATH::F_MUL( TYPE(0.5), MATH::F_ADD( tvT0, tvT1 ) ), tvC1 ) );
    tvTest = MATH::F_ADD( MATH::F_MUL( tvTest, tgCY1.Query_Radius() ), tvC1 );
    tyTest = MATH::F_DOT( MATH::F_SUB( tvTest, tvC1 ), tgCY1.Query_AxisUnit() )*tyFactor;

    if (tyTest > TYPE(0.0))
    {
        aF_Contact[nuiPoint].m_tyDepth = tyTest;
        aF_Contact[nuiPoint++].m_tvPos = MATH::F_SUB( tvTest, MATH::F_MUL( tyDN0*tyTest, tgCY0.Query_AxisUnit() ) );
    };

    return (TgS_OK);
};


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

// ---- F_Internal_CapContained ------------------------------------------------------------------------------------------------- //
//  -- Internal Function -- (Helper to create contact points between caps of two cylinders, given one is entirely contained)
// Input:  tvNormal: The axis of contact being used.
// Input:  tgCY0: Cylinder primitive
// Input:  tgCY1: Cylinder primitive - contact points are generated on this primitive
// Output: aF_Contact: An array of contact points to return for processing
// Output: nuiPoint: The number of valid points in the array
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM> TgFORCEINLINE
TgRESULT F_Internal_CapContained( // Cylinder 1 is known to be nested inside cylinder 0
    T_(CONTACT,DIM) aF_Contact[4], TgUINT32 &nuiPoint, M_(VECTOR,DIM) tvNormal, CR_(CYLINDER,DIM) tgCY0, CR_(CYLINDER,DIM) tgCY1
) {
    const TYPE                          tyDN0 = P::FSEL( MATH::F_DOT(tvNormal,tgCY0.Query_AxisUnit()), TYPE(1.0), TYPE(-1.0) );
    const TYPE                          tyDN1 = P::FSEL( MATH::F_DOT(tvNormal,tgCY1.Query_AxisUnit()), TYPE(-1.0), TYPE(1.0) );
    C_(VECTOR,DIM)                      tvC0 = MATH::F_ADD( tgCY0.Query_Origin(), MATH::F_MUL( tyDN0, tgCY0.Query_HalfAxis() ) );
    C_(VECTOR,DIM)                      tvC1 = MATH::F_ADD( tgCY1.Query_Origin(), MATH::F_MUL( tyDN1, tgCY1.Query_HalfAxis() ) );

    T_(VECTOR,DIM)                      tvX10,tvX11, tvTest;
    TYPE                                tyTest;

    // Create a reference frame for cylinder one based on the direction of deepest penetration.  To determine this reference frame
    // the primary axis of the first cylinder is projectd down onto the cross-sectional plane of the second cylinder.

    const TYPE                          tyK0 = -tyDN1*MATH::F_DOT( tgCY0.Query_BasisUnit0(), tgCY1.Query_AxisUnit() );
    const TYPE                          tyK1 = -tyDN1*MATH::F_DOT( tgCY0.Query_BasisUnit1(), tgCY1.Query_AxisUnit() );
    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyK0, tgCY0.Query_BasisUnit0() );
    C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyK1, tgCY0.Query_BasisUnit1() );

    tvX10 = MATH::F_NORM( &tyTest, MATH::F_ADD( tvK0, tvK1 ) );

    if (tyTest <= LIMITS<TYPE>::EPSILON)
    {
        tvX10 = tgCY1.Query_BasisUnit0();
        tvX11 = tgCY1.Query_BasisUnit1();
    }
    else
    {
        MATH::F_UCX( &tvX11, tvX10, tgCY1.Query_AxisUnit() );
    };

    // Used the reference frame of cylinder 1 to create three symmetrical contacts around the rim/base.

    tvTest = MATH::F_ADD( tvC1,  MATH::F_MUL( tgCY1.Query_Radius(), tvX10 ) );
    tyTest = MATH::F_DOT( MATH::F_SUB( tvC0, tvTest ), tvNormal );

    if (tyTest < TYPE(0.0))
    {
        TgASSERT_MSG( TgFALSE, TgT("F_Internal_CapContained: Axis determined a fully contained cylinder but found no contacts.") );
        return (TgE_FAIL);
    };

    aF_Contact[0].m_tyDepth = tyTest;
    aF_Contact[0].m_tvPos = tvTest;
    nuiPoint = 1;

    C_(VECTOR,DIM)                      tvK2 = MATH::F_ADD( tvX10, MATH::F_MUL( KF32_SQRT3, tvX11 ) );

    tvTest = MATH::F_ADD( tvC1, MATH::F_MUL( tgCY1.Query_Radius()*TYPE(-0.5), tvK2 ) );
    tyTest = MATH::F_DOT( MATH::F_SUB( tvC0, tvTest ), tvNormal );

    if (tyTest > TYPE(0.0))
    {
        aF_Contact[nuiPoint].m_tvPos = tvTest;
        aF_Contact[nuiPoint].m_tyDepth = tyTest;
        ++nuiPoint;
    };

    C_(VECTOR,DIM)                      tvK3 = MATH::F_SUB( tvX10, MATH::F_MUL( KF32_SQRT3, tvX11 ) );

    tvTest = MATH::F_ADD( tvC1, MATH::F_MUL( tgCY1.Query_Radius()*TYPE(-0.5), tvK3 ) );
    tyTest = MATH::F_DOT( MATH::F_SUB( tvC0, tvTest ), tvNormal );

    if (tyTest > TYPE(0.0))
    {
        aF_Contact[nuiPoint].m_tvPos = tvTest;
        aF_Contact[nuiPoint].m_tyDepth = tyTest;
        ++nuiPoint;
    };

    return (TgS_OK);
};


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

// ---- F_Internal_AxisCap ------------------------------------------------------------------------------------------------------ //
//  -- Internal Function -- (Helper to create contact points between the tube and cap of two cylinders)
// Input:  tvNormal: The axis of contact being used.
// Input:  tgCY0: Cylinder primitive
// Input:  tgCY1: Cylinder primitive - contact points are generated on this primitive
// Output: aF_Contact: An array of contact points to return for processing
// Output: nuiPoint: The number of valid points in the array
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM> TgFORCEINLINE
TgRESULT F_Internal_AxisCap(
    T_(CONTACT,DIM) aF_Contact[4], TgUINT32 &nuiPoint, C_(VECTOR,DIM) tvNormal, CR_(CYLINDER,DIM) tgCY0, CR_(CYLINDER,DIM) tgCY1
) {
    const TYPE                          tyDN0 = P::FSEL( MATH::F_DOT(tvNormal,tgCY0.Query_AxisUnit()), TYPE(-1.0), TYPE(1.0) );
    const TYPE                          tyDN1 = P::FSEL( MATH::F_DOT(tvNormal,tgCY1.Query_AxisUnit()), TYPE(1.0), TYPE(-1.0) );
    C_(VECTOR,DIM)                      tvC0 = MATH::F_ADD( tgCY0.Query_Origin(), MATH::F_MUL( tyDN0, tgCY0.Query_HalfAxis() ) );
    C_(VECTOR,DIM)                      tvC1 = MATH::F_ADD( tgCY1.Query_Origin(), MATH::F_MUL( tyDN1, tgCY1.Query_HalfAxis() ) );

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

    T_(VECTOR,DIM)                      tvX10,tvX11;
    TYPE                                tyT0,tyT1;

    // Create a reference frame for cylinder one based on the direction of deepest penetration.

    const TYPE                          tyK0 = -tyDN1*MATH::F_DOT( tgCY0.Query_BasisUnit0(), tgCY1.Query_AxisUnit() );
    const TYPE                          tyK1 = -tyDN1*MATH::F_DOT( tgCY0.Query_BasisUnit1(), tgCY1.Query_AxisUnit() );
    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyK0, tgCY0.Query_BasisUnit0() );
    C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyK1, tgCY0.Query_BasisUnit1() );

    tvX10 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK1 ) );
    MATH::F_UCX( &tvX11, tvX10, tgCY0.Query_AxisUnit() );
    TgASSERT_MSG( Near_One( MATH::F_LSQ( tvX10 ) ), TgT("F_Internal_AxisCap: Degenerate basis set formed.") );

    // Create a segment along the length of the cylinder, in the direction of deepest penetration.

    T_(VECTOR,DIM)                      tvS0 = MATH::F_ADD( tgSG_CY0.Query_Origin(), MATH::F_MUL( tgCY0.Query_Radius(), tvX10 ) );
    T_(VECTOR,DIM)                      tvD0 = tgSG_CY0.Query_DirN();

    if (TTgCLP_TULN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, static_cast<CR_(TUBE,DIM)>(tgCY1), tvS0, tvD0 ) < 0)
    {
        return (TgE_NOINTERSECT);
    };

    T_(PLANE,DIM)                       tgPN0;

    tgPN0.Set( tvNormal, tvC1 );

    tvS0 = MATH::F_MUL( tvS0, tyT0 );
    tvD0 = MATH::F_MUL( tvD0, tyT1 - tyT0 );

    if (TTgCLP_PNLN<TYPE,DIM,1,1>::DO( &tyT0,&tyT1, tgPN0, tvS0, tvD0 ) < 0)
    {
        return (TgE_NOINTERSECT);
    };

    // Segment was clipped to the cylinder surface.  Thus, the resulting point must be contained inside or on the cylinder.

    TYPE                                tyDH0, tyDH1;
    T_(VECTOR,DIM)                      tvT0, tvT1;

    tvT0 = MATH::F_ADD( tvS0, MATH::F_MUL( tyT0, tgSG_CY0.Query_DirN() ) );
    tyDH0 = MATH::F_DOT( MATH::F_SUB( tvT0, tvC1 ), tvNormal );

    tvT1 = MATH::F_ADD( tvS0, MATH::F_MUL( tyT1, tgSG_CY0.Query_DirN() ) );
    tyDH1 = MATH::F_DOT( MATH::F_SUB( tvT1, tvC1 ), tvNormal );

    aF_Contact[0].m_tvPos = tyDH0 - tyDH1 > TYPE(0.0) ? tvT0 : tvT1;
    aF_Contact[0].m_tyDepth = P::FSEL( tyDH0 - tyDH1, tyDH0, tyDH1 );
    aF_Contact[1].m_tvPos = tyDH0 - tyDH1 > TYPE(0.0) ? tvT1 : tvT0;
    aF_Contact[1].m_tyDepth = P::FSEL( tyDH0 - tyDH1, tyDH1, tyDH0 );

    // Always create the point of largest penetration but only use the second point if it would generate a counter-moment.

    nuiPoint = !Near_Zero( tyT0 - tyT1 ) && ((tyT0 - TYPE(0.5) < TYPE(0.0)) ^ (tyT1 - TYPE(0.5) < TYPE(0.0))) ? 2 : 1;

    return (TgS_OK);
};


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

// ---- F_Contact_Penetrate ----------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgCY0: Cylinder primitive
// Input:  tgCY1: 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_(CYLINDER,DIM) tgCY0, CR_(CYLINDER,DIM) tgCY1 )
{
    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ))
    TgASSERT(tgCY0.Is_Valid() && tgCY1.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.

    T_(AXIS_RESULT,DIM)                 tgAxS;

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

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

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

    P_(CONTACT,DIM)                     ptgContact;

    T_(CONTACT,DIM)                     aF_Contact[4];
    TgUINT32                            nuiPoint = 0;

    C_(VECTOR,DIM)                      tvDS = MATH::F_SUB( tgCY1.Query_Origin(), tgCY0.Query_Origin() );
    const TYPE                          tyDS_A0 = MATH::F_DOT(tvDS,tgCY0.Query_AxisUnit());
    const TYPE                          tyDS_A1 = MATH::F_DOT(tvDS,tgCY1.Query_AxisUnit());

    switch (tgAxS.m_iAxis)
    {
        case 0: { // -- Axis: Cylinder #0 Axis -------------------------------

            T_(ELLIPSE,DIM)                     tgEL1;

            if (Is_Cap_Contained( tgEL1, tgCY0,tyDS_A0,tgCY1,tyDS_A1 ))
            {
                if (P::ABS( MATH::F_DOT(tgCY0.Query_AxisUnit(),tgCY1.Query_AxisUnit()) ) < KF32_SQRT1_2)
                {
                    if (F_Internal_AxisCap( aF_Contact, nuiPoint, tgAxS.m_tvNormal, tgCY0,tgCY1 ) >= 0)
                    {
                        break;
                    };
                }
                else if (F_Internal_CapContained( aF_Contact, nuiPoint, tgAxS.m_tvNormal, tgCY0,tgCY1 ) >= 0)
                {
                    break;
                };
            }
            else
            {
                // The contact normal is the primary axis of cylinder zero.  Now we need to determine if the case to be generated
                // has the ends touching or an end and side touching.

                if (P::ABS( MATH::F_DOT(tgCY0.Query_AxisUnit(),tgCY1.Query_AxisUnit()) ) < KF32_SQRT1_2)
                {
                    if (F_Internal_AxisCap( aF_Contact, nuiPoint, tgAxS.m_tvNormal, tgCY0, tgCY1 ) >= 0)
                    {
                        break;
                    };
                }
                else if (F_Internal_CapCap( aF_Contact, nuiPoint, tgAxS.m_tvNormal, tgEL1, tgCY0,tgCY1 ) >= 0)
                {
                    break;
                };
            };

            return (TgE_NOINTERSECT);
        };

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

            T_(ELLIPSE,DIM)                     tgEL0;

            if (Is_Cap_Contained( tgEL0, tgCY1,-tyDS_A1,tgCY0,-tyDS_A0 ))
            {
                if (P::ABS( MATH::F_DOT(tgCY0.Query_AxisUnit(),tgCY1.Query_AxisUnit()) ) < KF32_SQRT1_2)
                {
                    C_TgRESULT tgResult = F_Internal_AxisCap( aF_Contact, nuiPoint, tgAxS.m_tvNormal, tgCY1,tgCY0 );

                    if (tgResult >= 0)
                    {
                        // Since we swapped the primitives and normal during the contact generation we now need to take the points and
                        // reverse them back out so they are being generated on the correect geometry in the correct direction.
                        for (TgUINT32 uiIndex = 0; uiIndex < nuiPoint; ++uiIndex)
                        {
                            C_(VECTOR,DIM) tvK20 = MATH::F_MUL( aF_Contact[uiIndex].m_tyDepth, tgAxS.m_tvNormal );
                            aF_Contact[uiIndex].m_tvPos = MATH::F_SUB( aF_Contact[uiIndex].m_tvPos, tvK20 );
                        };
                        break;
                    };
                }
                else
                {
                    C_TgRESULT tgResult = F_Internal_CapContained( aF_Contact, nuiPoint, tgAxS.m_tvNormal, tgCY1,tgCY0 );

                    if (tgResult >= 0)
                    {
                        // Since we swapped the primitives and normal during the contact generation we now need to take the points and
                        // reverse them back out so they are being generated on the correect geometry in the correct direction.
                        for (TgUINT32 uiIndex = 0; uiIndex < nuiPoint; ++uiIndex)
                        {
                            C_(VECTOR,DIM) tvK21 = MATH::F_MUL( aF_Contact[uiIndex].m_tyDepth, tgAxS.m_tvNormal );
                            aF_Contact[uiIndex].m_tvPos = MATH::F_ADD( aF_Contact[uiIndex].m_tvPos, tvK21 );
                        };
                        tgAxS.m_tvNormal = MATH::F_NEG( tgAxS.m_tvNormal );
                        break;
                    };
                };
            }
            else
            {
                if (P::ABS( MATH::F_DOT(tgCY0.Query_AxisUnit(),tgCY1.Query_AxisUnit()) ) < KF32_SQRT1_2)
                {
                    C_TgRESULT tgResult = F_Internal_AxisCap( aF_Contact, nuiPoint, tgAxS.m_tvNormal, tgCY0,tgCY1 );

                    if (tgResult >= 0)
                    {
                        // Since we swapped the primitives and normal during the contact generation we now need to take the points and
                        // reverse them back out so they are being generated on the correect geometry in the correct direction.
                        for (TgUINT32 uiIndex = 0; uiIndex < nuiPoint; ++uiIndex)
                        {
                            C_(VECTOR,DIM) tvK22 = MATH::F_MUL( aF_Contact[uiIndex].m_tyDepth, tgAxS.m_tvNormal );
                            aF_Contact[uiIndex].m_tvPos = MATH::F_SUB( aF_Contact[uiIndex].m_tvPos, tvK22 );
                        };
                        break;
                    };
                }

                C_TgRESULT tgResult = F_Internal_CapCap( aF_Contact, nuiPoint, tgAxS.m_tvNormal, tgEL0, tgCY0,tgCY1 );

                if (tgResult >= 0)
                {
                    // Since we swapped the primitives and normal during the contact generation we now need to take the points and
                    // reverse them back out so they are being generated on the correect geometry in the correct direction.
                    for (TgUINT32 uiIndex = 0; uiIndex < nuiPoint; ++uiIndex)
                    {
                        C_(VECTOR,DIM) tvK23 = MATH::F_MUL( aF_Contact[uiIndex].m_tyDepth, tgAxS.m_tvNormal );
                        aF_Contact[uiIndex].m_tvPos = MATH::F_SUB( aF_Contact[uiIndex].m_tvPos, tvK23 );
                    };
                    break;
                };
            };

            return (TgE_NOINTERSECT);
        };

        case 4: { // -- Axis: Perpendicular to Common Cylinder Axis ----------

            C_(VECTOR,DIM)                      tvDS = MATH::F_SUB( tgCY0.Query_Origin(), tgCY1.Query_Origin() );
            const TYPE                          tyDS_A1 = MATH::F_DOT( tvDS, tgCY1.Query_AxisUnit() );

            const TYPE                          tyMinY = TgMAX( -tgCY1.Query_Extent(), tyDS_A1 - tgCY0.Query_Extent() );
            const TYPE                          tyMaxY = TgMIN(  tgCY1.Query_Extent(), tyDS_A1 + tgCY0.Query_Extent() );
            P_(CONTACT,DIM)                     ptgContact;

            if (!Near_Zero(tyMinY - tyMaxY))
            {
                ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

                C_(VECTOR,DIM)                      tvK30 = MATH::F_MUL( tyMinY, tgCY1.Query_AxisUnit() );
                C_(VECTOR,DIM)                      tvK31 = MATH::F_MUL( tgCY1.Query_Radius(), tgAxS.m_tvNormal );

                ptgContact->m_tvPos = MATH::F_SUB( MATH::F_ADD( tgCY1.Query_Origin(), tvK30 ), tvK31 );
                ptgContact->m_tvNormal = tgAxS.m_tvNormal;
                ptgContact->m_tyT0 = TYPE(0.0);
                ptgContact->m_tyDepth = tgAxS.m_tyDepth;

                ++ptgPacket->m_niContact;

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

            C_(VECTOR,DIM)                      tvK32 = MATH::F_MUL( tyMaxY, tgCY1.Query_AxisUnit() );
            C_(VECTOR,DIM)                      tvK33 = MATH::F_MUL( tgCY1.Query_Radius(), tgAxS.m_tvNormal );

            ptgContact->m_tvPos = MATH::F_SUB( MATH::F_ADD( tgCY1.Query_Origin(), tvK32 ), tvK33 );
            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 8:   // -- Axis: Contact Between the Two Cylinder Rims ----------
        case 5: { // -- Axis: Between Points of Closest Proximity ------------

            //  Since this axis is only generated when the points of closest proximity do not lie at the termini of the respective
            // cylinder axes, the resulting direction vector is known to be orthogonal to both axes.  (Proof by Observation:
            // the minimal distance would be along a mutually perpendicular vector, which is known to exist since the lines are
            // also known to be skew.)

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

            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 6: { // -- Axis: Perpendicular to Axis of Cylinder 0 ------------

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

            ptgContact->m_tvPos = tgCY1.Calc_Support_Point( MATH::F_NEG( 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 7: { // -- Axis: Perpendicular to Axis of Cylinder 1 ------------

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

            C_(VECTOR,DIM)                      tvK34 = MATH::F_MUL( tgAxS.m_tyDepth, tgAxS.m_tvNormal );

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

            ++ptgPacket->m_niContact;

            return (TgS_OK);
        };
    };

    C_TgUINT32 nuiMax = TgMIN(nuiPoint,(TgUINT32)(ptgPacket->m_niMaxContact - ptgPacket->m_niContact));

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

        ptgContact->m_tvPos = aF_Contact[iIdx].m_tvPos;
        ptgContact->m_tvNormal = tgAxS.m_tvNormal;
        ptgContact->m_tyT0 = TYPE(0.0);
        ptgContact->m_tyDepth = aF_Contact[iIdx].m_tyDepth;

        ++ptgPacket->m_niContact;
    };

    return (nuiMax != nuiPoint ? TgS_MAXCONTACTS : TgS_OK);
};

template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4CYLINDER, CR_TgF4CYLINDER );


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

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