Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Circle-Linear.c_inc
    »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Circle;Line;Ray;Segment;
    »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".                                                   */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
/*  Algorithm taken from paper at: http://jgt.akpeters.com/papers/Vranek02/                                                                                               */
/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/* == Collision ========================================================================================================================================================= */

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

TgTYPE_STRUCT(VI(STg2_EqN_CI_LR),)
{
    TYPE                                fCF0, fCF1, fCF2, fCF3, fCF4, fCF5, fCF6;
};

static TgVOID                               VI(tgCO_FI_CI_Init_EqN_LR)( VI(PC_STg2_EqN_CI_LR), V(CPC_TgVEC), const TYPE, V(CPC_TgVEC), V(CPC_TgVEC), V(CPC_TgVEC));
static TYPE                                 VI(tgCO_FI_CI_Solve_EqN_LR)(PC_TgVOID, const TYPE);
static TYPE                                 VI(tgCO_FI_CI_Solve_Diff1_EqN_LR)(PC_TgVOID, const TYPE);
static TYPE                                 VI(tgCO_FI_CI_Solve_Diff2_EqN_LR)(PC_TgVOID, const TYPE);




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

/* ---- VI(tgCO_FI_CI_ParamSq_LR) --------------------------------------------------------------------------------------------------------------------------------------- */
/* Input:  vCIS0: Origin of the circle (and the point on the plane).                                                                                                      */
/* Input:  vCIN0: Normal to the plane containing the circle.                                                                                                              */
/* Input:  fRadius: Radius of the circle                                                                                                                                  */
/* Input:  vS0,vD0: Origin and Direction for the Linear.                                                                                                                  */
/* Output: vCI0: Point of closest proximity on the circle.                                                                                                                */
/* Output: tyLN0: Parametric parameter to generate point of minimal distance #1 based on the linear.                                                                      */
/* Return: Minimal distance between the two primitives or negative type max if they intersect or are invalid.                                                             */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TYPE VI(tgCO_FI_CI_ParamSq_LR)(V(PC_TgVEC) pvCI0, TYPE *pfLN0, V(CPC_TgCIRCLE) psCI0, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0)
{
    V(C_TgVEC)                          vDS = V(F_SUB)(pvS0, &psCI0->m.m.vOrigin);
    const TYPE                          fDS_DS = V(F_LSQ)(&vDS);
    const TYPE                          fD0_D0 = V(F_LSQ)(pvD0);
    const TYPE                          fMid = -V(F_DOT)(pvD0, &vDS) / fD0_D0;
    const TYPE                          fMinT = fMid - psCI0->m_fRadius;
    const TYPE                          fMaxT = fMid + psCI0->m_fRadius;

    VI(STg2_EqN_CI_LR)                  sEqN;
    TYPE                                fT0;
    TYPE                                fT1, fT2, fMinDistSq;

    if (TgEXPECT_FALSE( fDS_DS <= F(KTgEPS) ))
    {
        return (fDS_DS); /* Quick Out - the point is within tolerance of circle origin. */
    };

    VI(tgCO_FI_CI_Init_EqN_LR)(&sEqN, &psCI0->m.m.vNormal, psCI0->m_fRadius, &vDS, pvS0, pvD0);

    if (!F(F_BrentZ)(&fT0, &sEqN, VI(tgCO_FI_CI_Solve_Diff1_EqN_LR), fMinT, fMaxT))
    {
        return (-F(KTgMAX));
    };

    fT0 -= VI(tgCO_FI_CI_Solve_Diff1_EqN_LR)(&sEqN, fT0) / VI(tgCO_FI_CI_Solve_Diff2_EqN_LR)(&sEqN, fT0);


    if (VI(tgCO_FI_CI_Solve_Diff2_EqN_LR)(&sEqN, fT0) < MKL(0.0))
    {
        if (
            (!F(F_BrentZ)(&fT1, &sEqN, VI(tgCO_FI_CI_Solve_Diff1_EqN_LR), fMinT, fT0 - F(KTgROOT_EPS))) ||
            (!F(F_BrentZ)(&fT2, &sEqN, VI(tgCO_FI_CI_Solve_Diff1_EqN_LR), fT0 + F(KTgROOT_EPS), fMaxT))
        )
        {
            return (-F(KTgMAX));
        }
        else
        {
            const TYPE                          fMin1 = VI(tgCO_FI_CI_Solve_EqN_LR)(&sEqN, fT1);
            const TYPE                          fMin2 = VI(tgCO_FI_CI_Solve_EqN_LR)(&sEqN, fT2);

            fT0 = F(tgPM_FSEL)(fMin2 - fMin1, fT1, fT2);
            fMinDistSq = F(tgPM_FSEL)(fMin2 - fMin1, fMin1, fMin2);
        };
    }
    else
    {
        /* Compute coefficients of a polynomial */

        fMinDistSq = VI(tgCO_FI_CI_Solve_EqN_LR)(&sEqN, fT0);

        {
            const TYPE                          fPT3 = MKL(2.0)*sEqN.fCF5*sEqN.fCF6;
            const TYPE                          fPT4 = sEqN.fCF6*sEqN.fCF6;

            /* Return in the case of the coefficient */

            if (F(tgPM_ABS)(fPT4) > F(KTgROOT_EPS))
            {
                const TYPE                          fCF1 = fPT3 + MKL(2.0)*fPT4*fT0;
                const TYPE                          fCF2 = MKL(2.0)*sEqN.fCF6*(sEqN.fCF4 - fMinDistSq)
                    + MKL(2.0)*fPT3*fT0
                    + MKL(3.0)*fPT4*fT0*fT0
                    - sEqN.fCF3*sEqN.fCF3*sEqN.fCF2
                    + sEqN.fCF5*sEqN.fCF5;
                TYPE                                afRoot[2];
                TgSINT32                            niRoot;

                if (F(F_Calc_Root_2)(afRoot, &niRoot, fPT4, fCF1, fCF2))
                {
                    fT1 = F(tgPM_FSEL)(afRoot[0] - afRoot[1], afRoot[1], afRoot[0]);
                    fT2 = F(tgPM_FSEL)(afRoot[0] - afRoot[1], afRoot[0], afRoot[1]);

                    /* Find the global minimum */
                    F(F_BrentZ)(&fT0, &sEqN, VI(tgCO_FI_CI_Solve_Diff1_EqN_LR), fT1, fT2);
                    fMinDistSq = VI(tgCO_FI_CI_Solve_EqN_LR)(&sEqN, fT0);
                };
            };
        };
    };

    if (LN_CAP_1)
    {
        V(C_TgVEC)                          vK1 = V(F_ADD)(pvS0, pvD0);
        const TYPE                          fDistSq = V(tgCO_F_CI_DistSq_VT)(psCI0, &vK1);

        if (fDistSq < fMinDistSq || fT0 > MKL(1.0))
        {
            fMinDistSq = fDistSq;
            fT0 = MKL(1.0);
        };
    };

    if (LN_CAP_0)
    {
        const TYPE                          fDistSq = V(tgCO_F_CI_DistSq_VT)(psCI0, pvS0);

        if (fDistSq < fMinDistSq || fT0 < MKL(0.0))
        {
            fMinDistSq = fDistSq;
            fT0 = MKL(0.0);
        };
    };

    *pfLN0 = fT0;

    {
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fT0, pvD0);
        V(C_TgVEC)                          vT0 = V(F_ADD)(&vDS, &vK1);
        V(C_TgVEC)                          vK0 = V(F_MUL_SV)(V(F_DOT)(&vT0, &psCI0->m.m.vNormal), &psCI0->m.m.vNormal);
        V(C_TgVEC)                          vK2 = V(F_SUB)(&vT0, &vK0);
        V(C_TgVEC)                          vD1 = V(F_NORM)(&vK2);
        V(C_TgVEC)                          vK3 = V(F_MUL_SV)(psCI0->m_fRadius, &vD1);

        *pvCI0 = V(F_ADD)(&vT0, &vK3);

        return (fMinDistSq);
    };
}




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

