// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Sphere-Point.inl
//  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".
//
// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
#if !defined(_TGS_COLLISION_SPHERE_POINT_INL_)
#define _TGS_COLLISION_SPHERE_POINT_INL_
#pragma once

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

// F_Dist[Sq], F_Closest[Sq] - Return the minimal distance [squared] between the primitives or negative type max if intersecting.

// tgSP0        Sphere (Input)
// tvS0         Point (Input)
// tgPacket     Container of points resulting from a contact generator.  Generation parameters provided as well. (Input/Ouput)
// tyPM         Current normalized time of first contact. (Input/Output)
// bPenetrate   If the swept primitives are in penetration, if true the function will return points of penetration. (Input)
// tgDT         A structure holding the swept primitive displacement for the entire duration of the test period. (Input)

// tvSP0        The point of closest proximity on the sphere. (Output)
// tgPacket     Container of points resulting from a contact generator.  Generation parameters provided as well. (Input/Ouput)
// tyPM         Current normalized time of first contact. (Input/Output)

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




namespace TGS { // START TGS ///////////////////////////////////////////////////////////////////////////////////////////////////////
namespace COL { // START COL ///////////////////////////////////////////////////////////////////////////////////////////////////////

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

template <typename TYPE, int DIM> TgFORCEINLINE
TYPE F_DistSq( CR_(SPHERE,DIM) tgSP0, M_(VECTOR,DIM) tvS0 )
{
    C_(VECTOR,DIM)                      tvDS = MATH::F_SUB( tvS0, tgSP0.Query_Origin() );
    const TYPE                          tyDS_DS = MATH::F_LSQ(tvDS);

    const TYPE                          tyDist = P::FSEL( tyDS_DS - tgSP0.Query_RadiusSq(), P::SQRT(tyDS_DS) - tgSP0.Query_Radius(), TYPE(-1.0) );
    return (P::FSEL( tyDist, tyDist*tyDist, -LIMITS<TYPE>::MAX ));
};


template <typename TYPE, int DIM> TgFORCEINLINE
TYPE F_Dist( CR_(SPHERE,DIM) tgSP0, M_(VECTOR,DIM) tvS0 )
{
    C_(VECTOR,DIM)                      tvDS = MATH::F_SUB( tvS0, tgSP0.Query_Origin() );
    const TYPE                          tyDS_DS = MATH::F_LSQ(tvDS);

    return (P::FSEL( tyDS_DS - tgSP0.Query_RadiusSq(), P::SQRT(tyDS_DS) - tgSP0.Query_Radius(), -LIMITS<TYPE>::MAX ));
};


// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //

template <typename TYPE, int DIM> TgFORCEINLINE
TYPE F_ClosestSq( PC_(VECTOR,DIM) ptvSP0, CR_(SPHERE,DIM) tgSP0, M_(VECTOR,DIM) tvS0 )
{
    TYPE                                tyDS;
    C_(VECTOR,DIM)                      tvDS = MATH::F_NORM( &tyDS, MATH::F_SUB( tvS0, tgSP0.Query_Origin() ) );

    *ptvSP0 = MATH::F_ADD( tgSP0.Query_Origin(), MATH::F_MUL( tvDS, tgSP0.Query_Radius() ) );
    return (P::FSEL( tyDS, tyDS*tyDS, -LIMITS<TYPE>::MAX ));
};


template <typename TYPE, int DIM> TgFORCEINLINE
TYPE F_Closest( PC_(VECTOR,DIM) ptvSP0, CR_(SPHERE,DIM) tgSP0, M_(VECTOR,DIM) tvS0 )
{
    TYPE                                tyDS;
    C_(VECTOR,DIM)                      tvDS = MATH::F_NORM( &tyDS, MATH::F_SUB( tvS0, tgSP0.Query_Origin() ) );

    tyDS -= tgSP0.Query_Radius();

    *ptvSP0 = MATH::F_ADD( tgSP0.Query_Origin(), MATH::F_MUL( tvDS, tgSP0.Query_Radius() ) );
    return (P::FSEL( tyDS, tyDS, -LIMITS<TYPE>::MAX ));
};


// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //

template <typename TYPE, int DIM> TgFORCEINLINE
TgBOOL F_Contact_Test( CR_(SPHERE,DIM) tgSP0, M_(VECTOR,DIM) tvS0 )
{
    return (MATH::F_LSQ(tvS0 - tgSP0.Query_Origin()) == tgSP0.Query_RadiusSq());
};


// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //

template <typename TYPE, int DIM> TgFORCEINLINE
TgRESULT F_Contact_Penetrate( PC_(CONTACT_PACKET,DIM) ptgPacket, M_(VECTOR,DIM) tvS0, CR_(SPHERE,DIM) tgSP0 )
{
    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ))
    TgASSERT(tgSP0.Is_Valid() && MATH::F_Is_Point_Valid( tvS0 ))

