Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Capsule-Capsule.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Capsule;
    »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_FI_Penetrate_Parallel) ----------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psCP0: Capsule primitive                                                                                                                                       */
/* Input:  psCP1: Capsule primitive - contact points are generated on this primitive                                                                                      */
/* Input:  fX1_O0: Projection of capsule axis #2 onto the basis unit #1 of capsule #1                                                                                     */
/* Input:  fX1_O1: Projection of capsule axis #2 onto the basis unit #2 of capsule #1                                                                                     */
/* Output: tgPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_FI_Penetrate_Parallel)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgTUBE) psCP0, V(CPC_TgTUBE) psCP1, const TYPE fX1_O0, const TYPE fX1_O1 )
{
    /* Calculate the normal of penetration. */

    TYPE                                fNM;

    const TYPE                          fRS = psCP1->m_fRadius + psCP0->m_fRadius;
    V(C_TgVEC)                          vDS = V(F_SUB)(&psCP1->m.m.vOrigin, &psCP0->m.m.vOrigin);
    const TYPE                          fN0 = V(F_DOT)(&vDS, &psCP1->m.m.vU_Basis0);
    const TYPE                          fN1 = V(F_DOT)(&vDS, &psCP1->m.m.vU_Basis1);
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fN0, &psCP1->m.m.vU_Basis0);
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fN1, &psCP1->m.m.vU_Basis1);
    V(C_TgVEC)                          vK2 = V(F_ADD)(&vK0, &vK1);
    V(C_TgVEC)                          vNM = V(F_NORM_LEN)(&fNM, &vK2);

    /* Check to make sure the two capsules are within range of each other on their mutually defined cross-sectional plane. */

    if (fNM > fRS)
    {
        return (KTgE_NO_INTERSECT);
    }
    else
    {
        /* Find the overlap range of the two capsule axes. */

        const TYPE                          fMin = F(tgCM_MAX)(fX1_O1 - psCP1->m_fExtent, fX1_O0 - psCP0->m_fExtent);
        const TYPE                          fMax = F(tgCM_MIN)(fX1_O1 + psCP1->m_fExtent, fX1_O0 + psCP0->m_fExtent);

        /* Determine the first (possibly only) point of contact.  If the difference between the minimum and the maximum on the range of overlap is smaller than the */
        /* minimal radii, create only one point.  This is the prevent creating two points within too close of a range of each other.  In this case, create the point at */
        /* the median of the range. */

        const TYPE                          fRad = F(tgCM_MIN)(psCP1->m_fRadius, psCP0->m_fRadius);
        C_TgBOOL                            bOnePoint = (fMax - fMin) <= MKL(2.0) * fRad;
        const TYPE                          fK0 = (bOnePoint ? MKL(0.5)*(fMax + fMin) : fMin) - fX1_O1;
        V(C_TgVEC)                          vK3 = V(F_MUL_VS)(&vNM, psCP1->m_fRadius);
        V(C_TgVEC)                          vK4 = V(F_SUB)(&psCP1->m.m.vOrigin, &vK3);
        V(C_TgVEC)                          vK5 = V(F_MUL_SV)(fK0, &psCP1->m_vHAX);

        V(P_STg2_CO_Contact)                psContact;

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

        psContact->m_vS0 = V(F_ADD)(&vK4, &vK5);
        psContact->m_vN0 = vNM;
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = fRS - fNM;

        ++psPacket->m_niContact;

        if (!bOnePoint)
        {
            /* Create the second point at the maximal location on the overlap range. */

            V(C_TgVEC)                          vK6 = V(F_MUL_SV)(fMax - fX1_O1, &psCP1->m_vHAX);

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

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

            psContact->m_vS0 = V(F_ADD)(&vK4, &vK6);
            psContact->m_vN0 = vNM;
            psContact->m_fT0 = MKL(0.0);
            psContact->m_fDepth = fRS - fNM;

            ++psPacket->m_niContact;
        };

        return (KTgS_OK);
    };
}


