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