Home

Resume

Blog

Teikitu


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

/* Let S0 and S1 represent two spheres, with radius R0, R1, centers C0,C1 and velocities V0, V1 respectively.                                                             */
/* W.O.L.O.G. Perform the following calculations within S0 frame of reference.                                                                                            */
/* Assume the frame of reference in non-rotational (only translation).                                                                                                    */
/*                                                                                                                                                                        */
/* SWEEP TEST                                                                                                                                                             */
/*                                                                                                                                                                        */
/* As with all sweep tests, an arbitrary body is chosen as the principal reference frame so that only the relative                                                        */
/* motion of one of the bodies is relevant.  The solution is as follows,                                                                                                  */
/*  (1) Create a line (RV) using the centre of the second sphere and the vector of relative motion.                                                                       */
/*  (2) Form a right-sided triangle.                                                                                                                                      */
/*      (a) The hypotenuse is the sum of the sphere radii.                                                                                                                */
/*      (b) The known side is the line from the centre of the first sphere to the closest point on line RV.                                                               */
/*      (c) The length of the remaining side (along RV) is the distance from the point of closest proximity and the                                                       */
/*          point of first contact between the two spheres.                                                                                                               */
/*                                                                                                                                                                        */
/*                              .(C0)                                                                                                                                     */
/*                             /|\                                                                                                                                        */
/*                            / | \                                                                                                                                       */
/*                           /  |  \                                                                                                                                      */
/*                          /   |   \                                                                                                                                     */
/*                         /    |    \                                                                                                                                    */
/*                        /(R0) |     \(R0)                                                                                                                               */
/*          (R1)/    (R1)/      |                                                                                                                                         */
/*             /        /       |                                                                                                                                         */
/*            /        /        |                                                                                                                                         */
/*           .--------.---------.--------------> (RV)                                                                                                                     */
/*          (C1)     (P1)      (P0)                                                                                                                                       */
/*                                                                                                                                                                        */
/* Let RV = V1 - V0, the relative velocity of S1 in the frame of reference of S0                                                                                          */
/* Let RV_N be the normalized RV vector, and RV_L the length of the original RV vector                                                                                    */
/* Let DS = C1 - C0, the vector joining the two sphere centers                                                                                                            */
/* Let RS = R0+R1, the sum of the radii of the two spheres                                                                                                                */
/*                                                                                                                                                                        */
/* Test 1: Pre penetration - The spheres are in contact if DS•DS < RS•RS                                                                                                  */
/* Test 2: Movement - RV_L ~= 0, no contact takes place                                                                                                                   */
/* Test 3: Parallel - ψ = DS•RV_N                                                                                                                                         */
/*   Contact can not occur under the following conditions:                                                                                                                */
/*     [ψ > 0]          - Second sphere is moving away from the first sphere                                                                                              */
/*     [ψ < -(RV_L+RS)] - Motion is insufficient for them to come within proximity                                                                                        */
/* Test 4: Perpendicular - φ = DS - ψ•RV_N                                                                                                                                */
/*   Contact can not occur under the following conditions:                                                                                                                */
/*     [φ•φ > RS•RS] - The sphere path does not bring them within proximity                                                                                               */
/* Result: Υ = RS•RS - φ•φ, ζ = ψ + √Υ                                                                                                                                    */
/*   [ζ < RV_L] - The two sphere's contact at (ζ / RV_L) of the complete path of motion.                                                                                  */


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

/* ---- V(tgCO_F_SP_Closest_SP) ----------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  psSP0,psSP1: Sphere primitives                                                                                                                                 */
/* Output: vSP0,vSP1: Point of closest proximity on the sphere #1 and #2 respectively                                                                                     */
/* Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.                                                             */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE V(tgCO_F_SP_Closest_SP)(V(PC_TgVEC) pvSP0, V(PC_TgVEC) pvSP1, V(CPC_TgSPHERE) psSP0, V(CPC_TgSPHERE) psSP1)
{
    TYPE                                fDS;
    V(C_TgVEC)                          vDS = V(F_SUB)(&psSP1->m_vOrigin, &psSP0->m_vOrigin);
    V(C_TgVEC)                          vDN = V(F_NORM_LEN)(&fDS, &vDS);
    const TYPE                          fSumRad = psSP0->m_fRadius + psSP1->m_fRadius;

    TgERROR( V(tgGM_SP_Is_Valid)(psSP0) && V(tgGM_SP_Is_Valid)(psSP1) );

    if (fDS <= fSumRad)
    {
        return (-F(KTgMAX));
    }
    else
    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(psSP0->m_fRadius, &vDN);
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)(psSP1->m_fRadius, &vDN);

        *pvSP0 = V(F_ADD)(&psSP0->m_vOrigin, &vK0);
        *pvSP1 = V(F_SUB)(&psSP1->m_vOrigin, &vK1);

        return (fDS - fSumRad);
    };
}


