Home

Resume

Blog

Teikitu


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

/* ---- V(tgCO_F_ST_Sweep_SP) ------------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  fPM: Current normalized time of first contact.                                                                                                                 */
/* Input:  bPenetrate: If the swept primitives are in penetration, if true the function will return points of penetration.                                                */
/* Input:  psST0: Space Triangle primitive                                                                                                                                */
/* Input:  psSP0: Sphere primitive                                                                                                                                        */
/* Input:  psDT: A structure holding the swept primitive displacement for the entire duration of the test period                                                          */
/* Output: tgPacket: Contact points are added or replace the current set depending on the time comparison and given parameters                                            */
/* Output: fPM: New normalized time of first contact                                                                                                                      */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_ST_Sweep_SP)(V(PC_STg2_CO_Packet) psPacket, TYPE *pfPM, V(CPC_TgSTRI) psST0, V(CPC_TgSPHERE) psSP0, V(CPC_TgDELTA) psDT)
{
    TgPARAM_CHECK( V(tgGM_SP_Is_Valid)(psSP0) && V(tgGM_ST_Is_Valid)(psST0) );

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
    {
        return (KTgE_FAIL);
    }
    else
    {
        /* Check the displacement vector - if it is directed away from the triangle intersection is not possible. */

        V(C_TgVEC)                          vX0 = V(F_SUB)(&psSP0->m_vOrigin, psST0->m_sCT.m_sET.m_sPT.m_avPoint);
        const TYPE                          fX0_N = V(F_DOT)(&vX0, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
        const TYPE                          fDT_N = V(F_DOT)(&psDT->m_vDT, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);
        C_TgBOOL                            bPenetrate = TgTRUE == psPacket->m_bReport_Penetration;
        const TYPE                          fT = (psSP0->m_fRadius - fX0_N) / fDT_N;
        C_TgSINT32                          niContact = psPacket->m_niContact;

        TgRESULT                            iResult;

        /*TgDEBUG_COLLISION_TRIANGLE_CREATEID( iDBG_TriID, psST0, etgDEBUG_COLLISION_ENTERFCN ); */

        if (fX0_N > psSP0->m_fRadius && fDT_N > F(KTgEPS))
        {
            return (KTgE_NO_INTERSECT);
        };

        if (fX0_N < -psSP0->m_fRadius && fDT_N < -F(KTgEPS))
        {
            return (KTgE_NO_INTERSECT);
        };

        /* Calculate the extrapolation required to translate the distance from the origin to the triangle plane. */

        if (fT > *pfPM + psPacket->m_fSweepTol || fT > MKL(1.0))
        {
            if (bPenetrate)
            {
                goto SPHERE_PENETRATION;
            };

            return (KTgE_NO_INTERSECT);
        };

        /* Examine the resulting point on the plane to see if it is contained by the triangle. */

        if (fT >= MKL(0.0))
        {
            V(P_STg2_CO_Contact)                psContact;
            TgSINT32                            iEdge;

            V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fT, &psDT->m_vDT);
            V(C_TgVEC)                          vK1 = V(F_ADD)(&psSP0->m_vOrigin, &vK0);
            V(C_TgVEC)                          vK2 = V(F_MUL_SV)(psSP0->m_fRadius, &psST0->m_sCT.m_sET.m_sPT.m_vNormal);

            for (iEdge = 0; iEdge < 3; ++iEdge)
            {
                const TYPE                          fLimit = F(KTgEPS)*V(F_LEN)(psST0->m_sCT.m_sET.m_avEdge + iEdge);
                const TYPE                          fDist = V(tgCO_F_PN_Dist_VT)(psST0->m_avPlane + iEdge, &vK1);

                if (fDist < -fLimit)
                {
                    /* The sphere origin is behind the triangle plane.  Check to see if the delta moves away from the plane. */

                    if (!V(tgGM_ST_Test_Edge)(psST0, iEdge)
                        && V(F_DOT)(&psST0->m_avPlane[iEdge].m_vNormal, &psDT->m_vDT) < MKL(0.0))
                    {
                        return (KTgE_NO_INTERSECT);
                    };

                    /* The sphere does not intersect the triangle plane - check the edge/vertices for intersection. */

                    goto OutsideTriangle;
                }
                else if (fDist <= fLimit)
                {
                    /* The contact point is within tolerance of a specific triangle's edge.  Check to see if its been marked invalid. */

                    if (!V(tgGM_ST_Test_Edge)(psST0, iEdge) && V(tgGM_ET_Is_Tri_Edge_Ignored_Code)(&psST0->m_sCT.m_sET, iEdge))
                    {
                        return (KTgE_NO_INTERSECT);
                    };
                };
            };

            /* Valid point of contact. */

            if (fT < *pfPM - psPacket->m_fSweepTol)
            {
                psPacket->m_niContact = 0;
                *pfPM = fT;
            };

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

            psContact->m_vS0 = V(F_SUB)(&vK1, &vK2);
            psContact->m_vN0 = psST0->m_sCT.m_sET.m_sPT.m_vNormal;
            psContact->m_fT0 = fT;
            psContact->m_fDepth = MKL(0.0);

            ++psPacket->m_niContact;

            /*TgDEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_CODE1 ); */

            return (KTgS_OK);
        };

    OutsideTriangle:

        {   /* State Block for Edge Tests - Required because of the goto statement used for the penetration check */

            V(C_TgVEC)                          vK0 = V(F_SUB)(&psDT->m_vDT, psST0->m_sCT.m_sET.m_sPT.m_avPoint);
            V(C_TgVEC)                          vX1 = V(F_ADD)(&psSP0->m_vOrigin, &vK0);
            TgBOOL                              bHit = TgFALSE;
            TgSINT32                            iEdge, iPoint;

            /* Check for collisions against the three edges */
            for (iEdge = 0; iEdge < 3; ++iEdge)
            {
                if (V(tgGM_ST_Test_Edge)(psST0, iEdge) || !V(tgGM_ET_Is_Tri_Edge_Ignored_Code)(&psST0->m_sCT.m_sET, iEdge))
                {
                    const TYPE                          fLimit = -psSP0->m_fRadius*V(F_LEN)(psST0->m_sCT.m_sET.m_avEdge + iEdge);
                    const TYPE                          fT0 = V(F_DOT)(&psST0->m_avPlane[iEdge].m_vNormal, &vX0);
                    const TYPE                          fT1 = V(F_DOT)(&psST0->m_avPlane[iEdge].m_vNormal, &vX1);

                    if (fT0 < fLimit && fT1 < fLimit)
                    {
                        return (KTgE_NO_INTERSECT);
                    };

                    if (fT0 < F(KTgEPS) || fT1 < F(KTgEPS))
                    {
                        bHit |= 0 <= V(tgCO_FI_SP_Sweep_LR11)(
                            psPacket, pfPM, psST0->m_sCT.m_sET.m_sPT.m_avPoint + iEdge, psST0->m_sCT.m_sET.m_avEdge + iEdge,
                            psSP0, psDT);
                    };
                };
            };

            /* Check for collisions against the three vertices */

            for (iPoint = 0; iPoint < 3; ++iPoint)
            {
                if (V(tgGM_ST_Test_Point)(psST0, iPoint))
                {
                    bHit |= TgSUCCEEDED( V(tgCO_F_VT_Sweep_SP)(
                        psPacket, pfPM, psST0->m_sCT.m_sET.m_sPT.m_avPoint + iPoint, psSP0, psDT) );
                };
            };

            /*TgDEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_CODE2 ); */

            if (bHit)
            {
                return (KTgS_OK);
            };
        };

        /* If pre-penetration information is not requested or the sphere's origin is behind the triangle report a non-intersection. */

        if (!bPenetrate)
        {
            return (KTgE_NO_INTERSECT);
        };

    SPHERE_PENETRATION:

        /*TgDEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_PREPENETRATION ); */

        /* Record the current number of contacts before potentially clearing the list in case pre-penetration is not found. */

        if (*pfPM > psPacket->m_fSweepTol)
        {
            psPacket->m_niContact = 0;
        };

        iResult = V(tgCO_FI_ST_Penetrate_SP)(psPacket, psST0, psSP0);

        /* Restore the original number of contacts if pre-penetration was not found. */

        if (TgFAILED( iResult ))
        {
            psPacket->m_niContact = niContact;
            return (iResult);
        };

        /* Set the time parameter and return back the appropriate result code. */

        *pfPM = MKL(0.0);

        return (KTgE_MAX_CONTACTS == iResult ? KTgE_MAX_CONTACTS : (KTgS_OK == iResult ? KTgE_PREPENETRATION : iResult));
    };
}


