Home

Resume

Blog

Teikitu


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

/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */
/*  File Local Functions                                                                                                                                                  */
/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */

static TYPE                                 VI(tgCO_F_BX_Box_DoF3_LR)( TYPE*, V(PC_TgVEC), V(CPC_TgVEC), V(CPC_TgBOX) );
static TYPE                                 VI(tgCO_F_BX_Box_DoF2_LR)( TYPE*, V(PC_TgVEC), V(CPC_TgVEC), C_TgSINT32, C_TgSINT32, C_TgSINT32, V(CPC_TgBOX) );




/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */
/*  Public Functions                                                                                                                                                      */
/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */

/* ---- VI(tgCO_FI_BX_ParamSq_LR) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgBX0: Box primitive                                                                                                                                           */
/* Input:  vS0,vD0: Origin and Direction for Linear                                                                                                                       */
/* Output: tyB0,tyB1,tyB2: Parametric parameters to generate the point of closest proximity on the box (one for each axis)                                                */
/* Output: tyLN0: Parametric parameter to generate the point of proximity on the linear                                                                                   */
/* Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.                                                             */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE VI(tgCO_FI_BX_ParamSq_LR)( TYPE *pfB0, TYPE *pfB1, TYPE *pfB2, TYPE *pfLN0, V(CPC_TgBOX) psBX0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
 /* Compute coordinates of line in box coordinate system */

    V(TgVEC)                            vDS = V(F_SUB)(pvS0, &psBX0->m.m.vOrigin);

    const TYPE                          fD1_X0 = V(F_DOT)(pvD0, psBX0->m.m.avAxis + 0);
    const TYPE                          fD1_X1 = V(F_DOT)(pvD0, psBX0->m.m.avAxis + 1);
    const TYPE                          fD1_X2 = V(F_DOT)(pvD0, psBX0->m.m.avAxis + 2);
    const TYPE                          fDS_X0 = V(F_DOT)(&vDS, psBX0->m.m.avAxis + 0);
    const TYPE                          fDS_X1 = V(F_DOT)(&vDS, psBX0->m.m.avAxis + 1);
    const TYPE                          fDS_X2 = V(F_DOT)(&vDS, psBX0->m.m.avAxis + 2);

    TYPE                                fDistSq = MKL(0.0);
    V(TgVEC)                            vDN;

    vDS = V(FS_SETV)(
        F(tgPM_FSEL)(fD1_X0, fDS_X0, -fDS_X0),
        F(tgPM_FSEL)(fD1_X1, fDS_X1, -fDS_X1),
        F(tgPM_FSEL)(fD1_X2, fDS_X2, -fDS_X2)
    );

    vDN = V(FS_SETV)(F(tgPM_ABS)(fD1_X0), F(tgPM_ABS)(fD1_X1), F(tgPM_ABS)(fD1_X2));

    /* Calculate the distance and place of closest contact. */

    switch ((F(tgCM_NR0)(pvD0->m.x) ? 0 : 1) + (F(tgCM_NR0)(pvD0->m.y) ? 0 : 2) + (F(tgCM_NR0)(pvD0->m.z) ? 0 : 4))
    {
        case 7:
        { /* (+,+,+) */

            fDistSq += VI(tgCO_F_BX_Box_DoF3_LR)(pfLN0, &vDS, &vDN, psBX0);

            *pfB0 = F(tgPM_FSEL)(fD1_X0, vDS.m.x, -vDS.m.x);
            *pfB1 = F(tgPM_FSEL)(fD1_X1, vDS.m.y, -vDS.m.y);
            *pfB2 = F(tgPM_FSEL)(fD1_X2, vDS.m.z, -vDS.m.z);

            break;
        }

        case 6:
        { /* (+,+,0) */

            fDistSq += VI(tgCO_F_BX_Box_DoF2_LR)(pfLN0, &vDS, &vDN, 0, 1, 2, psBX0);

            {
                const TYPE                          fT2 = F(tgPM_ABS)(vDS.m.z) - psBX0->m_vExtent.m.z;
                const TYPE                          fK0 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.z, vDS.m.z);
                const TYPE                          fZ = F(tgPM_FSEL)(fT2, fK0, vDS.m.z);

                fDistSq += F(tgPM_FSEL)(fT2, fT2*fT2, MKL(0.0));

                *pfB0 = F(tgPM_FSEL)(fD1_X0, vDS.m.x, -vDS.m.x);
                *pfB1 = F(tgPM_FSEL)(fD1_X1, vDS.m.y, -vDS.m.y);
                *pfB2 = F(tgPM_FSEL)(fD1_X2, fZ, -fZ);
            }

            break;
        }

        case 5:
        { /* (+,0,+) */

            fDistSq += VI(tgCO_F_BX_Box_DoF2_LR)(pfLN0, &vDS, &vDN, 0, 2, 1, psBX0);

            {
                const TYPE                          fT1 = F(tgPM_ABS)(vDS.m.y) - psBX0->m_vExtent.m.y;
                const TYPE                          fK0 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.y, vDS.m.y);
                const TYPE                          fY = F(tgPM_FSEL)(fT1, fK0, vDS.m.y);

                fDistSq += F(tgPM_FSEL)(fT1, fT1*fT1, MKL(0.0));

                *pfB0 = F(tgPM_FSEL)(fD1_X0, vDS.m.x, -vDS.m.x);
                *pfB1 = F(tgPM_FSEL)(fD1_X1, fY, -fY);
                *pfB2 = F(tgPM_FSEL)(fD1_X2, vDS.m.z, -vDS.m.z);
            }

            break;
        }

        case 3:
        { /* (0,+,+) */

            fDistSq += VI(tgCO_F_BX_Box_DoF2_LR)(pfLN0, &vDS, &vDN, 1, 2, 0, psBX0);

            {
                const TYPE                          fT0 = F(tgPM_ABS)(vDS.m.x) - psBX0->m_vExtent.m.x;
                const TYPE                          fK0 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.x, vDS.m.x);
                const TYPE                          fX = F(tgPM_FSEL)(fT0, fK0, vDS.m.x);

                fDistSq += F(tgPM_FSEL)(fT0, fT0*fT0, MKL(0.0));

                *pfB0 = F(tgPM_FSEL)(fD1_X0, fX, -fX);
                *pfB1 = F(tgPM_FSEL)(fD1_X1, vDS.m.y, -vDS.m.y);
                *pfB2 = F(tgPM_FSEL)(fD1_X2, vDS.m.z, -vDS.m.z);
            }

            break;
        }

        case 4:
        { /* (+,0,0) */

            const TYPE                          fT1 = F(tgPM_ABS)(vDS.m.y) - psBX0->m_vExtent.m.y;
            const TYPE                          fT2 = F(tgPM_ABS)(vDS.m.z) - psBX0->m_vExtent.m.z;
            const TYPE                          fK0 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.y, vDS.m.y);
            const TYPE                          fY = F(tgPM_FSEL)(fT1, fK0, vDS.m.y);
            const TYPE                          fK1 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.z, vDS.m.z);
            const TYPE                          fZ = F(tgPM_FSEL)(fT2, fK1, vDS.m.z);

            fDistSq += F(tgPM_FSEL)(fT1, fT1*fT1, MKL(0.0)) + F(tgPM_FSEL)(fT2, fT2*fT2, MKL(0.0));

            *pfB0 = MKL(0.0);
            *pfB1 = F(tgPM_FSEL)(fD1_X1, fY, -fY);
            *pfB2 = F(tgPM_FSEL)(fD1_X2, fZ, -fZ);
            *pfLN0 = -(vDS.m.x / vDN.m.x);

            break;
        }

        case 2:
        { /* (0,+,0) */

            const TYPE                          fT0 = F(tgPM_ABS)(vDS.m.x) - psBX0->m_vExtent.m.x;
            const TYPE                          fT2 = F(tgPM_ABS)(vDS.m.z) - psBX0->m_vExtent.m.z;
            const TYPE                          fK0 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.x, vDS.m.x);
            const TYPE                          fX = F(tgPM_FSEL)(fT0, fK0, vDS.m.x);
            const TYPE                          fK1 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.z, vDS.m.z);
            const TYPE                          fZ = F(tgPM_FSEL)(fT2, fK1, vDS.m.z);

            fDistSq += F(tgPM_FSEL)(fT0, fT0*fT0, MKL(0.0)) + F(tgPM_FSEL)(fT2, fT2*fT2, MKL(0.0));

            *pfB0 = F(tgPM_FSEL)(fD1_X0, fX, -fX);
            *pfB1 = MKL(0.0);
            *pfB2 = F(tgPM_FSEL)(fD1_X2, fZ, -fZ);
            *pfLN0 = -(vDS.m.y / vDN.m.y);

            break;
        }

        case 1:
        { /* (0,0,+) */

            const TYPE                          fT0 = F(tgPM_ABS)(vDS.m.x) - psBX0->m_vExtent.m.x;
            const TYPE                          fT1 = F(tgPM_ABS)(vDS.m.y) - psBX0->m_vExtent.m.y;
            const TYPE                          fK0 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.x, vDS.m.x);
            const TYPE                          fX = F(tgPM_FSEL)(fT0, fK0, vDS.m.x);
            const TYPE                          fK1 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.y, vDS.m.y);
            const TYPE                          fY = F(tgPM_FSEL)(fT1, fK1, vDS.m.y);

            fDistSq += F(tgPM_FSEL)(fT0, fT0*fT0, MKL(0.0)) + F(tgPM_FSEL)(fT1, fT1*fT1, MKL(0.0));

            *pfB0 = F(tgPM_FSEL)(fD1_X0, fX, -fX);
            *pfB1 = F(tgPM_FSEL)(fD1_X1, fY, -fY);
            *pfB2 = MKL(0.0);
            *pfLN0 = -(vDS.m.z / vDN.m.z);

            break;
        }

        case 0:
        { /* (0,0,0) */
            const TYPE                          fT0 = F(tgPM_ABS)(vDS.m.x) - psBX0->m_vExtent.m.x;
            const TYPE                          fT1 = F(tgPM_ABS)(vDS.m.y) - psBX0->m_vExtent.m.y;
            const TYPE                          fT2 = F(tgPM_ABS)(vDS.m.z) - psBX0->m_vExtent.m.z;
            const TYPE                          fK0 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.x, vDS.m.x);
            const TYPE                          fX = F(tgPM_FSEL)(fT0, fK0, vDS.m.x);
            const TYPE                          fK1 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.y, vDS.m.y);
            const TYPE                          fY = F(tgPM_FSEL)(fT1, fK1, vDS.m.y);
            const TYPE                          fK2 = F(tgPM_COPY_SIGN)(psBX0->m_vExtent.m.z, vDS.m.z);
            const TYPE                          fZ = F(tgPM_FSEL)(fT2, fK2, vDS.m.z);

            fDistSq += F(tgPM_FSEL)(fT0, fT0*fT0, MKL(0.0))
                + F(tgPM_FSEL)(fT1, fT1*fT1, MKL(0.0))
                + F(tgPM_FSEL)(fT2, fT2*fT2, MKL(0.0));

            *pfB0 = F(tgPM_FSEL)(fD1_X0, fX, -fX);
            *pfB1 = F(tgPM_FSEL)(fD1_X1, fY, -fY);
            *pfB2 = F(tgPM_FSEL)(fD1_X2, fZ, -fZ);
            *pfLN0 = MKL(0.0);

            break;
        }
    };

    if (LN_CAP_0 && *pfLN0 < MKL(0.0))
    {
        *pfLN0 = MKL(0.0);
        return (V(tgCO_F_BX_ParamSq_VT)(pfB0, pfB1, pfB2, psBX0, pvS0));
    };

    if (LN_CAP_1 && *pfLN0 > MKL(1.0))
    {
        V(C_TgVEC)                          vK0 = V(F_ADD)(pvS0, pvD0);

        *pfLN0 = MKL(1.0);
        return (V(tgCO_F_BX_ParamSq_VT)(pfB0, pfB1, pfB2, psBX0, &vK0));
    };

    return (fDistSq);
}


