Commit bd95bd2d authored by Sandesh Venkatesh's avatar Sandesh Venkatesh
Browse files

few functions converted to fixed point in ivas_orient_trk.c 

Quaternion Product, angle, conjugate, Dotproduct converted.
parent e54fb132

lib_com/common_api_types.h

+12 −0
Original line number Diff line number Diff line
@@ -39,6 +39,10 @@ 
#include <stdint.h>

#include <stdio.h>

#include "ivas_error.h"

#ifdef IVAS_FLOAT_FIXED

#include "typedef.h"

#include "basop32.h"

#endif



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

 * Common API constants
@@ -131,6 +135,14 @@ typedef struct 


} IVAS_QUATERNION;



#ifdef IVAS_FLOAT_FIXED

typedef struct

{

    Word32 w_fx, x_fx, y_fx, z_fx;

    Word16 w_qfact, x_qfact, y_qfact, z_qfact;

} IVAS_QUATERNION_FX;

#endif



typedef struct

{

    float x, y, z;

lib_com/ivas_cnst.h

+2 −0
Original line number Diff line number Diff line
@@ -48,6 +48,8 @@ 
#define _180_OVER_PI                            ( 180.0f / EVS_PI )

#ifdef IVAS_FLOAT_FIXED

#define _180_OVER_PI_Q25                         1922527233

#define PI_OVER_4_Q29                            421657440

#define PI_OVER_Q29                              1686629760

#endif



#define SQRT2                                   1.414213562373095f

lib_rend/ivas_orient_trk.c

+276 −4
Original line number Diff line number Diff line
@@ -39,7 +39,10 @@ 
#include <math.h>

#include <assert.h>

#include "wmc_auto.h"



#ifdef IVAS_FLOAT_FIXED

#include "prot_fx1.h"

#include "prot_fx2.h"

#endif



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

 * Local constants
@@ -92,6 +95,56 @@ void QuaternionProduct( 
    return;

}



#ifdef IVAS_FLOAT_FIXED

void QuaternionProduct_fx(

    const IVAS_QUATERNION_FX q1,

    const IVAS_QUATERNION_FX q2,

    IVAS_QUATERNION_FX *const r )

{

    IVAS_QUATERNION_FX tmp;

    Word32 mul_1, mul_2, mul_3, mul_4 = 0;

    mul_1 = Mpy_32_32( q1.w_fx, q2.w_fx );

    mul_2 = Mpy_32_32( q1.x_fx, q2.x_fx );

    mul_3 = Mpy_32_32( q1.y_fx, q2.y_fx );

    mul_4 = Mpy_32_32( q1.z_fx, q2.z_fx );

    mul_3 = L_add( mul_3, mul_4 );

    mul_2 = L_add( mul_2, mul_3 );

    tmp.w_fx = L_sub( mul_1, mul_2 );

    tmp.w_qfact = q1.w_qfact + q2.w_qfact - 31;



    mul_1 = Mpy_32_32( q1.w_fx, q2.x_fx );

    mul_2 = Mpy_32_32( q1.x_fx, q2.w_fx );

    mul_3 = Mpy_32_32( q1.y_fx, q2.z_fx );

    mul_4 = Mpy_32_32( q1.z_fx, q2.y_fx );

    mul_2 = L_add( mul_1, mul_2 );

    mul_3 = L_add( mul_2, mul_3 );

    tmp.x_fx = L_sub( mul_3, mul_4 );

    tmp.x_qfact = q1.w_qfact + q2.x_qfact - 31;



    mul_1 = Mpy_32_32( q1.w_fx, q2.y_fx );

    mul_2 = Mpy_32_32( q1.x_fx, q2.z_fx );

    mul_3 = Mpy_32_32( q1.y_fx, q2.w_fx );

    mul_4 = Mpy_32_32( q1.z_fx, q2.x_fx );

    mul_3 = L_add( mul_1, mul_3 );

    mul_4 = L_add( mul_3, mul_4 );

    tmp.y_fx = L_sub( mul_4, mul_2 );

    tmp.y_qfact = q1.w_qfact + q2.y_qfact - 31;



    mul_1 = Mpy_32_32( q1.w_fx, q2.z_fx );

    mul_2 = Mpy_32_32( q1.x_fx, q2.y_fx );

    mul_3 = Mpy_32_32( q1.y_fx, q2.x_fx );

    mul_4 = Mpy_32_32( q1.z_fx, q2.w_fx );

    mul_2 = L_add( mul_1, mul_2 );

    mul_4 = L_add( mul_2, mul_4 );

    tmp.z_fx = L_sub( mul_4, mul_3 );

    tmp.z_qfact = q1.w_qfact + q2.z_qfact - 31;



    *r = tmp;



    return;

}

#endif



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

 * QuaternionDotProduct()

 *
@@ -105,6 +158,28 @@ static float QuaternionDotProduct( 
    return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;

}



#ifdef IVAS_FLOAT_FIXED

static Word32 QuaternionDotProduct_fx(

    const IVAS_QUATERNION_FX q1,

    const IVAS_QUATERNION_FX q2 )

{

    Word32 a[4], b[4] = { 0 };

    Word32 result = 0;

    a[0] = q1.w_fx;

    a[1] = q1.x_fx;

    a[2] = q1.y_fx;

    a[3] = q1.z_fx;

    b[0] = q2.w_fx;

    b[1] = q2.x_fx;

    b[2] = q2.y_fx;

    b[3] = q2.z_fx;

    for ( int i = 0; i < 4; i++ )

    {

        result = Madd_32_32( result, a[i], b[i] );

    }

    return ( result );

}

#endif



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

 * QuaternionDivision()
@@ -136,8 +211,22 @@ static void QuaternionNormalize( 
    const IVAS_QUATERNION q,

    IVAS_QUATERNION *const r )

{

#ifdef IVAS_FLOAT_FIXED

    IVAS_QUATERNION_FX q_fx;

    Word16 q_dot = 0;

    q_fx.w_qfact = q_fx.x_qfact = q_fx.y_qfact = q_fx.z_qfact = Q29;

    q_fx.w_fx = float_to_fix( q.w, q_fx.w_qfact );

    q_fx.x_fx = float_to_fix( q.x, q_fx.x_qfact );

    q_fx.y_fx = float_to_fix( q.y, q_fx.y_qfact );

    q_fx.z_fx = float_to_fix( q.z, q_fx.z_qfact );

    Word32 dot_prod_fx = QuaternionDotProduct_fx( q_fx, q_fx );

    q_dot = q_fx.w_qfact + q_fx.w_qfact - 31;

    float dot_prod = fix_to_float( dot_prod_fx, q_dot );

    float sqrt = sqrtf( dot_prod );

    QuaternionDivision( q, sqrt, r );

#else

    QuaternionDivision( q, sqrtf( QuaternionDotProduct( q, q ) ), r );



#endif

    return;

}
@@ -155,8 +244,25 @@ void QuaternionSlerp( 
    IVAS_QUATERNION *const r )

{

    float angle, denom, s, s2;



#ifdef IVAS_FLOAT_FIXED

    IVAS_QUATERNION_FX q1_fx, q2_fx;

    Word16 q_dot = 0;

    q1_fx.w_qfact = q1_fx.x_qfact = q1_fx.y_qfact = q1_fx.z_qfact = Q29;

    q2_fx.w_qfact = q2_fx.x_qfact = q2_fx.y_qfact = q2_fx.z_qfact = Q29;

    q1_fx.w_fx = float_to_fix( q1.w, q1_fx.w_qfact);

    q1_fx.x_fx = float_to_fix( q1.x, q1_fx.x_qfact);

    q1_fx.y_fx = float_to_fix( q1.y, q1_fx.y_qfact);

    q1_fx.z_fx = float_to_fix( q1.z, q1_fx.z_qfact);

    q2_fx.w_fx = float_to_fix( q2.w, q2_fx.w_qfact);

    q2_fx.x_fx = float_to_fix( q2.x, q2_fx.x_qfact);

    q2_fx.y_fx = float_to_fix( q2.y, q2_fx.y_qfact);

    q2_fx.z_fx = float_to_fix( q2.z, q2_fx.z_qfact);

    Word32 s_fx = QuaternionDotProduct_fx( q1_fx, q2_fx );

    q_dot = q1_fx.w_qfact + q2_fx.w_qfact - 31;

    s = fix_to_float( s_fx, q_dot );

#else

    s = QuaternionDotProduct( q1, q2 );

#endif



    if ( fabsf( s ) >= 1.0f )

    {
@@ -198,6 +304,22 @@ static void QuaternionConjugate( 
    return;

}



#ifdef IVAS_FLOAT_FIXED

static void QuaternionConjugate_fx(

    const IVAS_QUATERNION_FX q,

    IVAS_QUATERNION_FX *const r )

{

    r->w_fx = q.w_fx;

    r->x_fx = L_negate( q.x_fx );

    r->y_fx = L_negate( q.y_fx );

    r->z_fx = L_negate( q.z_fx );

    r->w_qfact = q.w_qfact;

    r->x_qfact = q.x_qfact;

    r->y_qfact = q.y_qfact;

    r->z_qfact = q.z_qfact;

    return;

}

#endif



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

 * QuaternionAngle()
@@ -219,6 +341,69 @@ static float QuaternionAngle( 
    return angle;

}



#ifdef IVAS_FLOAT_FIXED

static Word32 QuaternionAngle_fx(

    const IVAS_QUATERNION_FX q1,

    const IVAS_QUATERNION_FX q2 )

{

    IVAS_QUATERNION_FX q12;

    Word32 angle = 0;



    QuaternionConjugate_fx( q1, &q12 );

    QuaternionProduct_fx( q12, q2, &q12 ); // q12:Q25, q2:Q29, q1: Q27//

    Word16 sign_bit = ( L_shr( q12.w_fx, 31 ) ) & ( 1 );

    IF( NE_16( sign_bit, 1 ) )

    {

        Word32 temp = 0;

        Word16 q_dot, result_e = 0;

        temp = q12.w_fx;

        q12.w_fx = 0;

        Word32 result = 0;

        q12.x_fx = L_shl( q12.x_fx, ( 31 - q12.x_qfact ) ); // Q31

        q12.y_fx = L_shl( q12.y_fx, ( 31 - q12.y_qfact ) );

        q12.z_fx = L_shl( q12.z_fx, ( 31 - q12.z_qfact ) );

        result = QuaternionDotProduct_fx( q12, q12 );

        q_dot = Q31;

        q12.x_fx = L_shr( q12.x_fx, ( 31 - q12.x_qfact ) ); // original Q

        q12.y_fx = L_shr( q12.y_fx, ( 31 - q12.y_qfact ) );

        q12.z_fx = L_shr( q12.z_fx, ( 31 - q12.z_qfact ) );

        result = Sqrt32( result, &result_e );

        q12.w_fx = temp;

        // Converting numerator to same Q as denominator//

        IF( GT_32( 0, result_e ) )

        {

            result = L_shr( L_shr( result, -1 * ( result_e ) ), ( 31 - q12.x_qfact ) ); // Q25

        }

        ELSE

        {

            IF( GT_32( result_e, ( 31 - q12.x_qfact ) ) )

            {

                result = L_shl( result, ( result_e - ( 31 - q12.x_qfact ) ) );

            }

            ELSE

            {

                result = L_shr( result, ( ( 31 - q12.x_qfact ) - result_e ) );

            }

        }

        IF( GT_32( q12.w_fx, result ) )

        {

            Word16 tan_result = BASOP_util_atan2( result, q12.w_fx, 0 );

            result = L_deposit_h( tan_result ); // Q29

            return result;

        }

        ELSE

        {

            return PI_OVER_Q29;

        }

    }

    ELSE

    {

        angle = PI_OVER_Q29;

    }



    return angle;

}

#endif



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

 * QuaternionInverse()
@@ -231,9 +416,30 @@ void QuaternionInverse( 
    IVAS_QUATERNION *const r )

{

    float dot_product;



#ifdef IVAS_FLOAT_FIXED

    IVAS_QUATERNION_FX q_fx;

    Word16 q_dot = 0;

    q_fx.w_qfact = q_fx.x_qfact = q_fx.y_qfact = q_fx.z_qfact = Q29;

    q_fx.w_fx = float_to_fix( q.w, q_fx.w_qfact );

    q_fx.x_fx = float_to_fix( q.x, q_fx.x_qfact );

    q_fx.y_fx = float_to_fix( q.y, q_fx.y_qfact );

    q_fx.z_fx = float_to_fix( q.z, q_fx.z_qfact );

    Word32 dot_product_fx = QuaternionDotProduct_fx( q_fx, q_fx );

    q_dot = q_fx.w_qfact + q_fx.w_qfact - 31;

    dot_product = fix_to_float( dot_product_fx, q_dot );



    IVAS_QUATERNION_FX *r_fx = malloc( sizeof( IVAS_QUATERNION_FX ) );

    QuaternionConjugate_fx( q_fx, r_fx );

    r->w = fix_to_float( r_fx->w_fx, r_fx->w_qfact );

    r->x = fix_to_float( r_fx->x_fx, r_fx->x_qfact );

    r->y = fix_to_float( r_fx->y_fx, r_fx->y_qfact );

    r->z = fix_to_float( r_fx->z_fx, r_fx->z_qfact );



    free( r_fx );

#else

    dot_product = QuaternionDotProduct( q, q );

    QuaternionConjugate( q, r );

#endif

    QuaternionDivision( *r, dot_product, r );



    return;
@@ -618,7 +824,32 @@ ivas_error ivas_orient_trk_Process( 


            /* Compute relative orientation = (absolute orientation) - (reference orientation) */

            QuaternionInverse( pOTR->refRot, &pOTR->trkRot );

#ifdef IVAS_FLOAT_FIXED

            IVAS_QUATERNION_FX absRot_fx = { 0 };

            ivas_orient_trk_state_t_fx *pOTR_fx = malloc( sizeof( ivas_orient_trk_state_t_fx ) );

            pOTR_fx->trkRot_fx.w_qfact = pOTR_fx->trkRot_fx.x_qfact = pOTR_fx->trkRot_fx.y_qfact = pOTR_fx->trkRot_fx.z_qfact = Q29;

            absRot_fx.w_qfact = absRot_fx.x_qfact = absRot_fx.y_qfact = absRot_fx.z_qfact = Q29;

            pOTR_fx->trkRot_fx.w_fx = float_to_fix( pOTR->trkRot.w, pOTR_fx->trkRot_fx.w_qfact );

            pOTR_fx->trkRot_fx.x_fx = float_to_fix( pOTR->trkRot.x, pOTR_fx->trkRot_fx.x_qfact );

            pOTR_fx->trkRot_fx.y_fx = float_to_fix( pOTR->trkRot.y, pOTR_fx->trkRot_fx.y_qfact );

            pOTR_fx->trkRot_fx.z_fx = float_to_fix( pOTR->trkRot.z, pOTR_fx->trkRot_fx.z_qfact );

            absRot_fx.w_fx = float_to_fix( absRot.w, absRot_fx.w_qfact );

            absRot_fx.x_fx = float_to_fix( absRot.x, absRot_fx.x_qfact );

            absRot_fx.y_fx = float_to_fix( absRot.y, absRot_fx.y_qfact );

            absRot_fx.z_fx = float_to_fix( absRot.z, absRot_fx.z_qfact );

            QuaternionProduct_fx( pOTR_fx->trkRot_fx, absRot_fx, &pOTR_fx->trkRot_fx );

            pOTR->trkRot.w = fix_to_float( pOTR_fx->trkRot_fx.w_fx, pOTR_fx->trkRot_fx.w_qfact);

            pOTR->trkRot.x = fix_to_float( pOTR_fx->trkRot_fx.x_fx, pOTR_fx->trkRot_fx.x_qfact);

            pOTR->trkRot.y = fix_to_float( pOTR_fx->trkRot_fx.y_fx, pOTR_fx->trkRot_fx.y_qfact);

            pOTR->trkRot.z = fix_to_float( pOTR_fx->trkRot_fx.z_fx, pOTR_fx->trkRot_fx.z_qfact);

            absRot.w = fix_to_float( absRot_fx.w_fx, absRot_fx.w_qfact);

            absRot.x = fix_to_float( absRot_fx.x_fx, absRot_fx.x_qfact);

            absRot.y = fix_to_float( absRot_fx.y_fx, absRot_fx.y_qfact);

            absRot.z = fix_to_float( absRot_fx.z_fx, absRot_fx.z_qfact);

            free( pOTR_fx );

#else

            QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot );

#endif

            break;



        case IVAS_HEAD_ORIENT_TRK_AVG:
@@ -627,6 +858,46 @@ ivas_error ivas_orient_trk_Process( 


            /* Compute relative orientation = (absolute orientation) - (average absolute orientation) */

            QuaternionInverse( pOTR->absAvgRot, &pOTR->trkRot );

#ifdef IVAS_FLOAT_FIXED

            IVAS_QUATERNION_FX absRot1_fx = { 0 };

            ivas_orient_trk_state_t_fx *pOTR1_fx = malloc( sizeof( ivas_orient_trk_state_t_fx ) );

            Word32 angle_fx, relativeOrientationRate_fx = 0;

            pOTR1_fx->trkRot_fx.w_qfact = pOTR1_fx->trkRot_fx.x_qfact = pOTR1_fx->trkRot_fx.y_qfact = pOTR1_fx->trkRot_fx.z_qfact = Q29;

            absRot1_fx.w_qfact = absRot1_fx.x_qfact = absRot1_fx.y_qfact = absRot1_fx.z_qfact = Q29;

            pOTR1_fx->trkRot_fx.w_fx = float_to_fix( pOTR->trkRot.w, pOTR1_fx->trkRot_fx.w_qfact );

            pOTR1_fx->trkRot_fx.x_fx = float_to_fix( pOTR->trkRot.x, pOTR1_fx->trkRot_fx.x_qfact );

            pOTR1_fx->trkRot_fx.y_fx = float_to_fix( pOTR->trkRot.y, pOTR1_fx->trkRot_fx.y_qfact );

            pOTR1_fx->trkRot_fx.z_fx = float_to_fix( pOTR->trkRot.z, pOTR1_fx->trkRot_fx.z_qfact );

            absRot1_fx.w_fx = float_to_fix( absRot.w, absRot1_fx.w_qfact );

            absRot1_fx.x_fx = float_to_fix( absRot.x, absRot1_fx.x_qfact );

            absRot1_fx.y_fx = float_to_fix( absRot.y, absRot1_fx.y_qfact );

            absRot1_fx.z_fx = float_to_fix( absRot.z, absRot1_fx.z_qfact );

            QuaternionProduct_fx( pOTR1_fx->trkRot_fx, absRot1_fx, &pOTR1_fx->trkRot_fx );

            angle_fx = QuaternionAngle_fx( absRot1_fx, pOTR1_fx->trkRot_fx ); // Q29

            Word16 result_e = 0;

            Word16 temp_result = BASOP_Util_Divide3232_Scale( angle_fx, PI_OVER_4_Q29, &result_e );

            relativeOrientationRate_fx = L_deposit_h( temp_result );

            Word32 one_fx;

            Word16 temp = result_e;

            f2me( 1.0, &one_fx, &temp);

            IF( GT_32( relativeOrientationRate_fx, one_fx ) )

            {

                relativeOrientationRate = 1.0f;

            }

            ELSE

            {

                relativeOrientationRate = me2f( relativeOrientationRate_fx, result_e );

            }

            pOTR->trkRot.w = fix_to_float( pOTR1_fx->trkRot_fx.w_fx, pOTR1_fx->trkRot_fx.w_qfact );

            pOTR->trkRot.x = fix_to_float( pOTR1_fx->trkRot_fx.x_fx, pOTR1_fx->trkRot_fx.x_qfact );

            pOTR->trkRot.y = fix_to_float( pOTR1_fx->trkRot_fx.y_fx, pOTR1_fx->trkRot_fx.y_qfact );

            pOTR->trkRot.z = fix_to_float( pOTR1_fx->trkRot_fx.z_fx, pOTR1_fx->trkRot_fx.z_qfact );

            absRot.w = fix_to_float( absRot1_fx.w_fx, absRot1_fx.w_qfact );

            absRot.x = fix_to_float( absRot1_fx.x_fx, absRot1_fx.x_qfact );

            absRot.y = fix_to_float( absRot1_fx.y_fx, absRot1_fx.y_qfact );

            absRot.z = fix_to_float( absRot1_fx.z_fx, absRot1_fx.z_qfact );

            free( pOTR1_fx );

#else

            QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot );



            /* Adapt LPF constant based on orientation excursion relative to current mean:
@@ -642,6 +913,7 @@ ivas_error ivas_orient_trk_Process( 
            {

                relativeOrientationRate = 1.0f;

            }

#endif



            /* Compute range of the adaptation rate between center = lower rate and off-center = higher rate */

            rateRange = pOTR->offCenterAdaptationRate - pOTR->centerAdaptationRate;

lib_rend/ivas_stat_rend.h

+18 −0
Original line number Diff line number Diff line
@@ -742,6 +742,24 @@ typedef struct ivas_orient_trk_state_t 


} ivas_orient_trk_state_t;



#ifdef IVAS_FLOAT_FIXED



typedef struct ivas_orient_trk_state_t_fx

{

    IVAS_HEAD_ORIENT_TRK_T orientation_tracking_fx;

    Word16 centerAdaptationRate_fx;

    Word16 offCenterAdaptationRate_fx;

    Word16 adaptationAngle_fx;



    Word16 alpha_fx;

    IVAS_QUATERNION_FX absAvgRot_fx; /* average absolute orientation */

    IVAS_QUATERNION_FX refRot_fx;    /* reference orientation */

    IVAS_QUATERNION_FX trkRot_fx;    /* tracked rotation */



} ivas_orient_trk_state_t_fx;



#endif



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

 * Head rotation data structure

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