// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Box AA-Box AA.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".
//
// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //




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

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

// ---- F_DistSq ---------------------------------------------------------------------------------------------------------------- //
// Input:  tgBA0, tgBA1: Box, Axis-Aligned primitive
// Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TYPE F_DistSq( CR_(BOXAA,DIM) tgBA0, CR_(BOXAA,DIM) tgBA1 )
{
    TYPE                                tyDistSq = TYPE(0.0);

    if (tgBA1.Query_MaxX() < tgBA0.Query_MinX())
    {
        tyDistSq += (tgBA0.Query_MinX() - tgBA1.Query_MaxX())*(tgBA0.Query_MinX() - tgBA1.Query_MaxX());
    }
    else if (tgBA0.Query_MaxX() < tgBA1.Query_MinX())
    {
        tyDistSq += (tgBA1.Query_MinX() - tgBA0.Query_MaxX())*(tgBA1.Query_MinX() - tgBA0.Query_MaxX());
    };

    if (tgBA1.Query_MaxY() < tgBA0.Query_MinY())
    {
        tyDistSq += (tgBA0.Query_MinY() - tgBA1.Query_MaxY())*(tgBA0.Query_MinY() - tgBA1.Query_MaxY());
    }
    else if (tgBA0.Query_MaxY() < tgBA1.Query_MinY())
    {
        tyDistSq += (tgBA1.Query_MinY() - tgBA0.Query_MaxY())*(tgBA1.Query_MinY() - tgBA0.Query_MaxY());
    };

    if (tgBA1.Query_MaxZ() < tgBA0.Query_MinZ())
    {
        tyDistSq += (tgBA0.Query_MinZ() - tgBA1.Query_MaxZ())*(tgBA0.Query_MinZ() - tgBA1.Query_MaxZ());
    }
    else if (tgBA0.Query_MaxZ() < tgBA1.Query_MinZ())
    {
        tyDistSq += (tgBA1.Query_MinZ() - tgBA0.Query_MaxZ())*(tgBA1.Query_MinZ() - tgBA0.Query_MaxZ());
    };

    return (tyDistSq <= TYPE(0.0) ? -LIMITS<TYPE>::MAX : tyDistSq);
};

template TgFLOAT32 F_DistSq( CR_TgF4BOXAA, CR_TgF4BOXAA );


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

