Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Cylinder-Plane.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Cylinder;
    »Author«    Andrew Aye (EMail: mailto:andrew.aye@gmail.com, Web: http://www.andrewaye.com)
    »Version«   4.51 / »GUID« A9981407-3EC9-42AF-8B6F-8BE6DD919615                                                                                                        */
/*   -------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
/*  Copyright: © 2002-2017, 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:
        Redistribution of source code must retain this copyright notice, this list of conditions and the following disclaimers.
        Redistribution 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.
    The name of the author may not 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 ========================================================================================================================================================= */

/* ---- V(tgCO_F_CY_Dist_PN) -------------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psCY0: Cylinder primitive                                                                                                                                      */
/* Input:  psPN0: Plane primitive                                                                                                                                         */
/* Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.                                                             */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE V(tgCO_F_CY_Dist_PN)(V(CPC_TgTUBE) psCY0, V(CPC_TgPLANE) psPN0)
{
    /* Calculate out the projection of the cylinder onto the plane's normal */

    const TYPE                          fDist = V(tgCO_F_PN_Sign_Dist_VT)(psPN0, &psCY0->m.m.vOrigin);

    TgERROR( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_PN_Is_Valid)(psPN0) );

    if (fDist < MKL(0.0))
    {
        return (-F(KTgMAX));
    }
    else
    {
        const TYPE                          fUAX_N = V(F_DOT)(&psPN0->m_vNormal, &psCY0->m.m.vU_HAX);
        const TYPE                          fE2 = psCY0->m_fExtent*F(tgPM_ABS)(fUAX_N);
        const TYPE                          fE3 = psCY0->m_fRadius*F(tgPM_SQRT)(F(tgPM_ABS)(MKL(1.0) - fUAX_N*fUAX_N));

        if (fE2 + fE3 >= fDist)
        {
            /* The cylinder is penetration this plane, return back the error state. */

            return (-F(KTgMAX));
        };

        return (fDist - (fE2 + fE3));
    };
}


/* ---- V(tgCO_F_CY_Closest_PN) ----------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psCY0: Cylinder primitive                                                                                                                                      */
/* Input:  psPN0: Plane primitive                                                                                                                                         */
/* Output: vCY0,vPN0: 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                                                                                        */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE V(tgCO_F_CY_Closest_PN)(V(PC_TgVEC) pvCY0, V(PC_TgVEC) pvPN0, V(CPC_TgTUBE) psCY0, V(CPC_TgPLANE) psPN0)
{
    /* Calculate out the projection of the cylinder onto the plane's normal */

    TYPE                                fDist = V(tgCO_F_PN_Sign_Dist_VT)(psPN0, &psCY0->m.m.vOrigin);

    TgERROR( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_PN_Is_Valid)(psPN0) );

    if (fDist < MKL(0.0))
    {
        return (-F(KTgMAX));
    }
    else
    {
        const TYPE                          fUAX_N = V(F_DOT)(&psPN0->m_vNormal, &psCY0->m.m.vU_HAX);
        const TYPE                          fE2 = psCY0->m_fExtent*F(tgPM_ABS)(fUAX_N);
        const TYPE                          fE3 = psCY0->m_fRadius*F(tgPM_SQRT)(F(tgPM_ABS)(MKL(1.0) - fUAX_N*fUAX_N));

        if (fE2 + fE3 >= fDist)
        {   /* The cylinder is penetration this plane, return back the error state. */
            return (-F(KTgMAX));
        }
        else
        {
            V(TgVEC)                            vK6;
            TYPE                                fNM;

            V(C_TgVEC)                          vK0 = V(F_MUL_VS)(&psCY0->m.m.vU_HAX, fUAX_N);
            V(C_TgVEC)                          vK1 = V(F_SUB)(&vK0, &psPN0->m_vNormal);
            V(C_TgVEC)                          vK2 = V(F_NORM_LEN)(&fNM, &vK1);
            const TYPE                          fK0 = fUAX_N < -F(KTgEPS) ? MKL(1.0) : MKL(0.0);
            const TYPE                          fK1 = fUAX_N > F(KTgEPS) ? MKL(-1.0) : fK0;
            const TYPE                          fK2 = fNM <= F(KTgEPS) ? MKL(0.0) : psCY0->m_fRadius;
            V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK2, &vK2);
            V(C_TgVEC)                          vK4 = V(F_MUL_SV)(fK1, &psCY0->m_vHAX);
            V(C_TgVEC)                          vK5 = V(F_ADD)(&vK3, &vK4);
            V(C_TgVEC)                          vPnt = V(F_ADD)(&psCY0->m.m.vOrigin, &vK5);

            fDist -= fE2 + fE3;
            vK6 = V(F_MUL_SV)(fDist, &psPN0->m_vNormal);

            *pvCY0 = vPnt;
            *pvPN0 = V(F_SUB)(&vPnt, &vK6);

            return (fDist);
        };
    };
}


