Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Parallelogram-Linear.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Parallelogram;Line;Ray;Segment;
    »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".                                                   */
/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */

MSVC_WARN_DISABLE_PUSH( 6235 ) /* Analysis - (<non-zero constant> || <expression>) is always a non-zero constant. */
MSVC_WARN_DISABLE_PUSH( 6240 ) /* Analysis - (<expression> && <non-zero constant>) always evaluates to the result of <expression>. */


/*  Let E0 and E1 be the two defining edges for the parallelogram                                                                                                         */
/*                                                                                                                                                                        */
/*  The normal N = E0 x E1                                                                                                                                                */
/*  It follows that,                                                                                                                                                      */
/*      E1_(N × E0,DIM) = N_(E0 × E1,DIM) = N•N = |N|^2 ≥ 0                                                                                                               */
/*           - The second edge always has a non-negative projection onto the first edge's clip plane.                                                                     */
/*      E0_(E1 × N,DIM) = -E0_(N × E1,DIM) = -N_(E1 × E0,DIM) = N_(E0 × E1,DIM) = N•N = |N|^2 ≥ 0                                                                         */
/*           - The first edge always has a non-negative projection onto the second edge's clip plane.                                                                     */

/* == Collision ========================================================================================================================================================= */

/* ---- VI(tgCO_F_PE_ParamSq_LR) ---------------------------------------------------------------------------------------------------------------------------------------- */
/*  -- Internal Function -- LN_CAP_0, LN_CAP_1 : Indicate the termination condition of the linear {bc0,LN_CAP_1}                                                          */
/* Input:  psPE0: Parallelogram primitive                                                                                                                                 */
/* Input:  vS0,vD0: Origin and Direction for the Linear                                                                                                                   */
/* Output: _tyPE0, _tyPE1: Parametric parameters to generate point of minimal distance on the parallelogram                                                               */
/* Output: _tyLN0: Parametric parameter to generate point of minimal distance on the linear                                                                               */
/* Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.                                                             */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE VI(tgCO_F_PE_ParamSq_LR)( TYPE *pfPE0, TYPE *pfPE1, TYPE *pfLN0, V(CPC_TgPARALLELOGRAM) psPE0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
    V(C_TgVEC)                          vX0 = V(F_SUB)(pvS0, &psPE0->m_vOrigin);
    const TYPE                          fX0_X0 = V(F_LSQ)(&vX0);
    const TYPE                          fE1_EN0 = V(F_DOT)(psPE0->m_avEdgeNormal + 0, psPE0->m_avEdge + 1);
    const TYPE                          fE0_EN1 = V(F_DOT)(psPE0->m_avEdgeNormal + 1, psPE0->m_avEdge + 0);

    TgPARAM_CHECK( V(tgGM_PE_Is_Valid)(psPE0) && V(F_Is_Point_Valid)(pvS0) && V(F_Is_Vector_Valid)(pvD0) );

    if (TgEXPECT_FALSE( fX0_X0 <= F(KTgEPS) || fE1_EN0 <= F(KTgEPS) || fE0_EN1 <= F(KTgEPS) ))
    {
        /* Degenerate parallelogram - One or both of the edges has a near-zero length */
        /* Quick Out - the point is within tolerance of rectangle origin. */

        *pfPE0 = MKL(0.0);
        *pfPE1 = MKL(0.0);
        *pfLN0 = MKL(0.0);

        return (-F(KTgMAX));
    }
    else
    {
        const TYPE                          fD1_N = V(F_DOT)(pvD0, &psPE0->m_vNormal);
        const TYPE                          fX0_N = V(F_DOT)(&vX0, &psPE0->m_vNormal);

        if (F(tgPM_ABS)(fD1_N) > F(KTgEPS))
        {
            /* Non-trivial plane normal-component in the linear direction.  Check the intersection of the two primitives. */

            const TYPE                          fInt = -(fX0_N / fD1_N);

            if ((!LN_CAP_0 || fInt >= MKL(0.0)) && (!LN_CAP_1 || fInt <= MKL(1.0)))
            {
                V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fInt, pvD0);
                V(C_TgVEC)                          vK0 = V(F_ADD)(&vX0, &vK1);
                const TYPE                          fINT_EN0 = V(F_DOT)(psPE0->m_avEdgeNormal + 0, &vK0);
                const TYPE                          fINT_EN1 = V(F_DOT)(psPE0->m_avEdgeNormal + 1, &vK0);

                if (fINT_EN0 >= MKL(0.0) && fINT_EN0 <= fE1_EN0 && fINT_EN1 >= MKL(0.0) && fINT_EN1 <= fE0_EN1)
                {
                    *pfPE0 = fINT_EN1 / fE0_EN1;
                    *pfPE1 = fINT_EN0 / fE1_EN0;
                    *pfLN0 = fInt;

                    return (-F(KTgMAX));
                };
            };
        };

        {
            V(C_TgVEC)                          vK2 = V(F_SUB)(pvS0, &psPE0->m_vOrigin);
            V(C_TgVEC)                          vX1 = V(F_ADD)(&vK2, pvD0);
            const TYPE                          fNegE = -F(KTgEPS);

            const TYPE                          f00 = V(F_DOT)(psPE0->m_avEdgeNormal + 0, &vX0);
            const TYPE                          f01 = V(F_DOT)(psPE0->m_avEdgeNormal + 1, &vX0);
            const TYPE                          f10 = V(F_DOT)(psPE0->m_avEdgeNormal + 0, LN_CAP_1 ? &vX1 : pvD0);
            const TYPE                          f11 = V(F_DOT)(psPE0->m_avEdgeNormal + 1, LN_CAP_1 ? &vX1 : pvD0);

            C_TgBOOL                            bK0 = LN_CAP_0 ? (f00 < F(KTgEPS)) : (f10 > F(KTgEPS));
            C_TgBOOL                            bK1 = LN_CAP_1 ? (f10 < F(KTgEPS)) : (f10 < fNegE);
            C_TgBOOL                            bK2 = LN_CAP_0 ? (f01 < F(KTgEPS)) : (f11 > F(KTgEPS));
            C_TgBOOL                            bK3 = LN_CAP_1 ? (f11 < F(KTgEPS)) : (f11 < fNegE);
            C_TgBOOL                            bK4 = LN_CAP_0 ? (fE1_EN0 - f00 < F(KTgEPS)) : (f10 < fNegE);
            C_TgBOOL                            bK5 = LN_CAP_1 ? (fE1_EN0 - f10 < F(KTgEPS)) : (f10 > F(KTgEPS));
            C_TgBOOL                            bK6 = LN_CAP_0 ? (fE0_EN1 - f01 < F(KTgEPS)) : (f11 < fNegE);
            C_TgBOOL                            bK7 = LN_CAP_1 ? (fE0_EN1 - f11 < F(KTgEPS)) : (f11 > F(KTgEPS));

            TYPE                                fPE0 = F(KTgMAX), fPE1 = F(KTgMAX), fG1 = F(KTgMAX);
            TYPE                                fT0, fT1, fTest, fDistSq = F(KTgMAX);
            TgSINT32                            iTest = 0;

            (void)fNegE;

            iTest |= (LN_CAP_0 && f00 >= MKL(0.0) && f01 >= MKL(0.0) && fE1_EN0 >= f00 && fE0_EN1 >= f01) ? (1 << 4) : 0;
            iTest |= (LN_CAP_1 && f10 >= MKL(0.0) && f11 >= MKL(0.0) && fE1_EN0 >= f10 && fE0_EN1 >= f11) ? (1 << 5) : 0;

            /* If the linear is a segment, and both termination points are contained inside of the normal extruded triangle then find the closest point to the plane - */
            /* and terminate processing. */

            if (iTest & (3 << 4))
            {
                const TYPE                          fX1_N = F(tgPM_ABS)(V(F_DOT)(&vX1, &psPE0->m_vNormal));

                if (F(tgPM_ABS)(fX0_N) < fX1_N)
                {
                    *pfPE0 = f01 / fE0_EN1;
                    *pfPE1 = f00 / fE1_EN0;
                    *pfLN0 = MKL(0.0);

                    return (fX0_N*fX0_N);
                }
                else
                {
                    *pfPE0 = f11 / fE0_EN1;
                    *pfPE1 = f10 / fE1_EN0;
                    *pfLN0 = MKL(1.0);

                    return (fX1_N*fX1_N);
                };
            };

            /* Distance calculation for the linear's origin if its capped and within the parallelogram's normal-extruded space. */

            if (iTest & (1 << 4))
            {
                fDistSq = fX0_N * fX0_N;
                fPE0 = f01 / fE0_EN1;
                fPE1 = f00 / fE1_EN0;
                fG1 = MKL(0.0);
            };

            /* Distance calculation for the linear's termination if its capped and within the parallelogram's normal-extruded space. */

            if (iTest & (1 << 5))
            {
                const TYPE                          fX1_N = V(F_DOT)(&vX1, &psPE0->m_vNormal);

                fTest = fX1_N * fX1_N;

                if (fTest < fDistSq)
                {
                    fDistSq = fTest;
                    fPE0 = f11 / fE0_EN1;
                    fPE1 = f10 / fE1_EN0;
                    fG1 = MKL(1.0);
                };
            };

            /* Compare the segment against the four parallelogram edges. */

            iTest |= bK0 ? (1 << 0) : 0;
            iTest |= bK1 ? (1 << 0) : 0;
            iTest |= bK2 ? (1 << 1) : 0;
            iTest |= bK3 ? (1 << 1) : 0;
            iTest |= bK4 ? (1 << 2) : 0;
            iTest |= bK5 ? (1 << 2) : 0;
            iTest |= bK6 ? (1 << 3) : 0;
            iTest |= bK7 ? (1 << 3) : 0;

            if (iTest & (1 << 0))
            {
                fTest = V(tgCO_F_LR11_ParamSq_LR11)(&fT0, &fT1, &psPE0->m_vOrigin, psPE0->m_avEdge + 0, pvS0, pvD0);

                if (fTest < fDistSq)
                {
                    fDistSq = fTest;
                    fPE0 = fT0;
                    fPE1 = MKL(0.0);
                    fG1 = fT1;
                };
            };

            if (iTest & (1 << 1))
            {
                fTest = V(tgCO_F_LR11_ParamSq_LR11)(&fT0, &fT1, &psPE0->m_vOrigin, psPE0->m_avEdge + 1, pvS0, pvD0);

                if (fTest < fDistSq)
                {
                    fDistSq = fTest;
                    fPE0 = MKL(0.0);
                    fPE1 = fT0;
                    fG1 = fT1;
                };
            };

            if (iTest & (1 << 2))
            {
                V(C_TgVEC)                          vK3 = V(tgGM_PE_Query_Point_2)(psPE0);

                fTest = V(tgCO_F_LR11_ParamSq_LR11)(&fT0, &fT1, &vK3, psPE0->m_avEdge + 0, pvS0, pvD0);

                if (fTest < fDistSq)
                {
                    fDistSq = fTest;
                    fPE0 = fT0;
                    fPE1 = MKL(1.0);
                    fG1 = fT1;
                };
            };

            if (iTest & (1 << 3))
            {
                V(C_TgVEC)                          vK4 = V(tgGM_PE_Query_Point_1)(psPE0);

                fTest = V(tgCO_F_LR11_ParamSq_LR11)(&fT0, &fT1, &vK4, psPE0->m_avEdge + 1, pvS0, pvD0);

                if (fTest < fDistSq)
                {
                    fDistSq = fTest;
                    fPE0 = MKL(1.0);
                    fPE1 = fT0;
                    fG1 = fT1;
                };
            };

            *pfPE0 = fPE0;
            *pfPE1 = fPE1;
            *pfLN0 = fG1;

            return (fDistSq);
        };
    };
}


