Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Box AA-Box AA.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Box;Axis-Aligned;BoxAA;
    »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_BA_DistSq_BA) ------------------------------------------------------------------------------------------------------------------------------------------ */
/* Input:  tgBA0, tgBA1: Box, Axis-Aligned primitive                                                                                                                      */
/* Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.                                                             */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE V(tgCO_F_BA_DistSq_BA)(V(CPC_TgBOXAA) psBA0, V(CPC_TgBOXAA) psBA1)
{
    TYPE                                fDistSq = MKL(0.0);

    if (psBA1->m_vMax.m.x < psBA0->m_vMin.m.x)
    {
        fDistSq += (psBA0->m_vMin.m.x - psBA1->m_vMax.m.x)*(psBA0->m_vMin.m.x - psBA1->m_vMax.m.x);
    }
    else if (psBA0->m_vMax.m.x < psBA1->m_vMin.m.x)
    {
        fDistSq += (psBA1->m_vMin.m.x - psBA0->m_vMax.m.x)*(psBA1->m_vMin.m.x - psBA0->m_vMax.m.x);
    };

    if (psBA1->m_vMax.m.y < psBA0->m_vMin.m.y)
    {
        fDistSq += (psBA0->m_vMin.m.y - psBA1->m_vMax.m.y)*(psBA0->m_vMin.m.y - psBA1->m_vMax.m.y);
    }
    else if (psBA0->m_vMax.m.y < psBA1->m_vMin.m.y)
    {
        fDistSq += (psBA1->m_vMin.m.y - psBA0->m_vMax.m.y)*(psBA1->m_vMin.m.y - psBA0->m_vMax.m.y);
    };

    if (psBA1->m_vMax.m.z < psBA0->m_vMin.m.z)
    {
        fDistSq += (psBA0->m_vMin.m.z - psBA1->m_vMax.m.z)*(psBA0->m_vMin.m.z - psBA1->m_vMax.m.z);
    }
    else if (psBA0->m_vMax.m.z < psBA1->m_vMin.m.z)
    {
        fDistSq += (psBA1->m_vMin.m.z - psBA0->m_vMax.m.z)*(psBA1->m_vMin.m.z - psBA0->m_vMax.m.z);
    };

    return (fDistSq <= MKL(0.0) ? -F(KTgMAX) : fDistSq);
}


/* ---- V(tgCO_F_BA_ClosestSq_BA) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgBA0, tgBA1: Box, Axis-Aligned primitive                                                                                                                      */
/* Output: vBA0, vBA1: Point of closest proximity on the box, axis-aligned #1 and #2 respectively                                                                         */
/* Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.                                                             */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE V(tgCO_F_BA_ClosestSq_BA)(V(PC_TgVEC) pvBA0, V(PC_TgVEC) pvBA1, V(CPC_TgBOXAA) psBA0, V(CPC_TgBOXAA) psBA1)
{
    TYPE                                fX0, fX1, fY0, fY1, fZ0, fZ1;
    TYPE                                fDistSq = MKL(0.0);

    /* First (X) co-ordinate value. */

    if (psBA1->m_vMax.m.x < psBA0->m_vMin.m.x)
    {
        fDistSq += (psBA0->m_vMin.m.x - psBA1->m_vMax.m.x)*(psBA0->m_vMin.m.x - psBA1->m_vMax.m.x);

        fX0 = psBA0->m_vMin.m.x;
        fX1 = psBA1->m_vMax.m.x;
    }
    else if (psBA0->m_vMax.m.x < psBA1->m_vMin.m.x)
    {
        fDistSq += (psBA1->m_vMin.m.x - psBA0->m_vMax.m.x)*(psBA1->m_vMin.m.x - psBA0->m_vMax.m.x);

        fX0 = psBA0->m_vMax.m.x;
        fX1 = psBA1->m_vMin.m.x;
    }
    else
    {
        TYPE                                fT;

        fT = F(tgPM_FSEL)(psBA0->m_vMin.m.x - psBA1->m_vMin.m.x, psBA0->m_vMin.m.x, psBA1->m_vMin.m.x);
        fT += F(tgPM_FSEL)(psBA1->m_vMax.m.x - psBA0->m_vMax.m.x, psBA0->m_vMax.m.x, psBA1->m_vMax.m.x);
        fT *= MKL(0.5);

        fX0 = fT; fX1 = fT;
    };


    /* Second (Y) co-ordinate value. */

    if (psBA1->m_vMax.m.y < psBA0->m_vMin.m.y)
    {
        fDistSq += (psBA0->m_vMin.m.y - psBA1->m_vMax.m.y)*(psBA0->m_vMin.m.y - psBA1->m_vMax.m.y);

        fY0 = psBA0->m_vMin.m.y;
        fY1 = psBA1->m_vMax.m.y;
    }
    else if (psBA0->m_vMax.m.y < psBA1->m_vMin.m.y)
    {
        fDistSq += (psBA1->m_vMin.m.y - psBA0->m_vMax.m.y)*(psBA1->m_vMin.m.y - psBA0->m_vMax.m.y);

        fY0 = psBA0->m_vMax.m.y;
        fY1 = psBA1->m_vMin.m.y;
    }
    else
    {
        TYPE                                fT;

        fT = F(tgPM_FSEL)(psBA0->m_vMin.m.y - psBA1->m_vMin.m.y, psBA0->m_vMin.m.y, psBA1->m_vMin.m.y);
        fT += F(tgPM_FSEL)(psBA1->m_vMax.m.y - psBA0->m_vMax.m.y, psBA0->m_vMax.m.y, psBA1->m_vMax.m.y);
        fT *= MKL(0.5);

        fY0 = fT; fY1 = fT;
    };


    /* Third (Z) co-ordinate value. */

    if (psBA1->m_vMax.m.z < psBA0->m_vMin.m.z)
    {
        fDistSq += (psBA0->m_vMin.m.z - psBA1->m_vMax.m.z)*(psBA0->m_vMin.m.z - psBA1->m_vMax.m.z);

        fZ0 = psBA0->m_vMin.m.z;
        fZ1 = psBA1->m_vMax.m.z;
    }
    else if (psBA0->m_vMax.m.z < psBA1->m_vMin.m.z)
    {
        fDistSq += (psBA1->m_vMin.m.z - psBA0->m_vMax.m.z)*(psBA1->m_vMin.m.z - psBA0->m_vMax.m.z);

        fZ0 = psBA0->m_vMax.m.z;
        fZ1 = psBA1->m_vMin.m.z;
    }
    else
    {
        TYPE                                fT;

        fT = F(tgPM_FSEL)(psBA0->m_vMin.m.z - psBA1->m_vMin.m.z, psBA0->m_vMin.m.z, psBA1->m_vMin.m.z);
        fT += F(tgPM_FSEL)(psBA1->m_vMax.m.z - psBA0->m_vMax.m.z, psBA0->m_vMax.m.z, psBA1->m_vMax.m.z);
        fT *= MKL(0.5);

        fZ0 = fT; fZ1 = fT;
    };

    *pvBA0 = V(FS_SETP)(fX0, fY0, fZ0);
    *pvBA1 = V(FS_SETP)(fX1, fY1, fZ1);

    return (fDistSq <= MKL(0.0) ? -F(KTgMAX) : fDistSq);
}