// ---- F_ClosestSq ------------------------------------------------------------------------------------------------------------- //
// Input:  tgBA0, tgBA1: Box, Axis-Aligned primitive
// Output: tvBA0, tvBA1: 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.
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TYPE F_ClosestSq( PC_(VECTOR,DIM) ptvBA0, PC_(VECTOR,DIM) ptvBA1, CR_(BOXAA,DIM) tgBA0, CR_(BOXAA,DIM) tgBA1 )
{
    TYPE                                tyX0,tyX1, tyY0,tyY1, tyZ0,tyZ1;
    TYPE                                tyDistSq = TYPE(0.0);

    // First (X) co-ordinate value.

    if (tgBA1.Query_MaxX() < tgBA0.Query_MinX())
    {
        tyDistSq += (tgBA0.Query_MinX() - tgBA1.Query_MaxX())*(tgBA0.Query_MinX() - tgBA1.Query_MaxX());

        tyX0 = tgBA0.Query_MinX();
        tyX1 = tgBA1.Query_MaxX();
    }
    else if (tgBA0.Query_MaxX() < tgBA1.Query_MinX())
    {
        tyDistSq += (tgBA1.Query_MinX() - tgBA0.Query_MaxX())*(tgBA1.Query_MinX() - tgBA0.Query_MaxX());

        tyX0 = tgBA0.Query_MaxX();
        tyX1 = tgBA1.Query_MinX();
    }
    else
    {
        register TYPE tyT;

        tyT  = P::FSEL( tgBA0.Query_MinX() - tgBA1.Query_MinX(), tgBA0.Query_MinX(), tgBA1.Query_MinX() );
        tyT += P::FSEL( tgBA1.Query_MaxX() - tgBA0.Query_MaxX(), tgBA0.Query_MaxX(), tgBA1.Query_MaxX() );
        tyT *= TYPE(0.5);

        tyX0 = tyT; tyX1 = tyT;
    };


    // Second (Y) co-ordinate value.

    if (tgBA1.Query_MaxY() < tgBA0.Query_MinY())
    {
        tyDistSq += (tgBA0.Query_MinY() - tgBA1.Query_MaxY())*(tgBA0.Query_MinY() - tgBA1.Query_MaxY());

        tyY0 = tgBA0.Query_MinY();
        tyY1 = tgBA1.Query_MaxY();
    }
    else if (tgBA0.Query_MaxY() < tgBA1.Query_MinY())
    {
        tyDistSq += (tgBA1.Query_MinY() - tgBA0.Query_MaxY())*(tgBA1.Query_MinY() - tgBA0.Query_MaxY());

        tyY0 = tgBA0.Query_MaxY();
        tyY1 = tgBA1.Query_MinY();
    }
    else
    {
        register TYPE tyT;

        tyT  = P::FSEL( tgBA0.Query_MinY() - tgBA1.Query_MinY(), tgBA0.Query_MinY(), tgBA1.Query_MinY() );
        tyT += P::FSEL( tgBA1.Query_MaxY() - tgBA0.Query_MaxY(), tgBA0.Query_MaxY(), tgBA1.Query_MaxY() );
        tyT *= TYPE(0.5);

        tyY0 = tyT; tyY1 = tyT;
    };


    // Third (Z) co-ordinate value.

    if (tgBA1.Query_MaxZ() < tgBA0.Query_MinZ())
    {
        tyDistSq += (tgBA0.Query_MinZ() - tgBA1.Query_MaxZ())*(tgBA0.Query_MinZ() - tgBA1.Query_MaxZ());

        tyZ0 = tgBA0.Query_MinZ();
        tyZ1 = tgBA1.Query_MaxZ();
    }
    else if (tgBA0.Query_MaxZ() < tgBA1.Query_MinZ())
    {
        tyDistSq += (tgBA1.Query_MinZ() - tgBA0.Query_MaxZ())*(tgBA1.Query_MinZ() - tgBA0.Query_MaxZ());

        tyZ0 = tgBA0.Query_MaxZ();
        tyZ1 = tgBA1.Query_MinZ();
    }
    else
    {
        register TYPE tyT;

        tyT  = P::FSEL( tgBA0.Query_MinZ() - tgBA1.Query_MinZ(), tgBA0.Query_MinZ(), tgBA1.Query_MinZ() );
        tyT += P::FSEL( tgBA1.Query_MaxZ() - tgBA0.Query_MaxZ(), tgBA0.Query_MaxZ(), tgBA1.Query_MaxZ() );
        tyT *= TYPE(0.5);

        tyZ0 = tyT; tyZ1 = tyT;
    };

    *ptvBA0 = MATH::F_SETP<TYPE,DIM>( tyX0,tyY0,tyZ0 );
    *ptvBA1 = MATH::F_SETP<TYPE,DIM>( tyX1,tyY1,tyZ1 );

    return (tyDistSq <= TYPE(0.0) ? -LIMITS<TYPE>::MAX : tyDistSq);
};

template TgFLOAT32 F_ClosestSq( PC_TgF4VECTOR, PC_TgF4VECTOR, CR_TgF4BOXAA, CR_TgF4BOXAA );


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

