Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Triangle-Particle.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Triangle;Particle;
    »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 TgRESULT                             V(tgCO_F_PC_Internal_Sweep_ET)( V(PC_STg2_CO_Contact), V(CPC_TgPARTICLE), V(CPC_TgVEC), V(CPC_TgETRI) );




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

/* ---- V(tgCO_F_PC_Sweep_ET) ------------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket:  Contact generation parameters                                                                                                                       */
/* Input:  tyPM: Current normalized time of first contact for the contact query set.                                                                                      */
/* Input:  psET0: Edge triangle primitive - also undergoing a linear translation                                                                                          */
/* Input:  psPC0: Particle - this primitive is undergoing the sweep/translation.                                                                                          */
/* 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: tyPM: New normalized time of first contact                                                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_PC_Sweep_ET)( V(PC_STg2_CO_Packet) psPacket, TYPE *pfPM, V(CPC_TgPARTICLE) psPC0, V(CPC_TgETRI) psET0, V(CPC_TgDELTA) psDT )
{
    V(STg2_CO_Contact)                  sContact;
    V(P_STg2_CO_Contact)                psContact;
    TgRESULT                            iResult;

    V(C_TgVEC)                          vK0 = V(F_SUB)(&psPC0->m_vVel, &psDT->m_vDT);

    TgPARAM_CHECK( V(tgGM_ET_Is_Valid)(psET0) && V(tgGM_PC_Is_Valid)(psPC0) );

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

    iResult = V(tgCO_F_PC_Internal_Sweep_ET)(&sContact, psPC0, &vK0, psET0);

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

    if (sContact.m_fT0 > *pfPM + psPacket->m_fSweepTol || !V(tgGM_ET_Is_Contained)(psET0, &sContact.m_vS0))
    {
        return (KTgE_NO_INTERSECT);
    };

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

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

    psContact->m_vS0 = sContact.m_vS0;
    psContact->m_vN0 = sContact.m_vN0;
    psContact->m_fT0 = sContact.m_fT0;
    psContact->m_fDepth = sContact.m_fDepth;

    ++psPacket->m_niContact;

    return (KTgS_OK);
}


/* ---- V(tgCO_F_ET_Sweep_PC) ------------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket:  Contact generation parameters                                                                                                                       */
/* Input:  tyPM: Current normalized time of first contact for the contact query set.                                                                                      */
/* Input:  psET0: Edge triangle primitive                                                                                                                                 */
/* Input:  psPC0: Particle - this primitive is undergoing the sweep/translation.                                                                                          */
/* Output: tgPacket: Contact points are added or replace the current set depending on the time comparison and given parameters                                            */
/* Output: tyPM: New normalized time of first contact                                                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_ET_Sweep_PC)( V(PC_STg2_CO_Packet) psPacket, TYPE *pfPM, V(CPC_TgETRI) psET0, V(CPC_TgPARTICLE) psPC0, V(CPC_TgDELTA) UNUSED_PARAM psDT )
{
    V(STg2_CO_Contact)                  sContact;
    V(P_STg2_CO_Contact)                psContact;
    TgRESULT                            iResult;

    TgPARAM_CHECK( V(tgGM_ET_Is_Valid)(psET0) && V(tgGM_PC_Is_Valid)(psPC0) );

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

    iResult = V(tgCO_F_PC_Internal_Sweep_ET)(&sContact, psPC0, &psPC0->m_vVel, psET0);

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

    if (sContact.m_fT0 > *pfPM + psPacket->m_fSweepTol || !V(tgGM_ET_Is_Contained)(psET0, &sContact.m_vS0))
    {
        return (KTgE_NO_INTERSECT);
    };

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

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

    psContact->m_vS0 = sContact.m_vS0;
    psContact->m_vN0 = sContact.m_vN0;
    psContact->m_fT0 = sContact.m_fT0;
    psContact->m_fDepth = sContact.m_fDepth;

    ++psPacket->m_niContact;

    return (KTgS_OK);
}




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

/* ---- V(tgCO_F_PC_Internal_Sweep_ET) ---------------------------------------------------------------------------------------------------------------------------------- */
/*                                                                                                                                                                        */
/*   The culling situation for a particle is complicated by its parabolic path of motion.  Back face culling is done on a point by                                        */
/*  point basis.  Thus, it is possible for a particle to ignore a triangle as it comes up through the back face and then collide                                          */
/*  with it as it descends through the front face.                                                                                                                        */
/*                                                                                                                                                                        */
/* Input:  psET0: Edge triangle primitive - also undergoing a linear translation                                                                                          */
/* Input:  psPC0: Particle - this primitive is undergoing the sweep/translation.                                                                                          */
/* Input:  vRV:  Relative velocity of the particle                                                                                                                        */
/* Output: sContact: Contact point if one is registered for the time period.                                                                                              */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgRESULT V(tgCO_F_PC_Internal_Sweep_ET)( V(PC_STg2_CO_Contact) psContact, V(CPC_TgPARTICLE) psPC0, V(CPC_TgVEC) pvRV, V(CPC_TgETRI) psET0 )
{
    TYPE                                fT0;

    V(C_TgVEC)                          vK0 = V(F_SUB)(&psPC0->m_vPos, psET0->m_sPT.m_avPoint);
    const TYPE                          fDist = V(F_DOT)(&psET0->m_sPT.m_vNormal, &vK0);
    const TYPE                          fA_N = V(F_DOT)(&psET0->m_sPT.m_vNormal, &psPC0->m_vAccel);
    const TYPE                          fRV_N = V(F_DOT) (&psET0->m_sPT.m_vNormal, pvRV);

    /* Check to see if the particle is moving away from the triangle plane. */

    if (fDist > MKL(0.0) && fRV_N >= MKL(0.0) && fA_N >= MKL(0.0))
    {
        return (KTgE_NO_INTERSECT);
    };

    if (fDist < MKL(0.0) && fRV_N <= MKL(0.0) && fA_N <= MKL(0.0))
    {
        return (KTgE_NO_INTERSECT);
    };

    if (F(tgCM_NR0)(fA_N))
    {
        /*  The particle's acceleration is completely parallel to the triangle plane.  In this case, intersection can only exist if there is a velocity component towards */
        /* the triangle. In that case, the equation is only first order (linear). If the velocity's projection onto the triangle normal is positive then the particle can */
        /* only intersect with the back face of the triangle - a case which is ignored. */

        if (fRV_N >= MKL(0.0) || F(tgCM_NR0)(fRV_N))
        {
            return (KTgE_NO_INTERSECT);
        };

        fT0 = -fDist / fRV_N;

        if (fT0 < MKL(0.0))
        {
            /* Sanity check - this can occur because floating point error and epsilon testing. */

            return (KTgE_NO_INTERSECT);
        };
    }
    else
    {
        /*  The particle has an acceleration component towards the triangle plane.  The equation of motion is a quadratic and thus, will have two solutions.  The desired */
        /* answer will be the first positive root. */

        const TYPE                          fDSC = fRV_N*fRV_N - MKL(2.0)*fA_N*fDist;

        if (fDSC < MKL(0.0))
        {
            /*  There is no real result for the given discriminant.  This should never occur given the other logic before this point.  Register an error and return a */
            /* non-intersection. */

            TgWARN_CO(TgT("[PA][ET] Unexpected invalid discriminant in calculation.\n" ));
            return (KTgE_NO_INTERSECT);
        };

        /*  The math dictates that only one solution can possibly satisfy the constraints on the solution.  Specifically, the requirement that the velocity of the */
        /* particle at the time of intersection have a negative projection on the triangle's normal eliminates the second root of the solution set. */
        /*  N_(r + t•a,DIM) < 0.0, t = (-(r•N) ± β) / (a•N) */
        /*  N•r + t_(N•a,DIM) < 0.0 */
        /*  N•r + ((-(r•N) ± β) / (a•N))T_(N•a,DIM) < 0.0 */
        /*  N•r + (-(r•N) ± β) < 0.0 */
        /*  ± β < 0.0 */
        /*  β is known to always be positive since the solution space is restricted to the real plane.  The root constructed with the positive square root value can */
        /* never satisfy the velocity requirement, and thus, can always be ignored. */

        {
            const TYPE                          fSQRT_DSC = F(tgPM_SQRT)(fDSC);

            /* Check to see if the derived value for T0 would be negative, and if so, return with no intersection. */

            if ((fA_N > MKL(0.0) && -fRV_N < fSQRT_DSC) || (fA_N < MKL(0.0) && -fRV_N > fSQRT_DSC))
            {
                return (KTgE_NO_INTERSECT);
            };

            fT0 = (-fRV_N - fSQRT_DSC) / fA_N;
        };
    };

    /* Compute the point on the triangle plane and then test to see if its contained within the triangle itself. */

    {
        V(C_TgVEC)                          vK2 = V(F_MUL_VS)(pvRV, fT0);
        V(C_TgVEC)                          vK1 = V(F_ADD)(&psPC0->m_vPos, &vK2);
        V(C_TgVEC)                          vK3 = V(F_MUL_VS)(&psPC0->m_vAccel, fT0*fT0);

        psContact->m_vS0 = V(F_ADD)(&vK1, &vK3);
        psContact->m_vN0 = psET0->m_sPT.m_vNormal;
        psContact->m_fT0 = fT0;
        psContact->m_fDepth = MKL(0.0);

        return (KTgS_OK);
    };
}