// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Tube-Sphere.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_Contact_Sweep --------------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tyPM: Current normalized time of first contact.
// Input:  bPenetrate: If the swept primitives are in penetration, if true the function will return points of penetration.
// Input:  tgSP0: Sphere primitive
// Input:  tgTB0: Tube primitive
// Input:  tvDT: The swept primitive displacement for the duration of the period of interest.
// Output: tgPacket: Contact points are added or replace the current set depending on the time comparison and given parameters
// Output: tyPM: New normalized time of first contact
// Return: Result Code
//
//  This is a highly inaccurate test, meant to be used mainly as a child function for the cylinder, and capsule routines.
// Problems will rest in its false negative responses at values beyond the tube's length.  
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Sweep( PC_(CONTACT_PACKET,DIM) ptgPacket, TYPE *ptyPM, CR_(TUBE,DIM) tgTB0, CR_(SPHERE,DIM) tgSP0, CR_(DELTA,DIM) tgDT )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_SWEEP, (((TgUINT)ETgTUBE<<16)|(TgUINT)ETgSPHERE));
    TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) ));
    TgASSERT(tgSP0.Is_Valid() && tgTB0.Is_Valid());

    TgASSERT(!ptgPacket->m_bReport_Penetration); //« Penetration not implemented for tube-sphere.

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

    const TYPE                          tyRS = tgTB0.Query_Radius() + tgSP0.Query_Radius();

    if (!((tyRS > LIMITS<TYPE>::EPSILON) && (tgTB0.Query_Extent() > LIMITS<TYPE>::EPSILON)))
    {
        return (TgE_FAIL);
    };

    const TYPE                          tyR0 = tgSP0.Query_Radius();
    TTgCONTACT_PACKET<TYPE,DIM>         tgPacketSG;
    T_(CONTACT,DIM)                     atgContactSG[2];

    tgPacketSG.m_ptgContact = atgContactSG;
    tgPacketSG.m_tySweepTol = LIMITS<TYPE>::EPSILON;
    tgPacketSG.m_niContact = 0;
    tgPacketSG.m_niMaxContact = 2;
    tgPacketSG.m_iStride = sizeof( T_(CONTACT,DIM) );

    C_TgRESULT tgResult = TTgINT_TULN<TYPE,DIM,1,1,1,1>::DO( &tgPacketSG, tyR0, tgTB0, tgSP0.Query_Origin(), tgDT.m_tvDT );

    if (TgFAILED(tgResult))
    {
        return (tgResult);
    };
    TgASSERT(tgPacketSG.m_niContact >= 1)

    P_(CONTACT,DIM)                     ptgContactSG;
    P_(CONTACT,DIM)                     ptgContact;
    
    ptgContactSG = atgContactSG[0].m_tyT0 <= atgContactSG[(1 % tgPacketSG.m_niContact)].m_tyT0 ? atgContactSG : atgContactSG + 1;

    if (ptgContactSG->m_tyT0 > *ptyPM + ptgPacket->m_tySweepTol)
    {
        return (TgE_NOINTERSECT);
    };

    if (ptgContactSG->m_tyT0 < *ptyPM - ptgPacket->m_tySweepTol)
    {
        ptgPacket->m_niContact = 0;
        *ptyPM = ptgContactSG->m_tyT0;
    };

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

    ptgContact->m_tvPos = MATH::F_SUB( ptgContactSG->m_tvPos, MATH::F_MUL( tgSP0.Query_Radius(), ptgContactSG->m_tvNormal ) );
    ptgContact->m_tvNormal = ptgContactSG->m_tvNormal;
    ptgContact->m_tyT0 = ptgContactSG->m_tyT0;
    ptgContact->m_tyDepth = TYPE(0.0);

    ++ptgPacket->m_niContact;

    return (TgS_OK);
};

template TgRESULT F_Contact_Sweep( PC_TgF4CONTACT_PACKET, P_TgFLOAT32, CR_TgF4TUBE, CR_TgF4SPHERE, CR_TgF4DELTA );


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

// ---- F_Contact_Sweep --------------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tyPM: Current normalized time of first contact
// Input:  bPenetrate: If the swept primitives are in penetration, if true the function will return points of penetration.
// Input:  tgSP0: Sphere primitive
// Input:  tgTB0: Tube primitive
// Input:  tgDT: A structure holding the swept primitive displacement for the entire duration of the test period
// Output: tgPacket: Points of contact between the two primitives are added to it
// Output: tyPM: New normalized time of first contact
// Return: Result Code
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Sweep( PC_(CONTACT_PACKET,DIM) ptgPacket, TYPE *ptyPM, CR_(SPHERE,DIM) tgSP0, CR_(TUBE,DIM) tgTB0, CR_(DELTA,DIM) tgDT )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_SWEEP, (((TgUINT)ETgSPHERE<<16)|(TgUINT)ETgTUBE));

    const TgINT                         niContact = ptgPacket->m_niContact;
    const TYPE                          tyT = *ptyPM;
    T_(DELTA,DIM)                       tgNegDT;

    tgNegDT.m_tvUDT = MATH::F_NEG( tgDT.m_tvUDT );
    tgNegDT.m_tvDT = MATH::F_NEG( tgDT.m_tvDT );
    tgNegDT.m_tyDT = tgDT.m_tyDT;;
    tgNegDT.m_tyDT_DT = tgDT.m_tyDT_DT;
    tgNegDT.m_tyInv_DT = tgDT.m_tyInv_DT;
    
    C_TgRESULT tgResult = F_Contact_Sweep( ptgPacket, ptyPM, tgTB0, tgSP0, tgNegDT );

    P_(CONTACT,DIM)                     ptgContact;

    if (tgResult == TgE_PREPENETRATION)
    {
        for (TgINT iIdx = niContact; iIdx < ptgPacket->m_niContact; ++iIdx)
        {
            ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + iIdx*ptgPacket->m_iStride);

            ptgContact->m_tvPos = MATH::F_ADD( ptgContact->m_tvPos, MATH::F_MUL( ptgContact->m_tvNormal, ptgContact->m_tyDepth ) );
            ptgContact->m_tvNormal = MATH::F_NEG( ptgContact->m_tvNormal );
        };

        return (tgResult);
    }

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

    TgINT                               iIdx = (*ptyPM < tyT - ptgPacket->m_tySweepTol ? 0 : niContact);

    TgASSERT(iIdx < ptgPacket->m_niContact)

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

        ptgContact->m_tvPos = MATH::F_ADD( ptgContact->m_tvPos, MATH::F_MUL( ptgContact->m_tyT0, tgDT.m_tvDT ) );
        ptgContact->m_tvNormal = MATH::F_NEG( ptgContact->m_tvNormal );
    };

    return (tgResult);
};

template TgRESULT F_Contact_Sweep( PC_TgF4CONTACT_PACKET, P_TgFLOAT32, CR_TgF4SPHERE, CR_TgF4TUBE, CR_TgF4DELTA );


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

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