// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Box-Plane.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_Closest --------------------------------------------------------------------------------------------------------------- //
// Input:  tgBX0: Box primitive
// Input:  tgPN0: Plane primitive
// Output: tyB0,tyB1,tyB2: Parametric parameters to generate the point of closest proximity on the box (one for each axis)
// Output: tvPN0: Point of closest proximity on the plane
// 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( TYPE *ptyB0, TYPE *ptyB1, TYPE *ptyB2, PC_(VECTOR,DIM) ptvPN0, CR_(BOX,DIM) tgBX0, CR_(PLANE,DIM) tgPN0 )
{
    TgASSERT(tgBX0.Is_Valid() && tgPN0.Is_Valid())

    C_(VECTOR,DIM)                      tvNM = tgPN0.Query_Normal();

    TYPE                                tyDist = F_Dist( tgPN0, tgBX0.Query_Origin() );

    const TYPE                          tyX0_N = MATH::F_DOT(tgBX0.Query_AxisUnit0(),tvNM);
    const TYPE                          tyX1_N = MATH::F_DOT(tgBX0.Query_AxisUnit1(),tvNM);
    const TYPE                          tyX2_N = MATH::F_DOT(tgBX0.Query_AxisUnit2(),tvNM);

    *ptyB0 = tyX0_N < -TTgEPSILON ? tgBX0.Query_Extent0() : (tyX0_N > TTgEPSILON ? -tgBX0.Query_Extent0() : TYPE(0.0));
    *ptyB1 = tyX1_N < -TTgEPSILON ? tgBX0.Query_Extent1() : (tyX1_N > TTgEPSILON ? -tgBX0.Query_Extent1() : TYPE(0.0));
    *ptyB2 = tyX2_N < -TTgEPSILON ? tgBX0.Query_Extent2() : (tyX2_N > TTgEPSILON ? -tgBX0.Query_Extent2() : TYPE(0.0));

    tyDist += *ptyB0*tyX0_N + *ptyB1*tyX1_N + *ptyB2*tyX2_N;

    *ptvPN0 = MATH::F_SETP<TYPE,DIM>( *ptyB0 - tyDist*tvNM(0), *ptyB1 - tyDist*tvNM(1), *ptyB2 - tyDist*tvNM(2) );

    return (P::FSEL( tyDist, tyDist, -LIMITS<TYPE>::MAX ));
};

template TgFLOAT32 F_Closest( P_TgFLOAT32, P_TgFLOAT32, P_TgFLOAT32, PC_TgF4VECTOR, CR_TgF4BOX, CR_TgF4PLANE );


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

