// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//  »Project«   Teikitu Gaming System (TgS) (∂)
//  »File«      TgS Collision - F - Capsule-Point.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;Capsule;Point;
// ------------------------------------------------------------------------------------------------------------------------------ //
//  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_Penetrate_VT_CP -------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  vS0: Point
// Input:  psCP0: Capsule 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_VT_CP)( V(PCU_STg2_CO_Packet) psPacket, V(CPCU_TgVEC) pvS0, 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(F_Is_Point_Valid)( pvS0 ));

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || NULL == psPacket->m.psContact)
    {
        return (TgE_FAIL);
    }
    else
    {
        V(TgVEC)                            vK1, vCP0, vNormal;
        TYPE                                fNM;
        V(P_STg2_CO_Contact)                psContact;

        const TYPE                          fDistSq = V(tgCO_F_ClosestSq_SG_VT)( &vCP0, &psCP0->m_sAX, pvS0 );

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

        if (fDistSq <= F(KTgEPS))
        {
            if (F(tgCM_NR0)( psCP0->m.m.vU_HAX.m.z ))
            {
                vNormal = V(F_SETV_ELEM)( -psCP0->m.m.vU_HAX.m.y,psCP0->m.m.vU_HAX.m.x,MKL(0.0) );
            }
            else
            {
                vNormal = V(F_SETV_ELEM)( MKL(0.0),psCP0->m.m.vU_HAX.m.z,-psCP0->m.m.vU_HAX.m.y );
            };

            vNormal = V(F_NORM)( &vNormal );
            fNM = MKL(0.0);
        }
        else
        {
            V(C_TgVEC)                          vK0 = V(F_SUB_VV)( &vCP0, pvS0 );

            vNormal = V(F_NORM_LEN)( &fNM, &vK0 );
        };

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

        vK1 = V(F_MUL_SV)( psCP0->m_fRadius, &vNormal );
        psContact->m_vS0 = V(F_SUB_VV)( &vCP0, &vK1 );
        psContact->m_vN0 = vNormal;
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = psCP0->m_fRadius - fNM;

        ++psPacket->m_niContact;

        return (TgS_OK);
    };
}