// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Sphere-Sphere.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_SPHERE_INL_)
#define _TGS_COLLISION_SPHERE_SPHERE_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 #1 (Input)
// tgSP1        Sphere #2 (Input)
// tvUDT        Normalized direction of displacement of Sphere #2 (Input)
// tyDT         Length of displacement (Input)

// tvSP0        The point of closest proximity on sphere #1. (Output)
// tvSP1        The point of closest proximity on sphere #2. (Output)

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




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

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

template <typename TYPE, int DIM> TgFORCEINLINE
TYPE F_DistSq( CR_(SPHERE,DIM) tgSP0, CR_(SPHERE,DIM) tgSP1 )
{
    C_(VECTOR,DIM)                      tvK0 = MATH::F_SUB( tgSP1.Query_Origin(), tgSP0.Query_Origin() );
    const TYPE                          tyDist = MATH::F_LEN( tvK0 ) - (tgSP0.Query_Radius() + tgSP1.Query_Radius());

    return (P::FSEL( tyDist, tyDist*tyDist, -LIMITS<TYPE>::MAX ));
};


template <typename TYPE, int DIM> TgFORCEINLINE
TYPE F_Dist( CR_(SPHERE,DIM) tgSP0, CR_(SPHERE,DIM) tgSP1 )
{
    C_(VECTOR,DIM)                      tvK0 = MATH::F_SUB( tgSP1.Query_Origin(), tgSP0.Query_Origin() );
    const TYPE                          tyDist = MATH::F_LEN( tvK0 ) - (tgSP0.Query_Radius() + tgSP1.Query_Radius());

    return (P::FSEL( tyDist, tyDist, -LIMITS<TYPE>::MAX ));
};


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

template <typename TYPE, int DIM> TgFORCEINLINE
TYPE F_ClosestSq( PC_(VECTOR,DIM) ptvSP0, PC_(VECTOR,DIM) ptvSP1, CR_(SPHERE,DIM) tgSP0, CR_(SPHERE,DIM) tgSP1 )
{
    const TYPE                          tyDist = F_Closest( ptvSP0, ptvSP1, tgSP0, tgSP1 );

    return (P::FSEL( tyDist, tyDist*tyDist, -LIMITS<TYPE>::MAX ));
};


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

template <typename TYPE, int DIM> TgFORCEINLINE
TgBOOL F_Contact_Test( CR_(SPHERE,DIM) tgSP0, CR_(SPHERE,DIM) tgSP1 )
{
    TgASSERT(tgSP0.Is_Valid() && tgSP1.Is_Valid());

    const TYPE                          tyDS_DS = MATH::F_LSQ( MATH::F_SUB( tgSP1.Query_Origin(), tgSP0.Query_Origin() ) );
    const TYPE                          tySumRad = tgSP0.Query_Radius() + tgSP1.Query_Radius();

    return (tyDS_DS <= tySumRad*tySumRad);
};


template <typename TYPE, int DIM> TgFORCEINLINE
TgBOOL F_Contact_Test( CR_(SPHERE,DIM) tgSP0, CR_(SPHERE,DIM) tgSP1, M_(VECTOR,DIM) tvUDT, const TYPE tyDT )
{
    TgASSERT(tgSP0.Is_Valid() && tgSP1.Is_Valid());

    C_(VECTOR,DIM)                      tvDS = MATH::F_SUB( tgSP0.Query_Origin(), tgSP1.Query_Origin() );
    const TYPE                          tyDS_DS = MATH::F_LSQ( tvDS );

    const TYPE                          tyRS = tgSP0.Query_Radius() + tgSP1.Query_Radius();
    const TYPE                          tyRS2 = tyRS*tyRS;

    TYPE                                tyTMPC = tyDT - LIMITS<TYPE>::EPSILON;
    const TYPE                          tyDS_UDT = MATH::F_DOT(tvDS,tvUDT);

    tyTMPC = P::FSEL( tyTMPC, tyDS_UDT - LIMITS<TYPE>::EPSILON, TYPE(-1.0) ); // Negligible displacement
    tyTMPC = P::FSEL( tyTMPC, tyRS + tyDT - tyDS_UDT, TYPE(-1.0) ); // Negligible displacement towards each other.
    tyTMPC = P::FSEL( tyTMPC, tyRS2 - tyDS_DS + tyDS_UDT*tyDS_UDT, TYPE(-1.0) ); // Separation along displacement is too large.
    tyTMPC = P::FSEL( tyTMPC, tyDS_UDT - P::SQRT( tyTMPC ), TYPE(-1.0) ); // Orthogonal separation is too large.
    tyTMPC = P::FSEL( tyRS2 - tyDS_DS, TYPE(0.0), tyTMPC ); // Spheres are pre-penetrated.

    return (tyTMPC <= tyDT); // Time occurs after first time of contact.
};


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

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