// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//  »Project«   Teikitu Gaming System (TgS) (∂)
//  »File«      TgS Collision - F - Capsule-Linear.c_inc
//  »Author«    Andrew Aye (EMail: mailto:andrew.aye@gmail.com, Web: http://www.andrewaye.com)
//  »Version«   4.0
//  »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Capsule;Line;Ray;Segment;
// ------------------------------------------------------------------------------------------------------------------------------ //
//  Copyright: © 2002-2010, 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".
// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
// == Collision ================================================================================================================= //

// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //
//  File Local Functions
// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //

TgRESULT VI(tgCO_FI_Internal_CP_LR)( PCU_TYPE, V(PCU_TgVEC), const TYPE, V(CPCU_TgTUBE), V(CPCU_TgVEC), V(CPCU_TgVEC) );




// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //
//  Public Functions
// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //

// ---- tgCO_FI_Intersect_CP_LR ------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  psCP0: Capsule primitive
// Input:  vS0,vD0: Origin and Direction for Linear
// Output: tgPacket: Points of intersection between the two primitives are added to it
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //
TgRESULT VI(tgCO_FI_Intersect_CP_LR)(
    V(PCU_STg2_CO_Packet) psPacket, V(CPCU_TgTUBE) psCP0, V(CPCU_TgVEC) pvS0, V(CPCU_TgVEC) pvD0 )
{
    TYPE                                fLN0, fLN1;
    V(TgVEC)                            vN0, vN1;
    V(P_STg2_CO_Contact)                psContact;
    TgRESULT                            iResult;
    TgBOOL                              bContact = TgFALSE;

    // Check to make sure that a valid contact, and contact packet exist.

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || NULL == psPacket->m.psContact)
    {
        return (TgE_FAIL);
    };

    iResult = VI(tgCO_FI_Contact_CP_LR)( &fLN0,&fLN1, &vN0,&vN1, psCP0, pvS0,pvD0 );
    
    if (TgFAILED( iResult ))
    {
        return (iResult);
    };

    // Limit the variable to the cap regions

    if (LN_CAP_0 && fLN0 < MKL(0.0))
    {
        if(fLN1 <= MKL(0.0))
        {
            return (ETgE_NO_INTERSECT);
        };
    }
    else if (!LN_CAP_1 || fLN0 <= MKL(1.0))
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fLN0, pvD0 );

        psContact = (V(P_STg2_CO_Contact))(psPacket->m.piContact + psPacket->m_niContact*psPacket->m_iStride);

        psContact->m_vS0 = V(F_ADD_VV)( pvS0, &vK0 );
        psContact->m_vN0 = vN0;
        psContact->m_fT0 = fLN0;
        psContact->m_fDepth = MKL(0.0);

        ++psPacket->m_niContact;
        bContact = TgTRUE;
    };

    if (LN_CAP_1 && fLN1 > MKL(1.0))
    {
        if(fLN0 >= MKL(1.0))
        {
            return (ETgE_NO_INTERSECT);
        };
    }
    else if (F(tgPM_ABS)( fLN1 - fLN1 ) > F(KTgEPS))
    {
        if (psPacket->m_niContact >= psPacket->m_niMaxContact)
        {
            return (ETgE_MAX_CONTACTS);
        }
        else
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fLN1, pvD0 );

            psContact = (V(P_STg2_CO_Contact))(psPacket->m.piContact + psPacket->m_niContact*psPacket->m_iStride);

            psContact->m_vS0 = V(F_ADD_VV)( pvS0, &vK0 );
            psContact->m_vN0 = vN1;
            psContact->m_fT0 = fLN1;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;
            bContact = TgTRUE;
        };
    };

    return (bContact ? TgS_OK : ETgE_NO_INTERSECT);
}


