Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Capsule-Linear.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Capsule;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> || <non-zero constant>) is always a non-zero constant. */


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

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

static TgRESULT                             VI(tgCO_FI_CP_Internal_LR)( TYPE*, V(PC_TgVEC), const TYPE, V(CPC_TgTUBE), V(CPC_TgVEC), V(CPC_TgVEC));




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

/* ---- VI(tgCO_FI_CP_Intersect_LR) ------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psCP0: Capsule primitive                                                                                                                                       */
/* Input:  vS0,vD0: Origin and Direction for Linear                                                                                                                       */
/* Output: tgPacket: Points of intersection between the two primitives are added to it                                                                                    */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT VI(tgCO_FI_CP_Intersect_LR)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgTUBE) psCP0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
    TYPE                                fLN0, fLN1;
    V(TgVEC)                            vN0, vN1;
    V(P_STg2_CO_Contact)                psContact;
    TgRESULT                            iResult;
    TgBOOL                              bContact = TgFALSE;

    /* Check to make sure that a valid contact, and contact packet exist. */

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

    iResult = VI(tgCO_FI_CP_Contact_LR)(&fLN0, &fLN1, &vN0, &vN1, psCP0, pvS0, pvD0);

    if (TgFAILED( iResult ))
    {
        return (iResult);
    };

    /* Limit the variable to the cap regions */

    if (LN_CAP_0 && fLN0 < MKL(0.0))
    {
        if (fLN1 <= MKL(0.0))
        {
            return (KTgE_NO_INTERSECT);
        };
    }
    else if (!LN_CAP_1 || fLN0 <= MKL(1.0))
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fLN0, pvD0);

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

        psContact->m_vS0 = V(F_ADD)(pvS0, &vK0);
        psContact->m_vN0 = vN0;
        psContact->m_fT0 = fLN0;
        psContact->m_fDepth = MKL(0.0);

        ++psPacket->m_niContact;
        bContact = TgTRUE;
    };

    if (LN_CAP_1 && fLN1 > MKL(1.0))
    {
        if (fLN0 >= MKL(1.0))
        {
            return (KTgE_NO_INTERSECT);
        };
    }
    else if (F(tgPM_ABS)(fLN1 - fLN1) > F(KTgEPS))
    {
        if (psPacket->m_niContact >= psPacket->m_niMaxContact)
        {
            return (KTgE_MAX_CONTACTS);
        }
        else
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fLN1, pvD0);

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

            psContact->m_vS0 = V(F_ADD)(pvS0, &vK0);
            psContact->m_vN0 = vN1;
            psContact->m_fT0 = fLN1;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;
            bContact = TgTRUE;
        };
    };

    return (bContact ? KTgS_OK : KTgE_NO_INTERSECT);
}