/* ---- V(tgCO_F_PN_Penetrate_CY) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psPN0: Plane primitive                                                                                                                                         */
/* Input:  psCY0: 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                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_PN_Penetrate_CY)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgPLANE) psPN0, V(CPC_TgTUBE) psCY0)
{
    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psCY0) && V(tgGM_PN_Is_Valid)(psPN0) );
    TgPARAM_CHECK( psCY0->m_fExtent > F(KTgEPS) );

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
    {
        return (KTgE_FAIL);
    }
    else
    {
        /* Project the cylinder onto the plane normal and verify that some portion lies below the plane. */

        const TYPE                          fUAX_N = V(F_DOT)(&psPN0->m_vNormal, &psCY0->m.m.vU_HAX);
        const TYPE                          fDist = V(tgCO_F_PN_Sign_Dist_VT)(psPN0, &psCY0->m.m.vOrigin);
        const TYPE                          fE2 = psCY0->m_fExtent*F(tgPM_ABS)(fUAX_N);
        const TYPE                          fE3 = psCY0->m_fRadius*F(tgPM_SQRT)(F(tgPM_ABS)(MKL(1.0) - fUAX_N*fUAX_N));

        if (fE2 + fE3 < fDist)
        {
            return (KTgE_NO_INTERSECT);
        }
        else
        {
            V(P_STg2_CO_Contact)                psContact;
            TYPE                                fNM;

            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fUAX_N, &psCY0->m.m.vU_HAX);
            V(C_TgVEC)                          vK1 = V(F_SUB)(&vK0, &psPN0->m_vNormal);
            V(TgVEC)                            vK2 = V(F_NORM_LEN)(&fNM, &vK1);
            const TYPE                          fDN = F(tgPM_FSEL)(fUAX_N, MKL(-1.0), MKL(1.0));
            V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fDN, &psCY0->m_vHAX);
            V(C_TgVEC)                          vK4 = V(F_ADD)(&psCY0->m.m.vOrigin, &vK3);

            /* 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 (F(tgPM_ABS)(fUAX_N) < F(KTgF_SQRT1_2))
            {
                V(C_TgVEC)                          vK5 = V(F_MUL_VS)(&vK2, psCY0->m_fRadius);
                V(C_TgVEC)                          vK6 = V(F_SUB)(&psCY0->m.m.vOrigin, &vK3);

                psContact = psPacket->m_psContact + psPacket->m_niContact;

                psContact->m_vS0 = V(F_ADD)(&vK4, &vK5);
                psContact->m_vN0 = psPN0->m_vNormal;
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = fE2 + fE3 - fDist;

                ++psPacket->m_niContact;

                if (fE3 < fE2 + fDist)
                {
                    return (KTgS_OK);
                };

                if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                {
                    return (KTgE_MAX_CONTACTS);
                };

                psContact = (V(P_STg2_CO_Contact))(psPacket->m_psContact + psPacket->m_niContact);

                psContact->m_vS0 = V(F_ADD)(&vK6, &vK5);
                psContact->m_vN0 = psPN0->m_vNormal;
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = fE3 - fE2 - fDist;

                ++psPacket->m_niContact;

                return (KTgS_OK);
            }
            else
            {
                /* The cylinder is at 45 degrees or more to the plane, create 3 points around the rim of the cylinder for contact points. */
                V(C_TgVEC)                          vK5 = V(F_UCX)(&vK2, &psCY0->m.m.vU_HAX);
                V(C_TgVEC)                          vK6 = fNM <= F(KTgEPS) ? psCY0->m.m.vU_Basis1 : vK5;
                V(C_TgVEC)                          vK7 = fNM <= F(KTgEPS) ? psCY0->m.m.vU_Basis0 : vK2;
                const TYPE                          fDepth = fE2 + fE3*MKL(0.5) - fDist;
                V(C_TgVEC)                          vK8 = V(F_MUL_SV)(F(KTgF_SQRT3), &vK6);
                V(C_TgVEC)                          vKD = V(F_MUL_VS)(&vK7, psCY0->m_fRadius);

                psContact = psPacket->m_psContact + psPacket->m_niContact;

                psContact->m_vS0 = V(F_ADD)(&vK4, &vKD);
                psContact->m_vN0 = psPN0->m_vNormal;
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = fE2 + fE3 - fDist;

                ++psPacket->m_niContact;

                if (fDepth > MKL(0.0))
                {
                    V(C_TgVEC)                          vK9 = V(F_ADD)(&vK7, &vK8);
                    V(C_TgVEC)                          vKA = V(F_MUL_SV)(MKL(0.5)*psCY0->m_fRadius, &vK9);
                    V(C_TgVEC)                          vKB = V(F_SUB)(&vK7, &vK8);
                    V(C_TgVEC)                          vKC = V(F_MUL_SV)(MKL(0.5)*psCY0->m_fRadius, &vKB);

                    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                    {
                        return (KTgE_MAX_CONTACTS);
                    };

                    psContact = psPacket->m_psContact + psPacket->m_niContact;

                    psContact->m_vS0 = V(F_SUB)(&vK4, &vKA);
                    psContact->m_vN0 = psPN0->m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fDepth;

                    ++psPacket->m_niContact;

                    if (psPacket->m_niContact >= psPacket->m_niMaxContact)
                    {
                        return (KTgE_MAX_CONTACTS);
                    };

                    psContact = psPacket->m_psContact + psPacket->m_niContact;

                    psContact->m_vS0 = V(F_SUB)(&vK4, &vKC);
                    psContact->m_vN0 = psPN0->m_vNormal;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fDepth;

                    ++psPacket->m_niContact;
                };

                return (KTgS_OK);
            };
        };
    };
}