/* ---- V(tgCO_F_SP_Penetrate_SP) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  tgPacket: The current series of contact points for this query-series, and contact generation parameters.                                                       */
/* Input:  psSP0,psSP1: Sphere primitives - contact points are generated on sphere #2 (psSP1)                                                                             */
/* Output: tgPacket: Points of penetration between the two primitives are added to it                                                                                     */
/* Return: Result Code                                                                                                                                                    */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_SP_Penetrate_SP)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgSPHERE) psSP0, V(CPC_TgSPHERE) psSP1)
{
    TgPARAM_CHECK( V(tgGM_SP_Is_Valid)(psSP1) && V(tgGM_SP_Is_Valid)(psSP0) );

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
    {
        return (KTgE_FAIL);
    }
    else
    {
        TYPE                                fDS;
        V(C_TgVEC)                          vDS = V(F_SUB)(&psSP1->m_vOrigin, &psSP0->m_vOrigin);
        V(C_TgVEC)                          vDN = V(F_NORM_LEN)(&fDS, &vDS);
        const TYPE                          fSumRad = psSP1->m_fRadius + psSP0->m_fRadius;
        V(P_STg2_CO_Contact)                psContact;

        if (fDS > fSumRad)
        {
            return (KTgE_NO_INTERSECT);
        };

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

        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = fSumRad - fDS;

        if (fDS <= F(KTgEPS))
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_VS)(&V(KTgV_UNIT_Y), psSP1->m_fRadius);

            psContact->m_vS0 = V(F_SUB)(&psSP1->m_vOrigin, &vK0);
            psContact->m_vN0 = V(KTgV_UNIT_Y);
        }
        else
        {
            V(C_TgVEC)                          vK0 = V(F_MUL_VS)(&vDN, psSP1->m_fRadius);

            psContact->m_vS0 = V(F_SUB)(&psSP1->m_vOrigin, &vK0);
            psContact->m_vN0 = vDN;
        };

        ++psPacket->m_niContact;
        return (KTgS_OK);
    };
}


/* ---- V(tgCO_F_SP_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:  psSP0,psSP1: Sphere primitives                                                                                                                                 */
/* 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_SP_Sweep_SP)(V(PC_STg2_CO_Packet) psPacket, TYPE *pfPM, V(CPC_TgSPHERE) psSP0, V(CPC_TgSPHERE) psSP1, V(CPC_TgDELTA) psDT)
{
    TgERROR( V(tgGM_SP_Is_Valid)(psSP1) && V(tgGM_SP_Is_Valid)(psSP0) );

    if (0 == psPacket->m_niMaxContact || psPacket->m_niContact >= psPacket->m_niMaxContact || nullptr == psPacket->m_psContact)
    {
        return (KTgE_FAIL);
    }
    else
    {
        /* Check for pre-penetration. */

        const TYPE                          fRS = psSP1->m_fRadius + psSP0->m_fRadius;
        const TYPE                          fRS2 = fRS*fRS;
        V(C_TgVEC)                          vDS = V(F_SUB)(&psSP0->m_vOrigin, &psSP1->m_vOrigin);
        const TYPE                          fDS_DS = V(F_LSQ)(&vDS);

        if (fDS_DS <= fRS2)
        {   /* Pre-Penetration. */
            C_TgBOOL                            bPenetrate = TgTRUE == psPacket->m_bReport_Penetration;

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

            *pfPM = MKL(0.0);

            if (bPenetrate && KTgE_MAX_CONTACTS == V(tgCO_F_SP_Penetrate_SP)(psPacket, psSP0, psSP1))
            {
                return (KTgE_MAX_CONTACTS);
            };

            return (KTgE_PREPENETRATION);
        }
        else
        {
            TYPE                                fK0 = psDT->m_fDT - F(KTgEPS);
            const TYPE                          fDS_UDT = V(F_DOT)(&vDS, &psDT->m_vUDT);

            fK0 = F(tgPM_FSEL)(fK0, fDS_UDT - F(KTgEPS), MKL(-1.0)); /* Negligible distance towards each other. */
            fK0 = F(tgPM_FSEL)(fK0, fRS + *pfPM*psDT->m_fDT - fDS_UDT, MKL(-1.0)); /* Separation along displacement is too large. */
            fK0 = F(tgPM_FSEL)(fK0, fRS2 - fDS_DS + fDS_UDT*fDS_UDT, MKL(-1.0)); /* Orthogonal separation is too large. */
            fK0 = F(tgPM_FSEL)(fK0, fDS_UDT - F(tgPM_SQRT)(fK0), MKL(-1.0)); /* Time occurs after first time of contact. */

            if (fK0 > (*pfPM + psPacket->m_fSweepTol)*psDT->m_fDT)
            {
                return (KTgE_NO_INTERSECT); /* Outside of sweep space. */
            }
            else
            {
                const TYPE                          fT0 = fK0 / psDT->m_fDT;
                V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fK0, &psDT->m_vUDT);
                V(C_TgVEC)                          vSP0 = V(F_ADD)(&psSP1->m_vOrigin, &vK0);
                V(C_TgVEC)                          vK1 = V(F_SUB)(&vSP0, &psSP0->m_vOrigin);
                V(C_TgVEC)                          vNormal = V(F_NORM)(&vK1);
                V(C_TgVEC)                          vK2 = V(F_MUL_VS)(&vNormal, psSP1->m_fRadius);
                V(P_STg2_CO_Contact)                psContact;

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

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

                psContact->m_vS0 = V(F_SUB)(&vSP0, &vK2);
                psContact->m_vN0 = vNormal;
                psContact->m_fT0 = fT0;
                psContact->m_fDepth = MKL(0.0);

                ++psPacket->m_niContact;

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