/* ---- VI(tgCO_FI_CP_Penetrate_LR) ------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  vL0: The point of closest proximity on the linear to the capsule axis.                                                                                         */
/* Input:  psCP0: Capsule primitive                                                                                                                                       */
/* Input:  vSP0: The point of closest proximity on the capsule axis to the linear.                                                                                        */
/* Input:  fDistSq: The minimal distance squared between the capsule axis and the linear.                                                                                 */
/* Output: tgPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT VI(tgCO_FI_CP_Penetrate_LR)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgVEC) pvL0, V(CPC_TgTUBE) psCP0, V(CPC_TgVEC) pvCP1, const TYPE fDistSq )
{
    V(TgVEC)                            vNormal, vK0;
    TYPE                                fNM;
    V(P_STg2_CO_Contact)                psContact;

    if (fDistSq <= F(KTgEPS))
    {
        if (F(tgCM_NR0)(psCP0->m.m.vU_HAX.m.z))
        {
            vNormal = V(FS_SETV)(-psCP0->m.m.vU_HAX.m.y, psCP0->m.m.vU_HAX.m.x, MKL(0.0));
        }
        else
        {
            vNormal = V(FS_SETV)(MKL(0.0), psCP0->m.m.vU_HAX.m.z, -psCP0->m.m.vU_HAX.m.y);
        };
        vNormal = V(F_NORM)(&vNormal);
        fNM = MKL(0.0);
    }
    else
    {
        V(C_TgVEC)                          vK1 = V(F_SUB)(pvCP1, pvL0);

        vNormal = V(F_NORM_LEN)(&fNM, &vK1);
    };

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

    vK0 = V(F_MUL_SV)(psCP0->m_fRadius, &vNormal);

    psContact->m_vS0 = V(F_SUB)(pvCP1, &vK0);
    psContact->m_vN0 = vNormal;
    psContact->m_fT0 = MKL(0.0);
    psContact->m_fDepth = psCP0->m_fRadius - fNM;

    ++psPacket->m_niContact;

    return (KTgS_OK);
}


/* ---- VI(tgCO_FI_CP_Contact_LR) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psCP0: Capsule primitive                                                                                                                                       */
/* Input:  vS0,vD0: Origin and Direction for Linear                                                                                                                       */
/* Output: fLN0,fLN1: Parametric parameter to generate the two points of the linear in contact with the capsule surface                                                   */
/* Output: vN0, vN1: Capsule surface normal at the points of contact between the two primitives                                                                           */
/* Return: Result Code                                                                                                                                                    */
/*   The internal functions do not clip the linear.  All passed in linears are treated as lines - the boolean markers are used to                                         */
/*  generate possible quick-out logic to avoid further processing.                                                                                                        */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT VI(tgCO_FI_CP_Contact_LR)( TYPE *pfLN0, TYPE *pfLN1, V(PC_TgVEC) pvN0, V(PC_TgVEC) pvN1, V(CPC_TgTUBE) psCP0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
 /* Linear in the reference frame of the capsule */

    const TYPE                          fD0_U0 = V(F_DOT)(pvD0, &psCP0->m.m.vU_Basis0);
    const TYPE                          fD0_U1 = V(F_DOT)(pvD0, &psCP0->m.m.vU_Basis1);
    const TYPE                          fD0_UA = V(F_DOT)(pvD0, &psCP0->m.m.vU_HAX);

    const TYPE                          fA = fD0_U0*fD0_U0 + fD0_U1*fD0_U1;

    /* Relative position of the origin inside of the capsule's reference frame. */

    V(C_TgVEC)                          vDS = V(F_SUB)(pvS0, &psCP0->m.m.vOrigin);

    const TYPE                          fDS_UA = V(F_DOT)(&vDS, &psCP0->m.m.vU_HAX);
    const TYPE                          fDS_U0 = V(F_DOT)(&vDS, &psCP0->m.m.vU_Basis0);
    const TYPE                          fDS_U1 = V(F_DOT)(&vDS, &psCP0->m.m.vU_Basis1);

    /* Relative distance of the origin on the cross-sectional plane of the capsule. */

    const TYPE                          fRelSq = fDS_U0*fDS_U0 + fDS_U1*fDS_U1;

    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psCP0) && V(F_Is_Point_Valid)(pvS0) && V(F_Is_Vector_Valid)(pvD0) );

    if (LN_CAP_0)
    {
        /* If the origin lies outside of the capsule and only moves away - intersection can not take place. */

        if (!((fDS_UA > MKL(0.0)) ^ (fD0_UA > MKL(0.0))) && F(tgPM_ABS)(fDS_UA) > psCP0->m_fExtent + psCP0->m_fRadius)
        {
            return (KTgE_NO_INTERSECT);
        };

        /* If the origin lies outside of the capsule and only moves away - intersection can not take place. In the radial case moving away is determined by projecting */
        /* the direction vector onto the difference vector after both have been projected onto the cross-sectional plane. */

        if (fRelSq > psCP0->m_fRadiusSq && (fDS_U0*fD0_U0 + fDS_U1*fD0_U1) > MKL(0.0))
        {
            return (KTgE_NO_INTERSECT);
        };
    };

    /* R² = (DS_U0 + ζ•D0_U0)² + (DS_U1 + ζ•D0_U1)² */
    /* R² = DS_U0•DS_U0 + 2•ζ•DS_U0•D0_U0 + ζ•ζ•D0_U0•D0_U0 + DS_U1•DS_U1 + 2•ζ•DS_U1•D0_U1 + ζ•ζ•D0_U1•D0_U1 */
    /* 0  = ζ•ζ_(D0_U0•D0_U0 + D0_U1•D0_U1,DIM) + ζ_(2•DS_U0•D0_U0 + 2•DS_U1•D0_U1,DIM) + DS_U0•DS_U0 + DS_U1•DS_U1 - R² */

    {
        const TYPE                          fHalfNegB = MKL(-1.0) * (fDS_U0*fD0_U0 + fDS_U1*fD0_U1);
        const TYPE                          fC = fRelSq - psCP0->m_fRadiusSq;
        const TYPE                          fDet = fHalfNegB*fHalfNegB - fC*fA;
        const TYPE                          fInvA = MKL(1.0) / fA;

        if (fDet < MKL(0.0))
        {
            return (KTgE_NO_INTERSECT);
        }
        else
        {
            const TYPE                          fDetSqrt = F(tgPM_SQRT)(fDet);
            TYPE                                fD0;
            V(TgVEC)                            vUD0 = V(F_NORM_LEN)(&fD0, pvD0);
            const TYPE                          fT0 = (fHalfNegB - fDetSqrt) * fInvA;
            const TYPE                          fT1 = (fHalfNegB + fDetSqrt) * fInvA;

            if (fD0 < F(KTgEPS))
            {
                return (KTgE_NO_INTERSECT);
            }
            else
            {
                const TYPE                          fH0 = fDS_UA + fT0 * fD0_UA;
                const TYPE                          fH1 = fDS_UA + fT1 * fD0_UA;
                const TYPE                          fInvD0 = MKL(1.0) / fD0;

                if (F(tgPM_ABS)(fH0) > psCP0->m_fExtent)
                {
                    if (TgFAILED( VI(tgCO_FI_CP_Internal_LR)(
                        pfLN0, pvN0, (fH0 > MKL(0.0) ? MKL(1.0) : MKL(-1.0)), psCP0, pvS0, &vUD0) ))
                    {
                        TgERROR( F(tgPM_ABS)(fH1) > psCP0->m_fExtent );
                        return (KTgE_NO_INTERSECT);
                    };

                    *pfLN0 *= fInvD0;
                }
                else
                {
                    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT0, pvD0);
                    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fH0, &psCP0->m.m.vU_HAX);
                    V(C_TgVEC)                          vK2 = V(F_SUB)(&vK0, &vK1);
                    V(C_TgVEC)                          vK3 = V(F_ADD)(&vDS, &vK2);

                    *pfLN0 = fT0;
                    *pvN0 = V(F_NORM)(&vK3);
                };


                if (F(tgPM_ABS)(fH1) > psCP0->m_fExtent)
                {
                    if (TgFAILED( VI(tgCO_FI_CP_Internal_LR)(
                        pfLN1, pvN1, (fH1 > MKL(0.0) ? MKL(1.0) : MKL(-1.0)), psCP0, pvS0, &vUD0) ))
                    {
                        TgERROR(TgFALSE);
                    };

                    *pfLN1 *= fInvD0;
                }
                else
                {
                    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT1, pvD0);
                    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fH1, &psCP0->m.m.vU_HAX);
                    V(C_TgVEC)                          vK2 = V(F_SUB)(&vK0, &vK1);
                    V(C_TgVEC)                          vK3 = V(F_ADD)(&vDS, &vK2);

                    *pfLN1 = fT1;
                    *pvN1 = V(F_NORM)(&vK3);
                };

                TgERROR( *pfLN0 <= *pfLN1 );

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




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