/* ---- VI(tgCO_FI_BX_Clip_Param_LR) ------------------------------------------------------------------------------------------------------------------------------------ */
/* Input:  tgBX0: Box primitive                                                                                                                                           */
/* 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_BX_Clip_Param_LR)( TYPE *pfLN0, TYPE *pfLN1, V(CPC_TgBOX) psBX0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
    V(TgVEC)                            vDS = V(F_SUB)(pvS0, &psBX0->m.m.vOrigin);
    TYPE                                fDS_N, fD1_N;
    TYPE                                fMin = -F(KTgMAX);
    TYPE                                fMax = F(KTgMAX);

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

    TgPARAM_CHECK( V(tgGM_BX_Is_Valid)(psBX0) );

    fDS_N = V(F_DOT)(psBX0->m.m.avAxis + 0, &vDS);
    fD1_N = V(F_DOT)(psBX0->m.m.avAxis + 0, pvD0);

    if (LN_CAP_0 && fDS_N < -psBX0->m_vExtent.m.x)
    {
        if (fD1_N < MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N < -psBX0->m_vExtent.m.x)))
        {
            return (KTgE_NO_INTERSECT);
        };
    }
    else if (LN_CAP_0 && fDS_N > psBX0->m_vExtent.m.x)
    {
        if (fD1_N > MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N > psBX0->m_vExtent.m.x)))
        {
            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 - psBX0->m_vExtent.m.x, (psBX0->m_vExtent.m.x - fDS_N) / fD1_N, fMin);
        fMax = F(tgPM_FSEL)(fDS_N + fD1_N*fMax + psBX0->m_vExtent.m.x, fMax, -(psBX0->m_vExtent.m.x + fDS_N) / fD1_N);
    }
    else if (fD1_N > F(KTgEPS))
    {
        fMin = F(tgPM_FSEL)(fDS_N + fD1_N*fMin + psBX0->m_vExtent.m.x, fMin, -(psBX0->m_vExtent.m.x + fDS_N) / fD1_N);
        fMax = F(tgPM_FSEL)(fDS_N + fD1_N*fMax - psBX0->m_vExtent.m.x, (psBX0->m_vExtent.m.x - fDS_N) / fD1_N, fMax);
    };

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

    fDS_N = V(F_DOT)(psBX0->m.m.avAxis + 1, &vDS);
    fD1_N = V(F_DOT)(psBX0->m.m.avAxis + 1, pvD0);

    if (LN_CAP_0 && fDS_N < -psBX0->m_vExtent.m.y)
    {
        if (fD1_N < MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N < -psBX0->m_vExtent.m.y)))
        {
            return (KTgE_NO_INTERSECT);
        };
    }
    else if (LN_CAP_0 && fDS_N > psBX0->m_vExtent.m.y)
    {
        if (fD1_N > MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N > psBX0->m_vExtent.m.y)))
        {
            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 - psBX0->m_vExtent.m.y, (psBX0->m_vExtent.m.y - fDS_N) / fD1_N, fMin);
        fMax = F(tgPM_FSEL)(fDS_N + fD1_N*fMax + psBX0->m_vExtent.m.y, fMax, -(psBX0->m_vExtent.m.y + fDS_N) / fD1_N);
    }
    else if (fD1_N > F(KTgEPS))
    {
        fMin = F(tgPM_FSEL)(fDS_N + fD1_N*fMin + psBX0->m_vExtent.m.y, fMin, -(psBX0->m_vExtent.m.y + fDS_N) / fD1_N);
        fMax = F(tgPM_FSEL)(fDS_N + fD1_N*fMax - psBX0->m_vExtent.m.y, (psBX0->m_vExtent.m.y - fDS_N) / fD1_N, fMax);
    };

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

    fDS_N = V(F_DOT)(psBX0->m.m.avAxis + 2, &vDS);
    fD1_N = V(F_DOT)(psBX0->m.m.avAxis + 2, pvD0);

    if (LN_CAP_0 && fDS_N < -psBX0->m_vExtent.m.z)
    {
        if (fD1_N < MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N < -psBX0->m_vExtent.m.z)))
        {
            return (KTgE_NO_INTERSECT);
        };
    }
    else if (LN_CAP_0 && fDS_N > psBX0->m_vExtent.m.z)
    {
        if (fD1_N > MKL(0.0) || (LN_CAP_1 && (fDS_N + fD1_N > psBX0->m_vExtent.m.z)))
        {
            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 + fMin*fD1_N - psBX0->m_vExtent.m.z, (psBX0->m_vExtent.m.z - fDS_N) / fD1_N, fMin);
        fMax = F(tgPM_FSEL)(fDS_N + fMax*fD1_N + psBX0->m_vExtent.m.z, fMax, -(psBX0->m_vExtent.m.z + fDS_N) / fD1_N);
    }
    else if (fD1_N > F(KTgEPS))
    {
        fMin = F(tgPM_FSEL)(fDS_N + fMin*fD1_N + psBX0->m_vExtent.m.z, fMin, -(psBX0->m_vExtent.m.z + fDS_N) / fD1_N);
        fMax = F(tgPM_FSEL)(fDS_N + fMax*fD1_N - psBX0->m_vExtent.m.z, (psBX0->m_vExtent.m.z - 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);
}




/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */
/*  File Local Functions                                                                                                                                                  */
/* -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. */