/* ---- VI(tgCO_FI_CI_Init_EqN_LR) -------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TgVOID VI(tgCO_FI_CI_Init_EqN_LR)(
    VI(PC_STg2_EqN_CI_LR) psEQN, V(CPC_TgVEC) pvNormal, const TYPE fRadius, V(CPC_TgVEC) pvDS, V(CPC_TgVEC) UNUSED_PARAM pvS0, V(CPC_TgVEC) pvD0)
{
    const TYPE                          fN_D0 = V(F_DOT)(pvNormal, pvD0);
    const TYPE                          fN_DS = V(F_DOT)(pvNormal, pvDS);
    const TYPE                          fDS_DS = V(F_LSQ)(pvDS);
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)(fN_D0, pvNormal);
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)(fN_DS, pvNormal);
    V(C_TgVEC)                          vT0 = V(F_SUB)(pvD0, &vK0);
    V(C_TgVEC)                          vT1 = V(F_SUB)(pvDS, &vK1);

    psEQN->fCF0 = V(F_LSQ)(&vT1);
    psEQN->fCF1 = MKL(2.0)*V(F_DOT)(&vT0, &vT1);
    psEQN->fCF2 = V(F_LSQ)(&vT0);
    psEQN->fCF3 = -MKL(2.0)*fRadius;
    psEQN->fCF4 = fDS_DS + fRadius*fRadius;
    psEQN->fCF5 = MKL(2.0)*V(F_DOT)(pvDS, pvD0);
    psEQN->fCF6 = V(F_LSQ)(pvD0);
}


/* ---- VI(tgCO_FI_CI_Solve_EqN_LR) ------------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TYPE VI(tgCO_FI_CI_Solve_EqN_LR)(PC_TgVOID psEQN, const TYPE fT0)
{
    return (
        ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF6*fT0*fT0 +
        ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF5*fT0 +
        ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF4 +
        ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF3*F(tgPM_SQRT)
            (
                ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF2*fT0*fT0
                + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF1*fT0
                + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF0
                )
    );
}


/* ---- VI(tgCO_FI_CI_Solve_Diff1_EqN_LR) ------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TYPE VI(tgCO_FI_CI_Solve_Diff1_EqN_LR)(PC_TgVOID psEQN, const TYPE fT0)
{
    return (
          ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF6*fT0*MKL(2.0)
        + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF5
        +
        (
            ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF3
            * (((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF2*fT0*MKL(2.0) + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF1)
            )
        
        (
            MKL(2.0)*F(tgPM_SQRT)
            (
                ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF2*fT0*fT0
                + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF1*fT0
                + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF0
                )
            )
    );
}


/* ---- VI(tgCO_FI_CI_Solve_Diff2_EqN_LR) ------------------------------------------------------------------------------------------------------------------------------- */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
static TYPE VI(tgCO_FI_CI_Solve_Diff2_EqN_LR)(PC_TgVOID psEQN, const TYPE fT0)
{
    const TYPE                          fX4 = ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF2*fT0*fT0
                                            + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF1*fT0
                                            + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF0;
    const TYPE                          fX5 = F(tgPM_SQRT)(fX4);
    const TYPE                          fX1 = ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF2*fT0*MKL(2.0)
                                            + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF1;

    return
        (
              ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF6*MKL(2.0)
            - ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF3 / fX5 / fX4*fX1*fX1 / 4
            + ((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF3 / fX5*((VI(PC_STg2_EqN_CI_LR))(psEQN))->fCF2
        );
}