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 896 : 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 896 : push_wmops( "vbap_init" );
184 :
185 896 : set32_fx( speaker_node_azi_deg_internal_fx, 0, VBAP_MAX_NUM_SPEAKER_NODES );
186 896 : 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 896 : test();
193 896 : 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 896 : test();
201 896 : 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 896 : 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 896 : is_success = 1;
214 896 : move16();
215 896 : vbap->bottom_virtual_speaker_node_index = -1;
216 896 : move16();
217 896 : vbap->top_virtual_speaker_node_index = -1;
218 896 : move16();
219 896 : vbap->back_virtual_speaker_node_index = -1;
220 896 : move16();
221 896 : vbap->num_speaker_nodes = num_speaker_nodes;
222 896 : move16();
223 896 : vbap->num_speaker_nodes_internal = num_speaker_nodes;
224 896 : move16();
225 896 : vbap->bottom_virtual_speaker_node_division_gains_fx = NULL;
226 896 : vbap->top_virtual_speaker_node_division_gains_fx = NULL;
227 896 : vbap->back_virtual_speaker_node_division_gains_fx = NULL;
228 896 : vbap->object_mode_bottom_virtual_speaker_node_division_gains_fx = NULL;
229 896 : vbap->object_mode_top_virtual_speaker_node_division_gains_fx = NULL;
230 896 : 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 896 : 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 896 : 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 896 : 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 896 : Copy32( speaker_node_azi_deg_fx, speaker_node_azi_deg_internal_fx, num_speaker_nodes );
241 896 : Copy32( speaker_node_ele_deg_fx, speaker_node_ele_deg_internal_fx, num_speaker_nodes );
242 896 : test();
243 896 : IF( is_success && NE_16( virtual_bottom_type, NO_VIRTUAL_SPEAKER_NODE ) )
244 : {
245 896 : 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 896 : set16_fx( vbap->bottom_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes );
250 896 : is_success &= vbap->bottom_virtual_speaker_node_division_gains_fx != NULL;
251 :
252 896 : 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 896 : speaker_node_azi_deg_internal_fx[vbap->bottom_virtual_speaker_node_index] = 0;
262 896 : speaker_node_ele_deg_internal_fx[vbap->bottom_virtual_speaker_node_index] = -377487360; /* -90.0f in Q22 */
263 896 : move32();
264 896 : move32();
265 : }
266 896 : test();
267 896 : IF( is_success && NE_16( virtual_top_type, NO_VIRTUAL_SPEAKER_NODE ) )
268 : {
269 896 : 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 896 : set16_fx( vbap->top_virtual_speaker_node_division_gains_fx, 0, num_speaker_nodes );
274 896 : is_success &= vbap->top_virtual_speaker_node_division_gains_fx != NULL;
275 :
276 896 : 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 896 : speaker_node_azi_deg_internal_fx[vbap->top_virtual_speaker_node_index] = 0;
286 896 : speaker_node_ele_deg_internal_fx[vbap->top_virtual_speaker_node_index] = 377487360; /* 90.0f in Q22 */
287 896 : move32();
288 896 : move16();
289 : }
290 896 : test();
291 896 : 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 896 : 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 896 : max_num_connections = mult0( ( sub( vbap->num_speaker_nodes_internal, 2 ) ), 3 ); /* Theoretical maximum */
320 896 : 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 896 : IF( is_success )
326 : {
327 : Word16 ch;
328 896 : Word16 speaker_nodes_group1_internal = 0;
329 896 : move16();
330 896 : Word16 speaker_nodes_group2_internal = 0;
331 896 : move16();
332 896 : Word16 speaker_nodes_horiz_internal = 0;
333 896 : move16();
334 896 : UWord8 loop_done = 0;
335 896 : move16();
336 :
337 : /* Count nodes in different groups to reserve correct memory */
338 9986 : FOR( ch = 0; ch < vbap->num_speaker_nodes_internal && !loop_done; ch++ )
339 : {
340 9090 : test();
341 9090 : 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 900 : case SPEAKER_NODE_BOTTOM_HALF:
352 900 : speaker_nodes_group1_internal = add( speaker_nodes_group1_internal, 1 );
353 900 : BREAK;
354 3000 : case SPEAKER_NODE_TOP_HALF:
355 3000 : speaker_nodes_group2_internal = add( speaker_nodes_group2_internal, 1 );
356 3000 : BREAK;
357 5190 : case SPEAKER_NODE_HORIZONTAL:
358 : case SPEAKER_NODE_BACK:
359 5190 : speaker_nodes_group1_internal = add( speaker_nodes_group1_internal, 1 );
360 5190 : speaker_nodes_group2_internal = add( speaker_nodes_group2_internal, 1 );
361 5190 : speaker_nodes_horiz_internal = add( speaker_nodes_horiz_internal, 1 );
362 5190 : BREAK;
363 : }
364 9090 : }
365 :
366 896 : 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 896 : is_success = s_and( is_success, vbap->search_struct[0].triplets != NULL );
371 :
372 896 : IF( speaker_nodes_group2_internal > 0 )
373 : {
374 896 : vbap->num_search_structs = 2;
375 896 : move16();
376 896 : 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 896 : 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 896 : IF( is_success )
391 : {
392 896 : 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 896 : 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 896 : move16();
403 896 : 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 896 : move16();
405 : }
406 : }
407 :
408 : /* Determine how the virtual node gains should be distributed to real nodes, if necessary (checked within function). */
409 896 : IF( is_success )
410 : {
411 896 : 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 896 : 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 896 : 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 896 : 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 896 : pop_wmops();
423 :
424 896 : IF( is_success )
425 : {
426 896 : *hVBAPdata = vbap;
427 : }
428 : ELSE
429 : {
430 0 : vbap_free_data_fx( &vbap );
431 : }
432 :
433 896 : return IVAS_ERR_OK;
434 : }
435 :
436 :
437 : /*-------------------------------------------------------------------------*
438 : * vbap_free_data()
439 : *
440 : * Free VBAP data structure
441 : *-------------------------------------------------------------------------*/
442 :
443 1836 : void vbap_free_data_fx(
444 : VBAP_HANDLE *hVBAPdata /* i/o: VBAP handle to be freed */
445 : )
446 : {
447 1836 : test();
448 1836 : IF( hVBAPdata == NULL || *hVBAPdata == NULL )
449 : {
450 940 : return;
451 : }
452 :
453 896 : IF( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains_fx != NULL )
454 : {
455 896 : free( ( *hVBAPdata )->bottom_virtual_speaker_node_division_gains_fx );
456 : }
457 896 : IF( ( *hVBAPdata )->top_virtual_speaker_node_division_gains_fx != NULL )
458 : {
459 896 : free( ( *hVBAPdata )->top_virtual_speaker_node_division_gains_fx );
460 : }
461 896 : IF( ( *hVBAPdata )->back_virtual_speaker_node_division_gains_fx != NULL )
462 : {
463 0 : free( ( *hVBAPdata )->back_virtual_speaker_node_division_gains_fx );
464 : }
465 896 : 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 896 : 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 896 : 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 896 : IF( ( *hVBAPdata )->search_struct[0].triplets != NULL )
478 : {
479 896 : free( ( *hVBAPdata )->search_struct[0].triplets );
480 : }
481 :
482 896 : test();
483 896 : IF( EQ_16( ( *hVBAPdata )->num_search_structs, 2 ) && ( *hVBAPdata )->search_struct[1].triplets != NULL )
484 : {
485 896 : free( ( *hVBAPdata )->search_struct[1].triplets );
486 : }
487 896 : free( *hVBAPdata );
488 896 : *hVBAPdata = NULL;
489 :
490 896 : return;
491 : }
492 :
493 : /*-------------------------------------------------------------------------*
494 : * vbap_determine_gains()
495 : *
496 : * Obtain panning gains for all speaker nodes based on the given direction
497 : *-------------------------------------------------------------------------*/
498 5121378 : 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 5121378 : push_wmops( "vbap_gains" );
529 5121378 : num_speaker_nodes = hVBAPdata->num_speaker_nodes;
530 5121378 : move16();
531 5121378 : bottom_virtual_speaker_node_index = hVBAPdata->bottom_virtual_speaker_node_index;
532 5121378 : move16();
533 5121378 : top_virtual_speaker_node_index = hVBAPdata->top_virtual_speaker_node_index;
534 5121378 : move16();
535 5121378 : back_virtual_speaker_node_index = hVBAPdata->back_virtual_speaker_node_index;
536 5121378 : move16();
537 :
538 5121378 : IF( use_object_mode )
539 : {
540 32603 : bottom_virtual_speaker_node_division_gains_fx = hVBAPdata->object_mode_bottom_virtual_speaker_node_division_gains_fx;
541 32603 : top_virtual_speaker_node_division_gains_fx = hVBAPdata->object_mode_top_virtual_speaker_node_division_gains_fx;
542 32603 : back_virtual_speaker_node_division_gains_fx = hVBAPdata->object_mode_back_virtual_speaker_node_division_gains_fx;
543 : }
544 : ELSE
545 : {
546 5088775 : bottom_virtual_speaker_node_division_gains_fx = hVBAPdata->bottom_virtual_speaker_node_division_gains_fx;
547 5088775 : top_virtual_speaker_node_division_gains_fx = hVBAPdata->top_virtual_speaker_node_division_gains_fx;
548 5088775 : back_virtual_speaker_node_division_gains_fx = hVBAPdata->back_virtual_speaker_node_division_gains_fx;
549 : }
550 :
551 5121378 : panning_wrap_angles_fx( L_shl( azi_deg, Q22 ), L_shl( ele_deg, Q22 ), &azi_temp_fx, &ele_temp_fx );
552 5121378 : azi_norm = extract_l( L_shr( Mpy_32_32( azi_temp_fx, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
553 5121378 : ele_norm = extract_l( L_shr( Mpy_32_32( ele_temp_fx, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
554 :
555 5121378 : panning_unit_vec_fx[0] = mult( getCosWord16R2( azi_norm ), getCosWord16R2( ele_norm ) ); /* Q15 */
556 5121378 : panning_unit_vec_fx[1] = mult( getSineWord16R2( azi_norm ), getCosWord16R2( ele_norm ) ); /* Q15 */
557 5121378 : panning_unit_vec_fx[2] = getSineWord16R2( ele_norm ); /* Q15 */
558 5121378 : move16();
559 5121378 : move16();
560 5121378 : move16();
561 :
562 :
563 : /* Find the best VS triplet and speaker node gains for the panning direction using the prepared search structures. */
564 5121378 : test();
565 5121378 : IF( EQ_16( hVBAPdata->num_search_structs, 2 ) && ele_deg > 0 )
566 : {
567 2178766 : triplet_index = determine_best_triplet_and_gains_fx( &( hVBAPdata->search_struct[1] ), panning_unit_vec_fx, azi_deg, gain_triplet_fx );
568 2178766 : selected_triplet = &hVBAPdata->search_struct[1].triplets[triplet_index];
569 : }
570 : ELSE
571 : {
572 2942612 : triplet_index = determine_best_triplet_and_gains_fx( &( hVBAPdata->search_struct[0] ), panning_unit_vec_fx, azi_deg, gain_triplet_fx );
573 2942612 : selected_triplet = &hVBAPdata->search_struct[0].triplets[triplet_index];
574 : }
575 :
576 : /* Normalize to unit energy */
577 5121378 : gain_ene_fx = 1; /* Add small value to avoid divide by zero. */
578 5121378 : move32();
579 20485512 : FOR( ch = 0; ch < 3; ch++ )
580 : {
581 15364134 : gain_ene_fx = Madd_32_32( gain_ene_fx, gain_triplet_fx[ch], gain_triplet_fx[ch] ); /* Q(2 * VBAP_VS_TRIPLET.q_inverse_matrix - 31) */
582 : }
583 :
584 5121378 : 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 20485512 : FOR( ch = 0; ch < 3; ch++ )
587 : {
588 15364134 : 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 15364134 : move32();
590 :
591 : /* Sanity check for rounding issues */
592 15364134 : if ( gain_triplet_fx[ch] < 0 )
593 : {
594 383 : gain_triplet_fx[ch] = 0;
595 383 : move32();
596 : }
597 : }
598 :
599 : /* Flush gain target */
600 5121378 : 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 20485512 : FOR( ch = 0; ch < 3; ch++ )
604 : {
605 15364134 : triplet_ch = selected_triplet->speaker_node[ch];
606 :
607 15364134 : IF( EQ_16( triplet_ch, bottom_virtual_speaker_node_index ) )
608 : {
609 21850914 : FOR( ch2 = 0; ch2 < num_speaker_nodes; ch2++ )
610 : {
611 19322941 : 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 19322941 : move32();
613 : }
614 : }
615 12836161 : ELSE IF( EQ_16( triplet_ch, top_virtual_speaker_node_index ) )
616 : {
617 11761705 : FOR( ch2 = 0; ch2 < num_speaker_nodes; ch2++ )
618 : {
619 10319730 : 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 10319730 : move32();
621 : }
622 : }
623 11394186 : 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 11394186 : gains_fx[triplet_ch] = L_add( gains_fx[triplet_ch], L_shl( gain_triplet_fx[ch], Q13 ) ); /* Q16 + Q13 = Q29 */
634 11394186 : move32();
635 : }
636 : }
637 :
638 5121378 : pop_wmops();
639 :
640 5121378 : return;
641 : }
642 :
643 : /*-----------------------------------------------------------------------*
644 : * Local functions
645 : *-----------------------------------------------------------------------*/
646 :
647 : /*-------------------------------------------------------------------------*
648 : * vbap_crossp()
649 : *
650 : * 3-by-3 vector cross product
651 : *-------------------------------------------------------------------------*/
652 :
653 120868 : 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 120868 : crossProduct_fx[0] = L_sub( Mpy_32_32( vec1_fx[1], vec2_fx[2] ), Mpy_32_32( vec1_fx[2], vec2_fx[1] ) );
661 120868 : move32();
662 120868 : crossProduct_fx[1] = L_sub( Mpy_32_32( vec1_fx[2], vec2_fx[0] ), Mpy_32_32( vec1_fx[0], vec2_fx[2] ) );
663 120868 : move32();
664 120868 : crossProduct_fx[2] = L_sub( Mpy_32_32( vec1_fx[0], vec2_fx[1] ), Mpy_32_32( vec1_fx[1], vec2_fx[0] ) );
665 120868 : move32();
666 :
667 120868 : 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 17453730 : 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 17453730 : Word32 pointzero_one = Mpy_32_16_1( L_lshl( 1, q_matrix ), -327 /* -0.01 in Q15 */ );
685 17453730 : result[0] = Madd_32_16( Madd_32_16( Mpy_32_16_1( matrix[0][0], src_vector[0] ), matrix[1][0], src_vector[1] ), matrix[2][0], src_vector[2] ); /* Q(q_matrix) */
686 17453730 : move32();
687 :
688 17453730 : IF( LT_32( result[0], pointzero_one ) )
689 : {
690 1794783 : return 0;
691 : }
692 :
693 15658947 : result[1] = Madd_32_16( Madd_32_16( Mpy_32_16_1( matrix[0][1], src_vector[0] ), matrix[1][1], src_vector[1] ), matrix[2][1], src_vector[2] ); /* Q(q_matrix) */
694 15658947 : move32();
695 :
696 15658947 : IF( LT_32( result[1], pointzero_one ) )
697 : {
698 6285442 : return 0;
699 : }
700 :
701 9373505 : result[2] = Madd_32_16( Madd_32_16( Mpy_32_16_1( matrix[0][2], src_vector[0] ), matrix[1][2], src_vector[1] ), matrix[2][2], src_vector[2] ); /* Q(q_matrix) */
702 9373505 : move32();
703 :
704 9373505 : IF( LT_32( result[2], pointzero_one ) )
705 : {
706 4194790 : return 0;
707 : }
708 5178715 : return 1;
709 : }
710 :
711 110824 : static UWord8 vector_matrix_multiply_3x3_32_fx(
712 : const Word32 *src_vector, /* i : input vector Q30 */
713 : Word32 matrix[3][3], /* i : input matrix Q(q_matrix) */
714 : Word32 *result, /* o : output vector Q(q_matrix - 1) */
715 : Word16 q_matrix )
716 : {
717 110824 : result[0] = Mpy_32_32( matrix[0][0], src_vector[0] ); /* Q(q_matrix - 1) */
718 110824 : result[0] = L_add( result[0], Mpy_32_32( matrix[1][0], src_vector[1] ) ); /* Q(q_matrix - 1) */
719 110824 : result[0] = L_add( result[0], Mpy_32_32( matrix[2][0], src_vector[2] ) ); /* Q(q_matrix - 1) */
720 110824 : move32();
721 110824 : move32();
722 110824 : move32();
723 110824 : IF( LT_32( result[0], Mpy_32_32( L_shl( 1, ( sub( q_matrix, 1 ) ) ), -21474836 /* -0.01 in Q31 */ ) ) )
724 : {
725 35458 : return 0;
726 : }
727 :
728 75366 : result[1] = Mpy_32_32( matrix[0][1], src_vector[0] ); /* Q(q_matrix - 1) */
729 75366 : result[1] = L_add( result[1], Mpy_32_32( matrix[1][1], src_vector[1] ) ); /* Q(q_matrix - 1) */
730 75366 : result[1] = L_add( result[1], Mpy_32_32( matrix[2][1], src_vector[2] ) ); /* Q(q_matrix - 1) */
731 :
732 75366 : move32();
733 75366 : move32();
734 75366 : move32();
735 75366 : IF( LT_32( result[1], Mpy_32_32( L_shl( 1, ( sub( q_matrix, 1 ) ) ), -21474836 /* -0.01 in Q31 */ ) ) )
736 : {
737 49744 : return 0;
738 : }
739 :
740 25622 : result[2] = Mpy_32_32( matrix[0][2], src_vector[0] ); /* Q(q_matrix - 1) */
741 25622 : result[2] = L_add( result[2], Mpy_32_32( matrix[1][2], src_vector[1] ) ); /* Q(q_matrix - 1) */
742 25622 : result[2] = L_add( result[2], Mpy_32_32( matrix[2][2], src_vector[2] ) ); /* Q(q_matrix - 1) */
743 :
744 25622 : move32();
745 25622 : move32();
746 25622 : move32();
747 25622 : IF( LT_32( result[2], Mpy_32_32( L_shl( 1, ( sub( q_matrix, 1 ) ) ), -21474836 /* -0.01 in Q31 */ ) ) )
748 : {
749 25622 : return 0;
750 : }
751 :
752 0 : return 1;
753 : }
754 :
755 : /*----------------------------------------------------------------------------------------------*
756 : * determine_best_triplet_and_gains()
757 : *
758 : * Determine the best speaker node triplet and associated gains for panning to defined direction
759 : *----------------------------------------------------------------------------------------------*/
760 :
761 : /*! r: triplet id */
762 5121378 : static Word16 determine_best_triplet_and_gains_fx(
763 : VBAP_SEARCH_STRUCT *search_struct, /* i : VBAP search struct */
764 : const Word16 panning_unit_vec_fx[3], /* i : panning unit vector Q15 */
765 : const Word16 azi_deg, /* i : panning azimuth */
766 : Word32 gains_fx[3] /* o : panning gains Q(VBAP_VS_TRIPLET.q_inverse_matrix) */
767 : )
768 : {
769 : Word16 i, tr, k;
770 : UWord8 triplet_ok;
771 : Word16 best_triplet;
772 : Word32 best_min_gain_fx;
773 : Word32 min_gain_this_fx;
774 : Word32 unnormalized_gains_fx[3];
775 : Word16 sector;
776 : Word16 first_triplet;
777 : Word16 jump;
778 : Word16 num_triplets;
779 :
780 5121378 : num_triplets = search_struct->num_triplets;
781 5121378 : move16();
782 5121378 : best_min_gain_fx = -2147483647;
783 5121378 : move32();
784 5121378 : best_triplet = 0;
785 5121378 : move16();
786 5121378 : set32_fx( gains_fx, 0, 3 );
787 :
788 : /* Determine the correct search sector for that target panning direction using an optimized algorithm for
789 : * the chosen four sectors. */
790 5121378 : IF( GT_16( abs_s( azi_deg ), 90 ) )
791 : {
792 2351162 : sector = 1;
793 2351162 : move16();
794 2351162 : if ( azi_deg < 0 )
795 : {
796 369437 : sector = 2;
797 369437 : move16();
798 : }
799 : }
800 : ELSE
801 : {
802 2770216 : sector = 0;
803 2770216 : move16();
804 2770216 : if ( azi_deg < 0 )
805 : {
806 1278249 : sector = 3;
807 1278249 : move16();
808 : }
809 : }
810 5121378 : first_triplet = search_struct->initial_search_indices[sector];
811 5121378 : move16();
812 :
813 5121378 : tr = first_triplet;
814 5121378 : move16();
815 5121378 : jump = 1;
816 5121378 : move16();
817 17454113 : FOR( i = 0; i < num_triplets; i++ )
818 : {
819 17453730 : 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 );
820 17453730 : IF( triplet_ok )
821 : {
822 5178715 : min_gain_this_fx = L_min( ( L_min( unnormalized_gains_fx[0], unnormalized_gains_fx[1] ) ), unnormalized_gains_fx[2] );
823 :
824 5178715 : IF( GT_32( min_gain_this_fx, best_min_gain_fx ) )
825 : {
826 5177022 : best_min_gain_fx = min_gain_this_fx;
827 5177022 : move32();
828 5177022 : best_triplet = tr;
829 5177022 : move16();
830 20708088 : FOR( k = 0; k < 3; k++ )
831 : {
832 15531066 : gains_fx[k] = unnormalized_gains_fx[k]; /* Q(VBAP_VS_TRIPLET.q_inverse_matrix) */
833 15531066 : move32();
834 : }
835 5177022 : IF( best_min_gain_fx >= 0 )
836 : {
837 5120995 : return best_triplet;
838 : }
839 : }
840 : }
841 12332735 : tr = add( first_triplet, jump );
842 12332735 : IF( tr < 0 )
843 : {
844 89614 : tr = add( tr, num_triplets );
845 : }
846 12243121 : ELSE IF( GE_16( tr, num_triplets ) )
847 : {
848 2851182 : tr = sub( tr, num_triplets );
849 : }
850 :
851 12332735 : jump = negate( jump );
852 12332735 : IF( jump > 0 )
853 : {
854 5180226 : jump = add( jump, 1 );
855 : }
856 : }
857 :
858 383 : return best_triplet;
859 : }
860 :
861 : /*-------------------------------------------------------------------------*
862 : * determine_virtual_speaker_node_division_gains()
863 : *
864 : * Determines how the virtual node gains are distributed to real nodes
865 : *-------------------------------------------------------------------------*/
866 :
867 3705 : static void determine_virtual_speaker_node_division_gains_fx(
868 : const Word16 virtual_speaker_node_index, /* i : virtual speaker node index */
869 : Word16 *virtual_node_division_gains_fx, /* o : virtual speaker node division gains Q16 */
870 : Word16 connections[][2], /* i : vector of all connections */
871 : const enum VirtualSpeakerNodeType type, /* i : virtual speaker node typel */
872 : const Word16 max_num_connections, /* i : max number of connections */
873 : const Word16 num_speaker_nodes, /* i : max number of speaker nodes */
874 : const Word16 use_object_mode /* i : use VBAP in object panning mode vs. spatial panning mode */
875 : )
876 : {
877 : /* When node type is VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY, the gains of the virtual node
878 : are distributed to all neighboring real speaker nodes. An amplitude-division
879 : instead of energy division is utilized just in case to avoid excessive emphasis
880 : on the coherent distributed sound. */
881 : Word16 c, ch, i;
882 : Word16 sum_val_fx;
883 : Word16 *exp_virtual_node_division_gains;
884 3705 : exp_virtual_node_division_gains = (Word16 *) malloc( num_speaker_nodes * sizeof( Word16 ) );
885 :
886 3705 : set16_fx( exp_virtual_node_division_gains, 0, num_speaker_nodes );
887 :
888 3705 : IF( EQ_16( type, VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY ) )
889 : {
890 34150 : FOR( c = 0; c < max_num_connections; c++ )
891 : {
892 32844 : IF( NE_16( connections[c][0], VBAP_NOT_VALID_CONNECTION ) )
893 : {
894 32844 : Word16 connection_node = -1;
895 32844 : move16();
896 32844 : IF( EQ_16( connections[c][0], virtual_speaker_node_index ) )
897 : {
898 0 : connection_node = connections[c][1];
899 0 : move16();
900 : }
901 32844 : ELSE IF( EQ_16( connections[c][1], virtual_speaker_node_index ) )
902 : {
903 5895 : connection_node = connections[c][0];
904 5895 : move16();
905 : }
906 :
907 : /* The second condition allows division gains only to actual loudspeakers */
908 32844 : test();
909 32844 : IF( connection_node >= 0 && ( LT_16( connection_node, num_speaker_nodes ) ) )
910 : {
911 5895 : virtual_node_division_gains_fx[connection_node] = ONE_IN_Q14;
912 5895 : move16();
913 5895 : exp_virtual_node_division_gains[connection_node] = 1;
914 5895 : move16();
915 : }
916 : }
917 : }
918 :
919 1306 : sum_val_fx = 0;
920 1306 : move16();
921 1306 : Word16 guard_bits = find_guarded_bits_fx( num_speaker_nodes );
922 12254 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
923 : {
924 10948 : sum_val_fx = add( sum_val_fx, shr( virtual_node_division_gains_fx[ch], guard_bits ) );
925 : }
926 1306 : Word16 final_exp = 0, res_exp;
927 : Word32 tmp_1, tmp_2, tmp_3;
928 1306 : move16();
929 12254 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
930 : {
931 10948 : IF( virtual_node_division_gains_fx[ch] != 0 )
932 : {
933 5895 : 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 );
934 : }
935 : ELSE
936 : {
937 5053 : virtual_node_division_gains_fx[ch] = 0;
938 5053 : move16();
939 : }
940 10948 : exp_virtual_node_division_gains[ch] = final_exp;
941 10948 : move16();
942 10948 : IF( use_object_mode )
943 : {
944 5310 : IF( virtual_node_division_gains_fx[ch] == 0 )
945 : {
946 1927 : tmp_1 = 0;
947 1927 : move16();
948 1927 : virtual_node_division_gains_fx[ch] = 0;
949 1927 : move16();
950 : }
951 : ELSE
952 : {
953 3383 : Word32 tmp32 = L_deposit_h( virtual_node_division_gains_fx[ch] );
954 3383 : tmp_1 = L_add( BASOP_Util_Log2( tmp32 ), L_shl( ( sub( 31, sub( 31, final_exp ) ) ), 25 ) ); // Q25
955 3383 : tmp_2 = Mpy_32_32( 26843546 /* 0.8f in Q25 */, tmp_1 );
956 3383 : tmp_3 = BASOP_util_Pow2( tmp_2, Q31 - Q19, &res_exp );
957 3383 : exp_virtual_node_division_gains[ch] = res_exp;
958 3383 : move16();
959 3383 : virtual_node_division_gains_fx[ch] = extract_h( tmp_3 );
960 3383 : move16();
961 : }
962 : }
963 : }
964 : /*make a common exp*/
965 12254 : FOR( i = 0; i < num_speaker_nodes; i++ )
966 : {
967 10948 : virtual_node_division_gains_fx[i] = shr( virtual_node_division_gains_fx[i], sub( -1, exp_virtual_node_division_gains[i] ) ); /* Q16 */
968 10948 : move16();
969 : }
970 : }
971 :
972 3705 : free( exp_virtual_node_division_gains );
973 :
974 3705 : return;
975 : }
976 :
977 : /*-------------------------------------------------------------------------*
978 : * check_need_of_virtual_speaker_node()
979 : *
980 : * Check if virtual speaker node is required
981 : *-------------------------------------------------------------------------*/
982 :
983 : /*! r: virtual speaker node type */
984 :
985 2688 : static enum VirtualSpeakerNodeType check_need_of_virtual_speaker_node_fx(
986 : VBAP_HANDLE hVBAPdata, /* i/o: VBAP structure */
987 : const Word32 *speaker_node_azi_deg_fx, /* i : vector of speaker node azimuths Q22 */
988 : const Word32 *speaker_node_ele_deg_fx, /* i : vector of speaker node elevations Q22 */
989 : enum SpeakerNodeGroup group /* i : group of speaker nodes where this belongs */
990 : )
991 : {
992 : Word16 ch;
993 2688 : Word32 max_elevation_fx = 0; /* Q22 */
994 : Word16 Flag1, Flag2, Flag3;
995 2688 : move32();
996 :
997 : /* The following considers if SPEAKER_NODE_BACK virtual speaker is needed */
998 2688 : IF( EQ_16( group, SPEAKER_NODE_BACK ) )
999 : {
1000 896 : Word16 virtual_back_needed = 1;
1001 896 : move16();
1002 896 : const Word16 virtual_back_epsilon_fx = -573; /* -0.0175f in Q15 */
1003 896 : move16();
1004 3559 : FOR( ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++ )
1005 : {
1006 3559 : Flag1 = BASOP_Util_Cmp_Mant32Exp( speaker_node_ele_deg_fx[ch], Q31 - Q22, 23040 /*45.0f Q9*/, Q31 - Q9 );
1007 3559 : IF( EQ_16( Flag1, -1 ) )
1008 : {
1009 : Word16 azi_temp;
1010 :
1011 3559 : azi_temp = extract_l( L_shr( Mpy_32_32( speaker_node_azi_deg_fx[ch], ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
1012 3559 : Word16 cos_res = getCosWord16R2( azi_temp ); /* Q15 */
1013 :
1014 3559 : IF( LT_16( cos_res, virtual_back_epsilon_fx ) )
1015 : {
1016 896 : virtual_back_needed = 0;
1017 896 : move16();
1018 896 : BREAK;
1019 : }
1020 : }
1021 : }
1022 :
1023 896 : IF( virtual_back_needed )
1024 : {
1025 0 : hVBAPdata->back_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal;
1026 0 : move16();
1027 0 : hVBAPdata->num_speaker_nodes_internal = add( hVBAPdata->num_speaker_nodes_internal, 1 );
1028 0 : move16();
1029 0 : return VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY;
1030 : }
1031 :
1032 896 : return NO_VIRTUAL_SPEAKER_NODE; /* No virtual back needed */
1033 : }
1034 :
1035 : /* The following considers if TOP or BOTTOM virtual speaker is needed */
1036 16388 : FOR( ch = 0; ch < hVBAPdata->num_speaker_nodes; ch++ )
1037 : {
1038 14596 : IF( EQ_16( group, SPEAKER_NODE_TOP_HALF ) )
1039 : {
1040 7298 : if ( GT_32( speaker_node_ele_deg_fx[ch], max_elevation_fx ) )
1041 : {
1042 627 : max_elevation_fx = speaker_node_ele_deg_fx[ch];
1043 627 : move32();
1044 : }
1045 : }
1046 : ELSE
1047 : {
1048 7298 : IF( GT_32( ( L_negate( speaker_node_ele_deg_fx[ch] ) ), max_elevation_fx ) )
1049 : {
1050 1 : max_elevation_fx = L_negate( speaker_node_ele_deg_fx[ch] );
1051 : }
1052 : }
1053 : }
1054 1792 : Flag2 = BASOP_Util_Cmp_Mant32Exp( max_elevation_fx, Q31 - Q22, 23039 /* 44.9990005 in Q9 */, Q31 - Q9 );
1055 1792 : IF( EQ_16( Flag2, 1 ) )
1056 : {
1057 0 : return NO_VIRTUAL_SPEAKER_NODE;
1058 : }
1059 :
1060 : /* Use virtual node */
1061 1792 : IF( EQ_16( group, SPEAKER_NODE_BOTTOM_HALF ) )
1062 : {
1063 896 : hVBAPdata->bottom_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal;
1064 896 : move16();
1065 : }
1066 : ELSE
1067 : {
1068 896 : hVBAPdata->top_virtual_speaker_node_index = hVBAPdata->num_speaker_nodes_internal;
1069 896 : move16();
1070 : }
1071 :
1072 1792 : hVBAPdata->num_speaker_nodes_internal = add( hVBAPdata->num_speaker_nodes_internal, 1 );
1073 1792 : move16();
1074 1792 : Flag3 = BASOP_Util_Cmp_Mant32Exp( max_elevation_fx, Q31 - Q22, 20478 /* 19.9990005 in Q10 */, Q31 - Q10 );
1075 :
1076 1792 : IF( EQ_16( Flag3, 1 ) )
1077 : {
1078 628 : return VIRTUAL_SPEAKER_NODE_DISTRIBUTE_ENERGY;
1079 : }
1080 :
1081 1164 : return VIRTUAL_SPEAKER_NODE_DISCARD_ENERGY;
1082 : }
1083 :
1084 :
1085 : /*-------------------------------------------------------------------------*
1086 : * init_speaker_node_direction_data()
1087 : *
1088 : * Initialize speaker node data
1089 : *-------------------------------------------------------------------------*/
1090 :
1091 896 : static void init_speaker_node_direction_data_fx(
1092 : VBAP_SPEAKER_NODE *speaker_node_data, /* o : storage for speaker node data */
1093 : const Word32 *speaker_node_azi_deg_fx, /* i : vector of speaker node azimuths Q22 */
1094 : const Word32 *speaker_node_ele_deg_fx, /* i : vector of speaker node elevations Q22 */
1095 : const Word16 num_speaker_nodes /* i : number of speaker nodes */
1096 : )
1097 : {
1098 : Word16 ch;
1099 : Word16 azi_rad_fx;
1100 : Word16 ele_rad_fx;
1101 896 : Word16 num_horiz = 0;
1102 896 : UWord8 in_all_mode = TRUE;
1103 896 : move16();
1104 896 : move16();
1105 :
1106 9986 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1107 : {
1108 9090 : speaker_node_data[ch].azi_deg_fx = speaker_node_azi_deg_fx[ch];
1109 9090 : move32();
1110 :
1111 9090 : azi_rad_fx = extract_l( L_shr( Mpy_32_32( speaker_node_azi_deg_fx[ch], ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
1112 9090 : test();
1113 9090 : 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 ) )
1114 : {
1115 5190 : speaker_node_data[ch].ele_deg_fx = 0;
1116 5190 : move32();
1117 5190 : ele_rad_fx = 0;
1118 5190 : move32();
1119 5190 : speaker_node_data[ch].group = SPEAKER_NODE_HORIZONTAL;
1120 5190 : move16();
1121 5190 : num_horiz = add( num_horiz, 1 );
1122 : }
1123 : ELSE
1124 : {
1125 3900 : speaker_node_data[ch].ele_deg_fx = speaker_node_ele_deg_fx[ch];
1126 3900 : move32();
1127 3900 : ele_rad_fx = extract_l( L_shr( Mpy_32_32( speaker_node_ele_deg_fx[ch], ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
1128 :
1129 3900 : IF( ele_rad_fx < 0 )
1130 : {
1131 900 : speaker_node_data[ch].group = SPEAKER_NODE_BOTTOM_HALF;
1132 900 : move16();
1133 : }
1134 : ELSE
1135 : {
1136 3000 : speaker_node_data[ch].group = SPEAKER_NODE_TOP_HALF;
1137 3000 : move16();
1138 : }
1139 : }
1140 :
1141 9090 : 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 */
1142 9090 : move32();
1143 9090 : 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 */
1144 9090 : move32();
1145 9090 : speaker_node_data[ch].unit_vec_fx[2] = L_shr( L_deposit_h( getSineWord16R2( ele_rad_fx ) ), 1 ); /* Q15 + Q16 - Q1 = Q30 */
1146 9090 : move32();
1147 : }
1148 : /* Check for largest horizontal gap if there are at least 3 horizontal speaker nodes */
1149 896 : IF( GE_16( num_horiz, 3 ) )
1150 : {
1151 : Word16 i;
1152 : UWord16 horiz_azi[VBAP_MAX_NUM_SPEAKER_NODES];
1153 :
1154 : UWord16 largest_gap;
1155 : UWord16 temp;
1156 :
1157 896 : i = 0;
1158 896 : move16();
1159 6086 : FOR( ch = 0; ch < num_speaker_nodes && i < num_horiz; ch++ )
1160 : {
1161 5190 : test();
1162 5190 : IF( EQ_16( speaker_node_data[ch].group, SPEAKER_NODE_HORIZONTAL ) )
1163 : {
1164 : Word16 exp1;
1165 5190 : Word32 Mant2 = BASOP_Util_Add_Mant32Exp( speaker_node_azi_deg_fx[ch], Q31 - Q22, 23040 /* 360.0f in Q6 */, Q31 - Q6, &exp1 );
1166 :
1167 5190 : IF( L_shr( speaker_node_azi_deg_fx[ch], 22 ) < 0 )
1168 : {
1169 2159 : horiz_azi[i] = (UWord16) L_shr( Mant2, sub( 31, exp1 ) ); /* Q0 */
1170 : }
1171 : ELSE
1172 : {
1173 3031 : horiz_azi[i] = (UWord16) L_shr( speaker_node_azi_deg_fx[ch], Q22 ); /* Q0 */
1174 : }
1175 5190 : i = add( i, 1 );
1176 : }
1177 : }
1178 :
1179 : /* Reorder horizontal azi to increasing order */
1180 896 : sort( horiz_azi, num_horiz );
1181 :
1182 : /* Find largest gap. Initialize with the wrap over gap. */
1183 896 : largest_gap = add( sub( horiz_azi[0], horiz_azi[num_horiz - 1] ), 360 );
1184 :
1185 5190 : FOR( ch = 0; ch < num_horiz - 1; ch++ )
1186 : {
1187 4294 : temp = sub( horiz_azi[ch + 1], horiz_azi[ch] );
1188 4294 : if ( GT_16( temp, largest_gap ) )
1189 : {
1190 1771 : largest_gap = temp;
1191 1771 : move16();
1192 : }
1193 : }
1194 :
1195 : /* If largest gap is small enough, we have definitive zero elevation plane.
1196 : * Otherwise, we should assign all speaker nodes to one group. */
1197 896 : if ( LE_16( largest_gap, VBAP_MAX_HORIZONTAL_GAP ) )
1198 : {
1199 896 : in_all_mode = FALSE;
1200 896 : move16();
1201 : }
1202 : }
1203 :
1204 : /* Designate all speaker nodes to same group if there was no definitive zero
1205 : * elevation plane. */
1206 896 : IF( in_all_mode )
1207 : {
1208 0 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1209 : {
1210 0 : speaker_node_data[ch].group = SPEAKER_NODE_ALL;
1211 0 : move16();
1212 : }
1213 : }
1214 :
1215 896 : return;
1216 : }
1217 : /*-------------------------------------------------------------------------*
1218 : * matrix_inverse_3x3()
1219 : *
1220 : * 3-by-3 matrix inverse
1221 : *-------------------------------------------------------------------------*/
1222 :
1223 14596 : static void matrix_inverse_3x3_32_fx(
1224 : const Word32 **input_matrix_fx, /* i : input matrix Q30 */
1225 : Word32 inverse_matrix_fx[3][3], /* o : output matrix Q(31 - exp_inv_mat) */
1226 : Word16 *exp_inv_mat )
1227 : {
1228 : Word16 k;
1229 : Word32 determinant_fx; /* Q28 */
1230 : Word32 cross_vec_fx[3]; /* Q29 */
1231 : Word16 exp_inverse_matrix_fx[3][3];
1232 14596 : vbap_crossp_fx( input_matrix_fx[1], input_matrix_fx[2], cross_vec_fx );
1233 14596 : determinant_fx = dotp_fixed( input_matrix_fx[0], cross_vec_fx, 3 );
1234 14596 : Word16 inv_mat_exp = 0;
1235 14596 : move16();
1236 :
1237 58384 : FOR( k = 0; k < 3; k++ )
1238 : {
1239 43788 : inverse_matrix_fx[k][0] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) );
1240 43788 : move32();
1241 43788 : inv_mat_exp = add( inv_mat_exp, ( ( Q31 - Q29 ) - ( Q31 - Q28 ) ) );
1242 43788 : exp_inverse_matrix_fx[k][0] = inv_mat_exp;
1243 43788 : move16();
1244 : }
1245 :
1246 14596 : vbap_crossp_fx( input_matrix_fx[2], input_matrix_fx[0], cross_vec_fx );
1247 :
1248 58384 : FOR( k = 0; k < 3; k++ )
1249 : {
1250 43788 : inverse_matrix_fx[k][1] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) );
1251 43788 : move32();
1252 43788 : inv_mat_exp = add( inv_mat_exp, ( ( Q31 - Q29 ) - ( Q31 - Q28 ) ) );
1253 43788 : exp_inverse_matrix_fx[k][1] = inv_mat_exp;
1254 43788 : move16();
1255 : }
1256 :
1257 14596 : vbap_crossp_fx( input_matrix_fx[0], input_matrix_fx[1], cross_vec_fx );
1258 :
1259 58384 : FOR( k = 0; k < 3; k++ )
1260 : {
1261 43788 : inverse_matrix_fx[k][2] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) );
1262 43788 : move32();
1263 43788 : inv_mat_exp = add( inv_mat_exp, ( ( Q31 - Q29 ) - ( Q31 - Q28 ) ) );
1264 43788 : exp_inverse_matrix_fx[k][2] = inv_mat_exp;
1265 43788 : move16();
1266 : }
1267 :
1268 : /*make common exponant*/
1269 14596 : Word16 max_exp = 0, i, j;
1270 58384 : FOR( i = 0; i < 3; i++ )
1271 : {
1272 175152 : FOR( j = 0; j < 3; j++ )
1273 : {
1274 131364 : max_exp = s_max( max_exp, exp_inverse_matrix_fx[i][j] );
1275 : }
1276 : }
1277 :
1278 14596 : *exp_inv_mat = add( max_exp, 1 );
1279 14596 : move16();
1280 :
1281 58384 : FOR( i = 0; i < 3; i++ )
1282 : {
1283 175152 : FOR( j = 0; j < 3; j++ )
1284 : {
1285 131364 : test();
1286 131364 : IF( LT_16( exp_inverse_matrix_fx[i][j], -15 ) && inverse_matrix_fx[i][j] != 0 )
1287 : {
1288 0 : inverse_matrix_fx[i][j] = 1;
1289 0 : move32();
1290 0 : exp_inverse_matrix_fx[i][j] = 0;
1291 0 : move16();
1292 : }
1293 : ELSE
1294 : {
1295 131364 : 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) */
1296 131364 : move32();
1297 : }
1298 : }
1299 : }
1300 :
1301 14596 : return;
1302 : }
1303 : /*-------------------------------------------------------------------------*
1304 : * check_and_store_triplet()
1305 : *
1306 : * Check if the given loudspeaker triplet is a valid one and store data when
1307 : * valid triplet is found.
1308 : *-------------------------------------------------------------------------*/
1309 :
1310 14596 : static Word16 check_and_store_triplet_fx(
1311 : const Word16 chA, /* i : first channel index that forms the loudspeaker triplet */
1312 : const Word16 chB, /* i : second channel index that forms the loudspeaker triplet */
1313 : const Word16 chC, /* i : third channel index that forms the loudspeaker triplet */
1314 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1315 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */
1316 : VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */
1317 : Word16 *triplet_index, /* i/o: index for the next free triplet slot */
1318 : Word32 *triplet_azidegs_fx, /* o : center azimuths of the found triplets Q19 */
1319 : Word16 *triplet_order /* o : initial order of triplet indices */
1320 : )
1321 : {
1322 : Word16 ch_check;
1323 : Word16 k;
1324 : Word16 speaker_node_found_inside_triplet;
1325 : UWord8 triplet_ok;
1326 : Word16 exp_inv_mat;
1327 :
1328 : Word32 inverse_matrix_fx[3][3] /* Q(31 - exp_inv_mat) */, unnormalized_gains_fx[3] /* Q(31 - exp_inv_mat - 1) */;
1329 14596 : set32_fx( unnormalized_gains_fx, 0, 3 );
1330 : const Word32 *speaker_node_triplet_unit_vec_matrix_fx[3]; /* Q30 */
1331 :
1332 : /* Triplet found, determine inverse matrix for VBAP formulation */
1333 :
1334 14596 : speaker_node_triplet_unit_vec_matrix_fx[0] = speaker_node_data[chA].unit_vec_fx;
1335 14596 : move32();
1336 14596 : speaker_node_triplet_unit_vec_matrix_fx[1] = speaker_node_data[chB].unit_vec_fx;
1337 14596 : move32();
1338 14596 : speaker_node_triplet_unit_vec_matrix_fx[2] = speaker_node_data[chC].unit_vec_fx;
1339 14596 : move32();
1340 14596 : matrix_inverse_3x3_32_fx( speaker_node_triplet_unit_vec_matrix_fx, inverse_matrix_fx, &exp_inv_mat );
1341 14596 : triplets[*triplet_index].q_inverse_matrix = sub( 31, exp_inv_mat );
1342 14596 : move16();
1343 :
1344 : /* Check through all speaker nodes that none of them are within the triplet.
1345 : * Node within the triplet is identified by that all three panning gains are positive.
1346 : * Epsilon-condition is for some small rounding issues.*/
1347 14596 : speaker_node_found_inside_triplet = 0;
1348 14596 : move16();
1349 169208 : FOR( ch_check = 0; ch_check < num_speaker_nodes; ch_check++ )
1350 : {
1351 154612 : test();
1352 154612 : test();
1353 154612 : IF( ( NE_16( ch_check, chA ) ) && NE_16( ch_check, chB ) && NE_16( ch_check, chC ) )
1354 : {
1355 110824 : 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 ) );
1356 110824 : test();
1357 110824 : test();
1358 110824 : test();
1359 110824 : 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 ) ) ) ) )
1360 : {
1361 0 : speaker_node_found_inside_triplet = 1;
1362 0 : move16();
1363 0 : BREAK;
1364 : }
1365 : }
1366 : }
1367 :
1368 : /* No speaker node inside triplet -> appropriate triplet found, save data. */
1369 14596 : IF( speaker_node_found_inside_triplet == 0 )
1370 : {
1371 14596 : triplets[*triplet_index].speaker_node[0] = (UWord8) chA;
1372 14596 : triplets[*triplet_index].speaker_node[1] = (UWord8) chB;
1373 14596 : triplets[*triplet_index].speaker_node[2] = (UWord8) chC;
1374 14596 : move16();
1375 14596 : move16();
1376 14596 : move16();
1377 58384 : FOR( k = 0; k < 3; k++ )
1378 : {
1379 43788 : Copy32( inverse_matrix_fx[k], triplets[*triplet_index].inverse_matrix_fx[k], 3 );
1380 : }
1381 : /* Get center azimuth for fast search use */
1382 14596 : 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 */
1383 : /*Condition to make tmp_a 0 to adress precision loss seen*/
1384 14596 : if ( EQ_32( tmp_a, -8193 /* -0.0000305 in Q28 */ ) )
1385 : {
1386 0 : tmp_a = 0;
1387 0 : move32();
1388 : }
1389 14596 : 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 */
1390 14596 : Word16 tmp_tan = shr( BASOP_util_atan2( tmp_a, tmp_b, 0 ), Q13 - Q9 ); /* Q9 */
1391 14596 : triplet_azidegs_fx[*triplet_index] = L_mult( tmp_tan, 29335 /*_180_OVER_PI in Q9*/ ); /* Q19 */
1392 14596 : move32();
1393 : /* Store increasing order indices for the later sorting step. */
1394 14596 : triplet_order[*triplet_index] = *triplet_index;
1395 14596 : move16();
1396 :
1397 14596 : *triplet_index = add( *triplet_index, 1 );
1398 14596 : move16();
1399 :
1400 14596 : return 1;
1401 : }
1402 :
1403 : /* Triplet was not good */
1404 0 : return 0;
1405 : }
1406 : /*-------------------------------------------------------------------------*
1407 : * determine_virtual_surface_triplets()
1408 : *
1409 : * Determine virtual surface triples that are used for panning. This
1410 : * function is optimized for the use in cases where speaker nodes are in
1411 : * one group or divided into two separate groups divided by a horizontal
1412 : * layer.
1413 : *-------------------------------------------------------------------------*/
1414 :
1415 : /*! r: number of virtual surface triplets */
1416 :
1417 1792 : static Word16 determine_virtual_surface_triplets_fx(
1418 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1419 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */
1420 : Word16 connections[][2], /* i : vector of all connections */
1421 : const Word16 max_num_connections, /* i : max number of connections */
1422 : VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */
1423 : Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS], /* o : initial search indices for this set of triplets corresponding to the search struct */
1424 : enum SpeakerNodeGroup allowed_group /* i : group of allowed speaker nodes for forming the triplets in this call */
1425 : )
1426 : {
1427 : Word16 chA, chB, chC, k, l, m;
1428 1792 : Word16 num_triplets = 0;
1429 1792 : move16();
1430 : Word16 num_connected_to_chA;
1431 : Word16 connected_to_chA[VBAP_MAX_NUM_SPEAKER_NODES];
1432 : Word16 connection_uses_left[VBAP_MAX_NUM_SPEAKER_NODES];
1433 : Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS]; /* Q19 */
1434 : Word16 triplet_order[VBAP_MAX_NUM_TRIPLETS];
1435 :
1436 : /* Each connection can be used exactly by two different virtual surface triplets. */
1437 1792 : set16_fx( connection_uses_left, 2, VBAP_MAX_NUM_SPEAKER_NODES );
1438 :
1439 19972 : FOR( chA = 0; chA < num_speaker_nodes; chA++ )
1440 : {
1441 : /* Early skip if not in correct group. */
1442 18180 : IF( NE_16( speaker_node_data[chA].group, allowed_group ) )
1443 : {
1444 14280 : CONTINUE;
1445 : }
1446 :
1447 : /* Get all connections connected to current chA that have not been used by
1448 : * two triplets yet. */
1449 3900 : num_connected_to_chA = 0;
1450 3900 : move16();
1451 77992 : FOR( k = 0; k < max_num_connections; k++ )
1452 : {
1453 74092 : test();
1454 74092 : test();
1455 74092 : IF( ( EQ_16( connections[k][0], chA ) || EQ_16( connections[k][1], chA ) ) && ( connection_uses_left[k] > 0 ) )
1456 : {
1457 16054 : connected_to_chA[num_connected_to_chA] = k;
1458 16054 : move16();
1459 16054 : num_connected_to_chA = add( num_connected_to_chA, 1 );
1460 : }
1461 : }
1462 :
1463 : /* Check that we have enough connections to use. We need at least two available connections to form a triplet.
1464 : * This can fail in later stages when all connections are already used. */
1465 3900 : IF( LT_16( num_connected_to_chA, 2 ) )
1466 : {
1467 650 : CONTINUE;
1468 : }
1469 :
1470 : /* Try to form triplets from each valid connection. */
1471 19304 : FOR( k = 0; k < num_connected_to_chA; k++ )
1472 : {
1473 16054 : Word16 connect_index_k = connected_to_chA[k];
1474 16054 : move16();
1475 16054 : chB = connections[connect_index_k][0];
1476 16054 : move16();
1477 16054 : if ( EQ_16( connections[connect_index_k][0], chA ) )
1478 : {
1479 3588 : chB = connections[connect_index_k][1];
1480 3588 : move16();
1481 : }
1482 27843 : FOR( l = k + 1; l < num_connected_to_chA; l++ )
1483 : {
1484 24593 : Word16 connect_index_l = connected_to_chA[l];
1485 24593 : move16();
1486 24593 : chC = connections[connect_index_l][0];
1487 24593 : move16();
1488 24593 : if ( EQ_16( connections[connect_index_l][0], chA ) )
1489 : {
1490 6930 : chC = connections[connect_index_l][1];
1491 6930 : move16();
1492 : }
1493 :
1494 : /* With chA, chB, and chC selected, we still need to find connection between chB and chC and verify that the triplet is valid */
1495 301225 : FOR( m = 0; m < max_num_connections; m++ )
1496 : {
1497 291228 : test();
1498 291228 : test();
1499 291228 : test();
1500 291228 : 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 ) ) )
1501 : {
1502 14596 : Word16 flag = check_and_store_triplet_fx( chA, chB, chC, num_speaker_nodes, speaker_node_data, triplets, &num_triplets, triplet_azidegs_fx, triplet_order );
1503 14596 : IF( EQ_16( flag, 1 ) )
1504 : {
1505 14596 : connection_uses_left[connect_index_k] = sub( connection_uses_left[connect_index_k], 1 );
1506 14596 : move16();
1507 14596 : connection_uses_left[connect_index_l] = sub( connection_uses_left[connect_index_l], 1 );
1508 14596 : move16();
1509 14596 : connection_uses_left[m] = sub( connection_uses_left[m], 1 );
1510 14596 : move16();
1511 14596 : BREAK;
1512 : }
1513 : }
1514 : }
1515 :
1516 : /* Check if chA-chB connection has been used in two triplets already. If yes, then break out of loop
1517 : * as this connection cannot be used for more triplets and we need to continue with another chA-chB
1518 : * connection. */
1519 24593 : IF( LT_16( connection_uses_left[connect_index_k], 1 ) )
1520 : {
1521 12804 : BREAK;
1522 : }
1523 : }
1524 : }
1525 : }
1526 :
1527 : /* All triplets should be stored now. Sort them for search use and then determine the initial search indices for
1528 : * each search sector for this search struct. */
1529 1792 : v_sort_ind_fixed( triplet_azidegs_fx, triplet_order, num_triplets );
1530 1792 : reorder_triplets_fx( triplets, triplet_order, num_triplets );
1531 1792 : determine_initial_search_indices_fx( num_triplets, triplet_azidegs_fx, initial_search_indices );
1532 :
1533 1792 : return num_triplets;
1534 : }
1535 : /*-------------------------------------------------------------------------*
1536 : * determine_initial_search_indices()
1537 : *
1538 : * Determine initial search indices used for fast search of correct triangle
1539 : *-------------------------------------------------------------------------*/
1540 :
1541 :
1542 1792 : static void determine_initial_search_indices_fx(
1543 : const Word16 num_triplets, /* i : number of triplets */
1544 : const Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS], /* i : azimuths of triplets (in degrees) Q19 */
1545 : Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS] /* o : initial search indices */
1546 : )
1547 : {
1548 : Word16 i, j;
1549 : Word32 sector_reference_azideg_fx; /* Q0 */
1550 : Word32 sector_border_start_azideg_fx; /* Q0 */
1551 : Word32 sector_border_end_azideg_fx; /* Q0 */
1552 : Word16 best_index;
1553 : Word32 min_azideg_diff_fx;
1554 : Word32 azideg_diff_fx; /* Q19 */
1555 :
1556 8960 : FOR( i = 0; i < VBAP_NUM_SEARCH_SECTORS; i++ )
1557 : {
1558 7168 : sector_border_start_azideg_fx = imult3216( i, VBAP_SEARCH_SECTOR_SIZE_Q0 );
1559 7168 : sector_border_end_azideg_fx = imult3216( add( i, 1 ), VBAP_SEARCH_SECTOR_SIZE_Q0 );
1560 7168 : sector_reference_azideg_fx = L_shr( L_add( sector_border_start_azideg_fx, sector_border_end_azideg_fx ), 1 );
1561 :
1562 :
1563 7168 : best_index = 0;
1564 7168 : move16();
1565 7168 : min_azideg_diff_fx = ONE_IN_Q31;
1566 7168 : move32();
1567 :
1568 65552 : FOR( j = 0; j < num_triplets; j++ )
1569 : {
1570 58384 : azideg_diff_fx = L_sub( L_shl( sector_reference_azideg_fx, Q19 ), triplet_azidegs_fx[j] );
1571 :
1572 58384 : IF( GT_32( azideg_diff_fx, 94371840 /* 180.0f in Q19 */ ) )
1573 : {
1574 25380 : azideg_diff_fx = L_sub( azideg_diff_fx, 188743680 /* 360.0f in Q19 */ );
1575 : }
1576 33004 : ELSE IF( LT_32( azideg_diff_fx, -94371840 /* -180.0f in Q19 */ ) )
1577 : {
1578 0 : azideg_diff_fx = L_add( azideg_diff_fx, 188743680 /* 360.0f in Q19 */ );
1579 : }
1580 58384 : azideg_diff_fx = L_abs( azideg_diff_fx );
1581 :
1582 58384 : IF( LT_32( azideg_diff_fx, min_azideg_diff_fx ) )
1583 : {
1584 24506 : min_azideg_diff_fx = azideg_diff_fx;
1585 24506 : move32();
1586 24506 : best_index = j;
1587 24506 : move16();
1588 : }
1589 : }
1590 :
1591 7168 : initial_search_indices[i] = best_index;
1592 7168 : move16();
1593 : }
1594 :
1595 1792 : return;
1596 : }
1597 :
1598 : /*-------------------------------------------------------------------------*
1599 : * determine_connections()
1600 : *
1601 : * Determine all valid connections between all speaker nodes
1602 : *-------------------------------------------------------------------------*/
1603 :
1604 896 : static ivas_error determine_connections_fx(
1605 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1606 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
1607 : Word16 connections[][2], /* o : vector of connections */
1608 : const Word16 max_num_connections, /* i : max number of connections */
1609 : Word16 *group1_count, /* o : number of connections in first group */
1610 : Word16 *group2_start, /* o : start of second group of connections */
1611 : Word16 *group2_count /* o : number of connections in second group */
1612 : )
1613 : {
1614 : Word16 num_non_crossing_planes;
1615 : Word16 c;
1616 896 : Word16 connection_write_index = 0;
1617 896 : move16();
1618 : Word32 non_crossing_plane_elevation_deg_fx[VBAP_MAX_PLANES]; /* Q14 */
1619 :
1620 : ivas_error error;
1621 :
1622 896 : set32_fx( non_crossing_plane_elevation_deg_fx, 0, VBAP_MAX_PLANES );
1623 :
1624 : /* Reset connection data */
1625 22790 : FOR( c = 0; c < max_num_connections; c++ )
1626 : {
1627 21894 : connections[c][0] = VBAP_NOT_VALID_CONNECTION;
1628 21894 : move16();
1629 : }
1630 :
1631 : /* This function determines some prominent elevated planes, that are favoured in making node-node connections. */
1632 896 : num_non_crossing_planes = determine_non_crossing_planes_fx( num_speaker_nodes, speaker_node_data, non_crossing_plane_elevation_deg_fx );
1633 :
1634 : /* Process in different mode based on the grouping. It is enough to check for first node. */
1635 896 : IF( EQ_16( speaker_node_data[0].group, SPEAKER_NODE_ALL ) )
1636 : {
1637 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 ) )
1638 : {
1639 0 : return error;
1640 : }
1641 : }
1642 : ELSE
1643 : {
1644 : /* The node-node connections are determined in three stages: bottom, horizontal, and top. */
1645 896 : 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 ) )
1646 : {
1647 0 : return error;
1648 : }
1649 896 : *group2_start = connection_write_index;
1650 896 : move16();
1651 :
1652 896 : formulate_horizontal_connections_fx( speaker_node_data, num_speaker_nodes, connections, &connection_write_index );
1653 896 : *group1_count = connection_write_index;
1654 896 : move16();
1655 :
1656 896 : 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 ) )
1657 : {
1658 0 : return error;
1659 : }
1660 :
1661 896 : *group2_count = sub( connection_write_index, *group2_start );
1662 896 : move16();
1663 : }
1664 :
1665 896 : return IVAS_ERR_OK;
1666 : }
1667 : /*-------------------------------------------------------------------------*
1668 : * determine_connection_class()
1669 : *
1670 : * Determine the type of connection
1671 : *-------------------------------------------------------------------------*/
1672 :
1673 : /*! r: type of connection */
1674 27314 : static enum ConnectionClass determine_connection_class_fx(
1675 : const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */
1676 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1677 : const enum SpeakerNodeGroup group, /* i : speaker node group */
1678 : const Word16 chA, /* i : speaker node counter 1 */
1679 : const Word16 chB /* i : speaker node counter 2 */
1680 : )
1681 : {
1682 : Word16 ch, k;
1683 :
1684 : const Word32 *p1_fx, *v2_fx;
1685 : Word32 v1v1_fx, v1v2_fx, v2v2_fx, v1p1_fx, v2p1_fx; /* Q25, Q27, Q29, Q27, Q29 */
1686 : Word32 determinant_fx; /* Q23 */
1687 : Word32 norm_distance_on_v1_fx;
1688 : Word32 vec_diff_fx[3];
1689 : Word32 v1_fx[3]; /* Q28 */
1690 : Word32 vTarget_fx[3];
1691 : Word32 energy_sum_fx;
1692 : Word32 eq_value_fx;
1693 : Word32 uvecdot_fx; /* Q30 */
1694 :
1695 : /* Check if connection passes through origin. This is not desired.
1696 : * When this happens, unit vectors point in opposite directions. */
1697 27314 : 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
1698 :
1699 27314 : test();
1700 27314 : IF( LT_32( uvecdot_fx, VBAP_EPSILON_Q3O ) && GT_32( uvecdot_fx, L_negate( VBAP_EPSILON_Q3O ) ) )
1701 : {
1702 0 : return CONNECTION_WITH_SPEAKER_NODE_BEHIND;
1703 : }
1704 :
1705 : /* This loop checks the chA-chB connection with respect to all loudspeakers:
1706 : - in case there is a node behind or nearly behind the connection line. These
1707 : connections need to be discarded.
1708 : - in case there is a node that is closer to the connection line than 1/5 of the
1709 : connection length AND at the same horizontal plane. These connections need to be
1710 : weighted with a penalty (a special case, for example avoiding elevated L,R,C triplet)
1711 : */
1712 317266 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1713 : {
1714 : /* Select speaker_node only within TOP or BOTTOM sphere half, not being part of chA-chB pair */
1715 292852 : test();
1716 292852 : test();
1717 292852 : IF( ( EQ_16( group, node_data[ch].group ) ) && ( NE_16( ch, chA ) ) && ( NE_16( ch, chB ) ) )
1718 : {
1719 : /* The following lines formulate the point on the chA-chB-connection that is
1720 : nearest to the origo-ch-line */
1721 65340 : p1_fx = node_data[chA].unit_vec_fx; // Q30
1722 65340 : move32();
1723 :
1724 261360 : FOR( k = 0; k < 3; k++ )
1725 : {
1726 196020 : 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) */
1727 196020 : move32();
1728 : }
1729 65340 : v2_fx = node_data[ch].unit_vec_fx; // Q30
1730 65340 : move32();
1731 :
1732 65340 : v1v1_fx = dotp_fixed( v1_fx, v1_fx, 3 ); // Q25
1733 65340 : move32();
1734 65340 : v1v2_fx = dotp_fixed( v1_fx, v2_fx, 3 ); // Q27
1735 65340 : move32();
1736 65340 : v2v2_fx = ONE_IN_Q29;
1737 65340 : move32();
1738 :
1739 65340 : v1p1_fx = dotp_fixed( v1_fx, p1_fx, 3 ); // Q27
1740 65340 : move32();
1741 65340 : v2p1_fx = dotp_fixed( v2_fx, p1_fx, 3 ); // Q29
1742 65340 : move32();
1743 :
1744 65340 : Word32 tmp1 = Mpy_32_32( v1v2_fx, v1v2_fx ); // Q23
1745 65340 : Word32 tmp2 = Mpy_32_32( v1v1_fx, L_negate( v2v2_fx ) ); // Q23
1746 65340 : Word32 tmp3 = L_add( tmp1, tmp2 ); // Q23
1747 :
1748 65340 : determinant_fx = tmp3; // Q23
1749 65340 : move32();
1750 :
1751 : /* Norm distance = distance parameter on line chA-chB, determines point that is
1752 : nearest to origo-ch line. Distance 0 means chA and distance 1 means chB. Can be
1753 : outside this region as well.*/
1754 :
1755 65340 : Word32 tmp4 = Mpy_32_32( v2v2_fx, v1p1_fx ); // q25
1756 65340 : Word32 tmp5 = Mpy_32_32( L_negate( v1v2_fx ), v2p1_fx ); // q25
1757 65340 : Word32 tmp6 = L_add( tmp4, tmp5 ); // q25
1758 65340 : Word16 tmp7, exp = 0;
1759 65340 : move16();
1760 65340 : if ( determinant_fx == 0 )
1761 : {
1762 0 : determinant_fx = 1;
1763 0 : move32();
1764 : }
1765 65340 : tmp7 = BASOP_Util_Divide3232_Scale( ONE_IN_Q23, determinant_fx, &exp ); // Q(15 - exp)
1766 65340 : norm_distance_on_v1_fx = Mpy_32_16_1( tmp6, tmp7 ); // Q25 + Q(15 - exp) - Q15 = Q(25 - exp)
1767 :
1768 : /* Continue only if the nearest point is between chA and chB */
1769 65340 : Word16 exp_vTarget = 0, exp_energy_sum = 0, exp_vec_diff = 0;
1770 : Word32 var1, var2;
1771 : Word16 vTarget_fx_e[3], vec_diff_e[3];
1772 65340 : test();
1773 65340 : IF( norm_distance_on_v1_fx > 0 && LT_32( norm_distance_on_v1_fx, L_shr( ONE_IN_Q31, sub( 31, sub( Q25, exp ) ) ) ) )
1774 : {
1775 : /* Formulate vTarget, that is an unit vector that goes through the determined point on chA-chB connection */
1776 53542 : energy_sum_fx = 0;
1777 53542 : move32();
1778 214168 : FOR( k = 0; k < 3; k++ )
1779 : {
1780 160626 : var1 = Mpy_32_32( norm_distance_on_v1_fx, v1_fx[k] ); // Q(25 - exp) + Q28 - 31
1781 160626 : vTarget_fx[k] = BASOP_Util_Add_Mant32Exp( p1_fx[k], 1, var1, sub( Q31, add( sub( Q25, exp ), Q28 - Q31 ) ), &exp_vTarget );
1782 160626 : move16();
1783 160626 : vTarget_fx_e[k] = exp_vTarget;
1784 160626 : move16();
1785 :
1786 160626 : var2 = Mpy_32_32( vTarget_fx[k], vTarget_fx[k] ); // 2*exp_vTarget
1787 160626 : energy_sum_fx = BASOP_Util_Add_Mant32Exp( energy_sum_fx, exp_energy_sum, var2, shl( exp_vTarget, 1 ), &exp_energy_sum );
1788 160626 : vec_diff_fx[k] = BASOP_Util_Add_Mant32Exp( vTarget_fx[k], exp_vTarget, L_negate( v2_fx[k] ), 1, &exp_vec_diff );
1789 160626 : move16();
1790 160626 : vec_diff_e[k] = exp_vec_diff;
1791 160626 : move16();
1792 : }
1793 53542 : Word16 exp_eq = exp_energy_sum;
1794 53542 : move16();
1795 53542 : eq_value_fx = ISqrt32( energy_sum_fx, &exp_eq );
1796 214168 : FOR( k = 0; k < 3; k++ )
1797 : {
1798 160626 : vTarget_fx[k] = Mpy_32_32( vTarget_fx[k], eq_value_fx );
1799 160626 : move32();
1800 160626 : vTarget_fx_e[k] = add( vTarget_fx_e[k], exp_eq );
1801 160626 : move16();
1802 : }
1803 : /*make a common exponent*/
1804 53542 : Word16 max_vTarget_e = 0, max_vec_diff_e = 0;
1805 53542 : move16();
1806 53542 : move16();
1807 214168 : FOR( k = 0; k < 3; k++ )
1808 : {
1809 160626 : max_vTarget_e = s_max( vTarget_fx_e[k], max_vTarget_e );
1810 160626 : max_vec_diff_e = s_max( vec_diff_e[k], max_vec_diff_e );
1811 : }
1812 214168 : FOR( k = 0; k < 3; k++ )
1813 : {
1814 160626 : vTarget_fx[k] = L_shr( vTarget_fx[k], sub( max_vTarget_e, vTarget_fx_e[k] ) );
1815 160626 : move32();
1816 160626 : vec_diff_fx[k] = L_shr( vec_diff_fx[k], sub( max_vec_diff_e, vec_diff_e[k] ) );
1817 160626 : move32();
1818 : }
1819 : /* A check if the angle between vTarget and node_data[ch].unit_vec is less than 1 degree.
1820 : Essentially reveals if there is a speaker node too close "behind" the connection. Such
1821 : connections should be rejected.*/
1822 214168 : FOR( Word16 i = 0; i < 3; i++ )
1823 : {
1824 160626 : vTarget_fx[i] = L_shr( vTarget_fx[i], 2 ); // add guard bits
1825 160626 : move32();
1826 160626 : vec_diff_fx[i] = L_shr( vec_diff_fx[i], 2 );
1827 160626 : move32();
1828 : }
1829 53542 : Word32 res = dotp_fixed( vTarget_fx, v2_fx, 3 ); // 31 - (max_vTarget_e + 2) + 30 - 31 = 28 - max_vTarget_e
1830 53542 : move32();
1831 :
1832 53542 : IF( GT_32( res, L_shr( 2147054208 /* 0.9998f in Q31 */, sub( 31, sub( 28, max_vTarget_e ) ) ) ) )
1833 : {
1834 2900 : return CONNECTION_WITH_SPEAKER_NODE_BEHIND;
1835 : }
1836 :
1837 : /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes.
1838 : A triplet between these nodes is not desired if there is a top node,
1839 : a penalty is implemented to take care of this. */
1840 50642 : Word32 vec_diff_dotp = dotp_fixed( vec_diff_fx, vec_diff_fx, 3 ); // exp : 2 * max_vec_diff_e + 4
1841 50642 : move32();
1842 50642 : Word32 var = Mpy_32_32( vec_diff_dotp, 51200 /*25.0f in Q11*/ ); // exp : 2 * max_vec_diff_e + 4 + 20
1843 50642 : Word16 Flag1 = BASOP_Util_Cmp_Mant32Exp( v1v1_fx, Q31 - Q25, var, add( shl( max_vec_diff_e, 1 ), 4 + 20 ) );
1844 50642 : IF( EQ_16( Flag1, 1 ) )
1845 : {
1846 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 */ ) )
1847 : {
1848 0 : return ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION;
1849 : }
1850 : }
1851 : }
1852 : }
1853 : }
1854 :
1855 24414 : return REGULAR_CONNECTION;
1856 : }
1857 :
1858 : /*-------------------------------------------------------------------------*
1859 : * formulate_horizontal_connections()
1860 : *
1861 : * Formulate connections in the horizontal plane
1862 : *-------------------------------------------------------------------------*/
1863 :
1864 896 : static void formulate_horizontal_connections_fx(
1865 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
1866 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1867 : Word16 connections[][2], /* o : vector of all connections */
1868 : Word16 *connection_write_index )
1869 : {
1870 : Word16 ch;
1871 : Word16 chCheck;
1872 : Word16 next_index;
1873 :
1874 : Word32 min_arc_diff_fx;
1875 : Word32 arc_diff_fx;
1876 : Word16 Q_min_arc_diff;
1877 :
1878 9986 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1879 : {
1880 : /* Find next horizontal speaker node */
1881 9090 : IF( EQ_16( speaker_node_data[ch].group, SPEAKER_NODE_HORIZONTAL ) )
1882 : {
1883 5190 : next_index = -1;
1884 5190 : move16();
1885 5190 : min_arc_diff_fx = 19998 /*9999.0f in Q1*/;
1886 5190 : move32();
1887 5190 : Q_min_arc_diff = 1;
1888 5190 : move16();
1889 :
1890 58556 : FOR( chCheck = 0; chCheck < num_speaker_nodes; chCheck++ )
1891 : {
1892 53366 : test();
1893 53366 : IF( ( NE_16( ch, chCheck ) ) && ( EQ_16( speaker_node_data[chCheck].group, SPEAKER_NODE_HORIZONTAL ) ) )
1894 : {
1895 25728 : arc_diff_fx = L_sub( speaker_node_data[chCheck].azi_deg_fx, speaker_node_data[ch].azi_deg_fx ); // Q22
1896 :
1897 38592 : WHILE( arc_diff_fx < 0 )
1898 : {
1899 12864 : arc_diff_fx = L_add( arc_diff_fx, 1509949440 /*360.0f in Q22*/ );
1900 : }
1901 25728 : IF( LT_32( L_shr( arc_diff_fx, sub( 22, Q_min_arc_diff ) ), min_arc_diff_fx ) )
1902 : {
1903 11649 : min_arc_diff_fx = arc_diff_fx;
1904 11649 : move32();
1905 11649 : Q_min_arc_diff = 22;
1906 11649 : move16();
1907 11649 : next_index = chCheck;
1908 11649 : move16();
1909 : }
1910 : }
1911 : }
1912 5190 : connections[*connection_write_index][0] = ch;
1913 5190 : move16();
1914 5190 : connections[*connection_write_index][1] = next_index;
1915 5190 : move16();
1916 5190 : *connection_write_index = add( *connection_write_index, 1 );
1917 5190 : move16();
1918 : }
1919 : }
1920 :
1921 896 : return;
1922 : }
1923 :
1924 : /*-------------------------------------------------------------------------*
1925 : * check_plane_crossing()
1926 : *
1927 : * Check crossing of non-allowed planes
1928 : *-------------------------------------------------------------------------*/
1929 :
1930 : /*! r: truth value for crossing */
1931 24414 : static Word16 check_plane_crossing_fx(
1932 : const Word32 ele1_deg_fx, /* i : speaker node 1 elevation Q22 */
1933 : const Word32 ele2_deg_fx, /* i : speaker node 2 elevation Q22 */
1934 : const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */
1935 : const Word32 *non_crossing_plane_elevation_deg_fx /* i : vector non-crossing plane elevations Q14 */
1936 : )
1937 : {
1938 : /* Find if the connection crosses a non-crossing plane, with 1-degree threshold. */
1939 : Word16 plane;
1940 :
1941 40510 : FOR( plane = 0; plane < num_non_crossing_planes; plane++ )
1942 : {
1943 16912 : test();
1944 16912 : 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 ) ) ) )
1945 : {
1946 8 : return 1;
1947 : }
1948 16904 : test();
1949 16904 : 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 ) ) ) )
1950 : {
1951 808 : return 1;
1952 : }
1953 : }
1954 :
1955 23598 : return 0;
1956 : }
1957 :
1958 : /*-------------------------------------------------------------------------*
1959 : * get_half_sphere_connection_options()
1960 : *
1961 : * Get list of all potential connections at the half-sphere
1962 : *-------------------------------------------------------------------------*/
1963 1792 : static ivas_error get_half_sphere_connection_options_fx(
1964 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
1965 : const enum SpeakerNodeGroup group, /* i : speaker node group */
1966 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1967 : const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */
1968 : const Word32 *non_crossing_plane_elevation_deg_fx, /* i : vector of non-crossing plane elevations Q14 */
1969 : ConnectionOption **connection_options_pr, /* o : list of connection options */
1970 : Word16 *num_connection_options /* o : number of connection options */
1971 : )
1972 : {
1973 1792 : Word16 max_num_connection_options = 0;
1974 1792 : Word16 index = 0;
1975 : Word16 node, chA, chB, c, c_cmp;
1976 : ConnectionOption *c_options, *c_options_reorder;
1977 :
1978 1792 : move16();
1979 1792 : move16();
1980 :
1981 : /* Count max num connection options at the half sphere */
1982 19972 : FOR( node = 0; node < num_speaker_nodes; node++ )
1983 : {
1984 18180 : test();
1985 18180 : IF( EQ_16( speaker_node_data[node].group, group ) || EQ_16( speaker_node_data[node].group, SPEAKER_NODE_HORIZONTAL ) )
1986 : {
1987 14280 : max_num_connection_options = add( max_num_connection_options, index );
1988 14280 : index = add( index, 1 );
1989 : }
1990 : }
1991 :
1992 : /* Init memory for connection options */
1993 1792 : IF( ( c_options = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * max_num_connection_options ) ) == NULL )
1994 : {
1995 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
1996 : }
1997 :
1998 54834 : FOR( c = 0; c < max_num_connection_options; c++ )
1999 : {
2000 53042 : c_options[c].chA = -1;
2001 53042 : move16();
2002 53042 : c_options[c].chB = -1;
2003 53042 : move16();
2004 53042 : c_options[c].arc_fx = MAX_32;
2005 53042 : move32();
2006 53042 : c_options[c].arc_weighted_fx = MAX_32;
2007 53042 : move32();
2008 : }
2009 :
2010 : /* Determine connection options for the half-sphere */
2011 1792 : index = 0;
2012 1792 : move16();
2013 19972 : FOR( chA = 0; chA < num_speaker_nodes; chA++ )
2014 : {
2015 : /* First loudspeaker at the connection is at the half sphere */
2016 18180 : test();
2017 18180 : IF( EQ_16( speaker_node_data[chA].group, group ) || EQ_16( speaker_node_data[chA].group, SPEAKER_NODE_HORIZONTAL ) )
2018 : {
2019 92790 : FOR( chB = chA + 1; chB < num_speaker_nodes; chB++ )
2020 : {
2021 : /* Second loudspeaker at the connection is at the half sphere, but so that first and second are not both horizontal. */
2022 78510 : test();
2023 78510 : test();
2024 78510 : 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 ) ) )
2025 : {
2026 27314 : Word16 ConnectionClass = determine_connection_class_fx( speaker_node_data, num_speaker_nodes, group, chA, chB );
2027 : Word32 unit_vec_dotp, unit_vec_dotp_sq, one_minus_unit_vec_dotp_sq, one_minus_unit_vec_dotp_sq_root;
2028 : Word16 acos_val;
2029 : /* Connection is considered only if there is no speaker node behind it */
2030 27314 : IF( NE_16( ConnectionClass, CONNECTION_WITH_SPEAKER_NODE_BEHIND ) )
2031 : {
2032 : /* Store connection information */
2033 24414 : c_options[index].chA = chA;
2034 24414 : move16();
2035 24414 : c_options[index].chB = chB;
2036 24414 : move16();
2037 :
2038 24414 : unit_vec_dotp = dotp_fixed( speaker_node_data[chA].unit_vec_fx, speaker_node_data[chB].unit_vec_fx, 3 ); // Q29
2039 24414 : unit_vec_dotp_sq = Mpy_32_32( unit_vec_dotp, unit_vec_dotp ); // Q27
2040 24414 : one_minus_unit_vec_dotp_sq = L_sub( ONE_IN_Q27, unit_vec_dotp_sq );
2041 24414 : Word16 exp_uv = Q31 - Q27;
2042 24414 : move16();
2043 24414 : one_minus_unit_vec_dotp_sq_root = Sqrt32( one_minus_unit_vec_dotp_sq, &exp_uv );
2044 24414 : acos_val = BASOP_util_atan2( one_minus_unit_vec_dotp_sq_root, unit_vec_dotp, sub( exp_uv, 2 ) ); // Q13
2045 24414 : c_options[index].arc_fx = L_deposit_h( acos_val ); // Q29
2046 24414 : move32();
2047 24414 : c_options[index].arc_weighted_fx = c_options[index].arc_fx; // Q29
2048 24414 : move32();
2049 :
2050 : /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes.
2051 : A triplet between these nodes is not desired if there is a top node,
2052 : a penalty is implemented to take care of this. */
2053 24414 : IF( EQ_16( ConnectionClass, ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION ) )
2054 : {
2055 0 : c_options[index].arc_weighted_fx = L_shl( c_options[index].arc_weighted_fx, 1 );
2056 0 : move32();
2057 : }
2058 :
2059 : /* If the connection passes a pre-determined plane of speaker nodes, then add further penalty */
2060 :
2061 24414 : 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 ) )
2062 : {
2063 816 : c_options[index].arc_weighted_fx = L_shl( c_options[index].arc_weighted_fx, 1 );
2064 816 : move32();
2065 : }
2066 24414 : index = add( index, 1 );
2067 : }
2068 : }
2069 : }
2070 : }
2071 : }
2072 : /* Number of found connection options at the half sphere */
2073 1792 : *num_connection_options = index;
2074 1792 : move16();
2075 : /* Init memory for reordered connection options and order by arc_weighted,
2076 : * which informs of the preference order of the connections in case they cross */
2077 1792 : IF( ( c_options_reorder = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * ( *num_connection_options ) ) ) == NULL )
2078 : {
2079 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
2080 : }
2081 :
2082 26206 : FOR( c = 0; c < *num_connection_options; c++ )
2083 : {
2084 : Word16 min_arc_index;
2085 24414 : min_arc_index = -1;
2086 24414 : move16();
2087 24414 : Word32 min_arc_weighted_fx_new = MAX_32;
2088 24414 : move16();
2089 24414 : Word16 exp_min_arc_weighted_fx = 31;
2090 24414 : move16();
2091 24414 : Word16 res = 0;
2092 24414 : move16();
2093 603704 : FOR( c_cmp = 0; c_cmp < *num_connection_options; c_cmp++ )
2094 : {
2095 579290 : res = BASOP_Util_Cmp_Mant32Exp( c_options[c_cmp].arc_weighted_fx, 31 - Q29, min_arc_weighted_fx_new, exp_min_arc_weighted_fx );
2096 579290 : test();
2097 579290 : IF( EQ_16( res, -1 ) || res == 0 )
2098 : {
2099 126474 : min_arc_weighted_fx_new = c_options[c_cmp].arc_weighted_fx;
2100 126474 : move32();
2101 126474 : exp_min_arc_weighted_fx = Q31 - Q29;
2102 126474 : move16();
2103 126474 : min_arc_index = c_cmp;
2104 126474 : move16();
2105 : }
2106 : }
2107 24414 : c_options_reorder[c].chA = c_options[min_arc_index].chA;
2108 24414 : move16();
2109 24414 : c_options_reorder[c].chB = c_options[min_arc_index].chB;
2110 24414 : move16();
2111 :
2112 24414 : c_options_reorder[c].arc_fx = c_options[min_arc_index].arc_fx;
2113 24414 : move32();
2114 24414 : c_options_reorder[c].arc_weighted_fx = c_options[min_arc_index].arc_weighted_fx;
2115 24414 : move32();
2116 24414 : c_options[min_arc_index].arc_weighted_fx = MAX_32;
2117 24414 : move32();
2118 : }
2119 :
2120 : /* Set reordered connections as output and free temporary data */
2121 1792 : *connection_options_pr = c_options_reorder;
2122 1792 : free( c_options );
2123 :
2124 1792 : return IVAS_ERR_OK;
2125 : }
2126 :
2127 : /*-------------------------------------------------------------------------*
2128 : * formulate_half_sphere_connections()
2129 : *
2130 : * Formulate half-sphere connections
2131 : *-------------------------------------------------------------------------*/
2132 1792 : static ivas_error formulate_half_sphere_connections_fx(
2133 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
2134 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
2135 : const enum SpeakerNodeGroup group, /* i : speaker node group */
2136 : Word16 connections[][2], /* o : vector of connections */
2137 : Word16 *connection_write_index,
2138 : const Word16 max_num_connections, /* i : max number of connections */
2139 : const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */
2140 : const Word32 *non_crossing_plane_elevation_deg_fx /* i : vector of non-crossing plane elevations Q14*/
2141 : )
2142 : {
2143 : /* Variable initializations */
2144 : Word16 c, chA, chB, cmp_chA, cmp_chB, k, c_opt;
2145 : Word16 new_connection_is_valid;
2146 : Word16 within_first_arc;
2147 : Word32 new_cross_fx[3];
2148 : Word32 planeCrossingVec_fx[3];
2149 : Word16 Q_planeCrossingVec;
2150 : Word32 new_arc_fx; /* Q29 */
2151 : Word32 connection_arc_fx[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3]; /* Q29 */
2152 : Word32 connection_cross_fx[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3][3]; /* Q29 */
2153 : Word32 tmpFloat_fx;
2154 : Word32 cmp_arc_fx; /* Q29 */
2155 : Word32 normVal_fx;
2156 : Word16 angleCmp_fx;
2157 : ConnectionOption *connection_options;
2158 : Word16 num_connection_options;
2159 : Word16 half_sphere_first_connection;
2160 : ivas_error error;
2161 :
2162 1792 : half_sphere_first_connection = *connection_write_index;
2163 1792 : move16();
2164 :
2165 : /* Obtain all connection options (i.e., channel pairs) at the half sphere. The function orders them
2166 : * in terms of which connection to keep if two connections would cross each other. */
2167 1792 : IF( NE_32( ( error = get_half_sphere_connection_options_fx(
2168 : speaker_node_data,
2169 : group,
2170 : num_speaker_nodes,
2171 : num_non_crossing_planes,
2172 : non_crossing_plane_elevation_deg_fx,
2173 : &connection_options,
2174 : &num_connection_options ) ),
2175 : IVAS_ERR_OK ) )
2176 : {
2177 0 : return error;
2178 : }
2179 :
2180 1792 : set32_fx( connection_arc_fx, 0, max_num_connections );
2181 45580 : FOR( c = 0; c < max_num_connections; c++ )
2182 : {
2183 43788 : set32_fx( connection_cross_fx[c], 0, 3 );
2184 : }
2185 :
2186 : /* The following loop goes through all reasonable chA - chB pairs for the half-sphere */
2187 1792 : c_opt = 0;
2188 1792 : move16();
2189 21748 : WHILE( c_opt < num_connection_options && *connection_write_index < max_num_connections )
2190 : {
2191 19956 : test();
2192 19956 : chA = connection_options[c_opt].chA;
2193 19956 : move16();
2194 19956 : chB = connection_options[c_opt].chB;
2195 19956 : move16();
2196 19956 : new_arc_fx = connection_options[c_opt].arc_fx;
2197 19956 : move32();
2198 :
2199 : /* Cross-product is needed for later stages */
2200 19956 : vbap_crossp_fx( speaker_node_data[chA].unit_vec_fx, speaker_node_data[chB].unit_vec_fx, new_cross_fx );
2201 :
2202 : /* Determine if new connection between chA and chB is valid */
2203 19956 : new_connection_is_valid = 1;
2204 19956 : move16();
2205 19956 : c = half_sphere_first_connection;
2206 19956 : move16();
2207 : 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;
2208 19956 : 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;
2209 :
2210 19956 : test();
2211 135329 : WHILE( ( LT_16( c, *connection_write_index ) ) && new_connection_is_valid )
2212 : {
2213 115373 : cmp_chA = connections[c][0];
2214 115373 : move16();
2215 115373 : cmp_chB = connections[c][1];
2216 115373 : move16();
2217 : /* The connections are compared only if they don't involve same speaker nodes */
2218 115373 : test();
2219 115373 : test();
2220 115373 : test();
2221 115373 : IF( ( NE_16( cmp_chA, chA ) ) && ( NE_16( cmp_chA, chB ) ) && ( NE_16( cmp_chB, chA ) ) && ( NE_16( cmp_chB, chB ) ) )
2222 : {
2223 : /* The following lines determine if the connection chA-chB crosses with the connection cmp_chA-cmp_chB.*/
2224 : /* The connections, i.e., node-pairs determine a plane. The crossing can be determined by
2225 : * studying the intersection of these planes. */
2226 57124 : vbap_crossp_fx( connection_cross_fx[c], new_cross_fx, planeCrossingVec_fx );
2227 :
2228 57124 : tmpFloat_fx = 0;
2229 57124 : move16();
2230 :
2231 57124 : cmp_arc_fx = connection_arc_fx[c];
2232 57124 : move16();
2233 228496 : FOR( k = 0; k < 3; k++ )
2234 : {
2235 171372 : tmpFloat_fx = L_add( tmpFloat_fx, Mpy_32_32( planeCrossingVec_fx[k], planeCrossingVec_fx[k] ) );
2236 : }
2237 :
2238 57124 : Word16 tmp_exp = sub( 31, Q23 );
2239 57124 : if ( tmpFloat_fx == 0 )
2240 : {
2241 400 : tmpFloat_fx = 1;
2242 400 : move16();
2243 : }
2244 57124 : normVal_fx = ISqrt32( tmpFloat_fx, &tmp_exp );
2245 :
2246 228496 : FOR( k = 0; k < 3; k++ )
2247 : {
2248 171372 : planeCrossingVec_fx[k] = Mpy_32_32( planeCrossingVec_fx[k], normVal_fx ); // Q27 + Q31 - tmp_exp - Q31
2249 171372 : move32();
2250 : }
2251 : /*update Q for planeCrossingVec */
2252 57124 : Q_planeCrossingVec = sub( sub( Q27 + Q31, tmp_exp ), Q31 );
2253 : /* If the plane intersection is between both connections, then the two connections cross. */
2254 : /* Study first if the crossing is between arc chA-chB */
2255 :
2256 57124 : var1 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[chA].unit_vec_fx, 3 ); // Q24 = 26 - tmp_exp
2257 57124 : var2 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[chB].unit_vec_fx, 3 ); // Q24 = 26 - tmp_exp
2258 :
2259 57124 : var1_sq = Mpy_32_32( var1, var1 ); //(2 * (Q_planeCrossingVec - Q1) ) - Q31
2260 57124 : var2_sq = Mpy_32_32( var2, var2 );
2261 57124 : exp_var1_sq = shl( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ), 1 );
2262 57124 : exp_var2_sq = shl( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ), 1 );
2263 57124 : IF( GT_32( L_abs( var1_sq ), L_shr( ONE_IN_Q31, exp_var1_sq ) ) )
2264 : {
2265 2 : var1_sq = ONE_IN_Q31;
2266 2 : move16();
2267 2 : exp_var1_sq = 0;
2268 2 : move16();
2269 : }
2270 57124 : IF( GT_32( L_abs( var2_sq ), L_shr( ONE_IN_Q31, exp_var2_sq ) ) )
2271 : {
2272 0 : var2_sq = ONE_IN_Q31;
2273 0 : move16();
2274 0 : exp_var2_sq = 0;
2275 0 : move16();
2276 : }
2277 57124 : one_minus_var1_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var1_sq ), exp_var1_sq, &final_exp_A );
2278 57124 : var1_sqrt = Sqrt32( one_minus_var1_sq, &final_exp_A );
2279 57124 : var1_cos = BASOP_util_atan2( var1_sqrt, var1, sub( final_exp_A, ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ) ); // Q13
2280 57124 : angleCmp_fx = var1_cos;
2281 57124 : move16();
2282 :
2283 57124 : one_minus_var2_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var2_sq ), exp_var2_sq, &final_exp_B );
2284 57124 : var2_sqrt = Sqrt32( one_minus_var2_sq, &final_exp_B );
2285 57124 : var2_cos = BASOP_util_atan2( var2_sqrt, var2, sub( final_exp_B, ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ) ); // Q13
2286 :
2287 57124 : final_exp = BASOP_Util_Add_MantExp( angleCmp_fx, Q15 - Q13, var2_cos, Q15 - Q13, &angleCmp_fx );
2288 :
2289 57124 : sub_exp = 0, sub_exp_2 = 0, sub_final_exp = 0;
2290 57124 : var_a = BASOP_Util_Add_Mant32Exp( new_arc_fx, Q31 - Q29, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp );
2291 57124 : comp1 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_a ), sub_exp, 21474836 /* 0.01f in Q31 */, 0 );
2292 57124 : var_b = BASOP_Util_Add_Mant32Exp( 25735, Q31 - Q12, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp_2 );
2293 57124 : var_c = BASOP_Util_Add_Mant32Exp( new_arc_fx, Q31 - Q29, L_negate( var_b ), sub_exp_2, &sub_final_exp );
2294 57124 : comp2 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_c ), sub_final_exp, 21474836 /* 0.01f in Q31 */, 0 );
2295 :
2296 57124 : within_first_arc = 0;
2297 57124 : move16();
2298 57124 : IF( EQ_16( comp1, -1 ) )
2299 : {
2300 13120 : within_first_arc = 1;
2301 13120 : move16();
2302 : }
2303 44004 : ELSE IF( EQ_16( comp2, -1 ) )
2304 : {
2305 14144 : within_first_arc = 1;
2306 14144 : move16();
2307 : /* In this case, the plane crossing vector is inverted. The inverse is another
2308 : * plane-crossing vector, and detected to be between chA-chB connection.*/
2309 56576 : FOR( k = 0; k < 3; k++ )
2310 : {
2311 42432 : planeCrossingVec_fx[k] = Mpy_32_32( planeCrossingVec_fx[k], -134217728 /*-1.0f in Q27*/ ); // 27 + 27 - tmp_exp - 31 = 23 - tmp_exp
2312 42432 : move32();
2313 : }
2314 : /*update Q for planeCrossingVec */
2315 14144 : Q_planeCrossingVec = sub( sub( Q27 + Q27, tmp_exp ), Q31 );
2316 : }
2317 :
2318 : /* Study if the crossing is also between arc cmp_chA-cmp_chB */
2319 57124 : IF( within_first_arc > 0 )
2320 : {
2321 27264 : var1 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[cmp_chA].unit_vec_fx, 3 );
2322 27264 : move32();
2323 27264 : var2 = dotp_fixed( planeCrossingVec_fx, speaker_node_data[cmp_chB].unit_vec_fx, 3 );
2324 27264 : move32();
2325 :
2326 : // final_exp_A, final_exp_B, exp_var1_sq, exp_var2_sq;
2327 27264 : var1_sq = Mpy_32_32( var1, var1 ); //(2 * (Q_planeCrossingVec - Q1) ) - Q31
2328 27264 : var2_sq = Mpy_32_32( var2, var2 );
2329 27264 : exp_var1_sq = shl( ( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ) ), 1 );
2330 27264 : exp_var2_sq = shl( ( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ) ), 1 );
2331 27264 : IF( GT_32( L_abs( var1_sq ), L_shr( ONE_IN_Q31, exp_var1_sq ) ) )
2332 : {
2333 2 : var1_sq = ONE_IN_Q31;
2334 2 : move16();
2335 2 : exp_var1_sq = 0;
2336 2 : move16();
2337 : }
2338 27264 : IF( GT_32( L_abs( var2_sq ), L_shr( ONE_IN_Q31, exp_var2_sq ) ) )
2339 : {
2340 0 : var2_sq = ONE_IN_Q31;
2341 0 : move16();
2342 0 : exp_var2_sq = 0;
2343 0 : move16();
2344 : }
2345 27264 : one_minus_var1_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var1_sq ), exp_var1_sq, &final_exp_A );
2346 27264 : var1_sqrt = Sqrt32( one_minus_var1_sq, &final_exp_A );
2347 27264 : var1_cos = BASOP_util_atan2( var1_sqrt, var1, sub( final_exp_A, sub( Q31, add( Q_planeCrossingVec, Q30 - Q31 ) ) ) ); // Q13
2348 27264 : angleCmp_fx = var1_cos;
2349 27264 : move16();
2350 :
2351 27264 : one_minus_var2_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var2_sq ), exp_var2_sq, &final_exp_B );
2352 27264 : var2_sqrt = Sqrt32( one_minus_var2_sq, &final_exp_B );
2353 27264 : var2_cos = BASOP_util_atan2( var2_sqrt, var2, sub( final_exp_B, sub( Q31, add( Q_planeCrossingVec, Q30 - Q31 ) ) ) ); // Q13
2354 :
2355 27264 : final_exp = BASOP_Util_Add_MantExp( angleCmp_fx, Q15 - Q13, var2_cos, Q15 - Q13, &angleCmp_fx );
2356 :
2357 27264 : sub_exp = 0, sub_exp_2 = 0, sub_final_exp = 0;
2358 27264 : move16();
2359 27264 : move16();
2360 27264 : move16();
2361 27264 : var_a = BASOP_Util_Add_Mant32Exp( cmp_arc_fx, Q31 - Q29, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp );
2362 27264 : comp1 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_a ), sub_exp, 21474836 /* 0.01f in Q31 */, 0 );
2363 :
2364 27264 : if ( EQ_16( comp1, -1 ) )
2365 : {
2366 : /* A crossing is detected. The new connection is not valid, because
2367 : * the connections were ordered in order of preference (arc_weighted) */
2368 3252 : new_connection_is_valid = 0;
2369 3252 : move16();
2370 : }
2371 : }
2372 : }
2373 115373 : c = add( c, 1 );
2374 : }
2375 :
2376 : /* Store the new connection which has been confirmed valid */
2377 19956 : IF( new_connection_is_valid > 0 )
2378 : {
2379 16704 : connections[*connection_write_index][0] = chA;
2380 16704 : move16();
2381 16704 : connections[*connection_write_index][1] = chB;
2382 16704 : move16();
2383 16704 : connection_arc_fx[*connection_write_index] = new_arc_fx; /* Q29 */
2384 16704 : move16();
2385 16704 : connection_cross_fx[*connection_write_index][0] = new_cross_fx[0]; /* Q29 */
2386 16704 : move16();
2387 16704 : connection_cross_fx[*connection_write_index][1] = new_cross_fx[1]; /* Q29 */
2388 16704 : move16();
2389 16704 : connection_cross_fx[*connection_write_index][2] = new_cross_fx[2]; /* Q29 */
2390 16704 : move16();
2391 16704 : *connection_write_index = add( *connection_write_index, 1 );
2392 : }
2393 19956 : c_opt = add( c_opt, 1 );
2394 : }
2395 :
2396 1792 : free( connection_options );
2397 :
2398 1792 : return IVAS_ERR_OK;
2399 : }
2400 :
2401 : /*-------------------------------------------------------------------------*
2402 : * determine_non_crossing_planes()
2403 : *
2404 : * Determine non-crossing planes
2405 : *-------------------------------------------------------------------------*/
2406 :
2407 : /*! r: number of non-crossing planes */
2408 :
2409 896 : static Word16 determine_non_crossing_planes_fx(
2410 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
2411 : const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */
2412 : Word32 *non_crossing_plane_elevation_deg_fx /* o : vector of non-crossing plane elevations Q14 */
2413 : )
2414 : {
2415 : Word32 next_ele_check_fx; /* Q14 */
2416 : Word32 ele_check_fx; /* Q14 */
2417 : Word32 max_gap_fx; /* Q14 */
2418 : Word32 gap_to_next_ls_fx; /* Q14 */
2419 :
2420 : Word16 ch, ch_cmp;
2421 : Word16 num_planes;
2422 :
2423 896 : ele_check_fx = -16382362; /*Q14*/
2424 896 : move32();
2425 896 : num_planes = 0;
2426 896 : move16();
2427 :
2428 : /* For each plane, check if a non-crossing plane should be determined */
2429 3316 : WHILE( LT_32( ele_check_fx, 1474560 ) /*90.0f Q14*/ )
2430 : {
2431 2420 : next_ele_check_fx = 16382362; /*Q14*/
2432 2420 : move32();
2433 :
2434 : Word32 tmp1, tmp2;
2435 :
2436 : /* Find next node elevation that is not in horizontal plane */
2437 27494 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
2438 : {
2439 25074 : tmp1 = L_add( ele_check_fx, 16 /*VBAP_EPSILON in Q14*/ );
2440 25074 : tmp2 = L_sub( next_ele_check_fx, 16 /*VBAP_EPSILON in Q14*/ );
2441 25074 : test();
2442 25074 : test();
2443 25074 : 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 ) )
2444 : {
2445 3049 : next_ele_check_fx = L_shr( node_data[ch].ele_deg_fx, 8 ); // shift due to comparision with 90.0f
2446 : }
2447 : }
2448 2420 : ele_check_fx = next_ele_check_fx; // Q14
2449 2420 : move32();
2450 :
2451 2420 : IF( GT_32( ele_check_fx, 1474560 ) /*90.0f in Q14*/ )
2452 : {
2453 : /* When no next node elevation found, break loop */
2454 0 : BREAK;
2455 : }
2456 :
2457 2420 : max_gap_fx = -163838368; // Q14
2458 2420 : move32();
2459 27494 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
2460 : {
2461 : /* Find gap to the next speaker node at the same plane */
2462 25074 : IF( LT_32( L_abs( L_sub( L_shr( node_data[ch].ele_deg_fx, 8 ), ele_check_fx ) ), 16 /*0.001f in Q14*/ ) )
2463 : {
2464 3900 : gap_to_next_ls_fx = 1638398336; // Q14
2465 3900 : move32();
2466 46020 : FOR( ch_cmp = 0; ch_cmp < num_speaker_nodes; ch_cmp++ )
2467 : {
2468 42120 : test();
2469 42120 : 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*/ ) )
2470 : {
2471 5516 : Word32 gap_fx = L_sub( node_data[ch_cmp].azi_deg_fx, node_data[ch].azi_deg_fx );
2472 8274 : WHILE( gap_fx < 0 )
2473 : {
2474 2758 : gap_fx = L_add( gap_fx, 1509949440 /*360.0f in Q22*/ );
2475 : }
2476 5516 : IF( LT_32( L_shr( gap_fx, 8 ), gap_to_next_ls_fx ) )
2477 : {
2478 3812 : gap_to_next_ls_fx = L_shr( gap_fx, 8 );
2479 : }
2480 : }
2481 : }
2482 : /* Find maximum gap on that plane */
2483 3900 : IF( GT_32( gap_to_next_ls_fx, max_gap_fx ) )
2484 : {
2485 2648 : max_gap_fx = gap_to_next_ls_fx;
2486 2648 : move32();
2487 : }
2488 : }
2489 : }
2490 :
2491 : /* If maximum gap is small enough, then a non-crossing plane is detected */
2492 2420 : test();
2493 2420 : IF( LT_32( max_gap_fx, 2293776 /*Q14*/ ) && max_gap_fx > 0 )
2494 : {
2495 426 : non_crossing_plane_elevation_deg_fx[num_planes] = ele_check_fx; /* Q14 */
2496 426 : move32();
2497 426 : num_planes = add( num_planes, 1 );
2498 426 : IF( EQ_16( num_planes, VBAP_MAX_PLANES ) )
2499 : {
2500 : /* Memory init limit. Does not happen with any real speaker node configuration.
2501 : Triangulation succeeds even if number of non_crossing_planes are limited. */
2502 0 : BREAK;
2503 : }
2504 : }
2505 : }
2506 :
2507 896 : return num_planes;
2508 : }
2509 :
2510 : /*-------------------------------------------------------------------------*
2511 : * reorder_triplets()
2512 : *
2513 : * Reorder virtual surface triplets into provided target order.
2514 : *-------------------------------------------------------------------------*/
2515 :
2516 1792 : static void reorder_triplets_fx(
2517 : VBAP_VS_TRIPLET *triplets, /* i/o: VS triplets to be reordered */
2518 : const Word16 *target_order, /* i : Target order for VS triplets */
2519 : const Word16 num_triplets /* i : Number of VS triplets */
2520 : )
2521 : {
2522 : VBAP_VS_TRIPLET tempTriplets[VBAP_MAX_NUM_TRIPLETS];
2523 : Word16 c;
2524 :
2525 : /* First copy to temp array */
2526 16388 : FOR( c = 0; c < num_triplets; c++ )
2527 : {
2528 14596 : tempTriplets[c] = triplets[c];
2529 : }
2530 :
2531 : /* Then move back in sorted order */
2532 16388 : FOR( c = 0; c < num_triplets; c++ )
2533 : {
2534 14596 : triplets[c] = tempTriplets[target_order[c]];
2535 : }
2536 :
2537 1792 : return;
2538 : }
|
