[an error occurred while processing this directive] 
// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Sphere-Sphere.cpp
//  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".
//
// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //


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

// 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: Prepenetration - 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.
// 

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




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

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

// ---- F_Closest --------------------------------------------------------------------------------------------------------------- //
// Input:  tgSP0,tgSP1: Sphere primitives
// Output: tvSP0,tvSP1: 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.
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TYPE F_Closest( PC_(VECTOR,DIM) ptvSP0, PC_(VECTOR,DIM) ptvSP1, CR_(SPHERE,DIM) tgSP0, CR_(SPHERE,DIM) tgSP1 )
{
    TgASSERT(tgSP0.Is_Valid() && tgSP1.Is_Valid());

    TYPE                                tyDS;
    C_(VECTOR,DIM)                      tvDS = MATH::F_NORM( &tyDS, MATH::F_SUB( tgSP1.Query_Origin(), tgSP0.Query_Origin() ) );
    const TYPE                          tySumRad = tgSP0.Query_Radius() + tgSP1.Query_Radius();

    if (tyDS <= tySumRad)
    {
        return (-LIMITS<TYPE>::MAX);
    };

    *ptvSP0 = MATH::F_ADD( tgSP0.Query_Origin(), MATH::F_MUL( tgSP0.Query_Radius(), tvDS ) );
    *ptvSP1 = MATH::F_SUB( tgSP1.Query_Origin(), MATH::F_MUL( tgSP1.Query_Radius(), tvDS ) );

    return (tyDS - tySumRad);
};

template TgFLOAT32 F_Closest( PC_TgF4VECTOR, PC_TgF4VECTOR, CR_TgF4SPHERE, CR_TgF4SPHERE );


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

// ---- F_Contact_Penetrate ----------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgSP0,tgSP1: Sphere primitives - contact points are generated on sphere #2 (tgSP1)
// Output: tgPacket: Points of penetration between the two primitives are added to it
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Penetrate( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(SPHERE,DIM) tgSP0, CR_(SPHERE,DIM) tgSP1 )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgSPHERE<<16)|(TgUINT)ETgSPHERE));

    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ));
    TgASSERT(tgSP1.Is_Valid() && tgSP0.Is_Valid());

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

    TYPE                                tyDS;
    T_(VECTOR,DIM)                      tvDS = MATH::F_NORM( &tyDS, MATH::F_SUB( tgSP1.Query_Origin(), tgSP0.Query_Origin() ) );
    const TYPE                          tySumRad = tgSP1.Query_Radius() + tgSP0.Query_Radius();

    if (tyDS > tySumRad)
    {
        return (TgE_NOINTERSECT);
    };

    if (tyDS <= LIMITS<TYPE>::EPSILON)
    {
        tvDS = T_(VECTOR,DIM)::UNIT_Y;
    };

    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( tgSP1.Query_Origin(), MATH::F_MUL( tvDS, tgSP1.Query_Radius() ) );
    ptgContact->m_tvNormal = tvDS;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = tySumRad - tyDS;

    ++ptgPacket->m_niContact;

    return (TgS_OK);
};

template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4SPHERE, CR_TgF4SPHERE );


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

// ---- F_Contact_Sweep --------------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tyPM: 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:  tgSP0,tgSP1: Sphere primitives
// Input:  tgDT: 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: tyPM: New normalized time of first contact
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Sweep( PC_(CONTACT_PACKET,DIM) ptgPacket, TYPE *ptyPM, CR_(SPHERE,DIM) tgSP0, CR_(SPHERE,DIM) tgSP1, CR_(DELTA,DIM) tgDT )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_SWEEP, (((TgUINT)ETgSPHERE<<16)|(TgUINT)ETgSPHERE));

    TgASSERT(tgSP1.Is_Valid() && tgSP0.Is_Valid());

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

    // Check for pre-penetration.

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

    if (tyDS_DS <= tyRS2)
    {   // Pre-Penetration.
        if (*ptyPM > ptgPacket->m_tySweepTol)
        {
            ptgPacket->m_niContact = 0;
        };

        *ptyPM = TYPE(0.0);

        C_TgBOOL                            bPenetrate = TgTRUE == ptgPacket->m_bReport_Penetration;

        if (bPenetrate && TgS_MAXCONTACTS == F_Contact_Penetrate( ptgPacket, tgSP0, tgSP1 ))
        {
            return (TgE_PREPENETRATION_MAXCONTACTS);
        };

        return (TgE_PREPENETRATION);
    };

    TYPE                                tyTMPC = tgDT.m_tyDT - LIMITS<TYPE>::EPSILON;
    const TYPE                          tyDS_UDT = MATH::F_DOT(tvDS,tgDT.m_tvUDT);

    tyTMPC = P::FSEL( tyTMPC, tyDS_UDT - LIMITS<TYPE>::EPSILON, TYPE(-1.0) ); // Negligible distance towards each other.
    tyTMPC = P::FSEL( tyTMPC, tyRS + *ptyPM*tgDT.m_tyDT - tyDS_UDT, TYPE(-1.0) ); // Separation along displacement is too large.
    tyTMPC = P::FSEL( tyTMPC, tyRS2 - tyDS_DS + tyDS_UDT*tyDS_UDT, TYPE(-1.0) ); // Orthogonal separation is too large.
    tyTMPC = P::FSEL( tyTMPC, tyDS_UDT - P::SQRT( tyTMPC ), TYPE(-1.0) ); // Time occurs after first time of contact.

    if (tyTMPC > (*ptyPM + ptgPacket->m_tySweepTol)*tgDT.m_tyDT)
    {
        return (TgE_NOINTERSECT); // Outside of sweep space.
    };

    const TYPE                          tyT0 = tyTMPC / tgDT.m_tyDT;

    if (tyT0 < *ptyPM - ptgPacket->m_tySweepTol)
    {
        ptgPacket->m_niContact = 0;
        *ptyPM = tyT0;
    };

    C_(VECTOR,DIM)                      tvSP0 = MATH::F_ADD( tgSP1.Query_Origin(), MATH::F_MUL( tyTMPC, tgDT.m_tvUDT ) );
    C_(VECTOR,DIM)                      tvNormal = MATH::F_NORM( MATH::F_SUB( tvSP0, tgSP0.Query_Origin() ) );

    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( tvSP0, MATH::F_MUL( tvNormal, tgSP1.Query_Radius() ) );
    ptgContact->m_tvNormal = tvNormal;
    ptgContact->m_tyT0 = tyT0;
    ptgContact->m_tyDepth = TYPE(0.0);

    ++ptgPacket->m_niContact;

    return (TgS_OK);
};

template TgRESULT F_Contact_Sweep( PC_TgF4CONTACT_PACKET, P_TgFLOAT32, CR_TgF4SPHERE, CR_TgF4SPHERE, CR_TgF4DELTA );


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

}; // END COL //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}; // END TGS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
[an error occurred while processing this directive]