Home

Resume

Blog

Teikitu


/* =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= */
/*  »Project«   Teikitu Gaming System (TgS) (∂)
    »File«      TgS Collision - F - Linear.c_inc
    »Keywords«  Collision;Linear;Line;Ray;Segment;Point;Distance;Closest
    »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(F_Internal_Intersect) ----------------------------------------------------------------------------------------------------------------------------------------- */
/*  -- Internal Function -- Used in contact generation, for multiple point returns on parallel case.                                                                      */
/* Assume: The two segments are in contact.                                                                                                                               */
/* Input:  pvS0,pvD0: Origin and Direction for Segment #1                                                                                                                 */
/* Input:  vS1,pvD1: Origin and Direction for Segment #2                                                                                                                  */
/* Output:  vRN0, vRN1: Points of contact between the two segments.                                                                                                       */
/* Return: The number of contacts between the two segments, where a negative value represents an error condition.                                                         */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgSINT32 V(tgCO_F_LN_Internal_Intersect_LN)( V(PC_TgVEC) pvRN0, V(PC_TgVEC) pvRN1, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0, V(CPC_TgVEC) pvS1, V(CPC_TgVEC) pvD1 )
{
    V(C_TgVEC)                          vDS = V(F_SUB)( pvS1, pvS0 );

    const TYPE                          fD0_D0 = V(F_LSQ)( pvD0 );
    const TYPE                          fD1_D1 = V(F_LSQ)( pvD1 );
    const TYPE                          fD0_D1 = V(F_DOT)( pvD0, pvD1 );
    const TYPE                          fDS_D0 = V(F_DOT)( &vDS, pvD0 );
    const TYPE                          fDS_D1 = -V(F_DOT)( &vDS, pvD1 );
    const TYPE                          fDet = fD0_D0*fD1_D1 - fD0_D1*fD0_D1;

    V(C_TgVEC)                          vOrgSum = V(F_ADD)(pvS0, pvS1);

    if (fDet > F(KTgEPS)*fD0_D0*fD1_D1)
    {
        const TYPE                          fT0 = (fDS_D0*fD1_D1 + fDS_D1*fD0_D1) / fDet;
        const TYPE                          fT1 = (fDS_D0*fD0_D1 + fDS_D1*fD0_D0) / fDet;
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fT1, pvD1 );
        V(C_TgVEC)                          vK2 = V(F_MUL_SV)( fT0, pvD0 );
        V(C_TgVEC)                          vK3 = V(F_ADD)( &vOrgSum, &vK2 );
        V(C_TgVEC)                          vK4 = V(F_ADD)( &vK3, &vK1 );

        if ((fT0 < MKL(0.0)) || (fT0 > MKL(1.0)) || (fT1 < MKL(0.0)) || (fT1 > MKL(1.0)))
        {
            TgERROR_CO( TgT("Edge-Edge outside of range.\n" ) );
        };

        *pvRN0 = V(F_MUL_SV)( MKL(0.5), &vK4 );

        return (1);
    }
    else
    {
        /* Linears are parallel */

        const TYPE                          fDE_D0 = fDS_D0 + fD0_D1; /* vDE = vS1+pvD1, fDE_D1 = (vS1+pvD1 - pvS0)•pvD0 */
        const TYPE                          fDF_D1 = fD0_D1 - fDS_D1; /* vDF = pvS0+pvD0, fDF_D1 = (pvS0+pvD0 - vS1)•pvD1 */

        const TYPE                          fTA = fDS_D0 / fD0_D0;
        const TYPE                          fTC = fDE_D0 / fD0_D0;

        /* Point 0 of segment 0 if contained in segment 1, otherwise if segments are mutually directed point 0 of segment 1, else point 1 of segment 1. */
        const TYPE                          fF0 = F(tgPM_FSEL)( fDS_D1, F(tgPM_FSEL)( fD1_D1 - fDS_D1, MKL(1.0), MKL(-1.0)), MKL(-1.0) );
        const TYPE                          fT0 = F(tgPM_FSEL)( fF0, MKL(0.0), F(tgPM_FSEL)( fD0_D1, fTA, fTC ) );
        const TYPE                          fT1 = F(tgPM_FSEL)( fF0, (fDS_D1 / fD1_D1), F(tgPM_FSEL)( fD0_D1, MKL(0.0), MKL(1.0) ) );

        /* Point 1 of segment 0 if contained in segment 1, otherwise if segments are mutually directed point 1 of segment 1, else point 0 of segment 1. */
        const TYPE                          fF1 = F(tgPM_FSEL)( fDF_D1, F(tgPM_FSEL)( fD1_D1 - fDF_D1, MKL( 1.0 ), MKL( -1.0 )), MKL( -1.0 ) );
        const TYPE                          fT2 = F(tgPM_FSEL)( fF1, MKL( 1.0 ), F(tgPM_FSEL)( fD0_D1, fTC, fTA ) );
        const TYPE                          fT3 = F(tgPM_FSEL)( fF1, (fDF_D1 / fD1_D1), F(tgPM_FSEL)( fD0_D1, MKL( 1.0 ), MKL( 0.0 ) ) );

        V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT0, pvD0 );
        V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fT1, pvD1 );
        V(C_TgVEC)                          vK2 = V(F_MUL_SV)( fT2, pvD0 );
        V(C_TgVEC)                          vK3 = V(F_MUL_SV)( fT3, pvD1 );
        V(C_TgVEC)                          vK4 = V(F_ADD)( &vOrgSum, &vK0 );
        V(C_TgVEC)                          vK5 = V(F_ADD)( &vOrgSum, &vK2 );
        V(C_TgVEC)                          vK6 = V(F_ADD)( &vK4, &vK1 );
        V(C_TgVEC)                          vK7 = V(F_ADD)( &vK5, &vK3 );

        *pvRN0 = V(F_MUL_SV)( MKL(0.5), &vK6 );
        *pvRN1 = V(F_MUL_SV)( MKL(0.5), &vK7 );

        if ((fD0_D1 >= MKL(0.0) && (fDE_D0 < MKL(0.0) || fDS_D0 > MKL(1.0))) || (fD0_D1 <= MKL(0.0) && (fDS_D0 < MKL(0.0) || fDE_D0 > MKL(1.0))))
        {
            return (-1);
        };

        return (2);
    };
}