// ---- F_Contact_Penetrate ----------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgPN0: Plane primitive
// Input:  tgBX0: Box primitive - contact points are generated on this primitive
// Output: tgPacket: Points of penetration between the two primitives are added to it
// Return: Result Code
//
//   This routine will create up to four contact points.  Collisions/Physics systems need the best(closest) approximation of the
//  contact surface.  To represent the plane of contact between one box face and the plane, its necessary to use all four points
//  that define the box rectangle (face).
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Penetrate( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(PLANE,DIM) tgPN0, CR_(BOX,DIM) tgBX0 )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgPLANE<<16)|(TgUINT)ETgBOX))

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

    TgTYPE_PREFIX( TYPE );
    TgTYPE_DECLARE( T_(VECTOR,DIM), TgVECTOR );

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

    const TYPE                          tyDist = F_Dist( tgPN0, tgBX0.Query_Origin() );

    C_(VECTOR,DIM)                      tvE0 = MATH::F_MUL( tgBX0.Query_AxisUnit0(), tgBX0.Query_Extent0() );
    C_(VECTOR,DIM)                      tvE1 = MATH::F_MUL( tgBX0.Query_AxisUnit1(), tgBX0.Query_Extent1() );
    C_(VECTOR,DIM)                      tvE2 = MATH::F_MUL( tgBX0.Query_AxisUnit2(), tgBX0.Query_Extent2() );

    const TYPE                          tyX0_N = MATH::F_DOT( tgPN0.Query_Normal(), tvE0 );
    const TYPE                          tyX1_N = MATH::F_DOT( tgPN0.Query_Normal(), tvE1 );
    const TYPE                          tyX2_N = MATH::F_DOT( tgPN0.Query_Normal(), tvE2 );

    const TYPE                          tyABS_X0_N = P::FSEL( tyX0_N, tyX0_N, -tyX0_N );
    const TYPE                          tyABS_X1_N = P::FSEL( tyX1_N, tyX1_N, -tyX1_N );
    const TYPE                          tyABS_X2_N = P::FSEL( tyX2_N, tyX2_N, -tyX2_N );

    TYPE                                tyMin0, tyMin1, tyMin2;
    T_(VECTOR,DIM)                      tvMin0, tvMin1, tvMin2;

    if (tyABS_X0_N <= tyABS_X1_N)
    {
        if (tyABS_X1_N <= tyABS_X2_N)
        {
            // tyABS_X0_N <= tyABS_X1_N <= tyABS_X2_N

            tyMin0 = tyABS_X0_N;
            tyMin1 = tyABS_X1_N;
            tyMin2 = tyABS_X2_N;

            tvMin0 = (tyX0_N > TYPE(0.0) ? tvE0 : MATH::F_NEG( tvE0 ) );
            tvMin1 = (tyX1_N > TYPE(0.0) ? tvE1 : MATH::F_NEG( tvE1 ) );
            tvMin2 = (tyX2_N > TYPE(0.0) ? tvE2 : MATH::F_NEG( tvE2 ) );
        }
        else if (tyABS_X0_N <= tyABS_X2_N)
        {
            // tyABS_X0_N <= tyABS_X2_N < tyABS_X1_N

            tyMin0 = tyABS_X0_N;
            tyMin1 = tyABS_X2_N;
            tyMin2 = tyABS_X1_N;

            tvMin0 = (tyX0_N > TYPE(0.0) ? tvE0 : MATH::F_NEG( tvE0 ) );
            tvMin1 = (tyX2_N > TYPE(0.0) ? tvE2 : MATH::F_NEG( tvE2 ) );
            tvMin2 = (tyX1_N > TYPE(0.0) ? tvE1 : MATH::F_NEG( tvE1 ) );
        }
        else 
        {
            // tyABS_X2_N < tyABS_X0_N <= tyABS_X1_N

            tyMin0 = tyABS_X2_N;
            tyMin1 = tyABS_X0_N;
            tyMin2 = tyABS_X1_N;

            tvMin0 = (tyX2_N > TYPE(0.0) ? tvE2 : MATH::F_NEG( tvE2 ) );
            tvMin1 = (tyX0_N > TYPE(0.0) ? tvE0 : MATH::F_NEG( tvE0 ) );
            tvMin2 = (tyX1_N > TYPE(0.0) ? tvE1 : MATH::F_NEG( tvE1 ) );
        };
    }
    else
    {
        if (tyABS_X2_N <= tyABS_X1_N)
        {
            // tyABS_X2_N < = tyABS_X1_N < tyABS_X0_N

            tyMin0 = tyABS_X2_N;
            tyMin1 = tyABS_X1_N;
            tyMin2 = tyABS_X0_N;

            tvMin0 = (tyX2_N > TYPE(0.0) ? tvE2 : MATH::F_NEG( tvE2 ) );
            tvMin1 = (tyX1_N > TYPE(0.0) ? tvE1 : MATH::F_NEG( tvE1 ) );
            tvMin2 = (tyX0_N > TYPE(0.0) ? tvE0 : MATH::F_NEG( tvE0 ) );
        }
        else if (tyABS_X2_N <= tyABS_X0_N)
        {
            // tyABS_X1_N < tyABS_X2_N <= tyABS_X0_N

            tyMin0 = tyABS_X1_N;
            tyMin1 = tyABS_X2_N;
            tyMin2 = tyABS_X0_N;

            tvMin0 = (tyX1_N > TYPE(0.0) ? tvE1 : MATH::F_NEG( tvE1 ) );
            tvMin1 = (tyX2_N > TYPE(0.0) ? tvE2 : MATH::F_NEG( tvE2 ) );
            tvMin2 = (tyX0_N > TYPE(0.0) ? tvE0 : MATH::F_NEG( tvE0 ) );
        }
        else
        {
            // tyABS_X1_N < tyABS_X0_N < tyABS_X2_N

            tyMin0 = tyABS_X1_N;
            tyMin1 = tyABS_X0_N;
            tyMin2 = tyABS_X2_N;

            tvMin0 = (tyX1_N > TYPE(0.0) ? tvE1 : MATH::F_NEG( tvE1 ) );
            tvMin1 = (tyX0_N > TYPE(0.0) ? tvE0 : MATH::F_NEG( tvE0 ) );
            tvMin2 = (tyX2_N > TYPE(0.0) ? tvE2 : MATH::F_NEG( tvE2 ) );
        };
    };

    C_(VECTOR,DIM)                      tvPlnN = tgPN0.Query_Normal();
    const TYPE                          tyDepth = tyMin2 - tyDist;

    // Point 1 - The point of deepest penetration

    if (tyDepth + tyMin0 + tyMin1 > TYPE(0.0))
    {
        return (TgE_NOINTERSECT);
    };

    C_(VECTOR,DIM)                      tvPnt = MATH::F_SUB( tgBX0.Query_Origin(), tvMin2 );
    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( MATH::F_SUB( tvPnt, tvMin0 ), tvMin1 );
    ptgContact->m_tvNormal = tvPlnN;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = tyDepth + tyMin0 + tyMin1;

    ++ptgPacket->m_niContact;

    // Point 2 - The second point of penetration

    if (tyDepth - tyMin0 + tyMin1 <= TYPE(0.0))
    {
        return (TgS_OK);
    };

    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_SUB( MATH::F_ADD( tvPnt, tvMin0 ), tvMin1 );
    ptgContact->m_tvNormal = tvPlnN;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = tyDepth - tyMin0 + tyMin1;

    ++ptgPacket->m_niContact;

    // Point 3 - Next lowest point of penetration

    if (tyDepth + tyMin0 - tyMin1 <= TYPE(0.0))
    {
        return (TgS_OK);
    };

    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_SUB( tvPnt, tvMin0 ), tvMin1 );
    ptgContact->m_tvNormal = tvPlnN;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = tyDepth + tyMin0 - tyMin1;

    ++ptgPacket->m_niContact;

    // Point 4 - Point of closure - this completes the rectangle/face of the box of deepest penetration

    if (tyDepth - tyMin0 - tyMin1 <= TYPE(0.0))
    {
        return (TgS_OK);
    };

    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_SUB( MATH::F_SUB( tvPnt, tvMin0 ), tvMin1 );
    ptgContact->m_tvNormal = tvPlnN;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = tyDepth + tyMin0 + tyMin1;

    ++ptgPacket->m_niContact;

    return (TgS_OK);
};

template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4PLANE, CR_TgF4BOX );


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

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