Commit 2f3e3e7a authored by Sandesh Venkatesh's avatar Sandesh Venkatesh
Browse files

Merge branch 'quat_slerp_update' into 'main' 

Issue 738: Quaternion_slerp simplification

See merge request !345
parents 8fa7aaa7 329a6c52

lib_rend/ivas_orient_trk.c

+10 −29
Original line number Diff line number Diff line
@@ -322,14 +322,14 @@ void QuaternionSlerp_fx( 
    const Word32 t_fx,

    IVAS_QUATERNION *const r_fx )

{

    IVAS_QUATERNION r1, r2;

    IVAS_QUATERNION r1, r2, tmp_quat;

    Word32 sinPhi, cosPhi, temp_32;

    Word16 q_min, sin_e, phi, s1, s2, temp_16, e_div;

    Word16 q_min, sin_e, phi, s1, s2, temp_16;



    QuaternionNormalize_fx( q1_fx, &r1 );

    QuaternionNormalize_fx( q2_fx, &r2 );



    Word16 w_qfact, x_qfact, y_qfact, z_qfact, q_result, q_dot = 0;

    Word16 q_dot = 0;

    cosPhi = QuaternionDotProduct_fx( r1, r2, &q_dot );



    q_min = s_min( r1.q_fact, r2.q_fact );
@@ -380,32 +380,13 @@ void QuaternionSlerp_fx( 
        temp_16 = extract_h( temp_32 );                 // Q13

        s2 = getSineWord16R2( mult( temp_16, 20860 ) ); // Q15



        temp_32 = L_add( Mpy_32_16_1( r1.w_fx, s1 ), Mpy_32_16_1( r2.w_fx, s2 ) ); // q_min

        r_fx->w_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div );

        e_div = e_div + ( 31 - q_min - sin_e );

        w_qfact = 31 - e_div;



        temp_32 = L_add( Mpy_32_16_1( r1.x_fx, s1 ), Mpy_32_16_1( r2.x_fx, s2 ) ); // q_min

        r_fx->x_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div );

        e_div = e_div + ( 31 - q_min - sin_e );

        x_qfact = 31 - e_div;



        temp_32 = L_add( Mpy_32_16_1( r1.y_fx, s1 ), Mpy_32_16_1( r2.y_fx, s2 ) ); // q_min

        r_fx->y_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div );

        e_div = e_div + ( 31 - q_min - sin_e );

        y_qfact = 31 - e_div;



        temp_32 = L_add( Mpy_32_16_1( r1.z_fx, s1 ), Mpy_32_16_1( r2.z_fx, s2 ) ); // q_min

        r_fx->z_fx = BASOP_Util_Divide3232_Scale_cadence( temp_32, sinPhi, &e_div );

        e_div = e_div + ( 31 - q_min - sin_e );

        z_qfact = 31 - e_div;



        q_result = s_min( s_min( w_qfact, x_qfact ), s_min( y_qfact, z_qfact ) );

        r_fx->w_fx = L_shr( r_fx->w_fx, sub( w_qfact, q_result ) );

        r_fx->x_fx = L_shr( r_fx->x_fx, sub( x_qfact, q_result ) );

        r_fx->y_fx = L_shr( r_fx->y_fx, sub( y_qfact, q_result ) );

        r_fx->z_fx = L_shr( r_fx->z_fx, sub( z_qfact, q_result ) );

        r_fx->q_fact = q_result;

        tmp_quat.w_fx = L_add( Mpy_32_16_1( r1.w_fx, s1 ), Mpy_32_16_1( r2.w_fx, s2 ) ); // q_min

        tmp_quat.x_fx = L_add( Mpy_32_16_1( r1.x_fx, s1 ), Mpy_32_16_1( r2.x_fx, s2 ) ); // q_min

        tmp_quat.y_fx = L_add( Mpy_32_16_1( r1.y_fx, s1 ), Mpy_32_16_1( r2.y_fx, s2 ) ); // q_min

        tmp_quat.z_fx = L_add( Mpy_32_16_1( r1.z_fx, s1 ), Mpy_32_16_1( r2.z_fx, s2 ) ); // q_min

        tmp_quat.q_fact = q_min;



        QuaternionDivision_fx( tmp_quat, sinPhi, r_fx, sin_e );

    }



    QuaternionNormalize_fx( *r_fx, r_fx );