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 2256 : void vbap_free_data_fx(
444 : VBAP_HANDLE *hVBAPdata /* i/o: VBAP handle to be freed */
445 : )
446 : {
447 2256 : test();
448 2256 : IF( hVBAPdata == NULL || *hVBAPdata == NULL )
449 : {
450 1360 : 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 5123994 : 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 5123994 : push_wmops( "vbap_gains" );
529 5123994 : num_speaker_nodes = hVBAPdata->num_speaker_nodes;
530 5123994 : move16();
531 5123994 : bottom_virtual_speaker_node_index = hVBAPdata->bottom_virtual_speaker_node_index;
532 5123994 : move16();
533 5123994 : top_virtual_speaker_node_index = hVBAPdata->top_virtual_speaker_node_index;
534 5123994 : move16();
535 5123994 : back_virtual_speaker_node_index = hVBAPdata->back_virtual_speaker_node_index;
536 5123994 : move16();
537 :
538 5123994 : IF( use_object_mode )
539 : {
540 34768 : bottom_virtual_speaker_node_division_gains_fx = hVBAPdata->object_mode_bottom_virtual_speaker_node_division_gains_fx;
541 34768 : top_virtual_speaker_node_division_gains_fx = hVBAPdata->object_mode_top_virtual_speaker_node_division_gains_fx;
542 34768 : back_virtual_speaker_node_division_gains_fx = hVBAPdata->object_mode_back_virtual_speaker_node_division_gains_fx;
543 : }
544 : ELSE
545 : {
546 5089226 : bottom_virtual_speaker_node_division_gains_fx = hVBAPdata->bottom_virtual_speaker_node_division_gains_fx;
547 5089226 : top_virtual_speaker_node_division_gains_fx = hVBAPdata->top_virtual_speaker_node_division_gains_fx;
548 5089226 : back_virtual_speaker_node_division_gains_fx = hVBAPdata->back_virtual_speaker_node_division_gains_fx;
549 : }
550 :
551 5123994 : panning_wrap_angles_fx( L_shl( azi_deg, Q22 ), L_shl( ele_deg, Q22 ), &azi_temp_fx, &ele_temp_fx );
552 5123994 : azi_norm = extract_l( L_shr( Mpy_32_32( azi_temp_fx, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
553 5123994 : ele_norm = extract_l( L_shr( Mpy_32_32( ele_temp_fx, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
554 :
555 5123994 : panning_unit_vec_fx[0] = mult( getCosWord16R2( azi_norm ), getCosWord16R2( ele_norm ) ); /* Q15 */
556 5123994 : panning_unit_vec_fx[1] = mult( getSineWord16R2( azi_norm ), getCosWord16R2( ele_norm ) ); /* Q15 */
557 5123994 : panning_unit_vec_fx[2] = getSineWord16R2( ele_norm ); /* Q15 */
558 5123994 : move16();
559 5123994 : move16();
560 5123994 : move16();
561 :
562 :
563 : /* Find the best VS triplet and speaker node gains for the panning direction using the prepared search structures. */
564 5123994 : test();
565 5123994 : IF( EQ_16( hVBAPdata->num_search_structs, 2 ) && ele_deg > 0 )
566 : {
567 2176777 : triplet_index = determine_best_triplet_and_gains_fx( &( hVBAPdata->search_struct[1] ), panning_unit_vec_fx, azi_deg, gain_triplet_fx );
568 2176777 : selected_triplet = &hVBAPdata->search_struct[1].triplets[triplet_index];
569 : }
570 : ELSE
571 : {
572 2947217 : triplet_index = determine_best_triplet_and_gains_fx( &( hVBAPdata->search_struct[0] ), panning_unit_vec_fx, azi_deg, gain_triplet_fx );
573 2947217 : selected_triplet = &hVBAPdata->search_struct[0].triplets[triplet_index];
574 : }
575 :
576 : /* Normalize to unit energy */
577 5123994 : gain_ene_fx = 1; /* Add small value to avoid divide by zero. */
578 5123994 : move32();
579 20495976 : FOR( ch = 0; ch < 3; ch++ )
580 : {
581 15371982 : 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 5123994 : 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 20495976 : FOR( ch = 0; ch < 3; ch++ )
587 : {
588 15371982 : 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 15371982 : move32();
590 :
591 : /* Sanity check for rounding issues */
592 15371982 : if ( gain_triplet_fx[ch] < 0 )
593 : {
594 377 : gain_triplet_fx[ch] = 0;
595 377 : move32();
596 : }
597 : }
598 :
599 : /* Flush gain target */
600 5123994 : 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 20495976 : FOR( ch = 0; ch < 3; ch++ )
604 : {
605 15371982 : triplet_ch = selected_triplet->speaker_node[ch];
606 :
607 15371982 : IF( EQ_16( triplet_ch, bottom_virtual_speaker_node_index ) )
608 : {
609 21888245 : FOR( ch2 = 0; ch2 < num_speaker_nodes; ch2++ )
610 : {
611 19355529 : 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 19355529 : move32();
613 : }
614 : }
615 12839266 : ELSE IF( EQ_16( triplet_ch, top_virtual_speaker_node_index ) )
616 : {
617 11749188 : FOR( ch2 = 0; ch2 < num_speaker_nodes; ch2++ )
618 : {
619 10308914 : 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 10308914 : move32();
621 : }
622 : }
623 11398992 : 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 11398992 : gains_fx[triplet_ch] = L_add( gains_fx[triplet_ch], L_shl( gain_triplet_fx[ch], Q13 ) ); /* Q16 + Q13 = Q29 */
634 11398992 : move32();
635 : }
636 : }
637 :
638 5123994 : pop_wmops();
639 :
640 5123994 : 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 17459386 : 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 17459386 : Word32 pointzero_one = Mpy_32_16_1( L_lshl( 1, q_matrix ), -327 /* -0.01 in Q15 */ );
685 17459386 : 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 17459386 : move32();
687 :
688 17459386 : IF( LT_32( result[0], pointzero_one ) )
689 : {
690 1793413 : return 0;
691 : }
692 :
693 15665973 : 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 15665973 : move32();
695 :
696 15665973 : IF( LT_32( result[1], pointzero_one ) )
697 : {
698 6288149 : return 0;
699 : }
700 :
701 9377824 : 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 9377824 : move32();
703 :
704 9377824 : IF( LT_32( result[2], pointzero_one ) )
705 : {
706 4196367 : return 0;
707 : }
708 5181457 : 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 5123994 : 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 5123994 : num_triplets = search_struct->num_triplets;
781 5123994 : move16();
782 5123994 : best_min_gain_fx = -2147483647;
783 5123994 : move32();
784 5123994 : best_triplet = 0;
785 5123994 : move16();
786 5123994 : 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 5123994 : IF( GT_16( abs_s( azi_deg ), 90 ) )
791 : {
792 2351110 : sector = 1;
793 2351110 : move16();
794 2351110 : if ( azi_deg < 0 )
795 : {
796 370421 : sector = 2;
797 370421 : move16();
798 : }
799 : }
800 : ELSE
801 : {
802 2772884 : sector = 0;
803 2772884 : move16();
804 2772884 : if ( azi_deg < 0 )
805 : {
806 1279976 : sector = 3;
807 1279976 : move16();
808 : }
809 : }
810 5123994 : first_triplet = search_struct->initial_search_indices[sector];
811 5123994 : move16();
812 :
813 5123994 : tr = first_triplet;
814 5123994 : move16();
815 5123994 : jump = 1;
816 5123994 : move16();
817 17459763 : FOR( i = 0; i < num_triplets; i++ )
818 : {
819 17459386 : 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 17459386 : IF( triplet_ok )
821 : {
822 5181457 : min_gain_this_fx = L_min( ( L_min( unnormalized_gains_fx[0], unnormalized_gains_fx[1] ) ), unnormalized_gains_fx[2] );
823 :
824 5181457 : IF( GT_32( min_gain_this_fx, best_min_gain_fx ) )
825 : {
826 5179748 : best_min_gain_fx = min_gain_this_fx;
827 5179748 : move32();
828 5179748 : best_triplet = tr;
829 5179748 : move16();
830 20718992 : FOR( k = 0; k < 3; k++ )
831 : {
832 15539244 : gains_fx[k] = unnormalized_gains_fx[k]; /* Q(VBAP_VS_TRIPLET.q_inverse_matrix) */
833 15539244 : move32();
834 : }
835 5179748 : IF( best_min_gain_fx >= 0 )
836 : {
837 5123617 : return best_triplet;
838 : }
839 : }
840 : }
841 12335769 : tr = add( first_triplet, jump );
842 12335769 : IF( tr < 0 )
843 : {
844 89724 : tr = add( tr, num_triplets );
845 : }
846 12246045 : ELSE IF( GE_16( tr, num_triplets ) )
847 : {
848 2853352 : tr = sub( tr, num_triplets );
849 : }
850 :
851 12335769 : jump = negate( jump );
852 12335769 : IF( jump > 0 )
853 : {
854 5181276 : jump = add( jump, 1 );
855 : }
856 : }
857 :
858 377 : 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 :
1233 14596 : vbap_crossp_fx( input_matrix_fx[1], input_matrix_fx[2], cross_vec_fx );
1234 14596 : determinant_fx = dotp_fx32( input_matrix_fx[0], cross_vec_fx, 3 );
1235 14596 : Word16 inv_mat_exp = 0;
1236 14596 : move16();
1237 :
1238 58384 : FOR( k = 0; k < 3; k++ )
1239 : {
1240 43788 : inverse_matrix_fx[k][0] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) );
1241 43788 : move32();
1242 43788 : inv_mat_exp = add( inv_mat_exp, ( ( Q31 - Q29 ) - ( Q31 - Q28 ) ) );
1243 43788 : exp_inverse_matrix_fx[k][0] = inv_mat_exp;
1244 43788 : move16();
1245 : }
1246 :
1247 14596 : vbap_crossp_fx( input_matrix_fx[2], input_matrix_fx[0], cross_vec_fx );
1248 :
1249 58384 : FOR( k = 0; k < 3; k++ )
1250 : {
1251 43788 : inverse_matrix_fx[k][1] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) );
1252 43788 : move32();
1253 43788 : inv_mat_exp = add( inv_mat_exp, ( ( Q31 - Q29 ) - ( Q31 - Q28 ) ) );
1254 43788 : exp_inverse_matrix_fx[k][1] = inv_mat_exp;
1255 43788 : move16();
1256 : }
1257 :
1258 14596 : vbap_crossp_fx( input_matrix_fx[0], input_matrix_fx[1], cross_vec_fx );
1259 :
1260 58384 : FOR( k = 0; k < 3; k++ )
1261 : {
1262 43788 : inverse_matrix_fx[k][2] = L_deposit_h( BASOP_Util_Divide3232_Scale( cross_vec_fx[k], determinant_fx, &inv_mat_exp ) );
1263 43788 : move32();
1264 43788 : inv_mat_exp = add( inv_mat_exp, ( ( Q31 - Q29 ) - ( Q31 - Q28 ) ) );
1265 43788 : exp_inverse_matrix_fx[k][2] = inv_mat_exp;
1266 43788 : move16();
1267 : }
1268 :
1269 : /*make common exponant*/
1270 14596 : Word16 max_exp = 0, i, j;
1271 58384 : FOR( i = 0; i < 3; i++ )
1272 : {
1273 175152 : FOR( j = 0; j < 3; j++ )
1274 : {
1275 131364 : max_exp = s_max( max_exp, exp_inverse_matrix_fx[i][j] );
1276 : }
1277 : }
1278 :
1279 14596 : *exp_inv_mat = add( max_exp, 1 );
1280 14596 : move16();
1281 :
1282 58384 : FOR( i = 0; i < 3; i++ )
1283 : {
1284 175152 : FOR( j = 0; j < 3; j++ )
1285 : {
1286 131364 : test();
1287 131364 : IF( LT_16( exp_inverse_matrix_fx[i][j], -15 ) && inverse_matrix_fx[i][j] != 0 )
1288 : {
1289 0 : inverse_matrix_fx[i][j] = 1;
1290 0 : move32();
1291 0 : exp_inverse_matrix_fx[i][j] = 0;
1292 0 : move16();
1293 : }
1294 : ELSE
1295 : {
1296 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) */
1297 131364 : move32();
1298 : }
1299 : }
1300 : }
1301 :
1302 14596 : return;
1303 : }
1304 : /*-------------------------------------------------------------------------*
1305 : * check_and_store_triplet()
1306 : *
1307 : * Check if the given loudspeaker triplet is a valid one and store data when
1308 : * valid triplet is found.
1309 : *-------------------------------------------------------------------------*/
1310 :
1311 14596 : static Word16 check_and_store_triplet_fx(
1312 : const Word16 chA, /* i : first channel index that forms the loudspeaker triplet */
1313 : const Word16 chB, /* i : second channel index that forms the loudspeaker triplet */
1314 : const Word16 chC, /* i : third channel index that forms the loudspeaker triplet */
1315 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1316 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */
1317 : VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */
1318 : Word16 *triplet_index, /* i/o: index for the next free triplet slot */
1319 : Word32 *triplet_azidegs_fx, /* o : center azimuths of the found triplets Q19 */
1320 : Word16 *triplet_order /* o : initial order of triplet indices */
1321 : )
1322 : {
1323 : Word16 ch_check;
1324 : Word16 k;
1325 : Word16 speaker_node_found_inside_triplet;
1326 : UWord8 triplet_ok;
1327 : Word16 exp_inv_mat;
1328 :
1329 : Word32 inverse_matrix_fx[3][3] /* Q(31 - exp_inv_mat) */, unnormalized_gains_fx[3] /* Q(31 - exp_inv_mat - 1) */;
1330 14596 : set32_fx( unnormalized_gains_fx, 0, 3 );
1331 : const Word32 *speaker_node_triplet_unit_vec_matrix_fx[3]; /* Q30 */
1332 :
1333 : /* Triplet found, determine inverse matrix for VBAP formulation */
1334 :
1335 14596 : speaker_node_triplet_unit_vec_matrix_fx[0] = speaker_node_data[chA].unit_vec_fx;
1336 14596 : move32();
1337 14596 : speaker_node_triplet_unit_vec_matrix_fx[1] = speaker_node_data[chB].unit_vec_fx;
1338 14596 : move32();
1339 14596 : speaker_node_triplet_unit_vec_matrix_fx[2] = speaker_node_data[chC].unit_vec_fx;
1340 14596 : move32();
1341 14596 : matrix_inverse_3x3_32_fx( speaker_node_triplet_unit_vec_matrix_fx, inverse_matrix_fx, &exp_inv_mat );
1342 14596 : triplets[*triplet_index].q_inverse_matrix = sub( 31, exp_inv_mat );
1343 14596 : move16();
1344 :
1345 : /* Check through all speaker nodes that none of them are within the triplet.
1346 : * Node within the triplet is identified by that all three panning gains are positive.
1347 : * Epsilon-condition is for some small rounding issues.*/
1348 14596 : speaker_node_found_inside_triplet = 0;
1349 14596 : move16();
1350 169208 : FOR( ch_check = 0; ch_check < num_speaker_nodes; ch_check++ )
1351 : {
1352 154612 : test();
1353 154612 : test();
1354 154612 : IF( ( NE_16( ch_check, chA ) ) && NE_16( ch_check, chB ) && NE_16( ch_check, chC ) )
1355 : {
1356 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 ) );
1357 110824 : test();
1358 110824 : test();
1359 110824 : test();
1360 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 ) ) ) ) )
1361 : {
1362 0 : speaker_node_found_inside_triplet = 1;
1363 0 : move16();
1364 0 : BREAK;
1365 : }
1366 : }
1367 : }
1368 :
1369 : /* No speaker node inside triplet -> appropriate triplet found, save data. */
1370 14596 : IF( speaker_node_found_inside_triplet == 0 )
1371 : {
1372 14596 : triplets[*triplet_index].speaker_node[0] = (UWord8) chA;
1373 14596 : triplets[*triplet_index].speaker_node[1] = (UWord8) chB;
1374 14596 : triplets[*triplet_index].speaker_node[2] = (UWord8) chC;
1375 14596 : move16();
1376 14596 : move16();
1377 14596 : move16();
1378 58384 : FOR( k = 0; k < 3; k++ )
1379 : {
1380 43788 : Copy32( inverse_matrix_fx[k], triplets[*triplet_index].inverse_matrix_fx[k], 3 );
1381 : }
1382 : /* Get center azimuth for fast search use */
1383 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 */
1384 : /*Condition to make tmp_a 0 to adress precision loss seen*/
1385 14596 : if ( EQ_32( tmp_a, -8193 /* -0.0000305 in Q28 */ ) )
1386 : {
1387 0 : tmp_a = 0;
1388 0 : move32();
1389 : }
1390 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 */
1391 14596 : Word16 tmp_tan = shr( BASOP_util_atan2( tmp_a, tmp_b, 0 ), Q13 - Q9 ); /* Q9 */
1392 14596 : triplet_azidegs_fx[*triplet_index] = L_mult( tmp_tan, 29335 /*_180_OVER_PI in Q9*/ ); /* Q19 */
1393 14596 : move32();
1394 : /* Store increasing order indices for the later sorting step. */
1395 14596 : triplet_order[*triplet_index] = *triplet_index;
1396 14596 : move16();
1397 :
1398 14596 : *triplet_index = add( *triplet_index, 1 );
1399 14596 : move16();
1400 :
1401 14596 : return 1;
1402 : }
1403 :
1404 : /* Triplet was not good */
1405 0 : return 0;
1406 : }
1407 : /*-------------------------------------------------------------------------*
1408 : * determine_virtual_surface_triplets()
1409 : *
1410 : * Determine virtual surface triples that are used for panning. This
1411 : * function is optimized for the use in cases where speaker nodes are in
1412 : * one group or divided into two separate groups divided by a horizontal
1413 : * layer.
1414 : *-------------------------------------------------------------------------*/
1415 :
1416 : /*! r: number of virtual surface triplets */
1417 :
1418 1792 : static Word16 determine_virtual_surface_triplets_fx(
1419 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1420 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data structure */
1421 : Word16 connections[][2], /* i : vector of all connections */
1422 : const Word16 max_num_connections, /* i : max number of connections */
1423 : VBAP_VS_TRIPLET *triplets, /* o : vector of virtual surface triplets */
1424 : Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS], /* o : initial search indices for this set of triplets corresponding to the search struct */
1425 : enum SpeakerNodeGroup allowed_group /* i : group of allowed speaker nodes for forming the triplets in this call */
1426 : )
1427 : {
1428 : Word16 chA, chB, chC, k, l, m;
1429 1792 : Word16 num_triplets = 0;
1430 1792 : move16();
1431 : Word16 num_connected_to_chA;
1432 : Word16 connected_to_chA[VBAP_MAX_NUM_SPEAKER_NODES];
1433 : Word16 connection_uses_left[VBAP_MAX_NUM_SPEAKER_NODES];
1434 : Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS]; /* Q19 */
1435 : Word16 triplet_order[VBAP_MAX_NUM_TRIPLETS];
1436 :
1437 : /* Each connection can be used exactly by two different virtual surface triplets. */
1438 1792 : set16_fx( connection_uses_left, 2, VBAP_MAX_NUM_SPEAKER_NODES );
1439 :
1440 19972 : FOR( chA = 0; chA < num_speaker_nodes; chA++ )
1441 : {
1442 : /* Early skip if not in correct group. */
1443 18180 : IF( NE_16( speaker_node_data[chA].group, allowed_group ) )
1444 : {
1445 14280 : CONTINUE;
1446 : }
1447 :
1448 : /* Get all connections connected to current chA that have not been used by
1449 : * two triplets yet. */
1450 3900 : num_connected_to_chA = 0;
1451 3900 : move16();
1452 77992 : FOR( k = 0; k < max_num_connections; k++ )
1453 : {
1454 74092 : test();
1455 74092 : test();
1456 74092 : IF( ( EQ_16( connections[k][0], chA ) || EQ_16( connections[k][1], chA ) ) && ( connection_uses_left[k] > 0 ) )
1457 : {
1458 16054 : connected_to_chA[num_connected_to_chA] = k;
1459 16054 : move16();
1460 16054 : num_connected_to_chA = add( num_connected_to_chA, 1 );
1461 : }
1462 : }
1463 :
1464 : /* Check that we have enough connections to use. We need at least two available connections to form a triplet.
1465 : * This can fail in later stages when all connections are already used. */
1466 3900 : IF( LT_16( num_connected_to_chA, 2 ) )
1467 : {
1468 650 : CONTINUE;
1469 : }
1470 :
1471 : /* Try to form triplets from each valid connection. */
1472 19304 : FOR( k = 0; k < num_connected_to_chA; k++ )
1473 : {
1474 16054 : Word16 connect_index_k = connected_to_chA[k];
1475 16054 : move16();
1476 16054 : chB = connections[connect_index_k][0];
1477 16054 : move16();
1478 16054 : if ( EQ_16( connections[connect_index_k][0], chA ) )
1479 : {
1480 3588 : chB = connections[connect_index_k][1];
1481 3588 : move16();
1482 : }
1483 27843 : FOR( l = k + 1; l < num_connected_to_chA; l++ )
1484 : {
1485 24593 : Word16 connect_index_l = connected_to_chA[l];
1486 24593 : move16();
1487 24593 : chC = connections[connect_index_l][0];
1488 24593 : move16();
1489 24593 : if ( EQ_16( connections[connect_index_l][0], chA ) )
1490 : {
1491 6930 : chC = connections[connect_index_l][1];
1492 6930 : move16();
1493 : }
1494 :
1495 : /* With chA, chB, and chC selected, we still need to find connection between chB and chC and verify that the triplet is valid */
1496 301225 : FOR( m = 0; m < max_num_connections; m++ )
1497 : {
1498 291228 : test();
1499 291228 : test();
1500 291228 : test();
1501 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 ) ) )
1502 : {
1503 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 );
1504 14596 : IF( EQ_16( flag, 1 ) )
1505 : {
1506 14596 : connection_uses_left[connect_index_k] = sub( connection_uses_left[connect_index_k], 1 );
1507 14596 : move16();
1508 14596 : connection_uses_left[connect_index_l] = sub( connection_uses_left[connect_index_l], 1 );
1509 14596 : move16();
1510 14596 : connection_uses_left[m] = sub( connection_uses_left[m], 1 );
1511 14596 : move16();
1512 14596 : BREAK;
1513 : }
1514 : }
1515 : }
1516 :
1517 : /* Check if chA-chB connection has been used in two triplets already. If yes, then break out of loop
1518 : * as this connection cannot be used for more triplets and we need to continue with another chA-chB
1519 : * connection. */
1520 24593 : IF( LT_16( connection_uses_left[connect_index_k], 1 ) )
1521 : {
1522 12804 : BREAK;
1523 : }
1524 : }
1525 : }
1526 : }
1527 :
1528 : /* All triplets should be stored now. Sort them for search use and then determine the initial search indices for
1529 : * each search sector for this search struct. */
1530 1792 : v_sort_ind_fx( triplet_azidegs_fx, triplet_order, num_triplets );
1531 1792 : reorder_triplets_fx( triplets, triplet_order, num_triplets );
1532 1792 : determine_initial_search_indices_fx( num_triplets, triplet_azidegs_fx, initial_search_indices );
1533 :
1534 1792 : return num_triplets;
1535 : }
1536 :
1537 :
1538 : /*-------------------------------------------------------------------------*
1539 : * determine_initial_search_indices()
1540 : *
1541 : * Determine initial search indices used for fast search of correct triangle
1542 : *-------------------------------------------------------------------------*/
1543 :
1544 1792 : static void determine_initial_search_indices_fx(
1545 : const Word16 num_triplets, /* i : number of triplets */
1546 : const Word32 triplet_azidegs_fx[VBAP_MAX_NUM_TRIPLETS], /* i : azimuths of triplets (in degrees) Q19 */
1547 : Word16 initial_search_indices[VBAP_NUM_SEARCH_SECTORS] /* o : initial search indices */
1548 : )
1549 : {
1550 : Word16 i, j;
1551 : Word32 sector_reference_azideg_fx; /* Q0 */
1552 : Word32 sector_border_start_azideg_fx; /* Q0 */
1553 : Word32 sector_border_end_azideg_fx; /* Q0 */
1554 : Word16 best_index;
1555 : Word32 min_azideg_diff_fx;
1556 : Word32 azideg_diff_fx; /* Q19 */
1557 :
1558 8960 : FOR( i = 0; i < VBAP_NUM_SEARCH_SECTORS; i++ )
1559 : {
1560 7168 : sector_border_start_azideg_fx = imult3216( i, VBAP_SEARCH_SECTOR_SIZE_Q0 );
1561 7168 : sector_border_end_azideg_fx = imult3216( add( i, 1 ), VBAP_SEARCH_SECTOR_SIZE_Q0 );
1562 7168 : sector_reference_azideg_fx = L_shr( L_add( sector_border_start_azideg_fx, sector_border_end_azideg_fx ), 1 );
1563 :
1564 :
1565 7168 : best_index = 0;
1566 7168 : move16();
1567 7168 : min_azideg_diff_fx = ONE_IN_Q31;
1568 7168 : move32();
1569 :
1570 65552 : FOR( j = 0; j < num_triplets; j++ )
1571 : {
1572 58384 : azideg_diff_fx = L_sub( L_shl( sector_reference_azideg_fx, Q19 ), triplet_azidegs_fx[j] );
1573 :
1574 58384 : IF( GT_32( azideg_diff_fx, 94371840 /* 180.0f in Q19 */ ) )
1575 : {
1576 25380 : azideg_diff_fx = L_sub( azideg_diff_fx, 188743680 /* 360.0f in Q19 */ );
1577 : }
1578 33004 : ELSE IF( LT_32( azideg_diff_fx, -94371840 /* -180.0f in Q19 */ ) )
1579 : {
1580 0 : azideg_diff_fx = L_add( azideg_diff_fx, 188743680 /* 360.0f in Q19 */ );
1581 : }
1582 58384 : azideg_diff_fx = L_abs( azideg_diff_fx );
1583 :
1584 58384 : IF( LT_32( azideg_diff_fx, min_azideg_diff_fx ) )
1585 : {
1586 24506 : min_azideg_diff_fx = azideg_diff_fx;
1587 24506 : move32();
1588 24506 : best_index = j;
1589 24506 : move16();
1590 : }
1591 : }
1592 :
1593 7168 : initial_search_indices[i] = best_index;
1594 7168 : move16();
1595 : }
1596 :
1597 1792 : return;
1598 : }
1599 :
1600 :
1601 : /*-------------------------------------------------------------------------*
1602 : * determine_connections()
1603 : *
1604 : * Determine all valid connections between all speaker nodes
1605 : *-------------------------------------------------------------------------*/
1606 :
1607 896 : static ivas_error determine_connections_fx(
1608 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1609 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
1610 : Word16 connections[][2], /* o : vector of connections */
1611 : const Word16 max_num_connections, /* i : max number of connections */
1612 : Word16 *group1_count, /* o : number of connections in first group */
1613 : Word16 *group2_start, /* o : start of second group of connections */
1614 : Word16 *group2_count /* o : number of connections in second group */
1615 : )
1616 : {
1617 : Word16 num_non_crossing_planes;
1618 : Word16 c;
1619 896 : Word16 connection_write_index = 0;
1620 896 : move16();
1621 : Word32 non_crossing_plane_elevation_deg_fx[VBAP_MAX_PLANES]; /* Q14 */
1622 :
1623 : ivas_error error;
1624 :
1625 896 : set32_fx( non_crossing_plane_elevation_deg_fx, 0, VBAP_MAX_PLANES );
1626 :
1627 : /* Reset connection data */
1628 22790 : FOR( c = 0; c < max_num_connections; c++ )
1629 : {
1630 21894 : connections[c][0] = VBAP_NOT_VALID_CONNECTION;
1631 21894 : move16();
1632 : }
1633 :
1634 : /* This function determines some prominent elevated planes, that are favoured in making node-node connections. */
1635 896 : num_non_crossing_planes = determine_non_crossing_planes_fx( num_speaker_nodes, speaker_node_data, non_crossing_plane_elevation_deg_fx );
1636 :
1637 : /* Process in different mode based on the grouping. It is enough to check for first node. */
1638 896 : IF( EQ_16( speaker_node_data[0].group, SPEAKER_NODE_ALL ) )
1639 : {
1640 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 ) )
1641 : {
1642 0 : return error;
1643 : }
1644 : }
1645 : ELSE
1646 : {
1647 : /* The node-node connections are determined in three stages: bottom, horizontal, and top. */
1648 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 ) )
1649 : {
1650 0 : return error;
1651 : }
1652 896 : *group2_start = connection_write_index;
1653 896 : move16();
1654 :
1655 896 : formulate_horizontal_connections_fx( speaker_node_data, num_speaker_nodes, connections, &connection_write_index );
1656 896 : *group1_count = connection_write_index;
1657 896 : move16();
1658 :
1659 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 ) )
1660 : {
1661 0 : return error;
1662 : }
1663 :
1664 896 : *group2_count = sub( connection_write_index, *group2_start );
1665 896 : move16();
1666 : }
1667 :
1668 896 : return IVAS_ERR_OK;
1669 : }
1670 :
1671 :
1672 : /*-------------------------------------------------------------------------*
1673 : * determine_connection_class()
1674 : *
1675 : * Determine the type of connection
1676 : *-------------------------------------------------------------------------*/
1677 :
1678 : /*! r: type of connection */
1679 27314 : static enum ConnectionClass determine_connection_class_fx(
1680 : const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */
1681 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1682 : const enum SpeakerNodeGroup group, /* i : speaker node group */
1683 : const Word16 chA, /* i : speaker node counter 1 */
1684 : const Word16 chB /* i : speaker node counter 2 */
1685 : )
1686 : {
1687 : Word16 ch, k;
1688 : const Word32 *p1_fx, *v2_fx;
1689 : Word32 v1v1_fx, v1v2_fx, v2v2_fx, v1p1_fx, v2p1_fx; /* Q25, Q27, Q29, Q27, Q29 */
1690 : Word32 determinant_fx; /* Q23 */
1691 : Word32 norm_distance_on_v1_fx;
1692 : Word32 vec_diff_fx[3];
1693 : Word32 v1_fx[3]; /* Q28 */
1694 : Word32 vTarget_fx[3];
1695 : Word32 energy_sum_fx;
1696 : Word32 eq_value_fx;
1697 : Word32 uvecdot_fx; /* Q30 */
1698 :
1699 : /* Check if connection passes through origin. This is not desired.
1700 : * When this happens, unit vectors point in opposite directions. */
1701 27314 : uvecdot_fx = L_add( L_shl( dotp_fx32( node_data[chA].unit_vec_fx, node_data[chB].unit_vec_fx, 3 ), 1 ), ONE_IN_Q30 ); // Q30 - adding one guard bit
1702 :
1703 27314 : test();
1704 27314 : IF( LT_32( uvecdot_fx, VBAP_EPSILON_Q3O ) && GT_32( uvecdot_fx, L_negate( VBAP_EPSILON_Q3O ) ) )
1705 : {
1706 0 : return CONNECTION_WITH_SPEAKER_NODE_BEHIND;
1707 : }
1708 :
1709 : /* This loop checks the chA-chB connection with respect to all loudspeakers:
1710 : - in case there is a node behind or nearly behind the connection line. These
1711 : connections need to be discarded.
1712 : - in case there is a node that is closer to the connection line than 1/5 of the
1713 : connection length AND at the same horizontal plane. These connections need to be
1714 : weighted with a penalty (a special case, for example avoiding elevated L,R,C triplet)
1715 : */
1716 317266 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1717 : {
1718 : /* Select speaker_node only within TOP or BOTTOM sphere half, not being part of chA-chB pair */
1719 292852 : test();
1720 292852 : test();
1721 292852 : IF( ( EQ_16( group, node_data[ch].group ) ) && ( NE_16( ch, chA ) ) && ( NE_16( ch, chB ) ) )
1722 : {
1723 : /* The following lines formulate the point on the chA-chB-connection that is
1724 : nearest to the origo-ch-line */
1725 65340 : p1_fx = node_data[chA].unit_vec_fx; // Q30
1726 65340 : move32();
1727 :
1728 261360 : FOR( k = 0; k < 3; k++ )
1729 : {
1730 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) */
1731 196020 : move32();
1732 : }
1733 65340 : v2_fx = node_data[ch].unit_vec_fx; // Q30
1734 65340 : move32();
1735 :
1736 65340 : v1v1_fx = dotp_fx32( v1_fx, v1_fx, 3 ); // Q25
1737 65340 : move32();
1738 65340 : v1v2_fx = dotp_fx32( v1_fx, v2_fx, 3 ); // Q27
1739 65340 : move32();
1740 65340 : v2v2_fx = ONE_IN_Q29;
1741 65340 : move32();
1742 :
1743 65340 : v1p1_fx = dotp_fx32( v1_fx, p1_fx, 3 ); // Q27
1744 65340 : move32();
1745 65340 : v2p1_fx = dotp_fx32( v2_fx, p1_fx, 3 ); // Q29
1746 65340 : move32();
1747 :
1748 65340 : Word32 tmp1 = Mpy_32_32( v1v2_fx, v1v2_fx ); // Q23
1749 65340 : Word32 tmp2 = Mpy_32_32( v1v1_fx, L_negate( v2v2_fx ) ); // Q23
1750 65340 : Word32 tmp3 = L_add( tmp1, tmp2 ); // Q23
1751 :
1752 65340 : determinant_fx = tmp3; // Q23
1753 65340 : move32();
1754 :
1755 : /* Norm distance = distance parameter on line chA-chB, determines point that is
1756 : nearest to origo-ch line. Distance 0 means chA and distance 1 means chB. Can be
1757 : outside this region as well.*/
1758 :
1759 65340 : Word32 tmp4 = Mpy_32_32( v2v2_fx, v1p1_fx ); // q25
1760 65340 : Word32 tmp5 = Mpy_32_32( L_negate( v1v2_fx ), v2p1_fx ); // q25
1761 65340 : Word32 tmp6 = L_add( tmp4, tmp5 ); // q25
1762 65340 : Word16 tmp7, exp = 0;
1763 65340 : move16();
1764 65340 : if ( determinant_fx == 0 )
1765 : {
1766 0 : determinant_fx = 1;
1767 0 : move32();
1768 : }
1769 65340 : tmp7 = BASOP_Util_Divide3232_Scale( ONE_IN_Q23, determinant_fx, &exp ); // Q(15 - exp)
1770 65340 : norm_distance_on_v1_fx = Mpy_32_16_1( tmp6, tmp7 ); // Q25 + Q(15 - exp) - Q15 = Q(25 - exp)
1771 :
1772 : /* Continue only if the nearest point is between chA and chB */
1773 65340 : Word16 exp_vTarget = 0, exp_energy_sum = 0, exp_vec_diff = 0;
1774 : Word32 var1, var2;
1775 : Word16 vTarget_fx_e[3], vec_diff_e[3];
1776 65340 : test();
1777 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 ) ) ) ) )
1778 : {
1779 : /* Formulate vTarget, that is an unit vector that goes through the determined point on chA-chB connection */
1780 53542 : energy_sum_fx = 0;
1781 53542 : move32();
1782 214168 : FOR( k = 0; k < 3; k++ )
1783 : {
1784 160626 : var1 = Mpy_32_32( norm_distance_on_v1_fx, v1_fx[k] ); // Q(25 - exp) + Q28 - 31
1785 160626 : vTarget_fx[k] = BASOP_Util_Add_Mant32Exp( p1_fx[k], 1, var1, sub( Q31, add( sub( Q25, exp ), Q28 - Q31 ) ), &exp_vTarget );
1786 160626 : move16();
1787 160626 : vTarget_fx_e[k] = exp_vTarget;
1788 160626 : move16();
1789 :
1790 160626 : var2 = Mpy_32_32( vTarget_fx[k], vTarget_fx[k] ); // 2*exp_vTarget
1791 160626 : energy_sum_fx = BASOP_Util_Add_Mant32Exp( energy_sum_fx, exp_energy_sum, var2, shl( exp_vTarget, 1 ), &exp_energy_sum );
1792 160626 : vec_diff_fx[k] = BASOP_Util_Add_Mant32Exp( vTarget_fx[k], exp_vTarget, L_negate( v2_fx[k] ), 1, &exp_vec_diff );
1793 160626 : move16();
1794 160626 : vec_diff_e[k] = exp_vec_diff;
1795 160626 : move16();
1796 : }
1797 53542 : Word16 exp_eq = exp_energy_sum;
1798 53542 : move16();
1799 53542 : eq_value_fx = ISqrt32( energy_sum_fx, &exp_eq );
1800 214168 : FOR( k = 0; k < 3; k++ )
1801 : {
1802 160626 : vTarget_fx[k] = Mpy_32_32( vTarget_fx[k], eq_value_fx );
1803 160626 : move32();
1804 160626 : vTarget_fx_e[k] = add( vTarget_fx_e[k], exp_eq );
1805 160626 : move16();
1806 : }
1807 : /*make a common exponent*/
1808 53542 : Word16 max_vTarget_e = 0, max_vec_diff_e = 0;
1809 53542 : move16();
1810 53542 : move16();
1811 214168 : FOR( k = 0; k < 3; k++ )
1812 : {
1813 160626 : max_vTarget_e = s_max( vTarget_fx_e[k], max_vTarget_e );
1814 160626 : max_vec_diff_e = s_max( vec_diff_e[k], max_vec_diff_e );
1815 : }
1816 214168 : FOR( k = 0; k < 3; k++ )
1817 : {
1818 160626 : vTarget_fx[k] = L_shr( vTarget_fx[k], sub( max_vTarget_e, vTarget_fx_e[k] ) );
1819 160626 : move32();
1820 160626 : vec_diff_fx[k] = L_shr( vec_diff_fx[k], sub( max_vec_diff_e, vec_diff_e[k] ) );
1821 160626 : move32();
1822 : }
1823 : /* A check if the angle between vTarget and node_data[ch].unit_vec is less than 1 degree.
1824 : Essentially reveals if there is a speaker node too close "behind" the connection. Such
1825 : connections should be rejected.*/
1826 214168 : FOR( Word16 i = 0; i < 3; i++ )
1827 : {
1828 160626 : vTarget_fx[i] = L_shr( vTarget_fx[i], 2 ); // add guard bits
1829 160626 : move32();
1830 160626 : vec_diff_fx[i] = L_shr( vec_diff_fx[i], 2 );
1831 160626 : move32();
1832 : }
1833 53542 : Word32 res = dotp_fx32( vTarget_fx, v2_fx, 3 ); // 31 - (max_vTarget_e + 2) + 30 - 31 = 28 - max_vTarget_e
1834 53542 : move32();
1835 :
1836 53542 : IF( GT_32( res, L_shr( 2147054208 /* 0.9998f in Q31 */, sub( 31, sub( 28, max_vTarget_e ) ) ) ) )
1837 : {
1838 2900 : return CONNECTION_WITH_SPEAKER_NODE_BEHIND;
1839 : }
1840 :
1841 : /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes.
1842 : A triplet between these nodes is not desired if there is a top node,
1843 : a penalty is implemented to take care of this. */
1844 50642 : Word32 vec_diff_dotp = dotp_fx32( vec_diff_fx, vec_diff_fx, 3 ); // exp : 2 * max_vec_diff_e + 4
1845 50642 : move32();
1846 50642 : Word32 var = Mpy_32_32( vec_diff_dotp, 51200 /*25.0f in Q11*/ ); // exp : 2 * max_vec_diff_e + 4 + 20
1847 50642 : Word16 Flag1 = BASOP_Util_Cmp_Mant32Exp( v1v1_fx, Q31 - Q25, var, add( shl( max_vec_diff_e, 1 ), 4 + 20 ) );
1848 50642 : IF( EQ_16( Flag1, 1 ) )
1849 : {
1850 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 */ ) )
1851 : {
1852 0 : return ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION;
1853 : }
1854 : }
1855 : }
1856 : }
1857 : }
1858 :
1859 24414 : return REGULAR_CONNECTION;
1860 : }
1861 :
1862 : /*-------------------------------------------------------------------------*
1863 : * formulate_horizontal_connections()
1864 : *
1865 : * Formulate connections in the horizontal plane
1866 : *-------------------------------------------------------------------------*/
1867 :
1868 896 : static void formulate_horizontal_connections_fx(
1869 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
1870 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1871 : Word16 connections[][2], /* o : vector of all connections */
1872 : Word16 *connection_write_index )
1873 : {
1874 : Word16 ch;
1875 : Word16 chCheck;
1876 : Word16 next_index;
1877 :
1878 : Word32 min_arc_diff_fx;
1879 : Word32 arc_diff_fx;
1880 : Word16 Q_min_arc_diff;
1881 :
1882 9986 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
1883 : {
1884 : /* Find next horizontal speaker node */
1885 9090 : IF( EQ_16( speaker_node_data[ch].group, SPEAKER_NODE_HORIZONTAL ) )
1886 : {
1887 5190 : next_index = -1;
1888 5190 : move16();
1889 5190 : min_arc_diff_fx = 19998 /*9999.0f in Q1*/;
1890 5190 : move32();
1891 5190 : Q_min_arc_diff = 1;
1892 5190 : move16();
1893 :
1894 58556 : FOR( chCheck = 0; chCheck < num_speaker_nodes; chCheck++ )
1895 : {
1896 53366 : test();
1897 53366 : IF( ( NE_16( ch, chCheck ) ) && ( EQ_16( speaker_node_data[chCheck].group, SPEAKER_NODE_HORIZONTAL ) ) )
1898 : {
1899 25728 : arc_diff_fx = L_sub( speaker_node_data[chCheck].azi_deg_fx, speaker_node_data[ch].azi_deg_fx ); // Q22
1900 :
1901 38592 : WHILE( arc_diff_fx < 0 )
1902 : {
1903 12864 : arc_diff_fx = L_add( arc_diff_fx, 1509949440 /*360.0f in Q22*/ );
1904 : }
1905 25728 : IF( LT_32( L_shr( arc_diff_fx, sub( 22, Q_min_arc_diff ) ), min_arc_diff_fx ) )
1906 : {
1907 11649 : min_arc_diff_fx = arc_diff_fx;
1908 11649 : move32();
1909 11649 : Q_min_arc_diff = 22;
1910 11649 : move16();
1911 11649 : next_index = chCheck;
1912 11649 : move16();
1913 : }
1914 : }
1915 : }
1916 5190 : connections[*connection_write_index][0] = ch;
1917 5190 : move16();
1918 5190 : connections[*connection_write_index][1] = next_index;
1919 5190 : move16();
1920 5190 : *connection_write_index = add( *connection_write_index, 1 );
1921 5190 : move16();
1922 : }
1923 : }
1924 :
1925 896 : return;
1926 : }
1927 :
1928 : /*-------------------------------------------------------------------------*
1929 : * check_plane_crossing()
1930 : *
1931 : * Check crossing of non-allowed planes
1932 : *-------------------------------------------------------------------------*/
1933 :
1934 : /*! r: truth value for crossing */
1935 24414 : static Word16 check_plane_crossing_fx(
1936 : const Word32 ele1_deg_fx, /* i : speaker node 1 elevation Q22 */
1937 : const Word32 ele2_deg_fx, /* i : speaker node 2 elevation Q22 */
1938 : const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */
1939 : const Word32 *non_crossing_plane_elevation_deg_fx /* i : vector non-crossing plane elevations Q14 */
1940 : )
1941 : {
1942 : /* Find if the connection crosses a non-crossing plane, with 1-degree threshold. */
1943 : Word16 plane;
1944 :
1945 40510 : FOR( plane = 0; plane < num_non_crossing_planes; plane++ )
1946 : {
1947 16912 : test();
1948 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 ) ) ) )
1949 : {
1950 8 : return 1;
1951 : }
1952 16904 : test();
1953 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 ) ) ) )
1954 : {
1955 808 : return 1;
1956 : }
1957 : }
1958 :
1959 23598 : return 0;
1960 : }
1961 :
1962 : /*-------------------------------------------------------------------------*
1963 : * get_half_sphere_connection_options()
1964 : *
1965 : * Get list of all potential connections at the half-sphere
1966 : *-------------------------------------------------------------------------*/
1967 1792 : static ivas_error get_half_sphere_connection_options_fx(
1968 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
1969 : const enum SpeakerNodeGroup group, /* i : speaker node group */
1970 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
1971 : const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */
1972 : const Word32 *non_crossing_plane_elevation_deg_fx, /* i : vector of non-crossing plane elevations Q14 */
1973 : ConnectionOption **connection_options_pr, /* o : list of connection options */
1974 : Word16 *num_connection_options /* o : number of connection options */
1975 : )
1976 : {
1977 1792 : Word16 max_num_connection_options = 0;
1978 1792 : Word16 index = 0;
1979 : Word16 node, chA, chB, c, c_cmp;
1980 : ConnectionOption *c_options, *c_options_reorder;
1981 :
1982 1792 : move16();
1983 1792 : move16();
1984 :
1985 : /* Count max num connection options at the half sphere */
1986 19972 : FOR( node = 0; node < num_speaker_nodes; node++ )
1987 : {
1988 18180 : test();
1989 18180 : IF( EQ_16( speaker_node_data[node].group, group ) || EQ_16( speaker_node_data[node].group, SPEAKER_NODE_HORIZONTAL ) )
1990 : {
1991 14280 : max_num_connection_options = add( max_num_connection_options, index );
1992 14280 : index = add( index, 1 );
1993 : }
1994 : }
1995 :
1996 : /* Init memory for connection options */
1997 1792 : IF( ( c_options = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * max_num_connection_options ) ) == NULL )
1998 : {
1999 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
2000 : }
2001 :
2002 54834 : FOR( c = 0; c < max_num_connection_options; c++ )
2003 : {
2004 53042 : c_options[c].chA = -1;
2005 53042 : move16();
2006 53042 : c_options[c].chB = -1;
2007 53042 : move16();
2008 53042 : c_options[c].arc_fx = MAX_32;
2009 53042 : move32();
2010 53042 : c_options[c].arc_weighted_fx = MAX_32;
2011 53042 : move32();
2012 : }
2013 :
2014 : /* Determine connection options for the half-sphere */
2015 1792 : index = 0;
2016 1792 : move16();
2017 19972 : FOR( chA = 0; chA < num_speaker_nodes; chA++ )
2018 : {
2019 : /* First loudspeaker at the connection is at the half sphere */
2020 18180 : test();
2021 18180 : IF( EQ_16( speaker_node_data[chA].group, group ) || EQ_16( speaker_node_data[chA].group, SPEAKER_NODE_HORIZONTAL ) )
2022 : {
2023 92790 : FOR( chB = chA + 1; chB < num_speaker_nodes; chB++ )
2024 : {
2025 : /* Second loudspeaker at the connection is at the half sphere, but so that first and second are not both horizontal. */
2026 78510 : test();
2027 78510 : test();
2028 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 ) ) )
2029 : {
2030 27314 : Word16 ConnectionClass = determine_connection_class_fx( speaker_node_data, num_speaker_nodes, group, chA, chB );
2031 : Word32 unit_vec_dotp, unit_vec_dotp_sq, one_minus_unit_vec_dotp_sq, one_minus_unit_vec_dotp_sq_root;
2032 : Word16 acos_val;
2033 : /* Connection is considered only if there is no speaker node behind it */
2034 27314 : IF( NE_16( ConnectionClass, CONNECTION_WITH_SPEAKER_NODE_BEHIND ) )
2035 : {
2036 : /* Store connection information */
2037 24414 : c_options[index].chA = chA;
2038 24414 : move16();
2039 24414 : c_options[index].chB = chB;
2040 24414 : move16();
2041 :
2042 24414 : unit_vec_dotp = dotp_fx32( speaker_node_data[chA].unit_vec_fx, speaker_node_data[chB].unit_vec_fx, 3 ); // Q29
2043 24414 : unit_vec_dotp_sq = Mpy_32_32( unit_vec_dotp, unit_vec_dotp ); // Q27
2044 24414 : one_minus_unit_vec_dotp_sq = L_sub( ONE_IN_Q27, unit_vec_dotp_sq );
2045 24414 : Word16 exp_uv = Q31 - Q27;
2046 24414 : move16();
2047 24414 : one_minus_unit_vec_dotp_sq_root = Sqrt32( one_minus_unit_vec_dotp_sq, &exp_uv );
2048 24414 : acos_val = BASOP_util_atan2( one_minus_unit_vec_dotp_sq_root, unit_vec_dotp, sub( exp_uv, 2 ) ); // Q13
2049 24414 : c_options[index].arc_fx = L_deposit_h( acos_val ); // Q29
2050 24414 : move32();
2051 24414 : c_options[index].arc_weighted_fx = c_options[index].arc_fx; // Q29
2052 24414 : move32();
2053 :
2054 : /* A special case, mainly accounting for ELEVATED L,R,C speaker nodes.
2055 : A triplet between these nodes is not desired if there is a top node,
2056 : a penalty is implemented to take care of this. */
2057 24414 : IF( EQ_16( ConnectionClass, ELEVATED_PLANE_THIN_TRIANGLE_CONNECTION ) )
2058 : {
2059 0 : c_options[index].arc_weighted_fx = L_shl( c_options[index].arc_weighted_fx, 1 );
2060 0 : move32();
2061 : }
2062 :
2063 : /* If the connection passes a pre-determined plane of speaker nodes, then add further penalty */
2064 :
2065 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 ) )
2066 : {
2067 816 : c_options[index].arc_weighted_fx = L_shl( c_options[index].arc_weighted_fx, 1 );
2068 816 : move32();
2069 : }
2070 24414 : index = add( index, 1 );
2071 : }
2072 : }
2073 : }
2074 : }
2075 : }
2076 :
2077 : /* Number of found connection options at the half sphere */
2078 1792 : *num_connection_options = index;
2079 1792 : move16();
2080 :
2081 : /* Init memory for reordered connection options and order by arc_weighted,
2082 : * which informs of the preference order of the connections in case they cross */
2083 1792 : IF( ( c_options_reorder = (ConnectionOption *) malloc( sizeof( ConnectionOption ) * ( *num_connection_options ) ) ) == NULL )
2084 : {
2085 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for VBAP data\n" ) );
2086 : }
2087 :
2088 26206 : FOR( c = 0; c < *num_connection_options; c++ )
2089 : {
2090 : Word16 min_arc_index;
2091 24414 : min_arc_index = -1;
2092 24414 : move16();
2093 24414 : Word32 min_arc_weighted_fx_new = MAX_32;
2094 24414 : move16();
2095 24414 : Word16 exp_min_arc_weighted_fx = 31;
2096 24414 : move16();
2097 24414 : Word16 res = 0;
2098 24414 : move16();
2099 603704 : FOR( c_cmp = 0; c_cmp < *num_connection_options; c_cmp++ )
2100 : {
2101 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 );
2102 579290 : test();
2103 579290 : IF( EQ_16( res, -1 ) || res == 0 )
2104 : {
2105 126474 : min_arc_weighted_fx_new = c_options[c_cmp].arc_weighted_fx;
2106 126474 : move32();
2107 126474 : exp_min_arc_weighted_fx = Q31 - Q29;
2108 126474 : move16();
2109 126474 : min_arc_index = c_cmp;
2110 126474 : move16();
2111 : }
2112 : }
2113 24414 : c_options_reorder[c].chA = c_options[min_arc_index].chA;
2114 24414 : move16();
2115 24414 : c_options_reorder[c].chB = c_options[min_arc_index].chB;
2116 24414 : move16();
2117 :
2118 24414 : c_options_reorder[c].arc_fx = c_options[min_arc_index].arc_fx;
2119 24414 : move32();
2120 24414 : c_options_reorder[c].arc_weighted_fx = c_options[min_arc_index].arc_weighted_fx;
2121 24414 : move32();
2122 24414 : c_options[min_arc_index].arc_weighted_fx = MAX_32;
2123 24414 : move32();
2124 : }
2125 :
2126 : /* Set reordered connections as output and free temporary data */
2127 1792 : *connection_options_pr = c_options_reorder;
2128 1792 : free( c_options );
2129 :
2130 1792 : return IVAS_ERR_OK;
2131 : }
2132 :
2133 :
2134 : /*-------------------------------------------------------------------------*
2135 : * formulate_half_sphere_connections()
2136 : *
2137 : * Formulate half-sphere connections
2138 : *-------------------------------------------------------------------------*/
2139 :
2140 1792 : static ivas_error formulate_half_sphere_connections_fx(
2141 : const VBAP_SPEAKER_NODE *speaker_node_data, /* i : speaker node data */
2142 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
2143 : const enum SpeakerNodeGroup group, /* i : speaker node group */
2144 : Word16 connections[][2], /* o : vector of connections */
2145 : Word16 *connection_write_index,
2146 : const Word16 max_num_connections, /* i : max number of connections */
2147 : const Word16 num_non_crossing_planes, /* i : number of non-crossing planes */
2148 : const Word32 *non_crossing_plane_elevation_deg_fx /* i : vector of non-crossing plane elevations Q14*/
2149 : )
2150 : {
2151 : /* Variable initializations */
2152 : Word16 c, chA, chB, cmp_chA, cmp_chB, k, c_opt;
2153 : Word16 new_connection_is_valid;
2154 : Word16 within_first_arc;
2155 : Word32 new_cross_fx[3];
2156 : Word32 planeCrossingVec_fx[3];
2157 : Word16 Q_planeCrossingVec;
2158 : Word32 new_arc_fx; /* Q29 */
2159 : Word32 connection_arc_fx[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3]; /* Q29 */
2160 : Word32 connection_cross_fx[( VBAP_MAX_NUM_SPEAKER_NODES - 2 ) * 3][3]; /* Q29 */
2161 : Word32 tmpFloat_fx;
2162 : Word32 cmp_arc_fx; /* Q29 */
2163 : Word32 normVal_fx;
2164 : Word16 angleCmp_fx;
2165 : ConnectionOption *connection_options;
2166 : Word16 num_connection_options;
2167 : Word16 half_sphere_first_connection;
2168 : ivas_error error;
2169 :
2170 1792 : half_sphere_first_connection = *connection_write_index;
2171 1792 : move16();
2172 :
2173 : /* Obtain all connection options (i.e., channel pairs) at the half sphere. The function orders them
2174 : * in terms of which connection to keep if two connections would cross each other. */
2175 1792 : IF( NE_32( ( error = get_half_sphere_connection_options_fx(
2176 : speaker_node_data,
2177 : group,
2178 : num_speaker_nodes,
2179 : num_non_crossing_planes,
2180 : non_crossing_plane_elevation_deg_fx,
2181 : &connection_options,
2182 : &num_connection_options ) ),
2183 : IVAS_ERR_OK ) )
2184 : {
2185 0 : return error;
2186 : }
2187 :
2188 1792 : set32_fx( connection_arc_fx, 0, max_num_connections );
2189 45580 : FOR( c = 0; c < max_num_connections; c++ )
2190 : {
2191 43788 : set32_fx( connection_cross_fx[c], 0, 3 );
2192 : }
2193 :
2194 : /* The following loop goes through all reasonable chA - chB pairs for the half-sphere */
2195 1792 : c_opt = 0;
2196 1792 : move16();
2197 21748 : WHILE( c_opt < num_connection_options && *connection_write_index < max_num_connections )
2198 : {
2199 19956 : test();
2200 19956 : chA = connection_options[c_opt].chA;
2201 19956 : move16();
2202 19956 : chB = connection_options[c_opt].chB;
2203 19956 : move16();
2204 19956 : new_arc_fx = connection_options[c_opt].arc_fx;
2205 19956 : move32();
2206 :
2207 : /* Cross-product is needed for later stages */
2208 19956 : vbap_crossp_fx( speaker_node_data[chA].unit_vec_fx, speaker_node_data[chB].unit_vec_fx, new_cross_fx );
2209 :
2210 : /* Determine if new connection between chA and chB is valid */
2211 19956 : new_connection_is_valid = 1;
2212 19956 : move16();
2213 19956 : c = half_sphere_first_connection;
2214 19956 : move16();
2215 : 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;
2216 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;
2217 :
2218 19956 : test();
2219 135329 : WHILE( ( LT_16( c, *connection_write_index ) ) && new_connection_is_valid )
2220 : {
2221 115373 : cmp_chA = connections[c][0];
2222 115373 : move16();
2223 115373 : cmp_chB = connections[c][1];
2224 115373 : move16();
2225 : /* The connections are compared only if they don't involve same speaker nodes */
2226 115373 : test();
2227 115373 : test();
2228 115373 : test();
2229 115373 : IF( ( NE_16( cmp_chA, chA ) ) && ( NE_16( cmp_chA, chB ) ) && ( NE_16( cmp_chB, chA ) ) && ( NE_16( cmp_chB, chB ) ) )
2230 : {
2231 : /* The following lines determine if the connection chA-chB crosses with the connection cmp_chA-cmp_chB.*/
2232 : /* The connections, i.e., node-pairs determine a plane. The crossing can be determined by
2233 : * studying the intersection of these planes. */
2234 57124 : vbap_crossp_fx( connection_cross_fx[c], new_cross_fx, planeCrossingVec_fx );
2235 :
2236 57124 : tmpFloat_fx = 0;
2237 57124 : move16();
2238 :
2239 57124 : cmp_arc_fx = connection_arc_fx[c];
2240 57124 : move16();
2241 228496 : FOR( k = 0; k < 3; k++ )
2242 : {
2243 171372 : tmpFloat_fx = L_add( tmpFloat_fx, Mpy_32_32( planeCrossingVec_fx[k], planeCrossingVec_fx[k] ) );
2244 : }
2245 :
2246 57124 : Word16 tmp_exp = sub( 31, Q23 );
2247 57124 : if ( tmpFloat_fx == 0 )
2248 : {
2249 400 : tmpFloat_fx = 1;
2250 400 : move16();
2251 : }
2252 57124 : normVal_fx = ISqrt32( tmpFloat_fx, &tmp_exp );
2253 :
2254 228496 : FOR( k = 0; k < 3; k++ )
2255 : {
2256 171372 : planeCrossingVec_fx[k] = Mpy_32_32( planeCrossingVec_fx[k], normVal_fx ); // Q27 + Q31 - tmp_exp - Q31
2257 171372 : move32();
2258 : }
2259 : /*update Q for planeCrossingVec */
2260 57124 : Q_planeCrossingVec = sub( sub( Q27 + Q31, tmp_exp ), Q31 );
2261 : /* If the plane intersection is between both connections, then the two connections cross. */
2262 : /* Study first if the crossing is between arc chA-chB */
2263 :
2264 57124 : var1 = dotp_fx32( planeCrossingVec_fx, speaker_node_data[chA].unit_vec_fx, 3 ); // Q24 = 26 - tmp_exp
2265 57124 : var2 = dotp_fx32( planeCrossingVec_fx, speaker_node_data[chB].unit_vec_fx, 3 ); // Q24 = 26 - tmp_exp
2266 :
2267 57124 : var1_sq = Mpy_32_32( var1, var1 ); //(2 * (Q_planeCrossingVec - Q1) ) - Q31
2268 57124 : var2_sq = Mpy_32_32( var2, var2 );
2269 57124 : exp_var1_sq = shl( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ), 1 );
2270 57124 : exp_var2_sq = shl( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ), 1 );
2271 57124 : IF( GT_32( L_abs( var1_sq ), L_shr( ONE_IN_Q31, exp_var1_sq ) ) )
2272 : {
2273 2 : var1_sq = ONE_IN_Q31;
2274 2 : move16();
2275 2 : exp_var1_sq = 0;
2276 2 : move16();
2277 : }
2278 57124 : IF( GT_32( L_abs( var2_sq ), L_shr( ONE_IN_Q31, exp_var2_sq ) ) )
2279 : {
2280 0 : var2_sq = ONE_IN_Q31;
2281 0 : move16();
2282 0 : exp_var2_sq = 0;
2283 0 : move16();
2284 : }
2285 57124 : one_minus_var1_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var1_sq ), exp_var1_sq, &final_exp_A );
2286 57124 : var1_sqrt = Sqrt32( one_minus_var1_sq, &final_exp_A );
2287 57124 : var1_cos = BASOP_util_atan2( var1_sqrt, var1, sub( final_exp_A, ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ) ); // Q13
2288 57124 : angleCmp_fx = var1_cos;
2289 57124 : move16();
2290 :
2291 57124 : one_minus_var2_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var2_sq ), exp_var2_sq, &final_exp_B );
2292 57124 : var2_sqrt = Sqrt32( one_minus_var2_sq, &final_exp_B );
2293 57124 : var2_cos = BASOP_util_atan2( var2_sqrt, var2, sub( final_exp_B, ( Q31 - ( Q_planeCrossingVec + Q30 - Q31 ) ) ) ); // Q13
2294 :
2295 57124 : final_exp = BASOP_Util_Add_MantExp( angleCmp_fx, Q15 - Q13, var2_cos, Q15 - Q13, &angleCmp_fx );
2296 :
2297 57124 : sub_exp = 0, sub_exp_2 = 0, sub_final_exp = 0;
2298 57124 : var_a = BASOP_Util_Add_Mant32Exp( new_arc_fx, Q31 - Q29, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp );
2299 57124 : comp1 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_a ), sub_exp, 21474836 /* 0.01f in Q31 */, 0 );
2300 57124 : var_b = BASOP_Util_Add_Mant32Exp( 25735, Q31 - Q12, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp_2 );
2301 57124 : var_c = BASOP_Util_Add_Mant32Exp( new_arc_fx, Q31 - Q29, L_negate( var_b ), sub_exp_2, &sub_final_exp );
2302 57124 : comp2 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_c ), sub_final_exp, 21474836 /* 0.01f in Q31 */, 0 );
2303 :
2304 57124 : within_first_arc = 0;
2305 57124 : move16();
2306 57124 : IF( EQ_16( comp1, -1 ) )
2307 : {
2308 13120 : within_first_arc = 1;
2309 13120 : move16();
2310 : }
2311 44004 : ELSE IF( EQ_16( comp2, -1 ) )
2312 : {
2313 14144 : within_first_arc = 1;
2314 14144 : move16();
2315 : /* In this case, the plane crossing vector is inverted. The inverse is another
2316 : * plane-crossing vector, and detected to be between chA-chB connection.*/
2317 56576 : FOR( k = 0; k < 3; k++ )
2318 : {
2319 42432 : planeCrossingVec_fx[k] = Mpy_32_32( planeCrossingVec_fx[k], -134217728 /*-1.0f in Q27*/ ); // 27 + 27 - tmp_exp - 31 = 23 - tmp_exp
2320 42432 : move32();
2321 : }
2322 : /*update Q for planeCrossingVec */
2323 14144 : Q_planeCrossingVec = sub( sub( Q27 + Q27, tmp_exp ), Q31 );
2324 : }
2325 :
2326 : /* Study if the crossing is also between arc cmp_chA-cmp_chB */
2327 57124 : IF( within_first_arc > 0 )
2328 : {
2329 27264 : var1 = dotp_fx32( planeCrossingVec_fx, speaker_node_data[cmp_chA].unit_vec_fx, 3 );
2330 27264 : move32();
2331 27264 : var2 = dotp_fx32( planeCrossingVec_fx, speaker_node_data[cmp_chB].unit_vec_fx, 3 );
2332 27264 : move32();
2333 :
2334 : // final_exp_A, final_exp_B, exp_var1_sq, exp_var2_sq;
2335 27264 : var1_sq = Mpy_32_32( var1, var1 ); //(2 * (Q_planeCrossingVec - Q1) ) - Q31
2336 27264 : var2_sq = Mpy_32_32( var2, var2 );
2337 27264 : exp_var1_sq = shl( ( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ) ), 1 );
2338 27264 : exp_var2_sq = shl( ( sub( Q31, sub( Q_planeCrossingVec, Q1 ) ) ), 1 );
2339 27264 : IF( GT_32( L_abs( var1_sq ), L_shr( ONE_IN_Q31, exp_var1_sq ) ) )
2340 : {
2341 2 : var1_sq = ONE_IN_Q31;
2342 2 : move16();
2343 2 : exp_var1_sq = 0;
2344 2 : move16();
2345 : }
2346 27264 : IF( GT_32( L_abs( var2_sq ), L_shr( ONE_IN_Q31, exp_var2_sq ) ) )
2347 : {
2348 0 : var2_sq = ONE_IN_Q31;
2349 0 : move16();
2350 0 : exp_var2_sq = 0;
2351 0 : move16();
2352 : }
2353 27264 : one_minus_var1_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var1_sq ), exp_var1_sq, &final_exp_A );
2354 27264 : var1_sqrt = Sqrt32( one_minus_var1_sq, &final_exp_A );
2355 27264 : var1_cos = BASOP_util_atan2( var1_sqrt, var1, sub( final_exp_A, sub( Q31, add( Q_planeCrossingVec, Q30 - Q31 ) ) ) ); // Q13
2356 27264 : angleCmp_fx = var1_cos;
2357 27264 : move16();
2358 :
2359 27264 : one_minus_var2_sq = BASOP_Util_Add_Mant32Exp( ONE_IN_Q30, 1, L_negate( var2_sq ), exp_var2_sq, &final_exp_B );
2360 27264 : var2_sqrt = Sqrt32( one_minus_var2_sq, &final_exp_B );
2361 27264 : var2_cos = BASOP_util_atan2( var2_sqrt, var2, sub( final_exp_B, sub( Q31, add( Q_planeCrossingVec, Q30 - Q31 ) ) ) ); // Q13
2362 :
2363 27264 : final_exp = BASOP_Util_Add_MantExp( angleCmp_fx, Q15 - Q13, var2_cos, Q15 - Q13, &angleCmp_fx );
2364 :
2365 27264 : sub_exp = 0, sub_exp_2 = 0, sub_final_exp = 0;
2366 27264 : move16();
2367 27264 : move16();
2368 27264 : move16();
2369 27264 : var_a = BASOP_Util_Add_Mant32Exp( cmp_arc_fx, Q31 - Q29, L_negate( L_deposit_h( angleCmp_fx ) ), final_exp, &sub_exp );
2370 27264 : comp1 = BASOP_Util_Cmp_Mant32Exp( L_abs( var_a ), sub_exp, 21474836 /* 0.01f in Q31 */, 0 );
2371 :
2372 27264 : if ( EQ_16( comp1, -1 ) )
2373 : {
2374 : /* A crossing is detected. The new connection is not valid, because
2375 : * the connections were ordered in order of preference (arc_weighted) */
2376 3252 : new_connection_is_valid = 0;
2377 3252 : move16();
2378 : }
2379 : }
2380 : }
2381 115373 : c = add( c, 1 );
2382 : }
2383 :
2384 : /* Store the new connection which has been confirmed valid */
2385 19956 : IF( new_connection_is_valid > 0 )
2386 : {
2387 16704 : connections[*connection_write_index][0] = chA;
2388 16704 : move16();
2389 16704 : connections[*connection_write_index][1] = chB;
2390 16704 : move16();
2391 16704 : connection_arc_fx[*connection_write_index] = new_arc_fx; /* Q29 */
2392 16704 : move16();
2393 16704 : connection_cross_fx[*connection_write_index][0] = new_cross_fx[0]; /* Q29 */
2394 16704 : move16();
2395 16704 : connection_cross_fx[*connection_write_index][1] = new_cross_fx[1]; /* Q29 */
2396 16704 : move16();
2397 16704 : connection_cross_fx[*connection_write_index][2] = new_cross_fx[2]; /* Q29 */
2398 16704 : move16();
2399 16704 : *connection_write_index = add( *connection_write_index, 1 );
2400 : }
2401 19956 : c_opt = add( c_opt, 1 );
2402 : }
2403 :
2404 1792 : free( connection_options );
2405 :
2406 1792 : return IVAS_ERR_OK;
2407 : }
2408 :
2409 : /*-------------------------------------------------------------------------*
2410 : * determine_non_crossing_planes()
2411 : *
2412 : * Determine non-crossing planes
2413 : *-------------------------------------------------------------------------*/
2414 :
2415 : /*! r: number of non-crossing planes */
2416 :
2417 896 : static Word16 determine_non_crossing_planes_fx(
2418 : const Word16 num_speaker_nodes, /* i : number of speaker nodes */
2419 : const VBAP_SPEAKER_NODE *node_data, /* i : speaker node data */
2420 : Word32 *non_crossing_plane_elevation_deg_fx /* o : vector of non-crossing plane elevations Q14 */
2421 : )
2422 : {
2423 : Word32 next_ele_check_fx; /* Q14 */
2424 : Word32 ele_check_fx; /* Q14 */
2425 : Word32 max_gap_fx; /* Q14 */
2426 : Word32 gap_to_next_ls_fx; /* Q14 */
2427 :
2428 : Word16 ch, ch_cmp;
2429 : Word16 num_planes;
2430 :
2431 896 : ele_check_fx = -16382362; /*Q14*/
2432 896 : move32();
2433 896 : num_planes = 0;
2434 896 : move16();
2435 :
2436 : /* For each plane, check if a non-crossing plane should be determined */
2437 3316 : WHILE( LT_32( ele_check_fx, 1474560 ) /*90.0f Q14*/ )
2438 : {
2439 2420 : next_ele_check_fx = 16382362; /*Q14*/
2440 2420 : move32();
2441 :
2442 : Word32 tmp1, tmp2;
2443 :
2444 : /* Find next node elevation that is not in horizontal plane */
2445 27494 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
2446 : {
2447 25074 : tmp1 = L_add( ele_check_fx, 16 /*VBAP_EPSILON in Q14*/ );
2448 25074 : tmp2 = L_sub( next_ele_check_fx, 16 /*VBAP_EPSILON in Q14*/ );
2449 25074 : test();
2450 25074 : test();
2451 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 ) )
2452 : {
2453 3049 : next_ele_check_fx = L_shr( node_data[ch].ele_deg_fx, 8 ); // shift due to comparision with 90.0f
2454 : }
2455 : }
2456 2420 : ele_check_fx = next_ele_check_fx; // Q14
2457 2420 : move32();
2458 :
2459 2420 : IF( GT_32( ele_check_fx, 1474560 ) /*90.0f in Q14*/ )
2460 : {
2461 : /* When no next node elevation found, break loop */
2462 0 : BREAK;
2463 : }
2464 :
2465 2420 : max_gap_fx = -163838368; // Q14
2466 2420 : move32();
2467 27494 : FOR( ch = 0; ch < num_speaker_nodes; ch++ )
2468 : {
2469 : /* Find gap to the next speaker node at the same plane */
2470 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*/ ) )
2471 : {
2472 3900 : gap_to_next_ls_fx = 1638398336; // Q14
2473 3900 : move32();
2474 46020 : FOR( ch_cmp = 0; ch_cmp < num_speaker_nodes; ch_cmp++ )
2475 : {
2476 42120 : test();
2477 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*/ ) )
2478 : {
2479 5516 : Word32 gap_fx = L_sub( node_data[ch_cmp].azi_deg_fx, node_data[ch].azi_deg_fx );
2480 8274 : WHILE( gap_fx < 0 )
2481 : {
2482 2758 : gap_fx = L_add( gap_fx, 1509949440 /*360.0f in Q22*/ );
2483 : }
2484 5516 : IF( LT_32( L_shr( gap_fx, 8 ), gap_to_next_ls_fx ) )
2485 : {
2486 3812 : gap_to_next_ls_fx = L_shr( gap_fx, 8 );
2487 : }
2488 : }
2489 : }
2490 : /* Find maximum gap on that plane */
2491 3900 : IF( GT_32( gap_to_next_ls_fx, max_gap_fx ) )
2492 : {
2493 2648 : max_gap_fx = gap_to_next_ls_fx;
2494 2648 : move32();
2495 : }
2496 : }
2497 : }
2498 :
2499 : /* If maximum gap is small enough, then a non-crossing plane is detected */
2500 2420 : test();
2501 2420 : IF( LT_32( max_gap_fx, 2293776 /*Q14*/ ) && max_gap_fx > 0 )
2502 : {
2503 426 : non_crossing_plane_elevation_deg_fx[num_planes] = ele_check_fx; /* Q14 */
2504 426 : move32();
2505 426 : num_planes = add( num_planes, 1 );
2506 426 : IF( EQ_16( num_planes, VBAP_MAX_PLANES ) )
2507 : {
2508 : /* Memory init limit. Does not happen with any real speaker node configuration.
2509 : Triangulation succeeds even if number of non_crossing_planes are limited. */
2510 0 : BREAK;
2511 : }
2512 : }
2513 : }
2514 :
2515 896 : return num_planes;
2516 : }
2517 :
2518 : /*-------------------------------------------------------------------------*
2519 : * reorder_triplets()
2520 : *
2521 : * Reorder virtual surface triplets into provided target order.
2522 : *-------------------------------------------------------------------------*/
2523 :
2524 1792 : static void reorder_triplets_fx(
2525 : VBAP_VS_TRIPLET *triplets, /* i/o: VS triplets to be reordered */
2526 : const Word16 *target_order, /* i : Target order for VS triplets */
2527 : const Word16 num_triplets /* i : Number of VS triplets */
2528 : )
2529 : {
2530 : VBAP_VS_TRIPLET tempTriplets[VBAP_MAX_NUM_TRIPLETS];
2531 : Word16 c;
2532 :
2533 : /* First copy to temp array */
2534 16388 : FOR( c = 0; c < num_triplets; c++ )
2535 : {
2536 14596 : tempTriplets[c] = triplets[c];
2537 : }
2538 :
2539 : /* Then move back in sorted order */
2540 16388 : FOR( c = 0; c < num_triplets; c++ )
2541 : {
2542 14596 : triplets[c] = tempTriplets[target_order[c]];
2543 : }
2544 :
2545 1792 : return;
2546 : }
|
