Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Box-Plane.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Box;Plane;
    »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_BX_Param_PN) ------------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgBX0: Box primitive                                                                                                                                           */
/* Input:  psPN0: Plane primitive                                                                                                                                         */
/* Output: tyB0,tyB1,tyB2: Parametric parameters to generate the point of closest proximity on the box (one for each axis)                                                */
/* Output: vPN0: Point of closest proximity on the plane                                                                                                                  */
/* Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.                                                             */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE V(tgCO_F_BX_Param_PN)( TYPE *pfBX0, TYPE *pfBX1, TYPE *pfBX2, V(PC_TgVEC) pvPN0, V(CPC_TgBOX) psBX0, V(CPC_TgPLANE) psPN0 )
{
    const TYPE                          fTest = V(tgCO_F_PN_Dist_VT)(psPN0, &psBX0->m.m.vOrigin);
    const TYPE                          fX0_N = V(F_DOT)(psBX0->m.m.avAxis + 0, &psPN0->m_vNormal);
    const TYPE                          fX1_N = V(F_DOT)(psBX0->m.m.avAxis + 1, &psPN0->m_vNormal);
    const TYPE                          fX2_N = V(F_DOT)(psBX0->m.m.avAxis + 2, &psPN0->m_vNormal);
    const TYPE                          fK0 = F(tgPM_FSEL)(fX0_N - F(KTgEPS), -psBX0->m_vExtent.m.x, MKL(0.0));
    const TYPE                          fK1 = F(tgPM_FSEL)(fX1_N - F(KTgEPS), -psBX0->m_vExtent.m.y, MKL(0.0));
    const TYPE                          fK2 = F(tgPM_FSEL)(fX2_N - F(KTgEPS), -psBX0->m_vExtent.m.z, MKL(0.0));
    const TYPE                          fBX0 = F(tgPM_FSEL)(-F(KTgEPS) - fX0_N, psBX0->m_vExtent.m.x, (fK0));
    const TYPE                          fBX1 = F(tgPM_FSEL)(-F(KTgEPS) - fX1_N, psBX0->m_vExtent.m.y, (fK1));
    const TYPE                          fBX2 = F(tgPM_FSEL)(-F(KTgEPS) - fX2_N, psBX0->m_vExtent.m.z, (fK2));
    const TYPE                          fDist = fTest + fBX0*fX0_N + fBX1*fX1_N + fBX2*fX2_N;
    const TYPE                          fNX = fBX0 - fDist*psPN0->m_vNormal.m.x;
    const TYPE                          fNY = fBX1 - fDist*psPN0->m_vNormal.m.y;
    const TYPE                          fNZ = fBX2 - fDist*psPN0->m_vNormal.m.z;

    TgPARAM_CHECK( V(tgGM_BX_Is_Valid)(psBX0) && V(tgGM_PN_Is_Valid)(psPN0) );

    *pfBX0 = fBX0;
    *pfBX1 = fBX1;
    *pfBX2 = fBX2;
    *pvPN0 = V(FS_SETP)(fNX, fNY, fNZ);

    return (F(tgPM_FSEL)(fDist, fDist, -F(KTgMAX)));
}


