Line data Source code
1 : /******************************************************************************************************
2 :
3 : (C) 2022-2025 IVAS codec Public Collaboration with portions copyright Dolby International AB, Ericsson AB,
4 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
5 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
6 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
7 : contributors to this repository. All Rights Reserved.
8 :
9 : This software is protected by copyright law and by international treaties.
10 : The IVAS codec Public Collaboration consisting of Dolby International AB, Ericsson AB,
11 : Fraunhofer-Gesellschaft zur Foerderung der angewandten Forschung e.V., Huawei Technologies Co. LTD.,
12 : Koninklijke Philips N.V., Nippon Telegraph and Telephone Corporation, Nokia Technologies Oy, Orange,
13 : Panasonic Holdings Corporation, Qualcomm Technologies, Inc., VoiceAge Corporation, and other
14 : contributors to this repository retain full ownership rights in their respective contributions in
15 : the software. This notice grants no license of any kind, including but not limited to patent
16 : license, nor is any license granted by implication, estoppel or otherwise.
17 :
18 : Contributors are required to enter into the IVAS codec Public Collaboration agreement before making
19 : contributions.
20 :
21 : This software is provided "AS IS", without any express or implied warranties. The software is in the
22 : development stage. It is intended exclusively for experts who have experience with such software and
23 : solely for the purpose of inspection. All implied warranties of non-infringement, merchantability
24 : and fitness for a particular purpose are hereby disclaimed and excluded.
25 :
26 : Any dispute, controversy or claim arising under or in relation to providing this software shall be
27 : submitted to and settled by the final, binding jurisdiction of the courts of Munich, Germany in
28 : accordance with the laws of the Federal Republic of Germany excluding its conflict of law rules and
29 : the United Nations Convention on Contracts on the International Sales of Goods.
30 :
31 : *******************************************************************************************************/
32 :
33 : #include <stdint.h>
34 : #include "options.h"
35 : #include <assert.h>
36 : #include <math.h>
37 : #include "prot_fx.h"
38 : #include "ivas_prot_rend_fx.h"
39 : #include "ivas_stat_dec.h"
40 : #include "wmc_auto.h"
41 : #include "ivas_prot_fx.h"
42 :
43 : /*-----------------------------------------------------------------------*
44 : * Local constants
45 : *-----------------------------------------------------------------------*/
46 :
47 : /* 128 is maximum num_speaker_nodes number. This relates to memory optimization and maximum of triplets:
48 : - triplet indices are unsigned_char (see below structs) --> max triplets is 256
49 : - num_speaker_nodes_internal = num_speaker_nodes + 2 (potential virtual node channels, bottom and top)
50 : - max_num_triplets = 256 = (max_num_ls_internal - 2) * 2 = (max_num_ls) * 2
51 : --> max_num_ls = 128
52 : */
53 : #define VBAP_MAX_NUM_SPEAKER_NODES 128
54 : #define VBAP_MAX_NUM_TRIPLETS 256
55 : #define VBAP_EPSILON 0.001f /* The fairly large epsilon is for detecting planes etc and accounts for rounding issues */
56 : #define VBAP_EPSILON_Q3O 1073741
57 : #define VBAP_EPSILON_Q31 2147483
58 : #define VBAP_MAX_PLANES 50
59 : #define VBAP_MAX_HORIZONTAL_GAP_FOR_PLANE_DETECTION 140.0f
60 : /* If a speaker node is found
61 : - above VBAP_NO_VIRTUAL_SPEAKER_NODE_ELE_LIMIT, no virtual node is used
62 : - above VBAP_DISTRIBUTE_VIRTUAL_SPEAKER_NODE_ELE_LIMIT, energy-spreading virtual node is used
63 : - not above VBAP_DISTRIBUTE_VIRTUAL_SPEAKER_NODE_ELE_LIMIT, energy-omitting virtual node is used
64 : Same applies for both elevations and inclinations, i.e., the two half-spheres. */
65 : #define VBAP_NO_VIRTUAL_SPEAKER_NODE_ELE_LIMIT 45.0f
66 : #define VBAP_DISTRIBUTE_VIRTUAL_SPEAKER_NODE_ELE_LIMIT 20.0f
67 : #define VBAP_VIRTUAL_BACK_ELE_LIMIT 45.0f
68 : #define VBAP_NOT_VALID_CONNECTION ( -1 )
69 :
70 : /* Maximum azimuth gap between speaker nodes for detecting zero elevation horizontal plane */
71 : #define VBAP_MAX_HORIZONTAL_GAP 170u
72 :
73 : #define VBAP_SEARCH_SECTOR_SIZE ( 360.0f / ( VBAP_NUM_SEARCH_SECTORS ) )
74 : #define VBAP_SEARCH_SECTOR_SIZE_Q0 90
75 :
76 :
77 : enum VirtualSpeakerNodeType
78 : {
79 : NO_VIRTUAL_SPEAKER_NODE,
80 : VIRTUAL_SPEAKER_NODE_DISCARD_ENERGY,
81 : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY
82 : };
83 :
84 : enum ConnectionClass
85 : {
86 : REGULAR_CONNECTION,
87 : ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION,
88 : CONNECTION_WITH_SPEAKER_NODE_BEHIND
89 : };
90 :
91 : typedef struct connection_option
92 : {
93 : Word16 chA;
94 : Word16 chB;
95 : Word32 arc_weighted_fx; /* Q29 */
96 : Word32 arc_fx; /* Q29 */
97 : } ConnectionOption;
98 :
99 : enum SpeakerNodeGroup
100 : {
101 : SPEAKER_NODE_BOTTOM_HALF,
102 : SPEAKER_NODE_HORIZONTAL,
103 : SPEAKER_NODE_TOP_HALF,
104 : SPEAKER_NODE_BACK,
105 : SPEAKER_NODE_ALL
106 : };
107 :
108 : /* Defines a single speaker node */
109 : typedef struct vbap_speaker_node_structure
110 : {
111 : Word32 azi_deg_fx; /* Q22 */
112 : Word32 ele_deg_fx; /* Q22 */
113 : Word32 unit_vec_fx[3]; /* Q30 */
114 : enum SpeakerNodeGroup group;
115 :
116 : } VBAP_SPEAKER_NODE;
117 :
118 :
119 : /*-----------------------------------------------------------------------*
120 : * Local function prototypes
121 : *-----------------------------------------------------------------------*/
122 :
123 : static UWord8 vector_matrix_multiply_3x3_fx( const Word16 *src_vector, Word32 matrix[3][3], Word32 *result, Word16 q_matrix );
124 :
125 : static UWord8 vector_matrix_multiply_3x3_32_fx( const Word32 *src_vector, Word32 matrix[3][3], Word32 *result, Word16 q_matrix );
126 :
127 : 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 Word16 num_speaker_nodes );
128 :
129 : static Word16 determine_virtual_surface_triplets_fx( const Word16 num_speaker_nodes, const VBAP_SPEAKER_NODE *speaker_node_data, Word16 connections[][2], const Word16 max_num_connections, VBAP_VS_TRIPLET *triplets, Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS], enum SpeakerNodeGroup allowed_group );
130 :
131 : static void determine_initial_search_indices_fx( const Word16 num_triplets, const Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS], Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS] );
132 :
133 : 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, Word16 *group2_count );
134 :
135 : 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 );
136 :
137 : 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 );
138 :
139 : 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 );
140 :
141 : 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 );
142 :
143 : 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 );
144 :
145 : 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] );
146 :
147 : static void determine_virtual_speaker_node_division_gains_fx( const Word16 virtual_speaker_node_index, Word16 *virtual_node_division_gains_fx, Word16 connections[][2], const enum VirtualSpeakerNodeType type, const Word16 max_num_connections, const Word16 num_speaker_nodes, const Word16 use_object_mode );
148 :
149 : static void reorder_triplets_fx( VBAP_VS_TRIPLET *triplets, const Word16 *target_order, const Word16 num_triplets );
150 :
151 : /*-------------------------------------------------------------------------*
152 : * vbap_init_data()
153 : *
154 : * Initialize VBAP data structure for the speaker node set
155 : *-------------------------------------------------------------------------*/
156 :
157 835 : ivas_error vbap_init_data_fx(
158 : VBAP_HANDLE *hVBAPdata, /* i/o: handle for VBAP data structure that will be initialized */
159 : const Word32 *speaker_node_azi_deg_fx, /* i : vector of speaker node azimuths (positive left) Q22 */
160 : const Word32 *speaker_node_ele_deg_fx, /* i : vector of speaker node elevations (positive up) Q22 */
161 : const Word16 num_speaker_nodes, /* i : number of speaker nodes in the set */
162 : const IVAS_FORMAT ivas_format /* i : IVAS format */
163 : )
164 : {
165 : /* Variables */
166 : Word16 connections[VBAP_MAX_NUM_SPEAKER_NODES][2];
167 : Word16 max_num_connections;
168 : Word16 is_success;
169 : Word16 connection_group1_count;
170 : Word16 connection_group2_start;
171 : Word16 connection_group2_count;
172 : enum VirtualSpeakerNodeType virtual_top_type;
173 : enum VirtualSpeakerNodeType virtual_bottom_type;
174 : enum VirtualSpeakerNodeType virtual_back_type;
175 :
176 : Word32 speaker_node_azi_deg_internal_fx[VBAP_MAX_NUM_SPEAKER_NODES]; /* Q22 */
177 : Word32 speaker_node_ele_deg_internal_fx[VBAP_MAX_NUM_SPEAKER_NODES]; /* Q22 */
178 :
179 : VBAP_SPEAKER_NODE speaker_node_data[VBAP_MAX_NUM_SPEAKER_NODES];
180 : VBAP_DATA *vbap;
181 : ivas_error error;
182 :
183 835 : push_wmops( "vbap_init" );
184 :
185 835 : set32_fx( speaker_node_azi_deg_internal_fx, 0, VBAP_MAX_NUM_SPEAKER_NODES );
186 835 : set32_fx( speaker_node_ele_deg_internal_fx, 0, VBAP_MAX_NUM_SPEAKER_NODES );
187 :
188 : /* Basic init checks */
189 : /* If the requested layout is invalid, hVBAPdata is set to NULL and the signal will
190 : * be distributed with an equal gain into all output channels.
191 : * The surrounding code needs to handle the NULL pointer properly. */
192 835 : test();
193 835 : IF( GT_16( num_speaker_nodes, VBAP_MAX_NUM_SPEAKER_NODES ) || LT_16( num_speaker_nodes, 3 ) )
194 : {
195 0 : hVBAPdata = NULL;
196 0 : pop_wmops();
197 0 : return IVAS_ERR_OK;
198 : }
199 :
200 835 : test();
201 835 : IF( !speaker_node_azi_deg_fx || !speaker_node_ele_deg_fx )
202 : {
203 0 : hVBAPdata = NULL;
204 0 : return IVAS_ERR_OK;
205 : }
206 :
207 : /* Allocate VBAP structure */
208 835 : IF( ( vbap = (VBAP_HANDLE) malloc( sizeof( VBAP_DATA ) ) ) == NULL )
209 : {
210 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
211 : }
212 :
213 835 : is_success = 1;
214 835 : move16();
215 835 : vbap->bottom_virtual_speaker_node_index = -1;
216 835 : move16();
217 835 : vbap->top_virtual_speaker_node_index = -1;
218 835 : move16();
219 835 : vbap->back_virtual_speaker_node_index = -1;
220 835 : move16();
221 835 : vbap->num_speaker_nodes = num_speaker_nodes;
222 835 : move16();
223 835 : vbap->num_speaker_nodes_internal = num_speaker_nodes;
224 835 : move16();
225 835 : vbap->bottom_virtual_speaker_node_division_gains_fx = NULL;
226 835 : vbap->top_virtual_speaker_node_division_gains_fx = NULL;
227 835 : vbap->back_virtual_speaker_node_division_gains_fx = NULL;
228 835 : vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx = NULL;
229 835 : vbap->object_mode_top_virtual_speaker_node_division_gains_fx = NULL;
230 835 : vbap->object_mode_back_virtual_speaker_node_division_gains_fx = NULL;
231 :
232 : /* Check if the speaker node setup needs a virtual top or bottom node
233 : (function also increments vbap->num_speaker_nodes_internal when necessary) */
234 835 : 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 );
235 835 : 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 );
236 835 : virtual_back_type = check_need_of_virtual_speaker_node_fx( vbap, speaker_node_azi_deg_fx, speaker_node_ele_deg_fx, SPEAKER_NODE_BACK );
237 :
238 : /* Init internal speaker node configuration, which is the original configuration
239 : potentially appended with virtual top and/or bottom loudspeakers */
240 835 : Copy32( speaker_node_azi_deg_fx, speaker_node_azi_deg_internal_fx, num_speaker_nodes );
241 835 : Copy32( speaker_node_ele_deg_fx, speaker_node_ele_deg_internal_fx, num_speaker_nodes );
242 835 : test();
243 835 : IF( is_success && NE_16( virtual_bottom_type, NO_VIRTUAL_SPEAKER_NODE ) )
244 : {
245 835 : IF( ( vbap->bottom_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL )
246 : {
247 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
248 : }
249 835 : set16_fx( vbap->bottom_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes );
250 835 : is_success &= vbap->bottom_virtual_speaker_node_division_gains_fx != NULL;
251 :
252 835 : IF( EQ_16( ivas_format, MASA_ISM_FORMAT ) )
253 : {
254 339 : IF( ( vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL )
255 : {
256 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
257 : }
258 339 : set16_fx( vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes );
259 339 : is_success &= vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx != NULL;
260 : }
261 835 : speaker_node_azi_deg_internal_fx[vbap->bottom_virtual_speaker_node_index] = 0;
262 835 : speaker_node_ele_deg_internal_fx[vbap->bottom_virtual_speaker_node_index] = -377487360; /* -90.0f in Q22 */
263 835 : move32();
264 835 : move32();
265 : }
266 835 : test();
267 835 : IF( is_success && NE_16( virtual_top_type, NO_VIRTUAL_SPEAKER_NODE ) )
268 : {
269 835 : IF( ( vbap->top_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL )
270 : {
271 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
272 : }
273 835 : set16_fx( vbap->top_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes );
274 835 : is_success &= vbap->top_virtual_speaker_node_division_gains_fx != NULL;
275 :
276 835 : IF( EQ_16( ivas_format, MASA_ISM_FORMAT ) )
277 : {
278 339 : IF( ( vbap->object_mode_top_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL )
279 : {
280 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
281 : }
282 339 : set16_fx( vbap->object_mode_top_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes );
283 339 : is_success &= vbap->object_mode_top_virtual_speaker_node_division_gains_fx != NULL;
284 : }
285 835 : speaker_node_azi_deg_internal_fx[vbap->top_virtual_speaker_node_index] = 0;
286 835 : speaker_node_ele_deg_internal_fx[vbap->top_virtual_speaker_node_index] = 377487360; /* 90.0f in Q22 */
287 835 : move32();
288 835 : move16();
289 : }
290 835 : test();
291 835 : IF( is_success && NE_16( virtual_back_type, NO_VIRTUAL_SPEAKER_NODE ) )
292 : {
293 0 : IF( ( vbap->back_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL )
294 : {
295 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
296 : }
297 0 : set16_fx( vbap->back_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes );
298 0 : is_success &= vbap->back_virtual_speaker_node_division_gains_fx != NULL;
299 :
300 0 : IF( EQ_16( ivas_format, MASA_ISM_FORMAT ) )
301 : {
302 0 : IF( ( vbap->object_mode_back_virtual_speaker_node_division_gains_fx = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) ) ) == NULL )
303 : {
304 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
305 : }
306 0 : set16_fx( vbap->object_mode_back_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes );
307 0 : is_success &= vbap->object_mode_back_virtual_speaker_node_division_gains_fx != NULL;
308 : }
309 0 : speaker_node_azi_deg_internal_fx[vbap->back_virtual_speaker_node_index] = 754974720; /* 180.0f in Q22 */
310 0 : speaker_node_ele_deg_internal_fx[vbap->back_virtual_speaker_node_index] = 0;
311 :
312 0 : move32();
313 0 : move16();
314 : }
315 :
316 835 : 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 );
317 :
318 : /* Allocate and determine node-node connections */
319 835 : max_num_connections = mult0( ( sub( vbap->num_speaker_nodes_internal, 2 ) ), 3 ); /* Theoretical maximum */
320 835 : IF( NE_32( ( 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 ) )
321 : {
322 0 : return error;
323 : }
324 : /* Allocate and determine virtual surface speaker node triplets */
325 835 : IF( is_success )
326 : {
327 : Word16 ch;
328 835 : Word16 speaker_nodes_group1_internal = 0;
329 835 : move16();
330 835 : Word16 speaker_nodes_group2_internal = 0;
331 835 : move16();
332 835 : Word16 speaker_nodes_horiz_internal = 0;
333 835 : move16();
334 835 : UWord8 loop_done = 0;
335 835 : move16();
336 :
337 : /* Count nodes in different groups to reserve correct memory */
338 9220 : FOR( ch = 0; ch < vbap->num_speaker_nodes_internal && !loop_done; ch++ )
339 : {
340 8385 : test();
341 8385 : SWITCH( speaker_node_data[ch].group )
342 : {
343 0 : case SPEAKER_NODE_ALL:
344 : /* If there is even one speaker belonging to "all" group, then all speakers belong to the "all" group.
345 : * We can skip further counts here. */
346 0 : speaker_nodes_group1_internal = vbap->num_speaker_nodes_internal;
347 0 : move16();
348 0 : loop_done = 1;
349 0 : move16();
350 0 : BREAK;
351 839 : case SPEAKER_NODE_BOTTOM_HALF:
352 839 : speaker_nodes_group1_internal = add( speaker_nodes_group1_internal, 1 );
353 839 : BREAK;
354 2725 : case SPEAKER_NODE_TOP_HALF:
355 2725 : speaker_nodes_group2_internal = add( speaker_nodes_group2_internal, 1 );
356 2725 : BREAK;
357 4821 : case SPEAKER_NODE_HORIZONTAL:
358 : case SPEAKER_NODE_BACK:
359 4821 : speaker_nodes_group1_internal = add( speaker_nodes_group1_internal, 1 );
360 4821 : speaker_nodes_group2_internal = add( speaker_nodes_group2_internal, 1 );
361 4821 : speaker_nodes_horiz_internal = add( speaker_nodes_horiz_internal, 1 );
362 4821 : BREAK;
363 : }
364 8385 : }
365 :
366 835 : IF( ( vbap->search_struct[0].triplets = (VBAP_VS_TRIPLET *) malloc( ( ( speaker_nodes_group1_internal - 2 ) * 2 - ( s_max( 0, ( speaker_nodes_horiz_internal - 2 ) ) ) ) * sizeof( VBAP_VS_TRIPLET ) ) ) == NULL )
367 : {
368 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
369 : }
370 835 : is_success = s_and( is_success, vbap->search_struct[0].triplets != NULL );
371 :
372 835 : IF( speaker_nodes_group2_internal > 0 )
373 : {
374 835 : vbap->num_search_structs = 2;
375 835 : move16();
376 835 : IF( ( vbap->search_struct[1].triplets = (VBAP_VS_TRIPLET *) malloc( ( ( speaker_nodes_group2_internal - 2 ) * 2 - ( s_max( 0, ( speaker_nodes_horiz_internal - 2 ) ) ) ) * sizeof( VBAP_VS_TRIPLET ) ) ) == NULL )
377 : {
378 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
379 : }
380 835 : is_success = s_and( is_success, vbap->search_struct[1].triplets != NULL );
381 : }
382 : ELSE
383 : {
384 0 : vbap->num_search_structs = 1;
385 0 : move16();
386 0 : vbap->search_struct[1].triplets = NULL;
387 : }
388 : }
389 :
390 835 : IF( is_success )
391 : {
392 835 : IF( EQ_16( vbap->num_search_structs, 1 ) )
393 : {
394 : /* If all speaker nodes belong to ALL set, then we only create one triplet set and search structure */
395 0 : 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 );
396 0 : move16();
397 : }
398 : ELSE
399 : {
400 : /* Otherwise, we have two sets and can handle them separately for more opmitized processing. */
401 835 : 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 );
402 835 : move16();
403 835 : 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 );
404 835 : move16();
405 : }
406 : }
407 :
408 : /* Determine how the virtual node gains should be distributed to real nodes, if necessary (checked within function). */
409 835 : IF( is_success )
410 : {
411 835 : determine_virtual_speaker_node_division_gains_fx( vbap->top_virtual_speaker_node_index, vbap->top_virtual_speaker_node_division_gains_fx, connections, virtual_top_type, max_num_connections, num_speaker_nodes, 0 );
412 835 : determine_virtual_speaker_node_division_gains_fx( vbap->bottom_virtual_speaker_node_index, vbap->bottom_virtual_speaker_node_division_gains_fx, connections, virtual_bottom_type, max_num_connections, num_speaker_nodes, 0 );
413 835 : determine_virtual_speaker_node_division_gains_fx( vbap->back_virtual_speaker_node_index, vbap->back_virtual_speaker_node_division_gains_fx, connections, virtual_back_type, max_num_connections, num_speaker_nodes, 0 );
414 835 : IF( EQ_32( ivas_format, MASA_ISM_FORMAT ) )
415 : {
416 339 : determine_virtual_speaker_node_division_gains_fx( vbap->top_virtual_speaker_node_index, vbap->object_mode_top_virtual_speaker_node_division_gains_fx, connections, virtual_top_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1 );
417 339 : determine_virtual_speaker_node_division_gains_fx( vbap->bottom_virtual_speaker_node_index, vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx, connections, virtual_bottom_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1 );
418 339 : determine_virtual_speaker_node_division_gains_fx( vbap->back_virtual_speaker_node_index, vbap->object_mode_back_virtual_speaker_node_division_gains_fx, connections, virtual_back_type == NO_VIRTUAL_SPEAKER_NODE ? NO_VIRTUAL_SPEAKER_NODE : VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, max_num_connections, num_speaker_nodes, 1 );
419 : }
420 : }
421 :
422 835 : pop_wmops();
423 :
424 835 : IF( is_success )
425 : {
426 835 : *hVBAPdata = vbap;
427 : }
428 : ELSE
429 : {
430 0 : vbap_free_data_fx( &vbap );
431 : }
432 :
433 835 : return IVAS_ERR_OK;
434 : }
435 :
436 :
437 : /*-------------------------------------------------------------------------*
438 : * vbap_free_data()
439 : *
440 : * Free VBAP data structure
441 : *-------------------------------------------------------------------------*/
442 :
443 1762 : void vbap_free_data_fx(
444 : VBAP_HANDLE *hVBAPdata /* i/o: VBAP handle to be freed */
445 : )
446 : {
447 1762 : test();
448 1762 : IF( hVBAPdata == NULL || *hVBAPdata == NULL )
449 : {
450 927 : return;
451 : }
452 :
453 835 : IF( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains_fx != NULL )
454 : {
455 835 : free( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains_fx );
456 : }
457 835 : IF( ( *hVBAPdata )->top_virtual_speaker_node_division_gains_fx != NULL )
458 : {
459 835 : free( ( *hVBAPdata )->top_virtual_speaker_node_division_gains_fx );
460 : }
461 835 : IF( ( *hVBAPdata )->back_virtual_speaker_node_division_gains_fx != NULL )
462 : {
463 0 : free( ( *hVBAPdata )->back_virtual_speaker_node_division_gains_fx );
464 : }
465 835 : IF( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains_fx != NULL )
466 : {
467 339 : free( ( *hVBAPdata )->object_mode_bottom_virtual_speaker_node_division_gains_fx );
468 : }
469 835 : IF( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains_fx != NULL )
470 : {
471 339 : free( ( *hVBAPdata )->object_mode_top_virtual_speaker_node_division_gains_fx );
472 : }
473 835 : IF( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains_fx != NULL )
474 : {
475 0 : free( ( *hVBAPdata )->object_mode_back_virtual_speaker_node_division_gains_fx );
476 : }
477 835 : IF( ( *hVBAPdata )->search_struct[0].triplets != NULL )
478 : {
479 835 : free( ( *hVBAPdata )->search_struct[0].triplets );
480 : }
481 :
482 835 : test();
483 835 : IF( EQ_16( ( *hVBAPdata )->num_search_structs, 2 ) && ( *hVBAPdata )->search_struct[1].triplets != NULL )
484 : {
485 835 : free( ( *hVBAPdata )->search_struct[1].triplets );
486 : }
487 835 : free( *hVBAPdata );
488 835 : *hVBAPdata = NULL;
489 :
490 835 : return;
491 : }
492 :
493 : /*-------------------------------------------------------------------------*
494 : * vbap_determine_gains()
495 : *
496 : * Obtain panning gains for all speaker nodes based on the given direction
497 : *-------------------------------------------------------------------------*/
498 6071615 : void vbap_determine_gains_fx(
499 : const VBAP_HANDLE hVBAPdata, /* i : prepared VBAP structure */
500 : Word32 *gains_fx, /* o : gain vector for loudspeakers for given direction Q29 */
501 : const Word16 azi_deg, /* i : azimuth in degrees for panning direction (positive left) Q0 */
502 : const Word16 ele_deg, /* i : elevation in degrees for panning direction (positive up) Q0 */
503 : const Word16 use_object_mode /* i : select between object mode panning and spatial mode panning */
504 : )
505 : {
506 : /* This function formulates gains for the given angle. The triplet-selection has been pre-formulated. */
507 : Word16 ch, ch2;
508 : Word16 triplet_ch;
509 : Word16 triplet_index;
510 : Word16 panning_unit_vec_fx[3]; /* Q15 */
511 : Word32 gain_triplet_fx[3]; /* Q16 */
512 : Word32 norm_value_fx;
513 : Word32 gain_ene_fx;
514 : Word16 azi_norm; /* Q15 */
515 : Word16 ele_norm; /* Q15 */
516 : Word32 azi_temp_fx; /* Q22 */
517 : Word32 ele_temp_fx; /* Q22 */
518 : Word16 num_speaker_nodes;
519 : Word16 bottom_virtual_speaker_node_index;
520 : Word16 top_virtual_speaker_node_index;
521 : Word16 back_virtual_speaker_node_index;
522 : VBAP_VS_TRIPLET *selected_triplet;
523 : Word16 *bottom_virtual_speaker_node_division_gains_fx; /* Q16 */
524 : Word16 *top_virtual_speaker_node_division_gains_fx; /* Q16 */
525 : Word16 *back_virtual_speaker_node_division_gains_fx; /* Q16 */
526 :
527 :
528 6071615 : push_wmops( "vbap_gains" );
529 6071615 : num_speaker_nodes = hVBAPdata->num_speaker_nodes;
530 6071615 : move16();
531 6071615 : bottom_virtual_speaker_node_index = hVBAPdata->bottom_virtual_speaker_node_index;
532 6071615 : move16();
533 6071615 : top_virtual_speaker_node_index = hVBAPdata->top_virtual_speaker_node_index;
534 6071615 : move16();
535 6071615 : back_virtual_speaker_node_index = hVBAPdata->back_virtual_speaker_node_index;
536 6071615 : move16();
537 :
538 6071615 : IF( use_object_mode )
539 : {
540 32602 : bottom_virtual_speaker_node_division_gains_fx = hVBAPdata->object_mode_bottom_virtual_speaker_node_division_gains_fx;
541 32602 : top_virtual_speaker_node_division_gains_fx = hVBAPdata->object_mode_top_virtual_speaker_node_division_gains_fx;
542 32602 : back_virtual_speaker_node_division_gains_fx = hVBAPdata->object_mode_back_virtual_speaker_node_division_gains_fx;
543 : }
544 : ELSE
545 : {
546 6039013 : bottom_virtual_speaker_node_division_gains_fx = hVBAPdata->bottom_virtual_speaker_node_division_gains_fx;
547 6039013 : top_virtual_speaker_node_division_gains_fx = hVBAPdata->top_virtual_speaker_node_division_gains_fx;
548 6039013 : back_virtual_speaker_node_division_gains_fx = hVBAPdata->back_virtual_speaker_node_division_gains_fx;
549 : }
550 :
551 6071615 : panning_wrap_angles_fx( L_shl( azi_deg, Q22 ), L_shl( ele_deg, Q22 ), &azi_temp_fx, &ele_temp_fx );
552 6071615 : azi_norm = extract_l( L_shr( Mpy_32_32( azi_temp_fx, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
553 6071615 : ele_norm = extract_l( L_shr( Mpy_32_32( ele_temp_fx, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
554 :
555 6071615 : panning_unit_vec_fx[0] = mult( getCosWord16R2( azi_norm ), getCosWord16R2( ele_norm ) ); /* Q15 */
556 6071615 : panning_unit_vec_fx[1] = mult( getSineWord16R2( azi_norm ), getCosWord16R2( ele_norm ) ); /* Q15 */
557 6071615 : panning_unit_vec_fx[2] = getSineWord16R2( ele_norm ); /* Q15 */
558 6071615 : move16();
559 6071615 : move16();
560 6071615 : move16();
561 :
562 :
563 : /* Find the best VS triplet and speaker node gains for the panning direction using the prepared search structures. */
564 6071615 : test();
565 6071615 : IF( EQ_16( hVBAPdata->num_search_structs, 2 ) && ele_deg > 0 )
566 : {
567 2606737 : triplet_index = determine_best_triplet_and_gains_fx( &( hVBAPdata->search_struct[1] ), panning_unit_vec_fx, azi_deg, gain_triplet_fx );
568 2606737 : selected_triplet = &hVBAPdata->search_struct[1].triplets[triplet_index];
569 : }
570 : ELSE
571 : {
572 3464878 : triplet_index = determine_best_triplet_and_gains_fx( &( hVBAPdata->search_struct[0] ), panning_unit_vec_fx, azi_deg, gain_triplet_fx );
573 3464878 : selected_triplet = &hVBAPdata->search_struct[0].triplets[triplet_index];
574 : }
575 :
576 : /* Normalize to unit energy */
577 6071615 : gain_ene_fx = 1; /* Add small value to avoid divide by zero. */
578 6071615 : move32();
579 24286460 : FOR( ch = 0; ch < 3; ch++ )
580 : {
581 18214845 : gain_ene_fx = L_add( gain_ene_fx, Mpy_32_32( gain_triplet_fx[ch], gain_triplet_fx[ch] ) ); /* Q(2 * VBAP_VS_TRIPLET.q_inverse_matrix - 31) */
582 : }
583 :
584 6071615 : norm_value_fx = Isqrt( L_shr( gain_ene_fx, 1 ) ); /* Q(31 - (2 * VBAP_VS_TRIPLET.q_inverse_matrix - 31 - 1) / 2 ) = Q(47 - VBAP_VS_TRIPLET.q_inverse_matrix) */
585 :
586 24286460 : FOR( ch = 0; ch < 3; ch++ )
587 : {
588 18214845 : gain_triplet_fx[ch] = Mpy_32_32( gain_triplet_fx[ch], norm_value_fx ); /* 47 - VBAP_VS_TRIPLET.q_inverse_matrix + VBAP_VS_TRIPLET.q_inverse_matrix - 31 = Q16 */
589 18214845 : move32();
590 :
591 : /* Sanity check for rounding issues */
592 18214845 : if ( gain_triplet_fx[ch] < 0 )
593 : {
594 284 : gain_triplet_fx[ch] = 0;
595 284 : move32();
596 : }
597 : }
598 :
599 : /* Flush gain target */
600 6071615 : set32_fx( gains_fx, 0, num_speaker_nodes );
601 :
602 : /* Map gain triplet (internal speaker node configuration) to speaker node output (actual speaker node configuration) */
603 24286460 : FOR( ch = 0; ch < 3; ch++ )
604 : {
605 18214845 : triplet_ch = selected_triplet->speaker_node[ch];
606 :
607 18214845 : IF( EQ_16( triplet_ch, bottom_virtual_speaker_node_index ) )
608 : {
609 28127963 : FOR( ch2 = 0; ch2 < num_speaker_nodes; ch2++ )
610 : {
611 25077766 : 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] ), Q12 ) ); /* Q16 + Q16 - Q15 + Q12 = Q29 */
612 25077766 : move32();
613 : }
614 : }
615 15164648 : ELSE IF( EQ_16( triplet_ch, top_virtual_speaker_node_index ) )
616 : {
617 12447722 : FOR( ch2 = 0; ch2 < num_speaker_nodes; ch2++ )
618 : {
619 10948872 : 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] ), Q12 ) ); /* Q16 + Q16 - Q15 + Q12 = Q29 */
620 10948872 : move32();
621 : }
622 : }
623 13665798 : ELSE IF( EQ_16( triplet_ch, back_virtual_speaker_node_index ) )
624 : {
625 0 : FOR( ch2 = 0; ch2 < num_speaker_nodes; ch2++ )
626 : {
627 0 : 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] ), Q12 ) ); /* Q16 + Q16 - Q15 + Q12 = Q29 */
628 0 : move32();
629 : }
630 : }
631 : ELSE
632 : {
633 13665798 : gains_fx[triplet_ch] = L_add( gains_fx[triplet_ch], L_shl( gain_triplet_fx[ch], Q13 ) ); /* Q16 + Q13 = Q29 */
634 13665798 : move32();
635 : }
636 : }
637 :
638 6071615 : pop_wmops();
639 :
640 6071615 : return;
641 : }
642 :
643 : /*-----------------------------------------------------------------------*
644 : * Local functions
645 : *-----------------------------------------------------------------------*/
646 :
647 : /*-------------------------------------------------------------------------*
648 : * vbap_crossp()
649 : *
650 : * 3-by-3 vector cross product
651 : *-------------------------------------------------------------------------*/
652 :
653 109838 : static void vbap_crossp_fx(
654 : const Word32 *vec1_fx, /* i : input vector 1 Qx */
655 : const Word32 *vec2_fx, /* i : input vector 2 Qy */
656 : Word32 *crossProduct_fx /* o : cross product output Qx + Qy - 31 */
657 : )
658 : {
659 :
660 109838 : crossProduct_fx[0] = L_sub( Mpy_32_32( vec1_fx[1], vec2_fx[2] ), Mpy_32_32( vec1_fx[2], vec2_fx[1] ) );
661 109838 : move32();
662 109838 : crossProduct_fx[1] = L_sub( Mpy_32_32( vec1_fx[2], vec2_fx[0] ), Mpy_32_32( vec1_fx[0], vec2_fx[2] ) );
663 109838 : move32();
664 109838 : crossProduct_fx[2] = L_sub( Mpy_32_32( vec1_fx[0], vec2_fx[1] ), Mpy_32_32( vec1_fx[1], vec2_fx[0] ) );
665 109838 : move32();
666 :
667 109838 : return;
668 : }
669 :
670 : /*-------------------------------------------------------------------------*
671 : * vector_matrix_multiply_3x3()
672 : *
673 : * 3-by-3 vector multiply with matrix
674 : *-------------------------------------------------------------------------*/
675 :
676 :
677 : /*! r: Status result if triplet is usable for panning. Allows early exit. */
678 22025116 : static UWord8 vector_matrix_multiply_3x3_fx(
679 : const Word16 *src_vector, /* i : input vector Q15 */
680 : Word32 matrix[3][3], /* i : input matrix Q(q_matrix) */
681 : Word32 *result, /* o : output vector Q(q_matrix) */
682 : Word16 q_matrix )
683 : {
684 22025116 : result[0] = Mpy_32_16_1( matrix[0][0], src_vector[0] ); /* Q(q_matrix) */
685 22025116 : result[0] = L_add( result[0], Mpy_32_16_1( matrix[1][0], src_vector[1] ) ); /* Q(q_matrix) */
686 22025116 : result[0] = L_add( result[0], Mpy_32_16_1( matrix[2][0], src_vector[2] ) ); /* Q(q_matrix) */
687 22025116 : move32();
688 22025116 : move32();
689 22025116 : move32();
690 :
691 22025116 : IF( LT_32( result[0], Mpy_32_16_1( L_lshl( 1, q_matrix ), -327 /* -0.01 in Q15 */ ) ) )
692 : {
693 2590369 : return 0;
694 : }
695 :
696 19434747 : result[1] = Mpy_32_16_1( matrix[0][1], src_vector[0] ); /* Q(q_matrix) */
697 19434747 : result[1] = L_add( result[1], Mpy_32_16_1( matrix[1][1], src_vector[1] ) ); /* Q(q_matrix) */
698 19434747 : result[1] = L_add( result[1], Mpy_32_16_1( matrix[2][1], src_vector[2] ) ); /* Q(q_matrix) */
699 19434747 : move32();
700 19434747 : move32();
701 19434747 : move32();
702 :
703 19434747 : IF( LT_32( result[1], Mpy_32_16_1( L_shl( 1, q_matrix ), -327 /* -0.01 in Q15 */ ) ) )
704 : {
705 8178134 : return 0;
706 : }
707 :
708 11256613 : result[2] = Mpy_32_16_1( matrix[0][2], src_vector[0] ); /* Q(q_matrix) */
709 11256613 : result[2] = L_add( result[2], Mpy_32_16_1( matrix[1][2], src_vector[1] ) ); /* Q(q_matrix) */
710 11256613 : result[2] = L_add( result[2], Mpy_32_16_1( matrix[2][2], src_vector[2] ) ); /* Q(q_matrix) */
711 11256613 : move32();
712 11256613 : move32();
713 11256613 : move32();
714 :
715 11256613 : IF( LT_32( result[2], Mpy_32_16_1( L_shl( 1, q_matrix ), -327 /* -0.01 in Q15 */ ) ) )
716 : {
717 5110906 : return 0;
718 : }
719 :
720 6145707 : return 1;
721 : }
722 :
723 100584 : static UWord8 vector_matrix_multiply_3x3_32_fx(
724 : const Word32 *src_vector, /* i : input vector Q30 */
725 : Word32 matrix[3][3], /* i : input matrix Q(q_matrix) */
726 : Word32 *result, /* o : output vector Q(q_matrix - 1) */
727 : Word16 q_matrix )
728 : {
729 100584 : result[0] = Mpy_32_32( matrix[0][0], src_vector[0] ); /* Q(q_matrix - 1) */
730 100584 : result[0] = L_add( result[0], Mpy_32_32( matrix[1][0], src_vector[1] ) ); /* Q(q_matrix - 1) */
731 100584 : result[0] = L_add( result[0], Mpy_32_32( matrix[2][0], src_vector[2] ) ); /* Q(q_matrix - 1) */
732 100584 : move32();
733 100584 : move32();
734 100584 : move32();
735 100584 : IF( LT_32( result[0], Mpy_32_32( L_shl( 1, ( sub( q_matrix, 1 ) ) ), -21474836 /* -0.01 in Q31 */ ) ) )
736 : {
737 31964 : return 0;
738 : }
739 :
740 68620 : result[1] = Mpy_32_32( matrix[0][1], src_vector[0] ); /* Q(q_matrix - 1) */
741 68620 : result[1] = L_add( result[1], Mpy_32_32( matrix[1][1], src_vector[1] ) ); /* Q(q_matrix - 1) */
742 68620 : result[1] = L_add( result[1], Mpy_32_32( matrix[2][1], src_vector[2] ) ); /* Q(q_matrix - 1) */
743 :
744 68620 : move32();
745 68620 : move32();
746 68620 : move32();
747 68620 : IF( LT_32( result[1], Mpy_32_32( L_shl( 1, ( sub( q_matrix, 1 ) ) ), -21474836 /* -0.01 in Q31 */ ) ) )
748 : {
749 45261 : return 0;
750 : }
751 :
752 23359 : result[2] = Mpy_32_32( matrix[0][2], src_vector[0] ); /* Q(q_matrix - 1) */
753 23359 : result[2] = L_add( result[2], Mpy_32_32( matrix[1][2], src_vector[1] ) ); /* Q(q_matrix - 1) */
754 23359 : result[2] = L_add( result[2], Mpy_32_32( matrix[2][2], src_vector[2] ) ); /* Q(q_matrix - 1) */
755 :
756 23359 : move32();
757 23359 : move32();
758 23359 : move32();
759 23359 : IF( LT_32( result[2], Mpy_32_32( L_shl( 1, ( sub( q_matrix, 1 ) ) ), -21474836 /* -0.01 in Q31 */ ) ) )
760 : {
761 23359 : return 0;
762 : }
763 :
764 0 : return 1;
765 : }
766 :
767 : /*----------------------------------------------------------------------------------------------*
768 : * determine_best_triplet_and_gains()
769 : *
770 : * Determine the best speaker node triplet and associated gains for panning to defined direction
771 : *----------------------------------------------------------------------------------------------*/
772 :
773 : /*! r: triplet id */
774 6071615 : static Word16 determine_best_triplet_and_gains_fx(
775 : VBAP_SEARCH_STRUCT *search_struct, /* i : VBAP search struct */
776 : const Word16 panning_unit_vec_fx[3], /* i : panning unit vector Q15 */
777 : const Word16 azi_deg, /* i : panning azimuth */
778 : Word32 gains_fx[3] /* o : panning gains Q(VBAP_VS_TRIPLET.q_inverse_matrix) */
779 : )
780 : {
781 : Word16 i, tr, k;
782 : UWord8 triplet_ok;
783 : Word16 best_triplet;
784 : Word32 best_min_gain_fx;
785 : Word32 min_gain_this_fx;
786 : Word32 unnormalized_gains_fx[3];
787 : Word16 sector;
788 : Word16 first_triplet;
789 : Word16 jump;
790 : Word16 num_triplets;
791 :
792 6071615 : num_triplets = search_struct->num_triplets;
793 6071615 : move16();
794 6071615 : best_min_gain_fx = -2147483647;
795 6071615 : move32();
796 6071615 : best_triplet = 0;
797 6071615 : move16();
798 6071615 : set32_fx( gains_fx, 0, 3 );
799 :
800 : /* Determine the correct search sector for that target panning direction using an optimized algorithm for
801 : * the chosen four sectors. */
802 6071615 : IF( GT_16( abs_s( azi_deg ), 90 ) )
803 : {
804 2972421 : sector = 1;
805 2972421 : move16();
806 2972421 : if ( azi_deg < 0 )
807 : {
808 402339 : sector = 2;
809 402339 : move16();
810 : }
811 : }
812 : ELSE
813 : {
814 3099194 : sector = 0;
815 3099194 : move16();
816 3099194 : if ( azi_deg < 0 )
817 : {
818 1366340 : sector = 3;
819 1366340 : move16();
820 : }
821 : }
822 6071615 : first_triplet = search_struct->initial_search_indices[sector];
823 6071615 : move16();
824 :
825 6071615 : tr = first_triplet;
826 6071615 : move16();
827 6071615 : jump = 1;
828 6071615 : move16();
829 22025400 : FOR( i = 0; i < num_triplets; i++ )
830 : {
831 22025116 : triplet_ok = vector_matrix_multiply_3x3_fx( panning_unit_vec_fx, search_struct->triplets[tr].inverse_matrix_fx, unnormalized_gains_fx, search_struct->triplets[tr].q_inverse_matrix );
832 22025116 : IF( triplet_ok )
833 : {
834 6145707 : min_gain_this_fx = L_min( ( L_min( unnormalized_gains_fx[0], unnormalized_gains_fx[1] ) ), unnormalized_gains_fx[2] );
835 :
836 6145707 : IF( GT_32( min_gain_this_fx, best_min_gain_fx ) )
837 : {
838 6143951 : best_min_gain_fx = min_gain_this_fx;
839 6143951 : move32();
840 6143951 : best_triplet = tr;
841 6143951 : move16();
842 24575804 : FOR( k = 0; k < 3; k++ )
843 : {
844 18431853 : gains_fx[k] = unnormalized_gains_fx[k]; /* Q(VBAP_VS_TRIPLET.q_inverse_matrix) */
845 18431853 : move32();
846 : }
847 6143951 : IF( best_min_gain_fx >= 0 )
848 : {
849 6071331 : return best_triplet;
850 : }
851 : }
852 : }
853 15953785 : tr = add( first_triplet, jump );
854 15953785 : IF( tr < 0 )
855 : {
856 105807 : tr = add( tr, num_triplets );
857 : }
858 15847978 : ELSE IF( GE_16( tr, num_triplets ) )
859 : {
860 3838903 : tr = sub( tr, num_triplets );
861 : }
862 :
863 15953785 : jump = negate( jump );
864 15953785 : IF( jump > 0 )
865 : {
866 6738417 : jump = add( jump, 1 );
867 : }
868 : }
869 :
870 284 : return best_triplet;
871 : }
872 :
873 : /*-------------------------------------------------------------------------*
874 : * determine_virtual_speaker_node_division_gains()
875 : *
876 : * Determines how the virtual node gains are distributed to real nodes
877 : *-------------------------------------------------------------------------*/
878 :
879 3522 : static void determine_virtual_speaker_node_division_gains_fx(
880 : const Word16 virtual_speaker_node_index, /* i : virtual speaker node index */
881 : Word16 *virtual_node_division_gains_fx, /* o : virtual speaker node division gains Q16 */
882 : Word16 connections[][2], /* i : vector of all connections */
883 : const enum VirtualSpeakerNodeType type, /* i : virtual speaker node typel */
884 : const Word16 max_num_connections, /* i : max number of connections */
885 : const Word16 num_speaker_nodes, /* i : max number of speaker nodes */
886 : const Word16 use_object_mode /* i : use VBAP in object panning mode vs. spatial panning mode */
887 : )
888 : {
889 : /* When node type is VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, the gains of the virtual node
890 : are distributed to all neighboring real speaker nodes. An amplitude-division
891 : instead of energy division is utilized just in case to avoid excessive emphasis
892 : on the coherent distributed sound. */
893 : Word16 c, ch, i;
894 : Word16 sum_val_fx;
895 : Word16 *exp_virtual_node_division_gains;
896 3522 : exp_virtual_node_division_gains = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) );
897 :
898 3522 : set16_fx( exp_virtual_node_division_gains, 0, num_speaker_nodes );
899 :
900 3522 : IF( EQ_16( type, VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY ) )
901 : {
902 32450 : FOR( c = 0; c < max_num_connections; c++ )
903 : {
904 31200 : IF( NE_16( connections[c][0], VBAP_NOT_VALID_CONNECTION ) )
905 : {
906 31200 : Word16 connection_node = -1;
907 31200 : move16();
908 31200 : IF( EQ_16( connections[c][0], virtual_speaker_node_index ) )
909 : {
910 0 : connection_node = connections[c][1];
911 0 : move16();
912 : }
913 31200 : ELSE IF( EQ_16( connections[c][1], virtual_speaker_node_index ) )
914 : {
915 5671 : connection_node = connections[c][0];
916 5671 : move16();
917 : }
918 :
919 : /* The second condition allows division gains only to actual loudspeakers */
920 31200 : test();
921 31200 : IF( connection_node >= 0 && ( LT_16( connection_node, num_speaker_nodes ) ) )
922 : {
923 5671 : virtual_node_division_gains_fx[connection_node] = ONE_IN_Q14;
924 5671 : move16();
925 5671 : exp_virtual_node_division_gains[connection_node] = 1;
926 5671 : move16();
927 : }
928 : }
929 : }
930 :
931 1250 : sum_val_fx = 0;
932 1250 : move16();
933 1250 : Word16 guard_bits = find_guarded_bits_fx( num_speaker_nodes );
934 11650 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
935 : {
936 10400 : sum_val_fx = add( sum_val_fx, shr( virtual_node_division_gains_fx[ch], guard_bits ) );
937 : }
938 1250 : Word16 final_exp = 0, res_exp;
939 : Word32 tmp_1, tmp_2, tmp_3;
940 1250 : move16();
941 11650 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
942 : {
943 10400 : IF( virtual_node_division_gains_fx[ch] != 0 )
944 : {
945 5671 : BASOP_Util_Divide_MantExp( virtual_node_division_gains_fx[ch], 1, sum_val_fx, add( guard_bits, 1 ), &virtual_node_division_gains_fx[ch], &final_exp );
946 : }
947 : ELSE
948 : {
949 4729 : virtual_node_division_gains_fx[ch] = 0;
950 4729 : move16();
951 : }
952 10400 : exp_virtual_node_division_gains[ch] = final_exp;
953 10400 : move16();
954 10400 : IF( use_object_mode )
955 : {
956 5310 : IF( virtual_node_division_gains_fx[ch] == 0 )
957 : {
958 1927 : tmp_1 = 0;
959 1927 : move16();
960 1927 : virtual_node_division_gains_fx[ch] = 0;
961 1927 : move16();
962 : }
963 : ELSE
964 : {
965 3383 : Word32 tmp32 = L_deposit_h( virtual_node_division_gains_fx[ch] );
966 3383 : tmp_1 = L_add( BASOP_Util_Log2( tmp32 ), L_shl( ( sub( 31, sub( 31, final_exp ) ) ), 25 ) ); // Q25
967 3383 : tmp_2 = Mpy_32_32( 26843546 /* 0.8f in Q25 */, tmp_1 );
968 3383 : tmp_3 = BASOP_util_Pow2( tmp_2, Q31 - Q19, &res_exp );
969 3383 : exp_virtual_node_division_gains[ch] = res_exp;
970 3383 : move16();
971 3383 : virtual_node_division_gains_fx[ch] = extract_h( tmp_3 );
972 3383 : move16();
973 : }
974 : }
975 : }
976 : /*make a common exp*/
977 11650 : FOR( i = 0; i < num_speaker_nodes; i++ )
978 : {
979 10400 : virtual_node_division_gains_fx[i] = shr( virtual_node_division_gains_fx[i], sub( -1, exp_virtual_node_division_gains[i] ) ); /* Q16 */
980 10400 : move16();
981 : }
982 : }
983 :
984 3522 : free( exp_virtual_node_division_gains );
985 :
986 3522 : return;
987 : }
988 :
989 : /*-------------------------------------------------------------------------*
990 : * check_need_of_virtual_speaker_node()
991 : *
992 : * Check if virtual speaker node is required
993 : *-------------------------------------------------------------------------*/
994 :
995 : /*! r: virtual speaker node type */
996 :
997 2505 : static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node_fx(
998 : VBAP_HANDLE hVBAPdata, /* i/o: VBAP structure */
999 : const Word32 *speaker_node_azi_deg_fx, /* i : vector of speaker node azimuths Q22 */
1000 : const Word32 *speaker_node_ele_deg_fx, /* i : vector of speaker node elevations Q22 */
1001 : enum SpeakerNodeGroup group /* i : group of speaker nodes where this belongs */
1002 : )
1003 : {
1004 : Word16 ch;
1005 2505 : Word32 max_elevation_fx = 0; /* Q22 */
1006 : Word16 Flag1, Flag2, Flag3;
1007 2505 : move32();
1008 :
1009 : /* The following considers if SPEAKER_NODE_BACK virtual speaker is needed */
1010 2505 : IF( EQ_16( group, SPEAKER_NODE_BACK ) )
1011 : {
1012 835 : Word16 virtual_back_needed = 1;
1013 835 : move16();
1014 835 : const Word16 virtual_back_epsilon_fx = -573; /* -0.0175f in Q15 */
1015 835 : move16();
1016 3329 : FOR( ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++ )
1017 : {
1018 3329 : Flag1 = BASOP_Util_Cmp_Mant32Exp( speaker_node_ele_deg_fx[ch], Q31 - Q22, 23040 /*45.0f Q9*/, Q31 - Q9 );
1019 3329 : IF( EQ_16( Flag1, -1 ) )
1020 : {
1021 : Word16 azi_temp;
1022 :
1023 3329 : azi_temp = extract_l( L_shr( Mpy_32_32( speaker_node_azi_deg_fx[ch], ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
1024 3329 : Word16 cos_res = getCosWord16R2( azi_temp ); /* Q15 */
1025 :
1026 3329 : IF( LT_16( cos_res, virtual_back_epsilon_fx ) )
1027 : {
1028 835 : virtual_back_needed = 0;
1029 835 : move16();
1030 835 : BREAK;
1031 : }
1032 : }
1033 : }
1034 :
1035 835 : IF( virtual_back_needed )
1036 : {
1037 0 : hVBAPdata->back_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal;
1038 0 : move16();
1039 0 : hVBAPdata->num_speaker_nodes_internal = add( hVBAPdata->num_speaker_nodes_internal, 1 );
1040 0 : move16();
1041 0 : return VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY;
1042 : }
1043 :
1044 835 : return NO_VIRTUAL_SPEAKER_NODE; /* No virtual back needed */
1045 : }
1046 :
1047 : /* The following considers if TOP or BOTTOM virtual speaker is needed */
1048 15100 : FOR( ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++ )
1049 : {
1050 13430 : IF( EQ_16( group, SPEAKER_NODE_TOP_HALF ) )
1051 : {
1052 6715 : if ( GT_32( speaker_node_ele_deg_fx[ch], max_elevation_fx ) )
1053 : {
1054 571 : max_elevation_fx = speaker_node_ele_deg_fx[ch];
1055 571 : move32();
1056 : }
1057 : }
1058 : ELSE
1059 : {
1060 6715 : IF( GT_32( ( L_negate( speaker_node_ele_deg_fx[ch] ) ), max_elevation_fx ) )
1061 : {
1062 1 : max_elevation_fx = L_negate( speaker_node_ele_deg_fx[ch] );
1063 : }
1064 : }
1065 : }
1066 1670 : Flag2 = BASOP_Util_Cmp_Mant32Exp( max_elevation_fx, Q31 - Q22, 23039 /* 44.9990005 in Q9 */, Q31 - Q9 );
1067 1670 : IF( EQ_16( Flag2, 1 ) )
1068 : {
1069 0 : return NO_VIRTUAL_SPEAKER_NODE;
1070 : }
1071 :
1072 : /* Use virtual node */
1073 1670 : IF( EQ_16( group, SPEAKER_NODE_BOTTOM_HALF ) )
1074 : {
1075 835 : hVBAPdata->bottom_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal;
1076 835 : move16();
1077 : }
1078 : ELSE
1079 : {
1080 835 : hVBAPdata->top_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal;
1081 835 : move16();
1082 : }
1083 :
1084 1670 : hVBAPdata->num_speaker_nodes_internal = add( hVBAPdata->num_speaker_nodes_internal, 1 );
1085 1670 : move16();
1086 1670 : Flag3 = BASOP_Util_Cmp_Mant32Exp( max_elevation_fx, Q31 - Q22, 20478 /* 19.9990005 in Q10 */, Q31 - Q10 );
1087 :
1088 1670 : IF( EQ_16( Flag3, 1 ) )
1089 : {
1090 572 : return VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY;
1091 : }
1092 :
1093 1098 : return VIRTUAL_SPEAKER_NODE_DISCARD_ENERGY;
1094 : }
1095 :
1096 :
1097 : /*-------------------------------------------------------------------------*
1098 : * init_speaker_node_direction_data()
1099 : *
1100 : * Initialize speaker node data
1101 : *-------------------------------------------------------------------------*/
1102 :
1103 835 : static void init_speaker_node_direction_data_fx(
1104 : VBAP_SPEAKER_NODE *speaker_node_data, /* o : storage for speaker node data */
1105 : const Word32 *speaker_node_azi_deg_fx, /* i : vector of speaker node azimuths Q22 */
1106 : const Word32 *speaker_node_ele_deg_fx, /* i : vector of speaker node elevations Q22 */
1107 : const Word16 num_speaker_nodes /* i : number of speaker nodes */
1108 : )
1109 : {
1110 : Word16 ch;
1111 : Word16 azi_rad_fx;
1112 : Word16 ele_rad_fx;
1113 835 : Word16 num_horiz = 0;
1114 835 : UWord8 in_all_mode = TRUE;
1115 835 : move16();
1116 835 : move16();
1117 :
1118 9220 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1119 : {
1120 8385 : speaker_node_data[ch].azi_deg_fx = speaker_node_azi_deg_fx[ch];
1121 8385 : move32();
1122 :
1123 8385 : azi_rad_fx = extract_l( L_shr( Mpy_32_32( speaker_node_azi_deg_fx[ch], ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
1124 8385 : test();
1125 8385 : IF( GE_32( L_shr( speaker_node_ele_deg_fx[ch], Q22 ), -5 ) && LE_32( L_shr( speaker_node_ele_deg_fx[ch], Q22 ), 5 ) )
1126 : {
1127 4821 : speaker_node_data[ch].ele_deg_fx = 0;
1128 4821 : move32();
1129 4821 : ele_rad_fx = 0;
1130 4821 : move32();
1131 4821 : speaker_node_data[ch].group = SPEAKER_NODE_HORIZONTAL;
1132 4821 : move16();
1133 4821 : num_horiz = add( num_horiz, 1 );
1134 : }
1135 : ELSE
1136 : {
1137 3564 : speaker_node_data[ch].ele_deg_fx = speaker_node_ele_deg_fx[ch];
1138 3564 : move32();
1139 3564 : ele_rad_fx = extract_l( L_shr( Mpy_32_32( speaker_node_ele_deg_fx[ch], ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
1140 :
1141 3564 : IF( ele_rad_fx < 0 )
1142 : {
1143 839 : speaker_node_data[ch].group = SPEAKER_NODE_BOTTOM_HALF;
1144 839 : move16();
1145 : }
1146 : ELSE
1147 : {
1148 2725 : speaker_node_data[ch].group = SPEAKER_NODE_TOP_HALF;
1149 2725 : move16();
1150 : }
1151 : }
1152 :
1153 8385 : speaker_node_data[ch].unit_vec_fx[0] = L_shr( L_mult( getCosWord16R2( azi_rad_fx ), getCosWord16R2( ele_rad_fx ) ), 1 ); /* Q15 + Q15 + Q1 - Q1 = Q30 */
1154 8385 : move32();
1155 8385 : speaker_node_data[ch].unit_vec_fx[1] = L_shr( L_mult( getSineWord16R2( azi_rad_fx ), getCosWord16R2( ele_rad_fx ) ), 1 ); /* Q15 + Q15 + Q1 - Q1 = Q30 */
1156 8385 : move32();
1157 8385 : speaker_node_data[ch].unit_vec_fx[2] = L_shr( L_deposit_h( getSineWord16R2( ele_rad_fx ) ), 1 ); /* Q15 + Q16 - Q1 = Q30 */
1158 8385 : move32();
1159 : }
1160 : /* Check for largest horizontal gap if there are at least 3 horizontal speaker nodes */
1161 835 : IF( GE_16( num_horiz, 3 ) )
1162 : {
1163 : Word16 i;
1164 : UWord16 horiz_azi[VBAP_MAX_NUM_SPEAKER_NODES];
1165 :
1166 : UWord16 largest_gap;
1167 : UWord16 temp;
1168 :
1169 835 : i = 0;
1170 835 : move16();
1171 5656 : FOR( ch = 0; ch < num_speaker_nodes && i < num_horiz; ch++ )
1172 : {
1173 4821 : test();
1174 4821 : IF( EQ_16( speaker_node_data[ch].group, SPEAKER_NODE_HORIZONTAL ) )
1175 : {
1176 : Word16 exp1;
1177 4821 : Word32 Mant2 = BASOP_Util_Add_Mant32Exp( speaker_node_azi_deg_fx[ch], Q31 - Q22, 23040 /* 360.0f in Q6 */, Q31 - Q6, &exp1 );
1178 :
1179 4821 : IF( L_shr( speaker_node_azi_deg_fx[ch], 22 ) < 0 )
1180 : {
1181 1998 : horiz_azi[i] = (UWord16) L_shr( Mant2, sub( 31, exp1 ) ); /* Q0 */
1182 : }
1183 : ELSE
1184 : {
1185 2823 : horiz_azi[i] = (UWord16) L_shr( speaker_node_azi_deg_fx[ch], Q22 ); /* Q0 */
1186 : }
1187 4821 : i = add( i, 1 );
1188 : }
1189 : }
1190 :
1191 : /* Reorder horizontal azi to increasing order */
1192 835 : sort( horiz_azi, num_horiz );
1193 :
1194 : /* Find largest gap. Initialize with the wrap over gap. */
1195 835 : largest_gap = add( sub( horiz_azi[0], horiz_azi[num_horiz - 1] ), 360 );
1196 :
1197 4821 : FOR( ch = 0; ch < num_horiz - 1; ch++ )
1198 : {
1199 3986 : temp = sub( horiz_azi[ch + 1], horiz_azi[ch] );
1200 3986 : if ( GT_16( temp, largest_gap ) )
1201 : {
1202 1659 : largest_gap = temp;
1203 1659 : move16();
1204 : }
1205 : }
1206 :
1207 : /* If largest gap is small enough, we have definitive zero elevation plane.
1208 : * Otherwise, we should assign all speaker nodes to one group. */
1209 835 : if ( LE_16( largest_gap, VBAP_MAX_HORIZONTAL_GAP ) )
1210 : {
1211 835 : in_all_mode = FALSE;
1212 835 : move16();
1213 : }
1214 : }
1215 :
1216 : /* Designate all speaker nodes to same group if there was no definitive zero
1217 : * elevation plane. */
1218 835 : IF( in_all_mode )
1219 : {
1220 0 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1221 : {
1222 0 : speaker_node_data[ch].group = SPEAKER_NODE_ALL;
1223 0 : move16();
1224 : }
1225 : }
1226 :
1227 835 : return;
1228 : }
1229 : /*-------------------------------------------------------------------------*
1230 : * matrix_inverse_3x3()
1231 : *
1232 : * 3-by-3 matrix inverse
1233 : *-------------------------------------------------------------------------*/
1234 :
1235 13430 : static void matrix_inverse_3x3_32_fx(
1236 : const Word32 **input_matrix_fx, /* i : input matrix Q30 */
1237 : Word32 inverse_matrix_fx[3][3], /* o : output matrix Q(31 - exp_inv_mat) */
1238 : Word16 *exp_inv_mat )
1239 : {
1240 : Word16 k;
1241 : Word32 determinant_fx; /* Q28 */
1242 : Word32 cross_vec_fx[3]; /* Q29 */
1243 : Word16 exp_inverse_matrix_fx[3][3];
1244 13430 : vbap_crossp_fx( input_matrix_fx[1], input_matrix_fx[2], cross_vec_fx );
1245 13430 : determinant_fx = dotp_fixed( input_matrix_fx[0], cross_vec_fx, 3 );
1246 13430 : Word16 inv_mat_exp = 0;
1247 13430 : move16();
1248 :
1249 53720 : FOR( k = 0; k < 3; k++ )
1250 : {
1251 40290 : inverse_matrix_fx[k][0] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) );
1252 40290 : move32();
1253 40290 : inv_mat_exp = add( inv_mat_exp, ( ( Q31 - Q29 ) - ( Q31 - Q28 ) ) );
1254 40290 : exp_inverse_matrix_fx[k][0] = inv_mat_exp;
1255 40290 : move16();
1256 : }
1257 :
1258 13430 : vbap_crossp_fx( input_matrix_fx[2], input_matrix_fx[0], cross_vec_fx );
1259 :
1260 53720 : FOR( k = 0; k < 3; k++ )
1261 : {
1262 40290 : inverse_matrix_fx[k][1] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) );
1263 40290 : move32();
1264 40290 : inv_mat_exp = add( inv_mat_exp, ( ( Q31 - Q29 ) - ( Q31 - Q28 ) ) );
1265 40290 : exp_inverse_matrix_fx[k][1] = inv_mat_exp;
1266 40290 : move16();
1267 : }
1268 :
1269 13430 : vbap_crossp_fx( input_matrix_fx[0], input_matrix_fx[1], cross_vec_fx );
1270 :
1271 53720 : FOR( k = 0; k < 3; k++ )
1272 : {
1273 40290 : inverse_matrix_fx[k][2] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) );
1274 40290 : move32();
1275 40290 : inv_mat_exp = add( inv_mat_exp, ( ( Q31 - Q29 ) - ( Q31 - Q28 ) ) );
1276 40290 : exp_inverse_matrix_fx[k][2] = inv_mat_exp;
1277 40290 : move16();
1278 : }
1279 :
1280 : /*make common exponant*/
1281 13430 : Word16 max_exp = 0, i, j;
1282 53720 : FOR( i = 0; i < 3; i++ )
1283 : {
1284 161160 : FOR( j = 0; j < 3; j++ )
1285 : {
1286 120870 : max_exp = s_max( max_exp, exp_inverse_matrix_fx[i][j] );
1287 : }
1288 : }
1289 :
1290 13430 : *exp_inv_mat = add( max_exp, 1 );
1291 13430 : move16();
1292 :
1293 53720 : FOR( i = 0; i < 3; i++ )
1294 : {
1295 161160 : FOR( j = 0; j < 3; j++ )
1296 : {
1297 120870 : test();
1298 120870 : IF( LT_16( exp_inverse_matrix_fx[i][j], -15 ) && inverse_matrix_fx[i][j] != 0 )
1299 : {
1300 0 : inverse_matrix_fx[i][j] = 1;
1301 0 : move32();
1302 0 : exp_inverse_matrix_fx[i][j] = 0;
1303 0 : move16();
1304 : }
1305 : ELSE
1306 : {
1307 120870 : inverse_matrix_fx[i][j] = L_shr( inverse_matrix_fx[i][j], sub( *exp_inv_mat, exp_inverse_matrix_fx[i][j] ) ); /* Q(31 - *exp_inv_mat) */
1308 120870 : move32();
1309 : }
1310 : }
1311 : }
1312 :
1313 13430 : return;
1314 : }
1315 : /*-------------------------------------------------------------------------*
1316 : * check_and_store_triplet()
1317 : *
1318 : * Check if the given loudspeaker triplet is a valid one and store data when
1319 : * valid triplet is found.
1320 : *-------------------------------------------------------------------------*/
1321 :
1322 13430 : static Word16 check_and_store_triplet_fx(
1323 : const Word16 chA, /* i : first channel index that forms the loudspeaker triplet */
1324 : const Word16 chB, /* i : second channel index that forms the loudspeaker triplet */
1325 : const Word16 chC, /* i : third channel index that forms the loudspeaker triplet */
1326 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1327 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */
1328 : VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */
1329 : Word16 *triplet_index, /* i/o: index for the next free triplet slot */
1330 : Word32 *triplet_azidegs_fx, /* o : center azimuths of the found triplets Q19 */
1331 : Word16 *triplet_order /* o : initial order of triplet indices */
1332 : )
1333 : {
1334 : Word16 ch_check;
1335 : Word16 k;
1336 : Word16 speaker_node_found_inside_triplet;
1337 : UWord8 triplet_ok;
1338 : Word16 exp_inv_mat;
1339 :
1340 : Word32 inverse_matrix_fx[3][3] /* Q(31 - exp_inv_mat) */, unnormalized_gains_fx[3] /* Q(31 - exp_inv_mat - 1) */;
1341 13430 : set32_fx( unnormalized_gains_fx, 0, 3 );
1342 : const Word32 *speaker_node_triplet_unit_vec_matrix_fx[3]; /* Q30 */
1343 :
1344 : /* Triplet found, determine inverse matrix for VBAP formulation */
1345 :
1346 13430 : speaker_node_triplet_unit_vec_matrix_fx[0] = speaker_node_data[chA].unit_vec_fx;
1347 13430 : move32();
1348 13430 : speaker_node_triplet_unit_vec_matrix_fx[1] = speaker_node_data[chB].unit_vec_fx;
1349 13430 : move32();
1350 13430 : speaker_node_triplet_unit_vec_matrix_fx[2] = speaker_node_data[chC].unit_vec_fx;
1351 13430 : move32();
1352 13430 : matrix_inverse_3x3_32_fx( speaker_node_triplet_unit_vec_matrix_fx, inverse_matrix_fx, &exp_inv_mat );
1353 13430 : triplets[*triplet_index].q_inverse_matrix = sub( 31, exp_inv_mat );
1354 13430 : move16();
1355 :
1356 : /* Check through all speaker nodes that none of them are within the triplet.
1357 : * Node within the triplet is identified by that all three panning gains are positive.
1358 : * Epsilon-condition is for some small rounding issues.*/
1359 13430 : speaker_node_found_inside_triplet = 0;
1360 13430 : move16();
1361 154304 : FOR( ch_check = 0; ch_check < num_speaker_nodes; ch_check++ )
1362 : {
1363 140874 : test();
1364 140874 : test();
1365 140874 : IF( ( NE_16( ch_check, chA ) ) && NE_16( ch_check, chB ) && NE_16( ch_check, chC ) )
1366 : {
1367 100584 : triplet_ok = vector_matrix_multiply_3x3_32_fx( speaker_node_data[ch_check].unit_vec_fx, inverse_matrix_fx, unnormalized_gains_fx, sub( Q31, exp_inv_mat ) );
1368 100584 : test();
1369 100584 : test();
1370 100584 : test();
1371 100584 : IF( triplet_ok && ( GT_32( unnormalized_gains_fx[0], L_shl( VBAP_EPSILON_Q31, sub( Q30, exp_inv_mat ) ) ) ) && ( GT_32( unnormalized_gains_fx[1], L_shl( VBAP_EPSILON_Q31, sub( Q30, exp_inv_mat ) ) ) ) && ( GT_32( unnormalized_gains_fx[2], L_shl( VBAP_EPSILON_Q31, sub( Q30, exp_inv_mat ) ) ) ) )
1372 : {
1373 0 : speaker_node_found_inside_triplet = 1;
1374 0 : move16();
1375 0 : BREAK;
1376 : }
1377 : }
1378 : }
1379 :
1380 : /* No speaker node inside triplet -> appropriate triplet found, save data. */
1381 13430 : IF( speaker_node_found_inside_triplet == 0 )
1382 : {
1383 13430 : triplets[*triplet_index].speaker_node[0] = (UWord8) chA;
1384 13430 : triplets[*triplet_index].speaker_node[1] = (UWord8) chB;
1385 13430 : triplets[*triplet_index].speaker_node[2] = (UWord8) chC;
1386 13430 : move16();
1387 13430 : move16();
1388 13430 : move16();
1389 53720 : FOR( k = 0; k < 3; k++ )
1390 : {
1391 40290 : Copy32( inverse_matrix_fx[k], triplets[*triplet_index].inverse_matrix_fx[k], 3 );
1392 : }
1393 : /* Get center azimuth for fast search use */
1394 13430 : 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] ), Q2 ), L_shr( speaker_node_data[chC].unit_vec_fx[1], Q2 ) ); /* Q28 */
1395 : /*Condition to make tmp_a 0 to adress precision loss seen*/
1396 13430 : if ( EQ_32( tmp_a, -8193 /* -0.0000305 in Q28 */ ) )
1397 : {
1398 0 : tmp_a = 0;
1399 0 : move32();
1400 : }
1401 13430 : 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 ) ); /* Q28 */
1402 13430 : Word16 tmp_tan = shr( BASOP_util_atan2( tmp_a, tmp_b, 0 ), Q13 - Q9 ); /* Q9 */
1403 13430 : triplet_azidegs_fx[*triplet_index] = L_mult( tmp_tan, 29335 /*_180_OVER_PI in Q9*/ ); /* Q19 */
1404 13430 : move32();
1405 : /* Store increasing order indices for the later sorting step. */
1406 13430 : triplet_order[*triplet_index] = *triplet_index;
1407 13430 : move16();
1408 :
1409 13430 : *triplet_index = add( *triplet_index, 1 );
1410 13430 : move16();
1411 :
1412 13430 : return 1;
1413 : }
1414 :
1415 : /* Triplet was not good */
1416 0 : return 0;
1417 : }
1418 : /*-------------------------------------------------------------------------*
1419 : * determine_virtual_surface_triplets()
1420 : *
1421 : * Determine virtual surface triples that are used for panning. This
1422 : * function is optimized for the use in cases where speaker nodes are in
1423 : * one group or divided into two separate groups divided by a horizontal
1424 : * layer.
1425 : *-------------------------------------------------------------------------*/
1426 :
1427 : /*! r: number of virtual surface triplets */
1428 :
1429 1670 : static Word16 determine_virtual_surface_triplets_fx(
1430 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1431 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */
1432 : Word16 connections[][2], /* i : vector of all connections */
1433 : const Word16 max_num_connections, /* i : max number of connections */
1434 : VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */
1435 : Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS], /* o : initial search indices for this set of triplets corresponding to the search struct */
1436 : enum SpeakerNodeGroup allowed_group /* i : group of allowed speaker nodes for forming the triplets in this call */
1437 : )
1438 : {
1439 : Word16 chA, chB, chC, k, l, m;
1440 1670 : Word16 num_triplets = 0;
1441 1670 : move16();
1442 : Word16 num_connected_to_chA;
1443 : Word16 connected_to_chA[VBAP_MAX_NUM_SPEAKER_NODES];
1444 : Word16 connection_uses_left[VBAP_MAX_NUM_SPEAKER_NODES];
1445 : Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS]; /* Q19 */
1446 : Word16 triplet_order[VBAP_MAX_NUM_TRIPLETS];
1447 :
1448 : /* Each connection can be used exactly by two different virtual surface triplets. */
1449 1670 : set16_fx( connection_uses_left, 2, VBAP_MAX_NUM_SPEAKER_NODES );
1450 :
1451 18440 : FOR( chA = 0; chA < num_speaker_nodes; chA++ )
1452 : {
1453 : /* Early skip if not in correct group. */
1454 16770 : IF( NE_16( speaker_node_data[chA].group, allowed_group ) )
1455 : {
1456 13206 : CONTINUE;
1457 : }
1458 :
1459 : /* Get all connections connected to current chA that have not been used by
1460 : * two triplets yet. */
1461 3564 : num_connected_to_chA = 0;
1462 3564 : move16();
1463 70458 : FOR( k = 0; k < max_num_connections; k++ )
1464 : {
1465 66894 : test();
1466 66894 : test();
1467 66894 : IF( ( EQ_16( connections[k][0], chA ) || EQ_16( connections[k][1], chA ) ) && ( connection_uses_left[k] > 0 ) )
1468 : {
1469 14742 : connected_to_chA[num_connected_to_chA] = k;
1470 14742 : move16();
1471 14742 : num_connected_to_chA = add( num_connected_to_chA, 1 );
1472 : }
1473 : }
1474 :
1475 : /* Check that we have enough connections to use. We need at least two available connections to form a triplet.
1476 : * This can fail in later stages when all connections are already used. */
1477 3564 : IF( LT_16( num_connected_to_chA, 2 ) )
1478 : {
1479 582 : CONTINUE;
1480 : }
1481 :
1482 : /* Try to form triplets from each valid connection. */
1483 17724 : FOR( k = 0; k < num_connected_to_chA; k++ )
1484 : {
1485 14742 : Word16 connect_index_k = connected_to_chA[k];
1486 14742 : move16();
1487 14742 : chB = connections[connect_index_k][0];
1488 14742 : move16();
1489 14742 : if ( EQ_16( connections[connect_index_k][0], chA ) )
1490 : {
1491 3216 : chB = connections[connect_index_k][1];
1492 3216 : move16();
1493 : }
1494 25541 : FOR( l = k + 1; l < num_connected_to_chA; l++ )
1495 : {
1496 22559 : Word16 connect_index_l = connected_to_chA[l];
1497 22559 : move16();
1498 22559 : chC = connections[connect_index_l][0];
1499 22559 : move16();
1500 22559 : if ( EQ_16( connections[connect_index_l][0], chA ) )
1501 : {
1502 6217 : chC = connections[connect_index_l][1];
1503 6217 : move16();
1504 : }
1505 :
1506 : /* With chA, chB, and chC selected, we still need to find connection between chB and chC and verify that the triplet is valid */
1507 272960 : FOR( m = 0; m < max_num_connections; m++ )
1508 : {
1509 263831 : test();
1510 263831 : test();
1511 263831 : test();
1512 263831 : 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 ) ) )
1513 : {
1514 13430 : Word16 flag = check_and_store_triplet_fx( chA, chB, chC, num_speaker_nodes, speaker_node_data, triplets, &num_triplets, triplet_azidegs_fx, triplet_order );
1515 13430 : IF( EQ_16( flag, 1 ) )
1516 : {
1517 13430 : connection_uses_left[connect_index_k] = sub( connection_uses_left[connect_index_k], 1 );
1518 13430 : move16();
1519 13430 : connection_uses_left[connect_index_l] = sub( connection_uses_left[connect_index_l], 1 );
1520 13430 : move16();
1521 13430 : connection_uses_left[m] = sub( connection_uses_left[m], 1 );
1522 13430 : move16();
1523 13430 : BREAK;
1524 : }
1525 : }
1526 : }
1527 :
1528 : /* Check if chA-chB connection has been used in two triplets already. If yes, then break out of loop
1529 : * as this connection cannot be used for more triplets and we need to continue with another chA-chB
1530 : * connection. */
1531 22559 : IF( LT_16( connection_uses_left[connect_index_k], 1 ) )
1532 : {
1533 11760 : BREAK;
1534 : }
1535 : }
1536 : }
1537 : }
1538 :
1539 : /* All triplets should be stored now. Sort them for search use and then determine the initial search indices for
1540 : * each search sector for this search struct. */
1541 1670 : v_sort_ind_fixed( triplet_azidegs_fx, triplet_order, num_triplets );
1542 1670 : reorder_triplets_fx( triplets, triplet_order, num_triplets );
1543 1670 : determine_initial_search_indices_fx( num_triplets, triplet_azidegs_fx, initial_search_indices );
1544 :
1545 1670 : return num_triplets;
1546 : }
1547 : /*-------------------------------------------------------------------------*
1548 : * determine_initial_search_indices()
1549 : *
1550 : * Determine initial search indices used for fast search of correct triangle
1551 : *-------------------------------------------------------------------------*/
1552 :
1553 :
1554 1670 : static void determine_initial_search_indices_fx(
1555 : const Word16 num_triplets, /* i : number of triplets */
1556 : const Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS], /* i : azimuths of triplets (in degrees) Q19 */
1557 : Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS] /* o : initial search indices */
1558 : )
1559 : {
1560 : Word16 i, j;
1561 : Word32 sector_reference_azideg_fx; /* Q0 */
1562 : Word32 sector_border_start_azideg_fx; /* Q0 */
1563 : Word32 sector_border_end_azideg_fx; /* Q0 */
1564 : Word16 best_index;
1565 : Word32 min_azideg_diff_fx;
1566 : Word32 azideg_diff_fx; /* Q19 */
1567 :
1568 8350 : FOR( i = 0; i < VBAP_NUM_SEARCH_SECTORS; i++ )
1569 : {
1570 6680 : sector_border_start_azideg_fx = imult3216( i, VBAP_SEARCH_SECTOR_SIZE_Q0 );
1571 6680 : sector_border_end_azideg_fx = imult3216( add( i, 1 ), VBAP_SEARCH_SECTOR_SIZE_Q0 );
1572 6680 : sector_reference_azideg_fx = L_shr( L_add( sector_border_start_azideg_fx, sector_border_end_azideg_fx ), 1 );
1573 :
1574 :
1575 6680 : best_index = 0;
1576 6680 : move16();
1577 6680 : min_azideg_diff_fx = ONE_IN_Q31;
1578 6680 : move32();
1579 :
1580 60400 : FOR( j = 0; j < num_triplets; j++ )
1581 : {
1582 53720 : azideg_diff_fx = L_sub( L_shl( sector_reference_azideg_fx, Q19 ), triplet_azidegs_fx[j] );
1583 :
1584 53720 : IF( GT_32( azideg_diff_fx, 94371840 /* 180.0f in Q19 */ ) )
1585 : {
1586 23309 : azideg_diff_fx = L_sub( azideg_diff_fx, 188743680 /* 360.0f in Q19 */ );
1587 : }
1588 30411 : ELSE IF( LT_32( azideg_diff_fx, -94371840 /* -180.0f in Q19 */ ) )
1589 : {
1590 0 : azideg_diff_fx = L_add( azideg_diff_fx, 188743680 /* 360.0f in Q19 */ );
1591 : }
1592 53720 : azideg_diff_fx = L_abs( azideg_diff_fx );
1593 :
1594 53720 : IF( LT_32( azideg_diff_fx, min_azideg_diff_fx ) )
1595 : {
1596 22744 : min_azideg_diff_fx = azideg_diff_fx;
1597 22744 : move32();
1598 22744 : best_index = j;
1599 22744 : move16();
1600 : }
1601 : }
1602 :
1603 6680 : initial_search_indices[i] = best_index;
1604 6680 : move16();
1605 : }
1606 :
1607 1670 : return;
1608 : }
1609 :
1610 : /*-------------------------------------------------------------------------*
1611 : * determine_connections()
1612 : *
1613 : * Determine all valid connections between all speaker nodes
1614 : *-------------------------------------------------------------------------*/
1615 :
1616 835 : static ivas_error determine_connections_fx(
1617 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1618 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
1619 : Word16 connections[][2], /* o : vector of connections */
1620 : const Word16 max_num_connections, /* i : max number of connections */
1621 : Word16 *group1_count, /* o : number of connections in first group */
1622 : Word16 *group2_start, /* o : start of second group of connections */
1623 : Word16 *group2_count /* o : number of connections in second group */
1624 : )
1625 : {
1626 : Word16 num_non_crossing_planes;
1627 : Word16 c;
1628 835 : Word16 connection_write_index = 0;
1629 835 : move16();
1630 : Word32 non_crossing_plane_elevation_deg_fx[VBAP_MAX_PLANES]; /* Q14 */
1631 :
1632 : ivas_error error;
1633 :
1634 835 : set32_fx( non_crossing_plane_elevation_deg_fx, 0, VBAP_MAX_PLANES );
1635 :
1636 : /* Reset connection data */
1637 20980 : FOR( c = 0; c < max_num_connections; c++ )
1638 : {
1639 20145 : connections[c][0] = VBAP_NOT_VALID_CONNECTION;
1640 20145 : move16();
1641 : }
1642 :
1643 : /* This function determines some prominent elevated planes, that are favoured in making node-node connections. */
1644 835 : num_non_crossing_planes = determine_non_crossing_planes_fx( num_speaker_nodes, speaker_node_data, non_crossing_plane_elevation_deg_fx );
1645 :
1646 : /* Process in different mode based on the grouping. It is enough to check for first node. */
1647 835 : IF( EQ_16( speaker_node_data[0].group, SPEAKER_NODE_ALL ) )
1648 : {
1649 0 : IF( NE_32( ( 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 ) )
1650 : {
1651 0 : return error;
1652 : }
1653 : }
1654 : ELSE
1655 : {
1656 : /* The node-node connections are determined in three stages: bottom, horizontal, and top. */
1657 835 : IF( NE_32( ( 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 ) )
1658 : {
1659 0 : return error;
1660 : }
1661 835 : *group2_start = connection_write_index;
1662 835 : move16();
1663 :
1664 835 : formulate_horizontal_connections_fx( speaker_node_data, num_speaker_nodes, connections, &connection_write_index );
1665 835 : *group1_count = connection_write_index;
1666 835 : move16();
1667 :
1668 835 : IF( NE_32( ( 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 ) )
1669 : {
1670 0 : return error;
1671 : }
1672 :
1673 835 : *group2_count = sub( connection_write_index, *group2_start );
1674 835 : move16();
1675 : }
1676 :
1677 835 : return IVAS_ERR_OK;
1678 : }
1679 : /*-------------------------------------------------------------------------*
1680 : * determine_connection_class()
1681 : *
1682 : * Determine the type of connection
1683 : *-------------------------------------------------------------------------*/
1684 :
1685 : /*! r: type of connection */
1686 24765 : static enum ConnectionClass determine_connection_class_fx(
1687 : const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */
1688 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1689 : const enum SpeakerNodeGroup group, /* i : speaker node group */
1690 : const Word16 chA, /* i : speaker node counter 1 */
1691 : const Word16 chB /* i : speaker node counter 2 */
1692 : )
1693 : {
1694 : Word16 ch, k;
1695 :
1696 : const Word32 *p1_fx, *v2_fx;
1697 : Word32 v1v1_fx, v1v2_fx, v2v2_fx, v1p1_fx, v2p1_fx; /* Q25, Q27, Q29, Q27, Q29 */
1698 : Word32 determinant_fx; /* Q23 */
1699 : Word32 norm_distance_on_v1_fx;
1700 : Word32 vec_diff_fx[3];
1701 : Word32 v1_fx[3]; /* Q28 */
1702 : Word32 vTarget_fx[3];
1703 : Word32 energy_sum_fx;
1704 : Word32 eq_value_fx;
1705 : Word32 uvecdot_fx; /* Q30 */
1706 :
1707 : /* Check if connection passes through origin. This is not desired.
1708 : * When this happens, unit vectors point in opposite directions. */
1709 24765 : 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
1710 :
1711 24765 : test();
1712 24765 : IF( LT_32( uvecdot_fx, VBAP_EPSILON_Q3O ) && GT_32( uvecdot_fx, L_negate( VBAP_EPSILON_Q3O ) ) )
1713 : {
1714 0 : return CONNECTION_WITH_SPEAKER_NODE_BEHIND;
1715 : }
1716 :
1717 : /* This loop checks the chA-chB connection with respect to all loudspeakers:
1718 : - in case there is a node behind or nearly behind the connection line. These
1719 : connections need to be discarded.
1720 : - in case there is a node that is closer to the connection line than 1/5 of the
1721 : connection length AND at the same horizontal plane. These connections need to be
1722 : weighted with a penalty (a special case, for example avoiding elevated L,R,C triplet)
1723 : */
1724 285793 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1725 : {
1726 : /* Select speaker_node only within TOP or BOTTOM sphere half, not being part of chA-chB pair */
1727 263578 : test();
1728 263578 : test();
1729 263578 : IF( ( EQ_16( group, node_data[ch].group ) ) && ( NE_16( ch, chA ) ) && ( NE_16( ch, chB ) ) )
1730 : {
1731 : /* The following lines formulate the point on the chA-chB-connection that is
1732 : nearest to the origo-ch-line */
1733 58048 : p1_fx = node_data[chA].unit_vec_fx; // Q30
1734 58048 : move32();
1735 :
1736 232192 : FOR( k = 0; k < 3; k++ )
1737 : {
1738 174144 : 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) */
1739 174144 : move32();
1740 : }
1741 58048 : v2_fx = node_data[ch].unit_vec_fx; // Q30
1742 58048 : move32();
1743 :
1744 58048 : v1v1_fx = dotp_fixed( v1_fx, v1_fx, 3 ); // Q25
1745 58048 : move32();
1746 58048 : v1v2_fx = dotp_fixed( v1_fx, v2_fx, 3 ); // Q27
1747 58048 : move32();
1748 58048 : v2v2_fx = ONE_IN_Q29;
1749 58048 : move32();
1750 :
1751 58048 : v1p1_fx = dotp_fixed( v1_fx, p1_fx, 3 ); // Q27
1752 58048 : move32();
1753 58048 : v2p1_fx = dotp_fixed( v2_fx, p1_fx, 3 ); // Q29
1754 58048 : move32();
1755 :
1756 58048 : Word32 tmp1 = Mpy_32_32( v1v2_fx, v1v2_fx ); // Q23
1757 58048 : Word32 tmp2 = Mpy_32_32( v1v1_fx, L_negate( v2v2_fx ) ); // Q23
1758 58048 : Word32 tmp3 = L_add( tmp1, tmp2 ); // Q23
1759 :
1760 58048 : determinant_fx = tmp3; // Q23
1761 58048 : move32();
1762 :
1763 : /* Norm distance = distance parameter on line chA-chB, determines point that is
1764 : nearest to origo-ch line. Distance 0 means chA and distance 1 means chB. Can be
1765 : outside this region as well.*/
1766 :
1767 58048 : Word32 tmp4 = Mpy_32_32( v2v2_fx, v1p1_fx ); // q25
1768 58048 : Word32 tmp5 = Mpy_32_32( L_negate( v1v2_fx ), v2p1_fx ); // q25
1769 58048 : Word32 tmp6 = L_add( tmp4, tmp5 ); // q25
1770 58048 : Word16 tmp7, exp = 0;
1771 58048 : move16();
1772 58048 : if ( determinant_fx == 0 )
1773 : {
1774 0 : determinant_fx = 1;
1775 0 : move32();
1776 : }
1777 58048 : tmp7 = BASOP_Util_Divide3232_Scale( ONE_IN_Q23, determinant_fx, &exp ); // Q(15 - exp)
1778 58048 : norm_distance_on_v1_fx = Mpy_32_16_1( tmp6, tmp7 ); // Q25 + Q(15 - exp) - Q15 = Q(25 - exp)
1779 :
1780 : /* Continue only if the nearest point is between chA and chB */
1781 58048 : Word16 exp_vTarget = 0, exp_energy_sum = 0, exp_vec_diff = 0;
1782 : Word32 var1, var2;
1783 : Word16 vTarget_fx_e[3], vec_diff_e[3];
1784 58048 : test();
1785 58048 : IF( norm_distance_on_v1_fx > 0 && LT_32( norm_distance_on_v1_fx, L_shr( ONE_IN_Q31, sub( 31, sub( Q25, exp ) ) ) ) )
1786 : {
1787 : /* Formulate vTarget, that is an unit vector that goes through the determined point on chA-chB connection */
1788 47482 : energy_sum_fx = 0;
1789 47482 : move32();
1790 189928 : FOR( k = 0; k < 3; k++ )
1791 : {
1792 142446 : var1 = Mpy_32_32( norm_distance_on_v1_fx, v1_fx[k] ); // Q(25 - exp) + Q28 - 31
1793 142446 : vTarget_fx[k] = BASOP_Util_Add_Mant32Exp( p1_fx[k], 1, var1, sub( Q31, add( sub( Q25, exp ), Q28 - Q31 ) ), &exp_vTarget );
1794 142446 : move16();
1795 142446 : vTarget_fx_e[k] = exp_vTarget;
1796 142446 : move16();
1797 :
1798 142446 : var2 = Mpy_32_32( vTarget_fx[k], vTarget_fx[k] ); // 2*exp_vTarget
1799 142446 : energy_sum_fx = BASOP_Util_Add_Mant32Exp( energy_sum_fx, exp_energy_sum, var2, shl( exp_vTarget, 1 ), &exp_energy_sum );
1800 142446 : vec_diff_fx[k] = BASOP_Util_Add_Mant32Exp( vTarget_fx[k], exp_vTarget, L_negate( v2_fx[k] ), 1, &exp_vec_diff );
1801 142446 : move16();
1802 142446 : vec_diff_e[k] = exp_vec_diff;
1803 142446 : move16();
1804 : }
1805 47482 : Word16 exp_eq = exp_energy_sum;
1806 47482 : move16();
1807 47482 : eq_value_fx = ISqrt32( energy_sum_fx, &exp_eq );
1808 189928 : FOR( k = 0; k < 3; k++ )
1809 : {
1810 142446 : vTarget_fx[k] = Mpy_32_32( vTarget_fx[k], eq_value_fx );
1811 142446 : move32();
1812 142446 : vTarget_fx_e[k] = add( vTarget_fx_e[k], exp_eq );
1813 142446 : move16();
1814 : }
1815 : /*make a common exponent*/
1816 47482 : Word16 max_vTarget_e = 0, max_vec_diff_e = 0;
1817 47482 : move16();
1818 47482 : move16();
1819 189928 : FOR( k = 0; k < 3; k++ )
1820 : {
1821 142446 : max_vTarget_e = s_max( vTarget_fx_e[k], max_vTarget_e );
1822 142446 : max_vec_diff_e = s_max( vec_diff_e[k], max_vec_diff_e );
1823 : }
1824 189928 : FOR( k = 0; k < 3; k++ )
1825 : {
1826 142446 : vTarget_fx[k] = L_shr( vTarget_fx[k], sub( max_vTarget_e, vTarget_fx_e[k] ) );
1827 142446 : move32();
1828 142446 : vec_diff_fx[k] = L_shr( vec_diff_fx[k], sub( max_vec_diff_e, vec_diff_e[k] ) );
1829 142446 : move32();
1830 : }
1831 : /* A check if the angle between vTarget and node_data[ch].unit_vec is less than 1 degree.
1832 : Essentially reveals if there is a speaker node too close "behind" the connection. Such
1833 : connections should be rejected.*/
1834 189928 : FOR( Word16 i = 0; i < 3; i++ )
1835 : {
1836 142446 : vTarget_fx[i] = L_shr( vTarget_fx[i], 2 ); // add guard bits
1837 142446 : move32();
1838 142446 : vec_diff_fx[i] = L_shr( vec_diff_fx[i], 2 );
1839 142446 : move32();
1840 : }
1841 47482 : Word32 res = dotp_fixed( vTarget_fx, v2_fx, 3 ); // 31 - (max_vTarget_e + 2) + 30 - 31 = 28 - max_vTarget_e
1842 47482 : move32();
1843 :
1844 47482 : IF( GT_32( res, L_shr( 2147054208 /* 0.9998f in Q31 */, sub( 31, sub( 28, max_vTarget_e ) ) ) ) )
1845 : {
1846 2550 : return CONNECTION_WITH_SPEAKER_NODE_BEHIND;
1847 : }
1848 :
1849 : /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes.
1850 : A triplet between these nodes is not desired if there is a top node,
1851 : a penalty is implemented to take care of this. */
1852 44932 : Word32 vec_diff_dotp = dotp_fixed( vec_diff_fx, vec_diff_fx, 3 ); // exp : 2 * max_vec_diff_e + 4
1853 44932 : move32();
1854 44932 : Word32 var = Mpy_32_32( vec_diff_dotp, 51200 /*25.0f in Q11*/ ); // exp : 2 * max_vec_diff_e + 4 + 20
1855 44932 : Word16 Flag1 = BASOP_Util_Cmp_Mant32Exp( v1v1_fx, Q31 - Q25, var, add( shl( max_vec_diff_e, 1 ), 4 + 20 ) );
1856 44932 : IF( EQ_16( Flag1, 1 ) )
1857 : {
1858 0 : IF( LT_32( L_abs( L_sub( node_data[chB].unit_vec_fx[2], node_data[chA].unit_vec_fx[2] ) ), 1073742 /* 0.001f in Q30 */ ) )
1859 : {
1860 0 : return ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION;
1861 : }
1862 : }
1863 : }
1864 : }
1865 : }
1866 :
1867 22215 : return REGULAR_CONNECTION;
1868 : }
1869 :
1870 : /*-------------------------------------------------------------------------*
1871 : * formulate_horizontal_connections()
1872 : *
1873 : * Formulate connections in the horizontal plane
1874 : *-------------------------------------------------------------------------*/
1875 :
1876 835 : static void formulate_horizontal_connections_fx(
1877 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
1878 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1879 : Word16 connections[][2], /* o : vector of all connections */
1880 : Word16 *connection_write_index )
1881 : {
1882 : Word16 ch;
1883 : Word16 chCheck;
1884 : Word16 next_index;
1885 :
1886 : Word32 min_arc_diff_fx;
1887 : Word32 arc_diff_fx;
1888 : Word16 Q_min_arc_diff;
1889 :
1890 9220 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1891 : {
1892 : /* Find next horizontal speaker node */
1893 8385 : IF( EQ_16( speaker_node_data[ch].group, SPEAKER_NODE_HORIZONTAL ) )
1894 : {
1895 4821 : next_index = -1;
1896 4821 : move16();
1897 4821 : min_arc_diff_fx = 19998 /*9999.0f in Q1*/;
1898 4821 : move32();
1899 4821 : Q_min_arc_diff = 1;
1900 4821 : move16();
1901 :
1902 53868 : FOR( chCheck = 0; chCheck < num_speaker_nodes; chCheck++ )
1903 : {
1904 49047 : test();
1905 49047 : IF( ( NE_16( ch, chCheck ) ) && ( EQ_16( speaker_node_data[chCheck].group, SPEAKER_NODE_HORIZONTAL ) ) )
1906 : {
1907 23802 : arc_diff_fx = L_sub( speaker_node_data[chCheck].azi_deg_fx, speaker_node_data[ch].azi_deg_fx ); // Q22
1908 :
1909 35703 : WHILE( arc_diff_fx < 0 )
1910 : {
1911 11901 : arc_diff_fx = L_add( arc_diff_fx, 1509949440 /*360.0f in Q22*/ );
1912 : }
1913 23802 : IF( LT_32( L_shr( arc_diff_fx, sub( 22, Q_min_arc_diff ) ), min_arc_diff_fx ) )
1914 : {
1915 10789 : min_arc_diff_fx = arc_diff_fx;
1916 10789 : move32();
1917 10789 : Q_min_arc_diff = 22;
1918 10789 : move16();
1919 10789 : next_index = chCheck;
1920 10789 : move16();
1921 : }
1922 : }
1923 : }
1924 4821 : connections[*connection_write_index][0] = ch;
1925 4821 : move16();
1926 4821 : connections[*connection_write_index][1] = next_index;
1927 4821 : move16();
1928 4821 : *connection_write_index = add( *connection_write_index, 1 );
1929 4821 : move16();
1930 : }
1931 : }
1932 :
1933 835 : return;
1934 : }
1935 :
1936 : /*-------------------------------------------------------------------------*
1937 : * check_plane_crossing()
1938 : *
1939 : * Check crossing of non-allowed planes
1940 : *-------------------------------------------------------------------------*/
1941 :
1942 : /*! r: truth value for crossing */
1943 22215 : static Word16 check_plane_crossing_fx(
1944 : const Word32 ele1_deg_fx, /* i : speaker node 1 elevation Q22 */
1945 : const Word32 ele2_deg_fx, /* i : speaker node 2 elevation Q22 */
1946 : const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */
1947 : const Word32 *non_crossing_plane_elevation_deg_fx /* i : vector non-crossing plane elevations Q14 */
1948 : )
1949 : {
1950 : /* Find if the connection crosses a non-crossing plane, with 1-degree threshold. */
1951 : Word16 plane;
1952 :
1953 36392 : FOR( plane = 0; plane < num_non_crossing_planes; plane++ )
1954 : {
1955 14888 : test();
1956 14888 : 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 ) ) ) )
1957 : {
1958 8 : return 1;
1959 : }
1960 14880 : test();
1961 14880 : 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 ) ) ) )
1962 : {
1963 703 : return 1;
1964 : }
1965 : }
1966 :
1967 21504 : return 0;
1968 : }
1969 :
1970 : /*-------------------------------------------------------------------------*
1971 : * get_half_sphere_connection_options()
1972 : *
1973 : * Get list of all potential connections at the half-sphere
1974 : *-------------------------------------------------------------------------*/
1975 1670 : static ivas_error get_half_sphere_connection_options_fx(
1976 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
1977 : const enum SpeakerNodeGroup group, /* i : speaker node group */
1978 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1979 : const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */
1980 : const Word32 *non_crossing_plane_elevation_deg_fx, /* i : vector of non-crossing plane elevations Q14 */
1981 : ConnectionOption **connection_options_pr, /* o : list of connection options */
1982 : Word16 *num_connection_options /* o : number of connection options */
1983 : )
1984 : {
1985 1670 : Word16 max_num_connection_options = 0;
1986 1670 : Word16 index = 0;
1987 : Word16 node, chA, chB, c, c_cmp;
1988 : ConnectionOption *c_options, *c_options_reorder;
1989 :
1990 1670 : move16();
1991 1670 : move16();
1992 :
1993 : /* Count max num connection options at the half sphere */
1994 18440 : FOR( node = 0; node < num_speaker_nodes; node++ )
1995 : {
1996 16770 : test();
1997 16770 : IF( EQ_16( speaker_node_data[node].group, group ) || EQ_16( speaker_node_data[node].group, SPEAKER_NODE_HORIZONTAL ) )
1998 : {
1999 13206 : max_num_connection_options = add( max_num_connection_options, index );
2000 13206 : index = add( index, 1 );
2001 : }
2002 : }
2003 :
2004 : /* Init memory for connection options */
2005 1670 : IF( ( c_options = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * max_num_connection_options ) ) == NULL )
2006 : {
2007 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
2008 : }
2009 :
2010 50237 : FOR( c = 0; c < max_num_connection_options; c++ )
2011 : {
2012 48567 : c_options[c].chA = -1;
2013 48567 : move16();
2014 48567 : c_options[c].chB = -1;
2015 48567 : move16();
2016 48567 : c_options[c].arc_fx = MAX_32;
2017 48567 : move32();
2018 48567 : c_options[c].arc_weighted_fx = MAX_32;
2019 48567 : move32();
2020 : }
2021 :
2022 : /* Determine connection options for the half-sphere */
2023 1670 : index = 0;
2024 1670 : move16();
2025 18440 : FOR( chA = 0; chA < num_speaker_nodes; chA++ )
2026 : {
2027 : /* First loudspeaker at the connection is at the half sphere */
2028 16770 : test();
2029 16770 : IF( EQ_16( speaker_node_data[chA].group, group ) || EQ_16( speaker_node_data[chA].group, SPEAKER_NODE_HORIZONTAL ) )
2030 : {
2031 84942 : FOR( chB = chA + 1; chB < num_speaker_nodes; chB++ )
2032 : {
2033 : /* Second loudspeaker at the connection is at the half sphere, but so that first and second are not both horizontal. */
2034 71736 : test();
2035 71736 : test();
2036 71736 : IF( ( EQ_16( speaker_node_data[chB].group, group ) ) || ( EQ_16( speaker_node_data[chB].group, SPEAKER_NODE_HORIZONTAL ) && EQ_16( speaker_node_data[chA].group, group ) ) )
2037 : {
2038 24765 : Word16 ConnectionClass = determine_connection_class_fx( speaker_node_data, num_speaker_nodes, group, chA, chB );
2039 : Word32 unit_vec_dotp, unit_vec_dotp_sq, one_minus_unit_vec_dotp_sq, one_minus_unit_vec_dotp_sq_root;
2040 : Word16 acos_val;
2041 : /* Connection is considered only if there is no speaker node behind it */
2042 24765 : IF( NE_16( ConnectionClass, CONNECTION_WITH_SPEAKER_NODE_BEHIND ) )
2043 : {
2044 : /* Store connection information */
2045 22215 : c_options[index].chA = chA;
2046 22215 : move16();
2047 22215 : c_options[index].chB = chB;
2048 22215 : move16();
2049 :
2050 22215 : unit_vec_dotp = dotp_fixed( speaker_node_data[chA].unit_vec_fx, speaker_node_data[chB].unit_vec_fx, 3 ); // Q29
2051 22215 : unit_vec_dotp_sq = Mpy_32_32( unit_vec_dotp, unit_vec_dotp ); // Q27
2052 22215 : one_minus_unit_vec_dotp_sq = L_sub( ONE_IN_Q27, unit_vec_dotp_sq );
2053 22215 : Word16 exp_uv = Q31 - Q27;
2054 22215 : move16();
2055 22215 : one_minus_unit_vec_dotp_sq_root = Sqrt32( one_minus_unit_vec_dotp_sq, &exp_uv );
2056 22215 : acos_val = BASOP_util_atan2( one_minus_unit_vec_dotp_sq_root, unit_vec_dotp, sub( exp_uv, 2 ) ); // Q13
2057 22215 : c_options[index].arc_fx = L_deposit_h( acos_val ); // Q29
2058 22215 : move32();
2059 22215 : c_options[index].arc_weighted_fx = c_options[index].arc_fx; // Q29
2060 22215 : move32();
2061 :
2062 : /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes.
2063 : A triplet between these nodes is not desired if there is a top node,
2064 : a penalty is implemented to take care of this. */
2065 22215 : IF( EQ_16( ConnectionClass, ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION ) )
2066 : {
2067 0 : c_options[index].arc_weighted_fx = L_shl( c_options[index].arc_weighted_fx, 1 );
2068 0 : move32();
2069 : }
2070 :
2071 : /* If the connection passes a pre-determined plane of speaker nodes, then add further penalty */
2072 :
2073 22215 : IF( check_plane_crossing_fx( speaker_node_data[chA].ele_deg_fx, speaker_node_data[chB].ele_deg_fx, num_non_crossing_planes, non_crossing_plane_elevation_deg_fx ) )
2074 : {
2075 711 : c_options[index].arc_weighted_fx = L_shl( c_options[index].arc_weighted_fx, 1 );
2076 711 : move32();
2077 : }
2078 22215 : index = add( index, 1 );
2079 : }
2080 : }
2081 : }
2082 : }
2083 : }
2084 : /* Number of found connection options at the half sphere */
2085 1670 : *num_connection_options = index;
2086 1670 : move16();
2087 : /* Init memory for reordered connection options and order by arc_weighted,
2088 : * which informs of the preference order of the connections in case they cross */
2089 1670 : IF( ( c_options_reorder = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * ( *num_connection_options ) ) ) == NULL )
2090 : {
2091 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
2092 : }
2093 :
2094 23885 : FOR( c = 0; c < *num_connection_options; c++ )
2095 : {
2096 : Word16 min_arc_index;
2097 22215 : min_arc_index = -1;
2098 22215 : move16();
2099 22215 : Word32 min_arc_weighted_fx_new = MAX_32;
2100 22215 : move16();
2101 22215 : Word16 exp_min_arc_weighted_fx = 31;
2102 22215 : move16();
2103 22215 : Word16 res = 0;
2104 22215 : move16();
2105 539392 : FOR( c_cmp = 0; c_cmp < *num_connection_options; c_cmp++ )
2106 : {
2107 517177 : res = BASOP_Util_Cmp_Mant32Exp( c_options[c_cmp].arc_weighted_fx, 31 - Q29, min_arc_weighted_fx_new, exp_min_arc_weighted_fx );
2108 517177 : test();
2109 517177 : IF( EQ_16( res, -1 ) || res == 0 )
2110 : {
2111 114310 : min_arc_weighted_fx_new = c_options[c_cmp].arc_weighted_fx;
2112 114310 : move32();
2113 114310 : exp_min_arc_weighted_fx = Q31 - Q29;
2114 114310 : move16();
2115 114310 : min_arc_index = c_cmp;
2116 114310 : move16();
2117 : }
2118 : }
2119 22215 : c_options_reorder[c].chA = c_options[min_arc_index].chA;
2120 22215 : move16();
2121 22215 : c_options_reorder[c].chB = c_options[min_arc_index].chB;
2122 22215 : move16();
2123 :
2124 22215 : c_options_reorder[c].arc_fx = c_options[min_arc_index].arc_fx;
2125 22215 : move32();
2126 22215 : c_options_reorder[c].arc_weighted_fx = c_options[min_arc_index].arc_weighted_fx;
2127 22215 : move32();
2128 22215 : c_options[min_arc_index].arc_weighted_fx = MAX_32;
2129 22215 : move32();
2130 : }
2131 :
2132 : /* Set reordered connections as output and free temporary data */
2133 1670 : *connection_options_pr = c_options_reorder;
2134 1670 : free( c_options );
2135 :
2136 1670 : return IVAS_ERR_OK;
2137 : }
2138 :
2139 : /*-------------------------------------------------------------------------*
2140 : * formulate_half_sphere_connections()
2141 : *
2142 : * Formulate half-sphere connections
2143 : *-------------------------------------------------------------------------*/
2144 1670 : static ivas_error formulate_half_sphere_connections_fx(
2145 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
2146 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
2147 : const enum SpeakerNodeGroup group, /* i : speaker node group */
2148 : Word16 connections[][2], /* o : vector of connections */
2149 : Word16 *connection_write_index,
2150 : const Word16 max_num_connections, /* i : max number of connections */
2151 : const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */
2152 : const Word32 *non_crossing_plane_elevation_deg_fx /* i : vector of non-crossing plane elevations Q14*/
2153 : )
2154 : {
2155 : /* Variable initializations */
2156 : Word16 c, chA, chB, cmp_chA, cmp_chB, k, c_opt;
2157 : Word16 new_connection_is_valid;
2158 : Word16 within_first_arc;
2159 : Word32 new_cross_fx[3];
2160 : Word32 planeCrossingVec_fx[3];
2161 : Word16 Q_planeCrossingVec;
2162 : Word32 new_arc_fx; /* Q29 */
2163 : Word32 connection_arc_fx[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3]; /* Q29 */
2164 : Word32 connection_cross_fx[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3][3]; /* Q29 */
2165 : Word32 tmpFloat_fx;
2166 : Word32 cmp_arc_fx; /* Q29 */
2167 : Word32 normVal_fx;
2168 : Word16 angleCmp_fx;
2169 : ConnectionOption *connection_options;
2170 : Word16 num_connection_options;
2171 : Word16 half_sphere_first_connection;
2172 : ivas_error error;
2173 :
2174 1670 : half_sphere_first_connection = *connection_write_index;
2175 1670 : move16();
2176 :
2177 : /* Obtain all connection options (i.e., channel pairs) at the half sphere. The function orders them
2178 : * in terms of which connection to keep if two connections would cross each other. */
2179 1670 : IF( NE_32( ( error = get_half_sphere_connection_options_fx(
2180 : speaker_node_data,
2181 : group,
2182 : num_speaker_nodes,
2183 : num_non_crossing_planes,
2184 : non_crossing_plane_elevation_deg_fx,
2185 : &connection_options,
2186 : &num_connection_options ) ),
2187 : IVAS_ERR_OK ) )
2188 : {
2189 0 : return error;
2190 : }
2191 :
2192 1670 : set32_fx( connection_arc_fx, 0, max_num_connections );
2193 41960 : FOR( c = 0; c < max_num_connections; c++ )
2194 : {
2195 40290 : set32_fx( connection_cross_fx[c], 0, 3 );
2196 : }
2197 :
2198 : /* The following loop goes through all reasonable chA - chB pairs for the half-sphere */
2199 1670 : c_opt = 0;
2200 1670 : move16();
2201 20015 : WHILE( c_opt < num_connection_options && *connection_write_index < max_num_connections )
2202 : {
2203 18345 : test();
2204 18345 : chA = connection_options[c_opt].chA;
2205 18345 : move16();
2206 18345 : chB = connection_options[c_opt].chB;
2207 18345 : move16();
2208 18345 : new_arc_fx = connection_options[c_opt].arc_fx;
2209 18345 : move32();
2210 :
2211 : /* Cross-product is needed for later stages */
2212 18345 : vbap_crossp_fx( speaker_node_data[chA].unit_vec_fx, speaker_node_data[chB].unit_vec_fx, new_cross_fx );
2213 :
2214 : /* Determine if new connection between chA and chB is valid */
2215 18345 : new_connection_is_valid = 1;
2216 18345 : move16();
2217 18345 : c = half_sphere_first_connection;
2218 18345 : move16();
2219 : 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;
2220 18345 : 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;
2221 :
2222 18345 : test();
2223 122776 : WHILE( ( LT_16( c, *connection_write_index ) ) && new_connection_is_valid )
2224 : {
2225 104431 : cmp_chA = connections[c][0];
2226 104431 : move16();
2227 104431 : cmp_chB = connections[c][1];
2228 104431 : move16();
2229 : /* The connections are compared only if they don't involve same speaker nodes */
2230 104431 : test();
2231 104431 : test();
2232 104431 : test();
2233 104431 : IF( ( NE_16( cmp_chA, chA ) ) && ( NE_16( cmp_chA, chB ) ) && ( NE_16( cmp_chB, chA ) ) && ( NE_16( cmp_chB, chB ) ) )
2234 : {
2235 : /* The following lines determine if the connection chA-chB crosses with the connection cmp_chA-cmp_chB.*/
2236 : /* The connections, i.e., node-pairs determine a plane. The crossing can be determined by
2237 : * studying the intersection of these planes. */
2238 51203 : vbap_crossp_fx( connection_cross_fx[c], new_cross_fx, planeCrossingVec_fx );
2239 :
2240 51203 : tmpFloat_fx = 0;
2241 51203 : move16();
2242 :
2243 51203 : cmp_arc_fx = connection_arc_fx[c];
2244 51203 : move16();
2245 204812 : FOR( k = 0; k < 3; k++ )
2246 : {
2247 153609 : tmpFloat_fx = L_add( tmpFloat_fx, Mpy_32_32( planeCrossingVec_fx[k], planeCrossingVec_fx[k] ) );
2248 : }
2249 :
2250 51203 : Word16 tmp_exp = sub( 31, Q23 );
2251 51203 : if ( tmpFloat_fx == 0 )
2252 : {
2253 332 : tmpFloat_fx = 1;
2254 332 : move16();
2255 : }
2256 51203 : normVal_fx = ISqrt32( tmpFloat_fx, &tmp_exp );
2257 :
2258 204812 : FOR( k = 0; k < 3; k++ )
2259 : {
2260 153609 : planeCrossingVec_fx[k] = Mpy_32_32( planeCrossingVec_fx[k], normVal_fx ); // Q27 + Q31 - tmp_exp - Q31
2261 153609 : move32();
2262 : }
2263 : /*update Q for planeCrossingVec */
2264 51203 : Q_planeCrossingVec = sub( sub( Q27 + Q31, tmp_exp ), Q31 );
2265 : /* If the plane intersection is between both connections, then the two connections cross. */
2266 : /* Study first if the crossing is between arc chA-chB */
2267 :
2268 51203 : var1 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[chA].unit_vec_fx, 3 ); // Q24 = 26 - tmp_exp
2269 51203 : var2 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[chB].unit_vec_fx, 3 ); // Q24 = 26 - tmp_exp
2270 :
2271 51203 : var1_sq = Mpy_32_32( var1, var1 ); //(2 * (Q_planeCrossingVec - Q1) ) - Q31
2272 51203 : var2_sq = Mpy_32_32( var2, var2 );
2273 51203 : exp_var1_sq = shl( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ), 1 );
2274 51203 : exp_var2_sq = shl( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ), 1 );
2275 51203 : IF( GT_32( L_abs( var1_sq ), L_shr( ONE_IN_Q31, exp_var1_sq ) ) )
2276 : {
2277 2 : var1_sq = ONE_IN_Q31;
2278 2 : move16();
2279 2 : exp_var1_sq = 0;
2280 2 : move16();
2281 : }
2282 51203 : IF( GT_32( L_abs( var2_sq ), L_shr( ONE_IN_Q31, exp_var2_sq ) ) )
2283 : {
2284 0 : var2_sq = ONE_IN_Q31;
2285 0 : move16();
2286 0 : exp_var2_sq = 0;
2287 0 : move16();
2288 : }
2289 51203 : one_minus_var1_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var1_sq ), exp_var1_sq, &final_exp_A );
2290 51203 : var1_sqrt = Sqrt32( one_minus_var1_sq, &final_exp_A );
2291 51203 : var1_cos = BASOP_util_atan2( var1_sqrt, var1, sub( final_exp_A, ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ) ); // Q13
2292 51203 : angleCmp_fx = var1_cos;
2293 51203 : move16();
2294 :
2295 51203 : one_minus_var2_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var2_sq ), exp_var2_sq, &final_exp_B );
2296 51203 : var2_sqrt = Sqrt32( one_minus_var2_sq, &final_exp_B );
2297 51203 : var2_cos = BASOP_util_atan2( var2_sqrt, var2, sub( final_exp_B, ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ) ); // Q13
2298 :
2299 51203 : final_exp = BASOP_Util_Add_MantExp( angleCmp_fx, Q15 - Q13, var2_cos, Q15 - Q13, &angleCmp_fx );
2300 :
2301 51203 : sub_exp = 0, sub_exp_2 = 0, sub_final_exp = 0;
2302 51203 : var_a = BASOP_Util_Add_Mant32Exp( new_arc_fx, Q31 - Q29, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp );
2303 51203 : comp1 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_a ), sub_exp, 21474836 /* 0.01f in Q31 */, 0 );
2304 51203 : var_b = BASOP_Util_Add_Mant32Exp( 25735, Q31 - Q12, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp_2 );
2305 51203 : var_c = BASOP_Util_Add_Mant32Exp( new_arc_fx, Q31 - Q29, L_negate( var_b ), sub_exp_2, &sub_final_exp );
2306 51203 : comp2 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_c ), sub_final_exp, 21474836 /* 0.01f in Q31 */, 0 );
2307 :
2308 51203 : within_first_arc = 0;
2309 51203 : move16();
2310 51203 : IF( EQ_16( comp1, -1 ) )
2311 : {
2312 11824 : within_first_arc = 1;
2313 11824 : move16();
2314 : }
2315 39379 : ELSE IF( EQ_16( comp2, -1 ) )
2316 : {
2317 12755 : within_first_arc = 1;
2318 12755 : move16();
2319 : /* In this case, the plane crossing vector is inverted. The inverse is another
2320 : * plane-crossing vector, and detected to be between chA-chB connection.*/
2321 51020 : FOR( k = 0; k < 3; k++ )
2322 : {
2323 38265 : planeCrossingVec_fx[k] = Mpy_32_32( planeCrossingVec_fx[k], -134217728 /*-1.0f in Q27*/ ); // 27 + 27 - tmp_exp - 31 = 23 - tmp_exp
2324 38265 : move32();
2325 : }
2326 : /*update Q for planeCrossingVec */
2327 12755 : Q_planeCrossingVec = sub( sub( Q27 + Q27, tmp_exp ), Q31 );
2328 : }
2329 :
2330 : /* Study if the crossing is also between arc cmp_chA-cmp_chB */
2331 51203 : IF( within_first_arc > 0 )
2332 : {
2333 24579 : var1 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[cmp_chA].unit_vec_fx, 3 );
2334 24579 : move32();
2335 24579 : var2 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[cmp_chB].unit_vec_fx, 3 );
2336 24579 : move32();
2337 :
2338 : // final_exp_A, final_exp_B, exp_var1_sq, exp_var2_sq;
2339 24579 : var1_sq = Mpy_32_32( var1, var1 ); //(2 * (Q_planeCrossingVec - Q1) ) - Q31
2340 24579 : var2_sq = Mpy_32_32( var2, var2 );
2341 24579 : exp_var1_sq = shl( ( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ) ), 1 );
2342 24579 : exp_var2_sq = shl( ( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ) ), 1 );
2343 24579 : IF( GT_32( L_abs( var1_sq ), L_shr( ONE_IN_Q31, exp_var1_sq ) ) )
2344 : {
2345 2 : var1_sq = ONE_IN_Q31;
2346 2 : move16();
2347 2 : exp_var1_sq = 0;
2348 2 : move16();
2349 : }
2350 24579 : IF( GT_32( L_abs( var2_sq ), L_shr( ONE_IN_Q31, exp_var2_sq ) ) )
2351 : {
2352 0 : var2_sq = ONE_IN_Q31;
2353 0 : move16();
2354 0 : exp_var2_sq = 0;
2355 0 : move16();
2356 : }
2357 24579 : one_minus_var1_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var1_sq ), exp_var1_sq, &final_exp_A );
2358 24579 : var1_sqrt = Sqrt32( one_minus_var1_sq, &final_exp_A );
2359 24579 : var1_cos = BASOP_util_atan2( var1_sqrt, var1, sub( final_exp_A, sub( Q31, add( Q_planeCrossingVec, Q30 - Q31 ) ) ) ); // Q13
2360 24579 : angleCmp_fx = var1_cos;
2361 24579 : move16();
2362 :
2363 24579 : one_minus_var2_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var2_sq ), exp_var2_sq, &final_exp_B );
2364 24579 : var2_sqrt = Sqrt32( one_minus_var2_sq, &final_exp_B );
2365 24579 : var2_cos = BASOP_util_atan2( var2_sqrt, var2, sub( final_exp_B, sub( Q31, add( Q_planeCrossingVec, Q30 - Q31 ) ) ) ); // Q13
2366 :
2367 24579 : final_exp = BASOP_Util_Add_MantExp( angleCmp_fx, Q15 - Q13, var2_cos, Q15 - Q13, &angleCmp_fx );
2368 :
2369 24579 : sub_exp = 0, sub_exp_2 = 0, sub_final_exp = 0;
2370 24579 : move16();
2371 24579 : move16();
2372 24579 : move16();
2373 24579 : var_a = BASOP_Util_Add_Mant32Exp( cmp_arc_fx, Q31 - Q29, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp );
2374 24579 : comp1 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_a ), sub_exp, 21474836 /* 0.01f in Q31 */, 0 );
2375 :
2376 24579 : if ( EQ_16( comp1, -1 ) )
2377 : {
2378 : /* A crossing is detected. The new connection is not valid, because
2379 : * the connections were ordered in order of preference (arc_weighted) */
2380 3021 : new_connection_is_valid = 0;
2381 3021 : move16();
2382 : }
2383 : }
2384 : }
2385 104431 : c = add( c, 1 );
2386 : }
2387 :
2388 : /* Store the new connection which has been confirmed valid */
2389 18345 : IF( new_connection_is_valid > 0 )
2390 : {
2391 15324 : connections[*connection_write_index][0] = chA;
2392 15324 : move16();
2393 15324 : connections[*connection_write_index][1] = chB;
2394 15324 : move16();
2395 15324 : connection_arc_fx[*connection_write_index] = new_arc_fx; /* Q29 */
2396 15324 : move16();
2397 15324 : connection_cross_fx[*connection_write_index][0] = new_cross_fx[0]; /* Q29 */
2398 15324 : move16();
2399 15324 : connection_cross_fx[*connection_write_index][1] = new_cross_fx[1]; /* Q29 */
2400 15324 : move16();
2401 15324 : connection_cross_fx[*connection_write_index][2] = new_cross_fx[2]; /* Q29 */
2402 15324 : move16();
2403 15324 : *connection_write_index = add( *connection_write_index, 1 );
2404 : }
2405 18345 : c_opt = add( c_opt, 1 );
2406 : }
2407 :
2408 1670 : free( connection_options );
2409 :
2410 1670 : return IVAS_ERR_OK;
2411 : }
2412 :
2413 : /*-------------------------------------------------------------------------*
2414 : * determine_non_crossing_planes()
2415 : *
2416 : * Determine non-crossing planes
2417 : *-------------------------------------------------------------------------*/
2418 :
2419 : /*! r: number of non-crossing planes */
2420 :
2421 835 : static Word16 determine_non_crossing_planes_fx(
2422 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
2423 : const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */
2424 : Word32 *non_crossing_plane_elevation_deg_fx /* o : vector of non-crossing plane elevations Q14 */
2425 : )
2426 : {
2427 : Word32 next_ele_check_fx; /* Q14 */
2428 : Word32 ele_check_fx; /* Q14 */
2429 : Word32 max_gap_fx; /* Q14 */
2430 : Word32 gap_to_next_ls_fx; /* Q14 */
2431 :
2432 : Word16 ch, ch_cmp;
2433 : Word16 num_planes;
2434 :
2435 835 : ele_check_fx = -16382362; /*Q14*/
2436 835 : move32();
2437 835 : num_planes = 0;
2438 835 : move16();
2439 :
2440 : /* For each plane, check if a non-crossing plane should be determined */
2441 3077 : WHILE( LT_32( ele_check_fx, 1474560 ) /*90.0f Q14*/ )
2442 : {
2443 2242 : next_ele_check_fx = 16382362; /*Q14*/
2444 2242 : move32();
2445 :
2446 : Word32 tmp1, tmp2;
2447 :
2448 : /* Find next node elevation that is not in horizontal plane */
2449 25246 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
2450 : {
2451 23004 : tmp1 = L_add( ele_check_fx, 16 /*VBAP_EPSILON in Q14*/ );
2452 23004 : tmp2 = L_sub( next_ele_check_fx, 16 /*VBAP_EPSILON in Q14*/ );
2453 23004 : test();
2454 23004 : test();
2455 23004 : IF( NE_32( 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 ) )
2456 : {
2457 2815 : next_ele_check_fx = L_shr( node_data[ch].ele_deg_fx, 8 ); // shift due to comparision with 90.0f
2458 : }
2459 : }
2460 2242 : ele_check_fx = next_ele_check_fx; // Q14
2461 2242 : move32();
2462 :
2463 2242 : IF( GT_32( ele_check_fx, 1474560 ) /*90.0f in Q14*/ )
2464 : {
2465 : /* When no next node elevation found, break loop */
2466 0 : BREAK;
2467 : }
2468 :
2469 2242 : max_gap_fx = -163838368; // Q14
2470 2242 : move32();
2471 25246 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
2472 : {
2473 : /* Find gap to the next speaker node at the same plane */
2474 23004 : IF( LT_32( L_abs( L_sub( L_shr( node_data[ch].ele_deg_fx, 8 ), ele_check_fx ) ), 16 /*0.001f in Q14*/ ) )
2475 : {
2476 3564 : gap_to_next_ls_fx = 1638398336; // Q14
2477 3564 : move32();
2478 41724 : FOR( ch_cmp = 0; ch_cmp < num_speaker_nodes; ch_cmp++ )
2479 : {
2480 38160 : test();
2481 38160 : 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*/ ) )
2482 : {
2483 4894 : Word32 gap_fx = L_sub( node_data[ch_cmp].azi_deg_fx, node_data[ch].azi_deg_fx );
2484 7341 : WHILE( gap_fx < 0 )
2485 : {
2486 2447 : gap_fx = L_add( gap_fx, 1509949440 /*360.0f in Q22*/ );
2487 : }
2488 4894 : IF( LT_32( L_shr( gap_fx, 8 ), gap_to_next_ls_fx ) )
2489 : {
2490 3394 : gap_to_next_ls_fx = L_shr( gap_fx, 8 );
2491 : }
2492 : }
2493 : }
2494 : /* Find maximum gap on that plane */
2495 3564 : IF( GT_32( gap_to_next_ls_fx, max_gap_fx ) )
2496 : {
2497 2453 : max_gap_fx = gap_to_next_ls_fx;
2498 2453 : move32();
2499 : }
2500 : }
2501 : }
2502 :
2503 : /* If maximum gap is small enough, then a non-crossing plane is detected */
2504 2242 : test();
2505 2242 : IF( LT_32( max_gap_fx, 2293776 /*Q14*/ ) && max_gap_fx > 0 )
2506 : {
2507 375 : non_crossing_plane_elevation_deg_fx[num_planes] = ele_check_fx; /* Q14 */
2508 375 : move32();
2509 375 : num_planes = add( num_planes, 1 );
2510 375 : IF( EQ_16( num_planes, VBAP_MAX_PLANES ) )
2511 : {
2512 : /* Memory init limit. Does not happen with any real speaker node configuration.
2513 : Triangulation succeeds even if number of non_crossing_planes are limited. */
2514 0 : BREAK;
2515 : }
2516 : }
2517 : }
2518 :
2519 835 : return num_planes;
2520 : }
2521 :
2522 : /*-------------------------------------------------------------------------*
2523 : * reorder_triplets()
2524 : *
2525 : * Reorder virtual surface triplets into provided target order.
2526 : *-------------------------------------------------------------------------*/
2527 :
2528 1670 : static void reorder_triplets_fx(
2529 : VBAP_VS_TRIPLET *triplets, /* i/o: VS triplets to be reordered */
2530 : const Word16 *target_order, /* i : Target order for VS triplets */
2531 : const Word16 num_triplets /* i : Number of VS triplets */
2532 : )
2533 : {
2534 : VBAP_VS_TRIPLET tempTriplets[VBAP_MAX_NUM_TRIPLETS];
2535 : Word16 c;
2536 :
2537 : /* First copy to temp array */
2538 15100 : FOR( c = 0; c < num_triplets; c++ )
2539 : {
2540 13430 : tempTriplets[c] = triplets[c];
2541 : }
2542 :
2543 : /* Then move back in sorted order */
2544 15100 : FOR( c = 0; c < num_triplets; c++ )
2545 : {
2546 13430 : triplets[c] = tempTriplets[target_order[c]];
2547 : }
2548 :
2549 1670 : return;
2550 : }
|
