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

// ---- tgCO_F_DistSq_BA_BA ----------------------------------------------------------------------------------------------------- //
// Input:  tgBA0, tgBA1: Box, Axis-Aligned primitive
// Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.
// ------------------------------------------------------------------------------------------------------------------------------ //
TYPE V(tgCO_F_DistSq_BA_BA)( V(CPCU_TgBOXAA) psBA0, V(CPCU_TgBOXAA) psBA1 )
{
    TYPE                                fDistSq = MKL(0.0);
    
    if (psBA1->m_vMax.m.x < psBA0->m_vMin.m.x)
    {
        fDistSq += (psBA0->m_vMin.m.x - psBA1->m_vMax.m.x)*(psBA0->m_vMin.m.x - psBA1->m_vMax.m.x);
    }
    else if (psBA0->m_vMax.m.x < psBA1->m_vMin.m.x)
    {
        fDistSq += (psBA1->m_vMin.m.x - psBA0->m_vMax.m.x)*(psBA1->m_vMin.m.x - psBA0->m_vMax.m.x);
    };

    if (psBA1->m_vMax.m.y < psBA0->m_vMin.m.y)
    {
        fDistSq += (psBA0->m_vMin.m.y - psBA1->m_vMax.m.y)*(psBA0->m_vMin.m.y - psBA1->m_vMax.m.y);
    }
    else if (psBA0->m_vMax.m.y < psBA1->m_vMin.m.y)
    {
        fDistSq += (psBA1->m_vMin.m.y - psBA0->m_vMax.m.y)*(psBA1->m_vMin.m.y - psBA0->m_vMax.m.y);
    };

    if (psBA1->m_vMax.m.z < psBA0->m_vMin.m.z)
    {
        fDistSq += (psBA0->m_vMin.m.z - psBA1->m_vMax.m.z)*(psBA0->m_vMin.m.z - psBA1->m_vMax.m.z);
    }
    else if (psBA0->m_vMax.m.z < psBA1->m_vMin.m.z)
    {
        fDistSq += (psBA1->m_vMin.m.z - psBA0->m_vMax.m.z)*(psBA1->m_vMin.m.z - psBA0->m_vMax.m.z);
    };

    return (fDistSq <= MKL(0.0) ? -F(KTgMAX) : fDistSq);
}


