Commit a79a56a9 authored by Fabian Bauer's avatar Fabian Bauer
Browse files

merged VectorNormalize_fx and VectorCrossProduct_fx resp VectorDotProduct_fx

parent 05e1cfa4

lib_rend/ivas_orient_trk_fx.c

+104 −0
Original line number Diff line number Diff line
@@ -614,6 +614,10 @@ static IVAS_VECTOR3 VectorNormalize_fx( 
 * Computes a quaternion representing the rotation from vector p1 to vector p2

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



#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING

#define FIX_2398_PRECISSION_ORIENTATION_TRACKING_NormalizeVectorCrossProduct_VectorDotProduct

#endif



static void VectorRotationToQuaternion_fx(

    const IVAS_VECTOR3 p1,

    const IVAS_VECTOR3 p2,
@@ -623,10 +627,110 @@ static void VectorRotationToQuaternion_fx( 
    Word32 dot_product_fx;

    Word16 q_dot, e_add, q_result;



#ifdef FIX_2398_PRECISSION_ORIENTATION_TRACKING_NormalizeVectorCrossProduct_VectorDotProduct

    {



        IVAS_VECTOR3 result1_fx, result2_fx;

        Word32 length_fx;

        Word16 q_len, scale = 0, result1_e_x, result2_e_x, result1_e_y, result2_e_y, result1_e_z, result2_e_z, q_result;

        Word16 cross_product_e_x, cross_product_e_y, cross_product_e_z;

        move16();

        length_fx = VectorLength_fx( p1, &q_len );



        result1_fx.x_fx = BASOP_Util_Divide3232_Scale_newton( p1.x_fx, length_fx, &scale );

        move32();

        result1_e_x = add( scale, sub( q_len, p1.q_fact ) ); // e+(e1-e2)//



        result1_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p1.y_fx, length_fx, &scale );

        move32();

        result1_e_y = add( scale, sub( q_len, p1.q_fact ) );



        result1_fx.z_fx = BASOP_Util_Divide3232_Scale_newton( p1.z_fx, length_fx, &scale );

        move32();

        result1_e_z = add( scale, sub( q_len, p1.q_fact ) );



#if 0

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

            result1_fx.x_fx = L_shr( result1_fx.x_fx, sub( x_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.y_fx = L_shr( result1_fx.y_fx, sub( y_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.z_fx = L_shr( result1_fx.z_fx, sub( z_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.q_fact = q_result;

            move16();

#endif



        length_fx = VectorLength_fx( p2, &q_len );



        result2_fx.x_fx = BASOP_Util_Divide3232_Scale_newton( p2.x_fx, length_fx, &scale );

        move32();

        result2_e_x = add( scale, sub( q_len, p2.q_fact ) ); // e+(e1-e2)//



        result2_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p2.y_fx, length_fx, &scale );

        move32();

        result2_e_y = add( scale, sub( q_len, p2.q_fact ) );



        result2_fx.z_fx = BASOP_Util_Divide3232_Scale_newton( p2.z_fx, length_fx, &scale );

        move32();

        result2_e_z = add( scale, sub( q_len, p2.q_fact ) );



#if 0

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

            result1_fx.x_fx = L_shr( result1_fx.x_fx, sub( x_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.y_fx = L_shr( result1_fx.y_fx, sub( y_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.z_fx = L_shr( result1_fx.z_fx, sub( z_qfact, q_result ) ); // Q: q_result

            move32();

            result1_fx.q_fact = q_result;

            move16();

#endif



        // static IVAS_VECTOR3 VectorCrossProduct_fx(                const IVAS_VECTOR3 p1,                const IVAS_VECTOR3 p2 )



        cross_product_fx.x_fx = BASOP_Util_Add_Mant32Exp( Mpy_32_32( result1_fx.y_fx, result2_fx.z_fx ), add( result1_e_y, result2_e_z ), L_negate( Mpy_32_32( result1_fx.z_fx, result2_fx.y_fx ) ), add( result1_e_z, result2_e_y ), &cross_product_e_x );

        move32();

        cross_product_fx.y_fx = BASOP_Util_Add_Mant32Exp( Mpy_32_32( result1_fx.z_fx, result2_fx.x_fx ), add( result1_e_z, result2_e_x ), L_negate( Mpy_32_32( result1_fx.x_fx, result2_fx.z_fx ) ), add( result1_e_x, result2_e_z ), &cross_product_e_y );

        move32();

        cross_product_fx.z_fx = BASOP_Util_Add_Mant32Exp( Mpy_32_32( result1_fx.x_fx, result2_fx.y_fx ), add( result1_e_x, result2_e_y ), L_negate( Mpy_32_32( result1_fx.y_fx, result2_fx.x_fx ) ), add( result1_e_y, result2_e_x ), &cross_product_e_z );

        move32();

        scale = s_max( cross_product_e_x, ( s_max( cross_product_e_y, cross_product_e_z ) ) );



        cross_product_fx.x_fx = L_shr( cross_product_fx.x_fx, sub( scale, cross_product_e_x ) );

        move32();

        cross_product_fx.y_fx = L_shr( cross_product_fx.y_fx, sub( scale, cross_product_e_y ) );

        move32();

        cross_product_fx.z_fx = L_shr( cross_product_fx.z_fx, sub( scale, cross_product_e_z ) );

        move32();



        cross_product_fx.q_fact = sub( 31, scale );

        move16();



        // return result_fx;



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

        //static Word32 VectorDotProduct_fx(            const IVAS_VECTOR3 p1,            const IVAS_VECTOR3 p2,            Word16 *q_fact )

        {

            Word32 xx, yy, zz, dot0;

            xx = Mpy_32_32( result1_fx.x_fx, result2_fx.x_fx );

            yy = Mpy_32_32( result1_fx.y_fx, result2_fx.y_fx );

            zz = Mpy_32_32( result1_fx.z_fx, result2_fx.z_fx );



            dot0 = BASOP_Util_Add_Mant32Exp( xx, add( result1_e_x, result2_e_x ), yy, add( result1_e_y, result2_e_y ), &scale );

            dot_product_fx = BASOP_Util_Add_Mant32Exp( dot0, scale, zz, add( result1_e_z, result2_e_z ), &q_dot );



            q_dot = sub( 31, q_dot );

        }



    }

#else

    p1_normalized_fx = VectorNormalize_fx( p1 );

    p2_normalized_fx = VectorNormalize_fx( p2 );



    cross_product_fx = VectorCrossProduct_fx( p1_normalized_fx, p2_normalized_fx );

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

#endif

    // dot & cross product are same q//



    Word32 comp_fx = -2147481472; //-0.999999f in Q31