    if (0 == ptgPacket->m_niMaxContact || ptgPacket->m_niContact >= ptgPacket->m_niMaxContact || NULL == ptgPacket->m_ptgContact)
    {
        return (TgE_FAIL);
    };

    T_(VECTOR,DIM)                      tvNormal = MATH::F_SUB( tgSP0.Query_Origin(), tvS0 );
    TYPE                                tyNM = MATH::F_LSQ( tvNormal );

    if (tyNM > tgSP0.Query_RadiusSq())
    {
        return (TgE_NOINTERSECT);
    };

    if (tyNM <= LIMITS<TYPE>::EPSILON)
    {
        tvNormal = MATH::F_SETV<TYPE,DIM>( TYPE(0.0),TYPE(1.0),TYPE(0.0) );
        tyNM = TYPE(0.0);
    }
    else
    {
        tvNormal = MATH::F_NORM( &tyNM, tvNormal );
    };

    P_(CONTACT,DIM)                     ptgContact;

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

    ptgContact->m_tvPos = MATH::F_SUB( tgSP0.Query_Origin(), MATH::F_MUL( tgSP0.Query_Radius(), tvNormal ) );
    ptgContact->m_tvNormal = tvNormal;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = tgSP0.Query_Radius() - tyNM;

    ++ptgPacket->m_niContact;

    return (TgS_OK);
};


// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Sweep( PC_(CONTACT_PACKET,DIM) ptgPacket, TYPE *ptyPM, CR_(SPHERE,DIM) tgSP0, M_(VECTOR,DIM) tvS0, CR_(DELTA,DIM) tgDT )
{
    TgINT                               niContact = ptgPacket->m_niContact;
    const TYPE                          tyT = *ptyPM;
    T_(DELTA,DIM)                       tgNegDT;

    tgNegDT.m_tvUDT = MATH::F_NEG( tgDT.m_tvUDT );
    tgNegDT.m_tvDT = MATH::F_NEG( tgDT.m_tvDT );
    tgNegDT.m_tyDT = tgDT.m_tyDT;
    tgNegDT.m_tyDT_DT = tgDT.m_tyDT_DT;
    tgNegDT.m_tyInv_DT = tgDT.m_tyInv_DT;

    C_TgRESULT tgResult = F_Contact_Sweep( ptgPacket, ptyPM, tvS0, tgSP0, tgNegDT );

    P_(CONTACT,DIM)                     ptgContact;

    if (tgResult == TgE_PREPENETRATION)
    {
        for (TgINT iIdx = niContact; iIdx < ptgPacket->m_niContact; ++iIdx)
        {
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + iIdx*ptgPacket->m_iStride);

            ptgContact->m_tvPos = MATH::F_ADD( ptgContact->m_tvPos, MATH::F_MUL( ptgContact->m_tvNormal, ptgContact->m_tyDepth ) );
            ptgContact->m_tvNormal = MATH::F_NEG( ptgContact->m_tvNormal );
        };

        return (tgResult);
    }

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

    TgINT                               iIdx = (*ptyPM < tyT - ptgPacket->m_tySweepTol ? 0 : niContact);

    TgASSERT(iIdx < ptgPacket->m_niContact)

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

        ptgContact->m_tvPos = MATH::F_ADD( ptgContact->m_tvPos, MATH::F_MUL( ptgContact->m_tyT0, tgDT.m_tvDT ) );
        ptgContact->m_tvNormal = MATH::F_NEG( ptgContact->m_tvNormal );
    };

    return (tgResult);
};


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

}; // END COL //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}; // END TGS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#endif //  END  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////