// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//
//  Project:   Talina Gaming System (TgS) (∂)
//  File:      TgS Collision - Cylinder-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_Dist ------------------------------------------------------------------------------------------------------------------ //
// Input:  tgCY0: Cylinder primitive
// Input:  tgPN0: Plane primitive
// Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TYPE F_Dist( CR_(CYLINDER,DIM) tgCY0, CR_(PLANE,DIM) tgPN0 )
{
    TgASSERT( tgCY0.Is_Valid() && tgPN0.Is_Valid() )

    // Calculate out the projection of the cylinder onto the plane's normal

    const TYPE                          tyDist = F_Sign_Dist( tgPN0, tgCY0.Query_Origin() );

    if (tyDist < TYPE(0.0))
    {
        return (-LIMITS<TYPE>::MAX);
    };

    const TYPE                          tyUAX_N = MATH::F_DOT(tgPN0.Query_Normal(),tgCY0.Query_AxisUnit());
    const TYPE                          tyE2 = tgCY0.Query_Extent()*P::ABS( tyUAX_N );
    const TYPE                          tyE3 = tgCY0.Query_Radius()*P::SQRT( P::ABS( TYPE(1.0) - tyUAX_N*tyUAX_N ) );

    if (tyE2 + tyE3 >= tyDist)
    {
        // The cylinder is penetration this plane, return back the error state.

        return (-LIMITS<TYPE>::MAX);
    };

    return (tyDist - ( tyE2 + tyE3 ));
};

template TgFLOAT32 F_Dist( CR_TgF4CYLINDER, CR_TgF4PLANE );


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

// ---- F_Closest --------------------------------------------------------------------------------------------------------------- //
// Input:  tgCY0: Cylinder primitive
// Input:  tgPN0: Plane primitive
// Output: tvCY0,tvPN0: Point of closest proximity on the cylinder and plane respectively
// Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.
//
//   For distance functions, planes are not considered to be cutting (ie dividing into two half-spaces).  Thus, cylinders on the
//  negative side of the plane are not ignored/culled. The distance value returned
// ------------------------------------------------------------------------------------------------------------------------------ //

template <typename TYPE, int DIM>
TYPE F_Closest( PC_(VECTOR,DIM) ptvCY0, PC_(VECTOR,DIM) ptvPN0, CR_(CYLINDER,DIM) tgCY0, CR_(PLANE,DIM) tgPN0 )
{
    TgASSERT( tgCY0.Is_Valid() && tgPN0.Is_Valid() )

    // Calculate out the projection of the cylinder onto the plane's normal

    TYPE                                tyDist = F_Sign_Dist( tgPN0, tgCY0.Query_Origin() );

    if (tyDist < TYPE(0.0))
    {
        return (-LIMITS<TYPE>::MAX);
    };

    const TYPE                          tyUAX_N = MATH::F_DOT(tgPN0.Query_Normal(),tgCY0.Query_AxisUnit());
    const TYPE                          tyE2 = tgCY0.Query_Extent()*P::ABS( tyUAX_N );
    const TYPE                          tyE3 = tgCY0.Query_Radius()*P::SQRT( P::ABS( TYPE(1.0) - tyUAX_N*tyUAX_N ) );

    if (tyE2 + tyE3 >= tyDist)
    {
        // The cylinder is penetration this plane, return back the error state.

        return (-LIMITS<TYPE>::MAX);
    };

    C_(VECTOR,DIM)                      tvPN = tgPN0.Query_Normal();
    T_(VECTOR,DIM)                      tvPnt, tvX0;
    
    tvX0 = MATH::F_SUB( MATH::F_MUL( tgCY0.Query_AxisUnit(), tyUAX_N ), tvPN );

    TYPE                                tyNM;
    
    MATH::F_NORM( &tyNM, tvX0 );

    const TYPE                          tyK0 = tyUAX_N < -LIMITS<TYPE>::EPSILON ? TYPE( 1.0) : TYPE(0.0);
    const TYPE                          tyK1 = tyUAX_N >  LIMITS<TYPE>::EPSILON ? TYPE(-1.0) : tyK0;
    const TYPE                          tyK2 = tyNM <= LIMITS<TYPE>::EPSILON ? TYPE(0.0) : tgCY0.Query_Radius();

    tvX0 = MATH::F_MUL( tyK2, tvX0 );
    tyDist -= tyE2 + tyE3;
    tvPnt = MATH::F_ADD( tgCY0.Query_Origin(), MATH::F_ADD( tvX0, MATH::F_MUL( tyK1, tgCY0.Query_HalfAxis() ) ) );

    *ptvCY0 = tvPnt;
    *ptvPN0 = MATH::F_SUB( tvPnt, MATH::F_MUL( tyDist, tvPN ) );

    return (tyDist);
};