/* ---- V(F_Internal_Parallel) ------------------------------------------------------------------------------------------------------------------------------------------ */
/*  -- Internal Function -- Used in contact generation, for multiple point returns on parallel case.                                                                      */
/* Assume: The two segments are in contact, and are parallel.                                                                                                             */
/* Input:  pvS0,pvD0: Origin and Direction for Segment #1                                                                                                                 */
/* Input:  vS1,pvD1: Origin and Direction for Segment #2                                                                                                                  */
/* Output:  vRN0, vRN1: Points of contact between the two segments.                                                                                                       */
/* Return: The number of contacts between the two segments, where a negative value represents an error condition.                                                         */
/* ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
TgSINT32 V(tgCO_F_LN_Internal_Parallel_LN)( V(PC_TgVEC) pvRN0, V(PC_TgVEC) pvRN1, V(CPC_TgVEC) pvS0, V(CPC_TgVEC) pvD0, V(CPC_TgVEC) pvS1, V(CPC_TgVEC) pvD1 )
{
    const TYPE                          fD0_D0 = V(F_LSQ)( pvD0 ), fD1_D1 = V(F_LSQ)( pvD1 );
    V(C_TgVEC)                          vDS = V(F_SUB)( pvS1, pvS0 );

    /*                                  Projection Values */
    const TYPE                          fD0_D1 = V(F_DOT)( pvD0, pvD1 );
    const TYPE                          fDS_D0 = V(F_DOT)( &vDS, pvD0 );
    const TYPE                          fDS_D1 = -V(F_DOT)( &vDS, pvD1 );

    const TYPE                          fDE_D0 = fDS_D0 + fD0_D1; /* vDE = vS1+pvD1, fDE_D1 = (vS1+pvD1 - pvS0)•pvD0 */
    const TYPE                          fDF_D1 = fD0_D1 - fDS_D1; /* vDF = pvS0+pvD0, fDF_D1 = (pvS0+pvD0 - vS1)•pvD1 */

    const TYPE                          fTA = fDS_D0 / fD0_D0;
    const TYPE                          fTC = fDE_D0 / fD0_D0;

    /* Point 0 of segment 0 if contained in segment 1, otherwise if segments are mutually directed point 0 of segment 1, else point 1 of segment 1. */
    const TYPE                          fF0 = F(tgPM_FSEL)( fDS_D1, F(tgPM_FSEL)( fD1_D1 - fDS_D1, MKL(1.0), MKL(-1.0)), MKL(-1.0) );
    const TYPE                          fT0 = F(tgPM_FSEL)( fF0, MKL(0.0), F(tgPM_FSEL)( fD0_D1, fTA, fTC ) );
    const TYPE                          fT1 = F(tgPM_FSEL)( fF0, (fDS_D1 / fD1_D1), F(tgPM_FSEL)( fD0_D1, MKL(0.0), MKL(1.0) ) );

    /* Point 1 of segment 0 if contained in segment 1, otherwise if segments are mutually directed point 1 of segment 1, else point 0 of segment 1. */
    const TYPE                          fF1 = F(tgPM_FSEL)( fDF_D1, F(tgPM_FSEL)( fD1_D1 - fDF_D1, MKL(1.0), MKL(-1.0)), MKL(-1.0) );
    const TYPE                          fT2 = F(tgPM_FSEL)( fF1, MKL(1.0), F(tgPM_FSEL)( fD0_D1, fTC, fTA ) );
    const TYPE                          fT3 = F(tgPM_FSEL)( fF1, (fDF_D1 / fD1_D1), F(tgPM_FSEL)( fD0_D1, MKL(1.0), MKL(0.0) ) );

    V(C_TgVEC)                          vOrgSum = V(F_ADD)( pvS0, pvS1 );
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT0, pvD0 );
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fT1, pvD1 );
    V(C_TgVEC)                          vK2 = V(F_MUL_SV)( fT2, pvD0 );
    V(C_TgVEC)                          vK3 = V(F_MUL_SV)( fT3, pvD1 );
    V(C_TgVEC)                          vK4 = V(F_ADD)( &vOrgSum, &vK0 );
    V(C_TgVEC)                          vK5 = V(F_ADD)( &vOrgSum, &vK2 );
    V(C_TgVEC)                          vK6 = V(F_ADD)( &vK4, &vK1 );
    V(C_TgVEC)                          vK7 = V(F_ADD)( &vK5, &vK3 );

    *pvRN0 = V(F_MUL_SV)( MKL(0.5), &vK6 );
    *pvRN1 = V(F_MUL_SV)( MKL(0.5), &vK7 );

    if ((fD0_D1 >= MKL(0.0) && (fDE_D0 < MKL(0.0) || fDS_D0 > MKL(1.0))) || (fD0_D1 <= MKL(0.0) && (fDS_D0 < MKL(0.0) || fDE_D0 > MKL(1.0))))
    {
        return (-1);
    };

    return (2);
}