// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//  »Project«   Teikitu Gaming System (TgS) (∂)
//  »File«      TgS Collision - F - Sphere-Point.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;Sphere;Point;
// ------------------------------------------------------------------------------------------------------------------------------ //
//  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 ================================================================================================================= //

// ---- tgCO_F_Penetrate_VT_SP -------------------------------------------------------------------------------------------------- //
// ------------------------------------------------------------------------------------------------------------------------------ //
TgRESULT V(tgCO_F_Penetrate_VT_SP)( V(PCU_STg2_CO_Packet) psPacket, V(CPCU_TgVEC) pvS0, V(CPCU_TgSPHERE) psSP0 )
{
    V(TgVEC)                            vNormal, vT0;
    TYPE                                fNM;
    V(P_STg2_CO_Contact)                psContact;

    TgASSERT_PARAM((TgSIZE)psPacket->m_iStride >= sizeof( V(P_STg2_CO_Contact) ));
    TgASSERT_PARAM(V(tgGM_Is_Valid_SP)( psSP0 ) && V(F_Is_Point_Valid)( pvS0 ));

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

    vNormal = V(F_SUB_VV)( &psSP0->m_vOrigin, pvS0 );
    fNM = V(F_LSQ)( &vNormal );

    if (fNM > psSP0->m_fRadiusSq)
    {
        return (ETgE_NO_INTERSECT);
    };

    if (fNM <= F(KTgEPS))
    {
        vNormal = V(F_SETV_ELEM)( MKL(0.0), MKL(1.0), MKL(0.0) );
        fNM = MKL(0.0);
    }
    else
    {
        vNormal = V(F_NORM_LEN)( &fNM, &vNormal );
    };

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

    vT0 = V(F_MUL_SV)( psSP0->m_fRadius, &vNormal );

    psContact->m_vS0 = V(F_SUB_VV)( &psSP0->m_vOrigin, &vT0 );
    psContact->m_vN0 = vNormal;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = psSP0->m_fRadius - fNM;

    ++psPacket->m_niContact;

    return (TgS_OK);
}


// ---- tgCO_F_Sweep_SP_VT ------------------------------------------------------------------------------------------------------ //
// ------------------------------------------------------------------------------------------------------------------------------ //
TgRESULT V(tgCO_F_Sweep_SP_VT)(
    V(PCU_STg2_CO_Packet) psPacket, PCU_TYPE pfPM, V(CPCU_TgSPHERE) psSP0, V(CPCU_TgVEC) pvS0, V(CPCU_TgDELTA) psDT )
{
    TgSINT32                            niContact = psPacket->m_niContact;
    const TYPE                          fT = *pfPM;
    V(TgDELTA)                          tgNegDT;
    V(P_STg2_CO_Contact)                psContact;
    TgSINT32                            iIdx;
    TgRESULT                            tgResult;
    V(TgVEC)                            vT0;

    tgNegDT.m_vUDT = V(F_NEG)( &psDT->m_vUDT );
    tgNegDT.m_vDT = V(F_NEG)( &psDT->m_vDT );
    tgNegDT.m_fDT = psDT->m_fDT;
    tgNegDT.m_fDT_DT = psDT->m_fDT_DT;
    tgNegDT.m_fInv_DT = psDT->m_fInv_DT;

    tgResult =  V(tgCO_F_Sweep_VT_SP)( psPacket, pfPM, pvS0, psSP0, &tgNegDT );

    if (tgResult == ETgE_PREPENETRATION)
    {
        for (iIdx = niContact; iIdx < psPacket->m_niContact; ++iIdx)
        {
            psContact = (V(P_STg2_CO_Contact))(psPacket->m.piContact + iIdx*psPacket->m_iStride);

            vT0 = V(F_MUL_VS)( &psContact->m_vN0, psContact->m_fDepth );

            psContact->m_vS0 = V(F_ADD_VV)( &psContact->m_vS0, &vT0 );
            psContact->m_vN0 = V(F_NEG)( &psContact->m_vN0 );
        };

        return (tgResult);
    }

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

    iIdx = (*pfPM < fT - psPacket->m_fSweepTol ? 0 : niContact);

    TgASSERT(iIdx < psPacket->m_niContact);

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

        vT0 = V(F_MUL_SV)( psContact->m_fT0, &psDT->m_vDT );

        psContact->m_vS0 = V(F_ADD_VV)( &psContact->m_vS0, &vT0 );
        psContact->m_vN0 = V(F_NEG)( &psContact->m_vN0 );
    };

    return (tgResult);
}