/* ---- V(tgCO_F_PN_Penetrate_BX) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psPN0: Plane primitive                                                                                                                                         */
/* Input:  tgBX0: 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                                                                                                                                                    */
/*                                                                                                                                                                        */
/*   This routine will create up to four contact points.  Collisions/Physics systems need the best(closest) approximation of the contact surface.  To represent the plane */
/* of contact between one box face and the plane, its necessary to use all four points that define the box rectangle (face).                                              */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_PN_Penetrate_BX)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgPLANE) psPN0, V(CPC_TgBOX) psBX0)
{
    TgPARAM_CHECK( V(tgGM_BX_Is_Valid)(psBX0) && V(tgGM_PN_Is_Valid)(psPN0) );

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
    {
        return (KTgE_FAIL);
    }
    else
    {
        const TYPE                          fDist = V(tgCO_F_PN_Dist_VT)(psPN0, &psBX0->m.m.vOrigin);

        V(C_TgVEC)                          vE0 = V(F_MUL_VS)(psBX0->m.m.avAxis + 0, psBX0->m_vExtent.m.x);
        V(C_TgVEC)                          vE1 = V(F_MUL_VS)(psBX0->m.m.avAxis + 1, psBX0->m_vExtent.m.y);
        V(C_TgVEC)                          vE2 = V(F_MUL_VS)(psBX0->m.m.avAxis + 2, psBX0->m_vExtent.m.z);

        const TYPE                          fX0_N = V(F_DOT)(&psPN0->m_vNormal, &vE0);
        const TYPE                          fX1_N = V(F_DOT)(&psPN0->m_vNormal, &vE1);
        const TYPE                          fX2_N = V(F_DOT)(&psPN0->m_vNormal, &vE2);

        const TYPE                          fABS_X0_N = F(tgPM_FSEL)(fX0_N, fX0_N, -fX0_N);
        const TYPE                          fABS_X1_N = F(tgPM_FSEL)(fX1_N, fX1_N, -fX1_N);
        const TYPE                          fABS_X2_N = F(tgPM_FSEL)(fX2_N, fX2_N, -fX2_N);

        V(C_TgVEC)                          vPlnN = psPN0->m_vNormal;

        V(P_STg2_CO_Contact)                psContact;
        V(TgVEC)                            vK0, vPnt;
        TYPE                                fMin0, fMin1, fMin2, fDepth;
        V(TgVEC)                            vMin0, vMin1, vMin2;

        if (fABS_X0_N <= fABS_X1_N)
        {
            if (fABS_X1_N <= fABS_X2_N)
            {
                /* fABS_X0_N <= fABS_X1_N <= fABS_X2_N */

                fMin0 = fABS_X0_N;
                fMin1 = fABS_X1_N;
                fMin2 = fABS_X2_N;

                vMin0 = (fX0_N > MKL(0.0) ? vE0 : V(F_NEG)(&vE0));
                vMin1 = (fX1_N > MKL(0.0) ? vE1 : V(F_NEG)(&vE1));
                vMin2 = (fX2_N > MKL(0.0) ? vE2 : V(F_NEG)(&vE2));
            }
            else if (fABS_X0_N <= fABS_X2_N)
            {
                /* fABS_X0_N <= fABS_X2_N < fABS_X1_N */

                fMin0 = fABS_X0_N;
                fMin1 = fABS_X2_N;
                fMin2 = fABS_X1_N;

                vMin0 = (fX0_N > MKL(0.0) ? vE0 : V(F_NEG)(&vE0));
                vMin1 = (fX2_N > MKL(0.0) ? vE2 : V(F_NEG)(&vE2));
                vMin2 = (fX1_N > MKL(0.0) ? vE1 : V(F_NEG)(&vE1));
            }
            else
            {
                /* fABS_X2_N < fABS_X0_N <= fABS_X1_N */

                fMin0 = fABS_X2_N;
                fMin1 = fABS_X0_N;
                fMin2 = fABS_X1_N;

                vMin0 = (fX2_N > MKL(0.0) ? vE2 : V(F_NEG)(&vE2));
                vMin1 = (fX0_N > MKL(0.0) ? vE0 : V(F_NEG)(&vE0));
                vMin2 = (fX1_N > MKL(0.0) ? vE1 : V(F_NEG)(&vE1));
            };
        }
        else
        {
            if (fABS_X2_N <= fABS_X1_N)
            {
                /* fABS_X2_N < = fABS_X1_N < fABS_X0_N */

                fMin0 = fABS_X2_N;
                fMin1 = fABS_X1_N;
                fMin2 = fABS_X0_N;

                vMin0 = (fX2_N > MKL(0.0) ? vE2 : V(F_NEG)(&vE2));
                vMin1 = (fX1_N > MKL(0.0) ? vE1 : V(F_NEG)(&vE1));
                vMin2 = (fX0_N > MKL(0.0) ? vE0 : V(F_NEG)(&vE0));
            }
            else if (fABS_X2_N <= fABS_X0_N)
            {
                /* fABS_X1_N < fABS_X2_N <= fABS_X0_N */

                fMin0 = fABS_X1_N;
                fMin1 = fABS_X2_N;
                fMin2 = fABS_X0_N;

                vMin0 = (fX1_N > MKL(0.0) ? vE1 : V(F_NEG)(&vE1));
                vMin1 = (fX2_N > MKL(0.0) ? vE2 : V(F_NEG)(&vE2));
                vMin2 = (fX0_N > MKL(0.0) ? vE0 : V(F_NEG)(&vE0));
            }
            else
            {
                /* fABS_X1_N < fABS_X0_N < fABS_X2_N */

                fMin0 = fABS_X1_N;
                fMin1 = fABS_X0_N;
                fMin2 = fABS_X2_N;

                vMin0 = (fX1_N > MKL(0.0) ? vE1 : V(F_NEG)(&vE1));
                vMin1 = (fX0_N > MKL(0.0) ? vE0 : V(F_NEG)(&vE0));
                vMin2 = (fX2_N > MKL(0.0) ? vE2 : V(F_NEG)(&vE2));
            };
        };

        fDepth = fMin2 - fDist;

        /* Point 1 - The point of deepest penetration */

        if (fDepth + fMin0 + fMin1 > MKL(0.0))
        {
            return (KTgE_NO_INTERSECT);
        };

        vPnt = V(F_SUB)(&psBX0->m.m.vOrigin, &vMin2);

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

        vK0 = V(F_SUB)(&vPnt, &vMin0);
        psContact->m_vS0 = V(F_SUB)(&vK0, &vMin1);
        psContact->m_vN0 = vPlnN;
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = fDepth + fMin0 + fMin1;

        ++psPacket->m_niContact;

        /* Point 2 - The second point of penetration */

        if (fDepth - fMin0 + fMin1 <= MKL(0.0))
        {
            return (KTgS_OK);
        };

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

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

        vK0 = V(F_ADD)(&vPnt, &vMin0);
        psContact->m_vS0 = V(F_SUB)(&vK0, &vMin1);
        psContact->m_vN0 = vPlnN;
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = fDepth - fMin0 + fMin1;

        ++psPacket->m_niContact;

        /* Point 3 - Next lowest point of penetration */

        if (fDepth + fMin0 - fMin1 <= MKL(0.0))
        {
            return (KTgS_OK);
        };

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

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

        vK0 = V(F_SUB)(&vPnt, &vMin0);
        psContact->m_vS0 = V(F_ADD)(&vK0, &vMin1);
        psContact->m_vN0 = vPlnN;
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = fDepth + fMin0 - fMin1;

        ++psPacket->m_niContact;

        /* Point 4 - Point of closure - this completes the rectangle/face of the box of deepest penetration */

        if (fDepth - fMin0 - fMin1 <= MKL(0.0))
        {
            return (KTgS_OK);
        };

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

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

        vK0 = V(F_SUB)(&vPnt, &vMin0);
        psContact->m_vS0 = V(F_SUB)(&vK0, &vMin1);
        psContact->m_vN0 = vPlnN;
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = fDepth + fMin0 + fMin1;

        ++psPacket->m_niContact;

        return (KTgS_OK);
    }
}