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 <math.h>
35 : #include <assert.h>
36 : #include <stdio.h>
37 : #include "options.h"
38 : #include "prot_fx.h"
39 : #include "ivas_prot_rend_fx.h"
40 : #include "ivas_rom_rend.h"
41 : #include "ivas_stat_dec.h"
42 : #include "wmc_auto.h"
43 : #include "ivas_prot_fx.h"
44 :
45 : /*-----------------------------------------------------------------------*
46 : * Local constants
47 : *-----------------------------------------------------------------------*/
48 :
49 : #define EFAP_MAX_SIZE_TMP_BUFF 30
50 : #define EFAP_MAX_GHOST_LS 5 /* Maximum number of ghost Loudspeakers, for memory allocation purpose */
51 : #define POLY_THRESH_Q29 53687LL // Q29
52 : #define POLY_THRESH_Q28 26843 // Q28
53 : #ifdef DEBUG_EFAP_POLY_TOFILE
54 : #define PANNING_AZI_RESOLUTION 2
55 : #define PANNING_ELE_RESOLUTION 5
56 : #endif
57 : #define Q22_1 4194304
58 : #define Q22_45_DEG 188743680
59 : #define Q22_90_DEG 377487360
60 : #define Q22_180_DEG 754974720
61 : #define Q22_360_DEG 1509949440
62 : #define Q22_120_DEG 503316480
63 : #define Q22_240_DEG 1006632960
64 :
65 : /*-----------------------------------------------------------------------*
66 : * Local function prototypes
67 : *-----------------------------------------------------------------------*/
68 :
69 :
70 : static ivas_error poly_init_fx( EFAP *efap, const Word16 efip_flag );
71 : static ivas_error sphere_triangulation_fx( const Word16 numSpk, EFAP_VERTEX_DATA *vtxData, EFAP_POLYSET_DATA *polyData, Word32 ***dmTranspose /*q31*/, Word16 *numTot, const Word16 efip_flag );
72 : static void initial_polyeder_fx( EFAP_VERTEX_DATA *vtxData, EFAP_LS_TRIANGLE *triArray, Word16 *numTri, Word16 *vtxInHull );
73 : static void add_ghost_speakers_fx( EFAP_VERTEX *vertexArray, Word16 *numVtx, const Word16 efip_flag );
74 : static void add_vertex_to_convex_hull_fx( const EFAP_VERTEX_DATA *vtxData, const Word16 vtxIdx, Word16 *vtxInHull, EFAP_LS_TRIANGLE *triArray, Word16 *szTri );
75 : static void sort_vertices_fx( const EFAP_VERTEX *vertexArray, const Word16 *numVtx, Word16 *order );
76 : static void visible_edges_fx( const EFAP_LS_TRIANGLE *triArray, const Word16 *visible, const Word16 numSurface, Word16 *numEdges, Word16 *edges );
77 :
78 :
79 : static void flip_plane_fx( const EFAP_VERTEX *vtxArray, Word16 *surface, const Word32 centroid[3] /*q31*/ );
80 : static void remap_ghosts_fx( EFAP_VERTEX *vtxArray, EFAP_LS_TRIANGLE *triArray, Word16 numSpk, Word16 *numVertex, Word16 numTri, Word32 **downmixMatrix /*q31*/ );
81 : static void vertex_init_fx( const Word32 *aziSpk /*q22*/, const Word32 *eleSpk /*q22*/, EFAP_VERTEX_DATA *efapVtxData );
82 : static void efap_panning_fx( const Word32 azi /*q22*/, const Word32 ele /*q22*/, const EFAP_POLYSET_DATA *polyData, Word32 *bufferL /*q31*/ );
83 : static void get_poly_gains_fx( const Word32 azi /*q22*/, const Word32 ele /*q22*/, const Word32 aziPoly[EFAP_MAX_CHAN_NUM] /*q22*/, const Word32 elePoly[EFAP_MAX_CHAN_NUM] /*q22*/, const Word16 numChan, Word32 *buffer /*q31*/ );
84 : static Word32 get_tri_gain_fx( const Word32 A[2] /*q22*/, const Word32 B[2] /*q22*/, const Word32 C[2] /*q22*/, const Word32 P_minus_A[2] /*q22*/ );
85 :
86 : /*-----------------------------------------------------------------------*
87 : * EFAP Utils
88 : *-----------------------------------------------------------------------*/
89 :
90 : static void add_vertex_fx( EFAP_VERTEX *vtxArray, const Word32 azi /*q22*/, const Word32 ele /*q22*/, const Word16 pos, const EFAP_VTX_DMX_TYPE );
91 : static void efap_sort_s_fx( Word16 *x, Word16 *idx, const Word16 len );
92 :
93 :
94 : static Word32 vertex_distance_fx( const EFAP_VERTEX *vtxArray, const EFAP_LS_TRIANGLE tri, const Word16 vtxIdx );
95 : static Word32 point_plane_distance_fx( const Word32 P1[3] /*q31*/, const Word32 P2[3] /*q31*/, const Word32 P3[3] /*q31*/, const Word32 X[3] /*q31*/ );
96 : static Word32 point_poly_distance_fx( const EFAP_POLYSET poly, const Word32 X[3] /*q31*/ );
97 : static void efap_crossp_fx( const Word32 *v1 /*q30*/, const Word32 *v2 /*q30*/, Word32 *v /*q29*/ );
98 : static Word16 find_int_in_tri_fx( const EFAP_LS_TRIANGLE *tri, const Word16 n, const Word16 r, Word16 *pos );
99 : static void remove_vertex_fx( EFAP_VERTEX *vtxArray, const Word16 idx, const Word16 L );
100 : static Word16 get_neighbours_fx( const EFAP_LS_TRIANGLE *triArray, const Word16 vtxIdx, const Word16 numTri, Word16 *neighbours );
101 :
102 : static void matrix_times_row_fx( Word32 mat[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF] /*q31*/, const Word32 *vec /*q31*/, const Word16 L, Word32 *out /*q31*/ );
103 : static void tri_to_poly_fx( const EFAP_VERTEX *vtxArray, const EFAP_LS_TRIANGLE *triArray, const Word16 numVtx, const Word16 numTri, Word16 sortedChan[EFAP_MAX_POLY_SET][EFAP_MAX_CHAN_NUM], Word16 *outLengthPS, Word16 outLengthSorted[EFAP_MAX_POLY_SET] );
104 : static Word16 compare_poly_fx( Word16 *old, Word16 lenOld, Word16 *new, Word16 lenNew );
105 :
106 : static void sort_channels_vertex_fx( const EFAP_VERTEX *vtxArray, const EFAP_LS_TRIANGLE *triArray, Word16 channels[EFAP_MAX_CHAN_NUM], const Word16 lengthChannels, Word16 idxTri );
107 : static Word32 efap_32mod32( const Word32 x /*q22*/, const Word32 y /*q22*/ );
108 : static Word16 get_poly_num_fx( const Word32 P[2] /*q22*/, const EFAP_POLYSET_DATA *polyData );
109 : static Word16 in_poly_fx( const Word32 P[2] /*q22*/, const EFAP_POLYSET poly );
110 : static Word16 in_tri_fx( Word32 A[2] /*q22*/, Word32 B[2] /*q22*/, Word32 C[2] /*q22*/, Word32 P_minus_A[2] /*q22*/ );
111 : static void sph2cart_fx( const Word32 azi /*q22*/, const Word32 ele /*q22*/, Word32 *pos /*q31*/ );
112 :
113 : /*-----------------------------------------------------------------------*
114 : * Global function definitions
115 : *-----------------------------------------------------------------------*/
116 :
117 : /*-------------------------------------------------------------------------*
118 : * efap_init_data_fx()
119 : *
120 : * Wrap the internal functions to initialize the EFAP data structure
121 : *-------------------------------------------------------------------------*/
122 :
123 872 : ivas_error efap_init_data_fx(
124 : EFAP_HANDLE *hEFAPdata, /* i/o: handle for EFAP data structure that will be initialized */
125 : const Word32 *speaker_node_azi_deg, /* i : vector of speaker node azimuths (positive left) Q22 */
126 : const Word32 *speaker_node_ele_deg, /* i : vector of speaker node elevations (positive up) Q22 */
127 : const Word16 num_speaker_nodes, /* i : number of speaker nodes in the set */
128 : const Word16 efap_mode /* i : indicates whether EFAP or EFIP is used */
129 : )
130 : {
131 : /* Handle instance declaration */
132 : Word8 polyset_size;
133 : EFAP *efap;
134 : ivas_error error;
135 :
136 872 : error = IVAS_ERR_OK;
137 872 : move32();
138 :
139 : /* Basic init checks */
140 872 : test();
141 872 : IF( !speaker_node_azi_deg || !speaker_node_ele_deg )
142 : {
143 0 : hEFAPdata = NULL;
144 0 : return IVAS_ERROR( IVAS_ERR_WRONG_PARAMS, "EFAP requires arrays of speaker azimuths and elevations" );
145 : }
146 :
147 : /*-----------------------------------------------------------------*
148 : * Allocate memory
149 : *-----------------------------------------------------------------*/
150 :
151 : /* Memory allocation for main EFAP structure */
152 872 : IF( ( efap = (EFAP *) malloc( sizeof( EFAP ) ) ) == NULL )
153 : {
154 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP handle\n" ) );
155 : }
156 :
157 : /* Memory Allocation and update for aziSpk & eleSpk arrays*/
158 872 : IF( ( efap->aziSpk = (Word32 *) malloc( num_speaker_nodes * sizeof( Word32 ) ) ) == NULL )
159 : {
160 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP speaker azimuths\n" ) );
161 : }
162 872 : IF( ( efap->eleSpk = (Word32 *) malloc( num_speaker_nodes * sizeof( Word32 ) ) ) == NULL )
163 : {
164 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP speaker elevations\n" ) );
165 : }
166 :
167 : /* Memory allocation for vertexArray */
168 872 : IF( ( efap->vtxData.vertexArray = (EFAP_VERTEX *) malloc( ( num_speaker_nodes + EFAP_MAX_GHOST_LS ) * sizeof( EFAP_VERTEX ) ) ) == NULL )
169 : {
170 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP Vertex Array\n" ) );
171 : }
172 :
173 : /* Memory allocation for the tmp buffer short */
174 872 : IF( ( efap->bufferShort_fx = (Word32 *) malloc( num_speaker_nodes * sizeof( Word32 ) ) ) == NULL )
175 : {
176 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP bufferS\n" ) );
177 : }
178 : /* get upper bound of number of polygons required */
179 872 : polyset_size = efap_poly_limit[num_speaker_nodes - 1];
180 :
181 : /* Memory allocation for the polyset array */
182 872 : IF( ( efap->polyData.polysetArray = (EFAP_POLYSET *) malloc( polyset_size * sizeof( EFAP_POLYSET ) ) ) == NULL )
183 : {
184 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP bufferS\n" ) );
185 : }
186 :
187 : /* Memory allocation for the triangle array */
188 872 : IF( ( efap->polyData.triArray = (EFAP_LS_TRIANGLE *) malloc( polyset_size * sizeof( EFAP_LS_TRIANGLE ) ) ) == NULL )
189 : {
190 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP bufferS\n" ) );
191 : }
192 :
193 : /*-----------------------------------------------------------------*
194 : * Initialize values
195 : *-----------------------------------------------------------------*/
196 :
197 872 : efap->numSpk = num_speaker_nodes;
198 872 : move16();
199 :
200 : /* The number of vertex is first set to the number of LS but will evolve further */
201 872 : efap->vtxData.numVtx = num_speaker_nodes;
202 872 : move16();
203 :
204 : /* Loudspeaker configuration */
205 872 : Copy32( speaker_node_azi_deg, efap->aziSpk, num_speaker_nodes ); // Q22
206 872 : Copy32( speaker_node_ele_deg, efap->eleSpk, num_speaker_nodes ); // Q22
207 :
208 : /* Initialization of the vertex */
209 872 : vertex_init_fx( efap->aziSpk, efap->eleSpk, &efap->vtxData );
210 :
211 : /* Initialization of polygons and ghost LS */
212 872 : IF( ( error = poly_init_fx( efap, efap_mode ) ) != IVAS_ERR_OK )
213 : {
214 0 : return error;
215 : }
216 :
217 : /* Memory allocation for the tmp buffer long */
218 872 : IF( ( efap->bufferLong_fx = (Word32 *) malloc( efap->vtxData.numVtx * sizeof( Word32 ) ) ) == NULL )
219 : {
220 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP bufferL\n" ) );
221 : }
222 :
223 872 : *hEFAPdata = efap;
224 872 : return error;
225 : }
226 :
227 : /*-------------------------------------------------------------------------*
228 : * efap_determine_gains_fx()
229 : *
230 : * Obtain amplitude panning gains for all speaker nodes based on the
231 : * given direction
232 : *-------------------------------------------------------------------------*/
233 :
234 848887 : void efap_determine_gains_fx(
235 : EFAP_HANDLE hEFAPdata, /* i : EFAP structure */
236 : Word32 *gains, /* o : gain vector for speaker nodes for given direction Q30 */
237 : const Word32 azi_deg, /* i : azimuth in degrees for panning direction (positive left) Q22 */
238 : const Word32 ele_deg, /* i : elevation in degrees for panning direction (positive up) Q22 */
239 : const Word16 efap_mode /* i : indicates whether EFAP or EFIP is used */
240 : )
241 : {
242 : Word16 i, j;
243 : Word32 azi_wrap_int, ele_wrap_int;
244 : Word32 normBuffer;
245 :
246 : /* Resetting bufferShort and bufferLong */
247 848887 : set32_fx( hEFAPdata->bufferShort_fx, 0, hEFAPdata->numSpk );
248 848887 : set32_fx( hEFAPdata->bufferLong_fx, 0, hEFAPdata->vtxData.numVtx );
249 :
250 : /* Wrap angles to correct range */
251 848887 : panning_wrap_angles_fx( azi_deg, ele_deg, &azi_wrap_int, &ele_wrap_int ); // ouputs in q22
252 :
253 : /* Panning */
254 848887 : efap_panning_fx( azi_wrap_int, ele_wrap_int, &hEFAPdata->polyData, hEFAPdata->bufferLong_fx ); // hEFAPdata->bufferLong_fx q31
255 :
256 848887 : IF( efap_mode == EFAP_MODE_EFAP )
257 : {
258 843077 : normBuffer = 0;
259 843077 : move32();
260 7744782 : FOR( j = 0; j < hEFAPdata->numSpk; ++j )
261 : {
262 6901705 : hEFAPdata->bufferShort_fx[j] = 0;
263 6901705 : move32();
264 : /* Multiplying by the downmixMatrix */
265 85739180 : FOR( i = 0; i < hEFAPdata->vtxData.numVtx; ++i )
266 : {
267 78837475 : hEFAPdata->bufferShort_fx[j] = L_add_sat( hEFAPdata->bufferShort_fx[j], L_shr( Mpy_32_32( hEFAPdata->bufferLong_fx[i], hEFAPdata->dmTranspose_fx[i][j] ), Q1 ) ); /* Q30 */
268 78837475 : move32();
269 : }
270 6901705 : normBuffer = L_add_sat( normBuffer, Mpy_32_32( hEFAPdata->bufferShort_fx[j], hEFAPdata->bufferShort_fx[j] ) ); /* Q29 */
271 : }
272 843077 : Word16 exp = 2;
273 843077 : move16();
274 843077 : normBuffer = ISqrt32( normBuffer, &exp ); // Q=(31-exp)
275 :
276 7744782 : FOR( j = 0; j < hEFAPdata->numSpk; ++j )
277 : {
278 6901705 : hEFAPdata->bufferShort_fx[j] = Mpy_32_32( hEFAPdata->bufferShort_fx[j], normBuffer ); // Q=(30+31-exp-31)=>(30-exp)
279 6901705 : move32();
280 6901705 : hEFAPdata->bufferShort_fx[j] = L_shl( hEFAPdata->bufferShort_fx[j], exp ); /* Q30 */
281 6901705 : move32();
282 : }
283 : }
284 : ELSE
285 : {
286 5810 : normBuffer = 0;
287 5810 : move32();
288 59500 : FOR( j = 0; j < hEFAPdata->numSpk; ++j )
289 : {
290 53690 : hEFAPdata->bufferShort_fx[j] = 0;
291 53690 : move32();
292 : /* Multiplying by the downmixMatrix */
293 699860 : FOR( i = 0; i < hEFAPdata->vtxData.numVtx; ++i )
294 : {
295 646170 : hEFAPdata->bufferShort_fx[j] = L_add_sat( hEFAPdata->bufferShort_fx[j], L_shr( Mpy_32_32( hEFAPdata->bufferLong_fx[i], hEFAPdata->dmTranspose_fx[i][j] ), Q1 ) ); /* Q30 */
296 646170 : move32();
297 : }
298 53690 : normBuffer = L_add_sat( normBuffer, L_shr( hEFAPdata->bufferShort_fx[j], Q1 ) ); /* Q29 */
299 : }
300 5810 : Word16 exp = 2;
301 5810 : move16();
302 5810 : normBuffer = Inv16( extract_l( L_shr( normBuffer, Q16 ) ), &exp ); /*Q=(15-exp)*/
303 :
304 59500 : FOR( j = 0; j < hEFAPdata->numSpk; ++j )
305 : {
306 53690 : Word16 exp_temp = add( exp, 1 );
307 53690 : hEFAPdata->bufferShort_fx[j] = Sqrt32( Mpy_32_16_1( hEFAPdata->bufferShort_fx[j], extract_l( normBuffer ) ) /*Q(30-exp)*/, &exp_temp ); // Q=(31-exp_temp)
308 53690 : move32();
309 53690 : hEFAPdata->bufferShort_fx[j] = L_shl( hEFAPdata->bufferShort_fx[j], sub( exp_temp, 1 ) ); /* Q30 */
310 53690 : move32();
311 : }
312 : }
313 :
314 : /* Copy gains to output */
315 848887 : Copy32( hEFAPdata->bufferShort_fx, gains, hEFAPdata->numSpk ); /* Q30 */
316 :
317 848887 : return;
318 : }
319 :
320 :
321 : /*-------------------------------------------------------------------------*
322 : * efap_free_data()
323 : *
324 : * Wrapper to free EFAP data structure
325 : *-------------------------------------------------------------------------*/
326 :
327 1667 : void efap_free_data_fx(
328 : EFAP_HANDLE *hEFAPdata /* i/o: EFAP handle to be freed */
329 : )
330 : {
331 : Word16 i, dim1;
332 : void **p_dmTranspose;
333 :
334 1667 : test();
335 1667 : IF( hEFAPdata == NULL || *hEFAPdata == NULL )
336 : {
337 795 : return;
338 : }
339 :
340 872 : dim1 = ( *hEFAPdata )->numTot;
341 872 : move16();
342 :
343 : /* instance buffer members */
344 872 : free( ( *hEFAPdata )->aziSpk );
345 872 : ( *hEFAPdata )->aziSpk = NULL;
346 :
347 872 : free( ( *hEFAPdata )->eleSpk );
348 872 : ( *hEFAPdata )->eleSpk = NULL;
349 :
350 872 : free( ( *hEFAPdata )->vtxData.vertexArray );
351 872 : ( *hEFAPdata )->vtxData.vertexArray = NULL;
352 :
353 872 : free( ( *hEFAPdata )->vtxData.vtxOrder );
354 872 : ( *hEFAPdata )->vtxData.vtxOrder = NULL;
355 :
356 872 : free( ( *hEFAPdata )->polyData.polysetArray );
357 872 : ( *hEFAPdata )->vtxData.vtxOrder = NULL;
358 :
359 872 : free( ( *hEFAPdata )->polyData.triArray );
360 872 : ( *hEFAPdata )->vtxData.vtxOrder = NULL;
361 :
362 872 : free( ( *hEFAPdata )->bufferLong_fx );
363 872 : ( *hEFAPdata )->bufferLong_fx = NULL;
364 :
365 872 : free( ( *hEFAPdata )->bufferShort_fx );
366 872 : ( *hEFAPdata )->bufferShort_fx = NULL;
367 :
368 872 : p_dmTranspose = (void **) ( *hEFAPdata )->dmTranspose_fx;
369 :
370 10272 : FOR( i = 0; i < dim1; i++ )
371 : {
372 9400 : free( p_dmTranspose[i] );
373 : }
374 872 : free( p_dmTranspose );
375 :
376 : /* instance */
377 872 : free( *hEFAPdata );
378 872 : *hEFAPdata = NULL;
379 :
380 872 : return;
381 : }
382 :
383 :
384 : /*-----------------------------------------------------------------------*
385 : * Local function definitions
386 : *-----------------------------------------------------------------------*/
387 :
388 :
389 : /*-------------------------------------------------------------------------*
390 : * poly_init()
391 : *
392 : * Main function for the Efap initialization whose purpose is to initialize
393 : * the different polygons and to add the ghost speakers
394 : *-------------------------------------------------------------------------*/
395 :
396 872 : static ivas_error poly_init_fx(
397 : EFAP *efap, /* i/o: A pointer to a handle to efap instance */
398 : const Word16 efip_flag /* i : flag to indicate whether initialization is for EFIP (used for ALLRAD) */
399 : )
400 : {
401 : Word16 n, m, j;
402 : Word16 finalLength, lengthTri2PolyPS;
403 : Word16 lengthTri2PolySorted[EFAP_MAX_POLY_SET];
404 : Word16 sortedChan[EFAP_MAX_POLY_SET][EFAP_MAX_CHAN_NUM];
405 : Word32 tmpMax, tmpMin;
406 : ivas_error error;
407 :
408 872 : error = IVAS_ERR_OK;
409 872 : move32();
410 :
411 : /* Safety Check */
412 872 : assert( efap != NULL && "EFAP: efap == NULL" );
413 :
414 47960 : FOR( n = 0; n < EFAP_MAX_POLY_SET; n++ )
415 : {
416 47088 : set16_fx( sortedChan[n], 0, EFAP_MAX_CHAN_NUM );
417 : }
418 :
419 : /* Computing the different ghost vertex, the downmix matrix and the triangle array */
420 872 : IF( NE_32( ( error = sphere_triangulation_fx( efap->numSpk, &efap->vtxData, &efap->polyData, &efap->dmTranspose_fx, &efap->numTot, efip_flag ) ), IVAS_ERR_OK ) )
421 : {
422 0 : return error;
423 : }
424 :
425 : /* set isNaN for ghost loudspeakers */
426 10272 : FOR( n = 0; n < efap->vtxData.numVtx; ++n )
427 : {
428 9400 : test();
429 17928 : if ( GT_32( efap->vtxData.vertexArray[n].ele /*Q22*/, ( Q22_90_DEG /*90.0 Q22*/ - 4 /*1e-6 Q22*/ ) ) ||
430 8528 : LT_32( efap->vtxData.vertexArray[n].ele /*Q22*/, ( 4 /*1e-6 Q22*/ - Q22_90_DEG /*90.0 Q22*/ ) ) )
431 : {
432 1744 : efap->vtxData.vertexArray[n].isNaN = true;
433 1744 : move16();
434 : }
435 : }
436 :
437 : /* Converting the triangle to polygon structure */
438 872 : tri_to_poly_fx( efap->vtxData.vertexArray, efap->polyData.triArray, efap->vtxData.numVtx, efap->polyData.numTri, sortedChan, &lengthTri2PolyPS, lengthTri2PolySorted );
439 :
440 : /* Completing the polyData Structure */
441 872 : finalLength = -1;
442 872 : move16();
443 :
444 15467 : FOR( n = 0; n < lengthTri2PolyPS; ++n )
445 : {
446 14595 : m = add( finalLength, 1 );
447 :
448 : /* Complete the fields of the polygon */
449 59097 : FOR( j = 0; j < lengthTri2PolySorted[n]; ++j )
450 : {
451 44502 : efap->polyData.polysetArray[m].chan[j] = sortedChan[n][j];
452 44502 : move16();
453 44502 : efap->polyData.polysetArray[m].polyAzi[j] = efap->vtxData.vertexArray[sortedChan[n][j]].azi; /* Q22 */
454 44502 : move32();
455 44502 : efap->polyData.polysetArray[m].polyEle[j] = efap->vtxData.vertexArray[sortedChan[n][j]].ele; /* Q22 */
456 44502 : move32();
457 44502 : efap->polyData.polysetArray[m].isNaN[j] = efap->vtxData.vertexArray[sortedChan[n][j]].isNaN;
458 44502 : move16();
459 : }
460 :
461 14595 : efap->polyData.polysetArray[m].numChan = lengthTri2PolySorted[n];
462 14595 : move16();
463 :
464 : /* In case tmpMax - tmpMin > 180, wrap polygon azimuth */
465 14595 : maximum_l( efap->polyData.polysetArray[m].polyAzi, lengthTri2PolySorted[n], &tmpMax );
466 14595 : minimum_l( efap->polyData.polysetArray[m].polyAzi, lengthTri2PolySorted[n], &tmpMin );
467 :
468 14595 : IF( GT_32( L_sub( tmpMax /*q22*/, tmpMin /*q22*/ ), Q22_180_DEG /*180 in Q22*/ ) )
469 : {
470 10242 : FOR( j = 0; j < lengthTri2PolySorted[n]; ++j )
471 : {
472 7791 : assert( ( m + 2 < EFAP_MAX_POLY_SET ) && "EFAP: maximum polygons exceeded!" );
473 :
474 : /* add two new polygons with azimuths wrapped to differing bounds */
475 7791 : efap->polyData.polysetArray[m + 1].polyAzi[j] = efap_32mod32( efap->polyData.polysetArray[m].polyAzi[j] /*q22*/, Q22_360_DEG /*360 q22*/ ); /* Q22 */
476 7791 : move32();
477 7791 : efap->polyData.polysetArray[m + 2].polyAzi[j] = L_sub( efap->polyData.polysetArray[m + 1].polyAzi[j] /*q22*/, Q22_360_DEG /*360 q22*/ ); /* Q22 */
478 7791 : move32();
479 :
480 : /* Copy the rest of the fields */
481 7791 : efap->polyData.polysetArray[m + 1].chan[j] = efap->polyData.polysetArray[m].chan[j];
482 7791 : move16();
483 7791 : efap->polyData.polysetArray[m + 1].polyEle[j] = efap->polyData.polysetArray[m].polyEle[j]; /* Q22 */
484 7791 : move32();
485 7791 : efap->polyData.polysetArray[m + 1].isNaN[j] = efap->polyData.polysetArray[m].isNaN[j];
486 7791 : move16();
487 7791 : efap->polyData.polysetArray[m + 1].numChan = lengthTri2PolySorted[n];
488 7791 : move16();
489 :
490 7791 : efap->polyData.polysetArray[m + 2].chan[j] = efap->polyData.polysetArray[m].chan[j];
491 7791 : move16();
492 7791 : efap->polyData.polysetArray[m + 2].polyEle[j] = efap->polyData.polysetArray[m].polyEle[j]; /* Q22 */
493 7791 : move32();
494 7791 : efap->polyData.polysetArray[m + 2].isNaN[j] = efap->polyData.polysetArray[m].isNaN[j];
495 7791 : move16();
496 7791 : efap->polyData.polysetArray[m + 2].numChan = lengthTri2PolySorted[n];
497 7791 : move16();
498 : }
499 2451 : finalLength = add( finalLength, 2 );
500 : }
501 14595 : finalLength = add( finalLength, 1 );
502 : }
503 872 : finalLength = add( finalLength, 1 );
504 :
505 : /* Updating the number of polygons */
506 872 : efap->polyData.numPoly = finalLength;
507 872 : move16();
508 :
509 :
510 872 : return error;
511 : }
512 :
513 :
514 : /*-------------------------------------------------------------------------*
515 : * sphere_triangulation()
516 : *
517 : *
518 : *-------------------------------------------------------------------------*/
519 :
520 872 : static ivas_error sphere_triangulation_fx(
521 : const Word16 numSpk, /* i : Number of speakers */
522 : EFAP_VERTEX_DATA *vtxData, /* i/o: Vertex data structure */
523 : EFAP_POLYSET_DATA *polyData, /* o : Polygon data structure */
524 : Word32 ***dmTranspose_fx, /* o : Transpose of the downmix matrix Q31 */
525 : Word16 *numTot, /* o : Number of speakers (real + ghost) */
526 : const Word16 efip_flag /* i : flag to indicate whether initialization is for EFIP (used for ALLRAD) */
527 : )
528 : {
529 : Word16 i;
530 : void **p_dmTranspose;
531 : Word16 vtxInHull[EFAP_MAX_SIZE_TMP_BUFF];
532 :
533 872 : set16_fx( vtxInHull, 0, EFAP_MAX_SIZE_TMP_BUFF );
534 :
535 : /* Add Imaginary Speakers */
536 872 : add_ghost_speakers_fx( vtxData->vertexArray, &vtxData->numVtx, efip_flag );
537 :
538 : /* Sort the vertices according to their index */
539 872 : IF( ( vtxData->vtxOrder = (Word16 *) malloc( vtxData->numVtx * sizeof( Word16 ) ) ) == NULL )
540 : {
541 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP Vertex Order\n" ) );
542 : }
543 :
544 872 : sort_vertices_fx( vtxData->vertexArray, &vtxData->numVtx, vtxData->vtxOrder );
545 :
546 : /* Computing the initial Polyeder */
547 872 : initial_polyeder_fx( vtxData, polyData->triArray, &polyData->numTri, &vtxInHull[0] );
548 :
549 : /* Add the vertex to the convex hull */
550 10272 : FOR( i = 0; i < vtxData->numVtx; ++i )
551 : {
552 9400 : add_vertex_to_convex_hull_fx( vtxData, vtxData->vtxOrder[i], &vtxInHull[0], polyData->triArray, &polyData->numTri );
553 : }
554 :
555 872 : assert( polyData->numTri != 0 && "EFAP: failed to construct convex hull!" );
556 :
557 : /* Allocate the DM matrix transpose */
558 872 : IF( ( p_dmTranspose = malloc( vtxData->numVtx * sizeof( Word32 * ) ) ) == NULL )
559 : {
560 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "EFAP: can not allocate memory for dmTranspose\n" ) );
561 : }
562 :
563 : /* Store the value of numVtx to be used for freeing later (numVtx will change after remap_ghosts() ) */
564 872 : *numTot = vtxData->numVtx;
565 872 : move16();
566 :
567 10272 : FOR( i = 0; i < vtxData->numVtx; i++ ){
568 9400 : IF( ( p_dmTranspose[i] = malloc( numSpk * sizeof( Word32 ) ) ) == NULL ){
569 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "EFAP: can not allocate memory for dmTranspose\n" ) );
570 : }
571 : }
572 872 : *dmTranspose_fx = (Word32 **) p_dmTranspose;
573 :
574 : /* Remap Ghosts */
575 872 : remap_ghosts_fx( vtxData->vertexArray, polyData->triArray, numSpk, &vtxData->numVtx, polyData->numTri, *dmTranspose_fx ); // dmTranspose_fx q31
576 :
577 872 : return IVAS_ERR_OK;
578 : }
579 :
580 :
581 : /*-------------------------------------------------------------------------*
582 : * initial_polyeder()
583 : *
584 : *
585 : *-------------------------------------------------------------------------*/
586 :
587 :
588 872 : static void initial_polyeder_fx(
589 : EFAP_VERTEX_DATA *vtxData, /* i : Vertex data structure */
590 : EFAP_LS_TRIANGLE *triArray, /* o : Triangle array structure */
591 : Word16 *numTri, /* o : Size of triangle array */
592 : Word16 *vtxInHull /* o : Flag if the vertex was added to the inital simplex */
593 : )
594 : {
595 : Word16 i;
596 : Word16 numVtx;
597 : Word16 tmpSurface[3];
598 : Word16 tetrahedron[4];
599 : Word32 tmp;
600 : Word32 centroid[3];
601 : Word32 tmpCross[3];
602 : Word32 tmp1[3], tmp2[3], tmp3[3];
603 :
604 : /* Safety check */
605 872 : assert( triArray != NULL && "EFAP: triArray==NULL" );
606 :
607 : /* initialize variables */
608 872 : set16_fx( tmpSurface, -1, 3 );
609 872 : set32_fx( centroid, 0, 3 );
610 872 : set32_fx( tmp1, 0, 3 );
611 872 : set32_fx( tmp2, 0, 3 );
612 872 : set32_fx( tmp3, 0, 3 );
613 872 : set32_fx( tmpCross, 0, 3 );
614 :
615 872 : numVtx = vtxData->numVtx;
616 872 : move16();
617 :
618 : /* seed vertices */
619 4360 : FOR( i = 0; i < 4; i++ )
620 : {
621 3488 : tetrahedron[i] = i;
622 3488 : move16();
623 : }
624 :
625 : /* 1. attempt to create an edge with nonzero length */
626 872 : test();
627 872 : WHILE( EQ_32( vtxData->vertexArray[tetrahedron[0]].azi /*q22*/, vtxData->vertexArray[tetrahedron[1]].azi /*q22*/ ) &&
628 : EQ_32( vtxData->vertexArray[tetrahedron[0]].ele /*q22*/, vtxData->vertexArray[tetrahedron[1]].ele /*q22*/ ) )
629 : {
630 0 : test();
631 0 : tetrahedron[1] = add( tetrahedron[1], 1 );
632 0 : move16();
633 0 : assert( tetrahedron[1] < numVtx && "EFAP: convex hull construction failed, vertices are coincident!" );
634 : }
635 :
636 : /* 2. attempt to create a triangle with nonzero area */
637 872 : tmp = 0;
638 872 : move32();
639 872 : v_sub_fixed( vtxData->vertexArray[tetrahedron[1]].pos, vtxData->vertexArray[tetrahedron[0]].pos, tmp1, 3, 1 ); // tmp1 Q(31-1)
640 872 : WHILE( LT_16( tetrahedron[2], numVtx ) )
641 : {
642 872 : v_sub_fixed( vtxData->vertexArray[tetrahedron[2]].pos, vtxData->vertexArray[tetrahedron[0]].pos, tmp2, 3, 1 ); // tmp2 Q(31-1)
643 872 : efap_crossp_fx( tmp1, tmp2, tmpCross ); // tmpCross Q29
644 3488 : FOR( i = 0; i < 3; i++ )
645 : {
646 2616 : tmp = L_add( tmp, Mpy_32_32( tmpCross[i], tmpCross[i] ) ); // tmp Q27
647 : }
648 872 : IF( GT_32( L_abs( tmp ), ( POLY_THRESH_Q29 /*1e-4f Q29*/ * POLY_THRESH_Q29 /*1e-4f Q29*/ ) >> 31 ) ) /* compare tmp against POLY_THRESH^2 instead of sqrtf(tmp) */
649 : {
650 872 : BREAK;
651 : }
652 0 : tetrahedron[2] = add( tetrahedron[2], 1 );
653 0 : move16();
654 : }
655 872 : assert( tetrahedron[2] < numVtx && "EFAP: convex hull construction failed, vertices are colinear!" );
656 :
657 : /* 3. attempt to create a tetrahedron with nonzero volume */
658 872 : tmp = 0;
659 872 : move32();
660 3123 : WHILE( LT_16( tetrahedron[3], numVtx ) )
661 : {
662 3123 : v_sub_fixed( vtxData->vertexArray[tetrahedron[3]].pos, vtxData->vertexArray[tetrahedron[0]].pos, tmp3, 3, 1 ); // tmp3 Q30
663 3123 : tmp = dotp_fixed( tmp3, tmpCross, 3 ); // tmp Q28
664 3123 : IF( GT_32( L_abs( tmp ), POLY_THRESH_Q28 /*1e-4f Q28*/ ) )
665 : {
666 872 : BREAK;
667 : }
668 2251 : tetrahedron[3] = add( tetrahedron[3], 1 );
669 2251 : move16();
670 : }
671 872 : assert( tetrahedron[3] < numVtx && "EFAP: convex hull construction failed, vertices are coplanar!" );
672 :
673 : /* 4. compute the centroid of the initial tetrahedron */
674 4360 : FOR( i = 0; i < 4; i++ )
675 : {
676 3488 : vtxInHull[tetrahedron[i]] = 1; /* set vertex as added to hull*/
677 3488 : move16();
678 3488 : centroid[0] = L_add( centroid[0], L_shr( vtxData->vertexArray[tetrahedron[i]].pos[0], Q2 ) ); // Q29
679 3488 : move32();
680 3488 : centroid[1] = L_add( centroid[1], L_shr( vtxData->vertexArray[tetrahedron[i]].pos[1], Q2 ) ); // Q29
681 3488 : move32();
682 3488 : centroid[2] = L_add( centroid[2], L_shr( vtxData->vertexArray[tetrahedron[i]].pos[2], Q2 ) ); // Q29
683 3488 : move32();
684 : }
685 : /* Executed below float operation
686 : centroid[0] /= 4;
687 : centroid[1] /= 4;
688 : centroid[2] /= 4;
689 : */
690 :
691 : /* 5. create and orient planes */
692 872 : tmpSurface[0] = tetrahedron[0];
693 872 : move16();
694 872 : tmpSurface[1] = tetrahedron[1];
695 872 : move16();
696 872 : tmpSurface[2] = tetrahedron[2];
697 872 : move16();
698 872 : flip_plane_fx( vtxData->vertexArray, tmpSurface, centroid );
699 872 : Copy( tmpSurface, triArray[0].LS, 3 );
700 :
701 872 : tmpSurface[0] = tetrahedron[0];
702 872 : move16();
703 872 : tmpSurface[1] = tetrahedron[1];
704 872 : move16();
705 872 : tmpSurface[2] = tetrahedron[3];
706 872 : move16();
707 872 : flip_plane_fx( vtxData->vertexArray, tmpSurface, centroid );
708 872 : Copy( tmpSurface, triArray[1].LS, 3 );
709 :
710 872 : tmpSurface[0] = tetrahedron[0];
711 872 : move16();
712 872 : tmpSurface[1] = tetrahedron[2];
713 872 : move16();
714 872 : tmpSurface[2] = tetrahedron[3];
715 872 : move16();
716 872 : flip_plane_fx( vtxData->vertexArray, tmpSurface, centroid );
717 872 : Copy( tmpSurface, triArray[2].LS, 3 );
718 :
719 872 : tmpSurface[0] = tetrahedron[1];
720 872 : move16();
721 872 : tmpSurface[1] = tetrahedron[2];
722 872 : move16();
723 872 : tmpSurface[2] = tetrahedron[3];
724 872 : move16();
725 872 : flip_plane_fx( vtxData->vertexArray, tmpSurface, centroid );
726 872 : Copy( tmpSurface, triArray[3].LS, 3 );
727 :
728 : /* set numTri */
729 872 : *numTri = 4;
730 872 : move16();
731 :
732 872 : return;
733 : }
734 :
735 :
736 : /*-------------------------------------------------------------------------*
737 : * add_ghost_speakers_fx()
738 : *
739 : *
740 : *-------------------------------------------------------------------------*/
741 :
742 872 : static void add_ghost_speakers_fx(
743 : EFAP_VERTEX *vertexArray, /* i/o: Vertex array */
744 : Word16 *numVtx, /* i/o: Size of vertex array */
745 : const Word16 efip_flag /* i : flag to indicate whether initialization is for EFIP (used for ALLRAD) */
746 : )
747 : {
748 : Word16 numVertex;
749 : Word16 lengthVertGhst; /* Nb of vertical ghost added */
750 : Word16 lengthHorGhst; /* Nb of Horizontal Ghost */
751 : Word16 i, j, k, a; /* Integer for loops */
752 : Word16 num_new; /* Number of new vertices to add */
753 : Word32 maxAngle; /* Max azimuth tolerance for extend the LS setup horizontaly */
754 : Word32 newDiff; /* Angle differences that will help us set the extended LS setup */
755 : Word32 newAzi; /* New azimuth for the new horizontal LS */
756 : Word32 ele[EFAP_MAX_SIZE_TMP_BUFF];
757 : Word32 tmpEle;
758 : Word32 tmpAzi[EFAP_MAX_SIZE_TMP_BUFF];
759 : Word32 tmpAngleDiff[EFAP_MAX_SIZE_TMP_BUFF]; /* tmp array of angles differences */
760 : Word32 sectors[EFAP_MAX_SIZE_TMP_BUFF]; /* Help us determine the zone where we should extend LS */
761 : EFAP_VTX_DMX_TYPE vtxDmxType;
762 : Word16 new_diff_e;
763 :
764 872 : vtxDmxType = EFAP_DMX_INTENSITY;
765 872 : move32();
766 872 : numVertex = *numVtx;
767 872 : move16();
768 872 : maxAngle = 13421773; //(1.f / 160.0f) in Q31
769 872 : move32();
770 :
771 : /* Extracting Azi and Ele for computation purposes */
772 8298 : FOR( i = 0; i < numVertex; ++i )
773 : {
774 7426 : ele[i] = vertexArray[i].ele; // q22
775 7426 : move32();
776 : }
777 :
778 : /* ADD VOG IF NECESSERAY (i.e. if the elevation of the highest LS is < 90 deg) */
779 872 : a = 0;
780 872 : move16();
781 872 : maximum_l( ele, numVertex, &tmpEle ); // tmpEle q22
782 :
783 872 : lengthVertGhst = 0;
784 872 : move16();
785 872 : IF( LT_32( tmpEle, Q22_90_DEG /*90 q22*/ ) )
786 : {
787 872 : IF( efip_flag )
788 : {
789 83 : IF( GT_32( tmpEle, Q22_45_DEG /*45 q22*/ ) )
790 : {
791 3 : vtxDmxType = EFAP_DMX_NONE;
792 3 : move32();
793 : }
794 : ELSE
795 : {
796 80 : vtxDmxType = EFAP_DMX_AMPLITUDE;
797 80 : move32();
798 : }
799 : }
800 872 : add_vertex_fx( vertexArray, 0, Q22_90_DEG /*90 q22*/, add( numVertex, a ), vtxDmxType );
801 872 : lengthVertGhst = add( lengthVertGhst, 1 );
802 872 : a = add( a, 1 );
803 : }
804 :
805 : /* ADD VOH IF NECESSERAY (i.e. if the elevation of the lowest LS is > -90 deg) */
806 872 : minimum_l( ele, numVertex, &tmpEle ); /*tmpEle q22*/
807 872 : IF( GT_32( tmpEle, -Q22_90_DEG /*90 q22*/ ) )
808 : {
809 872 : IF( efip_flag )
810 : {
811 83 : IF( LT_32( tmpEle, -Q22_45_DEG /*45 q22*/ ) )
812 : {
813 3 : vtxDmxType = EFAP_DMX_NONE;
814 3 : move32();
815 : }
816 : ELSE
817 : {
818 80 : vtxDmxType = EFAP_DMX_AMPLITUDE;
819 80 : move32();
820 : }
821 : }
822 :
823 872 : add_vertex_fx( vertexArray, 0, -Q22_90_DEG /*90 q22*/, add( numVertex, a ), vtxDmxType );
824 :
825 872 : lengthVertGhst = add( lengthVertGhst, 1 );
826 872 : a = add( a, 1 );
827 : }
828 :
829 : /* LIST ALL SURROUNDING loudspeakers */
830 872 : k = 0;
831 872 : move16();
832 :
833 8298 : FOR( i = 0; i < numVertex; ++i )
834 : {
835 7426 : IF( LT_32( L_abs( vertexArray[i].ele ), Q22_45_DEG /*45 q22*/ ) )
836 : {
837 7222 : tmpAzi[k] = vertexArray[i].azi; // q22
838 7222 : move32();
839 7222 : k = add( k, 1 );
840 : }
841 : }
842 :
843 872 : lengthHorGhst = 0;
844 872 : move16();
845 872 : IF( k == 0 ) /* no speakers found: add a triangle of ghost speakers */
846 : {
847 0 : add_vertex_fx( vertexArray, 0, 0, add( numVertex, a ), EFAP_DMX_INTENSITY );
848 0 : add_vertex_fx( vertexArray, Q22_120_DEG /*120 q22*/, 0, add( add( numVertex, a ), 1 ), EFAP_DMX_INTENSITY );
849 0 : add_vertex_fx( vertexArray, Q22_240_DEG /*240 q22*/, 0, add( add( numVertex, a ), 2 ), EFAP_DMX_INTENSITY );
850 0 : a = add( a, 3 );
851 0 : lengthHorGhst = add( lengthHorGhst, 3 );
852 : }
853 872 : ELSE IF( EQ_16( k, 1 ) ) /* only one speaker found: add two ghost speakers to complete a triangle */
854 : {
855 89 : add_vertex_fx( vertexArray, L_add( tmpAzi[0], Q22_120_DEG /*120 q22*/ ), 0, add( numVertex, a ), EFAP_DMX_INTENSITY );
856 89 : add_vertex_fx( vertexArray, L_add( tmpAzi[0], Q22_240_DEG /*240 q22*/ ), 0, add( add( numVertex, a ), 1 ), EFAP_DMX_INTENSITY );
857 89 : a = add( a, 2 );
858 89 : lengthHorGhst = add( lengthHorGhst, 2 );
859 : }
860 : ELSE /* fill gaps greater than maxAngle */
861 : {
862 : /* Here, k correspond to the number of LS whose ele is < 45 deg, should be = numVertex */
863 783 : sort_l( tmpAzi, k ); // tmpAzi q22
864 :
865 : /* The next lines correspond to angle_diff = [azi(2:end), azi(1) + 360] - azi; in Matlab */
866 7133 : FOR( i = 0; i < k - 1; ++i )
867 : {
868 6350 : tmpAngleDiff[i] = L_sub( tmpAzi[i + 1], tmpAzi[i] ); // q22
869 6350 : move32();
870 6350 : sectors[i] = ceil_fixed( Mpy_32_32( tmpAngleDiff[i], maxAngle ), Q22 ); // q22
871 6350 : move32();
872 :
873 6350 : if ( GT_32( sectors[i], Q22_1 /*1 q22*/ ) )
874 : {
875 0 : lengthHorGhst = add( lengthHorGhst, 1 );
876 : }
877 : }
878 783 : tmpAngleDiff[k - 1] = L_sub( L_add( tmpAzi[0], Q22_360_DEG /*360 q22*/ ), tmpAzi[k - 1] ); // q22
879 :
880 783 : sectors[k - 1] = ceil_fixed( Mpy_32_32( tmpAngleDiff[k - 1], maxAngle ), Q22 ); // q22
881 :
882 783 : if ( GT_32( sectors[k - 1], Q22_1 /*1 q22*/ ) )
883 : {
884 52 : lengthHorGhst = add( lengthHorGhst, 1 );
885 : }
886 :
887 : /* Adding new virtual speakers */
888 7916 : FOR( i = 0; i < k; ++i )
889 : {
890 7133 : IF( GT_32( sectors[i], Q22_1 /*1 q22*/ ) )
891 : {
892 52 : newDiff = BASOP_Util_Divide3232_Scale( tmpAngleDiff[i], sectors[i], &new_diff_e ); // Q=15-new_diff_e
893 52 : newDiff = L_shl( newDiff, add( new_diff_e, 7 ) ); // q22
894 52 : num_new = extract_l( L_shr( L_sub( sectors[i], Q22_1 /*1 q22*/ ), Q22 ) ); // q0
895 :
896 104 : FOR( j = 0; j < num_new; ++j )
897 : {
898 52 : newAzi = L_add( tmpAzi[i], imult3216( newDiff, add( j, 1 ) ) ); // q22
899 :
900 52 : add_vertex_fx( vertexArray, newAzi, 0, add( numVertex, a ), EFAP_DMX_INTENSITY );
901 52 : a = add( a, 1 );
902 :
903 52 : if ( j > 0 )
904 : {
905 0 : lengthHorGhst = add( lengthHorGhst, 1 );
906 : }
907 : }
908 : }
909 : }
910 : }
911 872 : *numVtx = add( add( numVertex, lengthHorGhst ), lengthVertGhst );
912 872 : move16();
913 :
914 872 : return;
915 : }
916 :
917 : /*-------------------------------------------------------------------------*
918 : * sort_vertices()
919 : *
920 : *
921 : *-------------------------------------------------------------------------*/
922 :
923 872 : static void sort_vertices_fx(
924 : const EFAP_VERTEX *vertexArray, /* i : Vertex array */
925 : const Word16 *numVtx, /* i : Size of vertex array */
926 : Word16 *order /* o : Original index positions */
927 : )
928 : {
929 : Word16 tmpIdx[EFAP_MAX_SIZE_TMP_BUFF];
930 : Word16 i;
931 :
932 : /* Initializing tmpIdx */
933 10272 : FOR( i = 0; i < *numVtx; ++i )
934 : {
935 9400 : tmpIdx[i] = vertexArray[i].idx;
936 9400 : move16();
937 : }
938 :
939 : /* Sorting indexes */
940 872 : efap_sort_s_fx( tmpIdx, order, *numVtx );
941 :
942 872 : return;
943 : }
944 :
945 : /*-------------------------------------------------------------------------*
946 : * add_vertex_to_convex_hull_fx()
947 : *
948 : *
949 : *-------------------------------------------------------------------------*/
950 :
951 9400 : static void add_vertex_to_convex_hull_fx(
952 : const EFAP_VERTEX_DATA *vtxData, /* i : Vertex data structure */
953 : const Word16 vtxIdx, /* i : Vertex to be added to the hull */
954 : Word16 *vtxInHull, /* i/o: Array indicating whether the vertex is part of the hull */
955 : EFAP_LS_TRIANGLE *triArray, /* i/o: Triangle array */
956 : Word16 *szTri /* i/o: Size of Triangle array */
957 : )
958 : {
959 : Word16 i, k, l;
960 : Word16 visible[EFAP_MAX_SIZE_TMP_BUFF];
961 : Word16 edges[EFAP_MAX_SIZE_TMP_BUFF];
962 : Word16 numEdges[1];
963 : Word16 surface[3];
964 : Word32 numHullVtx;
965 : Word32 centroid[3];
966 9400 : const Word32 threshold = -268; // -1e-6f in Q28
967 9400 : move32();
968 : Word32 tmpDist;
969 : EFAP_LS_TRIANGLE triArrayNew[EFAP_MAX_POLY_SET];
970 : Word16 tmp16, tmp_e;
971 : Word32 tmp32;
972 :
973 : /* If the vertex is already part of the hull, nothing must be done */
974 9400 : IF( vtxInHull[vtxIdx] )
975 : {
976 3488 : return;
977 : }
978 :
979 : /* Compute the centroid of the current convex hull */
980 5912 : numHullVtx = 0;
981 5912 : move32();
982 5912 : set32_fx( centroid, 0, 3 );
983 80876 : FOR( i = 0; i < vtxData->numVtx; i++ )
984 : {
985 74964 : IF( vtxInHull[i] )
986 : {
987 46350 : numHullVtx = L_add( numHullVtx, 1 );
988 46350 : centroid[0] = L_add( centroid[0], L_shr( vtxData->vertexArray[i].pos[0], Q4 ) ); // q27
989 46350 : move32();
990 46350 : centroid[1] = L_add( centroid[1], L_shr( vtxData->vertexArray[i].pos[1], Q4 ) ); // q27
991 46350 : move32();
992 46350 : centroid[2] = L_add( centroid[2], L_shr( vtxData->vertexArray[i].pos[2], Q4 ) ); // q27
993 46350 : move32();
994 : }
995 : }
996 : /* numHullVtx = 1.0f / numHullVtx; */
997 5912 : tmp16 = BASOP_Util_Divide3232_Scale( 1, numHullVtx, &tmp_e ); // q15-tmp_e
998 5912 : tmp32 = L_shl_sat( tmp16, add( Q16, tmp_e ) ); /* Q31 */
999 :
1000 5912 : centroid[0] = Mpy_32_32( centroid[0], tmp32 ); // q27
1001 5912 : move32();
1002 5912 : centroid[1] = Mpy_32_32( centroid[1], tmp32 ); // q27
1003 5912 : move32();
1004 5912 : centroid[2] = Mpy_32_32( centroid[1], tmp32 ); // q27
1005 5912 : move32();
1006 :
1007 5912 : centroid[0] = L_shl( centroid[0], 4 ); // q31
1008 5912 : move32();
1009 5912 : centroid[1] = L_shl( centroid[1], 4 ); // q31
1010 5912 : move32();
1011 5912 : centroid[2] = L_shl( centroid[2], 4 ); // q31
1012 5912 : move32();
1013 :
1014 : /* Processing */
1015 5912 : k = 0;
1016 5912 : move16();
1017 5912 : l = 0;
1018 5912 : move16();
1019 :
1020 74964 : FOR( i = 0; i < *szTri; ++i )
1021 : {
1022 69052 : tmpDist = vertex_distance_fx( vtxData->vertexArray, triArray[i], vtxIdx ); // Q28
1023 69052 : IF( GT_32( tmpDist, threshold ) )
1024 : {
1025 14507 : visible[k] = i;
1026 14507 : move16();
1027 14507 : k = add( k, 1 );
1028 : }
1029 : ELSE
1030 : {
1031 54545 : Copy( triArray[i].LS, triArrayNew[l].LS, 3 );
1032 54545 : l = add( l, 1 );
1033 : }
1034 : }
1035 :
1036 5912 : visible_edges_fx( triArray, visible, k, numEdges, edges );
1037 :
1038 32243 : FOR( i = 0; i < numEdges[0]; i += 2 )
1039 : {
1040 26331 : surface[0] = edges[i];
1041 26331 : move16();
1042 26331 : surface[1] = edges[i + 1];
1043 26331 : move16();
1044 26331 : surface[2] = vtxIdx;
1045 26331 : move16();
1046 :
1047 26331 : flip_plane_fx( vtxData->vertexArray, surface, centroid );
1048 :
1049 26331 : Copy( surface, triArrayNew[l].LS, 3 );
1050 26331 : l = add( l, 1 );
1051 : }
1052 :
1053 : /* Outputs */
1054 86788 : FOR( i = 0; i < l; i++ )
1055 : {
1056 80876 : Copy( triArrayNew[i].LS, triArray[i].LS, 3 );
1057 : }
1058 5912 : *szTri = l;
1059 5912 : move16();
1060 :
1061 : /* Flag the vertex as added to the hull */
1062 5912 : vtxInHull[vtxIdx] = 1;
1063 5912 : move16();
1064 5912 : return;
1065 : }
1066 :
1067 : /*-------------------------------------------------------------------------*
1068 : * visible_edges()
1069 : *
1070 : *
1071 : *-------------------------------------------------------------------------*/
1072 :
1073 5912 : static void visible_edges_fx(
1074 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
1075 : const Word16 *visible, /* i : Visible surface flag */
1076 : const Word16 numSurface, /* i : Number of surfaces */
1077 : Word16 *numEdges, /* i/o: Number of edges */
1078 : Word16 *edges /* i/o: Array of edges */
1079 : )
1080 : {
1081 : Word16 maxVertex;
1082 : Word16 i, j, k;
1083 : Word16 a, b;
1084 : Word16 tmpSurface[4];
1085 : Word16 counter[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF];
1086 : Word16 counterTranspose[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF];
1087 : Word16 tmpMax[EFAP_MAX_SIZE_TMP_BUFF];
1088 :
1089 : /* Set counter and counterTranspose to 0 */
1090 183272 : FOR( i = 0; i < EFAP_MAX_SIZE_TMP_BUFF; i++ )
1091 : {
1092 177360 : set16_fx( counter[i], 0, EFAP_MAX_SIZE_TMP_BUFF );
1093 177360 : set16_fx( counterTranspose[i], 0, EFAP_MAX_SIZE_TMP_BUFF );
1094 : }
1095 :
1096 : /* Finding the max vertex */
1097 20419 : FOR( i = 0; i < numSurface; ++i )
1098 : {
1099 14507 : tmpMax[i] = triArray[visible[i]].LS[0]; // q0
1100 14507 : move16();
1101 43521 : FOR( j = 1; j < 3; ++j )
1102 : {
1103 29014 : if ( LT_16( tmpMax[i], triArray[visible[i]].LS[j] ) )
1104 : {
1105 14662 : tmpMax[i] = triArray[visible[i]].LS[j]; // q0
1106 14662 : move16();
1107 : }
1108 : }
1109 : }
1110 5912 : maxVertex = tmpMax[maximum_s( tmpMax, numSurface, NULL )];
1111 20419 : FOR( i = 0; i < numSurface; ++i )
1112 : {
1113 14507 : tmpSurface[0] = triArray[visible[i]].LS[0];
1114 14507 : move16();
1115 14507 : tmpSurface[1] = triArray[visible[i]].LS[1];
1116 14507 : move16();
1117 14507 : tmpSurface[2] = triArray[visible[i]].LS[2];
1118 14507 : move16();
1119 14507 : tmpSurface[3] = triArray[visible[i]].LS[0];
1120 14507 : move16();
1121 :
1122 58028 : FOR( j = 0; j < 3; ++j )
1123 : {
1124 43521 : a = tmpSurface[j];
1125 43521 : move16();
1126 43521 : b = tmpSurface[j + 1];
1127 43521 : move16();
1128 43521 : counter[a][b] = add( counter[a][b], 1 );
1129 43521 : move16();
1130 43521 : counterTranspose[b][a] = counter[a][b];
1131 43521 : move16();
1132 : }
1133 : }
1134 :
1135 75152 : FOR( i = 0; i < maxVertex + 1; ++i )
1136 : {
1137 943292 : FOR( j = 0; j < maxVertex + 1; ++j )
1138 : {
1139 874052 : counter[i][j] = add( counterTranspose[i][j], counterTranspose[j][i] );
1140 874052 : move16();
1141 : }
1142 : }
1143 :
1144 : /* Finding the edges */
1145 5912 : k = 0;
1146 5912 : move16();
1147 :
1148 69240 : FOR( a = 0; a < maxVertex; ++a )
1149 : {
1150 465734 : FOR( b = a + 1; b < maxVertex + 1; ++b )
1151 : {
1152 402406 : IF( EQ_16( counter[a][b], 1 ) )
1153 : {
1154 26331 : edges[k] = a;
1155 26331 : move16();
1156 26331 : edges[k + 1] = b;
1157 26331 : move16();
1158 26331 : k = add( k, 2 );
1159 : }
1160 : }
1161 : }
1162 :
1163 : /* Outputs */
1164 5912 : *numEdges = k;
1165 5912 : move16();
1166 :
1167 5912 : return;
1168 : }
1169 :
1170 : /*-------------------------------------------------------------------------* \
1171 : * flip_plane() \
1172 : * \
1173 : * \
1174 : *-------------------------------------------------------------------------*/
1175 :
1176 29819 : static void flip_plane_fx(
1177 : const EFAP_VERTEX *vtxArray, /* i : Vertex array */
1178 : Word16 *surface, /* i/o: Surface/vertices to be flipped */
1179 : const Word32 centroid[3] /* i : Centroid of convex hull from which to orient the planes outward q31*/
1180 : )
1181 : {
1182 : Word16 tmp;
1183 : Word32 dist;
1184 :
1185 29819 : dist = point_plane_distance_fx(
1186 29819 : vtxArray[surface[0]].pos,
1187 29819 : vtxArray[surface[1]].pos,
1188 29819 : vtxArray[surface[2]].pos,
1189 : centroid ); // q31
1190 :
1191 29819 : IF( dist > 0 )
1192 : {
1193 : /*efap_flipLeftRight( surface, 3 );*/
1194 15662 : tmp = surface[0];
1195 15662 : move16();
1196 15662 : surface[0] = surface[2];
1197 15662 : move16();
1198 15662 : surface[2] = tmp;
1199 15662 : move16();
1200 : }
1201 :
1202 29819 : return;
1203 : }
1204 :
1205 :
1206 : /*-------------------------------------------------------------------------*
1207 : * remap_ghosts()
1208 : *
1209 : *
1210 : *-------------------------------------------------------------------------*/
1211 872 : static void remap_ghosts_fx(
1212 : EFAP_VERTEX *vtxArray, /* i/o: Vertex array */
1213 : EFAP_LS_TRIANGLE *triArray, /* i/o: Triangle array */
1214 : Word16 numSpk, /* i : Number of speakers */
1215 : Word16 *numVertex, /* i/o: Size of vertex array */
1216 : Word16 numTri, /* i : Size of triangle array */
1217 : Word32 **downmixMatrixTranspose /* o : Transpose of downmix matrix Q31 */
1218 : )
1219 : {
1220 872 : Word16 numGhst = 0;
1221 872 : Word16 numVtx = *numVertex;
1222 : Word16 numTot;
1223 : Word16 g;
1224 : Word16 i, j;
1225 : Word16 tmpL;
1226 : Word32 inv_tmpL;
1227 : Word16 posFound[2];
1228 : Word16 neighbours[EFAP_MAX_SIZE_TMP_BUFF];
1229 : Word32 tmpVec[EFAP_MAX_SIZE_TMP_BUFF];
1230 : Word32 tmpVec2[EFAP_MAX_SIZE_TMP_BUFF];
1231 : Word32 tmpMat[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF]; /* Q31 */
1232 : Word32 tmpNewMat[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF]; /* Q31 */
1233 : Word32 tmpDist;
1234 : Word16 tmpDist_e;
1235 872 : const Word32 thresh = 214748; // 1e-4f in Q31
1236 : Word16 tmp16, tmp_e;
1237 872 : move32();
1238 :
1239 872 : set32_fx( tmpVec, 0, EFAP_MAX_SIZE_TMP_BUFF );
1240 872 : set32_fx( tmpVec2, 0, EFAP_MAX_SIZE_TMP_BUFF );
1241 :
1242 : /* Finding unused ghosts (i.e. ghost speakers that aren't used for triangulation */
1243 2846 : FOR( g = numVtx - 1; g > numSpk - 1; --g )
1244 : {
1245 : /* find(triangle_mat == ghost, 1, 'first') */
1246 1974 : IF( find_int_in_tri_fx( triArray, g, numTri, posFound ) == 0 )
1247 : {
1248 0 : remove_vertex_fx( vtxArray, g, numVtx );
1249 0 : numVtx = sub( numVtx, 1 );
1250 0 : FOR( i = 0; i < numTri; ++i )
1251 : {
1252 0 : FOR( j = 0; j < 3; ++j )
1253 : {
1254 0 : IF( GT_16( triArray[i].LS[j], g ) )
1255 : {
1256 0 : triArray[i].LS[j] = sub( g, 1 );
1257 0 : move16();
1258 : }
1259 : }
1260 : }
1261 : }
1262 : ELSE
1263 : {
1264 1974 : numGhst = add( numGhst, 1 );
1265 : }
1266 : }
1267 :
1268 : /* Final number of LS (real + ghosts) */
1269 872 : numTot = add( numSpk, numGhst );
1270 :
1271 : /* Initializing tmpMat as the identity matrix */
1272 10272 : FOR( i = 0; i < numTot; ++i )
1273 : {
1274 9400 : set32_fx( tmpMat[i], 0, numTot );
1275 9400 : set32_fx( tmpNewMat[i], 0, numTot );
1276 :
1277 9400 : tmpMat[i][i] = ONE_IN_Q31; // q31
1278 9400 : move32();
1279 9400 : tmpNewMat[i][i] = ONE_IN_Q31; // q31
1280 9400 : move32();
1281 : }
1282 :
1283 : /* Generate initial sound energy distribution matrix */
1284 2846 : FOR( i = numSpk; i < numTot; ++i )
1285 : {
1286 1974 : tmpL = get_neighbours_fx( triArray, i, numTri, neighbours );
1287 :
1288 : /* Initializing the column to 0 */
1289 21924 : FOR( j = 0; j < numTot; ++j )
1290 : {
1291 19950 : tmpMat[j][i] = 0;
1292 19950 : move32();
1293 19950 : tmpNewMat[j][i] = 0;
1294 19950 : move32();
1295 : }
1296 :
1297 : /* The neighbours are set to 1.0/tmpL */
1298 1974 : tmp16 = BASOP_Util_Divide3232_Scale( 1, tmpL, &tmp_e ); // Q=15-tmp_e
1299 1974 : inv_tmpL = L_shl_sat( tmp16, add( Q16, tmp_e ) ); /* Q31 */
1300 11290 : FOR( j = 0; j < tmpL; ++j )
1301 : {
1302 9316 : tmpMat[neighbours[j]][i] = inv_tmpL; /* Q31 */
1303 9316 : move32();
1304 9316 : tmpNewMat[neighbours[j]][i] = inv_tmpL; /* Q31 */
1305 9316 : move32();
1306 : }
1307 : }
1308 :
1309 : /* Redistributing sound energy */
1310 10272 : FOR( i = 0; i < numTot; ++i )
1311 : {
1312 121964 : FOR( j = 0; j < numTot; ++j )
1313 : {
1314 112564 : tmpNewMat[i][j] = tmpMat[j][i]; /* Q31 */
1315 112564 : move32();
1316 : }
1317 : }
1318 :
1319 2846 : FOR( i = numSpk; i < numTot; ++i )
1320 : {
1321 1974 : Copy32( tmpNewMat[i], tmpVec, numTot ); // q31
1322 :
1323 1974 : tmpDist_e = 0;
1324 1974 : move16();
1325 1974 : tmpDist = sum_32_fx( &tmpVec[numSpk], sub( numTot, numSpk ), &tmpDist_e ); // Q=31-tmpDist_e
1326 :
1327 13146 : WHILE( EQ_16( BASOP_Util_Cmp_Mant32Exp( tmpDist, tmpDist_e, thresh, 0 ), 1 ) )
1328 : {
1329 11172 : matrix_times_row_fx( tmpMat, tmpVec, numTot, tmpVec2 ); // tmpVec2 Q31
1330 11172 : Copy32( tmpVec2, tmpVec, numTot ); // Q31
1331 11172 : set32_fx( tmpVec2, 0, numTot );
1332 11172 : tmpDist_e = 0;
1333 11172 : move16();
1334 11172 : tmpDist = sum_32_fx( &tmpVec[numSpk], sub( numTot, numSpk ), &tmpDist_e );
1335 : }
1336 1974 : Copy32( tmpVec, tmpNewMat[i], numTot ); // q31
1337 : }
1338 :
1339 8298 : FOR( i = 0; i < numSpk; ++i )
1340 : {
1341 : /* Applying a sqrt(2) coeff and obtaining the dmMatrix*/
1342 84906 : FOR( j = 0; j < numSpk; ++j )
1343 : {
1344 77480 : test();
1345 77480 : IF( tmpNewMat[j][i] == 0 || EQ_32( tmpNewMat[j][i], 0x7fffffff /*q31*/ ) )
1346 : {
1347 77480 : downmixMatrixTranspose[j][i] = tmpNewMat[j][i]; /* Q31 */
1348 77480 : move32();
1349 : }
1350 : ELSE
1351 : {
1352 0 : Word16 exp = 0;
1353 0 : move16();
1354 0 : Word32 tmp_sqrt = Sqrt32( tmpNewMat[j][i], &exp ); /*31-exp*/
1355 0 : tmp_sqrt = L_shl( tmp_sqrt, exp ); /*q31*/
1356 0 : downmixMatrixTranspose[j][i] = tmp_sqrt; /* Q31 */
1357 0 : move32();
1358 : }
1359 : }
1360 : /* Downmix ghost loudspeakers according to dmxType */
1361 22560 : FOR( ; j < numTot; ++j )
1362 : {
1363 15134 : SWITCH( vtxArray[j].dmxType )
1364 : {
1365 72 : case EFAP_DMX_NONE:
1366 72 : downmixMatrixTranspose[j][i] = 0; /* Q31 */
1367 72 : move32();
1368 72 : BREAK;
1369 1462 : case EFAP_DMX_AMPLITUDE:
1370 1462 : downmixMatrixTranspose[j][i] = tmpNewMat[j][i]; /* Q31 */
1371 1462 : move32();
1372 1462 : BREAK;
1373 13600 : case EFAP_DMX_INTENSITY:
1374 : default:
1375 13600 : test();
1376 13600 : IF( tmpNewMat[j][i] == 0 || EQ_32( tmpNewMat[j][i], ONE_IN_Q31 /*q31*/ ) )
1377 : {
1378 6256 : downmixMatrixTranspose[j][i] = tmpNewMat[j][i]; /* Q31 */
1379 6256 : move32();
1380 : }
1381 : ELSE
1382 : {
1383 7344 : Word16 exp = 0;
1384 7344 : move16();
1385 7344 : Word32 tmp_sqrt = Sqrt32( tmpNewMat[j][i], &exp ); /*31-exp*/
1386 7344 : tmp_sqrt = L_shl( tmp_sqrt, exp ); /*31*/
1387 7344 : downmixMatrixTranspose[j][i] = tmp_sqrt; /* Q31 */
1388 7344 : move32();
1389 : }
1390 13600 : BREAK;
1391 : }
1392 : }
1393 : }
1394 :
1395 : /* Output */
1396 872 : *numVertex = numTot;
1397 872 : move16();
1398 :
1399 872 : return;
1400 : }
1401 :
1402 : /*-------------------------------------------------------------------------*
1403 : * vertex_init_fx()
1404 : *
1405 : * Initialize the vertex structures
1406 : *-------------------------------------------------------------------------*/
1407 :
1408 872 : static void vertex_init_fx(
1409 : const Word32 *aziSpk, /* i : Azimuths of the LS setup q22 */
1410 : const Word32 *eleSpk, /* i : Elevations of the LS setup q22 */
1411 : EFAP_VERTEX_DATA *efapVtxData /* i/o: Vertex data structure that will be updated */
1412 : )
1413 : {
1414 : Word16 i;
1415 :
1416 : /* Main Processing */
1417 8298 : FOR( i = 0; i < efapVtxData->numVtx; i++ )
1418 : {
1419 7426 : add_vertex_fx( efapVtxData->vertexArray, aziSpk[i], eleSpk[i], i, EFAP_DMX_INTENSITY );
1420 : }
1421 :
1422 872 : return;
1423 : }
1424 :
1425 : /*-------------------------------------------------------------------------*
1426 : * efap_panning_fx()
1427 : *
1428 : * Compute the gain without applying the downmix Matrix and the norm of the array
1429 : *-------------------------------------------------------------------------*/
1430 :
1431 848887 : static void efap_panning_fx(
1432 : const Word32 azi, /* i : Value of the azimuth q22 */
1433 : const Word32 ele, /* i : Value of the elevation q22 */
1434 : const EFAP_POLYSET_DATA *polyData, /* i : Polygon data */
1435 : Word32 *bufferL /* o : 1D array of length numSpk that will contain the tmp values q31*/
1436 : )
1437 : {
1438 : Word16 i;
1439 : Word16 polyIdx;
1440 : Word16 numChan;
1441 : Word16 chan[EFAP_MAX_CHAN_NUM];
1442 : Word32 aziPoly[EFAP_MAX_CHAN_NUM];
1443 : Word32 elePoly[EFAP_MAX_CHAN_NUM];
1444 : Word32 tmpBuff[EFAP_MAX_CHAN_NUM];
1445 : Word32 normTmpBuff;
1446 : Word32 P[2];
1447 :
1448 848887 : P[0] = azi; // q22
1449 848887 : move32();
1450 848887 : P[1] = ele; // q22
1451 848887 : move32();
1452 848887 : set32_fx( tmpBuff, 0, EFAP_MAX_CHAN_NUM );
1453 848887 : set32_fx( aziPoly, 0, EFAP_MAX_CHAN_NUM );
1454 848887 : set32_fx( elePoly, 0, EFAP_MAX_CHAN_NUM );
1455 848887 : set16_fx( chan, 0, EFAP_MAX_CHAN_NUM );
1456 :
1457 : /* Finding in which polygon the point is */
1458 848887 : polyIdx = get_poly_num_fx( P, polyData );
1459 :
1460 848887 : assert( polyIdx != -1 && "EFAP: polygon not found!" );
1461 :
1462 : /* Extracting the chan, the azimuth and the ele of the considered poly */
1463 848887 : numChan = polyData->polysetArray[polyIdx].numChan;
1464 848887 : move16();
1465 :
1466 3451248 : FOR( i = 0; i < numChan; ++i )
1467 : {
1468 2602361 : chan[i] = polyData->polysetArray[polyIdx].chan[i];
1469 2602361 : move16();
1470 2602361 : aziPoly[i] = polyData->polysetArray[polyIdx].polyAzi[i]; // q22
1471 2602361 : move32();
1472 :
1473 2602361 : if ( EQ_16( polyData->polysetArray[polyIdx].isNaN[i], 1 ) )
1474 : {
1475 683972 : aziPoly[i] = P[0]; // q22
1476 683972 : move32();
1477 : }
1478 :
1479 2602361 : elePoly[i] = polyData->polysetArray[polyIdx].polyEle[i]; // q22
1480 2602361 : move32();
1481 : }
1482 :
1483 : /* Computing the gain for the polygon */
1484 848887 : get_poly_gains_fx( P[0], P[1], aziPoly, elePoly, numChan, tmpBuff ); // tmpBuff q31
1485 :
1486 : /* Computing the norm of the tmp buffer */
1487 : Word64 suma;
1488 848887 : suma = Mpy_32_32( tmpBuff[0], tmpBuff[0] );
1489 2602361 : FOR( i = 1; i < numChan; i++ )
1490 : {
1491 1753474 : suma = W_add( suma, Mpy_32_32( tmpBuff[i], tmpBuff[i] ) );
1492 : }
1493 848887 : IF( GT_64( suma, MAX_32 ) )
1494 : {
1495 0 : normTmpBuff = MAX_32;
1496 0 : move32();
1497 : }
1498 : ELSE
1499 : {
1500 848887 : normTmpBuff = W_extract_l( suma ); // Q31
1501 : }
1502 :
1503 848887 : Word16 exp = 0;
1504 848887 : move16();
1505 848887 : normTmpBuff = ISqrt32( normTmpBuff, &exp ); // Q=31-exp
1506 :
1507 : /* Updating the buffer structure */
1508 3451248 : FOR( i = 0; i < numChan; ++i )
1509 : {
1510 2602361 : bufferL[chan[i]] = Mpy_32_32( tmpBuff[i], normTmpBuff ); // 31+(31-exp)-31=>31-exp
1511 2602361 : move32();
1512 2602361 : bufferL[chan[i]] = L_shl( bufferL[chan[i]], exp ); // Q31
1513 2602361 : move32();
1514 : }
1515 :
1516 848887 : return;
1517 : }
1518 :
1519 :
1520 : /*-------------------------------------------------------------------------*
1521 : * get_poly_gains_fx()
1522 : *
1523 : * Compute the gain for a precise polygon
1524 : *-------------------------------------------------------------------------*/
1525 :
1526 848887 : static void get_poly_gains_fx(
1527 : const Word32 azi, /* i : Value of the azimuth q22 */
1528 : const Word32 ele, /* i : Value of the elevation q22 */
1529 : const Word32 aziPoly[EFAP_MAX_CHAN_NUM], /* i : Azimuths of the considered polygons q22 */
1530 : const Word32 elePoly[EFAP_MAX_CHAN_NUM], /* i : Elevations of the considered polygons q22 */
1531 : const Word16 numChan, /* i : Length of aziPoly & elePoly */
1532 : Word32 *buffer /* o : 1D array of length numSpk that will contain the tmp values q31*/
1533 : )
1534 : {
1535 : Word16 i, j;
1536 : Word16 idx1, idx2;
1537 : Word32 P[2];
1538 : Word32 A[2], B[2], C[2];
1539 : Word32 P_minus_A[2];
1540 :
1541 848887 : P[0] = azi; // q22
1542 848887 : move32();
1543 848887 : P[1] = ele; // q22
1544 848887 : move32();
1545 :
1546 : /* Processing, we search for the triangle in which belong P, then we compute the gain */
1547 3451248 : FOR( i = 1; i < numChan + 1; ++i )
1548 : {
1549 2602361 : A[0] = aziPoly[i - 1]; // q22
1550 2602361 : move32();
1551 2602361 : A[1] = elePoly[i - 1]; // q22
1552 2602361 : move32();
1553 :
1554 2602361 : v_sub_fixed_no_hdrm( P, A, P_minus_A, 2 ); /* Precalculate value of (P-A) q22*/
1555 :
1556 2715217 : FOR( j = i; j < numChan - 2 + i; ++j )
1557 : {
1558 2713521 : idx1 = add( 1, ( j % numChan ) );
1559 2713521 : idx2 = add( 1, ( idx1 % numChan ) );
1560 :
1561 2713521 : B[0] = aziPoly[idx1 - 1]; // q22
1562 2713521 : move32();
1563 2713521 : B[1] = elePoly[idx1 - 1]; // q22
1564 2713521 : move32();
1565 :
1566 2713521 : C[0] = aziPoly[idx2 - 1]; // q22
1567 2713521 : move32();
1568 2713521 : C[1] = elePoly[idx2 - 1]; // q22
1569 2713521 : move32();
1570 :
1571 2713521 : IF( in_tri_fx( A, B, C, P_minus_A ) )
1572 : {
1573 2600665 : buffer[i - 1] = L_shl_sat( get_tri_gain_fx( A, B, C, P_minus_A ), Q18 ); // q13+q18->q31
1574 2600665 : move32();
1575 2600665 : BREAK;
1576 : }
1577 : }
1578 : }
1579 :
1580 848887 : return;
1581 : }
1582 :
1583 : /*-------------------------------------------------------------------------*
1584 : * get_tri_gain_fx()
1585 : *
1586 : * Compute the value of the gain for a given triangle
1587 : *-------------------------------------------------------------------------*/
1588 :
1589 2600665 : static Word32 get_tri_gain_fx(
1590 : const Word32 A[2], /* i : Coordinate of one apex of the triangle q22*/
1591 : const Word32 B[2], /* i : Coordinate of one apex of the triangle q22*/
1592 : const Word32 C[2], /* i : Coordinate of one apex of the triangle q22*/
1593 : const Word32 P_minus_A[2] /* i : Value of (P - A) q22 */
1594 : )
1595 : {
1596 : Word32 N[2], tmpN[2];
1597 : Word32 tmpSub1[2];
1598 : Word32 tmpDot1, tmpDot2;
1599 : Word32 gain;
1600 :
1601 : /* Processing */
1602 2600665 : tmpN[0] = L_sub( B[1], C[1] ); // q22
1603 2600665 : move32();
1604 2600665 : tmpN[1] = L_sub( C[0], B[0] ); // q22
1605 2600665 : move32();
1606 :
1607 2600665 : v_sub_fixed_no_hdrm( B, A, tmpSub1, 2 ); // tmpSub1 q22
1608 :
1609 2600665 : tmpDot1 = dotp_fixed( tmpN, tmpSub1, 2 ); // Q13
1610 :
1611 2600665 : Word16 exp = Q18;
1612 2600665 : move16();
1613 2600665 : Word32 inv_tmpDot2 = L_shl( tmpDot1, norm_l( tmpDot1 ) );
1614 2600665 : exp = sub( exp, norm_l( tmpDot1 ) );
1615 2600665 : Word16 inv_tmpDot1 = Inv16( extract_h( inv_tmpDot2 ), &exp ); // 15-exp
1616 2600665 : v_multc_fixed( tmpN, L_shl( inv_tmpDot1, add( Q16, exp ) ), N, 2 ); // 22+31-31->22
1617 :
1618 2600665 : tmpDot2 = dotp_fixed( P_minus_A, N, 2 ); // 22+22-31->13
1619 :
1620 2600665 : if ( EQ_32( tmpDot2, 8191 ) )
1621 : {
1622 153692 : tmpDot2 = 8192; // Q13
1623 153692 : move32();
1624 : }
1625 :
1626 2600665 : gain = L_sub( 8192, tmpDot2 ); // Q13
1627 : /* Set gains <= -60dB to 0 to avoid problems in SVD */
1628 2600665 : if ( EQ_16( BASOP_Util_Cmp_Mant32Exp( L_abs( gain ), 18, 2147 /*1e-6 q31*/, 0 ), -1 ) )
1629 : {
1630 403841 : gain = 0;
1631 403841 : move32();
1632 : }
1633 2600665 : return gain; // Q13
1634 : }
1635 :
1636 :
1637 : /*-------------------------------------------------------------------------*
1638 : * add_vertex_fx()
1639 : *
1640 : * Add a vertex to the vertex array
1641 : *-------------------------------------------------------------------------*/
1642 :
1643 9400 : static void add_vertex_fx(
1644 : EFAP_VERTEX *vtxArray, /* i/o: Handle to the vertex array that will be updated */
1645 : const Word32 azi, /* i : Azimuth of the vertex Q22 */
1646 : const Word32 ele, /* i : Elevation of the vertex Q22 */
1647 : const Word16 pos, /* i : Index in the vtxArray where we want to add the vertex */
1648 : const EFAP_VTX_DMX_TYPE dmxType /* i : downmix type for the vertex */
1649 : )
1650 : {
1651 : Word32 idxAziTmp, idxEleTmp;
1652 : Word32 tmp;
1653 :
1654 9400 : assert( vtxArray != NULL && "EFAP: vtxArray == NULL" );
1655 :
1656 : /* Updating the vertex array */
1657 :
1658 9400 : tmp = efap_32mod32( L_sub( Q22_180_DEG /*180 q22*/, azi ), Q22_360_DEG /*360 q22*/ ); // q22
1659 9400 : vtxArray[pos].azi = L_sub( Q22_180_DEG /*180 q22*/, tmp ); // q22
1660 9400 : move32();
1661 :
1662 9400 : IF( LT_32( Q22_180_DEG /*180 q22*/, ele ) )
1663 0 : tmp = Q22_180_DEG /*180 q22*/;
1664 : ELSE
1665 9400 : tmp = ele; // Q22
1666 9400 : move32();
1667 9400 : IF( GT_32( -Q22_180_DEG /*180 q22*/, tmp ) )
1668 0 : vtxArray[pos].ele = -Q22_180_DEG /*180 q22*/;
1669 : ELSE
1670 9400 : vtxArray[pos]
1671 9400 : .ele = tmp; // q22
1672 9400 : move32();
1673 :
1674 : /* Converting spherical coordinates to cartesians, assuming radius = 1 */
1675 9400 : sph2cart_fx( vtxArray[pos].azi, vtxArray[pos].ele, &vtxArray[pos].pos[0] ); // vtxArray[pos].pos[0] q31
1676 :
1677 : /* Computing the index defined by idx = idxAziTmp + 181 * idxEleTmp */
1678 :
1679 : /* IdxAziTmp */
1680 9400 : tmp = L_abs( L_sub( Q22_90_DEG /*90 q22*/, L_abs( vtxArray[pos].azi ) ) ); // Q22
1681 9400 : idxAziTmp = L_shr( anint_fixed( tmp, Q22 ), Q22 ); // q22-q22->q0
1682 :
1683 : /* IdxEleTmp */
1684 9400 : tmp = L_abs( vtxArray[pos].ele ); // q22
1685 9400 : idxEleTmp = tmp; // q22
1686 9400 : move16();
1687 9400 : idxEleTmp = L_sub( Q22_90_DEG /*90 q22*/, idxEleTmp ); // q22
1688 :
1689 : /* Final Idx */
1690 9400 : vtxArray[pos].idx = add( extract_l( idxAziTmp ), i_mult( 181, extract_l( L_shr( idxEleTmp, Q22 ) ) ) ); // q0
1691 :
1692 : /* Setting the nan flag to 0 */
1693 9400 : vtxArray[pos].isNaN = false;
1694 9400 : move16();
1695 :
1696 : /* Set the default downmix type */
1697 9400 : vtxArray[pos].dmxType = dmxType;
1698 9400 : move32();
1699 :
1700 9400 : return;
1701 : }
1702 :
1703 :
1704 : /*-------------------------------------------------------------------------*
1705 : * efap_sort_s()
1706 : *
1707 : * Sort an integer array
1708 : * (modified version of sort() to return an index array)
1709 : *-------------------------------------------------------------------------*/
1710 :
1711 2846 : static void efap_sort_s_fx(
1712 : Word16 *x, /* i/o: Vector to be sorted */
1713 : Word16 *idx, /* o : Original index positions */
1714 : const Word16 len /* i : vector length */
1715 : )
1716 : {
1717 : Word16 i, j;
1718 : Word16 tempr, tempi;
1719 :
1720 40194 : FOR( i = 0; i < len; i++ )
1721 : {
1722 37348 : idx[i] = i;
1723 37348 : move16();
1724 : }
1725 :
1726 37348 : FOR( i = len - 2; i >= 0; i-- )
1727 : {
1728 34502 : tempr = x[i];
1729 34502 : move16();
1730 34502 : tempi = idx[i];
1731 34502 : move16();
1732 34502 : test();
1733 145167 : FOR( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
1734 : {
1735 110665 : test();
1736 110665 : x[j - 1] = x[j];
1737 110665 : move16();
1738 110665 : idx[j - 1] = idx[j];
1739 110665 : move16();
1740 : }
1741 34502 : x[j - 1] = tempr;
1742 34502 : move16();
1743 34502 : idx[j - 1] = tempi;
1744 34502 : move16();
1745 : }
1746 :
1747 2846 : return;
1748 : }
1749 :
1750 :
1751 : /*-------------------------------------------------------------------------*
1752 : * vertex_distance()
1753 : *
1754 : * Compute the signed distance between a vertex and a hull surface
1755 : *-------------------------------------------------------------------------*/
1756 :
1757 69052 : static Word32 vertex_distance_fx(
1758 : const EFAP_VERTEX *vtxArray, /* i : The considered vertex */
1759 : const EFAP_LS_TRIANGLE tri, /* i : The considered triangle */
1760 : const Word16 vtxIdx /* i : Index of the considered vertex */
1761 : )
1762 : {
1763 : Word32 A[3], B[3], C[3], P[3];
1764 : Word16 i;
1765 :
1766 : /* Assigning the coordinates to the vector */
1767 276208 : FOR( i = 0; i < 3; ++i )
1768 : {
1769 207156 : A[i] = vtxArray[tri.LS[0]].pos[i]; // q31
1770 207156 : move32();
1771 207156 : B[i] = vtxArray[tri.LS[1]].pos[i]; // q31
1772 207156 : move32();
1773 207156 : C[i] = vtxArray[tri.LS[2]].pos[i]; // q31
1774 207156 : move32();
1775 :
1776 207156 : P[i] = vtxArray[vtxIdx].pos[i]; // q31
1777 207156 : move32();
1778 : }
1779 :
1780 69052 : return point_plane_distance_fx( A, B, C, P ); // q28
1781 : }
1782 :
1783 :
1784 : /*-------------------------------------------------------------------------*
1785 : * point_poly_distance_fx()
1786 : *
1787 : * Compute the signed distance between a point and polygon
1788 : *-------------------------------------------------------------------------*/
1789 :
1790 2056677 : static Word32 point_poly_distance_fx(
1791 : const EFAP_POLYSET poly, /* i : The polygon which forms a plane */
1792 : const Word32 X[3] /* i : Cartesian coordinates of the point of interest q31*/
1793 : )
1794 : {
1795 : Word32 P1[3], P2[3], P3[3];
1796 :
1797 2056677 : sph2cart_fx( poly.polyAzi[0], poly.polyEle[0], &P1[0] ); // P1[0] q31
1798 2056677 : sph2cart_fx( poly.polyAzi[1], poly.polyEle[1], &P2[0] ); // P2[0] q31
1799 2056677 : sph2cart_fx( poly.polyAzi[2], poly.polyEle[2], &P3[0] ); // P3[0] q31
1800 :
1801 2056677 : return point_plane_distance_fx( P1, P2, P3, X ); // q28
1802 : }
1803 :
1804 : /*-------------------------------------------------------------------------*
1805 : * point_plane_distance_fx()
1806 : *
1807 : * Compute the signed distance between a point and a given plane
1808 : *-------------------------------------------------------------------------*/
1809 :
1810 2343076 : static Word32 point_plane_distance_fx( // returns output in Q28
1811 : const Word32 P1[3], /* i : First point of the triangle that defines the planes q31*/
1812 : const Word32 P2[3], /* i : Second point of the triangle q31*/
1813 : const Word32 P3[3], /* i : Third point of the triangle q31*/
1814 : const Word32 X[3] /* i : The point of interest q31*/
1815 : )
1816 : {
1817 : Word32 tmpCross1[3], tmpCross2[3];
1818 : Word32 resultCross[3];
1819 : Word32 tmpDot1[3], tmpDot2[3];
1820 : Word32 tmpNorm;
1821 : Word32 dist;
1822 :
1823 : /* Check if the point already matches a triangle vertex */
1824 2343076 : test();
1825 2343076 : test();
1826 2343076 : test();
1827 2343076 : test();
1828 2343076 : test();
1829 2343076 : test();
1830 2343076 : test();
1831 2343076 : test();
1832 2343076 : IF( ( EQ_32( X[0], P1[0] ) && EQ_32( X[1], P1[1] ) && EQ_32( X[2], P1[2] ) ) ||
1833 : ( EQ_32( X[0], P2[0] ) && EQ_32( X[1], P2[1] ) && EQ_32( X[2], P2[2] ) ) ||
1834 : ( EQ_32( X[0], P3[0] ) && EQ_32( X[1], P3[1] ) && EQ_32( X[2], P3[2] ) ) )
1835 : {
1836 60184 : return 0;
1837 : }
1838 :
1839 : /* Cross Product */
1840 2282892 : v_sub_fixed( P1, P2, tmpCross1, 3, 1 ); // tmpCross1 q30
1841 2282892 : v_sub_fixed( P1, P3, tmpCross2, 3, 1 ); // tmpCross2 q30
1842 :
1843 : /* resultCross = cross(P1-P2,P1-P3) */
1844 2282892 : efap_crossp_fx( tmpCross1, tmpCross2, resultCross ); // Q29
1845 :
1846 : /* Dot Product */
1847 2282892 : tmpNorm = dotp_fixed( resultCross, resultCross, 3 ); // Q27
1848 2282892 : Word16 exp = 4;
1849 2282892 : move16();
1850 2282892 : tmpNorm = ISqrt32( tmpNorm, &exp ); // Q29
1851 2282892 : v_sub_fixed( X, P1, tmpDot1, 3, 1 ); // Q30
1852 2282892 : v_multc_fixed( resultCross, tmpNorm, tmpDot2, 3 ); // Q29 - exp
1853 2282892 : dist = L_shl( dotp_fixed( tmpDot1, tmpDot2, 3 ), exp ); // Q28
1854 2282892 : return dist;
1855 : }
1856 :
1857 :
1858 : /*-------------------------------------------------------------------------*
1859 : * efap_crossp_fx()
1860 : *
1861 : * Compute the cross product between column vectors of float of size 3x1
1862 : *-------------------------------------------------------------------------*/
1863 :
1864 2283764 : static void efap_crossp_fx(
1865 : const Word32 *v1, /* i : First float vector Q30 */
1866 : const Word32 *v2, /* i : Second float vector Q30 */
1867 : Word32 *v /* o : Output vector Q29 */
1868 : )
1869 : {
1870 2283764 : v[0] = L_sub( Mpy_32_32( v1[1], v2[2] ), Mpy_32_32( v1[2], v2[1] ) ); /* Q29 */
1871 2283764 : move32();
1872 2283764 : v[1] = L_sub( Mpy_32_32( v1[2], v2[0] ), Mpy_32_32( v1[0], v2[2] ) ); /* Q29 */
1873 2283764 : move32();
1874 2283764 : v[2] = L_sub( Mpy_32_32( v1[0], v2[1] ), Mpy_32_32( v1[1], v2[0] ) ); /* Q29 */
1875 2283764 : move32();
1876 :
1877 2283764 : return;
1878 : }
1879 :
1880 : /*-------------------------------------------------------------------------*
1881 : * find_int_in_tri()
1882 : *
1883 : * Find an integer in triangle array of integers
1884 : *-------------------------------------------------------------------------*/
1885 :
1886 13264 : static Word16 find_int_in_tri_fx(
1887 : const EFAP_LS_TRIANGLE *tri, /* i : Triangle array */
1888 : const Word16 n, /* i : The integer to find */
1889 : const Word16 r, /* i : Number of rows */
1890 : Word16 *pos /* o : Position of the integer */
1891 : )
1892 : {
1893 : Word16 i, j;
1894 :
1895 : /* Find the first element equal to n */
1896 117682 : FOR( i = 0; i < r; ++i )
1897 : {
1898 440797 : FOR( j = 0; j < 3; ++j )
1899 : {
1900 336379 : IF( EQ_16( tri[i].LS[j], n ) )
1901 : {
1902 11290 : pos[0] = i;
1903 11290 : move16();
1904 11290 : pos[1] = j;
1905 11290 : move16();
1906 11290 : return 1;
1907 : }
1908 : }
1909 : }
1910 :
1911 1974 : return 0;
1912 : }
1913 :
1914 : /*-------------------------------------------------------------------------*
1915 : * remove_vertex()
1916 : *
1917 : * Remove a vertex from a vertex structure
1918 : *-------------------------------------------------------------------------*/
1919 0 : static void remove_vertex_fx(
1920 : EFAP_VERTEX *vtxArray, /* i : Vertex array */
1921 : const Word16 idx, /* i : Index of the vertex to remove */
1922 : const Word16 L /* i : Length of the Vertex array */
1923 : )
1924 : {
1925 : Word16 i;
1926 :
1927 0 : assert( idx < L && "EFAP: index out of bounds" );
1928 :
1929 : /* Shift all vertex of one position, so vtxArray[i] will be vtxArray[i+1] and so on */
1930 0 : FOR( i = idx; i < L - 1; ++i )
1931 : {
1932 0 : add_vertex_fx( vtxArray, vtxArray[i + 1].azi, vtxArray[i + 1].ele, i, EFAP_DMX_INTENSITY );
1933 : }
1934 :
1935 : /* The last vertex is set to 0 */
1936 0 : add_vertex_fx( vtxArray, 0, 0, sub( L, 1 ), EFAP_DMX_INTENSITY );
1937 :
1938 0 : return;
1939 : }
1940 :
1941 :
1942 : /*-------------------------------------------------------------------------*
1943 : * get_neighbours()
1944 : *
1945 : * Returns the neighbouring triangles of a vertex
1946 : *-------------------------------------------------------------------------*/
1947 1974 : static Word16 get_neighbours_fx(
1948 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
1949 : const Word16 vtxIdx, /* i : Index of the vertex */
1950 : const Word16 numTri, /* i : Number of Triangles */
1951 : Word16 *neighbours /* o : Output vector */
1952 : )
1953 : {
1954 : Word16 i, j, k;
1955 : Word16 tmpPos[2];
1956 : Word16 tmpNeighbours[EFAP_MAX_SIZE_TMP_BUFF];
1957 : Word16 dummy[EFAP_MAX_SIZE_TMP_BUFF];
1958 : EFAP_LS_TRIANGLE tmpTriArray[EFAP_MAX_POLY_SET];
1959 :
1960 : /* Processing */
1961 33978 : FOR( i = 0; i < numTri; ++i )
1962 : {
1963 32004 : Copy( triArray[i].LS, tmpTriArray[i].LS, 3 );
1964 : }
1965 :
1966 1974 : k = 0;
1967 1974 : move16();
1968 : WHILE( 1 )
1969 : {
1970 11290 : IF( find_int_in_tri_fx( tmpTriArray, vtxIdx, numTri, tmpPos ) == 0 )
1971 : {
1972 1974 : BREAK;
1973 : }
1974 : ELSE
1975 : {
1976 9316 : tmpNeighbours[k] = tmpTriArray[tmpPos[0]].LS[0];
1977 9316 : move16();
1978 9316 : tmpNeighbours[k + 1] = tmpTriArray[tmpPos[0]].LS[1];
1979 9316 : move16();
1980 9316 : tmpNeighbours[k + 2] = tmpTriArray[tmpPos[0]].LS[2];
1981 9316 : move16();
1982 9316 : k = add( k, 3 );
1983 9316 : tmpTriArray[tmpPos[0]].LS[tmpPos[1]] = -1;
1984 9316 : move16();
1985 : }
1986 :
1987 9316 : IF( GT_16( k, i_mult( 3, numTri ) ) )
1988 : {
1989 0 : BREAK;
1990 : }
1991 : }
1992 :
1993 : /* Sorting the neighbours vector */
1994 1974 : efap_sort_s_fx( tmpNeighbours, dummy, k );
1995 :
1996 : /* Creating the output vector, by eliminating redundancies and also deleting the indice == vtxIdx*/
1997 1974 : neighbours[0] = tmpNeighbours[0];
1998 1974 : move16();
1999 1974 : j = 1;
2000 1974 : move16();
2001 :
2002 27948 : FOR( i = 1; i < k; ++i )
2003 : {
2004 25974 : test();
2005 25974 : IF( NE_16( tmpNeighbours[i], tmpNeighbours[i - 1] ) &&
2006 : NE_16( tmpNeighbours[i], vtxIdx ) )
2007 : {
2008 7342 : neighbours[j] = tmpNeighbours[i];
2009 7342 : move16();
2010 7342 : j = add( j, 1 );
2011 : }
2012 : }
2013 :
2014 : /* Output, length of neighbours */
2015 1974 : return j;
2016 : }
2017 :
2018 : /*-------------------------------------------------------------------------*
2019 : * matrix_times_row_fx()
2020 : *
2021 : * Computes the product of a matrix and a row vector
2022 : *-------------------------------------------------------------------------*/
2023 :
2024 11172 : static void matrix_times_row_fx(
2025 : Word32 mat[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF], /* i : The input matrix q31 */
2026 : const Word32 *vec, /* i : The input row vector q31*/
2027 : const Word16 L, /* i : Row length */
2028 : Word32 *out /* o : Output vector q31 */
2029 : )
2030 : {
2031 : Word16 i, j;
2032 :
2033 67032 : FOR( i = 0; i < L; ++i )
2034 : {
2035 335160 : FOR( j = 0; j < L; ++j )
2036 : {
2037 279300 : out[i] = L_add( out[i], Mpy_32_32( mat[i][j], vec[j] ) ); /*31+31-31=>31*/
2038 279300 : move32();
2039 : }
2040 : }
2041 :
2042 11172 : return;
2043 : }
2044 :
2045 : /*-------------------------------------------------------------------------*
2046 : * tri_to_poly()
2047 : *
2048 : * Combines triangles of a surface in order to create polygons
2049 : *-------------------------------------------------------------------------*/
2050 :
2051 872 : static void tri_to_poly_fx(
2052 : const EFAP_VERTEX *vtxArray, /* i : Vertex array */
2053 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
2054 : const Word16 numVtx, /* i : Number of vertices */
2055 : const Word16 numTri, /* i : Number of triangles */
2056 : Word16 sortedChan[EFAP_MAX_POLY_SET][EFAP_MAX_CHAN_NUM], /* o : The matrix that will contain the sorted channels */
2057 : Word16 *outLengthPS, /* o : The length of the sorted channels */
2058 : Word16 outLengthSorted[EFAP_MAX_POLY_SET] /* o : The number of channels for each poly (i.e. outLengthSorted[i] = length(sortedChan[i]) */
2059 : )
2060 : {
2061 : Word16 i, j;
2062 : Word16 lenPoly;
2063 : Word16 lenPolySet;
2064 : Word16 found;
2065 : Word16 replaceIdx;
2066 :
2067 : Word16 poly[EFAP_MAX_CHAN_NUM];
2068 :
2069 : Word16 sortedLengths[EFAP_MAX_POLY_SET];
2070 : Word16 sortedTri[EFAP_MAX_POLY_SET];
2071 :
2072 : Word32 dist;
2073 :
2074 872 : lenPolySet = 0;
2075 872 : move16();
2076 : /* Sorting the polygons */
2077 16184 : FOR( i = 0; i < numTri; ++i )
2078 : {
2079 : /* search for coplanar vertices and add them to the polygon */
2080 15312 : lenPoly = 0;
2081 15312 : move16();
2082 202840 : FOR( j = 0; j < numVtx; ++j )
2083 : {
2084 187528 : dist = L_abs( point_plane_distance_fx(
2085 187528 : vtxArray[triArray[i].LS[0]].pos,
2086 187528 : vtxArray[triArray[i].LS[1]].pos,
2087 187528 : vtxArray[triArray[i].LS[2]].pos,
2088 187528 : vtxArray[j].pos ) ); // Q28
2089 :
2090 187528 : IF( LT_32( dist, 268435 /* 1e-3f in Q28 */ ) )
2091 : {
2092 47370 : assert( lenPoly < EFAP_MAX_CHAN_NUM && "EFAP: exceeded max polygon vertices!" );
2093 47370 : poly[lenPoly] = j;
2094 47370 : move16();
2095 47370 : lenPoly = add( lenPoly, 1 );
2096 : }
2097 : }
2098 :
2099 : /* search existing polygons to determine whether the new one already exists/is a subset or is a superset */
2100 15312 : found = 0;
2101 15312 : move16();
2102 15312 : replaceIdx = -1;
2103 15312 : move16();
2104 160751 : FOR( j = 0; j < lenPolySet; ++j )
2105 : {
2106 146156 : found = compare_poly_fx( sortedChan[j], sortedLengths[j], poly, lenPoly );
2107 :
2108 146156 : IF( found > 0 )
2109 : {
2110 717 : BREAK;
2111 : }
2112 145439 : ELSE IF( found < 0 )
2113 : {
2114 0 : replaceIdx = j;
2115 0 : move16();
2116 : }
2117 : }
2118 :
2119 15312 : IF( found == 0 )
2120 : {
2121 : /* append new poly */
2122 14595 : Copy( poly, sortedChan[lenPolySet], lenPoly );
2123 14595 : sortedTri[lenPolySet] = i;
2124 14595 : move16();
2125 14595 : sortedLengths[lenPolySet] = lenPoly;
2126 14595 : move16();
2127 14595 : lenPolySet = add( lenPolySet, 1 );
2128 : }
2129 717 : ELSE IF( EQ_16( found, -1 ) )
2130 : {
2131 : /* replace with superset */
2132 0 : Copy( poly, sortedChan[replaceIdx], lenPoly );
2133 0 : sortedTri[replaceIdx] = i;
2134 0 : move16();
2135 0 : sortedLengths[replaceIdx] = lenPoly;
2136 0 : move16();
2137 : }
2138 : }
2139 :
2140 : /* Sorting the vertex */
2141 15467 : FOR( i = 0; i < lenPolySet; ++i )
2142 : {
2143 14595 : sort_channels_vertex_fx( vtxArray, triArray, sortedChan[i], sortedLengths[i], sortedTri[i] );
2144 : }
2145 :
2146 : /* Output */
2147 872 : *outLengthPS = lenPolySet;
2148 872 : move16();
2149 872 : Copy( sortedLengths, outLengthSorted, EFAP_MAX_POLY_SET );
2150 872 : return;
2151 : }
2152 :
2153 :
2154 : /*-------------------------------------------------------------------------*
2155 : * compare_poly()
2156 : *
2157 : * Compares a newly created polygon with an existing one
2158 : *-------------------------------------------------------------------------*/
2159 :
2160 146156 : static Word16 compare_poly_fx(
2161 : Word16 *old, /* i : Existing polygon */
2162 : Word16 lenOld, /* i : Length of existing polygon */
2163 : Word16 *new, /* i : New polygon */
2164 : Word16 lenNew /* i : Length of new polygon */
2165 : )
2166 : {
2167 : Word16 i, j;
2168 : Word16 count;
2169 :
2170 146156 : count = 0;
2171 146156 : move16();
2172 :
2173 588433 : FOR( i = 0; i < lenOld; ++i )
2174 : {
2175 1538917 : FOR( j = count; j < lenNew; ++j )
2176 : {
2177 1193056 : IF( EQ_16( old[i], new[j] ) )
2178 : {
2179 96416 : count = add( count, 1 );
2180 96416 : BREAK;
2181 : }
2182 : }
2183 : }
2184 :
2185 146156 : test();
2186 146156 : test();
2187 146156 : IF( EQ_16( count, lenOld ) && LT_16( lenOld, lenNew ) )
2188 : {
2189 : /* new polygon is a superset */
2190 0 : return -1;
2191 : }
2192 146156 : ELSE IF( EQ_16( count, lenNew ) && GE_16( lenOld, lenNew ) )
2193 : {
2194 : /* found as subset or identical */
2195 717 : return 1;
2196 : }
2197 : ELSE
2198 : {
2199 : /* not found */
2200 145439 : return 0;
2201 : }
2202 : }
2203 :
2204 :
2205 : /*-------------------------------------------------------------------------*
2206 : * sort_channels_vertex()
2207 : *
2208 : * Sort the channels of a polygon set according to the vertex azimuth
2209 : *-------------------------------------------------------------------------*/
2210 :
2211 14595 : static void sort_channels_vertex_fx(
2212 : const EFAP_VERTEX *vtxArray, /* i : Vertex array */
2213 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
2214 : Word16 channels[EFAP_MAX_CHAN_NUM], /* o : Channels array to be modified */
2215 : const Word16 lengthChannels, /* i : Length of the channels array */
2216 : Word16 idxTri /* i : Index of the considered triangle */
2217 : )
2218 : {
2219 : Word16 i, j;
2220 :
2221 : Word32 P1[3], P2[3], P3[3];
2222 : Word32 tmpU[3];
2223 : Word32 U[3], V[3];
2224 : Word32 tmpV1[3], tmpV2[3], tmpV3[3];
2225 : Word32 normU, normV;
2226 : Word32 MC[3];
2227 : Word32 tmpP[3], P[3];
2228 :
2229 : Word32 x, y;
2230 :
2231 : Word32 azi[EFAP_MAX_CHAN_NUM];
2232 : Word16 order[EFAP_MAX_CHAN_NUM];
2233 :
2234 : Word16 newChannels[EFAP_MAX_CHAN_NUM];
2235 :
2236 :
2237 : /* Initializing coordinates with the vertices of the considered triangle */
2238 58380 : FOR( i = 0; i < 3; ++i )
2239 : {
2240 43785 : P1[i] = vtxArray[triArray[idxTri].LS[0]].pos[i]; // Q31
2241 43785 : move32();
2242 43785 : P2[i] = vtxArray[triArray[idxTri].LS[1]].pos[i]; // Q31
2243 43785 : move32();
2244 43785 : P3[i] = vtxArray[triArray[idxTri].LS[2]].pos[i]; // Q31
2245 43785 : move32();
2246 : }
2247 :
2248 : /* First Base Vector */
2249 14595 : v_sub_fixed( P2, P1, tmpU, 3, 1 ); // tmpU Q30
2250 14595 : Word16 exp1 = 2;
2251 14595 : move16();
2252 14595 : normU = ISqrt32( dotp_fixed( tmpU, tmpU, 3 ) /*q29*/, &exp1 ); /*q=31-exp1*/
2253 : // normU = L_shl_sat( normU, exp ); //normU Q31
2254 14595 : v_multc_fixed( tmpU, normU, U, 3 ); // U Q30 - exp1
2255 :
2256 : /* Second Base Vector */
2257 14595 : v_sub_fixed( P3, P2, tmpV1, 3, 1 ); // tmpV1 Q30
2258 14595 : v_multc_fixed( U, dotp_fixed( U, tmpV1, 3 ), tmpV2, 3 ); // tmpV2 Q28 - 2*exp1
2259 :
2260 58380 : FOR( i = 0; i < 3; i++ )
2261 : {
2262 43785 : tmpV2[i] = L_shl( tmpV2[i], add( Q2, shl( exp1, 1 ) ) ); // q30
2263 43785 : move32();
2264 : }
2265 :
2266 14595 : v_sub_fixed_no_hdrm( tmpV1, tmpV2, tmpV3, 3 ); // tmpV3 Q30
2267 14595 : Word16 exp2 = 2;
2268 14595 : move16();
2269 14595 : normV = ISqrt32( dotp_fixed( tmpV3, tmpV3, 3 ) /*q29*/, &exp2 ); // q=31-exp2
2270 :
2271 14595 : v_multc_fixed( tmpV3, normV, V, 3 ); // V Q30 - exp2
2272 :
2273 : /* Center of the first Triangle */
2274 58380 : FOR( i = 0; i < 3; ++i )
2275 : {
2276 43785 : MC[i] = L_shl( Mpy_32_32( L_add( L_add( L_shr( P1[i], Q2 ), L_shr( P2[i], Q2 ) ), L_shr( P3[i], Q2 ) ), 715827883 /* 1 / 3 in Q31 */ ), Q2 ); // q29+2=>q31
2277 43785 : move32();
2278 : }
2279 :
2280 : /* Sort Vertices */
2281 59097 : FOR( i = 0; i < lengthChannels; ++i )
2282 : {
2283 178008 : FOR( j = 0; j < 3; ++j )
2284 : {
2285 133506 : tmpP[j] = vtxArray[channels[i]].pos[j]; // q31
2286 133506 : move32();
2287 : }
2288 :
2289 44502 : v_sub_fixed( tmpP, MC, P, 3, 1 ); // P Q30
2290 :
2291 44502 : x = dotp_fixed( P, U, 3 ); // x Q29 - exp1
2292 44502 : y = dotp_fixed( P, V, 3 ); // y Q29 - exp2
2293 :
2294 : // Executing azi[i] = atan2f( y, x );
2295 44502 : azi[i] = L_shl( BASOP_util_atan2( y, x, sub( exp2, exp1 ) ), Q16 ); // azi 2Q29
2296 44502 : move32();
2297 : }
2298 :
2299 : /* Sorting the azi vec */
2300 14595 : v_sort_ind_fixed( azi, order, lengthChannels );
2301 :
2302 : /* Updating the channel array */
2303 59097 : FOR( i = 0; i < lengthChannels; ++i )
2304 : {
2305 44502 : newChannels[i] = channels[order[i]];
2306 44502 : move16();
2307 : }
2308 :
2309 : /* return Success */
2310 14595 : Copy( newChannels, channels, lengthChannels );
2311 :
2312 14595 : return;
2313 : }
2314 :
2315 : /*-------------------------------------------------------------------------*
2316 : * efap_32mod32()
2317 : *
2318 : * Modulus operation that will handle negative values the same way as matlab
2319 : *-------------------------------------------------------------------------*/
2320 :
2321 17191 : static Word32 efap_32mod32(
2322 : const Word32 x, /* i : Dividend q22*/
2323 : const Word32 y /* i : Divisor q22 */
2324 : )
2325 : {
2326 17191 : Word32 result = x % y; // q22
2327 17191 : move32();
2328 17191 : IF( result >= 0 )
2329 : {
2330 13995 : return result; // q22
2331 : }
2332 : ELSE
2333 : {
2334 3196 : return L_add( result, y ); // q22
2335 : }
2336 : }
2337 :
2338 : /*-------------------------------------------------------------------------*
2339 : * get_poly_num()
2340 : *
2341 : * Returns the index of the polygon in which the coordinate is
2342 : *-------------------------------------------------------------------------*/
2343 :
2344 848887 : static Word16 get_poly_num_fx(
2345 : const Word32 P[2], /* i : Azimuth and elevation of the point q22*/
2346 : const EFAP_POLYSET_DATA *polyData /* i : Polyset struct */
2347 : )
2348 : {
2349 : Word16 i;
2350 : Word16 num_poly, found_poly;
2351 : Word16 poly_tmp[EFAP_MAX_CHAN_NUM];
2352 : Word32 poly_dist[EFAP_MAX_CHAN_NUM];
2353 :
2354 : Word32 dist_tmp;
2355 : Word32 pos[3];
2356 :
2357 848887 : num_poly = 0;
2358 848887 : move16();
2359 :
2360 848887 : sph2cart_fx( P[0], P[1], &pos[0] ); // pos[0] q31
2361 :
2362 : /* Filter the polygon list with a fast 2d check */
2363 18333212 : FOR( i = 0; i < polyData->numPoly; ++i )
2364 : {
2365 17498599 : IF( in_poly_fx( P, polyData->polysetArray[i] ) )
2366 : {
2367 : /* select only polygons which are visible from the point */
2368 2056677 : dist_tmp = point_poly_distance_fx( polyData->polysetArray[i], pos ); // q28
2369 2056677 : IF( dist_tmp == 0 )
2370 : {
2371 14274 : return i;
2372 : }
2373 2042403 : ELSE IF( dist_tmp > 0 )
2374 : {
2375 1241032 : poly_tmp[num_poly] = i;
2376 1241032 : move16();
2377 1241032 : poly_dist[num_poly] = dist_tmp; // q28
2378 1241032 : move32();
2379 1241032 : num_poly = add( num_poly, 1 );
2380 : }
2381 : }
2382 : }
2383 834613 : IF( num_poly == 0 )
2384 : {
2385 0 : return -1;
2386 : }
2387 :
2388 : /* select the polygon with the smallest distance */
2389 834613 : found_poly = poly_tmp[0];
2390 834613 : move16();
2391 834613 : dist_tmp = poly_dist[0]; // q28
2392 834613 : move32();
2393 1241032 : FOR( i = 1; i < num_poly; i++ )
2394 : {
2395 406419 : IF( LT_32( poly_dist[i], dist_tmp ) )
2396 : {
2397 292445 : found_poly = poly_tmp[i];
2398 292445 : move16();
2399 292445 : dist_tmp = poly_dist[i]; // q28
2400 292445 : move32();
2401 : }
2402 : }
2403 :
2404 834613 : return found_poly;
2405 : }
2406 :
2407 : /*-------------------------------------------------------------------------*
2408 : * in_poly()
2409 : *
2410 : * Determines if a given point is within a polygon or not
2411 : *-------------------------------------------------------------------------*/
2412 :
2413 17498599 : static Word16 in_poly_fx( /* Angles are in Q22 */
2414 : const Word32 P[2], /* i : Azimuth and elevation of the point q22*/
2415 : const EFAP_POLYSET poly /* i : Polyset struct */
2416 : )
2417 : {
2418 : Word16 n;
2419 17498599 : Word16 numVertices = poly.numChan;
2420 : Word32 A[2];
2421 : Word32 B[2];
2422 : Word32 C[2];
2423 : Word32 P_minus_A[2];
2424 17498599 : move16();
2425 :
2426 : /* Safety check */
2427 :
2428 17498599 : IF( LT_16( numVertices, 3 ) )
2429 : {
2430 0 : return 0;
2431 : }
2432 :
2433 : /* See if the point is in one of the triangles available in the polygon */
2434 :
2435 17498599 : IF( poly.isNaN[0] )
2436 : {
2437 1189462 : A[0] = P[0]; // q22
2438 1189462 : move32();
2439 : }
2440 : ELSE
2441 : {
2442 16309137 : A[0] = poly.polyAzi[0]; // q22
2443 16309137 : move32();
2444 : }
2445 17498599 : A[1] = poly.polyEle[0]; // q22
2446 17498599 : move32();
2447 :
2448 17498599 : v_sub_fixed_no_hdrm( P, A, P_minus_A, 2 ); /* Precalculate value of (P-A) q22*/
2449 :
2450 34275831 : FOR( n = 1; n < sub( numVertices, 1 ); ++n )
2451 : {
2452 18833909 : IF( poly.isNaN[n] )
2453 : {
2454 9587033 : B[0] = P[0]; // q22
2455 9587033 : move32();
2456 : }
2457 : ELSE
2458 : {
2459 9246876 : B[0] = poly.polyAzi[n]; // q22
2460 9246876 : move32();
2461 : }
2462 18833909 : B[1] = poly.polyEle[n]; // q22
2463 18833909 : move32();
2464 :
2465 18833909 : IF( poly.isNaN[n + 1] )
2466 : {
2467 1186682 : C[0] = P[0]; // q22
2468 1186682 : move32();
2469 : }
2470 : ELSE
2471 : {
2472 17647227 : C[0] = poly.polyAzi[n + 1]; // q22
2473 17647227 : move32();
2474 : }
2475 18833909 : C[1] = poly.polyEle[n + 1]; // q22
2476 18833909 : move32();
2477 :
2478 18833909 : IF( in_tri_fx( A, B, C, P_minus_A ) )
2479 : {
2480 2056677 : return 1;
2481 : }
2482 : }
2483 :
2484 15441922 : return 0;
2485 : }
2486 :
2487 : /*-------------------------------------------------------------------------*
2488 : * in_tri()
2489 : *
2490 : * Determines if a given point is within a triangle or not
2491 : *-------------------------------------------------------------------------*/
2492 :
2493 21547430 : static Word16 in_tri_fx(
2494 : Word32 A[2], /* i : Coordinate of one apex of the triangle q22*/
2495 : Word32 B[2], /* i : Coordinate of one apex of the triangle q22*/
2496 : Word32 C[2], /* i : Coordinate of one apex of the triangle q22*/
2497 : Word32 P_minus_A[2] /* i : Value of (P - A) q22*/
2498 : )
2499 : {
2500 : Word32 tmpDot1[2], tmpDot2[2];
2501 : Word32 matInv[2][2];
2502 : Word32 invFactor;
2503 : Word16 tmp_e;
2504 : Word32 tmp32;
2505 : Word64 S[2];
2506 : /* Threshold adjusted */
2507 21547430 : Word64 thresh_int = 35184640; // 1e-6f in Q45
2508 21547430 : move64();
2509 :
2510 : /*
2511 : Not a Valid Triangle : Colinear edges
2512 : In the matlab implementation, the rcond() function is used
2513 : Since it's very complex to implement this in C
2514 : I'll just compute the determinant and if it's equal to 0, that means the two vectors are colinear
2515 : */
2516 :
2517 21547430 : v_sub_fixed_no_hdrm( B, A, tmpDot1, 2 ); // tmpDot1 q22
2518 21547430 : v_sub_fixed_no_hdrm( C, A, tmpDot2, 2 ); // tmpDot2 q22
2519 :
2520 : /* Verification of the non-colinearity : Q22 * Q22 = Q13 */
2521 21547430 : invFactor = Msub_32_32( Mpy_32_32( tmpDot1[0], tmpDot2[1] ), tmpDot1[1], tmpDot2[0] ); /*q22+q22-q31->q13*/
2522 :
2523 21547430 : IF( invFactor == 0 )
2524 : {
2525 408 : return 0;
2526 : }
2527 :
2528 : /* invFactor = 1.f / invFactor; */
2529 21547022 : tmp32 = BASOP_Util_Divide3232_Scale_newton( ONE_IN_Q13, invFactor, &tmp_e ); /*31-tmp_e*/
2530 21547022 : invFactor = L_shl_sat( tmp32, tmp_e ); /* Q31 */
2531 :
2532 21547022 : Word16 invFactor_exp = norm_l( invFactor );
2533 21547022 : invFactor = L_shl( invFactor, invFactor_exp ); // 31+invFactor_exp
2534 :
2535 : // Q22 = Q22 * Q31
2536 21547022 : matInv[0][0] = Mpy_32_32( tmpDot2[1], invFactor ); // q=22+invFactor_exp
2537 21547022 : move32();
2538 21547022 : matInv[0][1] = Mpy_32_32( L_negate( tmpDot2[0] ), invFactor ); // q=22+invFactor_exp
2539 21547022 : move32();
2540 21547022 : matInv[1][0] = Mpy_32_32( L_negate( tmpDot1[1] ), invFactor ); // q=22+invFactor_exp
2541 21547022 : move32();
2542 21547022 : matInv[1][1] = Mpy_32_32( tmpDot1[0], invFactor ); // q=22+invFactor_exp
2543 21547022 : move32();
2544 21547022 : S[0] = W_mac_32_32( W_mult_32_32( matInv[0][0], P_minus_A[0] ), matInv[0][1], P_minus_A[1] ); // Q22+invFactor_exp +Q22 + 1
2545 21547022 : move64();
2546 21547022 : S[0] = W_shr( S[0], invFactor_exp ); // q45
2547 21547022 : move64();
2548 21547022 : S[1] = W_mac_32_32( W_mult_32_32( matInv[1][0], P_minus_A[0] ), matInv[1][1], P_minus_A[1] ); // Q22+invFactor_exp +Q22 + 1
2549 21547022 : move64();
2550 21547022 : S[1] = W_shr( S[1], invFactor_exp ); // q45
2551 21547022 : move64();
2552 :
2553 21547022 : test();
2554 21547022 : test();
2555 21547022 : IF( LT_64( S[0], -thresh_int ) || LT_64( S[1], -thresh_int ) || GT_64( W_add( S[0], S[1] ), W_add( ONE_IN_Q45, thresh_int ) ) )
2556 : {
2557 16889680 : return 0;
2558 : }
2559 : ELSE
2560 : {
2561 4657342 : return 1;
2562 : }
2563 : }
2564 :
2565 : /*-------------------------------------------------------------------------*
2566 : * sph2cart_fx()
2567 : *
2568 : * Converts a vertex position to cartesian coordinates
2569 : *-------------------------------------------------------------------------*/
2570 :
2571 7028318 : static void sph2cart_fx(
2572 : const Word32 azi, /* i : Azimuth in degrees Q22 */
2573 : const Word32 ele, /* i : Elevation in degrees Q22 */
2574 : Word32 *pos /* o : Cartesian coordinates vector (x, y, z) Q31 */
2575 : )
2576 : {
2577 : Word16 azi_temp, ele_temp;
2578 :
2579 7028318 : azi_temp = extract_l( L_shr( Mpy_32_32( azi, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
2580 7028318 : ele_temp = extract_l( L_shr( Mpy_32_32( ele, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
2581 :
2582 7028318 : pos[0] = Mpy_32_16( getCosWord16R2( azi_temp ), 0, getCosWord16R2( ele_temp ) ); /* Q31 */
2583 7028318 : move32();
2584 7028318 : pos[1] = Mpy_32_16( getSineWord16R2( azi_temp ), 0, getCosWord16R2( ele_temp ) ); /* Q31 */
2585 7028318 : move32();
2586 7028318 : pos[2] = L_shl( getSineWord16R2( ele_temp ), Q16 ); /* Q31 */
2587 7028318 : move32();
2588 :
2589 7028318 : return;
2590 : }
|
