Home

Resume

Blog

Teikitu


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

/* ---- VI2(tgCO_FI_TB,Intersect_LR) ------------------------------------------------------------------------------------------------------------------------------------ */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psTB0, fPad: Tube primitive and scalar value by which to extend the tube radius                                                                                */
/* Input:  vS0,vD0: Origin and Direction for the Linear                                                                                                                   */
/* Output: tgPacket: Points of intersection between the two primitives are added to it                                                                                    */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT VI2(tgCO_FI_TB,Intersect_LR)( V(PC_STg2_CO_Packet) psPacket, const TYPE fPad, V(CPC_TgTUBE) psTB0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
    TYPE                                fLN0, fLN1;
    V(TgVEC)                            vN0, vN1;
    V(P_STg2_CO_Contact)                psContact;
    TgBOOL                              bIntersect = TgFALSE;

    C_TgRESULT iResult = VI2(tgCO_FI_TB,Param_LR)(&fLN0, &fLN1, &vN0, &vN1, fPad, psTB0, 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
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fLN0, pvD0);
        V(C_TgVEC)                          vL0 = V(F_ADD)(pvS0, &vK0);

    #if TB_CAP_0
        TYPE                                fLN0_UA = V(F_DOT)(&vL0, &psTB0->m.m.vU_HAX);
        #if TB_CAP_1
            if ((fLN0_UA >= -psTB0->m_fExtent) && (fLN0_UA <= psTB0->m_fExtent))
        #else
            if (fLN0_UA >= -psTB0->m_fExtent)
        #endif
    #endif

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

            psContact->m_vS0 = vL0;
            psContact->m_vN0 = vN0;
            psContact->m_fT0 = fLN0;
            psContact->m_fDepth = MKL(0.0);

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

    if (LN_CAP_1 && fLN1 > MKL(1.0))
    {
        if (fLN0 >= MKL(1.0))
        {
            return (KTgE_NO_INTERSECT);
        };
    }
    else
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fLN1, pvD0);
        V(C_TgVEC)                          vL1 = V(F_ADD)(pvS0, &vK0);

    #if TB_CAP_0
        TYPE                                fLN1_UA = V(F_DOT)(&vL1, &psTB0->m.m.vU_HAX);
        #if TB_CAP_1
            if ((fLN1_UA >= -psTB0->m_fExtent) && (fLN1_UA <= psTB0->m_fExtent))
        #else
            if (fLN1_UA >= -psTB0->m_fExtent)
        #endif
    #endif

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

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

            psContact->m_vS0 = vL1;
            psContact->m_vN0 = vN1;
            psContact->m_fT0 = fLN1;
            psContact->m_fDepth = MKL(0.0);

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

    return (bIntersect ? KTgS_OK : KTgE_NO_INTERSECT);
}


/* ---- VI2(tgCO_FI_TB, Param_LR) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psTB0, fPad: Tube primitive and scalar value by which to extend the tube radius                                                                                */
/* Input:  vS0,vD0: Origin and Direction for the Linear                                                                                                                   */
/* Output: fLN0,fLN1: Parametric parameter to generate the two points of the linear in contact with the extended tube surface                                             */
/* Output: vN0, vN1: Tube surface normal at the points of contact between the two primitives                                                                              */
/* Return: Result Code                                                                                                                                                    */
/*  Duplicated into F_Internal_Intersect to avoid doing some extraneous calculations.  Modifications have to be ported.                                                   */
/*  The termination condition of the linear is only used in quick-rejection code - the corresponding parametric parameters are                                            */
/* not guaranteed to obey their constraints.                                                                                                                              */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT VI2(tgCO_FI_TB, Param_LR)(
    TYPE *pfLN0, TYPE *pfLN1, V(PC_TgVEC) pvN0, V(PC_TgVEC) pvN1, const TYPE fPad, V(CPC_TgTUBE) psTB0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
    /* Segment in the reference frame of the tube */

    const TYPE                          fD1_U0 = V(F_DOT)(pvD0, &psTB0->m.m.vU_Basis0);
    const TYPE                          fD1_U1 = V(F_DOT)(pvD0, &psTB0->m.m.vU_Basis1);
    const TYPE                          fD1_UA = V(F_DOT)(pvD0, &psTB0->m.m.vU_HAX);

    const TYPE                          fA = fD1_U0*fD1_U0 + fD1_U1*fD1_U1;

    /* Branch for the case where the segment is perpendicular to the tube's cross-sectional plane. */

    TgERROR( V(tgGM_TB_Is_Valid)(psTB0) && V(F_Is_Point_Valid)(pvS0) && V(F_Is_Vector_Valid)(pvD0) );

    if (F(tgCM_NR0)(fA))
    {
        return (KTgE_NO_INTERSECT);
    }
    else
    {
        /* Relative position of the origin inside of the tube's reference frame. */

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

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

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

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

        const TYPE                          fRad = psTB0->m_fRadius + fPad;
        const TYPE                          fRadSq = fRad * fRad;

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

            if (!((fDS_UA > MKL(0.0)) ^ (fD1_UA > MKL(0.0))))
            {
                if (TB_CAP_0 && fDS_UA < -psTB0->m_fExtent)
                {
                    return (KTgE_NO_INTERSECT);
                };

                if (TB_CAP_1 && fDS_UA > psTB0->m_fExtent)
                {
                    return (KTgE_NO_INTERSECT);
                };
            };

            /* If the origin lies outside of the tube 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 > fRadSq && (fDS_U0*fD1_U0 + fDS_U1*fD1_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*fD1_U0 + fDS_U1*fD1_U1);
            const TYPE                          fC = fRelSq - fRadSq;
            const TYPE                          fDet = fHalfNegB*fHalfNegB - fC*fA;

            if (fDet < MKL(0.0))
            {
                return (KTgE_NO_INTERSECT);
            }
            else
            {
                const TYPE                          fInvA = MKL(1.0) / fA;
                const TYPE                          fDetSqrt = F(tgPM_SQRT)(fDet);
                const TYPE                          fT0 = (fHalfNegB - fDetSqrt) * fInvA;
                const TYPE                          fT1 = (fHalfNegB + fDetSqrt) * fInvA;
                const TYPE                          fK0 = fDS_UA + fT0 * fD1_UA;
                const TYPE                          fK1 = fDS_UA + fT1 * fD1_UA;

                V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT0, pvD0);
                V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fT1, pvD0);
                V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fK0, &psTB0->m.m.vU_HAX);
                V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fK1, &psTB0->m.m.vU_HAX);
                V(C_TgVEC)                          vK4 = V(F_SUB)(&vK0, &vK2);
                V(C_TgVEC)                          vK5 = V(F_SUB)(&vK1, &vK3);
                V(C_TgVEC)                          vK6 = V(F_ADD)(&vDS, &vK4);
                V(C_TgVEC)                          vK7 = V(F_ADD)(&vDS, &vK5);

                *pvN0 = V(F_NORM_LEN)(pfLN0, &vK6);
                *pvN1 = V(F_NORM_LEN)(pfLN1, &vK7);

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