Home

Resume

Blog

Teikitu


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

/*                                                                                                                                                                        */
/* ---- F_Internal_Sweep ------------------------------------------------------------------------------------------------------------------------------------------------ */
/*                                                                                                                                                                        */
/*  INTERNAL FUNCTION:                                                                                                                                                    */
/*   Used by the cylinder and capsule routines to capture tube-tube contacts. The end-caps are expected to capture most of the contacts, and thus, this routine actively  */
/*  responds with false-negatives when it expects the other routines to have already found the correct contacts.                                                          */
/*                                                                                                                                                                        */
/*  Let α,β represent any two arbitrary tubes, and W.O.L.O.G.let β be the tube undergoing linear translation Φ.                                                           */
/*  Let Sα, Sβ be the origin of the tube axes.                                                                                                                            */
/*  Let Dα, Dβ be the direction of the tube axes.                                                                                                                         */
/*                                                                                                                                                                        */
/*  Let N = Dα x Dβ, and n = N/|N| <-- Invariant under translation.                                                                                                       */
/*                                                                                                                                                                        */
/*  Let ξ = Sα - Sβ                                                                                                                                                       */
/*  Let γ(t) = ξ - Φ*t | t ε [ 0, 1]                                                                                                                                      */
/*                                                                                                                                                                        */
/*  Minimal distance between the two lines is |(γ(t)•n)|                                                                                                                  */
/*                                                                                                                                                                        */
/*  Solve for when distance is equal to the sum of the radii                                                                                                              */
/*                                                                                                                                                                        */
/*  Let RS be the some of the radii of the two tubes.                                                                                                                     */
/*                                                                                                                                                                        */
/*  RS*RS   = (γ(t)•n)*(γ(t)•n)                                                                                                                                           */
/*          = ((ξ - Φ*t)•n)*((ξ - Φ*t)•n)                                                                                                                                 */
/*          = (ξ•n - Φ•n*t)*(ξ•n - Φ•n*t)                                                                                                                                 */
/*          = (ξ•n)*(ξ•n) - 2*(ξ•n)*(Φ•n)*t + (Φ•n)*(Φ•n)*t*t                                                                                                             */
/*                                                                                                                                                                        */
/*  Solve the quadratic equation,                                                                                                                                         */
/*   (Φ•n)*(Φ•n)*t*t - 2*(ξ•n)*(Φ•n)*t + (ξ•n)*(ξ•n) - RS*RS = 0                                                                                                          */
/*                                                                                                                                                                        */
/*  t = (-(-2*(ξ•n)*(Φ•n)) ± √((-2*(ξ•n)*(Φ•n))*(-2*(ξ•n)*(Φ•n)) - 4*(Φ•n)*(Φ•n)*((ξ•n)*(ξ•n) - RS*RS))) / 2*(Φ•n)*(Φ•n)                                                  */
/*    = (2*(ξ•n)*(Φ•n) ± √(4*(ξ•n)*(ξ•n)*(Φ•n)*(Φ•n) - 4*(Φ•n)*(Φ•n)*(ξ•n)*(ξ•n) + 4*(Φ•n)*(Φ•n)*RS*RS)) / 2*(Φ•n)*(Φ•n)                                                  */
/*    = (2*(ξ•n)*(Φ•n) ± 2*(Φ•n)*√((ξ•n)*(ξ•n) - (ξ•n)*(ξ•n) + RS*RS)) / 2*(Φ•n)*(Φ•n)                                                                                    */
/*    = ((ξ•n) ± √(RS*RS))) / (Φ•n)                                                                                                                                       */
/*    = ((ξ•n) ± RS) / (Φ•n)                                                                                                                                              */
/*                                                                                                                                                                        */

/* == Collision ========================================================================================================================================================= */