/* ---- VI(tgCO_FI_CP_Internal_LR) -------------------------------------------------------------------------------------------------------------------------------------- */
/*  -- Internal Function -- (Test against a capsule cap)                                                                                                                  */
/* Input:  fAx: A positive/negative indicator to select which end of the axis to test                                                                                     */
/* Input:  psCP0: Capsule primitive                                                                                                                                       */
/* Input:  vS0,vUD0: Origin and Normalized Direction for Linear                                                                                                           */
/* Output: fLN0: Parametric parameter to generate the point on the linear in contact with the capsule surface                                                             */
/* Output: vN0: Capsule surface normal at the points of contact between the two primitives                                                                                */
/* Return: Result Code                                                                                                                                                    */
/*                                                                                                                                                                        */
/*   The internal functions do not clip the linear.  All passed in linears are treated as lines - the boolean markers are used to                                         */
/*  generate possible quick-out logic to avoid further processing.                                                                                                        */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT VI(tgCO_FI_CP_Internal_LR)( TYPE *pfL0, V(PC_TgVEC) pvN0, const TYPE fAx, V(CPC_TgTUBE) psCP0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvUD0 )
{
    V(C_TgVEC)                          vD1 = V(F_MUL_SV)(fAx, &psCP0->m_vHAX);
    V(C_TgVEC)                          vS1 = V(F_ADD)(&psCP0->m.m.vOrigin, &vD1);
    V(C_TgVEC)                          vDS = V(F_SUB)(&vS1, pvS0);
    const TYPE                          fDS_DS = V(F_LSQ)(&vDS);
    const TYPE                          fT0 = V(F_DOT)(&vDS, pvUD0);
    const TYPE                          fDSC = fT0*fT0 - fDS_DS + psCP0->m_fRadiusSq;
    V(TgVEC)                            vT0, vK0;

    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psCP0) && V(F_Is_Point_Valid)(pvS0) && V(F_Is_Vector_Valid)(pvUD0) );

    if (fDSC > F(KTgEPS))
    {
        const TYPE                          fRoot = F(tgPM_SQRT)(fDSC);
        TYPE                                fT1;

        fT1 = fT0 + fRoot;
        vK0 = V(F_MUL_SV)(fT1, pvUD0);
        vT0 = V(F_SUB)(&vK0, &vDS);

        if (V(F_DOT)(&vT0, &vD1) >= MKL(0.0))
        {
            *pvN0 = V(F_NORM)(&vT0);
            *pfL0 = fT1;

            return (KTgS_OK);
        };

        fT1 = fT0 - fRoot;
        vK0 = V(F_MUL_SV)(fT1, pvUD0);
        vT0 = V(F_SUB)(&vK0, &vDS);

        if (V(F_DOT)(&vT0, &vD1) >= MKL(0.0))
        {
            *pvN0 = V(F_NORM)(&vT0);
            *pfL0 = fT1;

            return (KTgS_OK);
        };
    }
    else if (fDSC > -F(KTgEPS))
    {
        vK0 = V(F_MUL_SV)(fT0, pvUD0);
        vT0 = V(F_SUB)(&vK0, &vDS);

        if (V(F_DOT)(&vT0, &vD1) >= MKL(0.0))
        {
            *pvN0 = V(F_NORM)(&vT0);
            *pfL0 = fT0;

            return (KTgS_OK);
        };
    };

    return (KTgE_NO_INTERSECT);
}


MSVC_WARN_DISABLE_POP( 6235 )