Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Box-Sphere.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Box;Sphere;
    »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_SP_Penetrate_BX) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psSP0: Sphere primitive                                                                                                                                        */
/* Input:  tgBX1: Box 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_SP_Penetrate_BX)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgSPHERE) psSP0, V(CPC_TgBOX) psBX0)
{
    TgPARAM_CHECK( V(tgGM_BX_Is_Valid)(psBX0) && V(tgGM_SP_Is_Valid)(psSP0) );

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
    {
        return (KTgE_FAIL);
    }
    else
    {
        V(C_TgVEC)                          vDS = V(F_SUB)(&psSP0->m_vOrigin, &psBX0->m.m.vOrigin);

        /* Difference vector in box's reference frame. */

        const TYPE                          fT0 = V(F_DOT)(&vDS, psBX0->m.m.avAxis + 0);
        const TYPE                          fT1 = V(F_DOT)(&vDS, psBX0->m.m.avAxis + 1);
        const TYPE                          fT2 = V(F_DOT)(&vDS, psBX0->m.m.avAxis + 2);

        /* A measurement of how far the sphere's origin is from a box face.  Negative is contained in box. */

        const TYPE                          fX0 = F(tgPM_ABS)(fT0) - psBX0->m_vExtent.m.x;
        const TYPE                          fX1 = F(tgPM_ABS)(fT1) - psBX0->m_vExtent.m.y;
        const TYPE                          fX2 = F(tgPM_ABS)(fT2) - psBX0->m_vExtent.m.z;

        const TYPE                          fBE0 = psBX0->m_vExtent.m.x;
        const TYPE                          fBE1 = psBX0->m_vExtent.m.y;
        const TYPE                          fBE2 = psBX0->m_vExtent.m.z;

        const TYPE                          fP0 = F(tgPM_FSEL)(fX0, F(tgPM_FSEL)(fT0, fBE0, -fBE0), fT0);
        const TYPE                          fP1 = F(tgPM_FSEL)(fX1, F(tgPM_FSEL)(fT1, fBE1, -fBE1), fT1);
        const TYPE                          fP2 = F(tgPM_FSEL)(fX2, F(tgPM_FSEL)(fT2, fBE2, -fBE2), fT2);

        V(P_STg2_CO_Contact)                psContact;

        if (fX0 > psSP0->m_fRadius || fX1 > psSP0->m_fRadius || fX2 > psSP0->m_fRadius)
        {
            /* If the sphere is at worst radius away from every box face, then their is no penetration. */
            return (KTgE_NO_INTERSECT);
        };

        psContact = psPacket->m_psContact + psPacket->m_niContact;
        psContact->m_vS0 = V(tgGM_BX_Calc_Point)(psBX0, fP0, fP1, fP2);

        if (fX0 > MKL(0.0) || fX1 > MKL(0.0) || fX2 > MKL(0.0))
        {
            /*  If the origin lies outside even one face then calculating the penetration contact is done by simply creating the point of closest proximity on the box to */
            /* the sphere origin. This point is the contact point, and the vector difference is the normal of contact. */
            V(C_TgVEC)                          vK0 = V(F_SUB)(&psContact->m_vS0, &psSP0->m_vOrigin);
            TYPE                                fK6;

            psContact->m_vN0 = V(F_NORM_LEN)(&fK6, &vK0);
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = psSP0->m_fRadius - fK6;

            ++psPacket->m_niContact;

            return (KTgS_OK);
        };

        /*  The sphere origin must be contained within the box.  Penetration is determined by finding the axis of minimum penetration and creating a contact to eject the */
        /* sphere out of the nearest face aligned with that axis.  All of the measurements must be negative (ie the point is contained), thus, it is the least negative */
        /* (or the greatest value) that determines the axis what has the minimal penetration. */

        if (fX0 > fX1)
        {
            if (fX0 > fX2)
            {
                psContact->m_vS0.m.x = fT0 < MKL(0.0) ? -fBE0 : fBE0;
                psContact->m_vN0 = fT0 > MKL(0.0) ? V(F_NEG)(psBX0->m.m.avAxis + 0) : psBX0->m.m.avAxis[0];
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = psSP0->m_fRadius - fX0;

                ++psPacket->m_niContact;

                return (KTgS_OK);
            }
            else
            {
                psContact->m_vS0.m.z = fT2 < MKL(0.0) ? -fBE2 : fBE2;
                psContact->m_vN0 = fT2 > MKL(0.0) ? V(F_NEG)(psBX0->m.m.avAxis + 2) : psBX0->m.m.avAxis[2];
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = psSP0->m_fRadius - fX2;

                ++psPacket->m_niContact;

                return (KTgS_OK);
            };
        }
        else
        {
            if (fX1 > fX2)
            {
                psContact->m_vS0.m.y = fT1 < MKL(0.0) ? -fBE1 : fBE1;
                psContact->m_vN0 = fT1 > MKL(0.0) ? V(F_NEG)(psBX0->m.m.avAxis + 1) : psBX0->m.m.avAxis[1];
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = psSP0->m_fRadius - fX1;

                ++psPacket->m_niContact;

                return (KTgS_OK);
            }
            else
            {
                psContact->m_vS0.m.z = fT2 < MKL(0.0) ? -fBE2 : fBE2;
                psContact->m_vN0 = fT2 > MKL(0.0) ? V(F_NEG)(psBX0->m.m.avAxis + 2) : psBX0->m.m.avAxis[2];
                psContact->m_fT0 = MKL(0.0);
                psContact->m_fDepth = psSP0->m_fRadius - fX2;

                ++psPacket->m_niContact;

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