// ---- tgCO_F_ClosestSq_BA_BA -------------------------------------------------------------------------------------------------- //
// Input:  tgBA0, tgBA1: Box, Axis-Aligned primitive
// Output: vBA0, vBA1: Point of closest proximity on the box, axis-aligned #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_ClosestSq_BA_BA)( V(PCU_TgVEC) pvBA0, V(PCU_TgVEC) pvBA1, V(CPCU_TgBOXAA) psBA0, V(CPCU_TgBOXAA) psBA1 )
{
    TYPE                                fX0,fX1, fY0,fY1, fZ0,fZ1;
    TYPE                                fDistSq = MKL(0.0);

    // First (X) co-ordinate value.

    if (psBA1->m_vMax.m.x < psBA0->m_vMin.m.x)
    {
        fDistSq += (psBA0->m_vMin.m.x - psBA1->m_vMax.m.x)*(psBA0->m_vMin.m.x - psBA1->m_vMax.m.x);

        fX0 = psBA0->m_vMin.m.x;
        fX1 = psBA1->m_vMax.m.x;
    }
    else if (psBA0->m_vMax.m.x < psBA1->m_vMin.m.x)
    {
        fDistSq += (psBA1->m_vMin.m.x - psBA0->m_vMax.m.x)*(psBA1->m_vMin.m.x - psBA0->m_vMax.m.x);

        fX0 = psBA0->m_vMax.m.x;
        fX1 = psBA1->m_vMin.m.x;
    }
    else
    {
        TYPE                                fT;

        fT  = F(tgPM_FSEL)( psBA0->m_vMin.m.x - psBA1->m_vMin.m.x, psBA0->m_vMin.m.x, psBA1->m_vMin.m.x );
        fT += F(tgPM_FSEL)( psBA1->m_vMax.m.x - psBA0->m_vMax.m.x, psBA0->m_vMax.m.x, psBA1->m_vMax.m.x );
        fT *= MKL(0.5);

        fX0 = fT; fX1 = fT;
    };


    // Second (Y) co-ordinate value.

    if (psBA1->m_vMax.m.y < psBA0->m_vMin.m.y)
    {
        fDistSq += (psBA0->m_vMin.m.y - psBA1->m_vMax.m.y)*(psBA0->m_vMin.m.y - psBA1->m_vMax.m.y);

        fY0 = psBA0->m_vMin.m.y;
        fY1 = psBA1->m_vMax.m.y;
    }
    else if (psBA0->m_vMax.m.y < psBA1->m_vMin.m.y)
    {
        fDistSq += (psBA1->m_vMin.m.y - psBA0->m_vMax.m.y)*(psBA1->m_vMin.m.y - psBA0->m_vMax.m.y);

        fY0 = psBA0->m_vMax.m.y;
        fY1 = psBA1->m_vMin.m.y;
    }
    else
    {
        TYPE                                fT;

        fT  = F(tgPM_FSEL)( psBA0->m_vMin.m.y - psBA1->m_vMin.m.y, psBA0->m_vMin.m.y, psBA1->m_vMin.m.y );
        fT += F(tgPM_FSEL)( psBA1->m_vMax.m.y - psBA0->m_vMax.m.y, psBA0->m_vMax.m.y, psBA1->m_vMax.m.y );
        fT *= MKL(0.5);

        fY0 = fT; fY1 = fT;
    };


    // Third (Z) co-ordinate value.

    if (psBA1->m_vMax.m.z < psBA0->m_vMin.m.z)
    {
        fDistSq += (psBA0->m_vMin.m.z - psBA1->m_vMax.m.z)*(psBA0->m_vMin.m.z - psBA1->m_vMax.m.z);

        fZ0 = psBA0->m_vMin.m.z;
        fZ1 = psBA1->m_vMax.m.z;
    }
    else if (psBA0->m_vMax.m.z < psBA1->m_vMin.m.z)
    {
        fDistSq += (psBA1->m_vMin.m.z - psBA0->m_vMax.m.z)*(psBA1->m_vMin.m.z - psBA0->m_vMax.m.z);

        fZ0 = psBA0->m_vMax.m.z;
        fZ1 = psBA1->m_vMin.m.z;
    }
    else
    {
        TYPE                                fT;

        fT  = F(tgPM_FSEL)( psBA0->m_vMin.m.z - psBA1->m_vMin.m.z, psBA0->m_vMin.m.z, psBA1->m_vMin.m.z );
        fT += F(tgPM_FSEL)( psBA1->m_vMax.m.z - psBA0->m_vMax.m.z, psBA0->m_vMax.m.z, psBA1->m_vMax.m.z );
        fT *= MKL(0.5);

        fZ0 = fT; fZ1 = fT;
    };

    *pvBA0 = V(F_SETP_ELEM)( fX0,fY0,fZ0 );
    *pvBA1 = V(F_SETP_ELEM)( fX1,fY1,fZ1 );

    return (fDistSq <= MKL(0.0) ? -F(KTgMAX) : fDistSq);
}