// ---- tgCO_FI_Penetrate_CP_LR ------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  vL0: The point of closest proximity on the linear to the capsule axis.
// Input:  psCP0: Capsule primitive
// Input:  vSP0: The point of closest proximity on the capsule axis to the linear.
// Input:  fDistSq: The minimal distance squared between the capsule axis and the linear.
// Output: tgPacket: Points of penetration between the two primitives are added to it
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //
TgRESULT VI(tgCO_FI_Penetrate_CP_LR)(
    V(PCU_STg2_CO_Packet) psPacket, V(CPCU_TgVEC) pvL0, V(CPCU_TgTUBE) psCP0, V(CPCU_TgVEC) pvCP1, const TYPE fDistSq )
{
    V(TgVEC)                            vNormal, vK0;
    TYPE                                fNM;
    V(P_STg2_CO_Contact)                psContact;

    if (fDistSq <= F(KTgEPS))
    {
        if (F(tgCM_NR0)( psCP0->m.m.vU_HAX.m.z ))
        {
            vNormal = V(F_SETV_ELEM)( -psCP0->m.m.vU_HAX.m.y, psCP0->m.m.vU_HAX.m.x, MKL(0.0) );
        }
        else
        {
            vNormal = V(F_SETV_ELEM)( MKL(0.0),psCP0->m.m.vU_HAX.m.z,-psCP0->m.m.vU_HAX.m.y );
        };
        vNormal = V(F_NORM)( &vNormal );
        fNM = MKL(0.0);
    }
    else
    {
        V(C_TgVEC)                          vK1 = V(F_SUB_VV)( pvCP1, pvL0 );

        vNormal = V(F_NORM_LEN)( &fNM, &vK1 );
    };

    psContact = (V(P_STg2_CO_Contact))(psPacket->m.piContact + psPacket->m_niContact*psPacket->m_iStride);

    vK0 = V(F_MUL_SV)( psCP0->m_fRadius, &vNormal );

    psContact->m_vS0 = V(F_SUB_VV)( pvCP1, &vK0 );
    psContact->m_vN0 = vNormal;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = psCP0->m_fRadius - fNM;

    ++psPacket->m_niContact;

    return (TgS_OK);
}