// ---- tgCO_F_Sweep_VT_SP ------------------------------------------------------------------------------------------------------ //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  fPM: Current normalized time of first contact.
// Input:  bPenetrate: If the swept primitives are in penetration, if true the function will return points of penetration.
// Input:  psSP0: Sphere primitives
// Input:  vS0: Point
// Input:  psDT: A structure holding the swept primitive displacement for the entire duration of the test period
// Output: tgPacket: Contact points are added or replace the current set depending on the time comparison and given parameters
// Output: fPM: New normalized time of first contact
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //
TgRESULT V(tgCO_F_Sweep_VT_SP)(
    V(PCU_STg2_CO_Packet) psPacket, PCU_TYPE pfPM, V(CPCU_TgVEC) pvS0, V(CPCU_TgSPHERE) psSP0, V(CPCU_TgDELTA) psDT )
{
    V(TgVEC)                            vDS = V(F_SUB_VV)( pvS0, &psSP0->m_vOrigin );
    TYPE                                fDS_DS = V(F_LSQ)( &vDS );

    TgASSERT_PARAM( V(tgGM_Is_Valid_DT)( psDT ) && V(tgGM_Is_Valid_SP)( psSP0 ) && V(F_Is_Point_Valid)( pvS0 ) );
    TgASSERT_PARAM((TgSIZE)psPacket->m_iStride >= sizeof( V(P_STg2_CO_Contact) ));

    if (fDS_DS <= psSP0->m_fRadiusSq)
    {
        if (*pfPM > psPacket->m_fSweepTol)
        {
            psPacket->m_niContact = 0;
        };

        *pfPM = MKL(0.0);

        if ((TgTRUE == psPacket->m_bReport_Penetration) && ETgE_MAX_CONTACTS == V(tgCO_F_Penetrate_VT_SP)( psPacket, pvS0,psSP0 ))
        {
            return (ETgE_MAX_CONTACTS);
        };

        return (ETgE_PREPENETRATION);
    }
    else
    {
        TYPE                                fK0 = psDT->m_fDT - F(KTgEPS);
        const TYPE                          fDS_UDT = V(F_DOT_VV)( &vDS, &psDT->m_vUDT );

        fK0 = F(tgPM_FSEL)( fK0, fDS_UDT - F(KTgEPS), MKL(-1.0) ); // Negligible distance towards each other.
        fK0 = F(tgPM_FSEL)( fK0, psSP0->m_fRadius + *pfPM*psDT->m_fDT - fDS_UDT, MKL(-1.0) ); // Separation along displacement
        fK0 = F(tgPM_FSEL)( fK0, psSP0->m_fRadiusSq - fDS_DS + fDS_UDT*fDS_UDT, MKL(-1.0) ); // Orthogonal separation
        fK0 = F(tgPM_FSEL)( fK0, fDS_UDT - F(tgPM_SQRT)( fK0 ), MKL(-1.0) ); // Time occurs after first time of contact.

        if (fK0 > (*pfPM + psPacket->m_fSweepTol)*psDT->m_fDT)
        {
            return (ETgE_NO_INTERSECT); // Outside of sweep space.
        }
        else
        {
            const TYPE                          fT0 = fK0 / psDT->m_fDT;

            V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fK0, &psDT->m_vUDT );
            V(C_TgVEC)                          vS1 = V(F_ADD_VV)( &psSP0->m_vOrigin, &vK0 );
            V(C_TgVEC)                          vK1 = V(F_SUB_VV)( &vS1, pvS0 );
            V(C_TgVEC)                          vNormal = V(F_NORM)( &vK1 );
            V(C_TgVEC)                          vK2 = V(F_MUL_VS)( &vNormal, psSP0->m_fRadius );
            V(P_STg2_CO_Contact)                psContact;

            if (fT0 < *pfPM - psPacket->m_fSweepTol)
            {
                psPacket->m_niContact = 0;
                *pfPM = fT0;
            };

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

            psContact->m_vS0 = V(F_SUB_VV)( &vS1, &vK2 );
            psContact->m_vN0 = vNormal;
            psContact->m_fT0 = fT0;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

            return (TgS_OK);
        };
    };
}