/* ---- VI(tgCO_FI_PE_Clip_Param_LR) ------------------------------------------------------------------------------------------------------------------------------------ */
/*  -- Internal Function -- LN_CAP_0, LN_CAP_1 : Indicate the termination condition of the linear {bc0,LN_CAP_1}                                                          */
/* Input:  psPE0: Parallelogram primitive - F_Clip-space is the region defined by the infinite extrusion along the normal.                                                */
/* Input:  vS0,vD0: Origin and Direction for the Linear                                                                                                                   */
/* Output: fLN0,fLN1: Parametric parameter to generate the two points of the linear contained inside the clip space.                                                      */
/* Return: Result Code.                                                                                                                                                   */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT VI(tgCO_FI_PE_Clip_Param_LR)( TYPE *pfLN0, TYPE *pfLN1, V(CPC_TgPARALLELOGRAM) psPE0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
    const TYPE                          fE1_EN0 = V(F_DOT)(psPE0->m_avEdgeNormal + 0, psPE0->m_avEdge + 1);
    const TYPE                          fE0_EN1 = V(F_DOT)(psPE0->m_avEdgeNormal + 1, psPE0->m_avEdge + 0);
    V(TgVEC)                            vDS;
    TYPE                                fDS_N, fD1_N;
    TYPE                                fMin;
    TYPE                                fMax;

    if (fE1_EN0 <= F(KTgEPS) || fE0_EN1 <= F(KTgEPS))
    {
        /* Degenerate rectangle - One or both of the edges has a near-zero length */
        return (KTgE_FAIL);
    }

    vDS = V(F_SUB)(pvS0, &psPE0->m_vOrigin);
    fMin = -F(KTgMAX);
    fMax = F(KTgMAX);

    /* Quick out - Does the linear exist outside of the clip region. */

    fDS_N = V(F_DOT)(psPE0->m_avEdgeNormal + 0, &vDS);
    fD1_N = V(F_DOT)(psPE0->m_avEdgeNormal + 0, pvD0);

    if (LN_CAP_0 && fDS_N < MKL(0.0))
    {
        if (fD1_N < MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N < MKL(0.0))))
        {
            return (KTgE_NO_INTERSECT);
        };
    }
    else if (LN_CAP_0 && fDS_N > fE1_EN0)
    {
        if (fD1_N > MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N > fE1_EN0)))
        {
            return (KTgE_NO_INTERSECT);
        };
    };

    /* Find the non-capped intersections of this linear with the two enclosing planes. */

    if (fD1_N < -F(KTgEPS))
    {
        fMin = F(tgPM_FSEL)(fDS_N + fD1_N*fMin - fE1_EN0, (fE1_EN0 - fDS_N) / fD1_N, fMin);
        fMax = F(tgPM_FSEL)(fDS_N + fD1_N*fMax, fMax, -fDS_N / fD1_N);
    }
    else if (fD1_N > F(KTgEPS))
    {
        fMin = F(tgPM_FSEL)(fD1_N*fMin + fDS_N, fMin, -fDS_N / fD1_N);
        fMax = F(tgPM_FSEL)(fDS_N + fD1_N*fMax - fE1_EN0, (fE1_EN0 - fDS_N) / fD1_N, fMax);
    };

    /* Quick out - Does the linear exist outside of the clip region. */

    fDS_N = V(F_DOT)(psPE0->m_avEdgeNormal + 1, &vDS);
    fD1_N = V(F_DOT)(psPE0->m_avEdgeNormal + 1, pvD0);

    if (LN_CAP_0 && fDS_N < MKL(0.0))
    {
        if (fD1_N < MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N < MKL(0.0))))
        {
            return (KTgE_NO_INTERSECT);
        };
    }
    else if (LN_CAP_0 && fDS_N > fE0_EN1)
    {
        if (fD1_N > MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N > fE0_EN1)))
        {
            return (KTgE_NO_INTERSECT);
        };
    };

    /* Find the non-capped intersections of this linear with the two enclosing planes. */

    if (fD1_N < -F(KTgEPS))
    {
        fMin = F(tgPM_FSEL)(fDS_N + fD1_N*fMin - fE0_EN1, (fE0_EN1 - fDS_N) / fD1_N, fMin);
        fMax = F(tgPM_FSEL)(fDS_N + fD1_N*fMax, fMax, -fDS_N / fD1_N);
    }
    else if (fD1_N > F(KTgEPS))
    {
        fMin = F(tgPM_FSEL)(fD1_N*fMin + fDS_N, fMin, -fDS_N / fD1_N);
        fMax = F(tgPM_FSEL)(fDS_N + fD1_N*fMax - fE0_EN1, (fE0_EN1 - fDS_N) / fD1_N, fMax);
    };

    /* Return the results - capped to the linear legal region. */

    if (fMin > fMax || fMin <= -F(KTgMAX) || fMax >= F(KTgMAX))
    {
        return (KTgE_FAIL);
    };

    if (LN_CAP_0)
    {
        *pfLN0 = F(tgPM_FSEL)(*pfLN0, *pfLN0, MKL(0.0));
        *pfLN1 = F(tgPM_FSEL)(*pfLN1, *pfLN1, MKL(0.0));
    };

    if (LN_CAP_1)
    {
        *pfLN0 = F(tgPM_FSEL)(*pfLN0 - MKL(1.0), *pfLN0, MKL(1.0));
        *pfLN1 = F(tgPM_FSEL)(*pfLN1 - MKL(1.0), *pfLN1, MKL(1.0));
    };

    return (KTgS_OK);
}


MSVC_WARN_DISABLE_POP( 6235 )
MSVC_WARN_DISABLE_POP( 6240 )