Commit 7fd11a77 authored by Sandesh Venkatesh's avatar Sandesh Venkatesh
Browse files

Ported vector operation functions in Ivas_orient_trk.c 

[x] Vector cross & Dot Product,length,Normalise,subtract
are converted to fixed point.
parent ef8595cf

lib_com/common_api_types.h

+9 −0
Original line number Diff line number Diff line
@@ -149,6 +149,15 @@ typedef struct 


} IVAS_VECTOR3;



#ifdef IVAS_FLOAT_FIXED

typedef struct

{

    Word32 x_fx, y_fx, z_fx;

    Word16 x_qfact, y_qfact, z_qfact;



} IVAS_VECTOR3_FX;

#endif



typedef enum

{

    IVAS_HEAD_ORIENT_TRK_NONE,

lib_rend/ivas_orient_trk.c

+195 −0
Original line number Diff line number Diff line
@@ -465,6 +465,24 @@ static IVAS_VECTOR3 VectorSubtract( 
    return result;

}



#ifdef IVAS_FLOAT_FIXED

static IVAS_VECTOR3_FX VectorSubtract_fx(

    const IVAS_VECTOR3_FX p1,

    const IVAS_VECTOR3_FX p2 )

{

    IVAS_VECTOR3_FX result;



    result.x_fx = L_sub( p1.x_fx, p2.x_fx );

    result.y_fx = L_sub( p1.y_fx, p2.y_fx );

    result.z_fx = L_sub( p1.z_fx, p2.z_fx );

    result.x_qfact = p1.x_qfact;

    result.y_qfact = p1.y_qfact;

    result.y_qfact = p1.y_qfact;



    return result;

}

#endif





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

 * VectorCrossProduct()
@@ -484,6 +502,66 @@ static IVAS_VECTOR3 VectorCrossProduct( 
    return result;

}



#ifdef IVAS_FLOAT_FIXED

static IVAS_VECTOR3_FX VectorCrossProduct_fx(

    const IVAS_VECTOR3_FX p1,

    const IVAS_VECTOR3_FX p2 )

{

    IVAS_VECTOR3_FX result_fx;

    Word32 mul1, mul2;

    Word16 diff, q_mul1, q_mul2;

    mul1 = Mpy_32_32( p1.y_fx, p2.z_fx );

    mul2 = Mpy_32_32( p1.z_fx, p2.y_fx );

    q_mul1 = p1.y_qfact + p2.z_qfact - 31;

    q_mul2 = p1.z_qfact + p2.x_qfact - 31;

    IF( GT_16( q_mul1, q_mul2 ) )

    {

        diff = q_mul1 - q_mul2;

        result_fx.x_fx = L_sub( ( L_shr( mul1, diff ) ), mul2 );

        result_fx.x_qfact = q_mul2;

    }

    ELSE

    {

        diff = q_mul2 - q_mul1;

        result_fx.x_fx = L_sub( mul1, ( L_shr( mul2, diff ) ) );

        result_fx.x_qfact = q_mul1;

    }

    mul1 = Mpy_32_32( p1.z_fx, p2.x_fx );

    mul2 = Mpy_32_32( p1.x_fx, p2.z_fx );

    q_mul1 = p1.z_qfact + p2.x_qfact - 31;

    q_mul2 = p1.x_qfact + p2.z_qfact - 31;

    IF( GT_16( q_mul1, q_mul2 ) )

    {

        diff = q_mul1 - q_mul2;

        result_fx.y_fx = L_sub( ( L_shr( mul1, diff ) ), mul2 );

        result_fx.y_qfact = q_mul2;

    }

    ELSE

    {

        diff = q_mul2 - q_mul1;

        result_fx.y_fx = L_sub( mul1, ( L_shr( mul2, diff ) ) );

        result_fx.y_qfact = q_mul1;

    }

    mul1 = Mpy_32_32( p1.x_fx, p2.y_fx );

    mul2 = Mpy_32_32( p1.y_fx, p2.x_fx );

    q_mul1 = p1.x_qfact + p2.y_qfact - 31;

    q_mul2 = p1.y_qfact + p2.x_qfact - 31;

    IF( GT_16( q_mul1, q_mul2 ) )

    {

        diff = q_mul1 - q_mul2;

        result_fx.z_fx = L_sub( ( L_shr( mul1, diff ) ), mul2 );

        result_fx.z_qfact = q_mul2;

    }

    ELSE

    {

        diff = q_mul2 - q_mul1;

        result_fx.z_fx = L_sub( mul1, ( L_shr( mul2, diff ) ) );

        result_fx.z_qfact = q_mul1;

    }



    return result_fx;

}

#endif



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

 * VectorDotProduct(
@@ -498,6 +576,25 @@ static float VectorDotProduct( 
    return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z;

}



#ifdef IVAS_FLOAT_FIXED

static Word32 VectorDotProduct_fx(

    const IVAS_VECTOR3_FX p1,

    const IVAS_VECTOR3_FX p2,

    Word16 *q_fact )

{

    Word32 mul_1, mul_2, mul_3 = 0;

    Word32 result_fx = 0;



    mul_1 = Mpy_32_32( p1.x_fx, p2.x_fx );

    mul_2 = Mpy_32_32( p1.y_fx, p2.y_fx );

    mul_3 = Mpy_32_32( p1.z_fx, p2.z_fx );

    result_fx = L_add( mul_1, mul_2 );

    result_fx = L_add( result_fx, mul_3 );

    *q_fact = p1.x_qfact + p2.x_qfact - 31;



    return result_fx;

}

#endif 



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

 * VectorLength()
@@ -511,6 +608,25 @@ static float VectorLength( 
    return sqrtf( p.x * p.x + p.y * p.y + p.z * p.z );

}



#ifdef IVAS_FLOAT_FIXED

static Word32 VectorLength_fx(

    const IVAS_VECTOR3_FX p,

    Word16 *q_fact )

{

    Word32 mul_1, mul_2, mul_3 = 0;

    Word32 result_fx = 0;

    Word16 result_e = 0;

    mul_1 = Mpy_32_32( p.x_fx, p.x_fx );

    mul_2 = Mpy_32_32( p.y_fx, p.y_fx );

    mul_3 = Mpy_32_32( p.z_fx, p.z_fx );

    result_fx = L_add( mul_1, mul_2 );

    result_fx = L_add( result_fx, mul_3 );

    result_e = 31 - (2 * p.x_qfact - 31);

    result_fx = Sqrt32( result_fx, &result_e );

    *q_fact = 31 - result_e;

    return result_fx;

}

#endif



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

 * VectorNormalize()
@@ -532,6 +648,29 @@ static IVAS_VECTOR3 VectorNormalize( 
    return result;

}



#ifdef IVAS_FLOAT_FIXED

static IVAS_VECTOR3_FX VectorNormalize_fx(

    const IVAS_VECTOR3_FX p )

{

    IVAS_VECTOR3_FX result_fx;

    Word32 length_fx;

    Word16 q_len, scale = 0;

    length_fx = VectorLength_fx( p, &q_len );

    Word16 div;



    div = BASOP_Util_Divide3232_Scale( p.x_fx, length_fx, &scale );

    result_fx.x_fx = L_deposit_h( div );

    result_fx.x_qfact = ( 31 - ( scale + ( q_len - p.x_qfact ) ) ); // e+(e1-e2)//

    div = BASOP_Util_Divide3232_Scale( p.y_fx, length_fx, &scale );

    result_fx.y_fx = L_deposit_h( div );

    result_fx.y_qfact = ( 31 - ( scale + ( q_len - p.y_qfact ) ) );

    div = BASOP_Util_Divide3232_Scale( p.z_fx, length_fx, &scale );

    result_fx.z_fx = L_deposit_h( div );

    result_fx.z_qfact = ( 31 - ( scale + ( q_len - p.z_qfact ) ) );



    return result_fx;

}

#endif 



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

 * VectorRotationToQuaternion()
@@ -547,10 +686,33 @@ static void VectorRotationToQuaternion( 
    float dot_product;

    IVAS_VECTOR3 cross_product, p1_normalized, p2_normalized;



#ifdef IVAS_FLOAT_FIXED

    IVAS_VECTOR3_FX cross_product_fx, p1_normalized_fx, p2_normalized_fx;

    IVAS_VECTOR3_FX p1_fx, p2_fx;

    Word32 dot_product_fx;

    Word16 q_dot;

    p1_fx.x_qfact = p1_fx.y_qfact = p1_fx.z_qfact = Q29;

    p2_fx.x_qfact = p2_fx.y_qfact = p2_fx.z_qfact = Q29;

    p1_fx.x_fx = float_to_fix(p1.x, p1_fx.x_qfact);

    p1_fx.y_fx = float_to_fix(p1.y, p1_fx.y_qfact);

    p1_fx.z_fx = float_to_fix(p1.z, p1_fx.z_qfact);

    p2_fx.x_fx = float_to_fix(p2.x, p2_fx.x_qfact);

    p2_fx.y_fx = float_to_fix(p2.y, p2_fx.y_qfact);

    p2_fx.z_fx = float_to_fix(p2.z, p2_fx.z_qfact);

    p1_normalized_fx = VectorNormalize_fx( p1_fx );

    p2_normalized_fx = VectorNormalize_fx( p2_fx );

    cross_product_fx = VectorCrossProduct_fx( p1_normalized_fx, p2_normalized_fx );

    dot_product_fx = VectorDotProduct_fx( p1_normalized_fx, p2_normalized_fx, &q_dot );

    dot_product = me2f(dot_product_fx, (31 - q_dot));

    cross_product.x = me2f(cross_product_fx.x_fx, (31 - cross_product_fx.x_qfact));

    cross_product.y = me2f(cross_product_fx.y_fx, (31 - cross_product_fx.y_qfact));

    cross_product.z = me2f(cross_product_fx.z_fx, (31 - cross_product_fx.z_qfact));

#else

    p1_normalized = VectorNormalize( p1 );

    p2_normalized = VectorNormalize( p2 );

    cross_product = VectorCrossProduct( p1_normalized, p2_normalized );

    dot_product = VectorDotProduct( p1_normalized, p2_normalized );

#endif



    if ( dot_product < -0.999999 )

    {
@@ -736,6 +898,9 @@ ivas_error ivas_orient_trk_SetReferenceVector( 
    IVAS_VECTOR3 acousticFrontVector, ivasForwardVector;

    IVAS_VECTOR3 listenerPosLevel, refPosLevel;

    float acousticFrontVectorLength;

#ifdef IVAS_FLOAT_FIXED

    IVAS_VECTOR3_FX acousticFrontVector_fx, listenerPos_fx, refPos_fx, listenerPosLevel_fx, refPosLevel_fx;

#endif



    if ( pOTR == NULL )

    {
@@ -748,7 +913,22 @@ ivas_error ivas_orient_trk_SetReferenceVector( 
        case IVAS_HEAD_ORIENT_TRK_REF:

        case IVAS_HEAD_ORIENT_TRK_AVG:

        case IVAS_HEAD_ORIENT_TRK_REF_VEC:

#ifdef IVAS_FLOAT_FIXED

            listenerPos_fx.x_qfact = listenerPos_fx.y_qfact = listenerPos_fx.z_qfact = Q29;

            refPos_fx.x_qfact = refPos_fx.y_qfact = refPos_fx.z_qfact = Q29;

            listenerPos_fx.x_fx = float_to_fix( listenerPos.x, listenerPos_fx.x_qfact );

            listenerPos_fx.y_fx = float_to_fix( listenerPos.y, listenerPos_fx.y_qfact );

            listenerPos_fx.z_fx = float_to_fix( listenerPos.z, listenerPos_fx.z_qfact );

            refPos_fx.x_fx = float_to_fix( refPos.x, refPos_fx.x_qfact );

            refPos_fx.y_fx = float_to_fix( refPos.y, refPos_fx.y_qfact );

            refPos_fx.z_fx = float_to_fix( refPos.z, refPos_fx.z_qfact );

            acousticFrontVector_fx = VectorSubtract_fx( listenerPos_fx, refPos_fx );

            acousticFrontVector.x = me2f( acousticFrontVector_fx.x_fx, 31 - acousticFrontVector_fx.x_qfact );

            acousticFrontVector.y = me2f( acousticFrontVector_fx.y_fx, 31 - acousticFrontVector_fx.y_qfact );

            acousticFrontVector.z = me2f( acousticFrontVector_fx.z_fx, 31 - acousticFrontVector_fx.z_qfact );

#else

            acousticFrontVector = VectorSubtract( listenerPos, refPos );

#endif

            break;

        case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV:

            /* ignore the height difference between listener position and reference position */
@@ -757,7 +937,22 @@ ivas_error ivas_orient_trk_SetReferenceVector( 
            listenerPosLevel.y = listenerPos.y;

            refPosLevel.x = refPos.x;

            refPosLevel.y = refPos.y;

#ifdef IVAS_FLOAT_FIXED

            listenerPosLevel_fx.x_qfact = listenerPosLevel_fx.y_qfact = listenerPosLevel_fx.z_qfact = Q29;

            refPosLevel_fx.x_qfact = refPosLevel_fx.y_qfact = refPosLevel_fx.z_qfact = Q29;

            listenerPosLevel_fx.x_fx = float_to_fix( listenerPosLevel.x, listenerPosLevel_fx.x_qfact );

            listenerPosLevel_fx.y_fx = float_to_fix( listenerPosLevel.y, listenerPosLevel_fx.y_qfact );

            listenerPosLevel_fx.z_fx = float_to_fix( listenerPosLevel.z, listenerPosLevel_fx.z_qfact );

            refPosLevel_fx.x_fx = float_to_fix(refPosLevel.x, refPosLevel_fx.x_qfact );

            refPosLevel_fx.y_fx = float_to_fix(refPosLevel.y, refPosLevel_fx.y_qfact );

            refPosLevel_fx.z_fx = float_to_fix(refPosLevel.z, refPosLevel_fx.z_qfact );

            acousticFrontVector_fx = VectorSubtract_fx( listenerPosLevel_fx, refPosLevel_fx );

            acousticFrontVector.x = me2f( acousticFrontVector_fx.x_fx, 31 - acousticFrontVector_fx.x_qfact );

            acousticFrontVector.y = me2f( acousticFrontVector_fx.y_fx, 31 - acousticFrontVector_fx.y_qfact );

            acousticFrontVector.z = me2f( acousticFrontVector_fx.z_fx, 31 - acousticFrontVector_fx.z_qfact );

#else

            acousticFrontVector = VectorSubtract( listenerPosLevel, refPosLevel );

#endif

            break;

        default:

            return IVAS_ERR_WRONG_PARAMS;