/* ---- V(tgCO_FI_Penetrate_NonParallel) -------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psCP0: Capsule primitive                                                                                                                                       */
/* Input:  psCP1: Capsule primitive - contact points are generated on this primitive                                                                                      */
/* Input:  vNxN: The cross product of the two capsule axis                                                                                                                */
/* Output: tgPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_FI_Penetrate_NonParallel)( V(PC_STg2_CO_Packet) psPacket, V(CPC_TgTUBE) psCP0, V(CPC_TgTUBE) psCP1, V(CPC_TgVEC) pvNxN )
{
    const TYPE                          fRS = psCP1->m_fRadius + psCP0->m_fRadius;
    V(TgVEC)                            vCP0, vCP1;

    const TYPE                          fDistSq = V(tgCO_F_SG_ClosestSq_SG)(&vCP0, &vCP1, &psCP1->m_sAX, &psCP0->m_sAX);

    if (fDistSq > fRS*fRS)
    {
        return (KTgE_NO_INTERSECT);
    }
    else
    {
        V(C_TgVEC)                          vK0 = V(F_SUB)(&vCP0, &vCP1);
        TYPE                                fNM;
        V(C_TgVEC)                          vNM = V(F_NORM_LEN)(&fNM, &vK0);
        C_TgBOOL                            bUseNormal = fNM > F(KTgEPS);
        V(P_STg2_CO_Contact)                psContact;
        V(C_TgVEC)                          vK1 = V(F_MUL_VS)(bUseNormal ? &vNM : pvNxN, psCP1->m_fRadius);

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

        psContact->m_vS0 = V(F_SUB)(&vCP0, &vK1);
        psContact->m_vN0 = bUseNormal ? vNM : *pvNxN;
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = fRS - fNM;

        ++psPacket->m_niContact;

        return (KTgS_OK);
    }
}


/* ---- V(tgCO_F_CP_Test_Sweep_CP) -------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psCP0,psCP1: Capsule primitive                                                                                                                                 */
/* Input:  vUDT: Normalized direction of displacement for the swept primitive (capsule)                                                                                   */
/* Input:  fDT: Length of displacement for the swept primitive                                                                                                            */
/* Return: True if the two primitives are in contact at anytime during the sweep                                                                                          */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgBOOL V(tgCO_F_CP_Test_Sweep_CP)( V(CPC_TgTUBE) psCP0, V(CPC_TgTUBE) psCP1, V(CPC_TgVEC) pvUDT, const TYPE fDT )
{
    V(C_TgVEC)                          vC0O = psCP0->m_sAX.m_vOrigin;
    V(C_TgVEC)                          vC0A = psCP0->m_sAX.m_vDirN;
    V(C_TgVEC)                          vC1O = psCP1->m_sAX.m_vOrigin;
    V(C_TgVEC)                          vC1A = psCP1->m_sAX.m_vDirN;
    V(C_TgVEC)                          vDT = V(F_MUL_SV)(fDT, pvUDT);

    const TYPE                          fRS = psCP0->m_fRadius + psCP1->m_fRadius;
    const TYPE                          fRSSq = fRS*fRS;
    const TYPE                          fUDT_CA = V(F_DOT)(pvUDT, &psCP1->m.m.vU_HAX);

    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psCP0) && V(tgGM_TB_Is_Valid)(psCP1) );

    /* Delta vector is parallel to the capsule axis. */

    if (F(tgCM_NR1)( fUDT_CA ))
    {
        V(C_TgVEC)                          vK0 = V(F_ADD)(&vC1A, &vDT);
        TYPE                                fK0, fK1;

        return (V(tgCO_F_LR11_ParamSq_LR11)(&fK0, &fK1, &vC0O, &vC0A, &vC1O, &vK0) < fRSSq);
    };

    if (F(tgCM_NR0)(fUDT_CA + MKL(1.0)))
    {
        V(C_TgVEC)                          vK0 = V(F_ADD)(&vC1O, &vDT);
        V(C_TgVEC)                          vK1 = V(F_SUB)(&vC1A, &vDT);
        TYPE                                fK0, fK1;

        return (V(tgCO_F_LR11_ParamSq_LR11)(&fK0, &fK1, &vC0O, &vC0A, &vK0, &vK1) < fRSSq);
    }
    else
    {
        TYPE                                fT0, fT1, fT2;
        V(TgPARALLELOGRAM)                  sSweptCapsule;
        V(C_TgVEC)                          vNM = V(F_UCX)( &vC1A, &vDT );

        V(tgGM_PE_Init)(&sSweptCapsule, &vC1O, &vC1A, &vDT, &vNM);

        return (V(tgCO_F_PE_ParamSq_LR11)(&fT0, &fT1, &fT2, &sSweptCapsule, &vC0O, &vC0A) < fRSSq);
    }
}