/* ---- V(tgCO_FI_ST_Penetrate_SP) -------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psCT0: Collision Triangle primitive                                                                                                                            */
/* Input:  psSP0: Sphere primitive                                                                                                                                        */
/* Output: tgPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_FI_ST_Penetrate_SP)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgSTRI) psST0, V(CPC_TgSPHERE) psSP0)
{
    /*TgDEBUG_COLLISION_TRIANGLE_CREATEID( iDBG_TriID, psCT0, etgDEBUG_COLLISION_ENTERFCN ); */

    TYPE                                fCT0, fCT1;

    const TYPE                          fDistSq = V(tgCO_F_ET_ParamSq_VT)(&fCT0, &fCT1, &psST0->m_sCT.m_sET, &psSP0->m_vOrigin);

    /* Check to see if the proposed point of contact is on a reduced triangle feature. */

    C_TgBOOL                            bCulled = V(tgGM_ST_Is_Point_Culled)(psST0, fCT0, fCT1);

    /* Check to see if the reduced feature should be included for this case (only important if bCulled is true). */

    C_TgBOOL                            bEdge = V(tgGM_ET_Is_Tri_Edge_Ignored)(&psST0->m_sCT.m_sET, fCT0, fCT1);

    /*  No penetration if the minimal distance between the sphere origin and the triangle is greater than the sphere's radius. Also, if the point is on a reduced */
    /* feature, the contact can be ignored.  The exception is for an edge, specifically for a sphere ( limit of one contact point ) it is necessary to make sure that a */
    /* contact on the edge is not ignored at least once to prevent the system from culling out all contact points between a sphere and a triangle list. */

    TgERROR( !(0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact) );
    TgERROR( !(0 == psPacket->m_psContact) );

    if (fDistSq > psSP0->m_fRadiusSq || (bCulled && bEdge))
    {
        return (KTgE_NO_INTERSECT);
    }
    else
    {
        /*TgDEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_PASSED_REJECT ); */

        V(TgVEC)                            vCT0, vNormal;
        TYPE                                fDist, fDepth;

        V(tgGM_ST_Calc_Point)( &vCT0, psST0, fCT0, fCT1 );
        fDist = F(tgPM_SQRT)( fDistSq );
        fDepth = fDist >= psSP0->m_fRadius ? MKL(0.0) : psSP0->m_fRadius - fDist;
        vNormal = V(F_SUB)( &psSP0->m_vOrigin, &vCT0 );

        /*  Check to see if the normal of intersection should be replaced by the triangle's normal.  This is done to reduce floating point noise in the system where */
        /* near-normal results are returned.  By forcing it to the triangle's normal, extraneous rotations are minimized. The other possibility is that the sphere's */
        /* origin lies on the triangle itself, thus, requiring the selection of the triangle's normal for the intersection. */

        TgBOOL                              bUseNormal = !F(tgCM_NR0)(fDist);
        V(P_STg2_CO_Contact)                psContact;

        if (bUseNormal)
        {
            vNormal = V(F_NORM)(&vNormal);

            /*  Check to see if the resultant normal is near that of the triangle's.  If they are close then use the triangle's normal to help further reduce floating */
            /* point noise. */

            bUseNormal = F(tgCM_NR1)(V(F_DOT)(&vNormal, &psST0->m_sCT.m_sET.m_sPT.m_vNormal));
        };

        if (bCulled && bUseNormal)
        {
            /* If the point was marked to be culled (even if its on a valid edge), and the contact normal does not match the triangle's normal, ignore this contact.  Its */
            /* assumed that the matching triangle, attached to this one, will create the required contact point.  Prevents duplicate points from being created. */

            return (KTgE_NO_INTERSECT);
        }
        else
        {
            V(C_TgVEC)                          vKN = bUseNormal ? vNormal : psST0->m_sCT.m_sET.m_sPT.m_vNormal;
            V(C_TgVEC)                          vK1 = V(F_MUL_SV)(psSP0->m_fRadius, &vKN);

            /* Create contact point. */

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

            psContact->m_vN0 = vKN;
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = fDepth;
            psContact->m_vS0 = V(F_SUB)(&psSP0->m_vOrigin, &vK1);

            ++psPacket->m_niContact;

            /*TgDEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_CODE1 ); */

            return (KTgS_OK);
        };
    };
}