Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Ellipse-Ellipse.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Ellipse;
    »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_EL_Intersect_EL) --------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_EL_Intersect_EL)(V(PC_STg2_CO_Packet) psPacket, V(CPC_TgELLIPSE) psEL0, V(CPC_TgELLIPSE) psEL1)
{
    /*  Based/Modified from 3D Game Engine Design, by David H. Eberly */
    /*    Published: Morgan Kaufmann, 2000 - ISBN: 1558605932 */

    /* ---- Place Ellipses into Reference Frame -- */

    const TYPE                          fC00 = V(F_DOT)(&psEL0->m.m.vOrigin, &psEL0->m.m.vMajor_Axis);
    const TYPE                          fC01 = V(F_DOT)(&psEL0->m.m.vOrigin, &psEL0->m.m.vMinor_Axis);
    const TYPE                          fR00 = psEL0->m_fMajor;
    const TYPE                          fR01 = psEL0->m_fMinor;

    const TYPE                          fX000 = fR00;
    const TYPE                          fX011 = fR01;

    const TYPE                          fC10 = V(F_DOT)(&psEL1->m.m.vOrigin, &psEL0->m.m.vMajor_Axis);
    const TYPE                          fC11 = V(F_DOT)(&psEL1->m.m.vOrigin, &psEL0->m.m.vMinor_Axis);
    const TYPE                          fR10 = psEL1->m_fMajor;
    const TYPE                          fR11 = psEL1->m_fMinor;

    const TYPE                          fX100 = fR10*V(F_DOT)(&psEL1->m.m.vMajor_Axis, &psEL0->m.m.vMajor_Axis);
    const TYPE                          fX101 = fR10*V(F_DOT)(&psEL1->m.m.vMajor_Axis, &psEL0->m.m.vMinor_Axis);
    const TYPE                          fX110 = fR11*V(F_DOT)(&psEL1->m.m.vMinor_Axis, &psEL0->m.m.vMajor_Axis);
    const TYPE                          fX111 = fR11*V(F_DOT)(&psEL1->m.m.vMinor_Axis, &psEL0->m.m.vMinor_Axis);

    /* ---- Create AABB for the Ellipses -- */

    const TYPE                          fE00 = fR00*F(tgPM_ABS)(fX000);
    const TYPE                          fE01 = fR01*F(tgPM_ABS)(fX011);
    const TYPE                          fE10 = fR10*F(tgPM_ABS)(fX100) + fR11*F(tgPM_ABS)(fX110);
    const TYPE                          fE11 = fR10*F(tgPM_ABS)(fX101) + fR11*F(tgPM_ABS)(fX111);

    const TYPE                          fMin00 = fC00 - fE00;
    const TYPE                          fMin01 = fC01 - fE01;
    const TYPE                          fMin10 = fC10 - fE10;
    const TYPE                          fMin11 = fC11 - fE11;

    const TYPE                          fMax00 = fC00 + fE00;
    const TYPE                          fMax01 = fC01 + fE01;
    const TYPE                          fMax10 = fC10 + fE10;
    const TYPE                          fMax11 = fC11 + fE11;

    const TYPE                          fMin = F(tgCM_MIN)(fMin00, F(tgCM_MIN)(fMin01, F(tgCM_MIN)(fMin10, fMin11)));
    const TYPE                          fMax = F(tgCM_MAX)(fMax00, F(tgCM_MAX)(fMax01, F(tgCM_MAX)(fMax10, fMax11)));

    const TYPE                          fRNG = MKL(0.5)*(fMax - fMin);
    const TYPE                          fAVG = MKL(0.5)*(fMax + fMin);
    const TYPE                          fInvRNG = MKL(1.0) / fRNG;

    /* ---- Create Ellipse Quadratics -- */

    /* Ellipse 0 */                     /* Offset and Scale the Ellipse ( reduce floating point error ) */
    const TYPE                          fSC00 = fInvRNG*(fC00 - fAVG);
    const TYPE                          fSC01 = fInvRNG*(fC01 - fAVG);
    const TYPE                          fSR00 = fInvRNG*fR00;
    const TYPE                          fSR01 = fInvRNG*fR01;

                                        /* Scale the Ellipse axes */
    const TYPE                          fInvSR00 = MKL(1.0) / fSR00;
    const TYPE                          fInvSR01 = MKL(1.0) / fSR01;
    const TYPE                          fSX000 = fX000 * fInvSR00;
    const TYPE                          fSX011 = fX011 * fInvSR01;

                                        /* Create the Tensors to formulate the quadratic */
    const TYPE                          fT000 = fSX000*fSX000;
    const TYPE                          fT013 = fSX011*fSX011;
    const TYPE                          fA000 = fT000;
    const TYPE                          fA011 = fT013;

                                        /* Construct the quadratic form */
    const TYPE                          fA0C00 = fA000*fSC00;
    const TYPE                          fA1C01 = fA011*fSC01;

    const TYPE                          fK44 = fA000;
    const TYPE                          fK45 = fA011;
    const TYPE                          fK46 = -fA0C00 - fA0C00;
    const TYPE                          fK47 = -fA1C01 - fA1C01;
    const TYPE                          fK48 = fA0C00*fSC00 + fA1C01*fSC01 - MKL(1.0);

                                        /* "Normalize" the Equation */
    const TYPE                          fK12 = F(tgCM_MAX)(F(tgPM_ABS)(fK44), MKL(0.0));
    const TYPE                          fK13 = F(tgCM_MAX)(F(tgPM_ABS)(fK45), F(tgPM_ABS)(fK46));
    const TYPE                          fK14 = F(tgCM_MAX)(F(tgPM_ABS)(fK47), F(tgPM_ABS)(fK48));
    const TYPE                          fK15 = MKL(1.0) / (F(tgCM_MAX)(fK12, F(tgCM_MAX)(fK13, fK14)));

    const TYPE                          afQP00 = fK44*fK15;
    const TYPE                          afQP02 = fK45*fK15;
    const TYPE                          afQP03 = fK46*fK15;
    const TYPE                          afQP04 = fK47*fK15;
    const TYPE                          afQP05 = fK48*fK15;


    /* Ellipse 1 */                     /* Offset and Scale the Ellipse ( reduce floating point error ) */
    const TYPE                          fSC10 = fInvRNG*(fC10 - fAVG);
    const TYPE                          fSC11 = fInvRNG*(fC11 - fAVG);
    const TYPE                          fSR10 = fInvRNG*fR10;
    const TYPE                          fSR11 = fInvRNG*fR11;

                                        /* Scale the Ellipse axes */
    const TYPE                          fInvSR10 = MKL(1.0) / fSR10;
    const TYPE                          fInvSR11 = MKL(1.0) / fSR11;
    const TYPE                          fSX100 = fX100 * fInvSR10;
    const TYPE                          fSX101 = fX101 * fInvSR10;
    const TYPE                          fSX110 = fX110 * fInvSR11;
    const TYPE                          fSX111 = fX111 * fInvSR11;

                                        /* Create the Tensors to formulate the quadratic */
    const TYPE                          fT100 = fSX100*fSX100;
    const TYPE                          fT101 = fSX100*fSX101;
    const TYPE                          fT103 = fSX101*fSX101;
    const TYPE                          fT110 = fSX110*fSX110;
    const TYPE                          fT111 = fSX110*fSX111;
    const TYPE                          fT113 = fSX111*fSX111;
    const TYPE                          fA100 = fT100 + fT110;
    const TYPE                          fA101 = fT101 + fT111;
    const TYPE                          fA111 = fT103 + fT113;

                                        /* Construct the quadratic form */
    const TYPE                          fA0C10 = fA000*fSC10;
    const TYPE                          fA1C11 = fA011*fSC11;

    const TYPE                          fK38 = fA100;
    const TYPE                          fK39 = fA101 + fA101;
    const TYPE                          fK40 = fA111;
    const TYPE                          fK41 = -fA0C10 - fA0C10;
    const TYPE                          fK42 = -fA1C11 - fA1C11;
    const TYPE                          fK43 = fA0C10*fSC10 + fA1C11*fSC11 - MKL(1.0);

                                        /* "Normalize" the Equation */
    const TYPE                          fK16 = F(tgCM_MAX)(F(tgPM_ABS)(fK38), F(tgPM_ABS)(fK39));
    const TYPE                          fK17 = F(tgCM_MAX)(F(tgPM_ABS)(fK40), F(tgPM_ABS)(fK41));
    const TYPE                          fK18 = F(tgCM_MAX)(F(tgPM_ABS)(fK42), F(tgPM_ABS)(fK43));
    const TYPE                          fK19 = MKL(1.0) / (F(tgCM_MAX)(fK16, F(tgCM_MAX)(fK17, fK18)));

    const TYPE                          afQP10 = fK19*fK38;
    const TYPE                          afQP11 = fK19*fK39;
    const TYPE                          afQP12 = fK19*fK40;
    const TYPE                          afQP13 = fK19*fK41;
    const TYPE                          afQP14 = fK19*fK42;
    const TYPE                          afQP15 = fK19*fK43;

    /* ------------------------------------------------------------------------------------------------------------------------------------------------------------------ */

    const TYPE                          fDET00 = afQP00*afQP11;
    const TYPE                          fDET01 = afQP00*afQP12 - afQP10*afQP02;
    const TYPE                          fDET02 = afQP00*afQP13 - afQP10*afQP03;
    const TYPE                          fDET03 = afQP00*afQP14 - afQP10*afQP04;
    const TYPE                          fDET04 = afQP00*afQP15 - afQP10*afQP05;
    const TYPE                          fDET05 = afQP11*afQP02;
    const TYPE                          fDET06 = afQP11*afQP04;
    const TYPE                          fDET07 = afQP11*afQP05;
    const TYPE                          fDET08 = afQP02*afQP13 - afQP12*afQP03;
    const TYPE                          fDET09 = afQP03*afQP14 - afQP13*afQP04;
    const TYPE                          fDET10 = afQP03*afQP15 - afQP13*afQP05;

    const TYPE                          fK24 = fDET07 + fDET09;
    const TYPE                          fK25 = fDET06 - fDET08;

    const TYPE                          fK26 = fDET03*fDET04;
    const TYPE                          fK27 = fDET01*fDET04;
    const TYPE                          fK28 = fDET01*fDET03;

    const TYPE                          fK29 = fK26 + fK26;
    const TYPE                          fK30 = fK27 + fK27;
    const TYPE                          fK31 = fK28 + fK28;

    const TYPE                          fCoEff00 = fDET02*fDET10 - fDET04*fDET04;
    const TYPE                          fCoEff01 = fDET00*fDET10 + fDET02*fK24 - fK29;
    const TYPE                          fCoEff02 = fDET00*fK24 + fDET02*fK25 - fDET03*fDET03 - fK30;
    const TYPE                          fCoEff03 = fDET00*fK25 + fDET02*fDET05 - fK31;
    const TYPE                          fCoEff04 = fDET00*fDET05 - fDET01*fDET01;

    /* ------------------------------------------------------------------------------------------------------------------------------------------------------------------ */

    C_TgSINT32                          niContact = psPacket->m_niContact;
    V(P_STg2_CO_Contact)                psContact;
    TgSINT32                            iY;

    /* Adjustment for quadratics to allow for relative error testing when eliminating extraneous roots. */
    TYPE                                fNorm0 = afQP00*afQP00 + afQP02*afQP02;
    TYPE                                fNorm1 = afQP11*afQP11 + afQP10*afQP10 + afQP12*afQP12;

    TYPE                                afRootY[4];
    TgSINT32                            iCountY;

    TgPARAM_CHECK( F(tgCM_NR1)(V(F_DOT)(&psEL0->m.m.vNormal, &psEL1->m.m.vNormal)) );

    F(F_Calc_Root_4)(afRootY, &iCountY, fCoEff00, fCoEff01, fCoEff02, fCoEff03, fCoEff04);

    if (iCountY == 0)
    {
        return (KTgE_NO_INTERSECT);
    }

    /* Test roots to eliminate extraneous ones that occurred due to "squaring". */
    for (iY = 0; iY < iCountY; ++iY)
    {
        const TYPE                          fY = afRootY[iY];
        const TYPE                          fNum = -((fDET01*fY + fDET03)*fY + fDET04);
        const TYPE                          fDen = fDET00*fY + fDET02;
        const TYPE                          fX = fNum / fDen;

        const TYPE                          fK32 = afQP00*fX + afQP03;
        const TYPE                          fK33 = afQP02*fY + afQP04;
        const TYPE                          fK34 = fK32*fX + fK33*fY + afQP05;

        const TYPE                          fK35 = afQP10*fX + afQP11*fY + afQP13;
        const TYPE                          fK36 = afQP12*fY + afQP14;
        const TYPE                          fK37 = fK35*fX + fK36*fY + afQP15;

        if ((F(tgPM_ABS)(fK34) >= MKL(1E-3)*fNorm0) || (F(tgPM_ABS)(fK37) >= MKL(1E-3)*fNorm1))
        {
            continue;
        };

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

        psContact->m_vS0.m.x = fRNG*(fX)+fAVG;
        psContact->m_vS0.m.y = fRNG*(fY)+fAVG;
        psContact->m_vN0 = V(KTgV_ZERO);
        psContact->m_fT0 = MKL(0.0);
        psContact->m_fDepth = MKL(0.0);

        ++psPacket->m_niContact;
    }

    return (niContact < psPacket->m_niContact ? KTgS_OK : KTgE_NO_INTERSECT);
}


