// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Cylinder-Linear.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_Internal_Intersect ---------------------------------------------------------------------------------------------------- //
//  -- Internal Function -- bC0, bC1 : Indicate the termination condition of the linear {bc0,bC1}
//
// Input:  tgCY0: Cylinder primitive
// Input:  tvS0,tvD0: Origin and Direction for Linear
// Output: tyLN0,tyLN1: Parametric parameter to generate the two points of the linear in contact with the cylinder surface
// Output: tvN0, tvN1: Cylinder surface normal at the points of contact between the two primitives
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM, bool bC0, bool bC1>
TgRESULT TTgINL_CYLN<TYPE,DIM,bC0,bC1>::DO(
    TYPE *ptyLN0, TYPE *ptyLN1, PC_(VECTOR,DIM) ptvN0, PC_(VECTOR,DIM) ptvN1, CR_(CYLINDER,DIM) tgCY0, M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvD0
) {
    TgASSERT( MATH::Is_Valid( tvS0 ) && MATH::Is_Valid( tvD0 ) )

    // Segment in the reference frame of the cylinder

    const TYPE                          tyD0_U0 = MATH::F_DOT( tvD0, tgCY0.Query_BasisUnit0() );
    const TYPE                          tyD0_U1 = MATH::F_DOT( tvD0, tgCY0.Query_BasisUnit1() );
    const TYPE                          tyD0_UA = MATH::F_DOT( tvD0, tgCY0.Query_AxisUnit() );

    const TYPE                          tyA = tyD0_U0*tyD0_U0 + tyD0_U1*tyD0_U1;

    if (tyA + tyD0_UA*tyD0_UA <= LIMITS<TYPE>::EPSILON)
    {
        return (TgE_FAIL);
    };

    // Relative position of the origin inside of the cylinder's reference frame.

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

    const TYPE                          tyDS_U0 = MATH::F_DOT( tvDS, tgCY0.Query_BasisUnit0() );
    const TYPE                          tyDS_U1 = MATH::F_DOT( tvDS, tgCY0.Query_BasisUnit1() );
    const TYPE                          tyDS_UA = MATH::F_DOT( tvDS, tgCY0.Query_AxisUnit() );

    // Relative distance of the origin on the cross-sectional plane of the cylinder.

    const TYPE                          tyRelSq = tyDS_U0*tyDS_U0 + tyDS_U1*tyDS_U1;

    if (bC0)
    {
        // If the origin lies outside of the cylinder and only moves away - intersection can not take place.

        if (P::ABS( tyDS_UA ) > tgCY0.Query_Extent() && !((tyDS_UA > TYPE(0.0)) ^ (tyD0_UA >= TYPE(0.0))))
        {
            return (TgE_NOINTERSECT);
        };

        // If the origin lies outside of the cylinder and only moves away - intersection can not take place.
        //  In the radial case moving away is determined by projecting the direction vector onto the difference vector after both
        // have been projected onto the cross-sectional plane.

        if (tyRelSq > tgCY0.Query_RadiusSq() && (tyDS_U0*tyD0_U0 + tyDS_U1*tyD0_U1) > TYPE(0.0))
        {
            return (TgE_NOINTERSECT);
        };
    };


    // Branch for the case where the segment is perpendicular to the cylinder's cross-sectional plane.

    if (Near_Zero( tyA )) 
    {
        if (tyRelSq >= tgCY0.Query_RadiusSq())
        {
            return (TgE_NOINTERSECT);
        };

        const TYPE                          tyInvTMP = TYPE(1.0) / tyD0_UA;
        const TYPE                          tyT0 = (-tgCY0.Query_Extent() - tyDS_UA) * tyInvTMP;
        const TYPE                          tyT1 = ( tgCY0.Query_Extent() - tyDS_UA) * tyInvTMP;

        if (tyT0 < tyT1)
        {
            *ptyLN0 = tyT0;
            *ptyLN1 = tyT1;
            *ptvN0 = MATH::F_NEG( tgCY0.Query_AxisUnit() );
            *ptvN1 = tgCY0.Query_AxisUnit();
        }
        else
        {
            *ptyLN0 = tyT1;
            *ptyLN1 = tyT0;
            *ptvN0 = tgCY0.Query_AxisUnit();
            *ptvN1 = MATH::F_NEG( tgCY0.Query_AxisUnit() );
        };

        return (TgS_OK);
    };


    // Branch for the case where the segment is parallel to the cylinder's cross-sectional plane.

    if (Near_Zero( tyD0_UA ))
    {
        // Check to see if the line lies outside of the cylinder's extents.

        if (P::ABS( tyDS_UA ) > tgCY0.Query_Extent())
        {
            return (TgE_NOINTERSECT);
        };

        //  The following assumes that D0_UA is exactly zero, instead of just being approximately close to it.  This allows for some
        // fast math, avoiding the entire need to project the segment onto the plane.  The minor error this may cause should be
        // negligible.

        // Solve the planar problem. (DS_U0 + ζ•D0_U0)² + (DS_U1 + ζ•D0_U1)² = R²
        // DS_U0•DS_U0 + 2•ζ•DS_U0•D0_U0 + ζ•ζ•D0_U0•D0_U0 + DS_U1•DS_U1 + 2•ζ•DS_U1•D0_U1 + ζ•ζ•D0_U1•D0_U1 = R²
        // ζ•ζ_(D0_U0•D0_U0 + D0_U1•D0_U1,DIM) + ζ_(2•DS_U0•D0_U0 + 2•DS_U1•D0_U1,DIM) + DS_U0•DS_U0 + DS_U1•DS_U1 - R² = 0

        const TYPE                          tyHalfNegB = TYPE(-1.0) * (tyDS_U0*tyD0_U0 + tyDS_U1*tyD0_U1);
        const TYPE                          tyC = tyRelSq - tgCY0.Query_RadiusSq();
        const TYPE                          tyDet = tyHalfNegB*tyHalfNegB - tyA*tyC;

        if (tyDet < TYPE(0.0))
        {
            return (TgE_NOINTERSECT);
        }

        const TYPE                          tyDetSqrt = P::SQRT( tyDet );
        const TYPE                          tyInvA = TYPE(1.0) / tyA;

        const TYPE                          tyT0 = (tyHalfNegB - tyDetSqrt) * tyInvA;
        const TYPE                          tyT1 = (tyHalfNegB + tyDetSqrt) * tyInvA;

        if (tyT0 < tyT1)
        {
            C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyDS_U0 + tyT0*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyDS_U1 + tyT0*tyD0_U1, tgCY0.Query_BasisUnit1() );
            C_(VECTOR,DIM)                      tvK2 = MATH::F_MUL( tyDS_U0 + tyT1*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK3 = MATH::F_MUL( tyDS_U1 + tyT1*tyD0_U1, tgCY0.Query_BasisUnit1() );

            *ptyLN0 = tyT0;
            *ptyLN1 = tyT1;
            *ptvN0 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK1 ) );
            *ptvN1 = MATH::F_NORM( MATH::F_ADD( tvK2, tvK3 ) );
        }
        else
        {
            C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyDS_U0 + tyT1*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyDS_U1 + tyT1*tyD0_U1, tgCY0.Query_BasisUnit1() );
            C_(VECTOR,DIM)                      tvK2 = MATH::F_MUL( tyDS_U0 + tyT0*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK3 = MATH::F_MUL( tyDS_U1 + tyT0*tyD0_U1, tgCY0.Query_BasisUnit1() );

            *ptyLN0 = tyT1;
            *ptyLN1 = tyT0;
            *ptvN0 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK1 ) );
            *ptvN1 = MATH::F_NORM( MATH::F_ADD( tvK2, tvK3 ) );
        };

        return (TgS_OK);
    };


    // For the sake of speed, attempt to clip the line using only the cap planes.

    const TYPE                          tyInvTMP = TYPE(1.0) / tyD0_UA;
    TgBOOL                              bCapClip = TgFALSE;

    TYPE                                tyT0 = TYPE();

    const TYPE                          tyPLN0 = (-tgCY0.Query_Extent() - tyDS_UA) * tyInvTMP;
    const TYPE                          tyPLN1 = ( tgCY0.Query_Extent() - tyDS_UA) * tyInvTMP;

    const TYPE                          tyU0 = tyDS_U0 + tyPLN0 * tyD0_U0;
    const TYPE                          tyV0 = tyDS_U1 + tyPLN0 * tyD0_U1;
    const TYPE                          tyU1 = tyDS_U0 + tyPLN1 * tyD0_U0;
    const TYPE                          tyV1 = tyDS_U1 + tyPLN1 * tyD0_U1;

    if (tyU0*tyU0 + tyV0*tyV0 <= tgCY0.Query_RadiusSq())
    {
        if (tyU1*tyU1 + tyV1*tyV1 <= tgCY0.Query_RadiusSq())
        {
            if (tyPLN0 < tyPLN1)
            {
                *ptyLN0 = tyPLN0;
                *ptyLN1 = tyPLN1;
                *ptvN0 = MATH::F_NEG( tgCY0.Query_AxisUnit() );
                *ptvN1 = tgCY0.Query_AxisUnit();
            }
            else
            {
                *ptyLN0 = tyPLN1;
                *ptyLN1 = tyPLN0;
                *ptvN0 = tgCY0.Query_AxisUnit();
                *ptvN1 = MATH::F_NEG( tgCY0.Query_AxisUnit() );
            };

            return (TgS_OK);
        }

        bCapClip = TgTRUE;

        tyT0 = tyPLN0;
        *ptvN0 = MATH::F_NEG( tgCY0.Query_AxisUnit() );
    }
    else if (tyU1*tyU1 + tyV1*tyV1 <= tgCY0.Query_RadiusSq())
    {
        bCapClip = TgTRUE;

        tyT0 = tyPLN1;
        *ptvN0 = tgCY0.Query_AxisUnit();
    };

    // The segment may intersect the cylinder walls
    
    // Solve the planar problem. (DS_U0 + ζ•D0_U0)² + (DS_U1 + ζ•D0_U1)² = R²
    // DS_U0•DS_U0 + 2•ζ•DS_U0•D0_U0 + ζ•ζ•D0_U0•D0_U0 + DS_U1•DS_U1 + 2•ζ•DS_U1•D0_U1 + ζ•ζ•D0_U1•D0_U1 = R²
    // ζ•ζ_(D0_U0•D0_U0 + D0_U1•D0_U1,DIM) + ζ_(2•DS_U0•D0_U0 + 2•DS_U1•D0_U1,DIM) + DS_U0•DS_U0 + DS_U1•DS_U1 - R² = 0

    const TYPE                          tyHalfNegB = TYPE(-1.0) * (tyDS_U0*tyD0_U0 + tyDS_U1*tyD0_U1);
    const TYPE                          tyC = tyRelSq - tgCY0.Query_RadiusSq();
    const TYPE                          tyDet = tyHalfNegB*tyHalfNegB - tyC*tyA;

    if (tyDet < TYPE(0.0))
    {
        TgASSERT(!bCapClip )
        return (TgE_NOINTERSECT);
    }

    const TYPE                          tyInvA = TYPE(1.0) / tyA;
    const TYPE                          tyDetSqrt = P::SQRT( tyDet );
    const TYPE                          tyTA = (tyHalfNegB - tyDetSqrt) * tyInvA;
    const TYPE                          tyTB = (tyHalfNegB + tyDetSqrt) * tyInvA;

    // Only process the point if the contact is bound by the two cap-planes.

    if (bCapClip)
    {
        TgASSERT( (tyPLN0 <= tyTA && tyTA <= tyPLN1) ^ (tyPLN0 <= tyTB && tyTB <= tyPLN1) )

        const TYPE                          tyT1 = (tyPLN0 <= tyTA && tyTA <= tyPLN1) ? tyTA : tyTB;

        if (tyT0 < tyT1)
        {
            C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyDS_U0 + tyT1*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyDS_U1 + tyT1*tyD0_U1, tgCY0.Query_BasisUnit1() );

            *ptyLN0 = tyT0;
            *ptyLN1 = tyT1;
            *ptvN1 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK1 ) );
        }
        else
        {
            C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyDS_U0 + tyT1*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyDS_U1 + tyT1*tyD0_U1, tgCY0.Query_BasisUnit1() );

            *ptyLN0 = tyT1;
            *ptyLN1 = tyT0;
            *ptvN1 = *ptvN0;
            *ptvN0 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK1 ) );
        };
    }
    else
    {
        if ((tyPLN0 > tyTB || tyTB > tyPLN1) && (tyPLN0 > tyTA || tyTA > tyPLN1))
        {
            //return (TgE_NOINTERSECT);
        };

        //TgASSERT( tyPLN0 <= tyTA && tyTA <= tyPLN1 && tyPLN0 <= tyTB && tyTB <= tyPLN1 )

        if (tyTA < tyTB)
        {
            C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyDS_U0 + tyTA*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyDS_U1 + tyTA*tyD0_U1, tgCY0.Query_BasisUnit1() );
            C_(VECTOR,DIM)                      tvK2 = MATH::F_MUL( tyDS_U0 + tyTB*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK3 = MATH::F_MUL( tyDS_U1 + tyTB*tyD0_U1, tgCY0.Query_BasisUnit1() );

            *ptyLN0 = tyTA;
            *ptyLN1 = tyTB;
            *ptvN0 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK1 )  );
            *ptvN1 = MATH::F_NORM( MATH::F_ADD( tvK2, tvK3 )  );
        }
        else
        {
            C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyDS_U0 + tyTB*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK1 = MATH::F_MUL( tyDS_U1 + tyTB*tyD0_U1, tgCY0.Query_BasisUnit1() );
            C_(VECTOR,DIM)                      tvK2 = MATH::F_MUL( tyDS_U0 + tyTA*tyD0_U0, tgCY0.Query_BasisUnit0() );
            C_(VECTOR,DIM)                      tvK3 = MATH::F_MUL( tyDS_U1 + tyTA*tyD0_U1, tgCY0.Query_BasisUnit1() );

            *ptyLN0 = tyTB;
            *ptyLN1 = tyTA;
            *ptvN0 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK1 )  );
            *ptvN1 = MATH::F_NORM( MATH::F_ADD( tvK2, tvK3 )  );
        };
    };

    return (TgS_OK);
};


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

