// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//  »Project«   Teikitu Gaming System (TgS) (∂)
//  »File«      TgS Collision - F - Capsule-Ray.i_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;Capsule;Ray;
// ------------------------------------------------------------------------------------------------------------------------------ //
//  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 ================================================================================================================= //

#define VI(A) V(A##10)
#define LR_CAP_0 1
#define LR_CAP_1 0
#include "TgS Collision - F - Capsule-Linear.i_inc"
#undef LR_CAP_1
#undef LR_CAP_0
#undef VI

TgINLINE TYPE V(tgCO_F_DistSq_CP_RY)( V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    const TYPE                          fTest = V(tgCO_F_Dist_SG_RY)( &psCP0->m_sAX, psRY0 );
    const TYPE                          fDist = fTest - psCP0->m_fRadius;

    return (F(tgPM_FSEL)( fDist, fDist*fDist, -F(KTgMAX) ));
}


TgINLINE TYPE V(tgCO_F_Dist_CP_RY)( V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    const TYPE                          fTest = V(tgCO_F_Dist_SG_RY)( &psCP0->m_sAX, psRY0 );
    const TYPE                          fDist = fTest - psCP0->m_fRadius;

    return (F(tgPM_FSEL)( fDist, fDist, -F(KTgMAX) ));
}


TgINLINE TYPE V(tgCO_F_ClosestSq_CP_RY)( V(PCU_TgVEC) pvCP0, V(PCU_TgVEC) pvRY0, V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    const TYPE                          fDist = V(tgCO_F_Closest_CP_RY)( pvCP0, pvRY0, psCP0, psRY0 );

    return (F(tgPM_FSEL)( fDist, fDist*fDist, -F(KTgMAX) ));
}


TgINLINE TYPE V(tgCO_F_Closest_CP_RY)( V(PCU_TgVEC) pvCP0, V(PCU_TgVEC) pvRY0, V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    V(TgVEC)                            vCP0;

    const TYPE                          fRadius = psCP0->m_fRadius;
    const TYPE                          fTest = V(tgCO_F_Closest_SG_RY)( &vCP0, pvRY0, &psCP0->m_sAX, psRY0 );
    const TYPE                          fDist = F(tgPM_FSEL)( fTest - fRadius, fTest, MKL(-1.0) );
    V(C_TgVEC)                          vK0 = V(F_SUB_VV)( pvRY0, &vCP0 );
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fRadius / fDist, &vK0 );

    *pvCP0 = V(F_ADD_VV)( &vCP0, &vK1 );

    return (F(tgPM_FSEL)( fDist, fDist - fRadius, -F(KTgMAX) ));
}


TgINLINE TYPE V(tgCO_F_ParamSq_CP_RY)( V(PCU_TgVEC) pvCP0, PCU_TYPE pfRY0, V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    const TYPE                          fDist = V(tgCO_F_Param_CP_RY)( pvCP0, pfRY0, psCP0, psRY0 );

    return (F(tgPM_FSEL)( fDist, fDist*fDist, -F(KTgMAX) ));
}


TgINLINE TYPE V(tgCO_F_Param_CP_RY)( V(PCU_TgVEC) pvCP0, PCU_TYPE pfRY0, V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    TYPE                                fCP0;

    const TYPE                          fRadius = psCP0->m_fRadius;
    V(CPC_TgSEGMENT)                    psCP0_SG = &psCP0->m_sAX;

    const TYPE                          fTest = V(tgCO_F_Param_SG_RY)( &fCP0, pfRY0, psCP0_SG, psRY0 );
    const TYPE                          fDist = F(tgPM_FSEL)( fTest - fRadius, fTest, MKL(-1.0) );

    V(C_TgVEC)                          vK3 = V(F_MUL_SV)( *pfRY0, &psRY0->m_vDirN );
    V(C_TgVEC)                          vK4 = V(F_ADD_VV)( &psRY0->m_vOrigin, &vK3 );
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fCP0, &psCP0->m_sAX.m_vDirN );
    V(C_TgVEC)                          vK1 = V(F_ADD_VV)( &psCP0->m_sAX.m_vOrigin, &vK0 );
    V(C_TgVEC)                          vK5 = V(F_SUB_VV)( &vK4, &vK1 );
    V(C_TgVEC)                          vK2 = V(F_MUL_SV)( fRadius / fDist, &vK5 );

    *pvCP0 = V(F_ADD_VV)( &vK1, &vK2 );

    return (F(tgPM_FSEL)( fDist, fDist - fRadius, -F(KTgMAX) ));
}


TgINLINE TgBOOL V(tgCO_F_Test_CP_RY)( V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    return (V(tgCO_F_Dist_SG_RY)( &psCP0->m_sAX, psRY0 ) <= psCP0->m_fRadius);
}


TgINLINE TgRESULT V(tgCO_F_Intersect_CP_RY)( V(PCU_STg2_CO_Packet) psPacket, V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    return (V(tgCO_FI_Intersect_CP_LR10)( psPacket, psCP0, &psRY0->m_vOrigin, &psRY0->m_vDirN ));
}


TgINLINE TgRESULT V(tgCO_F_Penetrate_RY_CP)( V(PCU_STg2_CO_Packet) psPacket, V(CPCU_TgRAY) psRY0, V(CPCU_TgTUBE) psCP0 )
{
    TgASSERT_PARAM((TgSIZE)psPacket->m_iStride >= sizeof( V(P_STg2_CO_Contact) ));
    TgASSERT_PARAM(V(tgGM_Is_Valid_TB)( psCP0 ) && V(tgGM_Is_Valid_RY)( psRY0 ));

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || NULL == psPacket->m.psContact)
    {
        return (TgE_FAIL);
    }
    else
    {
        V(TgVEC)                            vCP0, vRY0;
        V(CPC_TgSEGMENT)                    psCP0_SG = &psCP0->m_sAX;
        const TYPE                          fDistSq = V(tgCO_F_ClosestSq_SG_RY)( &vCP0,&vRY0, psCP0_SG, psRY0 );

        if (fDistSq > psCP0->m_fRadiusSq)
        {
            return (ETgE_NO_INTERSECT);
        };

        return (V(tgCO_FI_Penetrate_CP_LR10)( psPacket, &vRY0, psCP0, &vCP0, fDistSq ));
    };
}


TgINLINE TgRESULT V(tgCO_F_Clip_Param_CP_RY)( PCU_TYPE pfRY0, PCU_TYPE pfRY1, V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    return (V(tgCO_FI_Clip_Param_CP_LR00)( pfRY0, pfRY1, psCP0, &psRY0->m_vOrigin, &psRY0->m_vDirN ));
}


TgINLINE TgRESULT V(tgCO_F_Clip_CP_RY)( V(PCU_STg2_CO_Clip_List) psCL, V(CPCU_TgTUBE) psCP0, V(CPCU_TgRAY) psRY0 )
{
    return (V(tgCO_FI_Clip_CP_LR00)( psCL, psCP0, &psRY0->m_vOrigin, &psRY0->m_vDirN ));
}