/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Box AA-Sphere.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Box;Axis-Aligned;BoxAA;Sphere;
    »Author«    Andrew Aye (EMail: mailto:andrew.aye@gmail.com, Web: http://www.andrewaye.com)
    »Version«   4.51 / »GUID« A9981407-3EC9-42AF-8B6F-8BE6DD919615                                                                                                        */
/*   -------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
/*  Copyright: © 2002-2017, 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:
        Redistribution of source code must retain this copyright notice, this list of conditions and the following disclaimers.
        Redistribution 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.
    The name of the author may not 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 ========================================================================================================================================================= */

/* ---- V(tgCO_F_SP_Penetrate_BA) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psSP0: Sphere primitive                                                                                                                                        */
/* Input:  tgBA0: Box, Axis-Aligned primitive - contact points are generated on this primitive                                                                            */
/* Output: tgPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_SP_Penetrate_BA)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgSPHERE) psSP0, V(CPC_TgBOXAA) psBA0 )
    V(P_STg2_CO_Contact)                psContact;

    TgPARAM_CHECK( V(tgGM_BA_Is_Valid)(psBA0) && V(tgGM_SP_Is_Valid)(psSP0) );

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
        return (KTgE_FAIL);
        /* Find the point of closest proximity on the box to the sphere's origin and create a normalized direction vector of the difference between the two points. */
        /* These are the potential contact points and normal. */

        const TYPE                          fX00 = psSP0->m_vOrigin.m.x - psBA0->m_vMin.m.x;
        const TYPE                          fX10 = psSP0->m_vOrigin.m.y - psBA0->m_vMin.m.y;
        const TYPE                          fX20 = psSP0->m_vOrigin.m.z - psBA0->m_vMin.m.z;
        const TYPE                          fX01 = psBA0->m_vMax.m.x - psSP0->m_vOrigin.m.x;
        const TYPE                          fX11 = psBA0->m_vMax.m.y - psSP0->m_vOrigin.m.y;
        const TYPE                          fX21 = psBA0->m_vMax.m.z - psSP0->m_vOrigin.m.z;
        const TYPE                          fK3 = F(tgPM_FSEL)(fX01, psSP0->m_vOrigin.m.x, psBA0->m_vMax.m.x);
        const TYPE                          fK4 = F(tgPM_FSEL)(fX11, psSP0->m_vOrigin.m.y, psBA0->m_vMax.m.y);
        const TYPE                          fK5 = F(tgPM_FSEL)(fX21, psSP0->m_vOrigin.m.z, psBA0->m_vMax.m.z);
        const TYPE                          fX = F(tgPM_FSEL)(fX00, fK3, psBA0->m_vMin.m.x);
        const TYPE                          fY = F(tgPM_FSEL)(fX10, fK4, psBA0->m_vMin.m.y);
        const TYPE                          fZ = F(tgPM_FSEL)(fX20, fK5, psBA0->m_vMin.m.z);
        V(C_TgVEC)                          vP1 = V(FS_SETP)(fX, fY, fZ);
        V(TgVEC)                            vNormal = V(F_SUB)(&vP1, &psSP0->m_vOrigin);

        psContact = psPacket->m_psContact + psPacket->m_niContact;

        if (V(F_LSQ)(&vNormal) > F(KTgEPS))
            TYPE                                fNM;

            vNormal = V(F_NORM_LEN)(&fNM, &vNormal);

            if (fNM > F(KTgEPS))
                /* Sphere origin is not contained inside of the box, and therefore the potential contact values are correct. */

                psContact->m_vS0 = vP1;
                psContact->m_vN0 = vNormal;
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = psSP0->m_fRadius - fNM;


                return (KTgS_OK);

            /* Sphere origin is contained within the box itself - find the axis of minimum penetration. */

            const TYPE                          fX0 = F(tgPM_FSEL)(fX01 - fX00, fX00, fX01);
            const TYPE                          fX1 = F(tgPM_FSEL)(fX11 - fX10, fX10, fX11);
            const TYPE                          fX2 = F(tgPM_FSEL)(fX21 - fX20, fX20, fX21);

            const TYPE                          fK0 = fX1 - fX0;
            const TYPE                          fK1 = fX2 - fX0;
            const TYPE                          fK2 = fX2 - fX1;

            const TYPE                          fTX = F(tgPM_FSEL)(fX01 - fX00, psBA0->m_vMin.m.x, psBA0->m_vMax.m.x);
            const TYPE                          fTY = F(tgPM_FSEL)(fX11 - fX10, psBA0->m_vMin.m.y, psBA0->m_vMax.m.y);
            const TYPE                          fTZ = F(tgPM_FSEL)(fX21 - fX20, psBA0->m_vMin.m.z, psBA0->m_vMax.m.z);

            const TYPE                          fK6 = F(tgPM_FSEL)(fK1, fTX, psSP0->m_vOrigin.m.x);
            const TYPE                          fK7 = F(tgPM_FSEL)(fK2, fTY, psSP0->m_vOrigin.m.y);
            const TYPE                          fK8 = F(tgPM_FSEL)(fK1, psSP0->m_vOrigin.m.x, fTZ);
            const TYPE                          fK9 = F(tgPM_FSEL)(fK2, psSP0->m_vOrigin.m.z, fTZ);

            const TYPE                          fKTX = F(tgPM_FSEL)(fK0, fK6, psSP0->m_vOrigin.m.x);
            const TYPE                          fKTY = F(tgPM_FSEL)(fK0, psSP0->m_vOrigin.m.y, fK7);
            const TYPE                          fKTZ = F(tgPM_FSEL)(fK0, fK8, fK9);

            const TYPE                          fNX = F(tgPM_FSEL)(fX01 - fX00, MKL(1.0), MKL(-1.0));
            const TYPE                          fNY = F(tgPM_FSEL)(fX11 - fX10, MKL(1.0), MKL(-1.0));
            const TYPE                          fNZ = F(tgPM_FSEL)(fX21 - fX20, MKL(1.0), MKL(-1.0));

            const TYPE                          fKA = F(tgPM_FSEL)(fK1, fNX, MKL(0.0));
            const TYPE                          fKB = F(tgPM_FSEL)(fK2, fNY, MKL(0.0));
            const TYPE                          fKC = F(tgPM_FSEL)(fK1, MKL(0.0), fNZ);
            const TYPE                          fKD = F(tgPM_FSEL)(fK2, MKL(0.0), fNZ);

            const TYPE                          fKNX = F(tgPM_FSEL)(fK0, fKA, MKL(0.0));
            const TYPE                          fKNY = F(tgPM_FSEL)(fK0, MKL(0.0), fKB);
            const TYPE                          fKNZ = F(tgPM_FSEL)(fK0, fKC, fKD);

            const TYPE                          fKE = F(tgPM_FSEL)(fK1, fX0, fX2);
            const TYPE                          fKF = F(tgPM_FSEL)(fK2, fX1, fX2);

            psContact->m_vS0 = V(FS_SETP)(fKTX, fKTY, fKTZ);
            psContact->m_vN0 = V(FS_SETV)(fKNX, fKNY, fKNZ);
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = psSP0->m_fRadius + F(tgPM_FSEL)(fK0, fKE, fKF);

            return (KTgS_OK);