// ---- F_Contact_Intersect ----------------------------------------------------------------------------------------------------- //
//  -- Internal Function -- bC0, bC1 : Indicate the termination condition of the linear {bc0,bC1}
//
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgCY0: Cylinder primitive
// Input:  tvS0,tvD0: Origin and Direction for Linear
// Output: tgPacket: Points of intersection between the two primitives are added to it
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM, bool bC0, bool bC1>
TgRESULT TTgINT_CYLN<TYPE,DIM,bC0,bC1>::DO( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(CYLINDER,DIM) tgCY0, M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvD0 )
{
    TYPE                                tyLN0, tyLN1;
    T_(VECTOR,DIM)                      tvN0, tvN1;

    C_TgRESULT tgResult = TTgINT_CYLN<TYPE,DIM,bC0,bC1>::DO( &tyLN0,&tyLN1, &tvN0,&tvN1, tgCY0, tvS0,tvD0 );

    if (TgFAILED(tgResult))
    {
        return (tgResult);
    };

    P_(CONTACT,DIM)                     ptgContact;

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

    ptgContact->m_tvPos = MATH::F_ADD( tvS0, MATH::F_MUL( tyLN0, tvD0 ) );
    ptgContact->m_tvNormal = tvN0;
    ptgContact->m_tyT0 = tyLN0;
    ptgContact->m_tyDepth = TYPE(0.0);

    ++ptgPacket->m_niContact;

    // Check to see if the solution was tangential.  If not add the second intersection point to the packet.

    if (P::ABS( tyLN0 - tyLN1 ) > LIMITS<TYPE>::EPSILON)
    {
        if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
        {
            return (TgS_MAXCONTACTS);
        };

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

        ptgContact->m_tvPos = MATH::F_ADD( tvS0, MATH::F_MUL( tyLN1, tvD0 ) );
        ptgContact->m_tvNormal = tvN1;
        ptgContact->m_tyT0 = tyLN1;
        ptgContact->m_tyDepth = TYPE(0.0);

        ++ptgPacket->m_niContact;
    }

    return (TgS_OK);
};


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

    template struct TTgINL_CYLN<TgFLOAT32,4,0,0>;
    template struct TTgINL_CYLN<TgFLOAT32,4,1,0>;
    template struct TTgINL_CYLN<TgFLOAT32,4,1,1>;

    template struct TTgINT_CYLN<TgFLOAT32,4,0,0>;
    template struct TTgINT_CYLN<TgFLOAT32,4,1,0>;
    template struct TTgINT_CYLN<TgFLOAT32,4,1,1>;

    template struct TTgCLP_CYLN<TgFLOAT32,4,0,0>;
    template struct TTgCLP_CYLN<TgFLOAT32,4,1,0>;
    template struct TTgCLP_CYLN<TgFLOAT32,4,1,1>;

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

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