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