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