// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Capsule-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_Penetrate ----------------------------------------------------------------------------------------------------- //
// Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.
// Input:  tgSP0: Sphere primitive
// Input:  tgCP0: 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
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TgRESULT F_Contact_Penetrate( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(SPHERE,DIM) tgSP0, CR_(CAPSULE,DIM) tgCP0 )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgSPHERE<<16)|(TgUINT)ETgCAPSULE))

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

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

    const TYPE                          tyRS = tgCP0.Query_Radius() + tgSP0.Query_Radius();
    const TYPE                          tyRSSq = tyRS*tyRS;
    T_(VECTOR,DIM)                      tvPnt;

    const TYPE                          tyDistSq = F_ClosestSq( &tvPnt, tgCP0.Query_Segment(), tgSP0.Query_Origin() );

    if (tyDistSq > tyRSSq)
    {
        return (TgE_NOINTERSECT);
    };

    T_(VECTOR,DIM)                      tvNormal;
    TYPE                                tyNM;

    if (LIMITS<TYPE>::EPSILON >= tyDistSq)
    {
        tvNormal = MATH::F_NORM( MATH::F_Set_Ortho( tgCP0.Query_AxisUnit() ) );
        tyNM = TYPE(0.0);
    }
    else
    {
        tvNormal = MATH::F_NORM( &tyNM, MATH::F_SUB( tvPnt, tgSP0.Query_Origin() ) );
    };

    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( tvPnt, MATH::F_MUL( tvNormal, tgCP0.Query_Radius() ) );
    ptgContact->m_tvNormal = tvNormal;
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = tyRS - tyNM;

    ++ptgPacket->m_niContact;

    return (TgS_OK);
};

template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4SPHERE, CR_TgF4CAPSULE );


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

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