/* ---- V(tgCO_F_BA_Penetrate_BA) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  tgBA0: Box, Axis-Aligned primitive                                                                                                                             */
/* Input:  tgBA1: Box, Axis-Aligned 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_BA_Penetrate_BA)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgBOXAA) psBA0, V(CPC_TgBOXAA) psBA1)
{
    TgSINT32                            iAxis = -1;
    TYPE                                fT, fDepth = -F(KTgMAX);
    V(TgVEC)                            vK0, vD0, vD1, vP0, vNM;
    V(P_STg2_CO_Contact)                psContact;

    TgPARAM_CHECK( V(tgGM_BA_Is_Valid)(psBA1) && V(tgGM_BA_Is_Valid)(psBA0) );

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
    {
        return (KTgE_FAIL);
    };

    /* First (X) co-ordinate value. */

    fT = psBA1->m_vMin.m.x - psBA0->m_vMax.m.x;
    if (fT > MKL(0.0))
    {
        return (KTgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 0;
    };

    fT = psBA0->m_vMin.m.x - psBA1->m_vMax.m.x;
    if (fT > MKL(0.0))
    {
        return (KTgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 1;
    };


    /* First (Y) co-ordinate value. */

    fT = psBA1->m_vMin.m.y - psBA0->m_vMax.m.y;
    if (fT > MKL(0.0))
    {
        return (KTgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 2;
    };

    fT = psBA0->m_vMin.m.y - psBA1->m_vMax.m.y;
    if (fT > MKL(0.0))
    {
        return (KTgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 3;
    };


    /* First (Z) co-ordinate value. */

    fT = psBA1->m_vMin.m.z - psBA0->m_vMax.m.z;
    if (fT > MKL(0.0))
    {
        return (KTgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 4;
    };

    fT = psBA0->m_vMin.m.z - psBA1->m_vMax.m.z;
    if (fT > MKL(0.0))
    {
        return (KTgE_NO_INTERSECT);
    };

    if (fT > fDepth)
    {
        fDepth = fT;
        iAxis = 5;
    };


    switch (iAxis)
    {
        case 0:
        case 1:
        {
            const TYPE fY0 = F(tgPM_FSEL)(psBA1->m_vMin.m.y - psBA0->m_vMin.m.y, psBA1->m_vMin.m.y, psBA0->m_vMin.m.y);
            const TYPE fZ0 = F(tgPM_FSEL)(psBA1->m_vMin.m.z - psBA0->m_vMin.m.z, psBA1->m_vMin.m.z, psBA0->m_vMin.m.z);
            const TYPE fY1 = F(tgPM_FSEL)(psBA0->m_vMax.m.y - psBA1->m_vMax.m.y, psBA1->m_vMax.m.y, psBA0->m_vMax.m.y);
            const TYPE fZ1 = F(tgPM_FSEL)(psBA0->m_vMax.m.z - psBA1->m_vMax.m.z, psBA1->m_vMax.m.z, psBA0->m_vMax.m.z);

            vD0 = V(FS_SETV)(MKL(0.0), fY1 - fY0, MKL(0.0));
            vD1 = V(FS_SETV)(MKL(0.0), MKL(0.0), fZ1 - fZ0);
            vP0 = V(FS_SETP)(0 == iAxis ? psBA1->m_vMin.m.x : psBA1->m_vMax.m.x, fY0, fZ0);
            vNM = V(FS_SETV)(0 == iAxis ? MKL(1.0) : MKL(-1.0), MKL(0.0), MKL(0.0));

            break;
        };

        case 2:
        case 3:
        {
            const TYPE fX0 = F(tgPM_FSEL)(psBA1->m_vMin.m.x - psBA0->m_vMin.m.x, psBA1->m_vMin.m.x, psBA0->m_vMin.m.x);
            const TYPE fZ0 = F(tgPM_FSEL)(psBA1->m_vMin.m.z - psBA0->m_vMin.m.z, psBA1->m_vMin.m.z, psBA0->m_vMin.m.z);
            const TYPE fX1 = F(tgPM_FSEL)(psBA0->m_vMax.m.x - psBA1->m_vMax.m.x, psBA1->m_vMax.m.x, psBA0->m_vMax.m.x);
            const TYPE fZ1 = F(tgPM_FSEL)(psBA0->m_vMax.m.z - psBA1->m_vMax.m.z, psBA1->m_vMax.m.z, psBA0->m_vMax.m.z);

            vD0 = V(FS_SETV)(fX1 - fX0, MKL(0.0), MKL(0.0));
            vD1 = V(FS_SETV)(MKL(0.0), MKL(0.0), fZ1 - fZ0);
            vP0 = V(FS_SETP)(fX0, 2 == iAxis ? psBA1->m_vMin.m.y : psBA1->m_vMax.m.y, fZ0);
            vNM = V(FS_SETV)(MKL(0.0), 2 == iAxis ? MKL(1.0) : MKL(-1.0), MKL(0.0));

            break;
        };

        case 4:
        case 5:
        {
            const TYPE fX0 = F(tgPM_FSEL)(psBA1->m_vMin.m.x - psBA0->m_vMin.m.x, psBA1->m_vMin.m.x, psBA0->m_vMin.m.x);
            const TYPE fY0 = F(tgPM_FSEL)(psBA1->m_vMin.m.y - psBA0->m_vMin.m.y, psBA1->m_vMin.m.y, psBA0->m_vMin.m.y);
            const TYPE fX1 = F(tgPM_FSEL)(psBA0->m_vMax.m.x - psBA1->m_vMax.m.x, psBA1->m_vMax.m.x, psBA0->m_vMax.m.x);
            const TYPE fY1 = F(tgPM_FSEL)(psBA0->m_vMax.m.y - psBA1->m_vMax.m.y, psBA1->m_vMax.m.y, psBA0->m_vMax.m.y);

            vD0 = V(FS_SETV)(fX1 - fX0, MKL(0.0), MKL(0.0));
            vD1 = V(FS_SETV)(MKL(0.0), fY1 - fY0, MKL(0.0));
            vP0 = V(FS_SETP)(fX0, fY0, 4 == iAxis ? psBA1->m_vMin.m.z : psBA1->m_vMax.m.z);
            vNM = V(FS_SETV)(MKL(0.0), MKL(0.0), 4 == iAxis ? MKL(1.0) : MKL(-1.0));

            break;
        };

        default:
            TgERROR_CO( TgT("[BA][BA] Algorithm - Illegal separation axis id.\n") );
            TgS_NO_DEFAULT( return (KTgE_FAIL) );
    };

    /* Point 1 */

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

    psContact->m_vS0 = vP0;
    psContact->m_vN0 = vNM;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = -fDepth;

    ++psPacket->m_niContact;

    /* Point 2 */

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

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

    psContact->m_vS0 = V(F_ADD)(&vP0, &vD0);
    psContact->m_vN0 = vNM;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = -fDepth;

    ++psPacket->m_niContact;

    /* Point 3 */

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

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

    psContact->m_vS0 = V(F_ADD)(&vP0, &vD1);
    psContact->m_vN0 = vNM;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = -fDepth;

    ++psPacket->m_niContact;

    /* Point 4 */

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

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

    vK0 = V(F_ADD)(&vP0, &vD0);
    psContact->m_vS0 = V(F_ADD)(&vK0, &vD1);
    psContact->m_vN0 = vNM;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = -fDepth;

    ++psPacket->m_niContact;

    return (KTgS_OK);
}