Home

Resume

Blog

Teikitu


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

/* ---- VI(tgCO_FI_CY_Intersect_LR) ------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psCY0: Cylinder 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_CY_Intersect_LR)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgTUBE) psCY0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
    TYPE                                fLN0, fLN1;
    V(TgVEC)                            vN0, vN1;

    C_TgRESULT iResult = V(tgCO_FI_CY_Internal_LR00)(&fLN0, &fLN1, &vN0, &vN1, psCY0, pvS0, pvD0);

    if (TgFAILED( iResult ))
    {
        return (iResult);
    }
    else
    {
        V(P_STg2_CO_Contact)                psContact;

        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;

        /* Check to see if the solution was tangential.  If not add the second intersection point to the packet. */

        if (F(tgPM_ABS)(fLN0 - fLN1) > F(KTgEPS))
        {
            V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fLN1, pvD0);

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

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

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

            ++psPacket->m_niContact;
        };
    };

    return (KTgS_OK);
}


/* ---- VI(tgCO_FI_CY_Internal_LR) -------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psCY0: Cylinder 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 cylinder surface                                                  */
/* Output: vN0, vN1: Cylinder surface normal at the points of contact between the two primitives                                                                          */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT VI(tgCO_FI_CY_Internal_LR)( TYPE *pfLN0, TYPE *pfLN1, V(PC_TgVEC) pvN0, V(PC_TgVEC) pvN1, V(CPC_TgTUBE) psCY0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0 )
{
    /* Segment in the reference frame of the cylinder */

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

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

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

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

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

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

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

    if (fA + fD0_UA*fD0_UA <= F(KTgEPS))
    {
        return (KTgE_FAIL);
    };

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

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

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

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

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


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

    if (F(tgCM_NR0)(fA))
    {
        const TYPE                          fInvTMP = MKL(1.0) / fD0_UA;
        const TYPE                          fT0 = (-psCY0->m_fExtent - fDS_UA) * fInvTMP;
        const TYPE                          fT1 = (psCY0->m_fExtent - fDS_UA) * fInvTMP;

        if (fRelSq >= psCY0->m_fRadiusSq)
        {
            return (KTgE_NO_INTERSECT);
        };

        if (fT0 < fT1)
        {
            *pfLN0 = fT0;
            *pfLN1 = fT1;
            *pvN0 = V(F_NEG)(&psCY0->m.m.vU_HAX);
            *pvN1 = psCY0->m.m.vU_HAX;
        }
        else
        {
            *pfLN0 = fT1;
            *pfLN1 = fT0;
            *pvN0 = psCY0->m.m.vU_HAX;
            *pvN1 = V(F_NEG)(&psCY0->m.m.vU_HAX);
        };

        return (KTgS_OK);
    };


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

    if (F(tgCM_NR0)(fD0_UA))
    {
        /* The following assumes that D0_UA is exactly zero, instead of just being approximately close to it.  This allows for some fast math, avoiding the entire need */
        /* to project the segment onto the plane.  The minor error this may cause should be negligible. */

        /* Solve the planar problem. (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 = R² */
        /* ζ•ζ_(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² = 0 */

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

        const TYPE                          fDetSqrt = F(tgPM_SQRT)(fDet);
        const TYPE                          fInvA = MKL(1.0) / fA;
        const TYPE                          fT0 = (fHalfNegB - fDetSqrt) * fInvA;
        const TYPE                          fT1 = (fHalfNegB + fDetSqrt) * fInvA;

        /* Check to see if the line lies outside of the cylinder's extents. */

        if (F(tgPM_ABS)(fDS_UA) > psCY0->m_fExtent)
        {
            return (KTgE_NO_INTERSECT);
        };

        if (fDet < MKL(0.0))
        {
            return (KTgE_NO_INTERSECT);
        }

        if (fT0 < fT1)
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fDS_U0 + fT0*fD0_U0, &psCY0->m.m.vU_Basis0);
            V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fDS_U1 + fT0*fD0_U1, &psCY0->m.m.vU_Basis1);
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fDS_U0 + fT1*fD0_U0, &psCY0->m.m.vU_Basis0);
            V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fDS_U1 + fT1*fD0_U1, &psCY0->m.m.vU_Basis1);
            V(C_TgVEC)                          vK4 = V(F_ADD)(&vK0, &vK1);
            V(C_TgVEC)                          vK5 = V(F_ADD)(&vK2, &vK3);

            *pfLN0 = fT0;
            *pfLN1 = fT1;
            *pvN0 = V(F_NORM)(&vK4);
            *pvN1 = V(F_NORM)(&vK5);
        }
        else
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fDS_U0 + fT1*fD0_U0, &psCY0->m.m.vU_Basis0);
            V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fDS_U1 + fT1*fD0_U1, &psCY0->m.m.vU_Basis1);
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fDS_U0 + fT0*fD0_U0, &psCY0->m.m.vU_Basis0);
            V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fDS_U1 + fT0*fD0_U1, &psCY0->m.m.vU_Basis1);
            V(C_TgVEC)                          vK4 = V(F_ADD)(&vK0, &vK1);
            V(C_TgVEC)                          vK5 = V(F_ADD)(&vK2, &vK3);

            *pfLN0 = fT1;
            *pfLN1 = fT0;
            *pvN0 = V(F_NORM)(&vK4);
            *pvN1 = V(F_NORM)(&vK5);
        };

        return (KTgS_OK);
    }
    else
    {
        /* For the sake of speed, attempt to clip the line using only the cap planes. */

        const TYPE                          fInvTMP = MKL(1.0) / fD0_UA;
        TgBOOL                              bCapClip = TgFALSE;

        TYPE                                fT0 = MKL(0.0);

        const TYPE                          fPLN0 = (-psCY0->m_fExtent - fDS_UA) * fInvTMP;
        const TYPE                          fPLN1 = (psCY0->m_fExtent - fDS_UA) * fInvTMP;

        const TYPE                          fU0 = fDS_U0 + fPLN0 * fD0_U0;
        const TYPE                          fV0 = fDS_U1 + fPLN0 * fD0_U1;
        const TYPE                          fU1 = fDS_U0 + fPLN1 * fD0_U0;
        const TYPE                          fV1 = fDS_U1 + fPLN1 * fD0_U1;

        if (fU0*fU0 + fV0*fV0 <= psCY0->m_fRadiusSq)
        {
            if (fU1*fU1 + fV1*fV1 <= psCY0->m_fRadiusSq)
            {
                if (fPLN0 < fPLN1)
                {
                    *pfLN0 = fPLN0;
                    *pfLN1 = fPLN1;
                    *pvN0 = V(F_NEG)(&psCY0->m.m.vU_HAX);
                    *pvN1 = psCY0->m.m.vU_HAX;
                }
                else
                {
                    *pfLN0 = fPLN1;
                    *pfLN1 = fPLN0;
                    *pvN0 = psCY0->m.m.vU_HAX;
                    *pvN1 = V(F_NEG)(&psCY0->m.m.vU_HAX);
                };

                return (KTgS_OK);
            }

            bCapClip = TgTRUE;

            fT0 = fPLN0;
            *pvN0 = V(F_NEG)(&psCY0->m.m.vU_HAX);
        }
        else if (fU1*fU1 + fV1*fV1 <= psCY0->m_fRadiusSq)
        {
            bCapClip = TgTRUE;

            fT0 = fPLN1;
            *pvN0 = psCY0->m.m.vU_HAX;
        };

        /* The segment may intersect the cylinder walls */

        /* Solve the planar problem. (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 = R² */
        /* ζ•ζ_(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² = 0 */

        {
            const TYPE                          fHalfNegB = MKL(-1.0) * (fDS_U0*fD0_U0 + fDS_U1*fD0_U1);
            const TYPE                          fC = fRelSq - psCY0->m_fRadiusSq;
            const TYPE                          fDet = fHalfNegB*fHalfNegB - fC*fA;
            const TYPE                          fInvA = MKL(1.0) / fA;
            const TYPE                          fDetSqrt = F(tgPM_SQRT)(fDet);
            const TYPE                          fTA = (fHalfNegB - fDetSqrt) * fInvA;
            const TYPE                          fTB = (fHalfNegB + fDetSqrt) * fInvA;

            if (fDet < MKL(0.0))
            {
                TgERROR(!bCapClip);
                return (KTgE_NO_INTERSECT);
            }

            /* Only process the point if the contact is bound by the two cap-planes. */

            if (bCapClip)
            {

                const TYPE                          fT1 = (fPLN0 <= fTA && fTA <= fPLN1) ? fTA : fTB;

                TgERROR( (fPLN0 <= fTA && fTA <= fPLN1) ^ (fPLN0 <= fTB && fTB <= fPLN1) );

                if (fT0 < fT1)
                {
                    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fDS_U0 + fT1*fD0_U0, &psCY0->m.m.vU_Basis0);
                    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fDS_U1 + fT1*fD0_U1, &psCY0->m.m.vU_Basis1);
                    V(C_TgVEC)                          vK2 = V(F_ADD)(&vK0, &vK1);

                    *pfLN0 = fT0;
                    *pfLN1 = fT1;
                    *pvN1 = V(F_NORM)(&vK2);
                }
                else
                {
                    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fDS_U0 + fT1*fD0_U0, &psCY0->m.m.vU_Basis0);
                    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fDS_U1 + fT1*fD0_U1, &psCY0->m.m.vU_Basis1);
                    V(C_TgVEC)                          vK2 = V(F_ADD)(&vK0, &vK1);

                    *pfLN0 = fT1;
                    *pfLN1 = fT0;
                    *pvN1 = *pvN0;
                    *pvN0 = V(F_NORM)(&vK2);
                };
            }
            else
            {
                if ((fPLN0 > fTB || fTB > fPLN1) && (fPLN0 > fTA || fTA > fPLN1))
                {
                    /*return (KTgE_NO_INTERSECT); */
                };

                /*TgERROR( fPLN0 <= fTA && fTA <= fPLN1 && fPLN0 <= fTB && fTB <= fPLN1 ); */

                if (fTA < fTB)
                {
                    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fDS_U0 + fTA*fD0_U0, &psCY0->m.m.vU_Basis0);
                    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fDS_U1 + fTA*fD0_U1, &psCY0->m.m.vU_Basis1);
                    V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fDS_U0 + fTB*fD0_U0, &psCY0->m.m.vU_Basis0);
                    V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fDS_U1 + fTB*fD0_U1, &psCY0->m.m.vU_Basis1);
                    V(C_TgVEC)                          vK4 = V(F_ADD)(&vK0, &vK1);
                    V(C_TgVEC)                          vK5 = V(F_ADD)(&vK2, &vK3);

                    *pfLN0 = fTA;
                    *pfLN1 = fTB;
                    *pvN0 = V(F_NORM)(&vK4);
                    *pvN1 = V(F_NORM)(&vK5);
                }
                else
                {
                    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fDS_U0 + fTB*fD0_U0, &psCY0->m.m.vU_Basis0);
                    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fDS_U1 + fTB*fD0_U1, &psCY0->m.m.vU_Basis1);
                    V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fDS_U0 + fTA*fD0_U0, &psCY0->m.m.vU_Basis0);
                    V(C_TgVEC)                          vK3 = V(F_MUL_SV)(fDS_U1 + fTA*fD0_U1, &psCY0->m.m.vU_Basis1);
                    V(C_TgVEC)                          vK4 = V(F_ADD)(&vK0, &vK1);
                    V(C_TgVEC)                          vK5 = V(F_ADD)(&vK2, &vK3);

                    *pfLN0 = fTB;
                    *pfLN1 = fTA;
                    *pvN0 = V(F_NORM)(&vK4);
                    *pvN1 = V(F_NORM)(&vK5);
                };
            };

            return (KTgS_OK);
        };
    };
}