/* ---- VI(tgCO_F_BX_Box_DoF3_LR) --------------------------------------------------------------------------------------------------------------------------------------- */
/*  -- Internal Function -- (Helper to create contact points the box and linear - three degrees of freedom)                                                               */
/* Input:  vDS: The difference vector between the origin of the two primitives (absolute value)                                                                           */
/* Input:  vD0: Direction of the Linear                                                                                                                                   */
/* Input:  tgBX0: Box primitive                                                                                                                                           */
/* Output: fLN0: Parametric parameter to generate the point of proximity on the linear                                                                                    */
/* Output: vDS: The difference vector between the two points of closest proximity                                                                                         */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE VI(tgCO_F_BX_Box_DoF3_LR)( TYPE *pfLN0, V(PC_TgVEC) pvDS, V(CPC_TgVEC) pvD0, V(CPC_TgBOX) psBX0 )
{
    V(TgVEC)                            vD1_D1, vD1_MN, vD1_MX, vMN_MN, vMX_MX;
    V(C_TgVEC)                          vMN = V(F_SUB)(pvDS, &psBX0->m_vExtent);
    V(C_TgVEC)                          vMX = V(F_ADD)(pvDS, &psBX0->m_vExtent);
    V(C_TgVEC)                          vD1xMN = V(F_CX)(pvD0, &vMN);
    const TYPE                          fD1_D1 = V(F_LSQ)(pvD0);
    TgUINT32                            i0, i1, i2;

    vD1_D1 = V(F_MUL)(pvD0, pvD0);
    vD1_MN = V(F_MUL)(pvD0, &vMN);
    vD1_MX = V(F_MUL)(pvD0, &vMX);
    vMN_MN = V(F_MUL)(&vMN, &vMN);
    vMX_MX = V(F_MUL)(&vMX, &vMX);

    if (vD1xMN.m.z >= MKL(0.0))
    {
        i0 = vD1xMN.m.x >= MKL(0.0) ? 2 : 1;
        i1 = vD1xMN.m.x >= MKL(0.0) ? 0 : 2;
        i2 = vD1xMN.m.x >= MKL(0.0) ? 1 : 0;
    }
    else
    {
        i0 = vD1xMN.m.y >= MKL(0.0) ? 0 : 2;
        i1 = vD1xMN.m.y >= MKL(0.0) ? 1 : 0;
        i2 = vD1xMN.m.y >= MKL(0.0) ? 2 : 1;
    }

    pvDS->m_aData[i0] = psBX0->m_vExtent.m_aData[i0];

    if (pvD0->m_aData[i0] * vMX.m_aData[i2] >= pvD0->m_aData[i2] * vMN.m_aData[i0] &&
        pvD0->m_aData[i0] * vMX.m_aData[i1] >= pvD0->m_aData[i1] * vMN.m_aData[i0])
    {
        const TYPE                          fINV = MKL(1.0) / pvD0->m_aData[i0];

        pvDS->m_aData[i1] -= pvD0->m_aData[i1] * vMN.m_aData[i0] * fINV;
        pvDS->m_aData[i2] -= pvD0->m_aData[i2] * vMN.m_aData[i0] * fINV;

        *pfLN0 = -vMN.m_aData[i0] * fINV;

        return (MKL(0.0));
    }
    else
    {
        const TYPE                          fK1 = vD1_MN.m_aData[i0] + vD1_MX.m_aData[i2];
        const TYPE                          fK2 = vD1_D1.m_aData[i0] + vD1_D1.m_aData[i2];
        const TYPE                          fK0 = vMX.m_aData[i1] - pvD0->m_aData[i1] * (fK1 / fK2);

        if (pvD0->m_aData[i0] * vMX.m_aData[i1] >= pvD0->m_aData[i1] * vMN.m_aData[i0] ||
            (fK0 >= MKL(0.0) && pvD0->m_aData[i0] * vMX.m_aData[i2] < pvD0->m_aData[i2] * vMN.m_aData[i0]))
        {
            const TYPE                          fLength = MKL(2.0)*psBX0->m_vExtent.m_aData[i1];
            const TYPE                          fT = F(tgPM_FSEL)(fLength - fK0, F(tgPM_FSEL)(fK0, fK0, MKL(0.0)), fLength);
            const TYPE                          fTB = vMX.m_aData[i1] - fT;
            const TYPE                          fDT = vD1_MN.m_aData[i0] + vD1_MX.m_aData[i2] + pvD0->m_aData[i1] * fTB;
            const TYPE                          fPM = -fDT / fD1_D1;

            pvDS->m_aData[i1] = -psBX0->m_vExtent.m_aData[i1] + fT;
            pvDS->m_aData[i2] = -psBX0->m_vExtent.m_aData[i2];
            *pfLN0 = fPM;

            return (vMN_MN.m_aData[i0] + vMX_MX.m_aData[i2] + fDT*fPM + fTB*fTB);
        }
        else
        {
            const TYPE                          fK4 = vD1_D1.m_aData[i0] + vD1_D1.m_aData[i1];
            const TYPE                          fK5 = vD1_MN.m_aData[i0] + vD1_MX.m_aData[i1];
            const TYPE                          fK3 = vMX.m_aData[i2] - pvD0->m_aData[i2] * (fK5 / fK4);

            const TYPE                          fLength = MKL(2.0)*psBX0->m_vExtent.m_aData[i2];
            const TYPE                          fT = F(tgPM_FSEL)(fLength - fK3, F(tgPM_FSEL)(fK3, fK3, MKL(0.0)), fLength);
            const TYPE                          fTB = vMX.m_aData[i2] - fT;
            const TYPE                          fDT = vD1_MN.m_aData[i0] + vD1_MX.m_aData[i1] + pvD0->m_aData[i2] * fTB;
            const TYPE                          fPM = -fDT / fD1_D1;

            pvDS->m_aData[i1] = -psBX0->m_vExtent.m_aData[i1];
            pvDS->m_aData[i2] = -psBX0->m_vExtent.m_aData[i2] + fT;
            *pfLN0 = fPM;

            return (vMN_MN.m_aData[i0] + vMX_MX.m_aData[i1] + fDT*fPM + fTB*fTB);
        };
    };
}