// ---- tgCO_F_Penetrate_BA_BA -------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgBA0: Box, Axis-Aligned primitive
// Input:  tgBA1: 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_Penetrate_BA_BA)( V(PCU_STg2_CO_Packet) psPacket, V(CPCU_TgBOXAA) psBA0, V(CPCU_TgBOXAA) psBA1 )
{
    TgSINT32                            iAxis = -1;
    TYPE                                fT, fDepth = -F(KTgMAX);
    V(TgVEC)                            vK0, vD0,vD1, vP0, vNM;
    V(P_STg2_CO_Contact)                psContact;

    TgASSERT_PARAM((TgSIZE)psPacket->m_iStride >= sizeof( V(P_STg2_CO_Contact) ));
    TgASSERT_PARAM(V(tgGM_Is_Valid_BA)( psBA1 ) && V(tgGM_Is_Valid_BA)( psBA0 ));

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || NULL == psPacket->m.psContact)
    {
        return (TgE_FAIL);
    };

    // First (X) co-ordinate value.

    fT = psBA1->m_vMin.m.x - psBA0->m_vMax.m.x;
    if (fT > MKL(0.0))
    {
        return (ETgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 0;
    };

    fT = psBA0->m_vMin.m.x - psBA1->m_vMax.m.x;
    if (fT > MKL(0.0))
    {
        return (ETgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 1;
    };


    // First (Y) co-ordinate value.

    fT = psBA1->m_vMin.m.y - psBA0->m_vMax.m.y;
    if (fT > MKL(0.0))
    {
        return (ETgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 2;
    };

    fT = psBA0->m_vMin.m.y - psBA1->m_vMax.m.y;
    if (fT > MKL(0.0))
    {
        return (ETgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 3;
    };


    // First (Z) co-ordinate value.

    fT = psBA1->m_vMin.m.z - psBA0->m_vMax.m.z;
    if (fT > MKL(0.0))
    {
        return (ETgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 4;
    };

    fT = psBA0->m_vMin.m.z - psBA1->m_vMax.m.z;
    if (fT > MKL(0.0))
    {
        return (ETgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 5;
    };


    switch (iAxis) {
    case 0:
    case 1: {
        const TYPE fY0 = F(tgPM_FSEL)( psBA1->m_vMin.m.y - psBA0->m_vMin.m.y, psBA1->m_vMin.m.y, psBA0->m_vMin.m.y );
        const TYPE fZ0 = F(tgPM_FSEL)( psBA1->m_vMin.m.z - psBA0->m_vMin.m.z, psBA1->m_vMin.m.z, psBA0->m_vMin.m.z );
        const TYPE fY1 = F(tgPM_FSEL)( psBA0->m_vMax.m.y - psBA1->m_vMax.m.y, psBA1->m_vMax.m.y, psBA0->m_vMax.m.y );
        const TYPE fZ1 = F(tgPM_FSEL)( psBA0->m_vMax.m.z - psBA1->m_vMax.m.z, psBA1->m_vMax.m.z, psBA0->m_vMax.m.z );

        vD0 = V(F_SETV_ELEM)( MKL(0.0), fY1 - fY0, MKL(0.0) );
        vD1 = V(F_SETV_ELEM)( MKL(0.0), MKL(0.0), fZ1 - fZ0 );
        vP0 = V(F_SETP_ELEM)( 0 == iAxis ? psBA1->m_vMin.m.x : psBA1->m_vMax.m.x, fY0, fZ0 );
        vNM = V(F_SETV_ELEM)( 0 == iAxis ? MKL(1.0) : MKL(-1.0), MKL(0.0), MKL(0.0) );
        
        break;
    };

    case 2:
    case 3: {
        const TYPE fX0 = F(tgPM_FSEL)( psBA1->m_vMin.m.x - psBA0->m_vMin.m.x, psBA1->m_vMin.m.x, psBA0->m_vMin.m.x );
        const TYPE fZ0 = F(tgPM_FSEL)( psBA1->m_vMin.m.z - psBA0->m_vMin.m.z, psBA1->m_vMin.m.z, psBA0->m_vMin.m.z );
        const TYPE fX1 = F(tgPM_FSEL)( psBA0->m_vMax.m.x - psBA1->m_vMax.m.x, psBA1->m_vMax.m.x, psBA0->m_vMax.m.x );
        const TYPE fZ1 = F(tgPM_FSEL)( psBA0->m_vMax.m.z - psBA1->m_vMax.m.z, psBA1->m_vMax.m.z, psBA0->m_vMax.m.z );

        vD0 = V(F_SETV_ELEM)( fX1 - fX0, MKL(0.0), MKL(0.0) );
        vD1 = V(F_SETV_ELEM)( MKL(0.0), MKL(0.0), fZ1 - fZ0 );
        vP0 = V(F_SETP_ELEM)( fX0, 2 == iAxis ? psBA1->m_vMin.m.y : psBA1->m_vMax.m.y, fZ0 );
        vNM = V(F_SETV_ELEM)( MKL(0.0), 2 == iAxis ? MKL(1.0) : MKL(-1.0), MKL(0.0) );

        break;
    };

    case 4:
    case 5: {
        const TYPE fX0 = F(tgPM_FSEL)( psBA1->m_vMin.m.x - psBA0->m_vMin.m.x, psBA1->m_vMin.m.x, psBA0->m_vMin.m.x );
        const TYPE fY0 = F(tgPM_FSEL)( psBA1->m_vMin.m.y - psBA0->m_vMin.m.y, psBA1->m_vMin.m.y, psBA0->m_vMin.m.y );
        const TYPE fX1 = F(tgPM_FSEL)( psBA0->m_vMax.m.x - psBA1->m_vMax.m.x, psBA1->m_vMax.m.x, psBA0->m_vMax.m.x );
        const TYPE fY1 = F(tgPM_FSEL)( psBA0->m_vMax.m.y - psBA1->m_vMax.m.y, psBA1->m_vMax.m.y, psBA0->m_vMax.m.y );

        vD0 = V(F_SETV_ELEM)( fX1 - fX0, MKL(0.0), MKL(0.0) );
        vD1 = V(F_SETV_ELEM)( MKL(0.0), fY1 - fY0, MKL(0.0) );
        vP0 = V(F_SETP_ELEM)( fX0, fY0, 4 == iAxis ? psBA1->m_vMin.m.z : psBA1->m_vMax.m.z );
        vNM = V(F_SETV_ELEM)( MKL(0.0), MKL(0.0), 4 == iAxis ? MKL(1.0) : MKL(-1.0) );

        break;
    };

    default:
        TgLOGF(
            ETgCON_CHANEL_ERROR, TgT("%-16.16s(%-32.32s): [BA][BA] Algorithm - Illegal separation axis id.\n"),
            TgT("Collision"), TgT("F_Penetrate")
        );
        TgS_NO_DEFAULT(return (TgE_FAIL));
    };

    // Point 1

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

    psContact->m_vS0 = vP0;
    psContact->m_vN0 = vNM;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = -fDepth;

    ++psPacket->m_niContact;

    // Point 2

    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
    {
        return (ETgE_MAX_CONTACTS);
    };

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

    psContact->m_vS0 = V(F_ADD_VV)( &vP0, &vD0 );
    psContact->m_vN0 = vNM;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = -fDepth;

    ++psPacket->m_niContact;

    // Point 3

    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
    {
        return (ETgE_MAX_CONTACTS);
    };

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

    psContact->m_vS0 = V(F_ADD_VV)( &vP0, &vD1 );
    psContact->m_vN0 = vNM;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = -fDepth;

    ++psPacket->m_niContact;

    // Point 4

    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
    {
        return (ETgE_MAX_CONTACTS);
    };

    psContact = (V(P_STg2_CO_Contact))(psPacket->m.piContact + psPacket->m_niContact*psPacket->m_iStride);
    
    vK0 = V(F_ADD_VV)( &vP0, &vD0 );
    psContact->m_vS0 = V(F_ADD_VV)( &vK0, &vD1 );
    psContact->m_vN0 = vNM;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = -fDepth;

    ++psPacket->m_niContact;

    return (TgS_OK);
}