From 3ee91cc0dc590f3430e7416a0d1f4a3345aa9747 Mon Sep 17 00:00:00 2001 From: Sandesh Venkatesh Date: Sun, 11 Feb 2024 10:43:36 +0530 Subject: [PATCH] Functions in ivas_vbap.c converted to fixed point. --- lib_dec/ivas_dirac_dec.c | 33 +- lib_rend/ivas_prot_rend.h | 11 + lib_rend/ivas_stat_rend.h | 14 +- lib_rend/ivas_vbap.c | 2890 +++++++++++++++++++++++++++++++------ 4 files changed, 2476 insertions(+), 472 deletions(-) diff --git a/lib_dec/ivas_dirac_dec.c b/lib_dec/ivas_dirac_dec.c index 6a407c3e3..ca086537e 100644 --- a/lib_dec/ivas_dirac_dec.c +++ b/lib_dec/ivas_dirac_dec.c @@ -138,6 +138,17 @@ static ivas_error ivas_dirac_rend_config( DIRAC_REND_HANDLE hDirACRend; SPAT_PARAM_REND_COMMON_DATA_HANDLE hSpatParamRendCom; +#ifdef IVAS_FLOAT_FIXED + Word32 ls_azimuth_fx[MAX_OUTPUT_CHANNELS]; + Word32 ls_elevation_fx[MAX_OUTPUT_CHANNELS]; + FOR ( i = 0; i < MAX_OUTPUT_CHANNELS; i++) { + ls_azimuth[i] = 0.0f; + ls_elevation[i] = 0.0f; + ls_azimuth_fx[i] = 0; + ls_elevation_fx[i] = 0; + } +#endif + flag_config = ( flag_config_inp == DIRAC_RECONFIGURE_MODE ) ? DIRAC_RECONFIGURE : flag_config_inp; error = IVAS_ERR_OK; @@ -270,7 +281,7 @@ static ivas_error ivas_dirac_rend_config( Word16 *frequency_axis_fx = (Word16 *) malloc(hSpatParamRendCom->num_freq_bands * sizeof( Word16 ) ); ivas_dirac_dec_get_frequency_axis_fx( frequency_axis_fx, output_Fs, hSpatParamRendCom->num_freq_bands ); - FOR (Word16 i = 0; i < hSpatParamRendCom->num_freq_bands; i++) + FOR ( i = 0; i < hSpatParamRendCom->num_freq_bands; i++) { hDirACRend->frequency_axis[i] = ( float ) frequency_axis_fx[i]; } @@ -543,15 +554,31 @@ static ivas_error ivas_dirac_rend_config( if ( hDirACRend->panningConf == DIRAC_PANNING_VBAP ) { +#ifdef IVAS_FLOAT_FIXED + for ( i = 0; i < MAX_OUTPUT_CHANNELS; i++) { + ls_azimuth_fx[i] = float_to_fix(ls_azimuth[i], Q22); + ls_elevation_fx[i] = float_to_fix(ls_elevation[i], Q22); + } if ( flag_config == DIRAC_RECONFIGURE && st_ivas->hVBAPdata != NULL ) { - vbap_free_data( &( st_ivas->hVBAPdata ) ); + vbap_free_data_fx( &( st_ivas->hVBAPdata ) ); } - if ( ( error = vbap_init_data( &( st_ivas->hVBAPdata ), ls_azimuth, ls_elevation, nchan_out_woLFE, st_ivas->ivas_format ) ) != IVAS_ERR_OK ) + if ( ( error = vbap_init_data_fx( &( st_ivas->hVBAPdata ), ls_azimuth_fx, ls_elevation_fx, nchan_out_woLFE, st_ivas->ivas_format ) ) != IVAS_ERR_OK ) { return error; } +#else + if (flag_config == DIRAC_RECONFIGURE && st_ivas->hVBAPdata != NULL) + { + vbap_free_data(&(st_ivas->hVBAPdata)); + } + + if ((error = vbap_init_data(&(st_ivas->hVBAPdata), ls_azimuth, ls_elevation, nchan_out_woLFE, st_ivas->ivas_format)) != IVAS_ERR_OK) + { + return error; + } +#endif } else if ( hDirACRend->synthesisConf == DIRAC_SYNTHESIS_MONO ) { diff --git a/lib_rend/ivas_prot_rend.h b/lib_rend/ivas_prot_rend.h index 458d0c03b..d3a8e5543 100644 --- a/lib_rend/ivas_prot_rend.h +++ b/lib_rend/ivas_prot_rend.h @@ -262,6 +262,14 @@ void vbap_determine_gains( const int16_t use_object_mode /* i : select between object mode panning and spatial mode panning */ ); #ifdef IVAS_FLOAT_FIXED +ivas_error vbap_init_data_fx( + VBAP_HANDLE *hVBAPdata, /* i/o: handle for VBAP data structure that will be initialized */ + const Word32 *speaker_node_azi_deg, /* i : vector of speaker node azimuths (positive left) */ + const Word32 *speaker_node_ele_deg, /* i : vector of speaker node elevations (positive up) */ + const Word16 num_speaker_nodes, /* i : number of speaker nodes in the set */ + const IVAS_FORMAT ivas_format /* i : IVAS format */ +); + void vbap_determine_gains_fx( const VBAP_HANDLE hVBAPdata, /* i : prepared VBAP structure */ Word32 *gains_fx, /* o : gain vector for loudspeakers for given direction */ @@ -269,6 +277,9 @@ void vbap_determine_gains_fx( const Word16 ele_deg, /* i : elevation in degrees for panning direction (positive up) */ const Word16 use_object_mode /* i : select between object mode panning and spatial mode panning */ ); +void vbap_free_data_fx( + VBAP_HANDLE *hVBAPdata /* i/o: VBAP handle to be freed */ +); #endif diff --git a/lib_rend/ivas_stat_rend.h b/lib_rend/ivas_stat_rend.h index ba4b5d5aa..316dd97d9 100644 --- a/lib_rend/ivas_stat_rend.h +++ b/lib_rend/ivas_stat_rend.h @@ -493,6 +493,7 @@ typedef struct vbap_vs_triplet_structure #ifdef IVAS_FLOAT_FIXED Word32 inverse_matrix_fx[3][3]; Word16 q_inverse_matrix; + Word16 exp_inv_matrix; #endif } VBAP_VS_TRIPLET; @@ -525,14 +526,13 @@ typedef struct vbap_data_structure float *object_mode_top_virtual_speaker_node_division_gains; float *object_mode_back_virtual_speaker_node_division_gains; #ifdef IVAS_FLOAT_FIXED - Word32 *bottom_virtual_speaker_node_division_gains_fx; - Word32 *top_virtual_speaker_node_division_gains_fx; - Word32 *back_virtual_speaker_node_division_gains_fx; - Word32 *object_mode_bottom_virtual_speaker_node_division_gains_fx; - Word32 *object_mode_top_virtual_speaker_node_division_gains_fx; - Word32 *object_mode_back_virtual_speaker_node_division_gains_fx; + Word16 *bottom_virtual_speaker_node_division_gains_fx; + Word16 *top_virtual_speaker_node_division_gains_fx; + Word16 *back_virtual_speaker_node_division_gains_fx; + Word16 *object_mode_bottom_virtual_speaker_node_division_gains_fx; + Word16 *object_mode_top_virtual_speaker_node_division_gains_fx; + Word16 *object_mode_back_virtual_speaker_node_division_gains_fx; #endif - } VBAP_DATA, *VBAP_HANDLE; diff --git a/lib_rend/ivas_vbap.c b/lib_rend/ivas_vbap.c index e33555d32..2ee455c7c 100644 --- a/lib_rend/ivas_vbap.c +++ b/lib_rend/ivas_vbap.c @@ -40,6 +40,8 @@ #include "ivas_prot_rend.h" #include "ivas_stat_dec.h" #include "wmc_auto.h" +#include "prot_fx1.h" +#include "prot_fx2.h" /*-----------------------------------------------------------------------* * Local constants @@ -54,6 +56,8 @@ #define VBAP_MAX_NUM_SPEAKER_NODES 128 #define VBAP_MAX_NUM_TRIPLETS 256 #define VBAP_EPSILON 0.001f /* The fairly large epsilon is for detecting planes etc and accounts for rounding issues */ +#define VBAP_EPSILON_Q3O 1073741 +#define VBAP_EPSILON_Q31 2147483 #define VBAP_MAX_PLANES 50 #define VBAP_MAX_HORIZONTAL_GAP_FOR_PLANE_DETECTION 140.0f /* If a speaker node is found @@ -70,6 +74,7 @@ #define VBAP_MAX_HORIZONTAL_GAP 170u #define VBAP_SEARCH_SECTOR_SIZE ( 360.0f / ( VBAP_NUM_SEARCH_SECTORS ) ) +#define VBAP_SEARCH_SECTOR_SIZE_Q0 90 enum VirtualSpeakerNodeType @@ -92,6 +97,10 @@ typedef struct connection_option int16_t chB; float arc; float arc_weighted; +#ifdef IVAS_FLOAT_FIXED + Word32 arc_weighted_fx; + Word32 arc_fx; +#endif } ConnectionOption; enum SpeakerNodeGroup @@ -108,6 +117,11 @@ typedef struct vbap_speaker_node_structure { float azi_deg; float ele_deg; +#ifdef IVAS_FLOAT_FIXED + Word32 azi_deg_fx; + Word32 ele_deg_fx; + Word32 unit_vec_fx[3]; +#endif float unit_vec[3]; enum SpeakerNodeGroup group; @@ -140,66 +154,96 @@ static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node( VBAP_HAND static int16_t determine_best_triplet_and_gains( VBAP_SEARCH_STRUCT *search_struct, const float panning_unit_vec[3], const int16_t azi_deg, float gains[3] ); +static void determine_virtual_speaker_node_division_gains(const int16_t virtual_speaker_node_index, float *virtual_node_division_gains, int16_t connections[][2], const enum VirtualSpeakerNodeType type, const int16_t max_num_connections, const int16_t num_speaker_nodes, const int16_t use_object_mode); + +static void reorder_triplets(VBAP_VS_TRIPLET *triplets, const int16_t *target_order, const int16_t num_triplets); + #ifdef IVAS_FLOAT_FIXED -static Word16 determine_best_triplet_and_gains_fx( VBAP_SEARCH_STRUCT *search_struct, const Word16 panning_unit_vec_fx[3], const Word16 azi_deg, Word32 gains_fx[3] ); -#endif +static UWord8 vector_matrix_multiply_3x3_fx( const Word16 *src_vector, Word32 matrix[3][3], Word32 *result, Word16 q_matrix ); + +static UWord8 vector_matrix_multiply_3x3_32_fx( const Word32 *src_vector, Word32 matrix[3][3], Word32 *result, Word16 q_matrix ); + +static void init_speaker_node_direction_data_fx( VBAP_SPEAKER_NODE *speaker_node_data, const Word32 *speaker_node_azi_deg_fx, const Word32 *speaker_node_ele_deg_fx, const int16_t num_speaker_nodes ); + +static int16_t determine_virtual_surface_triplets_fx( const int16_t num_speaker_nodes, const VBAP_SPEAKER_NODE *speaker_node_data, int16_t connections[][2], const int16_t max_num_connections, VBAP_VS_TRIPLET *triplets, int16_t initial_search_indices[VBAP_NUM_SEARCH_SECTORS], enum SpeakerNodeGroup allowed_group ); + +static void determine_initial_search_indices_fx( const int16_t num_triplets, const Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS], int16_t initial_search_indices[VBAP_NUM_SEARCH_SECTORS] ); + +static ivas_error determine_connections_fx( const Word16 num_speaker_nodes, const VBAP_SPEAKER_NODE *speaker_node_data, Word16 connections[][2], const Word16 max_num_connections, Word16 *group1_count, Word16 *group2_start, int16_t *group2_count ); + +static void formulate_horizontal_connections_fx( const VBAP_SPEAKER_NODE *speaker_node_data, const Word16 num_speaker_nodes, Word16 connections[][2], Word16 *connection_write_index ); + +static ivas_error get_half_sphere_connection_options_fx( const VBAP_SPEAKER_NODE *speaker_node_data, const enum SpeakerNodeGroup group, const Word16 num_speaker_nodes, const Word16 num_non_crossing_planes, const Word32 *non_crossing_plane_elevation_deg_fx, ConnectionOption **connection_options_pr, Word16 *num_connection_options ); -static void determine_virtual_speaker_node_division_gains( const int16_t virtual_speaker_node_index, float *virtual_node_division_gains, int16_t connections[][2], const enum VirtualSpeakerNodeType type, const int16_t max_num_connections, const int16_t num_speaker_nodes, const int16_t use_object_mode ); +static ivas_error formulate_half_sphere_connections_fx( const VBAP_SPEAKER_NODE *speaker_node_data, const Word16 num_speaker_nodes, const enum SpeakerNodeGroup group, Word16 connections[][2], Word16 *connection_write_index, const Word16 max_num_connections, const Word16 num_non_crossing_planes, const Word32 *non_crossing_plane_elevation_deg_fx ); -static void reorder_triplets( VBAP_VS_TRIPLET *triplets, const int16_t *target_order, const int16_t num_triplets ); +static Word16 determine_non_crossing_planes_fx( const Word16 num_speaker_nodes, const VBAP_SPEAKER_NODE *node_data, Word32 *non_crossing_plane_elevation_deg_fx ); +static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node_fx( VBAP_HANDLE hVBAPdata, const Word32 *speaker_node_azi_deg, const Word32 *speaker_node_ele_deg, enum SpeakerNodeGroup group ); + +static Word16 determine_best_triplet_and_gains_fx( VBAP_SEARCH_STRUCT *search_struct, const Word16 panning_unit_vec_fx[3], const Word16 azi_deg, Word32 gains_fx[3] ); + +static void determine_virtual_speaker_node_division_gains_fx( const Word16 virtual_speaker_node_index, Word16 *virtual_node_division_gains_fx, Word16 *max_exp, int16_t connections[][2], const enum VirtualSpeakerNodeType type, const Word16 max_num_connections, const Word16 num_speaker_nodes, const Word16 use_object_mode ); + +static void reorder_triplets_fx( VBAP_VS_TRIPLET *triplets, const Word16 *target_order, const Word16 num_triplets ); +#endif /*-------------------------------------------------------------------------* * vbap_init_data() * * Initialize VBAP data structure for the speaker node set *-------------------------------------------------------------------------*/ - -ivas_error vbap_init_data( - VBAP_HANDLE *hVBAPdata, /* i/o: handle for VBAP data structure that will be initialized */ - const float *speaker_node_azi_deg, /* i : vector of speaker node azimuths (positive left) */ - const float *speaker_node_ele_deg, /* i : vector of speaker node elevations (positive up) */ - const int16_t num_speaker_nodes, /* i : number of speaker nodes in the set */ - const IVAS_FORMAT ivas_format /* i : IVAS format */ +#ifdef IVAS_FLOAT_FIXED +ivas_error vbap_init_data_fx( + VBAP_HANDLE *hVBAPdata, /* i/o: handle for VBAP data structure that will be initialized */ + const Word32 *speaker_node_azi_deg_fx, /* i : vector of speaker node azimuths (positive left) */ + const Word32 *speaker_node_ele_deg_fx, /* i : vector of speaker node elevations (positive up) */ + const Word16 num_speaker_nodes, /* i : number of speaker nodes in the set */ + const IVAS_FORMAT ivas_format /* i : IVAS format */ ) { /* Variables */ - int16_t connections[VBAP_MAX_NUM_SPEAKER_NODES][2]; - int16_t max_num_connections; - int16_t is_success; - int16_t connection_group1_count; - int16_t connection_group2_start; - int16_t connection_group2_count; + Word16 connections[VBAP_MAX_NUM_SPEAKER_NODES][2]; + Word16 max_num_connections; + Word16 is_success; + Word16 connection_group1_count; + Word16 connection_group2_start; + Word16 connection_group2_count; enum VirtualSpeakerNodeType virtual_top_type; enum VirtualSpeakerNodeType virtual_bottom_type; enum VirtualSpeakerNodeType virtual_back_type; - float speaker_node_azi_deg_internal[VBAP_MAX_NUM_SPEAKER_NODES]; - float speaker_node_ele_deg_internal[VBAP_MAX_NUM_SPEAKER_NODES]; + + Word32 speaker_node_azi_deg_internal_fx[VBAP_MAX_NUM_SPEAKER_NODES]; + Word32 speaker_node_ele_deg_internal_fx[VBAP_MAX_NUM_SPEAKER_NODES]; + VBAP_SPEAKER_NODE speaker_node_data[VBAP_MAX_NUM_SPEAKER_NODES]; VBAP_DATA *vbap; ivas_error error; push_wmops( "vbap_init" ); + set32_fx( speaker_node_azi_deg_internal_fx, 0, VBAP_MAX_NUM_SPEAKER_NODES ); + set32_fx( speaker_node_ele_deg_internal_fx, 0, VBAP_MAX_NUM_SPEAKER_NODES ); + /* Basic init checks */ /* If the requested layout is invalid, hVBAPdata is set to NULL and the signal will * be distributed with an equal gain into all output channels. * The surrounding code needs to handle the NULL pointer properly. */ - if ( num_speaker_nodes > VBAP_MAX_NUM_SPEAKER_NODES || num_speaker_nodes < 3 ) + IF( num_speaker_nodes > VBAP_MAX_NUM_SPEAKER_NODES || num_speaker_nodes < 3 ) { hVBAPdata = NULL; pop_wmops(); return IVAS_ERR_OK; } - if ( !speaker_node_azi_deg || !speaker_node_ele_deg ) + IF( !speaker_node_azi_deg_fx || !speaker_node_ele_deg_fx ) { hVBAPdata = NULL; return IVAS_ERR_OK; } /* Allocate VBAP structure */ - if ( ( vbap = (VBAP_HANDLE) malloc( sizeof( VBAP_DATA ) ) ) == NULL ) + IF( ( vbap = (VBAP_HANDLE) malloc( sizeof( VBAP_DATA ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } @@ -208,320 +252,705 @@ ivas_error vbap_init_data( vbap->bottom_virtual_speaker_node_index = -1; vbap->top_virtual_speaker_node_index = -1; vbap->back_virtual_speaker_node_index = -1; - vbap->bottom_virtual_speaker_node_division_gains = NULL; - vbap->top_virtual_speaker_node_division_gains = NULL; - vbap->back_virtual_speaker_node_division_gains = NULL; - vbap->object_mode_bottom_virtual_speaker_node_division_gains = NULL; - vbap->object_mode_top_virtual_speaker_node_division_gains = NULL; - vbap->object_mode_back_virtual_speaker_node_division_gains = NULL; -#ifdef IVAS_FLOAT_FIXED + vbap->num_speaker_nodes = num_speaker_nodes; + vbap->num_speaker_nodes_internal = num_speaker_nodes; vbap->bottom_virtual_speaker_node_division_gains_fx = NULL; vbap->top_virtual_speaker_node_division_gains_fx = NULL; vbap->back_virtual_speaker_node_division_gains_fx = NULL; vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx = NULL; vbap->object_mode_top_virtual_speaker_node_division_gains_fx = NULL; vbap->object_mode_back_virtual_speaker_node_division_gains_fx = NULL; +#ifdef TRUE + /*Clean up float code*/ + vbap->bottom_virtual_speaker_node_division_gains = NULL; + vbap->top_virtual_speaker_node_division_gains = NULL; + vbap->back_virtual_speaker_node_division_gains = NULL; + vbap->object_mode_bottom_virtual_speaker_node_division_gains = NULL; + vbap->object_mode_top_virtual_speaker_node_division_gains = NULL; + vbap->object_mode_back_virtual_speaker_node_division_gains = NULL; #endif - vbap->num_speaker_nodes = num_speaker_nodes; - vbap->num_speaker_nodes_internal = num_speaker_nodes; - /* Check if the speaker node setup needs a virtual top or bottom node (function also increments vbap->num_speaker_nodes_internal when necessary) */ - virtual_bottom_type = check_need_of_virtual_speaker_node( vbap, speaker_node_azi_deg, speaker_node_ele_deg, SPEAKER_NODE_BOTTOM_HALF ); - virtual_top_type = check_need_of_virtual_speaker_node( vbap, speaker_node_azi_deg, speaker_node_ele_deg, SPEAKER_NODE_TOP_HALF ); - virtual_back_type = check_need_of_virtual_speaker_node( vbap, speaker_node_azi_deg, speaker_node_ele_deg, SPEAKER_NODE_BACK ); + virtual_bottom_type = check_need_of_virtual_speaker_node_fx( vbap, speaker_node_azi_deg_fx, speaker_node_ele_deg_fx, SPEAKER_NODE_BOTTOM_HALF ); + virtual_top_type = check_need_of_virtual_speaker_node_fx( vbap, speaker_node_azi_deg_fx, speaker_node_ele_deg_fx, SPEAKER_NODE_TOP_HALF ); + virtual_back_type = check_need_of_virtual_speaker_node_fx( vbap, speaker_node_azi_deg_fx, speaker_node_ele_deg_fx, SPEAKER_NODE_BACK ); /* Init internal speaker node configuration, which is the original configuration potentially appended with virtual top and/or bottom loudspeakers */ - mvr2r( speaker_node_azi_deg, speaker_node_azi_deg_internal, num_speaker_nodes ); - mvr2r( speaker_node_ele_deg, speaker_node_ele_deg_internal, num_speaker_nodes ); + Copy32( speaker_node_azi_deg_fx, speaker_node_azi_deg_internal_fx, num_speaker_nodes ); + Copy32( speaker_node_ele_deg_fx, speaker_node_ele_deg_internal_fx, num_speaker_nodes ); - if ( is_success && virtual_bottom_type != NO_VIRTUAL_SPEAKER_NODE ) + IF( is_success && virtual_bottom_type != NO_VIRTUAL_SPEAKER_NODE ) { - if ( ( vbap->bottom_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) + IF( ( vbap->bottom_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } - set_zero( vbap->bottom_virtual_speaker_node_division_gains, num_speaker_nodes ); -#ifdef IVAS_FLOAT_FIXED - IF ( ( vbap->bottom_virtual_speaker_node_division_gains_fx = (Word32 *) malloc( num_speaker_nodes * sizeof( Word32 ) ) ) == NULL ) + set16_fx( vbap->bottom_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); + is_success &= vbap->bottom_virtual_speaker_node_division_gains_fx != NULL; + + IF( ivas_format == MASA_ISM_FORMAT ) + { + IF( ( vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL ) + { + return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); + } + set16_fx( vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); + is_success &= vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx != NULL; + } + speaker_node_azi_deg_internal_fx[vbap->bottom_virtual_speaker_node_index] = float_to_fix( 0.0f, Q22 ); + speaker_node_ele_deg_internal_fx[vbap->bottom_virtual_speaker_node_index] = float_to_fix( -90.0f, Q22 ); +#ifdef TRUE + /*Clean up float code*/ + IF( ( vbap->bottom_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } - set32_fx( vbap->bottom_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); -#endif + set_zero( vbap->bottom_virtual_speaker_node_division_gains, num_speaker_nodes ); is_success &= vbap->bottom_virtual_speaker_node_division_gains != NULL; - if ( ivas_format == MASA_ISM_FORMAT ) + IF( ivas_format == MASA_ISM_FORMAT ) { - if ( ( vbap->object_mode_bottom_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) + IF( ( vbap->object_mode_bottom_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } set_zero( vbap->object_mode_bottom_virtual_speaker_node_division_gains, num_speaker_nodes ); -#ifdef IVAS_FLOAT_FIXED - IF ( ( vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx = (Word32 *) malloc( num_speaker_nodes * sizeof( Word32 ) ) ) == NULL ) - { - return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); - } - set32_fx( vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); -#endif is_success &= vbap->object_mode_bottom_virtual_speaker_node_division_gains != NULL; } - - speaker_node_azi_deg_internal[vbap->bottom_virtual_speaker_node_index] = 0.0f; - speaker_node_ele_deg_internal[vbap->bottom_virtual_speaker_node_index] = -90.0f; +#endif } - if ( is_success && virtual_top_type != NO_VIRTUAL_SPEAKER_NODE ) + IF( is_success && virtual_top_type != NO_VIRTUAL_SPEAKER_NODE ) { - if ( ( vbap->top_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) + IF( ( vbap->top_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } - set_zero( vbap->top_virtual_speaker_node_division_gains, num_speaker_nodes ); -#ifdef IVAS_FLOAT_FIXED - IF ( ( vbap->top_virtual_speaker_node_division_gains_fx = (Word32 *) malloc( num_speaker_nodes * sizeof( Word32 ) ) ) == NULL ) + set16_fx( vbap->top_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); + is_success &= vbap->top_virtual_speaker_node_division_gains_fx != NULL; + + IF( ( vbap->top_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } - set32_fx( vbap->top_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); -#endif - is_success &= vbap->top_virtual_speaker_node_division_gains != NULL; + set16_fx( vbap->top_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); + is_success &= vbap->top_virtual_speaker_node_division_gains_fx != NULL; - if ( ivas_format == MASA_ISM_FORMAT ) + IF( ivas_format == MASA_ISM_FORMAT ) { - if ( ( vbap->object_mode_top_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) + IF( ( vbap->object_mode_top_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } - set_zero( vbap->object_mode_top_virtual_speaker_node_division_gains, num_speaker_nodes ); -#ifdef IVAS_FLOAT_FIXED - IF ( ( vbap->object_mode_top_virtual_speaker_node_division_gains_fx = (Word32 *) malloc( num_speaker_nodes * sizeof( Word32 ) ) ) == NULL ) + set16_fx( vbap->object_mode_top_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); + is_success &= vbap->object_mode_top_virtual_speaker_node_division_gains_fx != NULL; + } + speaker_node_azi_deg_internal_fx[vbap->top_virtual_speaker_node_index] = float_to_fix( 0.0f, 22 ); + speaker_node_ele_deg_internal_fx[vbap->top_virtual_speaker_node_index] = float_to_fix( 90.0f, 22 ); +#ifdef TRUE + /*Clean up float code*/ + IF( ( vbap->top_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) + { + return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); + } + set_zero( vbap->top_virtual_speaker_node_division_gains, num_speaker_nodes ); + is_success &= vbap->top_virtual_speaker_node_division_gains != NULL; + + IF( ivas_format == MASA_ISM_FORMAT ) + { + IF( ( vbap->object_mode_top_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } - set32_fx( vbap->object_mode_top_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); -#endif + set_zero( vbap->object_mode_top_virtual_speaker_node_division_gains, num_speaker_nodes ); is_success &= vbap->object_mode_top_virtual_speaker_node_division_gains != NULL; } - - speaker_node_azi_deg_internal[vbap->top_virtual_speaker_node_index] = 0.0f; - speaker_node_ele_deg_internal[vbap->top_virtual_speaker_node_index] = 90.0f; } +#endif - if ( is_success && virtual_back_type != NO_VIRTUAL_SPEAKER_NODE ) + IF( is_success && virtual_back_type != NO_VIRTUAL_SPEAKER_NODE ) { - if ( ( vbap->back_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) + IF( ( vbap->back_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } - set_zero( vbap->back_virtual_speaker_node_division_gains, num_speaker_nodes ); -#ifdef IVAS_FLOAT_FIXED - IF ( ( vbap->back_virtual_speaker_node_division_gains_fx = (Word32 *) malloc( num_speaker_nodes * sizeof( Word32 ) ) ) == NULL ) + set16_fx( vbap->back_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); + is_success &= vbap->back_virtual_speaker_node_division_gains_fx != NULL; + + IF( ivas_format == MASA_ISM_FORMAT ) + { + IF( ( vbap->object_mode_back_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL ) + { + return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); + } + set16_fx( vbap->object_mode_back_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); + is_success &= vbap->object_mode_back_virtual_speaker_node_division_gains_fx != NULL; + } + speaker_node_azi_deg_internal_fx[vbap->back_virtual_speaker_node_index] = float_to_fix( 180.0f, Q22 ); + speaker_node_ele_deg_internal_fx[vbap->back_virtual_speaker_node_index] = float_to_fix( 0.0f, Q22 ); +#ifdef TRUE + /*Clean up float code*/ + IF( ( vbap->back_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } - set32_fx( vbap->back_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); -#endif + set_zero( vbap->back_virtual_speaker_node_division_gains, num_speaker_nodes ); is_success &= vbap->back_virtual_speaker_node_division_gains != NULL; - if ( ivas_format == MASA_ISM_FORMAT ) + IF( ivas_format == MASA_ISM_FORMAT ) { - if ( ( vbap->object_mode_back_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) + IF( ( vbap->object_mode_back_virtual_speaker_node_division_gains = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } set_zero( vbap->object_mode_back_virtual_speaker_node_division_gains, num_speaker_nodes ); -#ifdef IVAS_FLOAT_FIXED - IF ( ( vbap->object_mode_back_virtual_speaker_node_division_gains_fx = (Word32 *) malloc( num_speaker_nodes * sizeof( Word32 ) ) ) == NULL ) - { - return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); - } - set32_fx( vbap->object_mode_back_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes ); -#endif is_success &= vbap->object_mode_back_virtual_speaker_node_division_gains != NULL; } - speaker_node_azi_deg_internal[vbap->back_virtual_speaker_node_index] = 180.0f; - speaker_node_ele_deg_internal[vbap->back_virtual_speaker_node_index] = 0.0f; +#endif } - - init_speaker_node_direction_data( speaker_node_data, speaker_node_azi_deg_internal, speaker_node_ele_deg_internal, vbap->num_speaker_nodes_internal ); + init_speaker_node_direction_data_fx( speaker_node_data, speaker_node_azi_deg_internal_fx, speaker_node_ele_deg_internal_fx, vbap->num_speaker_nodes_internal ); +#ifdef TRUE + /*TODO: Clean up of update float buffers*/ + for ( int ch = 0; ch < vbap->num_speaker_nodes_internal; ch++ ) + { + speaker_node_data[ch].unit_vec[0] = fix_to_float( speaker_node_data[ch].unit_vec_fx[0], Q30 ); + speaker_node_data[ch].unit_vec[1] = fix_to_float( speaker_node_data[ch].unit_vec_fx[1], Q30 ); + speaker_node_data[ch].unit_vec[2] = fix_to_float( speaker_node_data[ch].unit_vec_fx[2], Q30 ); + speaker_node_data[ch].ele_deg = fix_to_float( speaker_node_data[ch].ele_deg_fx, Q22 ); + speaker_node_data[ch].azi_deg = fix_to_float( speaker_node_data[ch].azi_deg_fx, Q22 ); + } +#endif /* Allocate and determine node-node connections */ max_num_connections = ( vbap->num_speaker_nodes_internal - 2 ) * 3; /* Theoretical maximum */ - - if ( ( error = determine_connections( vbap->num_speaker_nodes_internal, speaker_node_data, connections, max_num_connections, &connection_group1_count, &connection_group2_start, &connection_group2_count ) ) != IVAS_ERR_OK ) + IF( ( error = determine_connections_fx( vbap->num_speaker_nodes_internal, speaker_node_data, connections, max_num_connections, &connection_group1_count, &connection_group2_start, &connection_group2_count ) ) != IVAS_ERR_OK ) { return error; } - /* Allocate and determine virtual surface speaker node triplets */ - if ( is_success ) + IF( is_success ) { - int16_t ch; - int16_t speaker_nodes_group1_internal = 0; - int16_t speaker_nodes_group2_internal = 0; - int16_t speaker_nodes_horiz_internal = 0; + Word16 ch; + Word16 speaker_nodes_group1_internal = 0; + Word16 speaker_nodes_group2_internal = 0; + Word16 speaker_nodes_horiz_internal = 0; uint8_t loop_done = 0; /* Count nodes in different groups to reserve correct memory */ - for ( ch = 0; ch < vbap->num_speaker_nodes_internal && !loop_done; ch++ ) + FOR( ch = 0; ch < vbap->num_speaker_nodes_internal && !loop_done; ch++ ) { - switch ( speaker_node_data[ch].group ) + SWITCH( speaker_node_data[ch].group ) { case SPEAKER_NODE_ALL: /* If there is even one speaker belonging to "all" group, then all speakers belong to the "all" group. * We can skip further counts here. */ speaker_nodes_group1_internal = vbap->num_speaker_nodes_internal; loop_done = 1; - break; + BREAK; case SPEAKER_NODE_BOTTOM_HALF: speaker_nodes_group1_internal++; - break; + BREAK; case SPEAKER_NODE_TOP_HALF: speaker_nodes_group2_internal++; - break; + BREAK; case SPEAKER_NODE_HORIZONTAL: case SPEAKER_NODE_BACK: speaker_nodes_group1_internal++; speaker_nodes_group2_internal++; speaker_nodes_horiz_internal++; - break; + BREAK; } } - if ( ( vbap->search_struct[0].triplets = (VBAP_VS_TRIPLET *) malloc( ( ( speaker_nodes_group1_internal - 2 ) * 2 - ( max( 0, ( speaker_nodes_horiz_internal - 2 ) ) ) ) * sizeof( VBAP_VS_TRIPLET ) ) ) == NULL ) + IF( ( vbap->search_struct[0].triplets = (VBAP_VS_TRIPLET *) malloc( ( ( speaker_nodes_group1_internal - 2 ) * 2 - ( max( 0, ( speaker_nodes_horiz_internal - 2 ) ) ) ) * sizeof( VBAP_VS_TRIPLET ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } is_success &= vbap->search_struct[0].triplets != NULL; - if ( speaker_nodes_group2_internal > 0 ) + IF( speaker_nodes_group2_internal > 0 ) { vbap->num_search_structs = 2; - if ( ( vbap->search_struct[1].triplets = (VBAP_VS_TRIPLET *) malloc( ( ( speaker_nodes_group2_internal - 2 ) * 2 - ( max( 0, ( speaker_nodes_horiz_internal - 2 ) ) ) ) * sizeof( VBAP_VS_TRIPLET ) ) ) == NULL ) + IF( ( vbap->search_struct[1].triplets = (VBAP_VS_TRIPLET *) malloc( ( ( speaker_nodes_group2_internal - 2 ) * 2 - ( max( 0, ( speaker_nodes_horiz_internal - 2 ) ) ) ) * sizeof( VBAP_VS_TRIPLET ) ) ) == NULL ) { return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); } is_success &= vbap->search_struct[1].triplets != NULL; } - else + ELSE { vbap->num_search_structs = 1; vbap->search_struct[1].triplets = NULL; } } - if ( is_success ) + IF( is_success ) { - if ( vbap->num_search_structs == 1 ) + IF( vbap->num_search_structs == 1 ) { /* If all speaker nodes belong to ALL set, then we only create one triplet set and search structure */ - vbap->search_struct[0].num_triplets = determine_virtual_surface_triplets( vbap->num_speaker_nodes_internal, speaker_node_data, connections, max_num_connections, vbap->search_struct[0].triplets, vbap->search_struct[0].initial_search_indices, SPEAKER_NODE_ALL ); + vbap->search_struct[0].num_triplets = determine_virtual_surface_triplets_fx( vbap->num_speaker_nodes_internal, speaker_node_data, connections, max_num_connections, vbap->search_struct[0].triplets, vbap->search_struct[0].initial_search_indices, SPEAKER_NODE_ALL ); } - else + ELSE { /* Otherwise, we have two sets and can handle them separately for more opmitized processing. */ - vbap->search_struct[0].num_triplets = determine_virtual_surface_triplets( vbap->num_speaker_nodes_internal, speaker_node_data, connections, connection_group1_count, vbap->search_struct[0].triplets, vbap->search_struct[0].initial_search_indices, SPEAKER_NODE_BOTTOM_HALF ); - vbap->search_struct[1].num_triplets = determine_virtual_surface_triplets( vbap->num_speaker_nodes_internal, speaker_node_data, connections + connection_group2_start, connection_group2_count, vbap->search_struct[1].triplets, vbap->search_struct[1].initial_search_indices, SPEAKER_NODE_TOP_HALF ); + vbap->search_struct[0].num_triplets = determine_virtual_surface_triplets_fx( vbap->num_speaker_nodes_internal, speaker_node_data, connections, connection_group1_count, vbap->search_struct[0].triplets, vbap->search_struct[0].initial_search_indices, SPEAKER_NODE_BOTTOM_HALF ); + vbap->search_struct[1].num_triplets = determine_virtual_surface_triplets_fx( vbap->num_speaker_nodes_internal, speaker_node_data, connections + connection_group2_start, connection_group2_count, vbap->search_struct[1].triplets, vbap->search_struct[1].initial_search_indices, SPEAKER_NODE_TOP_HALF ); } } /* Determine how the virtual node gains should be distributed to real nodes, if necessary (checked within function). */ - if ( is_success ) + IF( is_success ) { - determine_virtual_speaker_node_division_gains( vbap->top_virtual_speaker_node_index, vbap->top_virtual_speaker_node_division_gains, connections, virtual_top_type, max_num_connections, num_speaker_nodes, 0 ); - determine_virtual_speaker_node_division_gains( vbap->bottom_virtual_speaker_node_index, vbap->bottom_virtual_speaker_node_division_gains, connections, virtual_bottom_type, max_num_connections, num_speaker_nodes, 0 ); - determine_virtual_speaker_node_division_gains( vbap->back_virtual_speaker_node_index, vbap->back_virtual_speaker_node_division_gains, connections, virtual_back_type, max_num_connections, num_speaker_nodes, 0 ); - if ( ivas_format == MASA_ISM_FORMAT ) + Word16 max_exp = 31, i; + determine_virtual_speaker_node_division_gains_fx( vbap->top_virtual_speaker_node_index, vbap->top_virtual_speaker_node_division_gains_fx, &max_exp, connections, virtual_top_type, max_num_connections, num_speaker_nodes, 0 ); +#ifdef TRUE + IF( vbap->top_virtual_speaker_node_division_gains_fx != NULL ) + { + FOR( i = 0; i < num_speaker_nodes; i++ ) + { + vbap->top_virtual_speaker_node_division_gains[i] = me2f_16( vbap->top_virtual_speaker_node_division_gains_fx[i], max_exp ); + } + } +#endif + determine_virtual_speaker_node_division_gains_fx( vbap->bottom_virtual_speaker_node_index, vbap->bottom_virtual_speaker_node_division_gains_fx, &max_exp, connections, virtual_bottom_type, max_num_connections, num_speaker_nodes, 0 ); +#ifdef TRUE + IF( vbap->bottom_virtual_speaker_node_division_gains_fx != NULL ) + { + FOR( i = 0; i < num_speaker_nodes; i++ ) + { + vbap->bottom_virtual_speaker_node_division_gains[i] = me2f_16( vbap->bottom_virtual_speaker_node_division_gains_fx[i], max_exp ); + } + } +#endif + determine_virtual_speaker_node_division_gains_fx( vbap->back_virtual_speaker_node_index, vbap->back_virtual_speaker_node_division_gains_fx, &max_exp, connections, virtual_back_type, max_num_connections, num_speaker_nodes, 0 ); +#ifdef TRUE + IF( vbap->back_virtual_speaker_node_division_gains_fx != NULL ) { - determine_virtual_speaker_node_division_gains( vbap->top_virtual_speaker_node_index, vbap->object_mode_top_virtual_speaker_node_division_gains, connections, virtual_top_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1 ); - determine_virtual_speaker_node_division_gains( vbap->bottom_virtual_speaker_node_index, vbap->object_mode_bottom_virtual_speaker_node_division_gains, connections, virtual_bottom_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1 ); - determine_virtual_speaker_node_division_gains( vbap->back_virtual_speaker_node_index, vbap->object_mode_back_virtual_speaker_node_division_gains, connections, virtual_back_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1 ); + FOR( i = 0; i < num_speaker_nodes; i++ ) + { + vbap->back_virtual_speaker_node_division_gains[i] = me2f_16( vbap->back_virtual_speaker_node_division_gains_fx[i], max_exp ); + } + } +#endif + IF( ivas_format == MASA_ISM_FORMAT ) + { + determine_virtual_speaker_node_division_gains_fx( vbap->top_virtual_speaker_node_index, vbap->object_mode_top_virtual_speaker_node_division_gains_fx, &max_exp, connections, virtual_top_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1 ); + +#ifdef TRUE + IF( vbap->object_mode_top_virtual_speaker_node_division_gains_fx != NULL ) + { + FOR( i = 0; i < num_speaker_nodes; i++ ) + { + vbap->object_mode_top_virtual_speaker_node_division_gains[i] = me2f_16( vbap->object_mode_top_virtual_speaker_node_division_gains_fx[i], max_exp ); + } + } +#endif + determine_virtual_speaker_node_division_gains_fx( vbap->bottom_virtual_speaker_node_index, vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx, &max_exp, connections, virtual_bottom_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1 ); +#ifdef TRUE + IF( vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx != NULL ) + { + FOR( i = 0; i < num_speaker_nodes; i++ ) + { + vbap->object_mode_bottom_virtual_speaker_node_division_gains[i] = me2f_16( vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx[i], max_exp ); + } + } +#endif + determine_virtual_speaker_node_division_gains_fx( vbap->back_virtual_speaker_node_index, vbap->object_mode_back_virtual_speaker_node_division_gains_fx, &max_exp, connections, virtual_back_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1 ); +#ifdef TRUE + IF( vbap->object_mode_back_virtual_speaker_node_division_gains_fx != NULL ) + { + FOR( i = 0; i < num_speaker_nodes; i++ ) + { + vbap->object_mode_back_virtual_speaker_node_division_gains[i] = me2f_16( vbap->object_mode_back_virtual_speaker_node_division_gains_fx[i], max_exp ); + } + } +#endif } } pop_wmops(); - if ( is_success ) + IF( is_success ) { *hVBAPdata = vbap; } - else + ELSE { vbap_free_data( &vbap ); } return IVAS_ERR_OK; } +#endif - -/*-------------------------------------------------------------------------* - * vbap_free_data() - * - * Free VBAP data structure - *-------------------------------------------------------------------------*/ - -void vbap_free_data( - VBAP_HANDLE *hVBAPdata /* i/o: VBAP handle to be freed */ +ivas_error vbap_init_data( + VBAP_HANDLE *hVBAPdata, /* i/o: handle for VBAP data structure that will be initialized */ + const float *speaker_node_azi_deg, /* i : vector of speaker node azimuths (positive left) */ + const float *speaker_node_ele_deg, /* i : vector of speaker node elevations (positive up) */ + const int16_t num_speaker_nodes, /* i : number of speaker nodes in the set */ + const IVAS_FORMAT ivas_format /* i : IVAS format */ ) { - if ( hVBAPdata == NULL || *hVBAPdata == NULL ) - { - return; - } + /* Variables */ + int16_t connections[VBAP_MAX_NUM_SPEAKER_NODES][2]; + int16_t max_num_connections; + int16_t is_success; + int16_t connection_group1_count; + int16_t connection_group2_start; + int16_t connection_group2_count; + enum VirtualSpeakerNodeType virtual_top_type; + enum VirtualSpeakerNodeType virtual_bottom_type; + enum VirtualSpeakerNodeType virtual_back_type; + float speaker_node_azi_deg_internal[VBAP_MAX_NUM_SPEAKER_NODES]; + float speaker_node_ele_deg_internal[VBAP_MAX_NUM_SPEAKER_NODES]; + VBAP_SPEAKER_NODE speaker_node_data[VBAP_MAX_NUM_SPEAKER_NODES]; + VBAP_DATA *vbap; + ivas_error error; - if ( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains != NULL ) + push_wmops("vbap_init"); + + /* Basic init checks */ + /* If the requested layout is invalid, hVBAPdata is set to NULL and the signal will + * be distributed with an equal gain into all output channels. + * The surrounding code needs to handle the NULL pointer properly. */ + if (num_speaker_nodes > VBAP_MAX_NUM_SPEAKER_NODES || num_speaker_nodes < 3) { - free( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains ); + hVBAPdata = NULL; + pop_wmops(); + return IVAS_ERR_OK; } - if ( ( *hVBAPdata )->top_virtual_speaker_node_division_gains != NULL ) + + if (!speaker_node_azi_deg || !speaker_node_ele_deg) { - free( ( *hVBAPdata )->top_virtual_speaker_node_division_gains ); + hVBAPdata = NULL; + return IVAS_ERR_OK; } - if ( ( *hVBAPdata )->back_virtual_speaker_node_division_gains != NULL ) + + /* Allocate VBAP structure */ + if ((vbap = (VBAP_HANDLE)malloc(sizeof(VBAP_DATA))) == NULL) { - free( ( *hVBAPdata )->back_virtual_speaker_node_division_gains ); + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); } - if ( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains != NULL ) + + is_success = 1; + vbap->bottom_virtual_speaker_node_index = -1; + vbap->top_virtual_speaker_node_index = -1; + vbap->back_virtual_speaker_node_index = -1; + vbap->bottom_virtual_speaker_node_division_gains = NULL; + vbap->top_virtual_speaker_node_division_gains = NULL; + vbap->back_virtual_speaker_node_division_gains = NULL; + vbap->object_mode_bottom_virtual_speaker_node_division_gains = NULL; + vbap->object_mode_top_virtual_speaker_node_division_gains = NULL; + vbap->object_mode_back_virtual_speaker_node_division_gains = NULL; + vbap->num_speaker_nodes = num_speaker_nodes; + vbap->num_speaker_nodes_internal = num_speaker_nodes; + + /* Check if the speaker node setup needs a virtual top or bottom node + (function also increments vbap->num_speaker_nodes_internal when necessary) */ + virtual_bottom_type = check_need_of_virtual_speaker_node(vbap, speaker_node_azi_deg, speaker_node_ele_deg, SPEAKER_NODE_BOTTOM_HALF); + virtual_top_type = check_need_of_virtual_speaker_node(vbap, speaker_node_azi_deg, speaker_node_ele_deg, SPEAKER_NODE_TOP_HALF); + virtual_back_type = check_need_of_virtual_speaker_node(vbap, speaker_node_azi_deg, speaker_node_ele_deg, SPEAKER_NODE_BACK); + + /* Init internal speaker node configuration, which is the original configuration + potentially appended with virtual top and/or bottom loudspeakers */ + mvr2r(speaker_node_azi_deg, speaker_node_azi_deg_internal, num_speaker_nodes); + mvr2r(speaker_node_ele_deg, speaker_node_ele_deg_internal, num_speaker_nodes); + + if (is_success && virtual_bottom_type != NO_VIRTUAL_SPEAKER_NODE) { - free( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains ); + if ((vbap->bottom_virtual_speaker_node_division_gains = (float *)malloc(num_speaker_nodes * sizeof(float))) == NULL) + { + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); + } + set_zero(vbap->bottom_virtual_speaker_node_division_gains, num_speaker_nodes); + is_success &= vbap->bottom_virtual_speaker_node_division_gains != NULL; + + if (ivas_format == MASA_ISM_FORMAT) + { + if ((vbap->object_mode_bottom_virtual_speaker_node_division_gains = (float *)malloc(num_speaker_nodes * sizeof(float))) == NULL) + { + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); + } + set_zero(vbap->object_mode_bottom_virtual_speaker_node_division_gains, num_speaker_nodes); + is_success &= vbap->object_mode_bottom_virtual_speaker_node_division_gains != NULL; + } + + speaker_node_azi_deg_internal[vbap->bottom_virtual_speaker_node_index] = 0.0f; + speaker_node_ele_deg_internal[vbap->bottom_virtual_speaker_node_index] = -90.0f; } - if ( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains != NULL ) + + if (is_success && virtual_top_type != NO_VIRTUAL_SPEAKER_NODE) + { + if ((vbap->top_virtual_speaker_node_division_gains = (float *)malloc(num_speaker_nodes * sizeof(float))) == NULL) + { + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); + } + set_zero(vbap->top_virtual_speaker_node_division_gains, num_speaker_nodes); + is_success &= vbap->top_virtual_speaker_node_division_gains != NULL; + + if (ivas_format == MASA_ISM_FORMAT) + { + if ((vbap->object_mode_top_virtual_speaker_node_division_gains = (float *)malloc(num_speaker_nodes * sizeof(float))) == NULL) + { + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); + } + set_zero(vbap->object_mode_top_virtual_speaker_node_division_gains, num_speaker_nodes); + is_success &= vbap->object_mode_top_virtual_speaker_node_division_gains != NULL; + } + + speaker_node_azi_deg_internal[vbap->top_virtual_speaker_node_index] = 0.0f; + speaker_node_ele_deg_internal[vbap->top_virtual_speaker_node_index] = 90.0f; + } + + if (is_success && virtual_back_type != NO_VIRTUAL_SPEAKER_NODE) + { + if ((vbap->back_virtual_speaker_node_division_gains = (float *)malloc(num_speaker_nodes * sizeof(float))) == NULL) + { + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); + } + set_zero(vbap->back_virtual_speaker_node_division_gains, num_speaker_nodes); + is_success &= vbap->back_virtual_speaker_node_division_gains != NULL; + + if (ivas_format == MASA_ISM_FORMAT) + { + if ((vbap->object_mode_back_virtual_speaker_node_division_gains = (float *)malloc(num_speaker_nodes * sizeof(float))) == NULL) + { + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); + } + set_zero(vbap->object_mode_back_virtual_speaker_node_division_gains, num_speaker_nodes); + is_success &= vbap->object_mode_back_virtual_speaker_node_division_gains != NULL; + } + speaker_node_azi_deg_internal[vbap->back_virtual_speaker_node_index] = 180.0f; + speaker_node_ele_deg_internal[vbap->back_virtual_speaker_node_index] = 0.0f; + } + + init_speaker_node_direction_data(speaker_node_data, speaker_node_azi_deg_internal, speaker_node_ele_deg_internal, vbap->num_speaker_nodes_internal); + + /* Allocate and determine node-node connections */ + max_num_connections = (vbap->num_speaker_nodes_internal - 2) * 3; /* Theoretical maximum */ + + if ((error = determine_connections(vbap->num_speaker_nodes_internal, speaker_node_data, connections, max_num_connections, &connection_group1_count, &connection_group2_start, &connection_group2_count)) != IVAS_ERR_OK) + { + return error; + } + + /* Allocate and determine virtual surface speaker node triplets */ + if (is_success) + { + int16_t ch; + int16_t speaker_nodes_group1_internal = 0; + int16_t speaker_nodes_group2_internal = 0; + int16_t speaker_nodes_horiz_internal = 0; + uint8_t loop_done = 0; + + /* Count nodes in different groups to reserve correct memory */ + for (ch = 0; ch < vbap->num_speaker_nodes_internal && !loop_done; ch++) + { + switch (speaker_node_data[ch].group) + { + case SPEAKER_NODE_ALL: + /* If there is even one speaker belonging to "all" group, then all speakers belong to the "all" group. + * We can skip further counts here. */ + speaker_nodes_group1_internal = vbap->num_speaker_nodes_internal; + loop_done = 1; + break; + case SPEAKER_NODE_BOTTOM_HALF: + speaker_nodes_group1_internal++; + break; + case SPEAKER_NODE_TOP_HALF: + speaker_nodes_group2_internal++; + break; + case SPEAKER_NODE_HORIZONTAL: + case SPEAKER_NODE_BACK: + speaker_nodes_group1_internal++; + speaker_nodes_group2_internal++; + speaker_nodes_horiz_internal++; + break; + } + } + + if ((vbap->search_struct[0].triplets = (VBAP_VS_TRIPLET *)malloc(((speaker_nodes_group1_internal - 2) * 2 - (max(0, (speaker_nodes_horiz_internal - 2)))) * sizeof(VBAP_VS_TRIPLET))) == NULL) + { + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); + } + is_success &= vbap->search_struct[0].triplets != NULL; + + if (speaker_nodes_group2_internal > 0) + { + vbap->num_search_structs = 2; + if ((vbap->search_struct[1].triplets = (VBAP_VS_TRIPLET *)malloc(((speaker_nodes_group2_internal - 2) * 2 - (max(0, (speaker_nodes_horiz_internal - 2)))) * sizeof(VBAP_VS_TRIPLET))) == NULL) + { + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); + } + is_success &= vbap->search_struct[1].triplets != NULL; + } + else + { + vbap->num_search_structs = 1; + vbap->search_struct[1].triplets = NULL; + } + } + + if (is_success) + { + if (vbap->num_search_structs == 1) + { + /* If all speaker nodes belong to ALL set, then we only create one triplet set and search structure */ + vbap->search_struct[0].num_triplets = determine_virtual_surface_triplets(vbap->num_speaker_nodes_internal, speaker_node_data, connections, max_num_connections, vbap->search_struct[0].triplets, vbap->search_struct[0].initial_search_indices, SPEAKER_NODE_ALL); + } + else + { + /* Otherwise, we have two sets and can handle them separately for more opmitized processing. */ + vbap->search_struct[0].num_triplets = determine_virtual_surface_triplets(vbap->num_speaker_nodes_internal, speaker_node_data, connections, connection_group1_count, vbap->search_struct[0].triplets, vbap->search_struct[0].initial_search_indices, SPEAKER_NODE_BOTTOM_HALF); + vbap->search_struct[1].num_triplets = determine_virtual_surface_triplets(vbap->num_speaker_nodes_internal, speaker_node_data, connections + connection_group2_start, connection_group2_count, vbap->search_struct[1].triplets, vbap->search_struct[1].initial_search_indices, SPEAKER_NODE_TOP_HALF); + } + } + + /* Determine how the virtual node gains should be distributed to real nodes, if necessary (checked within function). */ + if (is_success) + { + determine_virtual_speaker_node_division_gains(vbap->top_virtual_speaker_node_index, vbap->top_virtual_speaker_node_division_gains, connections, virtual_top_type, max_num_connections, num_speaker_nodes, 0); + determine_virtual_speaker_node_division_gains(vbap->bottom_virtual_speaker_node_index, vbap->bottom_virtual_speaker_node_division_gains, connections, virtual_bottom_type, max_num_connections, num_speaker_nodes, 0); + determine_virtual_speaker_node_division_gains(vbap->back_virtual_speaker_node_index, vbap->back_virtual_speaker_node_division_gains, connections, virtual_back_type, max_num_connections, num_speaker_nodes, 0); + if (ivas_format == MASA_ISM_FORMAT) + { + determine_virtual_speaker_node_division_gains(vbap->top_virtual_speaker_node_index, vbap->object_mode_top_virtual_speaker_node_division_gains, connections, virtual_top_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1); + determine_virtual_speaker_node_division_gains(vbap->bottom_virtual_speaker_node_index, vbap->object_mode_bottom_virtual_speaker_node_division_gains, connections, virtual_bottom_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1); + determine_virtual_speaker_node_division_gains(vbap->back_virtual_speaker_node_index, vbap->object_mode_back_virtual_speaker_node_division_gains, connections, virtual_back_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1); + } + } + + pop_wmops(); + + if (is_success) + { + *hVBAPdata = vbap; + } + else + { + vbap_free_data(&vbap); + } + + return IVAS_ERR_OK; +} +/*-------------------------------------------------------------------------* + * vbap_free_data() + * + * Free VBAP data structure + *-------------------------------------------------------------------------*/ + +#ifdef IVAS_FLOAT_FIXED +void vbap_free_data_fx( + VBAP_HANDLE *hVBAPdata /* i/o: VBAP handle to be freed */ +) +{ + IF( hVBAPdata == NULL || *hVBAPdata == NULL ) + { + return; + } + + IF( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains != NULL ) + { + free( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains ); + } + IF( ( *hVBAPdata )->top_virtual_speaker_node_division_gains != NULL ) + { + free( ( *hVBAPdata )->top_virtual_speaker_node_division_gains ); + } + IF( ( *hVBAPdata )->back_virtual_speaker_node_division_gains != NULL ) + { + free( ( *hVBAPdata )->back_virtual_speaker_node_division_gains ); + } + IF( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains != NULL ) + { + free( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains ); + } + IF( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains != NULL ) { free( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains ); } - if ( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains != NULL ) + IF( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains != NULL ) { free( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains ); } + + IF( ( *hVBAPdata )->search_struct[0].triplets != NULL ) + { + free( ( *hVBAPdata )->search_struct[0].triplets ); + } + IF( ( *hVBAPdata )->num_search_structs == 2 && ( *hVBAPdata )->search_struct[1].triplets != NULL ) + { + free( ( *hVBAPdata )->search_struct[1].triplets ); + } #ifdef IVAS_FLOAT_FIXED - IF ( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains_fx != NULL ) + IF( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains_fx != NULL ) { free( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains_fx ); } - IF ( ( *hVBAPdata )->top_virtual_speaker_node_division_gains_fx != NULL ) + IF( ( *hVBAPdata )->top_virtual_speaker_node_division_gains_fx != NULL ) { free( ( *hVBAPdata )->top_virtual_speaker_node_division_gains_fx ); } - IF ( ( *hVBAPdata )->back_virtual_speaker_node_division_gains_fx != NULL ) + IF( ( *hVBAPdata )->back_virtual_speaker_node_division_gains_fx != NULL ) { free( ( *hVBAPdata )->back_virtual_speaker_node_division_gains_fx ); } - IF ( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains_fx != NULL ) + IF( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains_fx != NULL ) { free( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains_fx ); } - IF ( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains_fx != NULL ) + IF( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains_fx != NULL ) { free( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains_fx ); } - IF ( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains_fx != NULL ) + IF( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains_fx != NULL ) { free( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains_fx ); } #endif + free( *hVBAPdata ); + *hVBAPdata = NULL; + + return; +} +#endif + +void vbap_free_data( + VBAP_HANDLE *hVBAPdata /* i/o: VBAP handle to be freed */ +) +{ + if ( hVBAPdata == NULL || *hVBAPdata == NULL ) + { + return; + } + + if ( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains != NULL ) + { + free( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains ); + } + if ( ( *hVBAPdata )->top_virtual_speaker_node_division_gains != NULL ) + { + free( ( *hVBAPdata )->top_virtual_speaker_node_division_gains ); + } + if ((*hVBAPdata)->back_virtual_speaker_node_division_gains != NULL) + { + free((*hVBAPdata)->back_virtual_speaker_node_division_gains); + } + if ( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains != NULL ) + { + free( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains ); + } + if ( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains != NULL ) + { + free( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains ); + } + if ( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains != NULL ) + { + free( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains ); + } + if ( ( *hVBAPdata )->search_struct[0].triplets != NULL ) { free( ( *hVBAPdata )->search_struct[0].triplets ); @@ -530,7 +959,32 @@ void vbap_free_data( { free( ( *hVBAPdata )->search_struct[1].triplets ); } - +#ifdef IVAS_FLOAT_FIXED + IF((*hVBAPdata)->bottom_virtual_speaker_node_division_gains_fx != NULL) + { + free((*hVBAPdata)->bottom_virtual_speaker_node_division_gains_fx); + } + IF((*hVBAPdata)->top_virtual_speaker_node_division_gains_fx != NULL) + { + free((*hVBAPdata)->top_virtual_speaker_node_division_gains_fx); + } + IF((*hVBAPdata)->back_virtual_speaker_node_division_gains_fx != NULL) + { + free((*hVBAPdata)->back_virtual_speaker_node_division_gains_fx); + } + IF((*hVBAPdata)->object_mode_bottom_virtual_speaker_node_division_gains_fx != NULL) + { + free((*hVBAPdata)->object_mode_bottom_virtual_speaker_node_division_gains_fx); + } + IF((*hVBAPdata)->object_mode_top_virtual_speaker_node_division_gains_fx != NULL) + { + free((*hVBAPdata)->object_mode_top_virtual_speaker_node_division_gains_fx); + } + IF((*hVBAPdata)->object_mode_back_virtual_speaker_node_division_gains_fx != NULL) + { + free((*hVBAPdata)->object_mode_back_virtual_speaker_node_division_gains_fx); + } +#endif free( *hVBAPdata ); *hVBAPdata = NULL; @@ -698,9 +1152,9 @@ void vbap_determine_gains_fx( Word16 top_virtual_speaker_node_index; Word16 back_virtual_speaker_node_index; VBAP_VS_TRIPLET *selected_triplet; - Word32 *bottom_virtual_speaker_node_division_gains_fx; - Word32 *top_virtual_speaker_node_division_gains_fx; - Word32 *back_virtual_speaker_node_division_gains_fx; + Word16 *bottom_virtual_speaker_node_division_gains_fx; + Word16 *top_virtual_speaker_node_division_gains_fx; + Word16 *back_virtual_speaker_node_division_gains_fx; push_wmops( "vbap_gains" ); @@ -781,21 +1235,21 @@ void vbap_determine_gains_fx( { FOR ( ch2 = 0; ch2 < num_speaker_nodes; ch2++ ) { - gains_fx[ch2] = L_add(gains_fx[ch2], L_shl(Mpy_32_32(bottom_virtual_speaker_node_division_gains_fx[ch2], gain_triplet_fx[ch]), 13)); // Q29 + gains_fx[ch2] = L_add(gains_fx[ch2], L_shl(Mpy_32_16_1(gain_triplet_fx[ch] , bottom_virtual_speaker_node_division_gains_fx[ch2]), 13)); // Q29 } } ELSE IF ( EQ_16(triplet_ch, top_virtual_speaker_node_index) ) { FOR ( ch2 = 0; ch2 < num_speaker_nodes; ch2++ ) { - gains_fx[ch2] = L_add(gains_fx[ch2], L_shl(Mpy_32_32(top_virtual_speaker_node_division_gains_fx[ch2], gain_triplet_fx[ch]), 13)); // Q29 + gains_fx[ch2] = L_add(gains_fx[ch2], L_shl(Mpy_32_16_1(gain_triplet_fx[ch] , top_virtual_speaker_node_division_gains_fx[ch2]), 13)); // Q29 } } ELSE IF ( EQ_16(triplet_ch, back_virtual_speaker_node_index) ) { FOR ( ch2 = 0; ch2 < num_speaker_nodes; ch2++ ) { - gains_fx[ch2] = L_add(gains_fx[ch2], L_shl(Mpy_32_32(back_virtual_speaker_node_division_gains_fx[ch2], gain_triplet_fx[ch]), 13)); // Q29 + gains_fx[ch2] = L_add(gains_fx[ch2], L_shl(Mpy_32_16_1(gain_triplet_fx[ch] , back_virtual_speaker_node_division_gains_fx[ch2] ), 13)); // Q29 } } ELSE @@ -819,6 +1273,21 @@ void vbap_determine_gains_fx( * * 3-by-3 vector cross product *-------------------------------------------------------------------------*/ +#ifdef IVAS_FLOAT_FIXED +static void vbap_crossp_fx( + const Word32 *vec1_fx, /* i : input vector 1 */ + const Word32 *vec2_fx, /* i : input vector 2 */ + Word32 *crossProduct_fx /* o : cross product output */ +) +{ + + crossProduct_fx[0] = L_sub( Mpy_32_32( vec1_fx[1], vec2_fx[2] ), Mpy_32_32( vec1_fx[2], vec2_fx[1] ) ); + crossProduct_fx[1] = L_sub( Mpy_32_32( vec1_fx[2], vec2_fx[0] ), Mpy_32_32( vec1_fx[0], vec2_fx[2] ) ); + crossProduct_fx[2] = L_sub( Mpy_32_32( vec1_fx[0], vec2_fx[1] ), Mpy_32_32( vec1_fx[1], vec2_fx[0] ) ); + + return; +} +#endif static void vbap_crossp( const float *vec1, /* i : input vector 1 */ @@ -833,7 +1302,6 @@ static void vbap_crossp( return; } - /*-------------------------------------------------------------------------* * vector_matrix_multiply_3x3() * @@ -876,47 +1344,80 @@ static uint8_t vector_matrix_multiply_3x3( return 1; } + #ifdef IVAS_FLOAT_FIXED /*! r: Status result if triplet is usable for panning. Allows early exit. */ static uint8_t vector_matrix_multiply_3x3_fx( const Word16 *src_vector, /* i : input vector */ Word32 matrix[3][3], /* i : input matrix */ - Word32 *result, /* o : output vector */ - Word16 q_matrix -) + Word32 *result, /* o : output vector */ + Word16 q_matrix ) { - result[0] = Mpy_32_16_1(matrix[0][0], src_vector[0]); // q = q_matrix - result[0] = L_add(result[0], Mpy_32_16_1(matrix[1][0], src_vector[1])); - result[0] = L_add(result[0], Mpy_32_16_1(matrix[2][0], src_vector[2])); + result[0] = Mpy_32_16_1( matrix[0][0], src_vector[0] ); // q = q_matrix + result[0] = L_add( result[0], Mpy_32_16_1( matrix[1][0], src_vector[1] ) ); + result[0] = L_add( result[0], Mpy_32_16_1( matrix[2][0], src_vector[2] ) ); - IF ( result[0] < Mpy_32_16_1( 1 << q_matrix, -327) ) // 327 = 0.01 in Q = 15 + IF( result[0] < Mpy_32_16_1( 1 << q_matrix, -327 ) ) // 327 = 0.01 in Q = 15 { return 0; } - result[1] = Mpy_32_16_1(matrix[0][1], src_vector[0]); - result[1] = L_add(result[1], Mpy_32_16_1(matrix[1][1], src_vector[1])); - result[1] = L_add(result[1], Mpy_32_16_1(matrix[2][1], src_vector[2])); + result[1] = Mpy_32_16_1( matrix[0][1], src_vector[0] ); + result[1] = L_add( result[1], Mpy_32_16_1( matrix[1][1], src_vector[1] ) ); + result[1] = L_add( result[1], Mpy_32_16_1( matrix[2][1], src_vector[2] ) ); - IF ( result[1] < Mpy_32_16_1( 1 << q_matrix, -327) ) + IF( result[1] < Mpy_32_16_1( 1 << q_matrix, -327 ) ) { return 0; } - result[2] = Mpy_32_16_1(matrix[0][2], src_vector[0]); - result[2] = L_add(result[2], Mpy_32_16_1(matrix[1][2], src_vector[1])); - result[2] = L_add(result[2], Mpy_32_16_1(matrix[2][2], src_vector[2])); + result[2] = Mpy_32_16_1( matrix[0][2], src_vector[0] ); + result[2] = L_add( result[2], Mpy_32_16_1( matrix[1][2], src_vector[1] ) ); + result[2] = L_add( result[2], Mpy_32_16_1( matrix[2][2], src_vector[2] ) ); - IF ( result[2] < Mpy_32_16_1( 1 << q_matrix, -327) ) + IF( result[2] < Mpy_32_16_1( 1 << q_matrix, -327 ) ) { return 0; } return 1; } -#endif +static uint8_t vector_matrix_multiply_3x3_32_fx( + const Word32 *src_vector, /* i : input vector */ + Word32 matrix[3][3], /* i : input matrix */ + Word32 *result, /* o : output vector */ + Word16 q_matrix ) +{ + result[0] = Mpy_32_32( matrix[0][0], src_vector[0] ); // q = ( q_matrix + 30 ) - 31 + result[0] = L_add( result[0], Mpy_32_32( matrix[1][0], src_vector[1] ) ); + result[0] = L_add( result[0], Mpy_32_32( matrix[2][0], src_vector[2] ) ); + + IF( result[0] < Mpy_32_32( L_shl( 1, ( q_matrix - 1 ) ), -21474836 ) ) // 21474836 = 0.01 in Q = 31 + { + return 0; + } + + result[1] = Mpy_32_32( matrix[0][1], src_vector[0] ); + result[1] = L_add( result[1], Mpy_32_32( matrix[1][1], src_vector[1] ) ); + result[1] = L_add( result[1], Mpy_32_32( matrix[2][1], src_vector[2] ) ); + IF( result[1] < Mpy_32_32( L_shl( 1, ( q_matrix - 1 ) ), -21474836 ) ) // 21474836 = 0.01 in Q = 31 + { + return 0; + } + + result[2] = Mpy_32_32( matrix[0][2], src_vector[0] ); + result[2] = L_add( result[2], Mpy_32_32( matrix[1][2], src_vector[1] ) ); + result[2] = L_add( result[2], Mpy_32_32( matrix[2][2], src_vector[2] ) ); + + IF( result[2] < Mpy_32_32( L_shl( 1, ( q_matrix - 1 ) ), -21474836 ) ) // 21474836 = 0.01 in Q = 31 + { + return 0; + } + return 1; +} +#endif /*----------------------------------------------------------------------------------------------* * determine_best_triplet_and_gains() * @@ -1098,67 +1599,166 @@ static Word16 determine_best_triplet_and_gains_fx( * * Determines how the virtual node gains are distributed to real nodes *-------------------------------------------------------------------------*/ - -static void determine_virtual_speaker_node_division_gains( - const int16_t virtual_speaker_node_index, /* i : virtual speaker node index */ - float *virtual_node_division_gains, /* o : virtual speaker node division gains */ - int16_t connections[][2], /* i : vector of all connections */ - const enum VirtualSpeakerNodeType type, /* i : virtual speaker node typel */ - const int16_t max_num_connections, /* i : max number of connections */ - const int16_t num_speaker_nodes, /* i : max number of speaker nodes */ - const int16_t use_object_mode /* i : use VBAP in object panning mode vs. spatial panning mode */ +#ifdef IVAS_FLOAT_FIXED +static void determine_virtual_speaker_node_division_gains_fx( + const Word16 virtual_speaker_node_index, /* i : virtual speaker node index */ + Word16 *virtual_node_division_gains_fx, + Word16 *max_exp, + Word16 connections[][2], /* i : vector of all connections */ + const enum VirtualSpeakerNodeType type, /* i : virtual speaker node typel */ + const Word16 max_num_connections, /* i : max number of connections */ + const Word16 num_speaker_nodes, /* i : max number of speaker nodes */ + const Word16 use_object_mode /* i : use VBAP in object panning mode vs. spatial panning mode */ ) { /* When node type is VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, the gains of the virtual node are distributed to all neighboring real speaker nodes. An amplitude-division instead of energy division is utilized just in case to avoid excessive emphasis on the coherent distributed sound. */ - int16_t c, ch; - float sum_val; + Word16 c, ch, i; + Word16 sum_val_fx; + Word16 *exp_virtual_node_division_gains; + exp_virtual_node_division_gains = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ); - if ( type == VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY ) + set16_fx( exp_virtual_node_division_gains, 0, num_speaker_nodes ); + + IF( type == VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY ) { - for ( c = 0; c < max_num_connections; c++ ) + FOR( c = 0; c < max_num_connections; c++ ) { - if ( connections[c][0] != VBAP_NOT_VALID_CONNECTION ) + IF( connections[c][0] != VBAP_NOT_VALID_CONNECTION ) { - int16_t connection_node = -1; - if ( connections[c][0] == virtual_speaker_node_index ) + Word16 connection_node = -1; + IF( connections[c][0] == virtual_speaker_node_index ) { connection_node = connections[c][1]; } - else if ( connections[c][1] == virtual_speaker_node_index ) + ELSE IF( connections[c][1] == virtual_speaker_node_index ) { connection_node = connections[c][0]; } /* The second condition allows division gains only to actual loudspeakers */ - if ( connection_node >= 0 && ( connection_node < num_speaker_nodes ) ) + IF( connection_node >= 0 && ( connection_node < num_speaker_nodes ) ) { - virtual_node_division_gains[connection_node] = 1.0f; + virtual_node_division_gains_fx[connection_node] = ONE_IN_Q14; + exp_virtual_node_division_gains[connection_node] = 1; } } } - sum_val = 0.0f; - for ( ch = 0; ch < num_speaker_nodes; ch++ ) + sum_val_fx = 0; + Word16 guard_bits = find_guarded_bits_fx( num_speaker_nodes ); + FOR( ch = 0; ch < num_speaker_nodes; ch++ ) { - sum_val += virtual_node_division_gains[ch]; + sum_val_fx = add( sum_val_fx, shr( virtual_node_division_gains_fx[ch], guard_bits ) ); // Q10 } - - for ( ch = 0; ch < num_speaker_nodes; ch++ ) + Word16 final_exp = 0, res_exp; + Word32 tmp_1, tmp_2, tmp_3; + FOR( ch = 0; ch < num_speaker_nodes; ch++ ) { - virtual_node_division_gains[ch] /= sum_val; - if ( use_object_mode ) + IF( NE_16( virtual_node_division_gains_fx[ch], 0 ) ) { - virtual_node_division_gains[ch] = powf( virtual_node_division_gains[ch], 0.8f ); + BASOP_Util_Divide_MantExp( virtual_node_division_gains_fx[ch], 1, sum_val_fx, 1 + guard_bits, &virtual_node_division_gains_fx[ch], &final_exp ); // Q15 + } + ELSE + { + virtual_node_division_gains_fx[ch] = 0; + } + exp_virtual_node_division_gains[ch] = final_exp; + IF( use_object_mode ) + { + IF( virtual_node_division_gains_fx[ch] == 0 ) + { + tmp_1 = 0; + virtual_node_division_gains_fx[ch] = 0; + } + ELSE + { + Word32 tmp32 = L_deposit_h( virtual_node_division_gains_fx[ch] ); // q is 15-final_exp// ldep -> 31-final_exp (q) + tmp_1 = L_add( BASOP_Util_Log2( tmp32 ), L_shl( ( 31 - ( 31 - final_exp ) ), 25 ) ); // Q25 + tmp_2 = Mpy_32_32( float_to_fix( 0.8f, Q25 ), tmp_1 ); + tmp_3 = BASOP_util_Pow2( tmp_2, Q31 - Q19, &res_exp ); + exp_virtual_node_division_gains[ch] = res_exp; + virtual_node_division_gains_fx[ch] = extract_h( tmp_3 ); + } } } - } - - return; + /*make a common exp*/ + *max_exp = MIN16B; + FOR( i = 0; i < num_speaker_nodes; i++ ) + { + *max_exp = max( *max_exp, exp_virtual_node_division_gains[i] ); + } + FOR( i = 0; i < num_speaker_nodes; i++ ) + { + virtual_node_division_gains_fx[i] = shr( virtual_node_division_gains_fx[i], *max_exp - exp_virtual_node_division_gains[i] ); + } + } + + return; } +#endif + +static void determine_virtual_speaker_node_division_gains( + const int16_t virtual_speaker_node_index, /* i : virtual speaker node index */ + float *virtual_node_division_gains, /* o : virtual speaker node division gains */ + int16_t connections[][2], /* i : vector of all connections */ + const enum VirtualSpeakerNodeType type, /* i : virtual speaker node typel */ + const int16_t max_num_connections, /* i : max number of connections */ + const int16_t num_speaker_nodes, /* i : max number of speaker nodes */ + const int16_t use_object_mode /* i : use VBAP in object panning mode vs. spatial panning mode */ +) +{ + /* When node type is VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, the gains of the virtual node + are distributed to all neighboring real speaker nodes. An amplitude-division + instead of energy division is utilized just in case to avoid excessive emphasis + on the coherent distributed sound. */ + int16_t c, ch; + float sum_val; + + if ( type == VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY ) + { + for ( c = 0; c < max_num_connections; c++ ) + { + if ( connections[c][0] != VBAP_NOT_VALID_CONNECTION ) + { + int16_t connection_node = -1; + if ( connections[c][0] == virtual_speaker_node_index ) + { + connection_node = connections[c][1]; + } + else if ( connections[c][1] == virtual_speaker_node_index ) + { + connection_node = connections[c][0]; + } + + /* The second condition allows division gains only to actual loudspeakers */ + if ( connection_node >= 0 && ( connection_node < num_speaker_nodes ) ) + { + virtual_node_division_gains[connection_node] = 1.0f; + } + } + } + + sum_val = 0.0f; + for ( ch = 0; ch < num_speaker_nodes; ch++ ) + { + sum_val += virtual_node_division_gains[ch]; + } + + for ( ch = 0; ch < num_speaker_nodes; ch++ ) + { + virtual_node_division_gains[ch] /= sum_val; + if ( use_object_mode ) + { + virtual_node_division_gains[ch] = powf( virtual_node_division_gains[ch], 0.8f ); + } + } + } + return; +} /*-------------------------------------------------------------------------* * check_need_of_virtual_speaker_node() @@ -1167,6 +1767,106 @@ static void determine_virtual_speaker_node_division_gains( *-------------------------------------------------------------------------*/ /*! r: virtual speaker node type */ + +#ifdef IVAS_FLOAT_FIXED +static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node_fx( + VBAP_HANDLE hVBAPdata, /* i/o: VBAP structure */ + const Word32 *speaker_node_azi_deg_fx, /* i : vector of speaker node azimuths */ + const Word32 *speaker_node_ele_deg_fx, /* i : vector of speaker node elevations */ + enum SpeakerNodeGroup group /* i : group of speaker nodes where this belongs */ +) +{ + Word16 ch; + Word32 max_elevation_fx = 0; + Word16 Flag1, Flag2, Flag3; + + /* The following considers if SPEAKER_NODE_BACK virtual speaker is needed */ + IF( group == SPEAKER_NODE_BACK ) + { + Word16 virtual_back_needed = 1; + const Word16 virtual_back_epsilon_fx = -573; /* -0.0175f Q15*/ + FOR( ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++ ) + { + Flag1 = BASOP_Util_Cmp_Mant32Exp( speaker_node_ele_deg_fx[ch], 31 - Q22, 23040 /*45.0f Q9*/, 31 - Q9 ); + IF( EQ_16( Flag1, (Word16) -1 ) ) + { + Word16 azi_temp; + IF( GE_32( speaker_node_azi_deg_fx[ch], 0 ) ) + { + azi_temp = div_l( L_shr( speaker_node_azi_deg_fx[ch], Q7 ), 180 ); + } + ELSE + { + azi_temp = div_l( L_negate( L_shr( speaker_node_azi_deg_fx[ch], Q7 ) ), 180 ); + azi_temp = negate( azi_temp ); + } + + Word16 cos_res = getCosWord16R2( azi_temp ); // Q15 + + IF( LT_16( cos_res, virtual_back_epsilon_fx ) ) + { + virtual_back_needed = 0; + break; + } + } + } + + IF( virtual_back_needed ) + { + hVBAPdata->back_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal; + hVBAPdata->num_speaker_nodes_internal = add( hVBAPdata->num_speaker_nodes_internal, 1 ); + return VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY; + } + + return NO_VIRTUAL_SPEAKER_NODE; /* No virtual back needed */ + } + + /* The following considers if TOP or BOTTOM virtual speaker is needed */ + FOR( ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++ ) + { + IF( EQ_16( group, SPEAKER_NODE_TOP_HALF ) ) + { + IF( GT_32( speaker_node_ele_deg_fx[ch], max_elevation_fx ) ) + { + max_elevation_fx = speaker_node_ele_deg_fx[ch]; + } + } + ELSE + { + IF( GT_32( ( L_negate( speaker_node_ele_deg_fx[ch] ) ), max_elevation_fx ) ) + { + max_elevation_fx = L_negate( speaker_node_ele_deg_fx[ch] ); + } + } + } + Flag2 = BASOP_Util_Cmp_Mant32Exp( max_elevation_fx, 31 - Q22, 23039 /*44.9990005 Q9*/, 31 - Q9 ); + IF( EQ_16( Flag2, 1 ) ) + { + return NO_VIRTUAL_SPEAKER_NODE; + } + + /* Use virtual node */ + IF( EQ_16( group, SPEAKER_NODE_BOTTOM_HALF ) ) + { + hVBAPdata->bottom_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal; + } + ELSE + { + hVBAPdata->top_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal; + } + + hVBAPdata->num_speaker_nodes_internal = add( hVBAPdata->num_speaker_nodes_internal, 1 ); + Flag3 = BASOP_Util_Cmp_Mant32Exp( max_elevation_fx, 31 - Q22, 20478 /*19.9990005 Q10*/, 31 - Q10 ); + + IF( EQ_16( Flag3, 1 ) ) + { + return VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY; + } + + return VIRTUAL_SPEAKER_NODE_DISCARD_ENERGY; +} +#endif + static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node( VBAP_HANDLE hVBAPdata, /* i/o: VBAP structure */ const float *speaker_node_azi_deg, /* i : vector of speaker node azimuths */ @@ -1178,15 +1878,15 @@ static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node( float max_elevation = 0.0f; /* The following considers if SPEAKER_NODE_BACK virtual speaker is needed */ - if ( group == SPEAKER_NODE_BACK ) + if (group == SPEAKER_NODE_BACK) { int16_t virtual_back_needed = 1; const float virtual_back_epsilon = -0.0175f; /* Corresponds to approximately 91 degrees, see code below */ - for ( ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++ ) + for (ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++) { - if ( fabsf( speaker_node_ele_deg[ch] ) < VBAP_VIRTUAL_BACK_ELE_LIMIT ) + if (fabsf(speaker_node_ele_deg[ch]) < VBAP_VIRTUAL_BACK_ELE_LIMIT) { - if ( cosf( speaker_node_azi_deg[ch] * PI_OVER_180 ) < virtual_back_epsilon ) + if (cosf(speaker_node_azi_deg[ch] * PI_OVER_180) < virtual_back_epsilon) { virtual_back_needed = 0; break; @@ -1194,7 +1894,7 @@ static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node( } } - if ( virtual_back_needed ) + if (virtual_back_needed) { hVBAPdata->back_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal; hVBAPdata->num_speaker_nodes_internal++; @@ -1205,31 +1905,31 @@ static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node( } /* The following considers if TOP or BOTTOM virtual speaker is needed */ - for ( ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++ ) + for (ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++) { - if ( group == SPEAKER_NODE_TOP_HALF ) + if (group == SPEAKER_NODE_TOP_HALF) { - if ( speaker_node_ele_deg[ch] > max_elevation ) + if (speaker_node_ele_deg[ch] > max_elevation) { max_elevation = speaker_node_ele_deg[ch]; } } else { - if ( ( -speaker_node_ele_deg[ch] ) > max_elevation ) + if ((-speaker_node_ele_deg[ch]) > max_elevation) { max_elevation = -speaker_node_ele_deg[ch]; } } } - if ( max_elevation > VBAP_NO_VIRTUAL_SPEAKER_NODE_ELE_LIMIT - VBAP_EPSILON ) + if (max_elevation > VBAP_NO_VIRTUAL_SPEAKER_NODE_ELE_LIMIT - VBAP_EPSILON) { return NO_VIRTUAL_SPEAKER_NODE; } /* Use virtual node */ - if ( group == SPEAKER_NODE_BOTTOM_HALF ) + if (group == SPEAKER_NODE_BOTTOM_HALF) { hVBAPdata->bottom_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal; } @@ -1239,7 +1939,7 @@ static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node( } hVBAPdata->num_speaker_nodes_internal++; - if ( max_elevation > VBAP_DISTRIBUTE_VIRTUAL_SPEAKER_NODE_ELE_LIMIT - VBAP_EPSILON ) + if (max_elevation > VBAP_DISTRIBUTE_VIRTUAL_SPEAKER_NODE_ELE_LIMIT - VBAP_EPSILON) { return VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY; } @@ -1248,12 +1948,143 @@ static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node( } + /*-------------------------------------------------------------------------* * init_speaker_node_direction_data() * * Initialize speaker node data *-------------------------------------------------------------------------*/ +#ifdef IVAS_FLOAT_FIXED +static void init_speaker_node_direction_data_fx( + VBAP_SPEAKER_NODE *speaker_node_data, /* o : storage for speaker node data */ + const Word32 *speaker_node_azi_deg_fx, /* i : vector of speaker node azimuths */ + const Word32 *speaker_node_ele_deg_fx, /* i : vector of speaker node elevations */ + const Word16 num_speaker_nodes /* i : number of speaker nodes */ +) +{ + Word16 ch; + + Word16 azi_rad_fx = 0; + Word16 ele_rad_fx = 0; + + Word16 num_horiz = 0; + UWord8 in_all_mode = TRUE; + + FOR( ch = 0; ch < num_speaker_nodes; ch++ ) + { + speaker_node_data[ch].azi_deg_fx = speaker_node_azi_deg_fx[ch]; + + IF( GE_32( speaker_node_azi_deg_fx[ch], 0 ) ) + { + azi_rad_fx = div_l( L_shr( speaker_node_azi_deg_fx[ch], Q7 ), 180 ); + } + ELSE + { + azi_rad_fx = div_l( L_negate( L_shr( speaker_node_azi_deg_fx[ch], Q7 ) ), 180 ); + azi_rad_fx = negate( azi_rad_fx ); + } + + IF( GE_32( L_shr( speaker_node_ele_deg_fx[ch], 22 ), -5 ) && LE_32( L_shr( speaker_node_ele_deg_fx[ch], 22 ), 5 ) ) + { + speaker_node_data[ch].ele_deg_fx = 0; + ele_rad_fx = 0; + speaker_node_data[ch].group = SPEAKER_NODE_HORIZONTAL; + num_horiz = add( num_horiz, 1 ); + } + ELSE + { + speaker_node_data[ch].ele_deg_fx = speaker_node_ele_deg_fx[ch]; + IF( GE_32( speaker_node_ele_deg_fx[ch], 0 ) ) + { + ele_rad_fx = div_l( L_shr( speaker_node_ele_deg_fx[ch], Q7 ), 180 ); + } + ELSE + { + ele_rad_fx = div_l( L_negate( L_shr( speaker_node_ele_deg_fx[ch], Q7 ) ), 180 ); + ele_rad_fx = negate( ele_rad_fx ); + } + + IF( LT_16( ele_rad_fx, 0 ) ) + { + speaker_node_data[ch].group = SPEAKER_NODE_BOTTOM_HALF; + } + ELSE + { + speaker_node_data[ch].group = SPEAKER_NODE_TOP_HALF; + } + } + + speaker_node_data[ch].unit_vec_fx[0] = L_shr( L_mult( getCosWord16R2( azi_rad_fx ), getCosWord16R2( ele_rad_fx ) ), 1 ); // Q30 (add one gaurd bit , buffer being used in Q30) + speaker_node_data[ch].unit_vec_fx[1] = L_shr( L_mult( getSineWord16R2( azi_rad_fx ), getCosWord16R2( ele_rad_fx ) ), 1 ); // Q30 + speaker_node_data[ch].unit_vec_fx[2] = L_shr( L_deposit_h( getSineWord16R2( ele_rad_fx ) ), 1 ); // Q30 + } + /* Check for largest horizontal gap if there are at least 3 horizontal speaker nodes */ + IF( GE_16( num_horiz, 3 ) ) + { + Word16 i; + UWord16 horiz_azi[VBAP_MAX_NUM_SPEAKER_NODES]; + + UWord16 largest_gap; + UWord16 temp; + + i = 0; + FOR( ch = 0; ch < num_speaker_nodes && i < num_horiz; ch++ ) + { + IF( speaker_node_data[ch].group == SPEAKER_NODE_HORIZONTAL ) + { + Word16 exp1; + Word32 Mant2 = BASOP_Util_Add_Mant32Exp( speaker_node_azi_deg_fx[ch], 31 - 22, 23040, 31 - 6, &exp1 ); + + IF( LT_32( L_shr( speaker_node_azi_deg_fx[ch], 22 ), 0 ) ) + { + horiz_azi[i] = (UWord16) L_shr( Mant2, 31 - exp1 ); + } + ELSE + { + horiz_azi[i] = (UWord16) L_shr( speaker_node_azi_deg_fx[ch], 22 ); + } + i = add( i, 1 ); + } + } + + /* Reorder horizontal azi to increasing order */ + sort( horiz_azi, num_horiz ); + + /* Find largest gap. Initialize with the wrap over gap. */ + largest_gap = add( sub( horiz_azi[0], horiz_azi[num_horiz - 1] ), 360 ); + + FOR( ch = 0; ch < sub( num_horiz, 1 ); ch++ ) + { + temp = sub( horiz_azi[ch + 1], horiz_azi[ch] ); + IF( GT_16( temp, largest_gap ) ) + { + largest_gap = temp; + } + } + + /* If largest gap is small enough, we have definitive zero elevation plane. + * Otherwise, we should assign all speaker nodes to one group. */ + IF( LE_16( largest_gap, VBAP_MAX_HORIZONTAL_GAP ) ) + { + in_all_mode = FALSE; + } + } + + /* Designate all speaker nodes to same group if there was no definitive zero + * elevation plane. */ + IF( in_all_mode ) + { + FOR( ch = 0; ch < num_speaker_nodes; ch++ ) + { + speaker_node_data[ch].group = SPEAKER_NODE_ALL; + } + } + + return; +} +#endif + static void init_speaker_node_direction_data( VBAP_SPEAKER_NODE *speaker_node_data, /* o : storage for speaker node data */ const float *speaker_node_azi_deg, /* i : vector of speaker node azimuths */ @@ -1267,11 +2098,11 @@ static void init_speaker_node_direction_data( int16_t num_horiz = 0; uint8_t in_all_mode = TRUE; - for ( ch = 0; ch < num_speaker_nodes; ch++ ) + for (ch = 0; ch < num_speaker_nodes; ch++) { speaker_node_data[ch].azi_deg = speaker_node_azi_deg[ch]; azi_rad = speaker_node_azi_deg[ch] * PI_OVER_180; - if ( ( speaker_node_ele_deg[ch] >= -5 ) && ( speaker_node_ele_deg[ch] <= 5 ) ) + if ((speaker_node_ele_deg[ch] >= -5) && (speaker_node_ele_deg[ch] <= 5)) { speaker_node_data[ch].ele_deg = 0.0f; ele_rad = 0.0f; @@ -1282,7 +2113,7 @@ static void init_speaker_node_direction_data( { speaker_node_data[ch].ele_deg = speaker_node_ele_deg[ch]; ele_rad = speaker_node_ele_deg[ch] * PI_OVER_180; - if ( ele_rad < 0 ) + if (ele_rad < 0) { speaker_node_data[ch].group = SPEAKER_NODE_BOTTOM_HALF; } @@ -1292,13 +2123,13 @@ static void init_speaker_node_direction_data( } } - speaker_node_data[ch].unit_vec[0] = cosf( azi_rad ) * cosf( ele_rad ); - speaker_node_data[ch].unit_vec[1] = sinf( azi_rad ) * cosf( ele_rad ); - speaker_node_data[ch].unit_vec[2] = sinf( ele_rad ); + speaker_node_data[ch].unit_vec[0] = cosf(azi_rad) * cosf(ele_rad); + speaker_node_data[ch].unit_vec[1] = sinf(azi_rad) * cosf(ele_rad); + speaker_node_data[ch].unit_vec[2] = sinf(ele_rad); } /* Check for largest horizontal gap if there are at least 3 horizontal speaker nodes */ - if ( num_horiz >= 3 ) + if (num_horiz >= 3) { int16_t i; uint16_t horiz_azi[VBAP_MAX_NUM_SPEAKER_NODES]; @@ -1306,24 +2137,25 @@ static void init_speaker_node_direction_data( uint16_t temp; i = 0; - for ( ch = 0; ch < num_speaker_nodes && i < num_horiz; ch++ ) + for (ch = 0; ch < num_speaker_nodes && i < num_horiz; ch++) { - if ( speaker_node_data[ch].group == SPEAKER_NODE_HORIZONTAL ) + if (speaker_node_data[ch].group == SPEAKER_NODE_HORIZONTAL) { - horiz_azi[i] = speaker_node_azi_deg[ch] < 0.0f ? (uint16_t) floorf( speaker_node_azi_deg[ch] + 360.0f ) : (uint16_t) floorf( speaker_node_azi_deg[ch] ); + horiz_azi[i] = speaker_node_azi_deg[ch] < 0.0f ? (uint16_t)floorf(speaker_node_azi_deg[ch] + 360.0f) : (uint16_t)floorf(speaker_node_azi_deg[ch]); i++; } } /* Reorder horizontal azi to increasing order */ - sort( horiz_azi, num_horiz ); + sort(horiz_azi, num_horiz); /* Find largest gap. Initialize with the wrap over gap. */ largest_gap = horiz_azi[0] - horiz_azi[num_horiz - 1] + 360; - for ( ch = 0; ch < num_horiz - 1; ch++ ) + + for (ch = 0; ch < num_horiz - 1; ch++) { temp = horiz_azi[ch + 1] - horiz_azi[ch]; - if ( temp > largest_gap ) + if (temp > largest_gap) { largest_gap = temp; } @@ -1331,7 +2163,7 @@ static void init_speaker_node_direction_data( /* If largest gap is small enough, we have definitive zero elevation plane. * Otherwise, we should assign all speaker nodes to one group. */ - if ( largest_gap <= VBAP_MAX_HORIZONTAL_GAP ) + if (largest_gap <= VBAP_MAX_HORIZONTAL_GAP) { in_all_mode = FALSE; } @@ -1339,9 +2171,9 @@ static void init_speaker_node_direction_data( /* Designate all speaker nodes to same group if there was no definitive zero * elevation plane. */ - if ( in_all_mode ) + if (in_all_mode) { - for ( ch = 0; ch < num_speaker_nodes; ch++ ) + for (ch = 0; ch < num_speaker_nodes; ch++) { speaker_node_data[ch].group = SPEAKER_NODE_ALL; } @@ -1356,59 +2188,282 @@ static void init_speaker_node_direction_data( * 3-by-3 matrix inverse *-------------------------------------------------------------------------*/ -static void matrix_inverse_3x3( - const float **input_matrix, /* i : input matrix */ - float inverse_matrix[3][3] /* o : output matrix */ -) +#ifdef IVAS_FLOAT_FIXED +static void matrix_inverse_3x3_32_fx( + const Word32 **input_matrix_fx, /* i : input matrix */ + Word32 inverse_matrix_fx[3][3], /* o : output matrix */ + Word16 *exp_inv_mat ) { - int16_t k; - float determinant; - float cross_vec[3]; - - vbap_crossp( input_matrix[1], input_matrix[2], cross_vec ); + Word16 k; + Word32 determinant_fx; + Word32 cross_vec_fx[3]; + Word16 exp_inverse_matrix_fx[3][3]; + vbap_crossp_fx( input_matrix_fx[1], input_matrix_fx[2], cross_vec_fx ); + determinant_fx = dotp_fixed( input_matrix_fx[0], cross_vec_fx, 3 ); // Q30 + Q29 - Q31 = Q28 + Word16 inv_mat_exp = 0; + + FOR( k = 0; k < 3; k++ ) + { + inverse_matrix_fx[k][0] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) ); + inv_mat_exp = inv_mat_exp + ( ( 31 - 29 ) - ( 31 - 28 ) ); + exp_inverse_matrix_fx[k][0] = inv_mat_exp; + } - determinant = dotp( input_matrix[0], cross_vec, 3 ); + vbap_crossp_fx( input_matrix_fx[2], input_matrix_fx[0], cross_vec_fx ); - for ( k = 0; k < 3; k++ ) + FOR( k = 0; k < 3; k++ ) { - inverse_matrix[k][0] = cross_vec[k] / determinant; + inverse_matrix_fx[k][1] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) ); + inv_mat_exp = inv_mat_exp + ( ( 31 - 29 ) - ( 31 - 28 ) ); + exp_inverse_matrix_fx[k][1] = inv_mat_exp; } - vbap_crossp( input_matrix[2], input_matrix[0], cross_vec ); + vbap_crossp_fx( input_matrix_fx[0], input_matrix_fx[1], cross_vec_fx ); - for ( k = 0; k < 3; k++ ) + FOR( k = 0; k < 3; k++ ) { - inverse_matrix[k][1] = cross_vec[k] / determinant; + inverse_matrix_fx[k][2] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) ); + inv_mat_exp = inv_mat_exp + ( ( 31 - 29 ) - ( 31 - 28 ) ); + exp_inverse_matrix_fx[k][2] = inv_mat_exp; + } + + /*make common exponant*/ + Word16 max_exp = 0, i, j; + FOR( i = 0; i < 3; i++ ) + { + FOR( j = 0; j < 3; j++ ) + { + max_exp = s_max( max_exp, exp_inverse_matrix_fx[i][j] ); + } } - vbap_crossp( input_matrix[0], input_matrix[1], cross_vec ); + *exp_inv_mat = max_exp; - for ( k = 0; k < 3; k++ ) + FOR( i = 0; i < 3; i++ ) { - inverse_matrix[k][2] = cross_vec[k] / determinant; + FOR( j = 0; j < 3; j++ ) + { + IF( exp_inverse_matrix_fx[i][j] < -15 && inverse_matrix_fx[i][j] != 0 ) + { + inverse_matrix_fx[i][j] = 1; + exp_inverse_matrix_fx[i][j] = 0; + } + ELSE + { + inverse_matrix_fx[i][j] = L_shr( inverse_matrix_fx[i][j], max_exp - exp_inverse_matrix_fx[i][j] ); + } + } } return; } -/*-------------------------------------------------------------------------* - * check_and_store_triplet() - * - * Check if the given loudspeaker triplet is a valid one and store data when - * valid triplet is found. - *-------------------------------------------------------------------------*/ - -static int16_t check_and_store_triplet( - const int16_t chA, /* i : first channel index that forms the loudspeaker triplet */ - const int16_t chB, /* i : second channel index that forms the loudspeaker triplet */ - const int16_t chC, /* i : third channel index that forms the loudspeaker triplet */ - const int16_t num_speaker_nodes, /* i : number of speaker nodes */ - const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */ - VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */ - int16_t *triplet_index, /* i/o: index for the next free triplet slot */ - float *triplet_azidegs, /* o : center azimuths of the found triplets */ - int16_t *triplet_order /* o : initial order of triplet indices */ -) +static void matrix_inverse_3x3_fx( + const Word32 **input_matrix_fx, /* i : input matrix */ + Word16 inverse_matrix_fx[3][3], /* o : output matrix */ + Word16 *exp_inv_mat ) +{ + Word16 k; + Word32 determinant_fx; + Word32 cross_vec_fx[3]; + Word16 exp_inverse_matrix_fx[3][3]; + vbap_crossp_fx( input_matrix_fx[1], input_matrix_fx[2], cross_vec_fx ); + determinant_fx = dotp_fixed( input_matrix_fx[0], cross_vec_fx, 3 ); // Q30 + Q29 - Q31 = Q28 + Word16 inv_mat_exp = 0; + + FOR( k = 0; k < 3; k++ ) + { + inverse_matrix_fx[k][0] = BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ); + inv_mat_exp = inv_mat_exp + ( ( 31 - 29 ) - ( 31 - 28 ) ); + exp_inverse_matrix_fx[k][0] = inv_mat_exp; + } + + vbap_crossp_fx( input_matrix_fx[2], input_matrix_fx[0], cross_vec_fx ); + + FOR( k = 0; k < 3; k++ ) + { + inverse_matrix_fx[k][1] = BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ); + inv_mat_exp = inv_mat_exp + ( ( 31 - 29 ) - ( 31 - 28 ) ); + exp_inverse_matrix_fx[k][1] = inv_mat_exp; + } + + vbap_crossp_fx( input_matrix_fx[0], input_matrix_fx[1], cross_vec_fx ); + + FOR( k = 0; k < 3; k++ ) + { + inverse_matrix_fx[k][2] = BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ); + inv_mat_exp = inv_mat_exp + ( ( 31 - 29 ) - ( 31 - 28 ) ); + exp_inverse_matrix_fx[k][2] = inv_mat_exp; + } + + /*Find common exponant*/ + Word16 max_exp = 0, i, j; + FOR( i = 0; i < 3; i++ ) + { + FOR( j = 0; j < 3; j++ ) + { + max_exp = s_max( max_exp, exp_inverse_matrix_fx[i][j] ); + } + } + + *exp_inv_mat = max_exp; + + FOR( i = 0; i < 3; i++ ) + { + FOR( j = 0; j < 3; j++ ) + { + IF( exp_inverse_matrix_fx[i][j] < -15 && inverse_matrix_fx[i][j] != 0 ) + { + inverse_matrix_fx[i][j] = 1; + exp_inverse_matrix_fx[i][j] = 0; + } + ELSE + { + inverse_matrix_fx[i][j] = shr( inverse_matrix_fx[i][j], max_exp - exp_inverse_matrix_fx[i][j] ); + } + } + } + + return; +} +#endif + +static void matrix_inverse_3x3( + const float **input_matrix, /* i : input matrix */ + float inverse_matrix[3][3] /* o : output matrix */ +) +{ + int16_t k; + float determinant; + float cross_vec[3]; + + vbap_crossp(input_matrix[1], input_matrix[2], cross_vec); + + determinant = dotp(input_matrix[0], cross_vec, 3); + + for (k = 0; k < 3; k++) + { + inverse_matrix[k][0] = cross_vec[k] / determinant; + } + + vbap_crossp(input_matrix[2], input_matrix[0], cross_vec); + + for (k = 0; k < 3; k++) + { + inverse_matrix[k][1] = cross_vec[k] / determinant; + } + + vbap_crossp(input_matrix[0], input_matrix[1], cross_vec); + + for (k = 0; k < 3; k++) + { + inverse_matrix[k][2] = cross_vec[k] / determinant; + } + + return; +} +/*-------------------------------------------------------------------------* + * check_and_store_triplet() + * + * Check if the given loudspeaker triplet is a valid one and store data when + * valid triplet is found. + *-------------------------------------------------------------------------*/ +#ifdef IVAS_FLOAT_FIXED +static Word16 check_and_store_triplet_fx( + const Word16 chA, /* i : first channel index that forms the loudspeaker triplet */ + const Word16 chB, /* i : second channel index that forms the loudspeaker triplet */ + const Word16 chC, /* i : third channel index that forms the loudspeaker triplet */ + const Word16 num_speaker_nodes, /* i : number of speaker nodes */ + const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */ + VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */ + Word16 *triplet_index, /* i/o: index for the next free triplet slot */ + Word32 *triplet_azidegs_fx, + Word16 *triplet_order /* o : initial order of triplet indices */ +) +{ + Word16 ch_check; + Word16 k, i, j; + Word16 speaker_node_found_inside_triplet; + UWord8 triplet_ok; + + Word32 inverse_matrix_fx[3][3], unnormalized_gains_fx[3]; + set32_fx( unnormalized_gains_fx, 0, 3 ); + const Word32 *speaker_node_triplet_unit_vec_matrix_fx[3]; + + /* Triplet found, determine inverse matrix for VBAP formulation */ + + speaker_node_triplet_unit_vec_matrix_fx[0] = speaker_node_data[chA].unit_vec_fx; + speaker_node_triplet_unit_vec_matrix_fx[1] = speaker_node_data[chB].unit_vec_fx; + speaker_node_triplet_unit_vec_matrix_fx[2] = speaker_node_data[chC].unit_vec_fx; + Word16 exp_inv_mat = 31; + matrix_inverse_3x3_32_fx( speaker_node_triplet_unit_vec_matrix_fx, inverse_matrix_fx, &exp_inv_mat ); + triplets[*triplet_index].exp_inv_matrix = exp_inv_mat; + + /* Check through all speaker nodes that none of them are within the triplet. + * Node within the triplet is identified by that all three panning gains are positive. + * Epsilon-condition is for some small rounding issues.*/ + speaker_node_found_inside_triplet = 0; + FOR( ch_check = 0; ch_check < num_speaker_nodes; ch_check++ ) + { + IF( ( ch_check != chA ) && ( ch_check != chB ) && ( ch_check != chC ) ) + { + triplet_ok = vector_matrix_multiply_3x3_32_fx( speaker_node_data[ch_check].unit_vec_fx, inverse_matrix_fx, unnormalized_gains_fx, 31 - exp_inv_mat ); + IF( triplet_ok && ( GT_32( unnormalized_gains_fx[0], ( VBAP_EPSILON_Q31 << ( 30 - exp_inv_mat ) ) ) ) && ( GT_32( unnormalized_gains_fx[1], ( VBAP_EPSILON_Q31 << ( 30 - exp_inv_mat ) ) ) ) && ( GT_32( unnormalized_gains_fx[2], ( VBAP_EPSILON_Q31 << ( 30 - exp_inv_mat ) ) ) ) ) + { + speaker_node_found_inside_triplet = 1; + break; + } + } + } + + /* No speaker node inside triplet -> appropriate triplet found, save data. */ + IF( speaker_node_found_inside_triplet == 0 ) + { + triplets[*triplet_index].speaker_node[0] = (UWord8) chA; + triplets[*triplet_index].speaker_node[1] = (UWord8) chB; + triplets[*triplet_index].speaker_node[2] = (UWord8) chC; + FOR( k = 0; k < 3; k++ ) + { + Copy32( inverse_matrix_fx[k], triplets[*triplet_index].inverse_matrix_fx[k], 3 ); + } + /*TODO : Clean up float code*/ + FOR( i = 0; i < 3; i++ ) + { + FOR( j = 0; j < 3; j++ ) + { + triplets[*triplet_index].inverse_matrix[i][j] = me2f( triplets[*triplet_index].inverse_matrix_fx[i][j], exp_inv_mat ); + } + } + /* Get center azimuth for fast search use */ + Word32 tmp_a = L_add( L_shr( L_add( speaker_node_data[chA].unit_vec_fx[1], speaker_node_data[chB].unit_vec_fx[1] ), 2 ), L_shr( speaker_node_data[chC].unit_vec_fx[1], 2 ) ); // Q27 + Word32 tmp_b = L_add( L_shr( L_add( speaker_node_data[chA].unit_vec_fx[0], speaker_node_data[chB].unit_vec_fx[0] ), 2 ), L_shr( speaker_node_data[chC].unit_vec_fx[0], 2 ) ); // Q27 + Word16 tmp_tan = shr( BASOP_util_atan2( tmp_a, tmp_b, 0 ), Q13 - Q9 ); + triplet_azidegs_fx[*triplet_index] = L_mult( tmp_tan, 29335 /*_180_OVER_PI in Q9*/ ); // Q3 + + /* Store increasing order indices for the later sorting step. */ + triplet_order[*triplet_index] = *triplet_index; + + *triplet_index = add( *triplet_index, 1 ); + + return 1; + } + + /* Triplet was not good */ + return 0; +} +#endif + +static int16_t check_and_store_triplet( + const int16_t chA, /* i : first channel index that forms the loudspeaker triplet */ + const int16_t chB, /* i : second channel index that forms the loudspeaker triplet */ + const int16_t chC, /* i : third channel index that forms the loudspeaker triplet */ + const int16_t num_speaker_nodes, /* i : number of speaker nodes */ + const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */ + VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */ + int16_t *triplet_index, /* i/o: index for the next free triplet slot */ + float *triplet_azidegs, /* o : center azimuths of the found triplets */ + int16_t *triplet_order /* o : initial order of triplet indices */ +) { int16_t ch_check; int16_t k; @@ -1421,18 +2476,18 @@ static int16_t check_and_store_triplet( speaker_node_triplet_unit_vec_matrix[0] = speaker_node_data[chA].unit_vec; speaker_node_triplet_unit_vec_matrix[1] = speaker_node_data[chB].unit_vec; speaker_node_triplet_unit_vec_matrix[2] = speaker_node_data[chC].unit_vec; - matrix_inverse_3x3( speaker_node_triplet_unit_vec_matrix, inverse_matrix ); - + matrix_inverse_3x3(speaker_node_triplet_unit_vec_matrix, inverse_matrix); + /* Check through all speaker nodes that none of them are within the triplet. * Node within the triplet is identified by that all three panning gains are positive. * Epsilon-condition is for some small rounding issues.*/ speaker_node_found_inside_triplet = 0; - for ( ch_check = 0; ch_check < num_speaker_nodes; ch_check++ ) + for (ch_check = 0; ch_check < num_speaker_nodes; ch_check++) { - if ( ( ch_check != chA ) && ( ch_check != chB ) && ( ch_check != chC ) ) + if ((ch_check != chA) && (ch_check != chB) && (ch_check != chC)) { - triplet_ok = vector_matrix_multiply_3x3( speaker_node_data[ch_check].unit_vec, inverse_matrix, unnormalized_gains ); - if ( triplet_ok && unnormalized_gains[0] > VBAP_EPSILON && unnormalized_gains[1] > VBAP_EPSILON && unnormalized_gains[2] > VBAP_EPSILON ) + triplet_ok = vector_matrix_multiply_3x3(speaker_node_data[ch_check].unit_vec, inverse_matrix, unnormalized_gains); + if (triplet_ok && unnormalized_gains[0] > VBAP_EPSILON && unnormalized_gains[1] > VBAP_EPSILON && unnormalized_gains[2] > VBAP_EPSILON) { speaker_node_found_inside_triplet = 1; break; @@ -1441,25 +2496,25 @@ static int16_t check_and_store_triplet( } /* No speaker node inside triplet -> appropriate triplet found, save data. */ - if ( speaker_node_found_inside_triplet == 0 ) + if (speaker_node_found_inside_triplet == 0) { - triplets[*triplet_index].speaker_node[0] = (uint8_t) chA; - triplets[*triplet_index].speaker_node[1] = (uint8_t) chB; - triplets[*triplet_index].speaker_node[2] = (uint8_t) chC; - for ( k = 0; k < 3; k++ ) + triplets[*triplet_index].speaker_node[0] = (uint8_t)chA; + triplets[*triplet_index].speaker_node[1] = (uint8_t)chB; + triplets[*triplet_index].speaker_node[2] = (uint8_t)chC; + for (k = 0; k < 3; k++) { - mvr2r( inverse_matrix[k], triplets[*triplet_index].inverse_matrix[k], 3 ); + mvr2r(inverse_matrix[k], triplets[*triplet_index].inverse_matrix[k], 3); } /* Get center azimuth for fast search use */ - triplet_azidegs[*triplet_index] = atan2f( speaker_node_data[chA].unit_vec[1] + speaker_node_data[chB].unit_vec[1] + speaker_node_data[chC].unit_vec[1], - speaker_node_data[chA].unit_vec[0] + speaker_node_data[chB].unit_vec[0] + speaker_node_data[chC].unit_vec[0] ) * - _180_OVER_PI; + triplet_azidegs[*triplet_index] = atan2f(speaker_node_data[chA].unit_vec[1] + speaker_node_data[chB].unit_vec[1] + speaker_node_data[chC].unit_vec[1], + speaker_node_data[chA].unit_vec[0] + speaker_node_data[chB].unit_vec[0] + speaker_node_data[chC].unit_vec[0]) * + _180_OVER_PI; /* Store increasing order indices for the later sorting step. */ triplet_order[*triplet_index] = *triplet_index; - ( *triplet_index )++; + (*triplet_index)++; return 1; } @@ -1467,8 +2522,6 @@ static int16_t check_and_store_triplet( /* Triplet was not good */ return 0; } - - /*-------------------------------------------------------------------------* * determine_virtual_surface_triplets() * @@ -1498,12 +2551,12 @@ static int16_t determine_virtual_surface_triplets( int16_t triplet_order[VBAP_MAX_NUM_TRIPLETS]; /* Each connection can be used exactly by two different virtual surface triplets. */ - set_s( connection_uses_left, 2, VBAP_MAX_NUM_SPEAKER_NODES ); + set_s(connection_uses_left, 2, VBAP_MAX_NUM_SPEAKER_NODES); - for ( chA = 0; chA < num_speaker_nodes; chA++ ) + for (chA = 0; chA < num_speaker_nodes; chA++) { /* Early skip if not in correct group. */ - if ( speaker_node_data[chA].group != allowed_group ) + if (speaker_node_data[chA].group != allowed_group) { continue; } @@ -1511,9 +2564,9 @@ static int16_t determine_virtual_surface_triplets( /* Get all connections connected to current chA that have not been used by * two triplets yet. */ num_connected_to_chA = 0; - for ( k = 0; k < max_num_connections; k++ ) + for (k = 0; k < max_num_connections; k++) { - if ( ( connections[k][0] == chA || connections[k][1] == chA ) && connection_uses_left[k] > 0 ) + if ((connections[k][0] == chA || connections[k][1] == chA) && connection_uses_left[k] > 0) { connected_to_chA[num_connected_to_chA] = k; num_connected_to_chA++; @@ -1522,27 +2575,27 @@ static int16_t determine_virtual_surface_triplets( /* Check that we have enough connections to use. We need at least two available connections to form a triplet. * This can fail in later stages when all connections are already used. */ - if ( num_connected_to_chA < 2 ) + if (num_connected_to_chA < 2) { continue; } /* Try to form triplets from each valid connection. */ - for ( k = 0; k < num_connected_to_chA; k++ ) + for (k = 0; k < num_connected_to_chA; k++) { int16_t connect_index_k = connected_to_chA[k]; chB = connections[connect_index_k][0] == chA ? connections[connect_index_k][1] : connections[connect_index_k][0]; - for ( l = k + 1; l < num_connected_to_chA; l++ ) + for (l = k + 1; l < num_connected_to_chA; l++) { int16_t connect_index_l = connected_to_chA[l]; chC = connections[connect_index_l][0] == chA ? connections[connect_index_l][1] : connections[connect_index_l][0]; /* With chA, chB, and chC selected, we still need to find connection between chB and chC and verify that the triplet is valid */ - for ( m = 0; m < max_num_connections; m++ ) + for (m = 0; m < max_num_connections; m++) { - if ( ( connections[m][0] == chB && connections[m][1] == chC ) || ( connections[m][1] == chB && connections[m][0] == chC ) ) + if ((connections[m][0] == chB && connections[m][1] == chC) || (connections[m][1] == chB && connections[m][0] == chC)) { - if ( check_and_store_triplet( chA, chB, chC, num_speaker_nodes, speaker_node_data, triplets, &num_triplets, triplet_azidegs, triplet_order ) ) + if (check_and_store_triplet(chA, chB, chC, num_speaker_nodes, speaker_node_data, triplets, &num_triplets, triplet_azidegs, triplet_order)) { connection_uses_left[connect_index_k]--; connection_uses_left[connect_index_l]--; @@ -1555,7 +2608,7 @@ static int16_t determine_virtual_surface_triplets( /* Check if chA-chB connection has been used in two triplets already. If yes, then break out of loop * as this connection cannot be used for more triplets and we need to continue with another chA-chB * connection. */ - if ( connection_uses_left[connect_index_k] < 1 ) + if (connection_uses_left[connect_index_k] < 1) { break; } @@ -1565,19 +2618,113 @@ static int16_t determine_virtual_surface_triplets( /* All triplets should be stored now. Sort them for search use and then determine the initial search indices for * each search sector for this search struct. */ - v_sort_ind( triplet_azidegs, triplet_order, num_triplets ); - reorder_triplets( triplets, triplet_order, num_triplets ); - determine_initial_search_indices( num_triplets, triplet_azidegs, initial_search_indices ); + v_sort_ind(triplet_azidegs, triplet_order, num_triplets); + reorder_triplets(triplets, triplet_order, num_triplets); + determine_initial_search_indices(num_triplets, triplet_azidegs, initial_search_indices); return num_triplets; } +#ifdef IVAS_FLOAT_FIXED +static Word16 determine_virtual_surface_triplets_fx( + const Word16 num_speaker_nodes, /* i : number of speaker nodes */ + const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */ + Word16 connections[][2], /* i : vector of all connections */ + const Word16 max_num_connections, /* i : max number of connections */ + VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */ + Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS], /* o : initial search indices for this set of triplets corresponding to the search struct */ + enum SpeakerNodeGroup allowed_group /* i : group of allowed speaker nodes for forming the triplets in this call */ +) +{ + Word16 chA, chB, chC, k, l, m; + Word16 num_triplets = 0; + Word16 num_connected_to_chA; + Word16 connected_to_chA[VBAP_MAX_NUM_SPEAKER_NODES]; + Word16 connection_uses_left[VBAP_MAX_NUM_SPEAKER_NODES]; + Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS]; + Word16 triplet_order[VBAP_MAX_NUM_TRIPLETS]; + + /* Each connection can be used exactly by two different virtual surface triplets. */ + set16_fx( connection_uses_left, 2, VBAP_MAX_NUM_SPEAKER_NODES ); + + FOR( chA = 0; chA < num_speaker_nodes; chA++ ) + { + /* Early skip if not in correct group. */ + IF( NE_16( speaker_node_data[chA].group, allowed_group ) ) + { + continue; + } + + /* Get all connections connected to current chA that have not been used by + * two triplets yet. */ + num_connected_to_chA = 0; + FOR( k = 0; k < max_num_connections; k++ ) + { + IF( ( EQ_16( connections[k][0], chA ) || EQ_16( connections[k][1], chA ) ) && ( GT_16( connection_uses_left[k], 0 ) ) ) + { + connected_to_chA[num_connected_to_chA] = k; + num_connected_to_chA = add( num_connected_to_chA, 1 ); + } + } + + /* Check that we have enough connections to use. We need at least two available connections to form a triplet. + * This can fail in later stages when all connections are already used. */ + IF( LT_16( num_connected_to_chA, 2 ) ) + { + continue; + } + + /* Try to form triplets from each valid connection. */ + FOR( k = 0; k < num_connected_to_chA; k++ ) + { + Word16 connect_index_k = connected_to_chA[k]; + chB = connections[connect_index_k][0] == chA ? connections[connect_index_k][1] : connections[connect_index_k][0]; + FOR( l = k + 1; l < num_connected_to_chA; l++ ) + { + Word16 connect_index_l = connected_to_chA[l]; + chC = connections[connect_index_l][0] == chA ? connections[connect_index_l][1] : connections[connect_index_l][0]; + + /* With chA, chB, and chC selected, we still need to find connection between chB and chC and verify that the triplet is valid */ + FOR( m = 0; m < max_num_connections; m++ ) + { + IF( ( EQ_16( connections[m][0], chB ) && EQ_16( connections[m][1], chC ) ) || ( EQ_16( connections[m][1], chB ) && EQ_16( connections[m][0], chC ) ) ) + { + Word16 flag = check_and_store_triplet_fx( chA, chB, chC, num_speaker_nodes, speaker_node_data, triplets, &num_triplets, triplet_azidegs_fx, triplet_order ); + IF( flag == 1 ) + { + connection_uses_left[connect_index_k] = sub( connection_uses_left[connect_index_k], 1 ); + connection_uses_left[connect_index_l] = sub( connection_uses_left[connect_index_l], 1 ); + connection_uses_left[m] = sub( connection_uses_left[m], 1 ); + break; + } + } + } + + /* Check if chA-chB connection has been used in two triplets already. If yes, then break out of loop + * as this connection cannot be used for more triplets and we need to continue with another chA-chB + * connection. */ + IF( LT_16( connection_uses_left[connect_index_k], 1 ) ) + { + break; + } + } + } + } + + /* All triplets should be stored now. Sort them for search use and then determine the initial search indices for + * each search sector for this search struct. */ + v_sort_ind_fixed( triplet_azidegs_fx, triplet_order, num_triplets ); + reorder_triplets_fx( triplets, triplet_order, num_triplets ); + determine_initial_search_indices_fx( num_triplets, triplet_azidegs_fx /*Q19*/, initial_search_indices ); + + return num_triplets; +} +#endif /*-------------------------------------------------------------------------* * determine_initial_search_indices() * * Determine initial search indices used for fast search of correct triangle *-------------------------------------------------------------------------*/ - static void determine_initial_search_indices( const int16_t num_triplets, /* i : number of triplets */ const float triplet_azidegs[VBAP_MAX_NUM_TRIPLETS], /* i : azimuths of triplets (in degrees) */ @@ -1592,28 +2739,28 @@ static void determine_initial_search_indices( float min_azideg_diff; float azideg_diff; - for ( i = 0; i < VBAP_NUM_SEARCH_SECTORS; i++ ) + for (i = 0; i < VBAP_NUM_SEARCH_SECTORS; i++) { sector_border_start_azideg = i * VBAP_SEARCH_SECTOR_SIZE; - sector_border_end_azideg = ( i + 1 ) * VBAP_SEARCH_SECTOR_SIZE; - sector_reference_azideg = ( sector_border_start_azideg + sector_border_end_azideg ) / 2.0f; + sector_border_end_azideg = (i + 1) * VBAP_SEARCH_SECTOR_SIZE; + sector_reference_azideg = (sector_border_start_azideg + sector_border_end_azideg) / 2.0f; best_index = 0; min_azideg_diff = 9999.9f; - for ( j = 0; j < num_triplets; j++ ) + for (j = 0; j < num_triplets; j++) { azideg_diff = sector_reference_azideg - triplet_azidegs[j]; - if ( azideg_diff > 180.0f ) + if (azideg_diff > 180.0f) { azideg_diff -= 360.0f; } - else if ( azideg_diff < -180.0f ) + else if (azideg_diff < -180.0f) { azideg_diff += 360.0f; } - azideg_diff = fabsf( azideg_diff ); + azideg_diff = fabsf(azideg_diff); - if ( azideg_diff < min_azideg_diff ) + if (azideg_diff < min_azideg_diff) { min_azideg_diff = azideg_diff; best_index = j; @@ -1626,6 +2773,58 @@ static void determine_initial_search_indices( return; } +#ifdef IVAS_FLOAT_FIXED +static void determine_initial_search_indices_fx( + const Word16 num_triplets, /* i : number of triplets */ + const Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS], + Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS] /* o : initial search indices */ +) +{ + Word16 i, j; + Word32 sector_reference_azideg_fx; + Word32 sector_border_start_azideg_fx; + Word32 sector_border_end_azideg_fx; + int16_t best_index; + Word32 min_azideg_diff_fx; + Word32 azideg_diff_fx; + + FOR( i = 0; i < VBAP_NUM_SEARCH_SECTORS; i++ ) + { + sector_border_start_azideg_fx = imult3216( i, VBAP_SEARCH_SECTOR_SIZE_Q0 ); + sector_border_end_azideg_fx = imult3216( add( i, 1 ), VBAP_SEARCH_SECTOR_SIZE_Q0 ); + sector_reference_azideg_fx = L_shr( L_add( sector_border_start_azideg_fx, sector_border_end_azideg_fx ), 1 ); + + + best_index = 0; + min_azideg_diff_fx = ONE_IN_Q31; + + FOR( j = 0; j < num_triplets; j++ ) + { + azideg_diff_fx = L_sub( L_shl( sector_reference_azideg_fx, Q19 ), triplet_azidegs_fx[j] ); + + IF( GT_32( azideg_diff_fx, 94371840 ) ) + { + azideg_diff_fx = L_sub( azideg_diff_fx, 188743680 ); + } + ELSE IF( LT_32( azideg_diff_fx, -94371840 ) ) + { + azideg_diff_fx = L_add( azideg_diff_fx, 188743680 ); + } + azideg_diff_fx = L_abs( azideg_diff_fx ); + + IF( LT_32( azideg_diff_fx, min_azideg_diff_fx ) ) + { + min_azideg_diff_fx = azideg_diff_fx; + best_index = j; + } + } + + initial_search_indices[i] = best_index; + } + + return; +} +#endif /*-------------------------------------------------------------------------* * determine_connections() @@ -1633,6 +2832,67 @@ static void determine_initial_search_indices( * Determine all valid connections between all speaker nodes *-------------------------------------------------------------------------*/ +#ifdef IVAS_FLOAT_FIXED +static ivas_error determine_connections_fx( + const Word16 num_speaker_nodes, /* i : number of speaker nodes */ + const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */ + Word16 connections[][2], /* o : vector of connections */ + const Word16 max_num_connections, /* i : max number of connections */ + Word16 *group1_count, /* o : number of connections in first group */ + Word16 *group2_start, /* o : start of second group of connections */ + Word16 *group2_count /* o : number of connections in second group */ +) +{ + Word16 num_non_crossing_planes; + Word16 c; + Word16 connection_write_index = 0; + Word32 non_crossing_plane_elevation_deg_fx[VBAP_MAX_PLANES]; + + ivas_error error; + + set32_fx(non_crossing_plane_elevation_deg_fx, 0, VBAP_MAX_PLANES); + + /* Reset connection data */ + FOR (c = 0; c < max_num_connections; c++) + { + connections[c][0] = VBAP_NOT_VALID_CONNECTION; + } + + /* This function determines some prominent elevated planes, that are favoured in making node-node connections. */ + num_non_crossing_planes = determine_non_crossing_planes_fx(num_speaker_nodes, speaker_node_data, non_crossing_plane_elevation_deg_fx); + + /* Process in different mode based on the grouping. It is enough to check for first node. */ + IF (speaker_node_data[0].group == SPEAKER_NODE_ALL) + { + IF ((error = formulate_half_sphere_connections_fx(speaker_node_data, num_speaker_nodes, SPEAKER_NODE_ALL, connections, &connection_write_index, max_num_connections, num_non_crossing_planes, non_crossing_plane_elevation_deg_fx)) != IVAS_ERR_OK) + { + return error; + } + } + ELSE + { + /* The node-node connections are determined in three stages: bottom, horizontal, and top. */ + IF ((error = formulate_half_sphere_connections_fx(speaker_node_data, num_speaker_nodes, SPEAKER_NODE_BOTTOM_HALF, connections, &connection_write_index, max_num_connections, num_non_crossing_planes, non_crossing_plane_elevation_deg_fx)) != IVAS_ERR_OK) + { + return error; + } + *group2_start = connection_write_index; + + formulate_horizontal_connections_fx(speaker_node_data, num_speaker_nodes, connections, &connection_write_index); + *group1_count = connection_write_index; + + IF ((error = formulate_half_sphere_connections_fx(speaker_node_data, num_speaker_nodes, SPEAKER_NODE_TOP_HALF, connections, &connection_write_index, max_num_connections, num_non_crossing_planes, non_crossing_plane_elevation_deg_fx)) != IVAS_ERR_OK) + { + return error; + } + + *group2_count = connection_write_index - *group2_start; + } + + return IVAS_ERR_OK; +} +#endif + static ivas_error determine_connections( const int16_t num_speaker_nodes, /* i : number of speaker nodes */ const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */ @@ -1649,21 +2909,21 @@ static ivas_error determine_connections( float non_crossing_plane_elevation_deg[VBAP_MAX_PLANES]; ivas_error error; - set_f( non_crossing_plane_elevation_deg, 0.0f, VBAP_MAX_PLANES ); + set_f(non_crossing_plane_elevation_deg, 0.0f, VBAP_MAX_PLANES); /* Reset connection data */ - for ( c = 0; c < max_num_connections; c++ ) + for (c = 0; c < max_num_connections; c++) { connections[c][0] = VBAP_NOT_VALID_CONNECTION; } /* This function determines some prominent elevated planes, that are favoured in making node-node connections. */ - num_non_crossing_planes = determine_non_crossing_planes( num_speaker_nodes, speaker_node_data, non_crossing_plane_elevation_deg ); + num_non_crossing_planes = determine_non_crossing_planes(num_speaker_nodes, speaker_node_data, non_crossing_plane_elevation_deg); /* Process in different mode based on the grouping. It is enough to check for first node. */ - if ( speaker_node_data[0].group == SPEAKER_NODE_ALL ) + if (speaker_node_data[0].group == SPEAKER_NODE_ALL) { - if ( ( error = formulate_half_sphere_connections( speaker_node_data, num_speaker_nodes, SPEAKER_NODE_ALL, connections, &connection_write_index, max_num_connections, num_non_crossing_planes, non_crossing_plane_elevation_deg ) ) != IVAS_ERR_OK ) + if ((error = formulate_half_sphere_connections(speaker_node_data, num_speaker_nodes, SPEAKER_NODE_ALL, connections, &connection_write_index, max_num_connections, num_non_crossing_planes, non_crossing_plane_elevation_deg)) != IVAS_ERR_OK) { return error; } @@ -1671,16 +2931,16 @@ static ivas_error determine_connections( else { /* The node-node connections are determined in three stages: bottom, horizontal, and top. */ - if ( ( error = formulate_half_sphere_connections( speaker_node_data, num_speaker_nodes, SPEAKER_NODE_BOTTOM_HALF, connections, &connection_write_index, max_num_connections, num_non_crossing_planes, non_crossing_plane_elevation_deg ) ) != IVAS_ERR_OK ) + if ((error = formulate_half_sphere_connections(speaker_node_data, num_speaker_nodes, SPEAKER_NODE_BOTTOM_HALF, connections, &connection_write_index, max_num_connections, num_non_crossing_planes, non_crossing_plane_elevation_deg)) != IVAS_ERR_OK) { return error; } *group2_start = connection_write_index; - formulate_horizontal_connections( speaker_node_data, num_speaker_nodes, connections, &connection_write_index ); + formulate_horizontal_connections(speaker_node_data, num_speaker_nodes, connections, &connection_write_index); *group1_count = connection_write_index; - if ( ( error = formulate_half_sphere_connections( speaker_node_data, num_speaker_nodes, SPEAKER_NODE_TOP_HALF, connections, &connection_write_index, max_num_connections, num_non_crossing_planes, non_crossing_plane_elevation_deg ) ) != IVAS_ERR_OK ) + if ((error = formulate_half_sphere_connections(speaker_node_data, num_speaker_nodes, SPEAKER_NODE_TOP_HALF, connections, &connection_write_index, max_num_connections, num_non_crossing_planes, non_crossing_plane_elevation_deg)) != IVAS_ERR_OK) { return error; } @@ -1689,18 +2949,171 @@ static ivas_error determine_connections( return IVAS_ERR_OK; } - - /*-------------------------------------------------------------------------* * determine_connection_class() * * Determine the type of connection *-------------------------------------------------------------------------*/ +/*! r: type of connection */ +#ifdef IVAS_FLOAT_FIXED +static enum ConnectionClass determine_connection_class_fx( + const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */ + const Word16 num_speaker_nodes, /* i : number of speaker nodes */ + const enum SpeakerNodeGroup group, /* i : speaker node group */ + const Word16 chA, /* i : speaker node counter 1 */ + const Word16 chB /* i : speaker node counter 2 */ +) +{ + Word16 ch, k; + + const Word32 *p1_fx, *v2_fx; + Word32 v1v1_fx, v1v2_fx, v2v2_fx, v1p1_fx, v2p1_fx; + Word32 determinant_fx; + Word32 norm_distance_on_v1_fx; + Word32 vec_diff_fx[3]; + Word32 v1_fx[3]; + Word32 vTarget_fx[3]; + Word32 energy_sum_fx; + Word32 eq_value_fx; + Word32 uvecdot_fx; + + /* Check if connection passes through origin. This is not desired. + * When this happens, unit vectors point in opposite directions. */ + uvecdot_fx = L_add( L_shl( dotp_fixed( node_data[chA].unit_vec_fx, node_data[chB].unit_vec_fx, 3 ), 1 ), ONE_IN_Q30 ); // Q30 - adding one guard bit + + IF( LT_32( uvecdot_fx, VBAP_EPSILON_Q3O ) && GT_32( uvecdot_fx, L_negate( VBAP_EPSILON_Q3O ) ) ) + { + return CONNECTION_WITH_SPEAKER_NODE_BEHIND; + } + + /* This loop checks the chA-chB connection with respect to all loudspeakers: + - in case there is a node behind or nearly behind the connection line. These + connections need to be discarded. + - in case there is a node that is closer to the connection line than 1/5 of the + connection length AND at the same horizontal plane. These connections need to be + weighted with a penalty (a special case, for example avoiding elevated L,R,C triplet) + */ + FOR( ch = 0; ch < num_speaker_nodes; ch++ ) + { + /* Select speaker_node only within TOP or BOTTOM sphere half, not being part of chA-chB pair */ + IF( ( group == node_data[ch].group ) && ( ch != chA ) && ( ch != chB ) ) + { + /* The following lines formulate the point on the chA-chB-connection that is + nearest to the origo-ch-line */ + p1_fx = node_data[chA].unit_vec_fx; // Q30 + + FOR( k = 0; k < 3; k++ ) + { + v1_fx[k] = L_sub( L_shr( node_data[chB].unit_vec_fx[k], 2 ), L_shr( node_data[chA].unit_vec_fx[k], 2 ) ); // q28 (Add two guard bit) + } + v2_fx = node_data[ch].unit_vec_fx; // q30 + + v1v1_fx = dotp_fixed( v1_fx, v1_fx, 3 ); // Q25 + v1v2_fx = dotp_fixed( v1_fx, v2_fx, 3 ); // Q27 + v2v2_fx = ONE_IN_Q29; + + v1p1_fx = dotp_fixed( v1_fx, p1_fx, 3 ); // Q27 + v2p1_fx = dotp_fixed( v2_fx, p1_fx, 3 ); // Q29 + + Word32 tmp1 = Mpy_32_32( v1v2_fx, v1v2_fx ); // Q23 + Word32 tmp2 = Mpy_32_32( v1v1_fx, L_negate( v2v2_fx ) ); // Q23 + Word32 tmp3 = L_add( tmp1, tmp2 ); // Q23 + + determinant_fx = tmp3; // Q23 + + /* Norm distance = distance parameter on line chA-chB, determines point that is + nearest to origo-ch line. Distance 0 means chA and distance 1 means chB. Can be + outside this region as well.*/ + + Word32 tmp4 = Mpy_32_32( v2v2_fx, v1p1_fx ); // q25 + Word32 tmp5 = Mpy_32_32( L_negate( v1v2_fx ), v2p1_fx ); // q25 + Word32 tmp6 = L_add( tmp4, tmp5 ); // q25 + Word16 tmp7, exp = 0; + IF( determinant_fx == 0 ) + { + determinant_fx = 1; + } + tmp7 = BASOP_Util_Divide3232_Scale( ONE_IN_Q23, determinant_fx, &exp ); // Q(15 - exp) + norm_distance_on_v1_fx = Mpy_32_16_1( tmp6, tmp7 ); // Q25 + Q(15 - exp) - Q15 = Q(25 - exp) + + /* Continue only if the nearest point is between chA and chB */ + Word16 exp_vTarget = 0, exp_energy_sum = 0, exp_vec_diff = 0; + Word32 var1, var2; + Word16 vTarget_fx_e[3], vec_diff_e[3]; + IF( GT_32( norm_distance_on_v1_fx, 0 ) && LT_32( norm_distance_on_v1_fx, L_shr( ONE_IN_Q31, 31 - ( Q25 - exp ) ) ) ) + { + /* Formulate vTarget, that is an unit vector that goes through the determined point on chA-chB connection */ + energy_sum_fx = 0; + FOR( k = 0; k < 3; k++ ) + { + var1 = Mpy_32_32( norm_distance_on_v1_fx, v1_fx[k] ); // Q(25 - exp) + Q28 - 31 + vTarget_fx[k] = BASOP_Util_Add_Mant32Exp( p1_fx[k], 1, var1, 31 - ( Q25 - exp + Q28 - 31 ), &exp_vTarget ); + vTarget_fx_e[k] = exp_vTarget; + + var2 = Mpy_32_32( vTarget_fx[k], vTarget_fx[k] ); // 2*exp_vTarget + energy_sum_fx = BASOP_Util_Add_Mant32Exp( energy_sum_fx, exp_energy_sum, var2, 2 * exp_vTarget, &exp_energy_sum ); + vec_diff_fx[k] = BASOP_Util_Add_Mant32Exp( vTarget_fx[k], exp_vTarget, L_negate( v2_fx[k] ), 1, &exp_vec_diff ); + vec_diff_e[k] = exp_vec_diff; + } + Word16 exp_eq = exp_energy_sum; + eq_value_fx = ISqrt32( energy_sum_fx, &exp_eq ); + FOR( k = 0; k < 3; k++ ) + { + vTarget_fx[k] = Mpy_32_32( vTarget_fx[k], eq_value_fx ); + vTarget_fx_e[k] = vTarget_fx_e[k] + exp_eq; + } + /*make a common exponent*/ + Word16 max_vTarget_e = 0, max_vec_diff_e = 0; + FOR( k = 0; k < 3; k++ ) + { + max_vTarget_e = s_max( vTarget_fx_e[k], max_vTarget_e ); + max_vec_diff_e = s_max( vec_diff_e[k], max_vec_diff_e ); + } + FOR( k = 0; k < 3; k++ ) + { + vTarget_fx[k] = L_shr( vTarget_fx[k], sub( max_vTarget_e, vTarget_fx_e[k] ) ); + vec_diff_fx[k] = L_shr( vec_diff_fx[k], sub( max_vec_diff_e, vec_diff_e[k] ) ); + } + /* A check if the angle between vTarget and node_data[ch].unit_vec is less than 1 degree. + Essentially reveals if there is a speaker node too close "behind" the connection. Such + connections should be rejected.*/ + FOR( int i = 0; i < 3; i++ ) + { + vTarget_fx[i] = L_shr( vTarget_fx[i], 2 ); // add guard bits + vec_diff_fx[i] = L_shr( vec_diff_fx[i], 2 ); + } + Word32 res = dotp_fixed( vTarget_fx, v2_fx, 3 ); // 31 - (max_vTarget_e + 2) + 30 - 31 = 28 - max_vTarget_e + + IF( GT_32( res, L_shr( 2147054208, 31 - ( 28 - max_vTarget_e ) ) ) ) + { + return CONNECTION_WITH_SPEAKER_NODE_BEHIND; + } + + /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes. + A triplet between these nodes is not desired if there is a top node, + a penalty is implemented to take care of this. */ + Word32 vec_diff_dotp = dotp_fixed( vec_diff_fx, vec_diff_fx, 3 ); // exp : 2 * max_vec_diff_e + 4 + Word32 var = Mpy_32_32( vec_diff_dotp, 51200 /*25.0f in Q11*/ ); // exp : 2 * max_vec_diff_e + 4 + 20 + Word16 Flag1 = BASOP_Util_Cmp_Mant32Exp( v1v1_fx, 31 - Q25, var, 2 * max_vec_diff_e + 4 + 20 ); + IF( Flag1 == 1 ) + { + IF( LT_32( L_abs( L_sub( node_data[chB].unit_vec_fx[2], node_data[chA].unit_vec_fx[2] ) ), 2147483 ) ) + { + return ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION; + } + } + } + } + } + + return REGULAR_CONNECTION; +} +#endif /*! r: type of connection */ static enum ConnectionClass determine_connection_class( const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */ - const int16_t num_speaker_nodes, /* i : number of speaker nodes */ + const Word16 num_speaker_nodes, /* i : number of speaker nodes */ const enum SpeakerNodeGroup group, /* i : speaker node group */ const int16_t chA, /* i : speaker node counter 1 */ const int16_t chB /* i : speaker node counter 2 */ @@ -1720,8 +3133,8 @@ static enum ConnectionClass determine_connection_class( /* Check if connection passes through origin. This is not desired. * When this happens, unit vectors point in opposite directions. */ - uvecdot = dotp( node_data[chA].unit_vec, node_data[chB].unit_vec, 3 ) + 1.0f; - if ( uvecdot < VBAP_EPSILON && uvecdot > -VBAP_EPSILON ) + uvecdot = dotp(node_data[chA].unit_vec, node_data[chB].unit_vec, 3) + 1.0f; + if (uvecdot < VBAP_EPSILON && uvecdot > -VBAP_EPSILON) { return CONNECTION_WITH_SPEAKER_NODE_BEHIND; } @@ -1733,43 +3146,43 @@ static enum ConnectionClass determine_connection_class( connection length AND at the same horizontal plane. These connections need to be weighted with a penalty (a special case, for example avoiding elevated L,R,C triplet) */ - for ( ch = 0; ch < num_speaker_nodes; ch++ ) + for (ch = 0; ch < num_speaker_nodes; ch++) { /* Select speaker_node only within TOP or BOTTOM sphere half, not being part of chA-chB pair */ - if ( ( group == node_data[ch].group ) && ( ch != chA ) && ( ch != chB ) ) + if ((group == node_data[ch].group) && (ch != chA) && (ch != chB)) { /* The following lines formulate the point on the chA-chB-connection that is nearest to the origo-ch-line */ p1 = node_data[chA].unit_vec; - for ( k = 0; k < 3; k++ ) + for (k = 0; k < 3; k++) { v1[k] = node_data[chB].unit_vec[k] - node_data[chA].unit_vec[k]; } v2 = node_data[ch].unit_vec; - v1v1 = dotp( v1, v1, 3 ); - v1v2 = dotp( v1, v2, 3 ); + v1v1 = dotp(v1, v1, 3); + v1v2 = dotp(v1, v2, 3); v2v2 = 1.0f; /* dotp(v2, v2) is always 1. */ - v1p1 = dotp( v1, p1, 3 ); - v2p1 = dotp( v2, p1, 3 ); - determinant = ( v1v1 ) * ( -v2v2 ) + ( v1v2 * v1v2 ); + v1p1 = dotp(v1, p1, 3); + v2p1 = dotp(v2, p1, 3); + determinant = (v1v1) * (-v2v2) + (v1v2 * v1v2); /* Norm distance = distance parameter on line chA-chB, determines point that is nearest to origo-ch line. Distance 0 means chA and distance 1 means chB. Can be outside this region as well.*/ - norm_distance_on_v1 = ( 1.0f / determinant ) * ( ( v2v2 ) * ( v1p1 ) + ( -v1v2 ) * ( v2p1 ) ); + norm_distance_on_v1 = (1.0f / determinant) * ((v2v2) * (v1p1)+(-v1v2) * (v2p1)); /* Continue only if the nearest point is between chA and chB */ - if ( norm_distance_on_v1 > 0.0f && norm_distance_on_v1 < 1.0f ) + if (norm_distance_on_v1 > 0.0f && norm_distance_on_v1 < 1.0f) { /* Formulate vTarget, that is an unit vector that goes through the determined point on chA-chB connection */ energy_sum = 0.0f; - for ( k = 0; k < 3; k++ ) + for (k = 0; k < 3; k++) { vTarget[k] = p1[k] + norm_distance_on_v1 * v1[k]; energy_sum += vTarget[k] * vTarget[k]; vec_diff[k] = vTarget[k] - v2[k]; } - eq_value = sqrtf( 1.0f / energy_sum ); - for ( k = 0; k < 3; k++ ) + eq_value = sqrtf(1.0f / energy_sum); + for (k = 0; k < 3; k++) { vTarget[k] *= eq_value; } @@ -1777,7 +3190,7 @@ static enum ConnectionClass determine_connection_class( /* A check if the angle between vTarget and node_data[ch].unit_vec is less than 1 degree. Essentially reveals if there is a speaker node too close "behind" the connection. Such connections should be rejected.*/ - if ( dotp( vTarget, v2, 3 ) > 0.9998f ) + if (dotp(vTarget, v2, 3) > 0.9998f) { return CONNECTION_WITH_SPEAKER_NODE_BEHIND; } @@ -1785,9 +3198,9 @@ static enum ConnectionClass determine_connection_class( /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes. A triplet between these nodes is not desired if there is a top node, a penalty is implemented to take care of this. */ - if ( sqrtf( v1v1 ) > 5.0f * sqrtf( dotp( vec_diff, vec_diff, 3 ) ) ) + if (sqrtf(v1v1) > 5.0f * sqrtf(dotp(vec_diff, vec_diff, 3))) { - if ( fabsf( node_data[chB].unit_vec[2] - node_data[chA].unit_vec[2] ) < VBAP_EPSILON ) + if (fabsf(node_data[chB].unit_vec[2] - node_data[chA].unit_vec[2]) < VBAP_EPSILON) { return ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION; } @@ -1799,18 +3212,69 @@ static enum ConnectionClass determine_connection_class( return REGULAR_CONNECTION; } - /*-------------------------------------------------------------------------* * formulate_horizontal_connections() * * Formulate connections in the horizontal plane *-------------------------------------------------------------------------*/ +#ifdef IVAS_FLOAT_FIXED +static void formulate_horizontal_connections_fx( + const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */ + const Word16 num_speaker_nodes, /* i : number of speaker nodes */ + Word16 connections[][2], /* o : vector of all connections */ + Word16 *connection_write_index ) +{ + Word16 ch; + Word16 chCheck; + Word16 next_index; + + Word32 min_arc_diff_fx; + Word32 arc_diff_fx; + Word16 Q_min_arc_diff; + + FOR( ch = 0; ch < num_speaker_nodes; ch++ ) + { + /* Find next horizontal speaker node */ + IF( EQ_16( speaker_node_data[ch].group, SPEAKER_NODE_HORIZONTAL ) ) + { + next_index = -1; + min_arc_diff_fx = 19998 /*9999.0f in Q1*/; + Q_min_arc_diff = 1; + + FOR( chCheck = 0; chCheck < num_speaker_nodes; chCheck++ ) + { + IF( ( NE_16( ch, chCheck ) ) && ( EQ_16( speaker_node_data[chCheck].group, SPEAKER_NODE_HORIZONTAL ) ) ) + { + arc_diff_fx = L_sub( speaker_node_data[chCheck].azi_deg_fx, speaker_node_data[ch].azi_deg_fx ); // Q22 + + WHILE( LT_32( arc_diff_fx, 0 ) ) + { + arc_diff_fx = L_add( arc_diff_fx, 1509949440 /*360.0f in Q22*/ ); + } + IF( LT_32( L_shr( arc_diff_fx, 22 - Q_min_arc_diff ), min_arc_diff_fx ) ) + { + min_arc_diff_fx = arc_diff_fx; + Q_min_arc_diff = 22; + next_index = chCheck; + } + } + } + connections[*connection_write_index][0] = ch; + connections[*connection_write_index][1] = next_index; + *connection_write_index = add( *connection_write_index, 1 ); + } + } + + return; +} +#endif + static void formulate_horizontal_connections( const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */ const int16_t num_speaker_nodes, /* i : number of speaker nodes */ int16_t connections[][2], /* o : vector of all connections */ - int16_t *connection_write_index ) + int16_t *connection_write_index) { int16_t ch; int16_t chCheck; @@ -1818,77 +3282,238 @@ static void formulate_horizontal_connections( float min_arc_diff; float arc_diff; - for ( ch = 0; ch < num_speaker_nodes; ch++ ) + for (ch = 0; ch < num_speaker_nodes; ch++) { /* Find next horizontal speaker node */ - if ( speaker_node_data[ch].group == SPEAKER_NODE_HORIZONTAL ) + if (speaker_node_data[ch].group == SPEAKER_NODE_HORIZONTAL) { next_index = -1; min_arc_diff = 9999.0f; - for ( chCheck = 0; chCheck < num_speaker_nodes; chCheck++ ) + for (chCheck = 0; chCheck < num_speaker_nodes; chCheck++) { - if ( ( ch != chCheck ) && ( speaker_node_data[chCheck].group == SPEAKER_NODE_HORIZONTAL ) ) + if ((ch != chCheck) && (speaker_node_data[chCheck].group == SPEAKER_NODE_HORIZONTAL)) { arc_diff = speaker_node_data[chCheck].azi_deg - speaker_node_data[ch].azi_deg; - while ( arc_diff < 0.0f ) + while (arc_diff < 0.0f) { arc_diff += 360.0f; } - if ( arc_diff < min_arc_diff ) + if (arc_diff < min_arc_diff) { min_arc_diff = arc_diff; next_index = chCheck; } + } + } + connections[*connection_write_index][0] = ch; + connections[*connection_write_index][1] = next_index; + (*connection_write_index)++; + } + } + + return; +} + +/*-------------------------------------------------------------------------* + * check_plane_crossing() + * + * Check crossing of non-allowed planes + *-------------------------------------------------------------------------*/ + +/*! r: truth value for crossing */ +#ifdef IVAS_FLOAT_FIXED +static Word16 check_plane_crossing_fx( + const Word32 ele1_deg_fx, /* i : speaker node 1 elevation */ + const Word32 ele2_deg_fx, /* i : speaker node 2 elevation */ + const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */ + const Word32 *non_crossing_plane_elevation_deg_fx /* i : vector non-crossing plane elevations*/ +) +{ + /* Find if the connection crosses a non-crossing plane, with 1-degree threshold. */ + Word16 plane; + + FOR( plane = 0; plane < num_non_crossing_planes; plane++ ) + { + IF( ( GT_32( L_shr( ele1_deg_fx, 8 ), L_add( non_crossing_plane_elevation_deg_fx[plane], ONE_IN_Q14 ) ) ) && ( LT_32( L_shr( ele2_deg_fx, 8 ), L_sub( non_crossing_plane_elevation_deg_fx[plane], ONE_IN_Q14 ) ) ) ) + { + return 1; + } + IF( ( GT_32( L_shr( ele2_deg_fx, 8 ), L_add( non_crossing_plane_elevation_deg_fx[plane], ONE_IN_Q14 ) ) ) && ( LT_32( L_shr( ele1_deg_fx, 8 ), L_sub( non_crossing_plane_elevation_deg_fx[plane], ONE_IN_Q14 ) ) ) ) + { + return 1; + } + } + + return 0; +} +#endif + +static int16_t check_plane_crossing( + const float ele1_deg, /* i : speaker node 1 elevation */ + const float ele2_deg, /* i : speaker node 2 elevation */ + const int16_t num_non_crossing_planes, /* i : number of non-crossing planes */ + const float *non_crossing_plane_elevation_deg /* i : vector non-crossing plane elevations*/ +) +{ + /* Find if the connection crosses a non-crossing plane, with 1-degree threshold. */ + int16_t plane; + + for (plane = 0; plane < num_non_crossing_planes; plane++) + { + if ((ele1_deg > non_crossing_plane_elevation_deg[plane] + 1.0f) && (ele2_deg < non_crossing_plane_elevation_deg[plane] - 1.0f)) + { + return 1; + } + + if ((ele2_deg > non_crossing_plane_elevation_deg[plane] + 1.0f) && (ele1_deg < non_crossing_plane_elevation_deg[plane] - 1.0f)) + { + return 1; + } + } + + return 0; +} + + +/*-------------------------------------------------------------------------* + * get_half_sphere_connection_options() + * + * Get list of all potential connections at the half-sphere + *-------------------------------------------------------------------------*/ +#ifdef IVAS_FLOAT_FIXED +static ivas_error get_half_sphere_connection_options_fx( + const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */ + const enum SpeakerNodeGroup group, /* i : speaker node group */ + const Word16 num_speaker_nodes, /* i : number of speaker nodes */ + const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */ + const Word32 *non_crossing_plane_elevation_deg_fx, /* i : vector of non-crossing plane elevations */ + ConnectionOption **connection_options_pr, /* o : list of connection options */ + Word16 *num_connection_options /* o : number of connection options */ +) +{ + Word16 max_num_connection_options = 0; + Word16 index = 0; + Word16 node, chA, chB, c, c_cmp; + ConnectionOption *c_options, *c_options_reorder; + + /* Count max num connection options at the half sphere */ + FOR( node = 0; node < num_speaker_nodes; node++ ) + { + IF( speaker_node_data[node].group == group || speaker_node_data[node].group == SPEAKER_NODE_HORIZONTAL ) + { + max_num_connection_options = add( max_num_connection_options, index ); + index = add( index, 1 ); + } + } + + /* Init memory for connection options */ + IF( ( c_options = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * max_num_connection_options ) ) == NULL ) + { + return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); + } + + FOR( c = 0; c < max_num_connection_options; c++ ) + { + c_options[c].chA = -1; + c_options[c].chB = -1; + c_options[c].arc_fx = MAX_32; + c_options[c].arc_weighted_fx = MAX_32; + } + + /* Determine connection options for the half-sphere */ + index = 0; + FOR( chA = 0; chA < num_speaker_nodes; chA++ ) + { + /* First loudspeaker at the connection is at the half sphere */ + IF( speaker_node_data[chA].group == group || speaker_node_data[chA].group == SPEAKER_NODE_HORIZONTAL ) + { + FOR( chB = chA + 1; chB < num_speaker_nodes; chB++ ) + { + /* Second loudspeaker at the connection is at the half sphere, but so that first and second are not both horizontal. */ + IF( ( speaker_node_data[chB].group == group ) || ( speaker_node_data[chB].group == SPEAKER_NODE_HORIZONTAL && speaker_node_data[chA].group == group ) ) + { + Word16 ConnectionClass = determine_connection_class_fx( speaker_node_data, num_speaker_nodes, group, chA, chB ); + Word32 unit_vec_dotp, unit_vec_dotp_sq, one_minus_unit_vec_dotp_sq, one_minus_unit_vec_dotp_sq_root; + Word16 acos_val; + /* Connection is considered only if there is no speaker node behind it */ + IF( ConnectionClass != CONNECTION_WITH_SPEAKER_NODE_BEHIND ) + { + /* Store connection information */ + c_options[index].chA = chA; + c_options[index].chB = chB; + + unit_vec_dotp = dotp_fixed( speaker_node_data[chA].unit_vec_fx, speaker_node_data[chB].unit_vec_fx, 3 ); // Q29 + unit_vec_dotp_sq = Mpy_32_32( unit_vec_dotp, unit_vec_dotp ); // Q27 + one_minus_unit_vec_dotp_sq = L_sub( ONE_IN_Q27, unit_vec_dotp_sq ); + Word16 exp_uv = 31 - Q27; + one_minus_unit_vec_dotp_sq_root = Sqrt32( one_minus_unit_vec_dotp_sq, &exp_uv ); + acos_val = BASOP_util_atan2( one_minus_unit_vec_dotp_sq_root, unit_vec_dotp, exp_uv - 2 ); // Q13 + c_options[index].arc_fx = L_deposit_h( acos_val ); // Q29 + c_options[index].arc_weighted_fx = c_options[index].arc_fx; // Q29 + + /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes. + A triplet between these nodes is not desired if there is a top node, + a penalty is implemented to take care of this. */ + IF( ConnectionClass == ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION ) + { + c_options[index].arc_weighted_fx = L_shl( c_options[index].arc_weighted_fx, 1 ); + } + + /* If the connection passes a pre-determined plane of speaker nodes, then add further penalty */ + + IF( check_plane_crossing_fx( speaker_node_data[chA].ele_deg_fx /*q22*/, speaker_node_data[chB].ele_deg_fx, num_non_crossing_planes, non_crossing_plane_elevation_deg_fx ) ) + { + c_options[index].arc_weighted_fx = L_shl( c_options[index].arc_weighted_fx, 1 ); + } + index = add( index, 1 ); + } } } - connections[*connection_write_index][0] = ch; - connections[*connection_write_index][1] = next_index; - ( *connection_write_index )++; } } - - return; -} - -/*-------------------------------------------------------------------------* - * check_plane_crossing() - * - * Check crossing of non-allowed planes - *-------------------------------------------------------------------------*/ - -/*! r: truth value for crossing */ -static int16_t check_plane_crossing( - const float ele1_deg, /* i : speaker node 1 elevation */ - const float ele2_deg, /* i : speaker node 2 elevation */ - const int16_t num_non_crossing_planes, /* i : number of non-crossing planes */ - const float *non_crossing_plane_elevation_deg /* i : vector non-crossing plane elevations*/ -) -{ - /* Find if the connection crosses a non-crossing plane, with 1-degree threshold. */ - int16_t plane; - - for ( plane = 0; plane < num_non_crossing_planes; plane++ ) + /* Number of found connection options at the half sphere */ + *num_connection_options = index; + + /* Init memory for reordered connection options and order by arc_weighted, + * which informs of the preference order of the connections in case they cross */ + IF( ( c_options_reorder = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * ( *num_connection_options ) ) ) == NULL ) { - if ( ( ele1_deg > non_crossing_plane_elevation_deg[plane] + 1.0f ) && ( ele2_deg < non_crossing_plane_elevation_deg[plane] - 1.0f ) ) - { - return 1; - } - - if ( ( ele2_deg > non_crossing_plane_elevation_deg[plane] + 1.0f ) && ( ele1_deg < non_crossing_plane_elevation_deg[plane] - 1.0f ) ) + return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); + } + + FOR( c = 0; c < *num_connection_options; c++ ) + { + Word16 min_arc_index; + min_arc_index = -1; + Word32 min_arc_weighted_fx_new = MAX_32; + Word16 exp_min_arc_weighted_fx = 31; + Word16 res = 0; + FOR( c_cmp = 0; c_cmp < *num_connection_options; c_cmp++ ) { - return 1; + res = BASOP_Util_Cmp_Mant32Exp( c_options[c_cmp].arc_weighted_fx, 31 - Q29, min_arc_weighted_fx_new, exp_min_arc_weighted_fx ); + // todo : check condition + IF( res == -1 || res == 0 ) + { + min_arc_weighted_fx_new = c_options[c_cmp].arc_weighted_fx; + exp_min_arc_weighted_fx = 31 - Q29; + min_arc_index = c_cmp; + } } + c_options_reorder[c].chA = c_options[min_arc_index].chA; + c_options_reorder[c].chB = c_options[min_arc_index].chB; + + c_options_reorder[c].arc_fx = c_options[min_arc_index].arc_fx; + c_options_reorder[c].arc_weighted_fx = c_options[min_arc_index].arc_weighted_fx; + c_options[min_arc_index].arc_weighted_fx = MAX_32; } - - return 0; + + /* Set reordered connections as output and free temporary data */ + *connection_options_pr = c_options_reorder; + free( c_options ); + + return IVAS_ERR_OK; } - - -/*-------------------------------------------------------------------------* - * get_half_sphere_connection_options() - * - * Get list of all potential connections at the half-sphere - *-------------------------------------------------------------------------*/ +#endif static ivas_error get_half_sphere_connection_options( const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */ @@ -1906,9 +3531,9 @@ static ivas_error get_half_sphere_connection_options( ConnectionOption *c_options, *c_options_reorder; /* Count max num connection options at the half sphere */ - for ( node = 0; node < num_speaker_nodes; node++ ) + for (node = 0; node < num_speaker_nodes; node++) { - if ( speaker_node_data[node].group == group || speaker_node_data[node].group == SPEAKER_NODE_HORIZONTAL ) + if (speaker_node_data[node].group == group || speaker_node_data[node].group == SPEAKER_NODE_HORIZONTAL) { max_num_connection_options += index; index++; @@ -1916,12 +3541,12 @@ static ivas_error get_half_sphere_connection_options( } /* Init memory for connection options */ - if ( ( c_options = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * max_num_connection_options ) ) == NULL ) + if ((c_options = (ConnectionOption *)malloc(sizeof(ConnectionOption) * max_num_connection_options)) == NULL) { - return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); } - for ( c = 0; c < max_num_connection_options; c++ ) + for (c = 0; c < max_num_connection_options; c++) { c_options[c].chA = -1; c_options[c].chB = -1; @@ -1931,36 +3556,36 @@ static ivas_error get_half_sphere_connection_options( /* Determine connection options for the half-sphere */ index = 0; - for ( chA = 0; chA < num_speaker_nodes; chA++ ) + for (chA = 0; chA < num_speaker_nodes; chA++) { /* First loudspeaker at the connection is at the half sphere */ - if ( speaker_node_data[chA].group == group || speaker_node_data[chA].group == SPEAKER_NODE_HORIZONTAL ) + if (speaker_node_data[chA].group == group || speaker_node_data[chA].group == SPEAKER_NODE_HORIZONTAL) { - for ( chB = chA + 1; chB < num_speaker_nodes; chB++ ) + for (chB = chA + 1; chB < num_speaker_nodes; chB++) { /* Second loudspeaker at the connection is at the half sphere, but so that first and second are not both horizontal. */ - if ( ( speaker_node_data[chB].group == group ) || ( speaker_node_data[chB].group == SPEAKER_NODE_HORIZONTAL && speaker_node_data[chA].group == group ) ) + if ((speaker_node_data[chB].group == group) || (speaker_node_data[chB].group == SPEAKER_NODE_HORIZONTAL && speaker_node_data[chA].group == group)) { - int16_t ConnectionClass = determine_connection_class( speaker_node_data, num_speaker_nodes, group, chA, chB ); + int16_t ConnectionClass = determine_connection_class(speaker_node_data, num_speaker_nodes, group, chA, chB); /* Connection is considered only if there is no speaker node behind it */ - if ( ConnectionClass != CONNECTION_WITH_SPEAKER_NODE_BEHIND ) + if (ConnectionClass != CONNECTION_WITH_SPEAKER_NODE_BEHIND) { /* Store connection information */ c_options[index].chA = chA; c_options[index].chB = chB; - c_options[index].arc = acosf( dotp( speaker_node_data[chA].unit_vec, speaker_node_data[chB].unit_vec, 3 ) ); + c_options[index].arc = acosf(dotp(speaker_node_data[chA].unit_vec, speaker_node_data[chB].unit_vec, 3)); c_options[index].arc_weighted = c_options[index].arc; /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes. A triplet between these nodes is not desired if there is a top node, a penalty is implemented to take care of this. */ - if ( ConnectionClass == ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION ) + if (ConnectionClass == ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION) { c_options[index].arc_weighted *= 2.0f; } /* If the connection passes a pre-determined plane of speaker nodes, then add further penalty */ - if ( check_plane_crossing( speaker_node_data[chA].ele_deg, speaker_node_data[chB].ele_deg, num_non_crossing_planes, non_crossing_plane_elevation_deg ) ) + if (check_plane_crossing(speaker_node_data[chA].ele_deg, speaker_node_data[chB].ele_deg, num_non_crossing_planes, non_crossing_plane_elevation_deg)) { c_options[index].arc_weighted *= 2.0f; } @@ -1975,21 +3600,21 @@ static ivas_error get_half_sphere_connection_options( /* Init memory for reordered connection options and order by arc_weighted, * which informs of the preference order of the connections in case they cross */ - if ( ( c_options_reorder = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * ( *num_connection_options ) ) ) == NULL ) + if ((c_options_reorder = (ConnectionOption *)malloc(sizeof(ConnectionOption) * (*num_connection_options))) == NULL) { - return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) ); + return (IVAS_ERROR(IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n")); } - for ( c = 0; c < *num_connection_options; c++ ) + for (c = 0; c < *num_connection_options; c++) { float min_arc_weighted; int16_t min_arc_index; min_arc_weighted = 1e20f; min_arc_index = -1; - for ( c_cmp = 0; c_cmp < *num_connection_options; c_cmp++ ) + for (c_cmp = 0; c_cmp < *num_connection_options; c_cmp++) { - if ( c_options[c_cmp].arc_weighted <= min_arc_weighted ) + if (c_options[c_cmp].arc_weighted <= min_arc_weighted) { min_arc_weighted = c_options[c_cmp].arc_weighted; min_arc_index = c_cmp; @@ -2004,17 +3629,240 @@ static ivas_error get_half_sphere_connection_options( /* Set reordered connections as output and free temporary data */ *connection_options_pr = c_options_reorder; - free( c_options ); + free(c_options); return IVAS_ERR_OK; } - /*-------------------------------------------------------------------------* * formulate_half_sphere_connections() * * Formulate half-sphere connections *-------------------------------------------------------------------------*/ +#ifdef IVAS_FLOAT_FIXED +static ivas_error formulate_half_sphere_connections_fx( + const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */ + const Word16 num_speaker_nodes, /* i : number of speaker nodes */ + const enum SpeakerNodeGroup group, /* i : speaker node group */ + Word16 connections[][2], /* o : vector of connections */ + Word16 *connection_write_index, + const Word16 max_num_connections, /* i : max number of connections */ + const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */ + const Word32 *non_crossing_plane_elevation_deg_fx /* i : vector of non-crossing plane elevations */ +) +{ + /* Variable initializations */ + Word16 c, chA, chB, cmp_chA, cmp_chB, k, c_opt; + Word16 new_connection_is_valid; + Word16 within_first_arc; + Word32 new_cross_fx[3]; + Word32 planeCrossingVec_fx[3]; + Word16 Q_planeCrossingVec; + Word32 new_arc_fx; + Word32 connection_arc_fx[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3]; + Word32 connection_cross_fx[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3][3]; + Word32 tmpFloat_fx; + Word32 cmp_arc_fx; + Word32 normVal_fx; + Word16 angleCmp_fx; + ConnectionOption *connection_options; + Word16 num_connection_options; + Word16 half_sphere_first_connection; + ivas_error error; + + half_sphere_first_connection = *connection_write_index; + + /* Obtain all connection options (i.e., channel pairs) at the half sphere. The function orders them + * in terms of which connection to keep if two connections would cross each other. */ + IF ( ( error = get_half_sphere_connection_options_fx( + speaker_node_data, + group, + num_speaker_nodes, + num_non_crossing_planes, + non_crossing_plane_elevation_deg_fx, + &connection_options, + &num_connection_options ) ) != IVAS_ERR_OK ) + { + return error; + } + + set32_fx( connection_arc_fx, 0, max_num_connections ); + FOR( c = 0; c < max_num_connections; c++ ) + { + set32_fx( connection_cross_fx[c], 0, 3 ); + } + + /* The following loop goes through all reasonable chA - chB pairs for the half-sphere */ + c_opt = 0; + WHILE( c_opt < num_connection_options && *connection_write_index < max_num_connections ) + { + chA = connection_options[c_opt].chA; + chB = connection_options[c_opt].chB; + new_arc_fx = connection_options[c_opt].arc_fx; + + /* Cross-product is needed for later stages */ + vbap_crossp_fx( speaker_node_data[chA].unit_vec_fx, speaker_node_data[chB].unit_vec_fx, new_cross_fx ); + + /* Determine if new connection between chA and chB is valid */ + new_connection_is_valid = 1; + c = half_sphere_first_connection; + Word32 var1, var2, var1_sq, var2_sq, var1_sqrt, var2_sqrt, one_minus_var1_sq, one_minus_var2_sq, var_a, var_b, var_c; + Word16 var1_cos, var2_cos, final_exp, final_exp_A, final_exp_B, exp_var1_sq, exp_var2_sq, comp1, comp2, sub_exp = 0, sub_exp_2 = 0, sub_final_exp = 0; + WHILE( ( c < *connection_write_index ) && new_connection_is_valid ) + { + cmp_chA = connections[c][0]; + cmp_chB = connections[c][1]; + /* The connections are compared only if they don't involve same speaker nodes */ + IF( ( cmp_chA != chA ) && ( cmp_chA != chB ) && ( cmp_chB != chA ) && ( cmp_chB != chB ) ) + { + /* The following lines determine if the connection chA-chB crosses with the connection cmp_chA-cmp_chB.*/ + /* The connections, i.e., node-pairs determine a plane. The crossing can be determined by + * studying the intersection of these planes. */ + vbap_crossp_fx( connection_cross_fx[c], new_cross_fx, planeCrossingVec_fx ); + + tmpFloat_fx = 0; + + cmp_arc_fx = connection_arc_fx[c]; + FOR( k = 0; k < 3; k++ ) + { + tmpFloat_fx = L_add( tmpFloat_fx, Mpy_32_32( planeCrossingVec_fx[k], planeCrossingVec_fx[k] ) ); + } + + Word16 tmp_exp = 31 - Q23; + IF( tmpFloat_fx == 0 ) + { + tmpFloat_fx = 1; + } + normVal_fx = ISqrt32( tmpFloat_fx, &tmp_exp ); + + FOR( k = 0; k < 3; k++ ) + { + planeCrossingVec_fx[k] = Mpy_32_32( planeCrossingVec_fx[k], normVal_fx ); // Q27 + Q31 - tmp_exp - Q31 + } + /*update Q for planeCrossingVec */ + Q_planeCrossingVec = Q27 + Q31 - tmp_exp - Q31; + /* If the plane intersection is between both connections, then the two connections cross. */ + /* Study first if the crossing is between arc chA-chB */ + + var1 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[chA].unit_vec_fx, 3 ); // Q24 = 26 - tmp_exp + var2 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[chB].unit_vec_fx, 3 ); // Q24 = 26 - tmp_exp + + var1_sq = Mpy_32_32( var1, var1 ); //(2 * (Q_planeCrossingVec - Q1) ) - Q31 + var2_sq = Mpy_32_32( var2, var2 ); + exp_var1_sq = 2 * ( Q31 - ( Q_planeCrossingVec - Q1 ) ); + exp_var2_sq = 2 * ( Q31 - ( Q_planeCrossingVec - Q1 ) ); + IF( L_abs( var1_sq ) > L_shr( ONE_IN_Q31, exp_var1_sq ) ) + { + var1_sq = ONE_IN_Q31; + exp_var1_sq = 0; + } + IF( L_abs( var2_sq ) > L_shr( ONE_IN_Q31, exp_var2_sq ) ) + { + var2_sq = ONE_IN_Q31; + exp_var2_sq = 0; + } + one_minus_var1_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var1_sq ), exp_var1_sq, &final_exp_A ); + var1_sqrt = Sqrt32( one_minus_var1_sq, &final_exp_A ); + var1_cos = BASOP_util_atan2( var1_sqrt, var1, final_exp_A - ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ); // Q13 + angleCmp_fx = var1_cos; + + one_minus_var2_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var2_sq ), exp_var2_sq, &final_exp_B ); + var2_sqrt = Sqrt32( one_minus_var2_sq, &final_exp_B ); + var2_cos = BASOP_util_atan2( var2_sqrt, var2, final_exp_B - ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ); // Q13 + + final_exp = BASOP_Util_Add_MantExp( angleCmp_fx, Q15 - Q13, var2_cos, Q15 - Q13, &angleCmp_fx ); + + sub_exp = 0, sub_exp_2 = 0, sub_final_exp = 0; + var_a = BASOP_Util_Add_Mant32Exp( new_arc_fx, Q31 - Q29, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp ); + comp1 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_a ), sub_exp, 21474836, 0 ); + var_b = BASOP_Util_Add_Mant32Exp( 25735, Q31 - Q12, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp_2 ); + var_c = BASOP_Util_Add_Mant32Exp( new_arc_fx, Q31 - Q29, L_negate( var_b ), sub_exp_2, &sub_final_exp ); + comp2 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_c ), sub_final_exp, 21474836, 0 ); + + within_first_arc = 0; + IF( comp1 == -1 ) + { + within_first_arc = 1; + } + ELSE IF( comp2 == -1 ) + { + within_first_arc = 1; + /* In this case, the plane crossing vector is inverted. The inverse is another + * plane-crossing vector, and detected to be between chA-chB connection.*/ + FOR( k = 0; k < 3; k++ ) + { + planeCrossingVec_fx[k] = Mpy_32_32( planeCrossingVec_fx[k], -134217728 /*-1.0f in Q27*/ ); // 27 + 27 - tmp_exp - 31 = 23 - tmp_exp + } + /*update Q for planeCrossingVec */ + Q_planeCrossingVec = Q27 + Q27 - tmp_exp - Q31; + } + + /* Study if the crossing is also between arc cmp_chA-cmp_chB */ + IF( within_first_arc > 0 ) + { + var1 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[cmp_chA].unit_vec_fx, 3 ); + var2 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[cmp_chB].unit_vec_fx, 3 ); + + final_exp_A, final_exp_B, exp_var1_sq, exp_var2_sq; + var1_sq = Mpy_32_32( var1, var1 ); //(2 * (Q_planeCrossingVec - Q1) ) - Q31 + var2_sq = Mpy_32_32( var2, var2 ); + exp_var1_sq = 2 * ( Q31 - ( Q_planeCrossingVec - Q1 ) ); + exp_var2_sq = 2 * ( Q31 - ( Q_planeCrossingVec - Q1 ) ); + IF( L_abs( var1_sq ) > L_shr( ONE_IN_Q31, exp_var1_sq ) ) + { + var1_sq = ONE_IN_Q31; + exp_var1_sq = 0; + } + IF( L_abs( var2_sq ) > L_shr( ONE_IN_Q31, exp_var2_sq ) ) + { + var2_sq = ONE_IN_Q31; + exp_var2_sq = 0; + } + one_minus_var1_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var1_sq ), exp_var1_sq, &final_exp_A ); + var1_sqrt = Sqrt32( one_minus_var1_sq, &final_exp_A ); + var1_cos = BASOP_util_atan2( var1_sqrt, var1, final_exp_A - ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ); // Q13 + angleCmp_fx = var1_cos; + + one_minus_var2_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var2_sq ), exp_var2_sq, &final_exp_B ); + var2_sqrt = Sqrt32( one_minus_var2_sq, &final_exp_B ); + var2_cos = BASOP_util_atan2( var2_sqrt, var2, final_exp_B - ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ); // Q13 + + final_exp = BASOP_Util_Add_MantExp( angleCmp_fx, Q15 - Q13, var2_cos, Q15 - Q13, &angleCmp_fx ); + + sub_exp = 0, sub_exp_2 = 0, sub_final_exp = 0; + var_a = BASOP_Util_Add_Mant32Exp( cmp_arc_fx, Q31 - Q29, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp ); + comp1 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_a ), sub_exp, 21474836, 0 ); + + IF( EQ_16( comp1, -1 ) ) + { + /* A crossing is detected. The new connection is not valid, because + * the connections were ordered in order of preference (arc_weighted) */ + new_connection_is_valid = 0; + } + } + } + c = add( c, 1 ); + } + + /* Store the new connection which has been confirmed valid */ + IF( new_connection_is_valid > 0 ) + { + connections[*connection_write_index][0] = chA; + connections[*connection_write_index][1] = chB; + connection_arc_fx[*connection_write_index] = new_arc_fx; // Q29 + connection_cross_fx[*connection_write_index][0] = new_cross_fx[0]; + connection_cross_fx[*connection_write_index][1] = new_cross_fx[1]; + connection_cross_fx[*connection_write_index][2] = new_cross_fx[2]; + ( *connection_write_index )++; + } + c_opt = add( c_opt, 1 ); + } + + free( connection_options ); + + return IVAS_ERR_OK; +} +#endif static ivas_error formulate_half_sphere_connections( const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */ @@ -2038,8 +3886,8 @@ static ivas_error formulate_half_sphere_connections( float cmp_arc; float normVal; float angleCmp; - float connection_arc[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3]; - float connection_cross[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3][3]; + float connection_arc[(VBAP_MAX_NUM_SPEAKER_NODES - 2) * 3]; + float connection_cross[(VBAP_MAX_NUM_SPEAKER_NODES - 2) * 3][3]; ConnectionOption *connection_options; int16_t num_connection_options; int16_t half_sphere_first_connection; @@ -2049,89 +3897,89 @@ static ivas_error formulate_half_sphere_connections( /* Obtain all connection options (i.e., channel pairs) at the half sphere. The function orders them * in terms of which connection to keep if two connections would cross each other. */ - if ( ( error = get_half_sphere_connection_options( - speaker_node_data, - group, - num_speaker_nodes, - num_non_crossing_planes, - non_crossing_plane_elevation_deg, - &connection_options, - &num_connection_options ) ) != IVAS_ERR_OK ) + if ((error = get_half_sphere_connection_options( + speaker_node_data, + group, + num_speaker_nodes, + num_non_crossing_planes, + non_crossing_plane_elevation_deg, + &connection_options, + &num_connection_options)) != IVAS_ERR_OK) { return error; } - set_f( connection_arc, 0.0f, max_num_connections ); - for ( c = 0; c < max_num_connections; c++ ) + set_f(connection_arc, 0.0f, max_num_connections); + for (c = 0; c < max_num_connections; c++) { - set_f( connection_cross[c], 0.0f, 3 ); + set_f(connection_cross[c], 0.0f, 3); } /* The following loop goes through all reasonable chA - chB pairs for the half-sphere */ c_opt = 0; - while ( c_opt < num_connection_options && *connection_write_index < max_num_connections ) + while (c_opt < num_connection_options && *connection_write_index < max_num_connections) { chA = connection_options[c_opt].chA; chB = connection_options[c_opt].chB; new_arc = connection_options[c_opt].arc; /* Cross-product is needed for later stages */ - vbap_crossp( speaker_node_data[chA].unit_vec, speaker_node_data[chB].unit_vec, new_cross ); + vbap_crossp(speaker_node_data[chA].unit_vec, speaker_node_data[chB].unit_vec, new_cross); /* Determine if new connection between chA and chB is valid */ new_connection_is_valid = 1; c = half_sphere_first_connection; - while ( ( c < *connection_write_index ) && new_connection_is_valid ) + while ((c < *connection_write_index) && new_connection_is_valid) { cmp_chA = connections[c][0]; cmp_chB = connections[c][1]; /* The connections are compared only if they don't involve same speaker nodes */ - if ( ( cmp_chA != chA ) && ( cmp_chA != chB ) && ( cmp_chB != chA ) && ( cmp_chB != chB ) ) + if ((cmp_chA != chA) && (cmp_chA != chB) && (cmp_chB != chA) && (cmp_chB != chB)) { /* The following lines determine if the connection chA-chB crosses with the connection cmp_chA-cmp_chB.*/ /* The connections, i.e., node-pairs determine a plane. The crossing can be determined by * studying the intersection of these planes. */ - vbap_crossp( connection_cross[c], new_cross, planeCrossingVec ); + vbap_crossp(connection_cross[c], new_cross, planeCrossingVec); tmpFloat = 1e-12f; cmp_arc = connection_arc[c]; - for ( k = 0; k < 3; k++ ) + for (k = 0; k < 3; k++) { tmpFloat += planeCrossingVec[k] * planeCrossingVec[k]; } - normVal = sqrtf( 1.0f / tmpFloat ); + normVal = sqrtf(1.0f / tmpFloat); - for ( k = 0; k < 3; k++ ) + for (k = 0; k < 3; k++) { planeCrossingVec[k] *= normVal; } /* If the plane intersection is between both connections, then the two connections cross. */ /* Study first if the crossing is between arc chA-chB */ - angleCmp = acosf( dotp( planeCrossingVec, speaker_node_data[chA].unit_vec, 3 ) ); - angleCmp += acosf( dotp( planeCrossingVec, speaker_node_data[chB].unit_vec, 3 ) ); + angleCmp = acosf(dotp(planeCrossingVec, speaker_node_data[chA].unit_vec, 3)); + angleCmp += acosf(dotp(planeCrossingVec, speaker_node_data[chB].unit_vec, 3)); within_first_arc = 0; - if ( fabsf( new_arc - angleCmp ) < 0.01f ) + if (fabsf(new_arc - angleCmp) < 0.01f) { within_first_arc = 1; } - else if ( fabsf( new_arc - ( 2.0f * EVS_PI - angleCmp ) ) < 0.01f ) + else if (fabsf(new_arc - (2.0f * EVS_PI - angleCmp)) < 0.01f) { within_first_arc = 1; /* In this case, the plane crossing vector is inverted. The inverse is another * plane-crossing vector, and detected to be between chA-chB connection.*/ - for ( k = 0; k < 3; k++ ) + for (k = 0; k < 3; k++) { planeCrossingVec[k] *= -1.0f; } } /* Study if the crossing is also between arc cmp_chA-cmp_chB */ - if ( within_first_arc > 0 ) + if (within_first_arc > 0) { - angleCmp = acosf( dotp( planeCrossingVec, speaker_node_data[cmp_chA].unit_vec, 3 ) ); - angleCmp += acosf( dotp( planeCrossingVec, speaker_node_data[cmp_chB].unit_vec, 3 ) ); - if ( fabsf( cmp_arc - angleCmp ) < 0.01f ) + angleCmp = acosf(dotp(planeCrossingVec, speaker_node_data[cmp_chA].unit_vec, 3)); + angleCmp += acosf(dotp(planeCrossingVec, speaker_node_data[cmp_chB].unit_vec, 3)); + if (fabsf(cmp_arc - angleCmp) < 0.01f) { /* A crossing is detected. The new connection is not valid, because * the connections were ordered in order of preference (arc_weighted) */ @@ -2143,7 +3991,7 @@ static ivas_error formulate_half_sphere_connections( } /* Store the new connection which has been confirmed valid */ - if ( new_connection_is_valid > 0 ) + if (new_connection_is_valid > 0) { connections[*connection_write_index][0] = chA; connections[*connection_write_index][1] = chB; @@ -2151,12 +3999,12 @@ static ivas_error formulate_half_sphere_connections( connection_cross[*connection_write_index][0] = new_cross[0]; connection_cross[*connection_write_index][1] = new_cross[1]; connection_cross[*connection_write_index][2] = new_cross[2]; - ( *connection_write_index )++; + (*connection_write_index)++; } c_opt++; } - free( connection_options ); + free(connection_options); return IVAS_ERR_OK; } @@ -2169,6 +4017,98 @@ static ivas_error formulate_half_sphere_connections( *-------------------------------------------------------------------------*/ /*! r: number of non-crossing planes */ + +#ifdef IVAS_FLOAT_FIXED +static Word16 determine_non_crossing_planes_fx( + const Word16 num_speaker_nodes, /* i : number of speaker nodes */ + const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */ + Word32 *non_crossing_plane_elevation_deg_fx /* o : vector of non-crossing plane elevations */ +) +{ + Word32 next_ele_check_fx; + Word32 ele_check_fx; + Word32 max_gap_fx; + Word32 gap_to_next_ls_fx; + + Word16 ch, ch_cmp; + Word16 num_planes; + + ele_check_fx = -16382362; /*Q14*/ + num_planes = 0; + + /* For each plane, check if a non-crossing plane should be determined */ + WHILE( LT_32( ele_check_fx, 1474560 ) /*90.0f Q14*/ ) + { + next_ele_check_fx = 16382362; /*Q14*/ + + Word32 tmp1, tmp2; + + /* Find next node elevation that is not in horizontal plane */ + FOR( ch = 0; ch < num_speaker_nodes; ch++ ) + { + tmp1 = L_add( ele_check_fx, 16 /*VBAP_EPSILON in Q14*/ ); + tmp2 = L_sub( next_ele_check_fx, 16 /*VBAP_EPSILON in Q14*/ ); + IF( ( node_data[ch].group != SPEAKER_NODE_HORIZONTAL ) && GT_32( L_shr( node_data[ch].ele_deg_fx, 8 ), tmp1 ) && LT_32( L_shr( node_data[ch].ele_deg_fx, 8 ), tmp2 ) ) + { + next_ele_check_fx = L_shr( node_data[ch].ele_deg_fx, 8 ); // shift due to comparision with 90.0f + } + } + ele_check_fx = next_ele_check_fx; // Q14 + + IF( GT_32( ele_check_fx, 1474560 ) /*90.0f in Q14*/ ) + { + /* When no next node elevation found, break loop */ + break; + } + + max_gap_fx = -163838368; // Q14 + FOR( ch = 0; ch < num_speaker_nodes; ch++ ) + { + /* Find gap to the next speaker node at the same plane */ + IF( LT_32( L_abs( L_sub( L_shr( node_data[ch].ele_deg_fx, 8 ), ele_check_fx ) ), 16 /*0.001f in Q14*/ ) ) + { + gap_to_next_ls_fx = 1638398336; // Q14 + FOR( ch_cmp = 0; ch_cmp < num_speaker_nodes; ch_cmp++ ) + { + IF( NE_16( ch_cmp, ch ) && LT_32( L_abs( L_sub( L_shr( node_data[ch_cmp].ele_deg_fx, 8 ), ele_check_fx ) ), 16 /*0.001f in Q14*/ ) ) + { + Word32 gap_fx = L_sub( node_data[ch_cmp].azi_deg_fx, node_data[ch].azi_deg_fx ); + WHILE( LT_32( gap_fx, 0 ) ) + { + gap_fx = L_add( gap_fx, 1509949440 /*360.0f in Q22*/ ); + } + IF( LT_32( L_shr( gap_fx, 8 ), gap_to_next_ls_fx ) ) + { + gap_to_next_ls_fx = L_shr( gap_fx, 8 ); + } + } + } + /* Find maximum gap on that plane */ + IF( GT_32( gap_to_next_ls_fx, max_gap_fx ) ) + { + max_gap_fx = gap_to_next_ls_fx; + } + } + } + + /* If maximum gap is small enough, then a non-crossing plane is detected */ + IF( LT_32( max_gap_fx, 2293776 /*Q14*/ ) && GT_32( max_gap_fx, 0 ) ) + { + non_crossing_plane_elevation_deg_fx[num_planes] = ele_check_fx; // q14 + num_planes = add( num_planes, 1 ); + IF( EQ_16( num_planes, VBAP_MAX_PLANES ) ) + { + /* Memory init limit. Does not happen with any real speaker node configuration. + Triangulation succeeds even if number of non_crossing_planes are limited. */ + break; + } + } + } + + return num_planes; +} +#endif + static int16_t determine_non_crossing_planes( const int16_t num_speaker_nodes, /* i : number of speaker nodes */ const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */ @@ -2186,48 +4126,48 @@ static int16_t determine_non_crossing_planes( num_planes = 0; /* For each plane, check if a non-crossing plane should be determined */ - while ( ele_check < 90.0f ) + while (ele_check < 90.0f) { next_ele_check = 999.9f; /* Find next node elevation that is not in horizontal plane */ - for ( ch = 0; ch < num_speaker_nodes; ch++ ) + for (ch = 0; ch < num_speaker_nodes; ch++) { - if ( ( node_data[ch].group != SPEAKER_NODE_HORIZONTAL ) && ( node_data[ch].ele_deg > ele_check + VBAP_EPSILON ) && ( node_data[ch].ele_deg < next_ele_check - VBAP_EPSILON ) ) + if ((node_data[ch].group != SPEAKER_NODE_HORIZONTAL) && (node_data[ch].ele_deg > ele_check + VBAP_EPSILON) && (node_data[ch].ele_deg < next_ele_check - VBAP_EPSILON)) { next_ele_check = node_data[ch].ele_deg; } } ele_check = next_ele_check; - if ( ele_check > 90.0f ) + if (ele_check > 90.0f) { /* When no next node elevation found, break loop */ break; } max_gap = -9999.9f; - for ( ch = 0; ch < num_speaker_nodes; ch++ ) + for (ch = 0; ch < num_speaker_nodes; ch++) { /* Find gap to the next speaker node at the same plane */ - if ( fabsf( node_data[ch].ele_deg - ele_check ) < VBAP_EPSILON ) + if (fabsf(node_data[ch].ele_deg - ele_check) < VBAP_EPSILON) { gap_to_next_ls = 99999.9f; - for ( ch_cmp = 0; ch_cmp < num_speaker_nodes; ch_cmp++ ) + for (ch_cmp = 0; ch_cmp < num_speaker_nodes; ch_cmp++) { - if ( ch_cmp != ch && fabsf( node_data[ch_cmp].ele_deg - ele_check ) < VBAP_EPSILON ) + if (ch_cmp != ch && fabsf(node_data[ch_cmp].ele_deg - ele_check) < VBAP_EPSILON) { float gap = node_data[ch_cmp].azi_deg - node_data[ch].azi_deg; - while ( gap < 0 ) + while (gap < 0) { gap += 360.0f; } - if ( gap < gap_to_next_ls ) + if (gap < gap_to_next_ls) { gap_to_next_ls = gap; } } } /* Find maximum gap on that plane */ - if ( gap_to_next_ls > max_gap ) + if (gap_to_next_ls > max_gap) { max_gap = gap_to_next_ls; } @@ -2235,11 +4175,11 @@ static int16_t determine_non_crossing_planes( } /* If maximum gap is small enough, then a non-crossing plane is detected */ - if ( max_gap < ( VBAP_MAX_HORIZONTAL_GAP_FOR_PLANE_DETECTION + VBAP_EPSILON ) && max_gap > 0.0f ) + if (max_gap < (VBAP_MAX_HORIZONTAL_GAP_FOR_PLANE_DETECTION + VBAP_EPSILON) && max_gap > 0.0f) { non_crossing_plane_elevation_deg[num_planes] = ele_check; num_planes++; - if ( num_planes == VBAP_MAX_PLANES ) + if (num_planes == VBAP_MAX_PLANES) { /* Memory init limit. Does not happen with any real speaker node configuration. Triangulation succeeds even if number of non_crossing_planes are limited. */ @@ -2281,3 +4221,29 @@ static void reorder_triplets( return; } + +#ifdef IVAS_FLOAT_FIXED +static void reorder_triplets_fx( + VBAP_VS_TRIPLET *triplets, /* i/o: VS triplets to be reordered */ + const Word16 *target_order, /* i : Target order for VS triplets */ + const Word16 num_triplets /* i : Number of VS triplets */ +) +{ + VBAP_VS_TRIPLET tempTriplets[VBAP_MAX_NUM_TRIPLETS]; + Word16 c; + + /* First copy to temp array */ + FOR( c = 0; c < num_triplets; c++ ) + { + tempTriplets[c] = triplets[c]; + } + + /* Then move back in sorted order */ + FOR( c = 0; c < num_triplets; c++ ) + { + triplets[c] = tempTriplets[target_order[c]]; + } + + return; +} +#endif \ No newline at end of file -- GitLab