/* ---- V(tgCO_F_EL_Intersect2D_EL) ------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgRESULT V(tgCO_F_EL_Intersect2D_EL)(V(PC_TgVEC) pvR0, V(PC_TgVEC) pvR1, V(CPC_TgELLIPSE) psEL0, V(CPC_TgELLIPSE) psEL1)
{
    V(STg2_CO_Packet)                   sPacket;
    V(STg2_CO_Contact)                  asContact[8];
    TgRESULT                            iResult;

    sPacket.m_psContact = asContact;
    sPacket.m_fSweepTol = MKL(0.0);
    sPacket.m_fSnapTol = F(KTgEPS);
    sPacket.m_bReport_Penetration = TgFALSE;
    sPacket.m_niContact = 0;
    sPacket.m_niMaxContact = 8;

    iResult = V(tgCO_F_EL_Intersect_EL)(&sPacket, psEL0, psEL1);

    {
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(asContact[0].m_vS0.m.x, &psEL0->m.m.vMajor_Axis);
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)(asContact[0].m_vS0.m.y, &psEL0->m.m.vMinor_Axis);
        V(C_TgVEC)                          vK2 = V(F_MUL_SV)(asContact[1].m_vS0.m.x, &psEL0->m.m.vMajor_Axis);
        V(C_TgVEC)                          vK3 = V(F_MUL_SV)(asContact[1].m_vS0.m.y, &psEL0->m.m.vMinor_Axis);
        V(C_TgVEC)                          vK4 = V(F_ADD)(&vK0, &vK1);
        V(C_TgVEC)                          vK5 = V(F_ADD)(&vK2, &vK3);

        *pvR0 = V(F_ADD)(&psEL0->m.m.vOrigin, &vK4);
        *pvR1 = V(F_ADD)(&psEL1->m.m.vOrigin, &vK5);
    }

    return (iResult);
}