template TgFLOAT32 F_Closest( PC_TgF4VECTOR, PC_TgF4VECTOR, CR_TgF4CYLINDER, 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:  tgCY0: Cylinder 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_(PLANE,DIM) tgPN0, CR_(CYLINDER,DIM) tgCY0 )
{
    TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgPLANE<<16)|(TgUINT)ETgCYLINDER))

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

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

    // Project the cylinder onto the plane normal and verify that some portion lies below the plane.

    TgASSERT( tgCY0.Query_Extent() > LIMITS<TYPE>::EPSILON );

    const TYPE                          tyUAX_N = MATH::F_DOT(tgPN0.Query_Normal(),tgCY0.Query_AxisUnit());
    const TYPE                          tyDist = F_Sign_Dist( tgPN0, tgCY0.Query_Origin() );
    const TYPE                          tyE2 = tgCY0.Query_Extent()*P::ABS( tyUAX_N );
    const TYPE                          tyE3 = tgCY0.Query_Radius()*P::SQRT( P::ABS( TYPE(1.0) - tyUAX_N*tyUAX_N ) );

    if (tyE2 + tyE3 < tyDist)
    {
        return (TgE_NOINTERSECT);
    };

    TYPE                                tyNM;
    C_(VECTOR,DIM)                      tvK0 = MATH::F_MUL( tyUAX_N, tgCY0.Query_AxisUnit() );
    T_(VECTOR,DIM)                      tvX0 = MATH::F_NORM( &tyNM, MATH::F_SUB( tvK0, tgPN0.Query_Normal() ) );
    const TYPE                          tyDN = P::FSEL( tyUAX_N, TYPE(-1.0), TYPE(1.0) );

    P_(CONTACT,DIM)                     ptgContact;

    // If the cylinder is at a 45 degree angle or less to the plane, create contact points at both extreme points of the cylinder.

    if (P::ABS(tyUAX_N) < KF32_SQRT1_2)
    {
        ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride);

        C_(VECTOR,DIM)                      tvK1 = MATH::F_ADD( tgCY0.Query_Origin(), MATH::F_MUL( tyDN, tgCY0.Query_HalfAxis() ) );

        ptgContact->m_tvPos = MATH::F_ADD( tvK1, MATH::F_MUL( tvX0, tgCY0.Query_Radius() ) );
        ptgContact->m_tvNormal = tgPN0.Query_Normal();
        ptgContact->m_tyT0 = TYPE(0.0);
        ptgContact->m_tyDepth = tyE2 + tyE3 - tyDist;

        ++ptgPacket->m_niContact;

        if (tyE3 < tyE2 + tyDist)
        {
            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);

        C_(VECTOR,DIM)                      tvK2 = MATH::F_SUB( tgCY0.Query_Origin(), MATH::F_MUL( tyDN, tgCY0.Query_HalfAxis() ) );

        ptgContact->m_tvPos = MATH::F_ADD( tvK2, MATH::F_MUL( tvX0, tgCY0.Query_Radius() ) );
        ptgContact->m_tvNormal = tgPN0.Query_Normal();
        ptgContact->m_tyT0 = TYPE(0.0);
        ptgContact->m_tyDepth = tyE3 - tyE2 - tyDist;

        ++ptgPacket->m_niContact;

        return (TgS_OK);
    };

    // The cylinder is at 45 degrees or more to the plane, create 3 points around the rim of the cylinder for contact points.

    C_(VECTOR,DIM)                      tvC0 = MATH::F_ADD( tgCY0.Query_Origin(), MATH::F_MUL( tyDN, tgCY0.Query_HalfAxis() ) );
    T_(VECTOR,DIM)                      tvX1;

    if (tyNM <= LIMITS<TYPE>::EPSILON)
    {
        tvX0 = tgCY0.Query_BasisUnit0();
        tvX1 = tgCY0.Query_BasisUnit1();
    }
    else
    {
        MATH::F_UCX( &tvX1, tvX0, tgCY0.Query_AxisUnit() );
    };

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

    ptgContact->m_tvPos = MATH::F_ADD( tvC0, MATH::F_MUL( tvX0, tgCY0.Query_Radius() ) );
    ptgContact->m_tvNormal = tgPN0.Query_Normal();
    ptgContact->m_tyT0 = TYPE(0.0);
    ptgContact->m_tyDepth = tyE2 + tyE3 - tyDist;

    ++ptgPacket->m_niContact;

    tvX1 = MATH::F_MUL( KF32_SQRT3, tvX1 );

    const TYPE                          tyDepth = tyE2 + tyE3*TYPE(0.5) - tyDist;

    if (tyDepth > TYPE(0.0))
    {
        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( tvC0, MATH::F_MUL( TYPE(0.5)*tgCY0.Query_Radius(), MATH::F_ADD( tvX0, tvX1 ) ) );
        ptgContact->m_tvNormal = tgPN0.Query_Normal();
        ptgContact->m_tyT0 = TYPE(0.0);
        ptgContact->m_tyDepth = tyDepth;

        ++ptgPacket->m_niContact;

        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( tvC0, MATH::F_MUL( TYPE(0.5)*tgCY0.Query_Radius(), MATH::F_SUB( tvX0, tvX1 ) ) );
        ptgContact->m_tvNormal = tgPN0.Query_Normal();
        ptgContact->m_tyT0 = TYPE(0.0);
        ptgContact->m_tyDepth = tyDepth;

        ++ptgPacket->m_niContact;
    };

    return (TgS_OK);
};

template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4PLANE, CR_TgF4CYLINDER );


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

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