/* ---- V(tgCO_F_Sweep_TB_TB) ------------------------------------------------------------------------------------------------------------------------------------------- */
/* 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:  psTB0, psTB1: Tube 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_TB_Sweep_TB)( V(PC_STg2_CO_Packet) psPacket, TYPE *pfPM, V(CPC_TgTUBE) psTB0, V(CPC_TgTUBE) psTB1, V(CPC_TgDELTA) psDT )
{
    TYPE                                fTest;
    V(TgVEC)                            vN0;
    V(C_TgVEC)                          vK5 = V(F_CX)(&psTB0->m.m.vU_HAX, &psTB1->m.m.vU_HAX);

    TgPARAM_CHECK( V(tgGM_TB_Is_Valid)(psTB0) && V(tgGM_TB_Is_Valid)(psTB1) );

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

    vN0 = V(F_NORM_LEN)(&fTest, &vK5);

    if (F(tgCM_NR0)(fTest))
    {
        return (KTgE_NO_INTERSECT); /* Lines are near parallel */
    }
    else
    {
        const TYPE                          fSumRad = psTB0->m_fRadius + psTB1->m_fRadius;
        V(C_TgVEC)                          vDS = V(F_SUB)(&psTB0->m.m.vOrigin, &psTB1->m.m.vOrigin);
        const TYPE                          fDS_N0 = V(F_DOT)(&vDS, &vN0);

        if (F(tgPM_ABS)(fDS_N0) < fSumRad)
        {
            V(C_TgVEC)                          vTA = V(F_CX)(&vN0, &vDS);
            const TYPE                          fS0 = V(F_DOT)(&psTB1->m.m.vU_HAX, &vTA);
            const TYPE                          fS1 = V(F_DOT)(&psTB0->m.m.vU_HAX, &vTA);

            if (F(tgPM_ABS)(fS0) >= psTB0->m_fExtent || F(tgPM_ABS)(fS1) >= psTB1->m_fExtent)
            {
                return (KTgE_NO_INTERSECT); /* Proximity point lies outside of the tube's extents. */
            };

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

            *pfPM = MKL(0.0);

            if (psPacket->m_bReport_Penetration)
            {
                vN0 = V(F_MUL_VS)(&vN0, F(tgPM_FSEL)(fDS_N0, MKL(-1.0), MKL(1.0))); /* Direct the normal from TB0 to TB1 */

                if (F(tgCM_NR0)(fDS_N0)) /* Arbitrarily choose an orthonormal direction if penetrated distance to axis. */
                {
                    vN0 = V(F_Set_Ortho)(&psTB1->m.m.vU_HAX);
                    vN0 = V(F_NORM)(&vN0);
                };

                {
                    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fS1, &psTB1->m.m.vU_HAX);
                    V(C_TgVEC)                          vK1 = V(F_MUL_VS)(&vN0, psTB1->m_fRadius);
                    V(C_TgVEC)                          vK2 = V(F_SUB)(&vK0, &vK1);
                    V(P_STg2_CO_Contact)                psContact;

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

                    psContact->m_vS0 = V(F_ADD)(&psTB1->m.m.vOrigin, &vK2);
                    psContact->m_vN0 = vN0;
                    psContact->m_fT0 = MKL(0.0);
                    psContact->m_fDepth = fSumRad - F(tgPM_ABS)(fDS_N0);

                    ++psPacket->m_niContact;
                };
            };

            return (KTgE_PREPENETRATION);
        }
        else
        {
            const TYPE                          fDT_N0 = V(F_DOT)(&psDT->m_vDT, &vN0);
            const TYPE                          fT = (F(tgPM_ABS)(fDS_N0) - fSumRad) / F(tgPM_ABS)(fDT_N0);

            if (F(tgCM_NR0)(fDT_N0) || !((fDS_N0 < MKL(0.0)) ^ (fDT_N0 < MKL(0.0))))
            {
                return (KTgE_NO_INTERSECT); /* Displacement is either near zero, tangential to or directed along the distance vector. */
            };

            if (fT > *pfPM + psPacket->m_fSweepTol)
            {
                return (KTgE_NO_INTERSECT); /* Earlier intersection already recorded. */
            }
            else
            {
                V(P_STg2_CO_Contact)                psContact;

                V(C_TgVEC)                          vK0 = V(F_MUL_VS)(&psDT->m_vDT, fT);
                V(C_TgVEC)                          vK1 = V(F_SUB)(&vDS, &vK0);
                V(C_TgVEC)                          vTA = V(F_CX)(&vN0, &vK1);
                const TYPE                          fS0 = V(F_DOT)(&psTB1->m.m.vU_HAX, &vTA);
                const TYPE                          fS1 = V(F_DOT)(&psTB0->m.m.vU_HAX, &vTA);
                V(C_TgVEC)                          vKN = V(F_MUL_VS)(&vN0, F(tgPM_FSEL)(fDS_N0, MKL(-1.0), MKL(1.0)));
                V(C_TgVEC)                          vK2 = V(F_MUL_SV)(fS1, &psTB1->m.m.vU_HAX);
                V(C_TgVEC)                          vK3 = V(F_MUL_VS)(&vKN, psTB1->m_fRadius);
                V(C_TgVEC)                          vK4 = V(F_SUB)(&vK2, &vK3);

                if (F(tgPM_ABS)(fS0) >= psTB0->m_fExtent || F(tgPM_ABS)(fS1) >= psTB1->m_fExtent)
                {
                    return (KTgE_NO_INTERSECT); /* Proximity point lies outside of the tube's extents. */
                };

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

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

                psContact->m_vS0 = V(F_ADD)(&psTB1->m.m.vOrigin, &vK4);
                psContact->m_vN0 = vKN;
                psContact->m_fT0 = fT;
                psContact->m_fDepth = MKL(0.0);

                ++psPacket->m_niContact;

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