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

/* Let S0 and S1 represent two spheres, with radius R0, R1, centers C0,C1 and velocities V0, V1 respectively./
// W.O.L.O.G. Perform the following calculations within S0 frame of reference.
// Assume the frame of reference in non-rotational (only translation).
//
// SWEEP TEST
//
// As with all sweep tests, an arbitrary body is chosen as the principal reference frame so that only the relative
// motion of one of the bodies is relevant.  The solution is as follows,
//  (1) Create a line (RV) using the centre of the second sphere and the vector of relative motion.
//  (2) Form a right-sided triangle.
//      (a) The hypotenuse is the sum of the sphere radii.
//      (b) The known side is the line from the centre of the first sphere to the closest point on line RV.
//      (c) The length of the remaining side (along RV) is the distance from the point of closest proximity and the
//          point of first contact between the two spheres.
//
//                              .(C0)
//                             /|\
//                            / | \
//                           /  |  \
//                          /   |   \
//                         /    |    \
//                        /(R0) |     \(R0)
//          (R1)/    (R1)/      |
//             /        /       |
//            /        /        |
//           .--------.---------.--------------> (RV)
//          (C1)     (P1)      (P0)
//
// Let RV = V1 - V0, the relative velocity of S1 in the frame of reference of S0
// Let RV_N be the normalized RV vector, and RV_L the length of the original RV vector
// Let DS = C1 - C0, the vector joining the two sphere centers
// Let RS = R0+R1, the sum of the radii of the two spheres
// 
// Test 1: Pre penetration - The spheres are in contact if DS•DS < RS•RS
// Test 2: Movement - RV_L ~= 0, no contact takes place
// Test 3: Parallel - ψ = DS•RV_N
//   Contact can not occur under the following conditions:
//     [ψ > 0]          - Second sphere is moving away from the first sphere
//     [ψ < -(RV_L+RS)] - Motion is insufficient for them to come within proximity
// Test 4: Perpendicular - φ = DS - ψ•RV_N
//   Contact can not occur under the following conditions:
//     [φ•φ > RS•RS] - The sphere path does not bring them within proximity
// Result: Υ = RS•RS - φ•φ, ζ = ψ + √Υ
//   [ζ < RV_L] - The two sphere's contact at (ζ / RV_L) of the complete path of motion.
*/ 

// == Collision ================================================================================================================= //

// ---- tgCO_F_Closest_SP_SP ---------------------------------------------------------------------------------------------------- //
// Input:  psSP0,psSP1: Sphere primitives
// Output: vSP0,vSP1: Point of closest proximity on the sphere #1 and #2 respectively
// Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.
// ------------------------------------------------------------------------------------------------------------------------------ //
TYPE V(tgCO_F_Closest_SP_SP)( V(PCU_TgVEC) pvSP0, V(PCU_TgVEC) pvSP1, V(CPCU_TgSPHERE) psSP0, V(CPCU_TgSPHERE) psSP1 )
{
    TYPE                                fDS;
    V(C_TgVEC)                          vDS = V(F_SUB_VV)( &psSP1->m_vOrigin, &psSP0->m_vOrigin );
    V(C_TgVEC)                          vDN = V(F_NORM_LEN)( &fDS, &vDS );
    const TYPE                          fSumRad = psSP0->m_fRadius + psSP1->m_fRadius;

    TgASSERT(V(tgGM_Is_Valid_SP)( psSP0 ) && V(tgGM_Is_Valid_SP)( psSP1 ));

    if (fDS <= fSumRad)
    {
        return (-F(KTgMAX));
    }
    else
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)( psSP0->m_fRadius, &vDN );
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)( psSP1->m_fRadius, &vDN );

        *pvSP0 = V(F_ADD_VV)( &psSP0->m_vOrigin, &vK0 );
        *pvSP1 = V(F_SUB_VV)( &psSP1->m_vOrigin, &vK1 );

        return (fDS - fSumRad);
    };
}