// ---- F_Contact_Penetrate ----------------------------------------------------------------------------------------------------- //
// 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
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Penetrate( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(BOXAA,DIM) tgBA0, CR_(BOXAA,DIM) tgBA1 )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgBOXAA<<16)|(TgUINT)ETgBOXAA))

    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ))
    TgASSERT(tgBA1.Is_Valid() && tgBA0.Is_Valid())

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

    register TgINT                      iAxis = -1;
    register TYPE                       tyT, tyDepth = -LIMITS<TYPE>::MAX;


    // First (X) co-ordinate value.

    tyT = tgBA1.Query_MinX() - tgBA0.Query_MaxX();
    if (tyT > TYPE(0.0))
    {
        return (TgE_NOINTERSECT);
    };

    if (tyT > tyDepth)
    {
        tyDepth = tyT;
        iAxis = 0;
    };

    tyT = tgBA0.Query_MinX() - tgBA1.Query_MaxX();
    if (tyT > TYPE(0.0))
    {
        return (TgE_NOINTERSECT);
    };

    if (tyT > tyDepth)
    {
        tyDepth = tyT;
        iAxis = 1;
    };


    // First (Y) co-ordinate value.

    tyT = tgBA1.Query_MinY() - tgBA0.Query_MaxY();
    if (tyT > TYPE(0.0))
    {
        return (TgE_NOINTERSECT);
    };

    if (tyT > tyDepth)
    {
        tyDepth = tyT;
        iAxis = 2;
    };

    tyT = tgBA0.Query_MinY() - tgBA1.Query_MaxY();
    if (tyT > TYPE(0.0))
    {
        return (TgE_NOINTERSECT);
    };

    if (tyT > tyDepth)
    {
        tyDepth = tyT;
        iAxis = 3;
    };


    // First (Z) co-ordinate value.

    tyT = tgBA1.Query_MinZ() - tgBA0.Query_MaxZ();
    if (tyT > TYPE(0.0))
    {
        return (TgE_NOINTERSECT);
    };

    if (tyT > tyDepth)
    {
        tyDepth = tyT;
        iAxis = 4;
    };

    tyT = tgBA0.Query_MinZ() - tgBA1.Query_MaxZ();
    if (tyT > TYPE(0.0))
    {
        return (TgE_NOINTERSECT);
    };

    if (tyT > tyDepth)
    {
        tyDepth = tyT;
        iAxis = 5;
    };


    // 

    T_(VECTOR,DIM)                      tvD0,tvD1, tvP0, tvNM;

    switch (iAxis) {
    case 0:
    case 1: {
        register TYPE tyY0 = P::FSEL( tgBA1.Query_MinY() - tgBA0.Query_MinY(), tgBA1.Query_MinY(), tgBA0.Query_MinY() );
        register TYPE tyZ0 = P::FSEL( tgBA1.Query_MinZ() - tgBA0.Query_MinZ(), tgBA1.Query_MinZ(), tgBA0.Query_MinZ() );
        register TYPE tyY1 = P::FSEL( tgBA0.Query_MaxY() - tgBA1.Query_MaxY(), tgBA1.Query_MaxY(), tgBA0.Query_MaxY() );
        register TYPE tyZ1 = P::FSEL( tgBA0.Query_MaxZ() - tgBA1.Query_MaxZ(), tgBA1.Query_MaxZ(), tgBA0.Query_MaxZ() );

        tvD0 = MATH::F_SETV<TYPE,DIM>( TYPE(0.0), tyY1 - tyY0, TYPE(0.0) );
        tvD1 = MATH::F_SETV<TYPE,DIM>( TYPE(0.0), TYPE(0.0), tyZ1 - tyZ0 );
        tvP0 = MATH::F_SETP<TYPE,DIM>( 0 == iAxis ? tgBA1.Query_MinX() : tgBA1.Query_MaxX(), tyY0, tyZ0 );
        tvNM = MATH::F_SETV<TYPE,DIM>( 0 == iAxis ? TYPE(1.0) : TYPE(-1.0), TYPE(0.0), TYPE(0.0) );
        
        break;
    };

    case 2:
    case 3: {
        register TYPE tyX0 = P::FSEL( tgBA1.Query_MinX() - tgBA0.Query_MinX(), tgBA1.Query_MinX(), tgBA0.Query_MinX() );
        register TYPE tyZ0 = P::FSEL( tgBA1.Query_MinZ() - tgBA0.Query_MinZ(), tgBA1.Query_MinZ(), tgBA0.Query_MinZ() );
        register TYPE tyX1 = P::FSEL( tgBA0.Query_MaxX() - tgBA1.Query_MaxX(), tgBA1.Query_MaxX(), tgBA0.Query_MaxX() );
        register TYPE tyZ1 = P::FSEL( tgBA0.Query_MaxZ() - tgBA1.Query_MaxZ(), tgBA1.Query_MaxZ(), tgBA0.Query_MaxZ() );

        tvD0 = MATH::F_SETV<TYPE,DIM>( tyX1 - tyX0, TYPE(0.0), TYPE(0.0) );
        tvD1 = MATH::F_SETV<TYPE,DIM>( TYPE(0.0), TYPE(0.0), tyZ1 - tyZ0 );
        tvP0 = MATH::F_SETP<TYPE,DIM>( tyX0, 2 == iAxis ? tgBA1.Query_MinY() : tgBA1.Query_MaxY(), tyZ0 );
        tvNM = MATH::F_SETV<TYPE,DIM>( TYPE(0.0), 2 == iAxis ? TYPE(1.0) : TYPE(-1.0), TYPE(0.0) );

        break;
    };

    case 4:
    case 5: {
        register TYPE tyX0 = P::FSEL( tgBA1.Query_MinX() - tgBA0.Query_MinX(), tgBA1.Query_MinX(), tgBA0.Query_MinX() );
        register TYPE tyY0 = P::FSEL( tgBA1.Query_MinY() - tgBA0.Query_MinY(), tgBA1.Query_MinY(), tgBA0.Query_MinY() );
        register TYPE tyX1 = P::FSEL( tgBA0.Query_MaxX() - tgBA1.Query_MaxX(), tgBA1.Query_MaxX(), tgBA0.Query_MaxX() );
        register TYPE tyY1 = P::FSEL( tgBA0.Query_MaxY() - tgBA1.Query_MaxY(), tgBA1.Query_MaxY(), tgBA0.Query_MaxY() );

        tvD0 = MATH::F_SETV<TYPE,DIM>( tyX1 - tyX0, TYPE(0.0), TYPE(0.0) );
        tvD1 = MATH::F_SETV<TYPE,DIM>( TYPE(0.0), tyY1 - tyY0, TYPE(0.0) );
        tvP0 = MATH::F_SETP<TYPE,DIM>( tyX0, tyY0, 4 == iAxis ? tgBA1.Query_MinZ() : tgBA1.Query_MaxZ() );
        tvNM = MATH::F_SETV<TYPE,DIM>( TYPE(0.0), TYPE(0.0), 4 == iAxis ? TYPE(1.0) : TYPE(-1.0) );

        break;
    };

    default:
        COUT_NOOBJ(
            ERROR, TgT("%-16.16s(%-48.48s): [BA][BA] Algorithm - Illegal separation axis id.\n"), TgT("Collision"),
            TgT("F_Contact_Penetrate")
        );

        return (TgE_FAIL);
    };

    P_(CONTACT,DIM)                     ptgContact;

    // Point 1

    ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

    ptgContact->m_tvPos = tvP0;
    ptgContact->m_tvNormal = tvNM;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = -tyDepth;

    ++ptgPacket->m_niContact;

    // Point 2

    if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
    {
        return (TgS_MAXCONTACTS);
    };

    ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

    ptgContact->m_tvPos = MATH::F_ADD( tvP0, tvD0 );
    ptgContact->m_tvNormal = tvNM;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = -tyDepth;

    ++ptgPacket->m_niContact;

    // Point 3

    if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
    {
        return (TgS_MAXCONTACTS);
    };

    ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

    ptgContact->m_tvPos = MATH::F_ADD( tvP0, tvD1 );
    ptgContact->m_tvNormal = tvNM;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = -tyDepth;

    ++ptgPacket->m_niContact;

    // Point 4

    if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact)
    {
        return (TgS_MAXCONTACTS);
    };

    ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

    ptgContact->m_tvPos = MATH::F_ADD( MATH::F_ADD( tvP0, tvD0 ), tvD1 );
    ptgContact->m_tvNormal = tvNM;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = -tyDepth;

    ++ptgPacket->m_niContact;

    return (TgS_OK);
};

template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4BOXAA, CR_TgF4BOXAA );


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

}; // END COL //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}; // END TGS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////