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

TgINLINE TYPE V(tgCO_F_DistSq_BA_SP)( V(CPCU_TgBOXAA) psBA0, V(CPCU_TgSPHERE) psSP0 )
{
    const TYPE                          fDistSq = V(tgCO_F_DistSq_BA_VT)( psBA0, &psSP0->m_vOrigin );
    const TYPE                          fSP0_RadSq = psSP0->m_fRadiusSq;
    const TYPE                          fSP0_Rad = psSP0->m_fRadius;
    const TYPE                          fDist = F(tgPM_FSEL)( fDistSq - fSP0_RadSq, F(tgPM_SQRT)( fDistSq ), MKL(-1.0) );

    return (F(tgPM_FSEL)( fDist, (fDist - fSP0_Rad)*(fDist - fSP0_Rad), -F(KTgMAX) ));
}


TgINLINE TYPE V(tgCO_F_Dist_BA_SP)( V(CPCU_TgBOXAA) psBA0, V(CPCU_TgSPHERE) psSP0 )
{
    const TYPE                          fDistSq = V(tgCO_F_DistSq_BA_VT)( psBA0, &psSP0->m_vOrigin );

    return (F(tgPM_FSEL)( fDistSq - psSP0->m_fRadiusSq, F(tgPM_SQRT)( fDistSq ) - psSP0->m_fRadius, -F(KTgMAX) ));
}


TgINLINE TYPE V(tgCO_F_ClosestSq_BA_SP)( V(PCU_TgVEC) pvBA0, V(PCU_TgVEC) pvSP0, V(CPCU_TgBOXAA) psBA0, V(CPCU_TgSPHERE) psSP0 )
{
    const TYPE                          fDist = V(tgCO_F_Closest_BA_SP)( pvBA0, pvSP0, psBA0, psSP0 );

    return (F(tgPM_FSEL)( fDist, fDist*fDist, -F(KTgMAX) ));
}


TgINLINE TYPE V(tgCO_F_Closest_BA_SP)( V(PCU_TgVEC) pvBA0, V(PCU_TgVEC) pvSP0, V(CPCU_TgBOXAA) psBA0, V(CPCU_TgSPHERE) psSP0 )
{
    const TYPE                          fDistSq = V(tgCO_F_ClosestSq_BA_VT)( pvBA0, psBA0, &psSP0->m_vOrigin  );
    const TYPE                          fSP0_RadSq = psSP0->m_fRadiusSq;
    const TYPE                          fDist = F(tgPM_FSEL)( fDistSq - fSP0_RadSq, F(tgPM_SQRT)( fDistSq ), MKL(-1.0) );
    const TYPE                          fK0 = psSP0->m_fRadius / fDist;
    V(C_TgVEC)                          vK1 = V(F_SUB_VV)( pvBA0, &psSP0->m_vOrigin  );
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fK0, &vK1 );

    *pvSP0 = V(F_ADD_VV)( &psSP0->m_vOrigin , &vK0 );

    return (F(tgPM_FSEL)( fDist, fDist - psSP0->m_fRadius, -F(KTgMAX) ));
}


TgINLINE TgBOOL V(tgCO_F_Test_BA_SP)( V(CPCU_TgBOXAA) psBA0, V(CPCU_TgSPHERE) psSP0 )
{
    register TYPE                       fTN, fTX, fDistSq = MKL(0.0);
    V(C_TgVEC)                          vS0 = psSP0->m_vOrigin;

    fTN = vS0.m.x - psBA0->m_vMin.m.x;
    fTX = psBA0->m_vMax.m.x - vS0.m.x;
    fDistSq += F(tgPM_FSEL)( fTN, MKL(0.0), fTN*fTN );
    fDistSq += F(tgPM_FSEL)( fTX, MKL(0.0), fTX*fTX );

    fTN = vS0.m.y - psBA0->m_vMin.m.y;
    fTX = psBA0->m_vMax.m.y - vS0.m.y;
    fDistSq += F(tgPM_FSEL)( fTN, MKL(0.0), fTN*fTN );
    fDistSq += F(tgPM_FSEL)( fTX, MKL(0.0), fTX*fTX );

    fTN = vS0.m.z - psBA0->m_vMin.m.z;
    fTX = psBA0->m_vMax.m.z - vS0.m.z;
    fDistSq += F(tgPM_FSEL)( fTN, MKL(0.0), fTN*fTN );
    fDistSq += F(tgPM_FSEL)( fTX, MKL(0.0), fTX*fTX );

    return (fDistSq <= psSP0->m_fRadiusSq);
}


TgINLINE TgRESULT V(tgCO_F_Penetrate_BA_SP)( V(PCU_STg2_CO_Packet) psPacket, V(CPCU_TgBOXAA) psBA0, V(CPCU_TgSPHERE) psSP0 )
{
    C_TgSINT32                          niContact = psPacket->m_niContact;
    TgSINT32                            iIdx;
    V(P_STg2_CO_Contact)                psContact;

    C_TgRESULT tgResult = V(tgCO_F_Penetrate_SP_BA)( psPacket, psSP0, psBA0 );

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

    for (iIdx = niContact; iIdx < psPacket->m_niContact; ++iIdx)
    {
        V(TgVEC)                            vK0;

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

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

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

    return (tgResult);
}