/* ---- VI(tgCO_F_BX_Box_DoF2_LR) --------------------------------------------------------------------------------------------------------------------------------------- */
/*  -- Internal Function -- (Helper to create contact points the box and linear - two degrees of freedom)                                                                 */
/* Input:  vDS: The difference vector between the origin of the two primitives                                                                                            */
/* Input:  vD0: Direction of the Linear                                                                                                                                   */
/* Input:  i0,i1,i2: Used to give the face order for the test                                                                                                             */
/* Input:  tgBX0: Box primitive                                                                                                                                           */
/* Output: tyLN0: Parametric parameter to generate the point of proximity on the linear                                                                                   */
/* Output: vDS: The difference vector between the two points of closest proximity                                                                                         */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE VI(tgCO_F_BX_Box_DoF2_LR)( TYPE *pfLN0, V(PC_TgVEC) pvDS, V(CPC_TgVEC) pvD0, C_TgSINT32 i0, C_TgSINT32 i1, C_TgSINT32 UNUSED_PARAM i2, V(CPC_TgBOX) psBX0 )
{
    /* Construct the direction vector from the box corner at the positive extents to the line origin. */

    const TYPE                          fMinDT0 = pvDS->m_aData[i0] - psBX0->m_vExtent.m_aData[i0];
    const TYPE                          fMinDT1 = pvDS->m_aData[i1] - psBX0->m_vExtent.m_aData[i1];
    const TYPE                          fMinDT1_DN0 = pvD0->m_aData[i0] * fMinDT1;
    const TYPE                          fMinDT0_DN1 = pvD0->m_aData[i1] * fMinDT0;

    V(TgVEC)                            vD1_D1;

    vD1_D1 = V(F_MUL)(pvD0, pvD0);

    /*const TYPE                          fD1_D1 = V(F_LSQ)( vD0 ); */

    if (fMinDT0_DN1 >= fMinDT1_DN0)
    {
        const TYPE                          fMaxDT1 = pvDS->m_aData[i1] + psBX0->m_vExtent.m_aData[i1];
        const TYPE                          fDT = fMinDT0_DN1 - pvD0->m_aData[i0] * fMaxDT1;

        pvDS->m_aData[i0] = psBX0->m_vExtent.m_aData[i0];

        if (fDT >= MKL(0.0)) /* Line is outside of the box. */
        {
            const TYPE                          fK0 = MKL(1.0) / (vD1_D1.m_aData[i0] + vD1_D1.m_aData[i1]);

            pvDS->m_aData[i1] = -psBX0->m_vExtent.m_aData[i1];
            *pfLN0 = -(pvD0->m_aData[i0] * fMinDT0 + pvD0->m_aData[i1] * fMaxDT1)*fK0;

            return (fDT*fDT*fK0);
        }
        else /* Line intersects the box. */
        {
            const TYPE                          fK0 = MKL(1.0) / pvD0->m_aData[i0];

            pvDS->m_aData[i1] -= fMinDT0_DN1*fK0;
            *pfLN0 = -fMinDT0*fK0;

            return (MKL(0.0));
        }
    }
    else
    {
        const TYPE                          fMaxDT0 = pvDS->m_aData[i0] + psBX0->m_vExtent.m_aData[i0];
        const TYPE                          fDT = fMinDT1_DN0 - pvD0->m_aData[i1] * fMaxDT0;

        pvDS->m_aData[i1] = psBX0->m_vExtent.m_aData[i1];

        if (fDT >= MKL(0.0)) /* Line is outside of the box. */
        {
            const TYPE                          fK0 = MKL(1.0) / (vD1_D1.m_aData[i0] + vD1_D1.m_aData[i1]);

            pvDS->m_aData[i0] = -psBX0->m_vExtent.m_aData[i0];
            *pfLN0 = -(pvD0->m_aData[i0] * fMaxDT0 + pvD0->m_aData[i1] * fMinDT1)*fK0;

            return (fDT*fDT*fK0);
        }
        else /* Line intersects the box. */
        {
            const TYPE                          fK0 = MKL(1.0) / pvD0->m_aData[i1];

            pvDS->m_aData[i0] -= fMinDT1_DN0*fK0;
            *pfLN0 = -fMinDT1*fK0;

            return (MKL(0.0));
        }
    }
}