Home

Resume

Blog

Teikitu


// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= //
//  »Project«   Teikitu Gaming System (TgS) (∂)
//  »File«      TgS Collision - F - Ray.i_inc
//  »Author«    Andrew Aye (EMail: mailto:andrew.aye@gmail.com, Web: http://www.andrewaye.com)
//  »Version«   4.0
//  »Keywords«  Collision;Distance;Closest;Intersect;Penetrate;Sweep;Ray;
// ------------------------------------------------------------------------------------------------------------------------------ //
//  Copyright: © 2002-2010, 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: 
//    Redistributions of source code must retain this copyright notice, this list of conditions and the following disclaimers. 
//    Redistributions 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. 
//  Neither the names of the copyright owner nor the names of its contributors may 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 ================================================================================================================= //

// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //
//  Public Functions - Scalar Functions
// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //

TgINLINE TYPE V(tgCO_F_DistSq_RY_VT)( V(CPCU_TgRAY) psRY0, V(CPCU_TgVEC) pvS0 )
{
    V(C_TgVEC)                          vDS = V(F_SUB_VV)( pvS0, &psRY0->m_vOrigin );
    const TYPE                          fD0_D0 = V(F_LSQ)( &psRY0->m_vDirN );
    const TYPE                          fDS_DS = V(F_LSQ)( &vDS );
    const TYPE                          fDS_D0 = V(F_DOT_VV)( &vDS, &psRY0->m_vDirN );

    TgASSERT( fD0_D0 > F(KTgEPS) ); //« Must have a valid ray.
    return (fDS_DS - F(tgPM_FSEL)( fDS_D0, fDS_D0*(fDS_D0 / fD0_D0), MKL(0.0) ));
}


TgINLINE TYPE V(tgCO_F_Dist_RY_VT)( V(CPCU_TgRAY) psRY0, V(CPCU_TgVEC) pvS0 )
{
    const TYPE                          fTA = V(tgCO_F_DistSq_RY_VT)( psRY0, pvS0 );

    return (F(tgPM_FSEL)( fTA, F(tgPM_SQRT)( fTA ), fTA ));
}


TgINLINE TYPE V(tgCO_F_ClosestSq_RY_VT)( V(PCU_TgVEC) pvRY0, V(CPCU_TgRAY) psRY0, V(CPCU_TgVEC) pvS0 )
{
    TYPE                                fT0;
    const TYPE                          fDistSq = V(tgCO_F_ParamSq_RY_VT)( &fT0, psRY0, pvS0 );
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT0, &psRY0->m_vDirN );

    *pvRY0 = V(F_ADD_VV)( &psRY0->m_vOrigin, &vK0 );
    return (fDistSq);
}


TgINLINE TYPE V(tgCO_F_Closest_RY_VT)( V(PCU_TgVEC) pvRY0, V(CPCU_TgRAY) psRY0, V(CPCU_TgVEC) pvS0 )
{
    const TYPE                          fTA = V(tgCO_F_ClosestSq_RY_VT)( pvRY0, psRY0, pvS0 );

    return (F(tgPM_FSEL)( fTA, F(tgPM_SQRT)( fTA ), fTA ));
}


TgINLINE TYPE V(tgCO_F_ParamSq_RY_VT)( PCU_TYPE pfRY0, V(CPCU_TgRAY) psRY0, V(CPCU_TgVEC) pvS0 )
{
    return (V(tgCO_F_ParamSq_LR10_VT)( pfRY0, &psRY0->m_vOrigin, &psRY0->m_vDirN, pvS0 ));
}


TgINLINE TYPE V(tgCO_F_Param_RY_VT)( PCU_TYPE pfRY0, V(CPCU_TgRAY) psRY0, V(CPCU_TgVEC) pvS0 )
{
    const TYPE                          fTA = V(tgCO_F_ParamSq_RY_VT)( pfRY0, psRY0, pvS0 );

    return (F(tgPM_FSEL)( fTA, F(tgPM_SQRT)( fTA ), fTA ));
}


TgINLINE TYPE V(tgCO_F_ParamSq_LR10_VT)( PCU_TYPE pfRY0, V(CPCU_TgVEC) pvS0, V(CPCU_TgVEC) pvD0, V(CPCU_TgVEC) pvS1 )
{
    V(C_TgVEC)                          vDS = V(F_SUB_VV)( pvS1, pvS0 );
    const TYPE                          fD0_D0 = V(F_LSQ)( pvD0 );
    const TYPE                          fDS_DS = V(F_LSQ)( &vDS );
    const TYPE                          fDS_D0 = V(F_DOT_VV)( &vDS, pvD0 );
    const TYPE                          fRY0 = F(tgPM_FSEL)( fDS_D0, fDS_D0/fD0_D0, MKL(0.0) );

    TgASSERT( fD0_D0 > F(KTgEPS) ); //« Must have a valid ray.

    *pfRY0 = fRY0;
    return (fDS_DS - fDS_D0*fRY0);
}


// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //
//  Public Functions - Ray - Line
// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //

TgINLINE TYPE V(tgCO_F_DistSq_RY_LN)( V(CPCU_TgRAY) psRY0, V(CPCU_TgLINE) psLN0 )
{
    TYPE                                fT0,fT1;

    return (V(tgCO_F_ParamSq_RY_LN)( &fT0,&fT1, psRY0, psLN0 ));
}


TgINLINE TYPE V(tgCO_F_Dist_RY_LN)( V(CPCU_TgRAY) psRY0, V(CPCU_TgLINE) psLN0 )
{
    return (F(tgPM_SQRT)( V(tgCO_F_DistSq_RY_LN)( psRY0, psLN0 ) ));
}


TgINLINE TYPE V(tgCO_F_ClosestSq_RY_LN)( V(PCU_TgVEC) pvRY0, V(PCU_TgVEC) pvLN0, V(CPCU_TgRAY) psRY0, V(CPCU_TgLINE) psLN0 )
{
    TYPE                                fT0,fT1;
    const TYPE                          fDistSq = V(tgCO_F_ParamSq_RY_LN)( &fT0,&fT1, psRY0, psLN0 );
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT0, &psRY0->m_vDirN );
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fT1, &psLN0->m_vDirN );

    *pvRY0 = V(F_ADD_VV)( &psRY0->m_vOrigin, &vK0 );
    *pvLN0 = V(F_ADD_VV)( &psLN0->m_vOrigin, &vK1 );

    return (fDistSq);
}


TgINLINE TYPE V(tgCO_F_Closest_RY_LN)( V(PCU_TgVEC) pvRY0, V(PCU_TgVEC) pvLN0, V(CPCU_TgRAY) psRY0, V(CPCU_TgLINE) psLN0 )
{
    return (F(tgPM_SQRT)( V(tgCO_F_ClosestSq_RY_LN)( pvRY0,pvLN0, psRY0,psLN0 ) ));
}


TgINLINE TYPE V(tgCO_F_ParamSq_RY_LN)( PCU_TYPE pfRY0, PCU_TYPE pfLN0, V(CPCU_TgRAY) psRY0, V(CPCU_TgLINE) psLN0 )
{
    return (V(tgCO_F_ParamSq_LR10_LR00)( pfRY0,pfLN0, &psRY0->m_vOrigin,&psRY0->m_vDirN, &psLN0->m_vOrigin,&psLN0->m_vDirN ));
}


TgINLINE TYPE V(tgCO_F_Param_RY_LN)( PCU_TYPE pfRY0, PCU_TYPE pfLN0, V(CPCU_TgRAY) psRY0, V(CPCU_TgLINE) psLN0 )
{
    return (F(tgPM_SQRT)( V(tgCO_F_ParamSq_RY_LN)( pfRY0,pfLN0, psRY0,psLN0 ) ));
}


TgINLINE TgBOOL V(tgCO_F_Test_RY_LN)( V(CPCU_TgRAY) psRY0, V(CPCU_TgLINE) psLN0 )
{
    V(C_TgVEC)                          vDS = V(F_SUB_VV)( &psLN0->m_vOrigin, &psRY0->m_vOrigin );
    V(C_TgVEC)                          vNM = V(F_CX)( &psRY0->m_vDirN, &psLN0->m_vDirN );
    V(C_TgVEC)                          vK3 = V(F_CX)( &vNM, &vDS );
    V(C_TgVEC)                          vK4 = V(F_CX)( &vNM, &vDS );
    const TYPE                          fTA = V(F_LSQ)( &vNM );
    const TYPE                          fT0 = V(F_DOT_VV)( &psLN0->m_vDirN, &vK3 ) / fTA;
    const TYPE                          fT1 = V(F_DOT_VV)( &psRY0->m_vDirN, &vK4 ) / fTA;
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT1, &psLN0->m_vDirN );
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fT0, &psRY0->m_vDirN );
    V(C_TgVEC)                          vK5 = V(F_ADD_VV)( &vDS, &vK0 );
    V(C_TgVEC)                          vK6 = V(F_SUB_VV)( &vK5, &vK1 );

    return (!F(tgCM_NR0)( fTA ) && V(F_LSQ)( &vK6 ) <= F(KTgEPS));
}


// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //
//  Public Functions - Ray - Ray
// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //

TgINLINE TYPE V(tgCO_F_DistSq_RY_RY)( V(CPCU_TgRAY) psRY0, V(CPCU_TgRAY) psRY1 )
{
    TYPE                                fT0,fT1;

    return (V(tgCO_F_ParamSq_RY_RY)( &fT0,&fT1, psRY0, psRY1 ));
}


TgINLINE TYPE V(tgCO_F_Dist_RY_RY)( V(CPCU_TgRAY) psRY0, V(CPCU_TgRAY) psRY1 )
{
    return (F(tgPM_SQRT)( V(tgCO_F_DistSq_RY_RY)( psRY0, psRY1 ) ));
}


TgINLINE TYPE V(tgCO_F_ClosestSq_RY_RY)( V(PCU_TgVEC) pvRY0, V(PCU_TgVEC) pvRY1, V(CPCU_TgRAY) psRY0, V(CPCU_TgRAY) psRY1 )
{
    TYPE                                fT0,fT1;
    const TYPE                          fDistSq = V(tgCO_F_ParamSq_RY_RY)( &fT0,&fT1, psRY0, psRY1 );
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT0, &psRY0->m_vDirN );
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fT1, &psRY1->m_vDirN );

    *pvRY0 = V(F_ADD_VV)( &psRY0->m_vOrigin, &vK0 );
    *pvRY1 = V(F_ADD_VV)( &psRY1->m_vOrigin, &vK1 );

    return (fDistSq);
}


TgINLINE TYPE V(tgCO_F_Closest_RY_RY)( V(PCU_TgVEC) pvRY0, V(PCU_TgVEC) pvRY1, V(CPCU_TgRAY) psRY0, V(CPCU_TgRAY) psRY1 )
{
    return (F(tgPM_SQRT)( V(tgCO_F_ClosestSq_RY_RY)( pvRY0,pvRY1, psRY0,psRY1 ) ));
}


TgINLINE TYPE V(tgCO_F_ParamSq_RY_RY)( PCU_TYPE pfRY0, PCU_TYPE pfRY1, V(CPCU_TgRAY) psRY0, V(CPCU_TgRAY) psRY1 )
{
    return (V(tgCO_F_ParamSq_LR10_LR10)( pfRY0,pfRY1, &psRY0->m_vOrigin,&psRY0->m_vDirN, &psRY1->m_vOrigin,&psRY1->m_vDirN ));
}


TgINLINE TYPE V(tgCO_F_Param_RY_RY)( PCU_TYPE pfRY0, PCU_TYPE pfRY1, V(CPCU_TgRAY) psRY0, V(CPCU_TgRAY) psRY1 )
{
    return (F(tgPM_SQRT)( V(tgCO_F_ParamSq_RY_RY)( pfRY0,pfRY1, psRY0,psRY1 ) ));
}


TgINLINE TgBOOL V(tgCO_F_Test_RY_RY)( V(CPCU_TgRAY) psRY0, V(CPCU_TgRAY) psRY1 )
{
    V(C_TgVEC)                          vDS = V(F_SUB_VV)( &psRY1->m_vOrigin, &psRY0->m_vOrigin );
    V(C_TgVEC)                          vNM = V(F_CX)( &psRY0->m_vDirN, &psRY1->m_vDirN );
    V(C_TgVEC)                          vK3 = V(F_CX)( &vNM, &vDS );
    V(C_TgVEC)                          vK4 = V(F_CX)( &vNM, &vDS );
    const TYPE                          fTA = V(F_LSQ)( &vNM );
    const TYPE                          fT0 = V(F_DOT_VV)( &psRY1->m_vDirN, &vK3 ) / fTA;
    const TYPE                          fT1 = V(F_DOT_VV)( &psRY0->m_vDirN, &vK4 ) / fTA;
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT1, &psRY1->m_vDirN );
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fT0, &psRY0->m_vDirN );
    V(C_TgVEC)                          vK5 = V(F_ADD_VV)( &vDS, &vK0 );
    V(C_TgVEC)                          vK6 = V(F_SUB_VV)( &vK5, &vK1 );

    TgASSERT_PARAM(psRY0 != psRY1);

    return (!F(tgCM_NR0)( fTA ) && V(F_LSQ)( &vK6 ) <= F(KTgEPS));
}


// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //
//  Public Functions - Ray - Segment
// -.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. //

TgINLINE TYPE V(tgCO_F_DistSq_RY_SG)( V(CPCU_TgRAY) psRY0, V(CPCU_TgSEGMENT) psSG0 )
{
    TYPE                                fT0,fT1;

    return (V(tgCO_F_ParamSq_RY_SG)( &fT0,&fT1, psRY0, psSG0 ));
}


TgINLINE TYPE V(tgCO_F_Dist_RY_SG)( V(CPCU_TgRAY) psRY0, V(CPCU_TgSEGMENT) psSG0 )
{
    return (F(tgPM_SQRT)( V(tgCO_F_DistSq_RY_SG)( psRY0, psSG0 ) ));
}


TgINLINE TYPE V(tgCO_F_ClosestSq_RY_SG)( V(PCU_TgVEC) pvRY0, V(PCU_TgVEC) pvSG0, V(CPCU_TgRAY) psRY0, V(CPCU_TgSEGMENT) psSG0 )
{
    TYPE                                fT0,fT1;
    const TYPE                          fDistSq = V(tgCO_F_ParamSq_RY_SG)( &fT0,&fT1, psRY0, psSG0 );
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT0, &psRY0->m_vDirN );
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fT1, &psSG0->m_vDirN );

    *pvRY0 = V(F_ADD_VV)( &psRY0->m_vOrigin, &vK0 );
    *pvSG0 = V(F_ADD_VV)( &psSG0->m_vOrigin, &vK1 );

    return (fDistSq);
}


TgINLINE TYPE V(tgCO_F_Closest_RY_SG)( V(PCU_TgVEC) pvRY0, V(PCU_TgVEC) pvSG0, V(CPCU_TgRAY) psRY0, V(CPCU_TgSEGMENT) psSG0 )
{
    return (F(tgPM_SQRT)( V(tgCO_F_ClosestSq_RY_SG)( pvRY0,pvSG0, psRY0,psSG0 ) ));
}


TgINLINE TYPE V(tgCO_F_ParamSq_RY_SG)( PCU_TYPE pfRY0, PCU_TYPE pfSG0, V(CPCU_TgRAY) psRY0, V(CPCU_TgSEGMENT) psSG0 )
{
    return (V(tgCO_F_ParamSq_LR10_LR11)( pfRY0,pfSG0, &psRY0->m_vOrigin,&psRY0->m_vDirN, &psSG0->m_vOrigin,&psSG0->m_vDirN ));
}


TgINLINE TYPE V(tgCO_F_Param_RY_SG)( PCU_TYPE pfRY0, PCU_TYPE pfSG0, V(CPCU_TgRAY) psRY0, V(CPCU_TgSEGMENT) psSG0 )
{
    return (F(tgPM_SQRT)( V(tgCO_F_ParamSq_RY_SG)( pfRY0,pfSG0, psRY0,psSG0 ) ));
}


TgINLINE TgBOOL V(tgCO_F_Test_RY_SG)( V(CPCU_TgRAY) psRY0, V(CPCU_TgSEGMENT) psSG0 )
{
    V(C_TgVEC)                          vDS = V(F_SUB_VV)( &psSG0->m_vOrigin, &psRY0->m_vOrigin );
    V(C_TgVEC)                          vNM = V(F_CX)( &psRY0->m_vDirN, &psSG0->m_vDirN );
    V(C_TgVEC)                          vK3 = V(F_CX)( &vNM, &vDS );
    V(C_TgVEC)                          vK4 = V(F_CX)( &vNM, &vDS );
    const TYPE                          fTA = V(F_LSQ)( &vNM );
    const TYPE                          fT0 = V(F_DOT_VV)( &psSG0->m_vDirN, &vK3 ) / fTA;
    const TYPE                          fT1 = V(F_DOT_VV)( &psRY0->m_vDirN, &vK4 ) / fTA;
    V(C_TgVEC)                          vK0 = V(F_MUL_SV)( fT1, &psSG0->m_vDirN );
    V(C_TgVEC)                          vK1 = V(F_MUL_SV)( fT0, &psRY0->m_vDirN );
    V(C_TgVEC)                          vK5 = V(F_ADD_VV)( &vDS, &vK0 );
    V(C_TgVEC)                          vK6 = V(F_SUB_VV)( &vK5, &vK1 );

    return (!F(tgCM_NR0)( fTA ) && V(F_LSQ)( &vK6 ) <= F(KTgEPS));
}