// ---- tgCO_FI_Contact_CP_LR -------------------------------------------------------------------------------------------------- //
// Input:  psCP0: Capsule primitive
// Input:  vS0,vD0: Origin and Direction for Linear
// Output: fLN0,fLN1: Parametric parameter to generate the two points of the linear in contact with the capsule surface
// Output: vN0, vN1: Capsule surface normal at the points of contact between the two primitives
// Return: Result Code
//   The internal functions do not clip the linear.  All passed in linears are treated as lines - the boolean markers are used to
//  generate possible quick-out logic to avoid further processing.
// ------------------------------------------------------------------------------------------------------------------------------ //
TgRESULT VI(tgCO_FI_Contact_CP_LR)(
    PCU_TYPE pfLN0, PCU_TYPE pfLN1, V(PCU_TgVEC) pvN0, V(PCU_TgVEC) pvN1,
    V(CPCU_TgTUBE) psCP0, V(CPCU_TgVEC) pvS0, V(CPCU_TgVEC) pvD0
) {
    // Linear in the reference frame of the capsule

    const TYPE                          fD0_U0 = V(F_DOT_VV)( pvD0, &psCP0->m.m.vU_Basis0 );
    const TYPE                          fD0_U1 = V(F_DOT_VV)( pvD0, &psCP0->m.m.vU_Basis1 );
    const TYPE                          fD0_UA = V(F_DOT_VV)( pvD0, &psCP0->m.m.vU_HAX );

    const TYPE                          fA = fD0_U0*fD0_U0 + fD0_U1*fD0_U1;

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

    V(C_TgVEC)                          vDS = V(F_SUB_VV)( pvS0, &psCP0->m.m.vOrigin );

    const TYPE                          fDS_UA = V(F_DOT_VV)( &vDS, &psCP0->m.m.vU_HAX );
    const TYPE                          fDS_U0 = V(F_DOT_VV)( &vDS, &psCP0->m.m.vU_Basis0 );
    const TYPE                          fDS_U1 = V(F_DOT_VV)( &vDS, &psCP0->m.m.vU_Basis1 );

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

    const TYPE                          fRelSq = fDS_U0*fDS_U0 + fDS_U1*fDS_U1;

    TgASSERT_PARAM( V(tgGM_Is_Valid_TB)( psCP0 ) && V(F_Is_Point_Valid)( pvS0 ) && V(F_Is_Vector_Valid)( pvD0 ) );

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

        if (!((fDS_UA > MKL(0.0)) ^ (fD0_UA > MKL(0.0))) && F(tgPM_ABS)( fDS_UA ) > psCP0->m_fExtent + psCP0->m_fRadius)
        {
            return (ETgE_NO_INTERSECT);
        };

        // If the origin lies outside of the capsule 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 (fRelSq > psCP0->m_fRadiusSq && (fDS_U0*fD0_U0 + fDS_U1*fD0_U1) > MKL(0.0))
        {
            return (ETgE_NO_INTERSECT);
        };
    };

    // R² = (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
    // 0  = ζ•ζ_(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²

    {
        const TYPE                          fHalfNegB = MKL(-1.0) * (fDS_U0*fD0_U0 + fDS_U1*fD0_U1);
        const TYPE                          fC = fRelSq - psCP0->m_fRadiusSq;
        const TYPE                          fDet = fHalfNegB*fHalfNegB - fC*fA;
        const TYPE                          fInvA = MKL(1.0) / fA;

        if (fDet < MKL(0.0))
        {
            return (ETgE_NO_INTERSECT);
        }
        else
        {
            const TYPE                          fDetSqrt = F(tgPM_SQRT)( fDet );
            TYPE                                fD0;
            V(TgVEC)                            vUD0 = V(F_NORM_LEN)( &fD0, pvD0 );
            const TYPE                          fT0 = (fHalfNegB - fDetSqrt) * fInvA;
            const TYPE                          fT1 = (fHalfNegB + fDetSqrt) * fInvA;

            if (fD0 < F(KTgEPS))
            {
                return (ETgE_NO_INTERSECT);
            }
            else
            {
                const TYPE                          fH0 = fDS_UA + fT0 * fD0_UA;
                const TYPE                          fH1 = fDS_UA + fT1 * fD0_UA;
                const TYPE                          fInvD0 = MKL(1.0) / fD0;

                if (F(tgPM_ABS)( fH0 ) > psCP0->m_fExtent)
                {
                    if (TgFAILED(VI(tgCO_FI_Internal_CP_LR)(
                        pfLN0, pvN0, (fH0 > MKL(0.0) ? MKL(1.0) : MKL(-1.0)), psCP0, pvS0, &vUD0 ) ))
                    {
                        TgASSERT( F(tgPM_ABS)( fH1 ) > psCP0->m_fExtent );
                        return (ETgE_NO_INTERSECT);
                    };

                    *pfLN0 *= fInvD0;
                }
                else
                {
                    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT0, pvD0 );
                    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fH0, &psCP0->m.m.vU_HAX );
                    V(C_TgVEC)                          vK2 = V(F_SUB_VV)( &vK0, &vK1 );
                    V(C_TgVEC)                          vK3 = V(F_ADD_VV)( &vDS, &vK2 );

                    *pfLN0 = fT0;
                    *pvN0 = V(F_NORM)( &vK3 );
                };


                if (F(tgPM_ABS)( fH1 ) > psCP0->m_fExtent)
                {
                    if (TgFAILED(VI(tgCO_FI_Internal_CP_LR)(
                        pfLN1, pvN1, (fH1 > MKL(0.0) ? MKL(1.0) : MKL(-1.0)), psCP0, pvS0, &vUD0 ) ))
                    {
                        TgASSERT( TgFALSE );
                    };

                    *pfLN1 *= fInvD0;
                }
                else
                {
                    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT1, pvD0 );
                    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fH1, &psCP0->m.m.vU_HAX );
                    V(C_TgVEC)                          vK2 = V(F_SUB_VV)( &vK0, &vK1 );
                    V(C_TgVEC)                          vK3 = V(F_ADD_VV)( &vDS, &vK2 );

                    *pfLN1 = fT1;
                    *pvN1 = V(F_NORM)( &vK3 );
                };

                TgASSERT( *pfLN0 <= *pfLN1 );

                return (TgS_OK);
            };
        };
    };
}




// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //
//  File Local Functions
// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //

// ---- F_Internal_Intersect ---------------------------------------------------------------------------------------------------- //
//  -- Internal Function -- (Test against a capsule cap)
// Input:  fAx: A positive/negative indicator to select which end of the axis to test
// Input:  psCP0: Capsule primitive
// Input:  vS0,vUD0: Origin and Normalized Direction for Linear
// Output: fLN0: Parametric parameter to generate the point on the linear in contact with the capsule surface
// Output: vN0: Capsule surface normal at the points of contact between the two primitives
// Return: Result Code
//
//   The internal functions do not clip the linear.  All passed in linears are treated as lines - the boolean markers are used to
//  generate possible quick-out logic to avoid further processing.
// ------------------------------------------------------------------------------------------------------------------------------ //
TgRESULT VI(tgCO_FI_Internal_CP_LR)(
    PCU_TYPE pfL0, V(PCU_TgVEC) pvN0, const TYPE fAx, V(CPCU_TgTUBE) psCP0, V(CPCU_TgVEC) pvS0, V(CPCU_TgVEC) pvUD0 )
{
    V(C_TgVEC)                          vD1 = V(F_MUL_SV)( fAx, &psCP0->m_vHAX );
    V(C_TgVEC)                          vS1 = V(F_ADD_VV)( &psCP0->m.m.vOrigin, &vD1 );
    V(C_TgVEC)                          vDS = V(F_SUB_VV)( &vS1, pvS0 );
    const TYPE                          fDS_DS = V(F_LSQ)( &vDS );
    const TYPE                          fT0 = V(F_DOT_VV)( &vDS, pvUD0 );
    const TYPE                          fDSC = fT0*fT0 - fDS_DS + psCP0->m_fRadiusSq;
    V(TgVEC)                            vT0, vK0;

    TgASSERT_PARAM(V(tgGM_Is_Valid_TB)( psCP0 ) && V(F_Is_Point_Valid)( pvS0 ) && V(F_Is_Vector_Valid)( pvUD0 ));

    if (fDSC > F(KTgEPS))
    {
        const TYPE                          fRoot = F(tgPM_SQRT)( fDSC );
        TYPE                                fT1;

        fT1 = fT0 + fRoot;
        vK0 = V(F_MUL_SV)( fT1, pvUD0 );
        vT0 = V(F_SUB_VV)( &vK0, &vDS );

        if (V(F_DOT_VV)( &vT0, &vD1 ) >= MKL(0.0))
        {
            *pvN0 = V(F_NORM)( &vT0 );
            *pfL0 = fT1;

            return (TgS_OK);
        };

        fT1 = fT0 - fRoot;
        vK0 = V(F_MUL_SV)( fT1, pvUD0 );
        vT0 = V(F_SUB_VV)( &vK0, &vDS );

        if (V(F_DOT_VV)( &vT0, &vD1 ) >= MKL(0.0))
        {
            *pvN0 = V(F_NORM)( &vT0 );
            *pfL0 = fT1;

            return (TgS_OK);
        };
    }
    else if (fDSC > -F(KTgEPS))
    {
        vK0 = V(F_MUL_SV)( fT0, pvUD0 );
        vT0 = V(F_SUB_VV)( &vK0, &vDS );

        if (V(F_DOT_VV)( &vT0, &vD1 ) >= MKL(0.0))
        {
            *pvN0 = V(F_NORM)( &vT0 );
            *pfL0 = fT0;

            return (TgS_OK);
        };
    };

    return (ETgE_NO_INTERSECT);
}