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