// ---- tgCO_F_Penetrate_SP_SP -------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  psSP0,psSP1: Sphere primitives - contact points are generated on sphere #2 (psSP1)
// Output: tgPacket: Points of penetration between the two primitives are added to it
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //
TgRESULT V(tgCO_F_Penetrate_SP_SP)( V(PCU_STg2_CO_Packet) psPacket, V(CPCU_TgSPHERE) psSP0, V(CPCU_TgSPHERE) psSP1 )
{
    TgASSERT_PARAM((TgSIZE)psPacket->m_iStride >= sizeof( V(P_STg2_CO_Contact) ));
    TgASSERT_PARAM(V(tgGM_Is_Valid_SP)( psSP1 ) && V(tgGM_Is_Valid_SP)( psSP0 ));

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || NULL == psPacket->m.psContact)
    {
        return (TgE_FAIL);
    }
    else
    {
        TYPE                                fDS;
        V(C_TgVEC)                          vDS = V(F_SUB_VV)( &psSP1->m_vOrigin, &psSP0->m_vOrigin );
        V(C_TgVEC)                          vDN = V(F_NORM_LEN)( &fDS, &vDS );
        const TYPE                          fSumRad = psSP1->m_fRadius + psSP0->m_fRadius;
        V(P_STg2_CO_Contact)                psContact;

        if (fDS > fSumRad)
        {
            return (ETgE_NO_INTERSECT);
        };

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

        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = fSumRad - fDS;

        if (fDS <= F(KTgEPS))
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_VS)( &V(KTgV_UNIT_Y), psSP1->m_fRadius );

            psContact->m_vS0 = V(F_SUB_VV)( &psSP1->m_vOrigin, &vK0 );
            psContact->m_vN0 = V(KTgV_UNIT_Y);
        }
        else
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_VS)( &vDN, psSP1->m_fRadius );

            psContact->m_vS0 = V(F_SUB_VV)( &psSP1->m_vOrigin, &vK0 );
            psContact->m_vN0 = vDN;
        };

        ++psPacket->m_niContact;
        return (TgS_OK);
    };
}


// ---- tgCO_F_Sweep_SP_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,psSP1: Sphere primitives
// 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_SP_SP)(
    V(PCU_STg2_CO_Packet) psPacket, PCU_TYPE pfPM, V(CPCU_TgSPHERE) psSP0, V(CPCU_TgSPHERE) psSP1, V(CPCU_TgDELTA) psDT )
{
    TgASSERT(V(tgGM_Is_Valid_SP)( psSP1 ) && V(tgGM_Is_Valid_SP)( psSP0 ));

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || NULL == psPacket->m.psContact)
    {
        return (TgE_FAIL);
    }
    else
    {
        // Check for pre-penetration.

        const TYPE                          fRS = psSP1->m_fRadius + psSP0->m_fRadius;
        const TYPE                          fRS2 = fRS*fRS;
        V(C_TgVEC)                          vDS = V(F_SUB_VV)( &psSP0->m_vOrigin, &psSP1->m_vOrigin );
        const TYPE                          fDS_DS = V(F_LSQ)( &vDS );

        if (fDS_DS <= fRS2)
        {   // Pre-Penetration.
            C_TgBOOL                            bPenetrate = TgTRUE == psPacket->m_bReport_Penetration;

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

            *pfPM = MKL(0.0);

            if (bPenetrate && ETgE_MAX_CONTACTS == V(tgCO_F_Penetrate_SP_SP)( psPacket, psSP0, psSP1 ))
            {
                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, fRS + *pfPM*psDT->m_fDT - fDS_UDT, MKL(-1.0) ); // Separation along displacement is too large.
            fK0 = F(tgPM_FSEL)( fK0, fRS2 - fDS_DS + fDS_UDT*fDS_UDT, MKL(-1.0) ); // Orthogonal separation is too large.
            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)                          vSP0 = V(F_ADD_VV)( &psSP1->m_vOrigin, &vK0 );
                V(C_TgVEC)                          vK1 = V(F_SUB_VV)( &vSP0, &psSP0->m_vOrigin );
                V(C_TgVEC)                          vNormal = V(F_NORM)( &vK1 );
                V(C_TgVEC)                          vK2 = V(F_MUL_VS)( &vNormal, psSP1->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)( &vSP0, &vK2 );
                psContact->m_vN0 = vNormal;
                psContact->m_fT0 = fT0;
                psContact->m_fDepth = MKL(0.0);

                ++psPacket->m_niContact;

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