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 916 : 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 916 : error = IVAS_ERR_OK;
131 916 : move32();
132 :
133 : /* Basic init checks */
134 916 : test();
135 916 : 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 916 : 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 916 : 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 916 : 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 916 : 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 916 : 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 916 : efap->numSpk = num_speaker_nodes;
178 916 : move16();
179 :
180 : /* The number of vertex is first set to the number of LS but will evolve further */
181 916 : efap->vtxData.numVtx = num_speaker_nodes;
182 916 : move16();
183 :
184 : /* Loudspeaker configuration */
185 916 : Copy32( speaker_node_azi_deg, efap->aziSpk, num_speaker_nodes ); // Q22
186 916 : Copy32( speaker_node_ele_deg, efap->eleSpk, num_speaker_nodes ); // Q22
187 :
188 : /* Initialization of the vertex */
189 916 : vertex_init_fx( efap->aziSpk, efap->eleSpk, &efap->vtxData );
190 :
191 : /* Initialization of polygons and ghost LS */
192 916 : 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 916 : 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 916 : *hEFAPdata = efap;
204 916 : 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 791943 : 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 791943 : set32_fx( hEFAPdata->bufferShort_fx, 0, hEFAPdata->numSpk );
228 791943 : set32_fx( hEFAPdata->bufferLong_fx, 0, hEFAPdata->vtxData.numVtx );
229 :
230 : /* Wrap angles to correct range */
231 791943 : panning_wrap_angles_fx( azi_deg, ele_deg, &azi_wrap_int, &ele_wrap_int ); // ouputs in q22
232 :
233 : /* Panning */
234 791943 : efap_panning_fx( azi_wrap_int, ele_wrap_int, &hEFAPdata->polyData, hEFAPdata->bufferLong_fx ); // hEFAPdata->bufferLong_fx q31
235 :
236 791943 : IF( efap_mode == EFAP_MODE_EFAP )
237 : {
238 785293 : normBuffer = 0;
239 785293 : move32();
240 7264060 : FOR( j = 0; j < hEFAPdata->numSpk; ++j )
241 : {
242 6478767 : hEFAPdata->bufferShort_fx[j] = 0;
243 6478767 : move32();
244 : /* Multiplying by the downmixMatrix */
245 80865478 : FOR( i = 0; i < hEFAPdata->vtxData.numVtx; ++i )
246 : {
247 74386711 : 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 74386711 : move32();
249 : }
250 6478767 : normBuffer = L_add_sat( normBuffer, Mpy_32_32( hEFAPdata->bufferShort_fx[j], hEFAPdata->bufferShort_fx[j] ) ); /* Q29 */
251 : }
252 785293 : Word16 exp = 2;
253 785293 : move16();
254 785293 : normBuffer = ISqrt32( normBuffer, &exp ); // Q=(31-exp)
255 :
256 7264060 : FOR( j = 0; j < hEFAPdata->numSpk; ++j )
257 : {
258 6478767 : hEFAPdata->bufferShort_fx[j] = Mpy_32_32( hEFAPdata->bufferShort_fx[j], normBuffer ); // Q=(30+31-exp-31)=>(30-exp)
259 6478767 : move32();
260 6478767 : hEFAPdata->bufferShort_fx[j] = L_shl( hEFAPdata->bufferShort_fx[j], exp ); /* Q30 */
261 6478767 : 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 791943 : Copy32( hEFAPdata->bufferShort_fx, gains, hEFAPdata->numSpk ); /* Q30 */
296 :
297 791943 : return;
298 : }
299 :
300 :
301 : /*-------------------------------------------------------------------------*
302 : * efap_free_data()
303 : *
304 : * Wrapper to free EFAP data structure
305 : *-------------------------------------------------------------------------*/
306 :
307 1693 : 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 1693 : test();
315 1693 : IF( hEFAPdata == NULL || *hEFAPdata == NULL )
316 : {
317 777 : return;
318 : }
319 :
320 916 : dim1 = ( *hEFAPdata )->numTot;
321 916 : move16();
322 :
323 : /* instance buffer members */
324 916 : free( ( *hEFAPdata )->aziSpk );
325 916 : ( *hEFAPdata )->aziSpk = NULL;
326 :
327 916 : free( ( *hEFAPdata )->eleSpk );
328 916 : ( *hEFAPdata )->eleSpk = NULL;
329 :
330 916 : free( ( *hEFAPdata )->vtxData.vertexArray );
331 916 : ( *hEFAPdata )->vtxData.vertexArray = NULL;
332 :
333 916 : free( ( *hEFAPdata )->vtxData.vtxOrder );
334 916 : ( *hEFAPdata )->vtxData.vtxOrder = NULL;
335 :
336 916 : free( ( *hEFAPdata )->bufferLong_fx );
337 916 : ( *hEFAPdata )->bufferLong_fx = NULL;
338 :
339 916 : free( ( *hEFAPdata )->bufferShort_fx );
340 916 : ( *hEFAPdata )->bufferShort_fx = NULL;
341 :
342 916 : p_dmTranspose = (void **) ( *hEFAPdata )->dmTranspose_fx;
343 :
344 10756 : FOR( i = 0; i < dim1; i++ )
345 : {
346 9840 : free( p_dmTranspose[i] );
347 : }
348 916 : free( p_dmTranspose );
349 :
350 : /* instance */
351 916 : free( *hEFAPdata );
352 916 : *hEFAPdata = NULL;
353 :
354 916 : 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 916 : 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 916 : error = IVAS_ERR_OK;
383 916 : move32();
384 :
385 : /* Safety Check */
386 916 : assert( efap != NULL && "EFAP: efap == NULL" );
387 :
388 46716 : FOR( n = 0; n < EFAP_MAX_POLY_SET; n++ )
389 : {
390 45800 : 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 916 : 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 10756 : FOR( n = 0; n < efap->vtxData.numVtx; ++n )
401 : {
402 9840 : test();
403 18764 : if ( GT_32( efap->vtxData.vertexArray[n].ele /*Q22*/, ( Q22_90_DEG /*90.0 Q22*/ - 4 /*1e-6 Q22*/ ) ) ||
404 8924 : LT_32( efap->vtxData.vertexArray[n].ele /*Q22*/, ( 4 /*1e-6 Q22*/ - Q22_90_DEG /*90.0 Q22*/ ) ) )
405 : {
406 1832 : efap->vtxData.vertexArray[n].isNaN = 1;
407 1832 : move16();
408 : }
409 : }
410 :
411 : /* Converting the triangle to polygon structure */
412 916 : 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 916 : finalLength = -1;
416 916 : move16();
417 :
418 16181 : FOR( n = 0; n < lengthTri2PolyPS; ++n )
419 : {
420 15265 : m = add( finalLength, 1 );
421 :
422 : /* Complete the fields of the polygon */
423 61811 : FOR( j = 0; j < lengthTri2PolySorted[n]; ++j )
424 : {
425 46546 : efap->polyData.polysetArray[m].chan[j] = sortedChan[n][j];
426 46546 : move16();
427 46546 : efap->polyData.polysetArray[m].polyAzi[j] = efap->vtxData.vertexArray[sortedChan[n][j]].azi; /* Q22 */
428 46546 : move32();
429 46546 : efap->polyData.polysetArray[m].polyEle[j] = efap->vtxData.vertexArray[sortedChan[n][j]].ele; /* Q22 */
430 46546 : move32();
431 46546 : efap->polyData.polysetArray[m].isNaN[j] = efap->vtxData.vertexArray[sortedChan[n][j]].isNaN;
432 46546 : move16();
433 : }
434 :
435 15265 : efap->polyData.polysetArray[m].numChan = lengthTri2PolySorted[n];
436 15265 : move16();
437 :
438 : /* In case tmpMax - tmpMin > 180, wrap polygon azimuth */
439 15265 : maximum_l( efap->polyData.polysetArray[m].polyAzi, lengthTri2PolySorted[n], &tmpMax );
440 15265 : minimum_l( efap->polyData.polysetArray[m].polyAzi, lengthTri2PolySorted[n], &tmpMin );
441 :
442 15265 : IF( GT_32( L_sub( tmpMax /*q22*/, tmpMin /*q22*/ ), Q22_180_DEG /*180 in Q22*/ ) )
443 : {
444 10704 : FOR( j = 0; j < lengthTri2PolySorted[n]; ++j )
445 : {
446 8143 : assert( ( m + 2 < EFAP_MAX_POLY_SET ) && "EFAP: maximum polygons exceeded!" );
447 :
448 : /* add two new polygons with azimuths wrapped to differing bounds */
449 8143 : efap->polyData.polysetArray[m + 1].polyAzi[j] = efap_32mod32( efap->polyData.polysetArray[m].polyAzi[j] /*q22*/, Q22_360_DEG /*360 q22*/ ); /* Q22 */
450 8143 : move32();
451 8143 : efap->polyData.polysetArray[m + 2].polyAzi[j] = L_sub( efap->polyData.polysetArray[m + 1].polyAzi[j] /*q22*/, Q22_360_DEG /*360 q22*/ ); /* Q22 */
452 8143 : move32();
453 :
454 : /* Copy the rest of the fields */
455 8143 : efap->polyData.polysetArray[m + 1].chan[j] = efap->polyData.polysetArray[m].chan[j];
456 8143 : move16();
457 8143 : efap->polyData.polysetArray[m + 1].polyEle[j] = efap->polyData.polysetArray[m].polyEle[j]; /* Q22 */
458 8143 : move32();
459 8143 : efap->polyData.polysetArray[m + 1].isNaN[j] = efap->polyData.polysetArray[m].isNaN[j];
460 8143 : move16();
461 8143 : efap->polyData.polysetArray[m + 1].numChan = lengthTri2PolySorted[n];
462 8143 : move16();
463 :
464 8143 : efap->polyData.polysetArray[m + 2].chan[j] = efap->polyData.polysetArray[m].chan[j];
465 8143 : move16();
466 8143 : efap->polyData.polysetArray[m + 2].polyEle[j] = efap->polyData.polysetArray[m].polyEle[j]; /* Q22 */
467 8143 : move32();
468 8143 : efap->polyData.polysetArray[m + 2].isNaN[j] = efap->polyData.polysetArray[m].isNaN[j];
469 8143 : move16();
470 8143 : efap->polyData.polysetArray[m + 2].numChan = lengthTri2PolySorted[n];
471 8143 : move16();
472 : }
473 2561 : finalLength = add( finalLength, 2 );
474 : }
475 15265 : finalLength = add( finalLength, 1 );
476 : }
477 916 : finalLength = add( finalLength, 1 );
478 :
479 : /* Updating the number of polygons */
480 916 : efap->polyData.numPoly = finalLength;
481 916 : move16();
482 :
483 :
484 916 : return error;
485 : }
486 :
487 :
488 : /*-------------------------------------------------------------------------*
489 : * sphere_triangulation()
490 : *
491 : *
492 : *-------------------------------------------------------------------------*/
493 :
494 916 : 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 916 : set16_fx( vtxInHull, 0, EFAP_MAX_SIZE_TMP_BUFF );
508 :
509 : /* Add Imaginary Speakers */
510 916 : add_ghost_speakers_fx( vtxData->vertexArray, &vtxData->numVtx, efip_flag );
511 :
512 : /* Sort the vertices according to their index */
513 916 : 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 916 : sort_vertices_fx( vtxData->vertexArray, &vtxData->numVtx, vtxData->vtxOrder );
519 :
520 : /* Computing the initial Polyeder */
521 916 : initial_polyeder_fx( vtxData, polyData->triArray, &polyData->numTri, &vtxInHull[0] );
522 :
523 : /* Add the vertex to the convex hull */
524 10756 : FOR( i = 0; i < vtxData->numVtx; ++i )
525 : {
526 9840 : add_vertex_to_convex_hull_fx( vtxData, vtxData->vtxOrder[i], &vtxInHull[0], polyData->triArray, &polyData->numTri );
527 : }
528 :
529 916 : assert( polyData->numTri != 0 && "EFAP: failed to construct convex hull!" );
530 :
531 : /* Allocate the DM matrix transpose */
532 916 : 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 916 : *numTot = vtxData->numVtx;
539 916 : move16();
540 :
541 10756 : FOR( i = 0; i < vtxData->numVtx; i++ ){
542 9840 : 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 916 : *dmTranspose_fx = (Word32 **) p_dmTranspose;
547 :
548 : /* Remap Ghosts */
549 916 : remap_ghosts_fx( vtxData->vertexArray, polyData->triArray, numSpk, &vtxData->numVtx, polyData->numTri, *dmTranspose_fx ); // dmTranspose_fx q31
550 :
551 916 : return IVAS_ERR_OK;
552 : }
553 :
554 :
555 : /*-------------------------------------------------------------------------*
556 : * initial_polyeder()
557 : *
558 : *
559 : *-------------------------------------------------------------------------*/
560 :
561 :
562 916 : 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 916 : assert( triArray != NULL && "EFAP: triArray==NULL" );
580 :
581 : /* initialize variables */
582 916 : set16_fx( tmpSurface, -1, 3 );
583 916 : set32_fx( centroid, 0, 3 );
584 916 : set32_fx( tmp1, 0, 3 );
585 916 : set32_fx( tmp2, 0, 3 );
586 916 : set32_fx( tmp3, 0, 3 );
587 916 : set32_fx( tmpCross, 0, 3 );
588 :
589 916 : numVtx = vtxData->numVtx;
590 916 : move16();
591 :
592 : /* seed vertices */
593 4580 : FOR( i = 0; i < 4; i++ )
594 : {
595 3664 : tetrahedron[i] = i;
596 3664 : move16();
597 : }
598 :
599 : /* 1. attempt to create an edge with nonzero length */
600 916 : test();
601 916 : 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 916 : tmp = 0;
612 916 : move32();
613 916 : v_sub_fixed( vtxData->vertexArray[tetrahedron[1]].pos, vtxData->vertexArray[tetrahedron[0]].pos, tmp1, 3, 1 ); // tmp1 Q(31-1)
614 916 : WHILE( LT_16( tetrahedron[2], numVtx ) )
615 : {
616 916 : v_sub_fixed( vtxData->vertexArray[tetrahedron[2]].pos, vtxData->vertexArray[tetrahedron[0]].pos, tmp2, 3, 1 ); // tmp2 Q(31-1)
617 916 : efap_crossp_fx( tmp1, tmp2, tmpCross ); // tmpCross Q29
618 3664 : FOR( i = 0; i < 3; i++ )
619 : {
620 2748 : tmp = L_add( tmp, Mpy_32_32( tmpCross[i], tmpCross[i] ) ); // tmp Q27
621 : }
622 916 : 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 916 : BREAK;
625 : }
626 0 : tetrahedron[2] = add( tetrahedron[2], 1 );
627 0 : move16();
628 : }
629 916 : assert( tetrahedron[2] < numVtx && "EFAP: convex hull construction failed, vertices are colinear!" );
630 :
631 : /* 3. attempt to create a tetrahedron with nonzero volume */
632 916 : tmp = 0;
633 916 : move32();
634 3287 : WHILE( LT_16( tetrahedron[3], numVtx ) )
635 : {
636 3287 : v_sub_fixed( vtxData->vertexArray[tetrahedron[3]].pos, vtxData->vertexArray[tetrahedron[0]].pos, tmp3, 3, 1 ); // tmp3 Q30
637 3287 : tmp = dotp_fixed( tmp3, tmpCross, 3 ); // tmp Q28
638 3287 : IF( GT_32( L_abs( tmp ), POLY_THRESH_Q28 /*1e-4f Q28*/ ) )
639 : {
640 916 : BREAK;
641 : }
642 2371 : tetrahedron[3] = add( tetrahedron[3], 1 );
643 2371 : move16();
644 : }
645 916 : assert( tetrahedron[3] < numVtx && "EFAP: convex hull construction failed, vertices are coplanar!" );
646 :
647 : /* 4. compute the centroid of the initial tetrahedron */
648 4580 : FOR( i = 0; i < 4; i++ )
649 : {
650 3664 : vtxInHull[tetrahedron[i]] = 1; /* set vertex as added to hull*/
651 3664 : move16();
652 3664 : centroid[0] = L_add( centroid[0], L_shr( vtxData->vertexArray[tetrahedron[i]].pos[0], Q2 ) ); // Q29
653 3664 : move32();
654 3664 : centroid[1] = L_add( centroid[1], L_shr( vtxData->vertexArray[tetrahedron[i]].pos[1], Q2 ) ); // Q29
655 3664 : move32();
656 3664 : centroid[2] = L_add( centroid[2], L_shr( vtxData->vertexArray[tetrahedron[i]].pos[2], Q2 ) ); // Q29
657 3664 : 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 916 : tmpSurface[0] = tetrahedron[0];
667 916 : move16();
668 916 : tmpSurface[1] = tetrahedron[1];
669 916 : move16();
670 916 : tmpSurface[2] = tetrahedron[2];
671 916 : move16();
672 916 : flip_plane_fx( vtxData->vertexArray, tmpSurface, centroid );
673 916 : Copy( tmpSurface, triArray[0].LS, 3 );
674 :
675 916 : tmpSurface[0] = tetrahedron[0];
676 916 : move16();
677 916 : tmpSurface[1] = tetrahedron[1];
678 916 : move16();
679 916 : tmpSurface[2] = tetrahedron[3];
680 916 : move16();
681 916 : flip_plane_fx( vtxData->vertexArray, tmpSurface, centroid );
682 916 : Copy( tmpSurface, triArray[1].LS, 3 );
683 :
684 916 : tmpSurface[0] = tetrahedron[0];
685 916 : move16();
686 916 : tmpSurface[1] = tetrahedron[2];
687 916 : move16();
688 916 : tmpSurface[2] = tetrahedron[3];
689 916 : move16();
690 916 : flip_plane_fx( vtxData->vertexArray, tmpSurface, centroid );
691 916 : Copy( tmpSurface, triArray[2].LS, 3 );
692 :
693 916 : tmpSurface[0] = tetrahedron[1];
694 916 : move16();
695 916 : tmpSurface[1] = tetrahedron[2];
696 916 : move16();
697 916 : tmpSurface[2] = tetrahedron[3];
698 916 : move16();
699 916 : flip_plane_fx( vtxData->vertexArray, tmpSurface, centroid );
700 916 : Copy( tmpSurface, triArray[3].LS, 3 );
701 :
702 : /* set numTri */
703 916 : *numTri = 4;
704 916 : move16();
705 :
706 916 : return;
707 : }
708 :
709 :
710 : /*-------------------------------------------------------------------------*
711 : * add_ghost_speakers_fx()
712 : *
713 : *
714 : *-------------------------------------------------------------------------*/
715 :
716 916 : 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 916 : vtxDmxType = EFAP_DMX_INTENSITY;
739 916 : move32();
740 916 : numVertex = *numVtx;
741 916 : move16();
742 916 : maxAngle = 13421773; //(1.f / 160.0f) in Q31
743 916 : move32();
744 :
745 : /* Extracting Azi and Ele for computation purposes */
746 8688 : FOR( i = 0; i < numVertex; ++i )
747 : {
748 7772 : ele[i] = vertexArray[i].ele; // q22
749 7772 : move32();
750 : }
751 :
752 : /* ADD VOG IF NECESSERAY (i.e. if the elevation of the highest LS is < 90 deg) */
753 916 : a = 0;
754 916 : move16();
755 916 : maximum_l( ele, numVertex, &tmpEle ); // tmpEle q22
756 :
757 916 : lengthVertGhst = 0;
758 916 : move16();
759 916 : IF( LT_32( tmpEle, Q22_90_DEG /*90 q22*/ ) )
760 : {
761 916 : 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 916 : add_vertex_fx( vertexArray, 0, Q22_90_DEG /*90 q22*/, add( numVertex, a ), vtxDmxType );
775 916 : lengthVertGhst = add( lengthVertGhst, 1 );
776 916 : a = add( a, 1 );
777 : }
778 :
779 : /* ADD VOH IF NECESSERAY (i.e. if the elevation of the lowest LS is > -90 deg) */
780 916 : minimum_l( ele, numVertex, &tmpEle ); /*tmpEle q22*/
781 916 : IF( GT_32( tmpEle, -Q22_90_DEG /*90 q22*/ ) )
782 : {
783 916 : 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 916 : add_vertex_fx( vertexArray, 0, -Q22_90_DEG /*90 q22*/, add( numVertex, a ), vtxDmxType );
798 :
799 916 : lengthVertGhst = add( lengthVertGhst, 1 );
800 916 : a = add( a, 1 );
801 : }
802 :
803 : /* LIST ALL SURROUNDING loudspeakers */
804 916 : k = 0;
805 916 : move16();
806 :
807 8688 : FOR( i = 0; i < numVertex; ++i )
808 : {
809 7772 : IF( LT_32( L_abs( vertexArray[i].ele ), Q22_45_DEG /*45 q22*/ ) )
810 : {
811 7568 : tmpAzi[k] = vertexArray[i].azi; // q22
812 7568 : move32();
813 7568 : k = add( k, 1 );
814 : }
815 : }
816 :
817 916 : lengthHorGhst = 0;
818 916 : move16();
819 916 : 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 916 : ELSE IF( EQ_16( k, 1 ) ) /* only one speaker found: add two ghost speakers to complete a triangle */
828 : {
829 89 : add_vertex_fx( vertexArray, L_add( tmpAzi[0], Q22_120_DEG /*120 q22*/ ), 0, add( numVertex, a ), EFAP_DMX_INTENSITY );
830 89 : add_vertex_fx( vertexArray, L_add( tmpAzi[0], Q22_240_DEG /*240 q22*/ ), 0, add( add( numVertex, a ), 1 ), EFAP_DMX_INTENSITY );
831 89 : a = add( a, 2 );
832 89 : 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 827 : 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 7479 : FOR( i = 0; i < k - 1; ++i )
841 : {
842 6652 : tmpAngleDiff[i] = L_sub( tmpAzi[i + 1], tmpAzi[i] ); // q22
843 6652 : move32();
844 6652 : sectors[i] = ceil_fixed( Mpy_32_32( tmpAngleDiff[i], maxAngle ), Q22 ); // q22
845 6652 : move32();
846 :
847 6652 : if ( GT_32( sectors[i], Q22_1 /*1 q22*/ ) )
848 : {
849 0 : lengthHorGhst = add( lengthHorGhst, 1 );
850 : }
851 : }
852 827 : tmpAngleDiff[k - 1] = L_sub( L_add( tmpAzi[0], Q22_360_DEG /*360 q22*/ ), tmpAzi[k - 1] ); // q22
853 :
854 827 : sectors[k - 1] = ceil_fixed( Mpy_32_32( tmpAngleDiff[k - 1], maxAngle ), Q22 ); // q22
855 :
856 827 : 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 8306 : FOR( i = 0; i < k; ++i )
863 : {
864 7479 : 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 916 : *numVtx = add( add( numVertex, lengthHorGhst ), lengthVertGhst );
886 916 : move16();
887 :
888 916 : return;
889 : }
890 :
891 : /*-------------------------------------------------------------------------*
892 : * sort_vertices()
893 : *
894 : *
895 : *-------------------------------------------------------------------------*/
896 :
897 916 : 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 10756 : FOR( i = 0; i < *numVtx; ++i )
908 : {
909 9840 : tmpIdx[i] = vertexArray[i].idx;
910 9840 : move16();
911 : }
912 :
913 : /* Sorting indexes */
914 916 : efap_sort_s_fx( tmpIdx, order, *numVtx );
915 :
916 916 : return;
917 : }
918 :
919 : /*-------------------------------------------------------------------------*
920 : * add_vertex_to_convex_hull_fx()
921 : *
922 : *
923 : *-------------------------------------------------------------------------*/
924 :
925 9840 : 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 9840 : const Word32 threshold = -268; // -1e-6f in Q28
941 9840 : 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 9840 : IF( vtxInHull[vtxIdx] )
949 : {
950 3664 : return;
951 : }
952 :
953 : /* Compute the centroid of the current convex hull */
954 6176 : numHullVtx = 0;
955 6176 : move32();
956 6176 : set32_fx( centroid, 0, 3 );
957 84128 : FOR( i = 0; i < vtxData->numVtx; i++ )
958 : {
959 77952 : IF( vtxInHull[i] )
960 : {
961 48240 : numHullVtx = L_add( numHullVtx, 1 );
962 48240 : centroid[0] = L_add( centroid[0], L_shr( vtxData->vertexArray[i].pos[0], Q4 ) ); // q27
963 48240 : move32();
964 48240 : centroid[1] = L_add( centroid[1], L_shr( vtxData->vertexArray[i].pos[1], Q4 ) ); // q27
965 48240 : move32();
966 48240 : centroid[2] = L_add( centroid[2], L_shr( vtxData->vertexArray[i].pos[2], Q4 ) ); // q27
967 48240 : move32();
968 : }
969 : }
970 : /* numHullVtx = 1.0f / numHullVtx; */
971 6176 : tmp16 = BASOP_Util_Divide3232_Scale( 1, numHullVtx, &tmp_e ); // q15-tmp_e
972 6176 : tmp32 = L_shl_sat( tmp16, add( Q16, tmp_e ) ); /* Q31 */
973 :
974 6176 : centroid[0] = Mpy_32_32( centroid[0], tmp32 ); // q27
975 6176 : move32();
976 6176 : centroid[1] = Mpy_32_32( centroid[1], tmp32 ); // q27
977 6176 : move32();
978 6176 : centroid[2] = Mpy_32_32( centroid[1], tmp32 ); // q27
979 6176 : move32();
980 :
981 6176 : centroid[0] = L_shl( centroid[0], 4 ); // q31
982 6176 : move32();
983 6176 : centroid[1] = L_shl( centroid[1], 4 ); // q31
984 6176 : move32();
985 6176 : centroid[2] = L_shl( centroid[2], 4 ); // q31
986 6176 : move32();
987 :
988 : /* Processing */
989 6176 : k = 0;
990 6176 : move16();
991 6176 : l = 0;
992 6176 : move16();
993 :
994 77952 : FOR( i = 0; i < *szTri; ++i )
995 : {
996 71776 : tmpDist = vertex_distance_fx( vtxData->vertexArray, triArray[i], vtxIdx ); // Q28
997 71776 : IF( GT_32( tmpDist, threshold ) )
998 : {
999 15093 : visible[k] = i;
1000 15093 : move16();
1001 15093 : k = add( k, 1 );
1002 : }
1003 : ELSE
1004 : {
1005 56683 : Copy( triArray[i].LS, triArrayNew[l].LS, 3 );
1006 56683 : l = add( l, 1 );
1007 : }
1008 : }
1009 :
1010 6176 : visible_edges_fx( triArray, visible, k, numEdges, edges );
1011 :
1012 33621 : FOR( i = 0; i < numEdges[0]; i += 2 )
1013 : {
1014 27445 : surface[0] = edges[i];
1015 27445 : move16();
1016 27445 : surface[1] = edges[i + 1];
1017 27445 : move16();
1018 27445 : surface[2] = vtxIdx;
1019 27445 : move16();
1020 :
1021 27445 : flip_plane_fx( vtxData->vertexArray, surface, centroid );
1022 :
1023 27445 : Copy( surface, triArrayNew[l].LS, 3 );
1024 27445 : l = add( l, 1 );
1025 : }
1026 :
1027 : /* Outputs */
1028 90304 : FOR( i = 0; i < l; i++ )
1029 : {
1030 84128 : Copy( triArrayNew[i].LS, triArray[i].LS, 3 );
1031 : }
1032 6176 : *szTri = l;
1033 6176 : move16();
1034 :
1035 : /* Flag the vertex as added to the hull */
1036 6176 : vtxInHull[vtxIdx] = 1;
1037 6176 : move16();
1038 6176 : return;
1039 : }
1040 :
1041 : /*-------------------------------------------------------------------------*
1042 : * visible_edges()
1043 : *
1044 : *
1045 : *-------------------------------------------------------------------------*/
1046 :
1047 6176 : 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 191456 : FOR( i = 0; i < EFAP_MAX_SIZE_TMP_BUFF; i++ )
1065 : {
1066 185280 : set16_fx( counter[i], 0, EFAP_MAX_SIZE_TMP_BUFF );
1067 185280 : set16_fx( counterTranspose[i], 0, EFAP_MAX_SIZE_TMP_BUFF );
1068 : }
1069 :
1070 : /* Finding the max vertex */
1071 21269 : FOR( i = 0; i < numSurface; ++i )
1072 : {
1073 15093 : tmpMax[i] = triArray[visible[i]].LS[0]; // q0
1074 15093 : move16();
1075 45279 : FOR( j = 1; j < 3; ++j )
1076 : {
1077 30186 : if ( LT_16( tmpMax[i], triArray[visible[i]].LS[j] ) )
1078 : {
1079 15254 : tmpMax[i] = triArray[visible[i]].LS[j]; // q0
1080 15254 : move16();
1081 : }
1082 : }
1083 : }
1084 6176 : maxVertex = tmpMax[maximum_s( tmpMax, numSurface, NULL )];
1085 21269 : FOR( i = 0; i < numSurface; ++i )
1086 : {
1087 15093 : tmpSurface[0] = triArray[visible[i]].LS[0];
1088 15093 : move16();
1089 15093 : tmpSurface[1] = triArray[visible[i]].LS[1];
1090 15093 : move16();
1091 15093 : tmpSurface[2] = triArray[visible[i]].LS[2];
1092 15093 : move16();
1093 15093 : tmpSurface[3] = triArray[visible[i]].LS[0];
1094 15093 : move16();
1095 :
1096 60372 : FOR( j = 0; j < 3; ++j )
1097 : {
1098 45279 : a = tmpSurface[j];
1099 45279 : move16();
1100 45279 : b = tmpSurface[j + 1];
1101 45279 : move16();
1102 45279 : counter[a][b] = add( counter[a][b], 1 );
1103 45279 : move16();
1104 45279 : counterTranspose[b][a] = counter[a][b];
1105 45279 : move16();
1106 : }
1107 : }
1108 :
1109 78210 : FOR( i = 0; i < maxVertex + 1; ++i )
1110 : {
1111 977280 : FOR( j = 0; j < maxVertex + 1; ++j )
1112 : {
1113 905246 : counter[i][j] = add( counterTranspose[i][j], counterTranspose[j][i] );
1114 905246 : move16();
1115 : }
1116 : }
1117 :
1118 : /* Finding the edges */
1119 6176 : k = 0;
1120 6176 : move16();
1121 :
1122 72034 : FOR( a = 0; a < maxVertex; ++a )
1123 : {
1124 482464 : FOR( b = a + 1; b < maxVertex + 1; ++b )
1125 : {
1126 416606 : IF( EQ_16( counter[a][b], 1 ) )
1127 : {
1128 27445 : edges[k] = a;
1129 27445 : move16();
1130 27445 : edges[k + 1] = b;
1131 27445 : move16();
1132 27445 : k = add( k, 2 );
1133 : }
1134 : }
1135 : }
1136 :
1137 : /* Outputs */
1138 6176 : *numEdges = k;
1139 6176 : move16();
1140 :
1141 6176 : return;
1142 : }
1143 :
1144 : /*-------------------------------------------------------------------------* \
1145 : * flip_plane() \
1146 : * \
1147 : * \
1148 : *-------------------------------------------------------------------------*/
1149 :
1150 31109 : 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 31109 : dist = point_plane_distance_fx(
1160 31109 : vtxArray[surface[0]].pos,
1161 31109 : vtxArray[surface[1]].pos,
1162 31109 : vtxArray[surface[2]].pos,
1163 : centroid ); // q31
1164 :
1165 31109 : IF( dist > 0 )
1166 : {
1167 : /*efap_flipLeftRight( surface, 3 );*/
1168 16350 : tmp = surface[0];
1169 16350 : move16();
1170 16350 : surface[0] = surface[2];
1171 16350 : move16();
1172 16350 : surface[2] = tmp;
1173 16350 : move16();
1174 : }
1175 :
1176 31109 : return;
1177 : }
1178 :
1179 :
1180 : /*-------------------------------------------------------------------------*
1181 : * remap_ghosts()
1182 : *
1183 : *
1184 : *-------------------------------------------------------------------------*/
1185 916 : 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 916 : Word16 numGhst = 0;
1195 916 : 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 916 : const Word32 thresh = 214748; // 1e-4f in Q31
1210 : Word16 tmp16, tmp_e;
1211 916 : move32();
1212 :
1213 916 : set32_fx( tmpVec, 0, EFAP_MAX_SIZE_TMP_BUFF );
1214 916 : 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 2984 : FOR( g = numVtx - 1; g > numSpk - 1; --g )
1218 : {
1219 : /* find(triangle_mat == ghost, 1, 'first') */
1220 2068 : 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 2068 : numGhst = add( numGhst, 1 );
1239 : }
1240 : }
1241 :
1242 : /* Final number of LS (real + ghosts) */
1243 916 : numTot = add( numSpk, numGhst );
1244 :
1245 : /* Initializing tmpMat as the identity matrix */
1246 10756 : FOR( i = 0; i < numTot; ++i )
1247 : {
1248 9840 : set32_fx( tmpMat[i], 0, numTot );
1249 9840 : set32_fx( tmpNewMat[i], 0, numTot );
1250 :
1251 9840 : tmpMat[i][i] = ONE_IN_Q31; // q31
1252 9840 : move32();
1253 9840 : tmpNewMat[i][i] = ONE_IN_Q31; // q31
1254 9840 : move32();
1255 : }
1256 :
1257 : /* Generate initial sound energy distribution matrix */
1258 2984 : FOR( i = numSpk; i < numTot; ++i )
1259 : {
1260 2068 : tmpL = get_neighbours_fx( triArray, i, numTri, neighbours );
1261 :
1262 : /* Initializing the column to 0 */
1263 22928 : FOR( j = 0; j < numTot; ++j )
1264 : {
1265 20860 : tmpMat[j][i] = 0;
1266 20860 : move32();
1267 20860 : tmpNewMat[j][i] = 0;
1268 20860 : move32();
1269 : }
1270 :
1271 : /* The neighbours are set to 1.0/tmpL */
1272 2068 : tmp16 = BASOP_Util_Divide3232_Scale( 1, tmpL, &tmp_e ); // Q=15-tmp_e
1273 2068 : inv_tmpL = L_shl_sat( tmp16, add( Q16, tmp_e ) ); /* Q31 */
1274 11852 : FOR( j = 0; j < tmpL; ++j )
1275 : {
1276 9784 : tmpMat[neighbours[j]][i] = inv_tmpL; /* Q31 */
1277 9784 : move32();
1278 9784 : tmpNewMat[neighbours[j]][i] = inv_tmpL; /* Q31 */
1279 9784 : move32();
1280 : }
1281 : }
1282 :
1283 : /* Redistributing sound energy */
1284 10756 : FOR( i = 0; i < numTot; ++i )
1285 : {
1286 127152 : FOR( j = 0; j < numTot; ++j )
1287 : {
1288 117312 : tmpNewMat[i][j] = tmpMat[j][i]; /* Q31 */
1289 117312 : move32();
1290 : }
1291 : }
1292 :
1293 2984 : FOR( i = numSpk; i < numTot; ++i )
1294 : {
1295 2068 : Copy32( tmpNewMat[i], tmpVec, numTot ); // q31
1296 :
1297 2068 : tmpDist_e = 0;
1298 2068 : move16();
1299 2068 : tmpDist = sum_32_fx( &tmpVec[numSpk], sub( numTot, numSpk ), &tmpDist_e ); // Q=31-tmpDist_e
1300 :
1301 13420 : WHILE( EQ_16( BASOP_Util_Cmp_Mant32Exp( tmpDist, tmpDist_e, thresh, 0 ), 1 ) )
1302 : {
1303 11352 : matrix_times_row_fx( tmpMat, tmpVec, numTot, tmpVec2 ); // tmpVec2 Q31
1304 11352 : Copy32( tmpVec2, tmpVec, numTot ); // Q31
1305 11352 : set32_fx( tmpVec2, 0, numTot );
1306 11352 : tmpDist_e = 0;
1307 11352 : move16();
1308 11352 : tmpDist = sum_32_fx( &tmpVec[numSpk], sub( numTot, numSpk ), &tmpDist_e );
1309 : }
1310 2068 : Copy32( tmpVec, tmpNewMat[i], numTot ); // q31
1311 : }
1312 :
1313 8688 : FOR( i = 0; i < numSpk; ++i )
1314 : {
1315 : /* Applying a sqrt(2) coeff and obtaining the dmMatrix*/
1316 88386 : FOR( j = 0; j < numSpk; ++j )
1317 : {
1318 80614 : test();
1319 80614 : IF( tmpNewMat[j][i] == 0 || EQ_32( tmpNewMat[j][i], 0x7fffffff /*q31*/ ) )
1320 : {
1321 80614 : downmixMatrixTranspose[j][i] = tmpNewMat[j][i]; /* Q31 */
1322 80614 : 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 23610 : FOR( ; j < numTot; ++j )
1336 : {
1337 15838 : 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 14040 : case EFAP_DMX_INTENSITY:
1348 : default:
1349 14040 : test();
1350 14040 : IF( tmpNewMat[j][i] == 0 || EQ_32( tmpNewMat[j][i], ONE_IN_Q31 /*q31*/ ) )
1351 : {
1352 6384 : downmixMatrixTranspose[j][i] = tmpNewMat[j][i]; /* Q31 */
1353 6384 : move32();
1354 : }
1355 : ELSE
1356 : {
1357 7656 : Word16 exp = 0;
1358 7656 : move16();
1359 7656 : Word32 tmp_sqrt = Sqrt32( tmpNewMat[j][i], &exp ); /*31-exp*/
1360 7656 : tmp_sqrt = L_shl( tmp_sqrt, exp ); /*31*/
1361 7656 : downmixMatrixTranspose[j][i] = tmp_sqrt; /* Q31 */
1362 7656 : move32();
1363 : }
1364 14040 : BREAK;
1365 : }
1366 : }
1367 : }
1368 :
1369 : /* Output */
1370 916 : *numVertex = numTot;
1371 916 : move16();
1372 :
1373 916 : return;
1374 : }
1375 :
1376 : /*-------------------------------------------------------------------------*
1377 : * vertex_init_fx()
1378 : *
1379 : * Initialize the vertex structures
1380 : *-------------------------------------------------------------------------*/
1381 :
1382 916 : 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 8688 : FOR( i = 0; i < efapVtxData->numVtx; i++ )
1392 : {
1393 7772 : add_vertex_fx( efapVtxData->vertexArray, aziSpk[i], eleSpk[i], i, EFAP_DMX_INTENSITY );
1394 : }
1395 :
1396 916 : 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 791943 : 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 791943 : P[0] = azi; // q22
1423 791943 : move32();
1424 791943 : P[1] = ele; // q22
1425 791943 : move32();
1426 791943 : set32_fx( tmpBuff, 0, EFAP_MAX_CHAN_NUM );
1427 791943 : set32_fx( aziPoly, 0, EFAP_MAX_CHAN_NUM );
1428 791943 : set32_fx( elePoly, 0, EFAP_MAX_CHAN_NUM );
1429 791943 : set16_fx( chan, 0, EFAP_MAX_CHAN_NUM );
1430 :
1431 : /* Finding in which polygon the point is */
1432 791943 : polyIdx = get_poly_num_fx( P, polyData );
1433 :
1434 791943 : assert( polyIdx != -1 && "EFAP: polygon not found!" );
1435 :
1436 : /* Extracting the chan, the azimuth and the ele of the considered poly */
1437 791943 : numChan = polyData->polysetArray[polyIdx].numChan;
1438 791943 : move16();
1439 :
1440 3221379 : FOR( i = 0; i < numChan; ++i )
1441 : {
1442 2429436 : chan[i] = polyData->polysetArray[polyIdx].chan[i];
1443 2429436 : move16();
1444 2429436 : aziPoly[i] = polyData->polysetArray[polyIdx].polyAzi[i]; // q22
1445 2429436 : move32();
1446 :
1447 2429436 : if ( EQ_16( polyData->polysetArray[polyIdx].isNaN[i], 1 ) )
1448 : {
1449 634310 : aziPoly[i] = P[0]; // q22
1450 634310 : move32();
1451 : }
1452 :
1453 2429436 : elePoly[i] = polyData->polysetArray[polyIdx].polyEle[i]; // q22
1454 2429436 : move32();
1455 : }
1456 :
1457 : /* Computing the gain for the polygon */
1458 791943 : 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 791943 : suma = Mpy_32_32( tmpBuff[0], tmpBuff[0] );
1463 2429436 : FOR( i = 1; i < numChan; i++ )
1464 : {
1465 1637493 : suma = W_add( suma, Mpy_32_32( tmpBuff[i], tmpBuff[i] ) );
1466 : }
1467 791943 : IF( GT_64( suma, MAX_32 ) )
1468 : {
1469 0 : normTmpBuff = MAX_32;
1470 0 : move32();
1471 : }
1472 : ELSE
1473 : {
1474 791943 : normTmpBuff = W_extract_l( suma ); // Q31
1475 : }
1476 :
1477 791943 : Word16 exp = 0;
1478 791943 : move16();
1479 791943 : normTmpBuff = ISqrt32( normTmpBuff, &exp ); // Q=31-exp
1480 :
1481 : /* Updating the buffer structure */
1482 3221379 : FOR( i = 0; i < numChan; ++i )
1483 : {
1484 2429436 : bufferL[chan[i]] = Mpy_32_32( tmpBuff[i], normTmpBuff ); // 31+(31-exp)-31=>31-exp
1485 2429436 : move32();
1486 2429436 : bufferL[chan[i]] = L_shl( bufferL[chan[i]], exp ); // Q31
1487 2429436 : move32();
1488 : }
1489 :
1490 791943 : return;
1491 : }
1492 :
1493 :
1494 : /*-------------------------------------------------------------------------*
1495 : * get_poly_gains_fx()
1496 : *
1497 : * Compute the gain for a precise polygon
1498 : *-------------------------------------------------------------------------*/
1499 :
1500 791943 : 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 791943 : P[0] = azi; // q22
1516 791943 : move32();
1517 791943 : P[1] = ele; // q22
1518 791943 : move32();
1519 :
1520 : /* Processing, we search for the triangle in which belong P, then we compute the gain */
1521 3221379 : FOR( i = 1; i < numChan + 1; ++i )
1522 : {
1523 2429436 : A[0] = aziPoly[i - 1]; // q22
1524 2429436 : move32();
1525 2429436 : A[1] = elePoly[i - 1]; // q22
1526 2429436 : move32();
1527 :
1528 2429436 : v_sub_fixed_no_hdrm( P, A, P_minus_A, 2 ); /* Precalculate value of (P-A) q22*/
1529 :
1530 2538013 : FOR( j = i; j < numChan - 2 + i; ++j )
1531 : {
1532 2536423 : idx1 = add( 1, ( j % numChan ) );
1533 2536423 : idx2 = add( 1, ( idx1 % numChan ) );
1534 :
1535 2536423 : B[0] = aziPoly[idx1 - 1]; // q22
1536 2536423 : move32();
1537 2536423 : B[1] = elePoly[idx1 - 1]; // q22
1538 2536423 : move32();
1539 :
1540 2536423 : C[0] = aziPoly[idx2 - 1]; // q22
1541 2536423 : move32();
1542 2536423 : C[1] = elePoly[idx2 - 1]; // q22
1543 2536423 : move32();
1544 :
1545 2536423 : IF( in_tri_fx( A, B, C, P_minus_A ) )
1546 : {
1547 2427846 : buffer[i - 1] = L_shl_sat( get_tri_gain_fx( A, B, C, P_minus_A ), Q18 ); // q13+q18->q31
1548 2427846 : move32();
1549 2427846 : BREAK;
1550 : }
1551 : }
1552 : }
1553 :
1554 791943 : return;
1555 : }
1556 :
1557 : /*-------------------------------------------------------------------------*
1558 : * get_tri_gain_fx()
1559 : *
1560 : * Compute the value of the gain for a given triangle
1561 : *-------------------------------------------------------------------------*/
1562 :
1563 2427846 : static Word32 get_tri_gain_fx(
1564 : const Word32 A[2], /* i : Coordinate of one apex of the triangle q22*/
1565 : const Word32 B[2], /* i : Coordinate of one apex of the triangle q22*/
1566 : const Word32 C[2], /* i : Coordinate of one apex of the triangle q22*/
1567 : const Word32 P_minus_A[2] /* i : Value of (P - A) q22 */
1568 : )
1569 : {
1570 : Word32 N[2], tmpN[2];
1571 : Word32 tmpSub1[2];
1572 : Word32 tmpDot1, tmpDot2;
1573 : Word32 gain;
1574 :
1575 : /* Processing */
1576 2427846 : tmpN[0] = L_sub( B[1], C[1] ); // q22
1577 2427846 : move32();
1578 2427846 : tmpN[1] = L_sub( C[0], B[0] ); // q22
1579 2427846 : move32();
1580 :
1581 2427846 : v_sub_fixed_no_hdrm( B, A, tmpSub1, 2 ); // tmpSub1 q22
1582 :
1583 2427846 : tmpDot1 = dotp_fixed( tmpN, tmpSub1, 2 ); // Q13
1584 :
1585 2427846 : Word16 exp = Q18;
1586 2427846 : move16();
1587 2427846 : Word32 inv_tmpDot2 = L_shl( tmpDot1, norm_l( tmpDot1 ) );
1588 2427846 : exp = sub( exp, norm_l( tmpDot1 ) );
1589 2427846 : Word16 inv_tmpDot1 = Inv16( extract_h( inv_tmpDot2 ), &exp ); // 15-exp
1590 2427846 : v_multc_fixed( tmpN, L_shl( inv_tmpDot1, add( Q16, exp ) ), N, 2 ); // 22+31-31->22
1591 :
1592 2427846 : tmpDot2 = dotp_fixed( P_minus_A, N, 2 ); // 22+22-31->13
1593 :
1594 2427846 : if ( EQ_32( tmpDot2, 8191 ) )
1595 : {
1596 142547 : tmpDot2 = 8192; // Q13
1597 142547 : move32();
1598 : }
1599 :
1600 2427846 : gain = L_sub( 8192, tmpDot2 ); // Q13
1601 : /* Set gains <= -60dB to 0 to avoid problems in SVD */
1602 2427846 : if ( EQ_16( BASOP_Util_Cmp_Mant32Exp( L_abs( gain ), 18, 2147 /*1e-6 q31*/, 0 ), -1 ) )
1603 : {
1604 366520 : gain = 0;
1605 366520 : move32();
1606 : }
1607 2427846 : return gain; // Q13
1608 : }
1609 :
1610 :
1611 : /*-------------------------------------------------------------------------*
1612 : * add_vertex_fx()
1613 : *
1614 : * Add a vertex to the vertex array
1615 : *-------------------------------------------------------------------------*/
1616 :
1617 9840 : static void add_vertex_fx(
1618 : EFAP_VERTEX *vtxArray, /* i/o: Handle to the vertex array that will be updated */
1619 : const Word32 azi, /* i : Azimuth of the vertex Q22 */
1620 : const Word32 ele, /* i : Elevation of the vertex Q22 */
1621 : const Word16 pos, /* i : Index in the vtxArray where we want to add the vertex */
1622 : const EFAP_VTX_DMX_TYPE dmxType /* i : downmix type for the vertex */
1623 : )
1624 : {
1625 : Word32 idxAziTmp, idxEleTmp;
1626 : Word32 tmp;
1627 :
1628 9840 : assert( vtxArray != NULL && "EFAP: vtxArray == NULL" );
1629 :
1630 : /* Updating the vertex array */
1631 :
1632 9840 : tmp = efap_32mod32( L_sub( Q22_180_DEG /*180 q22*/, azi ), Q22_360_DEG /*360 q22*/ ); // q22
1633 9840 : vtxArray[pos].azi = L_sub( Q22_180_DEG /*180 q22*/, tmp ); // q22
1634 9840 : move32();
1635 :
1636 9840 : IF( LT_32( Q22_180_DEG /*180 q22*/, ele ) )
1637 0 : tmp = Q22_180_DEG /*180 q22*/;
1638 : ELSE
1639 9840 : tmp = ele; // Q22
1640 9840 : move32();
1641 9840 : IF( GT_32( -Q22_180_DEG /*180 q22*/, tmp ) )
1642 0 : vtxArray[pos].ele = -Q22_180_DEG /*180 q22*/;
1643 : ELSE
1644 9840 : vtxArray[pos]
1645 9840 : .ele = tmp; // q22
1646 9840 : move32();
1647 :
1648 : /* Converting spherical coordinates to cartesians, assuming radius = 1 */
1649 9840 : sph2cart_fx( vtxArray[pos].azi, vtxArray[pos].ele, &vtxArray[pos].pos[0] ); // vtxArray[pos].pos[0] q31
1650 :
1651 : /* Computing the index defined by idx = idxAziTmp + 181 * idxEleTmp */
1652 :
1653 : /* IdxAziTmp */
1654 9840 : tmp = L_abs( L_sub( Q22_90_DEG /*90 q22*/, L_abs( vtxArray[pos].azi ) ) ); // Q22
1655 9840 : idxAziTmp = L_shr( anint_fixed( tmp, Q22 ), Q22 ); // q22-q22->q0
1656 :
1657 : /* IdxEleTmp */
1658 9840 : tmp = L_abs( vtxArray[pos].ele ); // q22
1659 9840 : idxEleTmp = tmp; // q22
1660 9840 : move16();
1661 9840 : idxEleTmp = L_sub( Q22_90_DEG /*90 q22*/, idxEleTmp ); // q22
1662 :
1663 : /* Final Idx */
1664 9840 : vtxArray[pos].idx = add( extract_l( idxAziTmp ), i_mult( 181, extract_l( L_shr( idxEleTmp, Q22 ) ) ) ); // q0
1665 :
1666 : /* Setting the nan flag to 0 */
1667 9840 : vtxArray[pos].isNaN = 0;
1668 9840 : move16();
1669 :
1670 : /* Set the default downmix type */
1671 9840 : vtxArray[pos].dmxType = dmxType;
1672 9840 : move32();
1673 :
1674 9840 : return;
1675 : }
1676 :
1677 :
1678 : /*-------------------------------------------------------------------------*
1679 : * efap_sort_s()
1680 : *
1681 : * Sort an integer array
1682 : * (modified version of sort() to return an index array)
1683 : *-------------------------------------------------------------------------*/
1684 :
1685 2984 : static void efap_sort_s_fx(
1686 : Word16 *x, /* i/o: Vector to be sorted */
1687 : Word16 *idx, /* o : Original index positions */
1688 : const Word16 len /* i : vector length */
1689 : )
1690 : {
1691 : Word16 i, j;
1692 : Word16 tempr, tempi;
1693 :
1694 42176 : FOR( i = 0; i < len; i++ )
1695 : {
1696 39192 : idx[i] = i;
1697 39192 : move16();
1698 : }
1699 :
1700 39192 : FOR( i = len - 2; i >= 0; i-- )
1701 : {
1702 36208 : tempr = x[i];
1703 36208 : move16();
1704 36208 : tempi = idx[i];
1705 36208 : move16();
1706 36208 : test();
1707 152345 : FOR( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
1708 : {
1709 116137 : test();
1710 116137 : x[j - 1] = x[j];
1711 116137 : move16();
1712 116137 : idx[j - 1] = idx[j];
1713 116137 : move16();
1714 : }
1715 36208 : x[j - 1] = tempr;
1716 36208 : move16();
1717 36208 : idx[j - 1] = tempi;
1718 36208 : move16();
1719 : }
1720 :
1721 2984 : return;
1722 : }
1723 :
1724 :
1725 : /*-------------------------------------------------------------------------*
1726 : * vertex_distance()
1727 : *
1728 : * Compute the signed distance between a vertex and a hull surface
1729 : *-------------------------------------------------------------------------*/
1730 :
1731 71776 : static Word32 vertex_distance_fx(
1732 : const EFAP_VERTEX *vtxArray, /* i : The considered vertex */
1733 : const EFAP_LS_TRIANGLE tri, /* i : The considered triangle */
1734 : const Word16 vtxIdx /* i : Index of the considered vertex */
1735 : )
1736 : {
1737 : Word32 A[3], B[3], C[3], P[3];
1738 : Word16 i;
1739 :
1740 : /* Assigning the coordinates to the vector */
1741 287104 : FOR( i = 0; i < 3; ++i )
1742 : {
1743 215328 : A[i] = vtxArray[tri.LS[0]].pos[i]; // q31
1744 215328 : move32();
1745 215328 : B[i] = vtxArray[tri.LS[1]].pos[i]; // q31
1746 215328 : move32();
1747 215328 : C[i] = vtxArray[tri.LS[2]].pos[i]; // q31
1748 215328 : move32();
1749 :
1750 215328 : P[i] = vtxArray[vtxIdx].pos[i]; // q31
1751 215328 : move32();
1752 : }
1753 :
1754 71776 : return point_plane_distance_fx( A, B, C, P ); // q28
1755 : }
1756 :
1757 :
1758 : /*-------------------------------------------------------------------------*
1759 : * point_poly_distance_fx()
1760 : *
1761 : * Compute the signed distance between a point and polygon
1762 : *-------------------------------------------------------------------------*/
1763 :
1764 1893397 : static Word32 point_poly_distance_fx(
1765 : const EFAP_POLYSET poly, /* i : The polygon which forms a plane */
1766 : const Word32 X[3] /* i : Cartesian coordinates of the point of interest q31*/
1767 : )
1768 : {
1769 : Word32 P1[3], P2[3], P3[3];
1770 :
1771 1893397 : sph2cart_fx( poly.polyAzi[0], poly.polyEle[0], &P1[0] ); // P1[0] q31
1772 1893397 : sph2cart_fx( poly.polyAzi[1], poly.polyEle[1], &P2[0] ); // P2[0] q31
1773 1893397 : sph2cart_fx( poly.polyAzi[2], poly.polyEle[2], &P3[0] ); // P3[0] q31
1774 :
1775 1893397 : return point_plane_distance_fx( P1, P2, P3, X ); // q28
1776 : }
1777 :
1778 : /*-------------------------------------------------------------------------*
1779 : * point_plane_distance_fx()
1780 : *
1781 : * Compute the signed distance between a point and a given plane
1782 : *-------------------------------------------------------------------------*/
1783 :
1784 2191546 : static Word32 point_plane_distance_fx( // returns output in Q28
1785 : const Word32 P1[3], /* i : First point of the triangle that defines the planes q31*/
1786 : const Word32 P2[3], /* i : Second point of the triangle q31*/
1787 : const Word32 P3[3], /* i : Third point of the triangle q31*/
1788 : const Word32 X[3] /* i : The point of interest q31*/
1789 : )
1790 : {
1791 : Word32 tmpCross1[3], tmpCross2[3];
1792 : Word32 resultCross[3];
1793 : Word32 tmpDot1[3], tmpDot2[3];
1794 : Word32 tmpNorm;
1795 : Word32 dist;
1796 :
1797 : /* Check if the point already matches a triangle vertex */
1798 2191546 : test();
1799 2191546 : test();
1800 2191546 : test();
1801 2191546 : test();
1802 2191546 : test();
1803 2191546 : test();
1804 2191546 : test();
1805 2191546 : test();
1806 2191546 : IF( ( EQ_32( X[0], P1[0] ) && EQ_32( X[1], P1[1] ) && EQ_32( X[2], P1[2] ) ) ||
1807 : ( EQ_32( X[0], P2[0] ) && EQ_32( X[1], P2[1] ) && EQ_32( X[2], P2[2] ) ) ||
1808 : ( EQ_32( X[0], P3[0] ) && EQ_32( X[1], P3[1] ) && EQ_32( X[2], P3[2] ) ) )
1809 : {
1810 63626 : return 0;
1811 : }
1812 :
1813 : /* Cross Product */
1814 2127920 : v_sub_fixed( P1, P2, tmpCross1, 3, 1 ); // tmpCross1 q30
1815 2127920 : v_sub_fixed( P1, P3, tmpCross2, 3, 1 ); // tmpCross2 q30
1816 :
1817 : /* resultCross = cross(P1-P2,P1-P3) */
1818 2127920 : efap_crossp_fx( tmpCross1, tmpCross2, resultCross ); // Q29
1819 :
1820 : /* Dot Product */
1821 2127920 : tmpNorm = dotp_fixed( resultCross, resultCross, 3 ); // Q27
1822 2127920 : Word16 exp = 4;
1823 2127920 : move16();
1824 2127920 : tmpNorm = ISqrt32( tmpNorm, &exp ); // Q29
1825 2127920 : v_sub_fixed( X, P1, tmpDot1, 3, 1 ); // Q30
1826 2127920 : v_multc_fixed( resultCross, tmpNorm, tmpDot2, 3 ); // Q29 - exp
1827 2127920 : dist = L_shl( dotp_fixed( tmpDot1, tmpDot2, 3 ), exp ); // Q28
1828 2127920 : return dist;
1829 : }
1830 :
1831 :
1832 : /*-------------------------------------------------------------------------*
1833 : * efap_crossp_fx()
1834 : *
1835 : * Compute the cross product between column vectors of float of size 3x1
1836 : *-------------------------------------------------------------------------*/
1837 :
1838 2128836 : static void efap_crossp_fx(
1839 : const Word32 *v1, /* i : First float vector Q30 */
1840 : const Word32 *v2, /* i : Second float vector Q30 */
1841 : Word32 *v /* o : Output vector Q29 */
1842 : )
1843 : {
1844 2128836 : v[0] = L_sub( Mpy_32_32( v1[1], v2[2] ), Mpy_32_32( v1[2], v2[1] ) ); /* Q29 */
1845 2128836 : move32();
1846 2128836 : v[1] = L_sub( Mpy_32_32( v1[2], v2[0] ), Mpy_32_32( v1[0], v2[2] ) ); /* Q29 */
1847 2128836 : move32();
1848 2128836 : v[2] = L_sub( Mpy_32_32( v1[0], v2[1] ), Mpy_32_32( v1[1], v2[0] ) ); /* Q29 */
1849 2128836 : move32();
1850 :
1851 2128836 : return;
1852 : }
1853 :
1854 : /*-------------------------------------------------------------------------*
1855 : * find_int_in_tri()
1856 : *
1857 : * Find an integer in triangle array of integers
1858 : *-------------------------------------------------------------------------*/
1859 :
1860 13920 : static Word16 find_int_in_tri_fx(
1861 : const EFAP_LS_TRIANGLE *tri, /* i : Triangle array */
1862 : const Word16 n, /* i : The integer to find */
1863 : const Word16 r, /* i : Number of rows */
1864 : Word16 *pos /* o : Position of the integer */
1865 : )
1866 : {
1867 : Word16 i, j;
1868 :
1869 : /* Find the first element equal to n */
1870 123268 : FOR( i = 0; i < r; ++i )
1871 : {
1872 461663 : FOR( j = 0; j < 3; ++j )
1873 : {
1874 352315 : IF( EQ_16( tri[i].LS[j], n ) )
1875 : {
1876 11852 : pos[0] = i;
1877 11852 : move16();
1878 11852 : pos[1] = j;
1879 11852 : move16();
1880 11852 : return 1;
1881 : }
1882 : }
1883 : }
1884 :
1885 2068 : return 0;
1886 : }
1887 :
1888 : /*-------------------------------------------------------------------------*
1889 : * remove_vertex()
1890 : *
1891 : * Remove a vertex from a vertex structure
1892 : *-------------------------------------------------------------------------*/
1893 0 : static void remove_vertex_fx(
1894 : EFAP_VERTEX *vtxArray, /* i : Vertex array */
1895 : const Word16 idx, /* i : Index of the vertex to remove */
1896 : const Word16 L /* i : Length of the Vertex array */
1897 : )
1898 : {
1899 : Word16 i;
1900 :
1901 0 : assert( idx < L && "EFAP: index out of bounds" );
1902 :
1903 : /* Shift all vertex of one position, so vtxArray[i] will be vtxArray[i+1] and so on */
1904 0 : FOR( i = idx; i < L - 1; ++i )
1905 : {
1906 0 : add_vertex_fx( vtxArray, vtxArray[i + 1].azi, vtxArray[i + 1].ele, i, EFAP_DMX_INTENSITY );
1907 : }
1908 :
1909 : /* The last vertex is set to 0 */
1910 0 : add_vertex_fx( vtxArray, 0, 0, sub( L, 1 ), EFAP_DMX_INTENSITY );
1911 :
1912 0 : return;
1913 : }
1914 :
1915 :
1916 : /*-------------------------------------------------------------------------*
1917 : * get_neighbours()
1918 : *
1919 : * Returns the neighbouring triangles of a vertex
1920 : *-------------------------------------------------------------------------*/
1921 2068 : static Word16 get_neighbours_fx(
1922 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
1923 : const Word16 vtxIdx, /* i : Index of the vertex */
1924 : const Word16 numTri, /* i : Number of Triangles */
1925 : Word16 *neighbours /* o : Output vector */
1926 : )
1927 : {
1928 : Word16 i, j, k;
1929 : Word16 tmpPos[2];
1930 : Word16 tmpNeighbours[EFAP_MAX_SIZE_TMP_BUFF];
1931 : Word16 dummy[EFAP_MAX_SIZE_TMP_BUFF];
1932 : EFAP_LS_TRIANGLE tmpTriArray[EFAP_MAX_POLY_SET];
1933 :
1934 : /* Processing */
1935 35516 : FOR( i = 0; i < numTri; ++i )
1936 : {
1937 33448 : Copy( triArray[i].LS, tmpTriArray[i].LS, 3 );
1938 : }
1939 :
1940 2068 : k = 0;
1941 2068 : move16();
1942 : WHILE( 1 )
1943 : {
1944 11852 : IF( find_int_in_tri_fx( tmpTriArray, vtxIdx, numTri, tmpPos ) == 0 )
1945 : {
1946 2068 : BREAK;
1947 : }
1948 : ELSE
1949 : {
1950 9784 : tmpNeighbours[k] = tmpTriArray[tmpPos[0]].LS[0];
1951 9784 : move16();
1952 9784 : tmpNeighbours[k + 1] = tmpTriArray[tmpPos[0]].LS[1];
1953 9784 : move16();
1954 9784 : tmpNeighbours[k + 2] = tmpTriArray[tmpPos[0]].LS[2];
1955 9784 : move16();
1956 9784 : k = add( k, 3 );
1957 9784 : tmpTriArray[tmpPos[0]].LS[tmpPos[1]] = -1;
1958 9784 : move16();
1959 : }
1960 :
1961 9784 : IF( GT_16( k, i_mult( 3, numTri ) ) )
1962 : {
1963 0 : BREAK;
1964 : }
1965 : }
1966 :
1967 : /* Sorting the neighbours vector */
1968 2068 : efap_sort_s_fx( tmpNeighbours, dummy, k );
1969 :
1970 : /* Creating the output vector, by eliminating redundancies and also deleting the indice == vtxIdx*/
1971 2068 : neighbours[0] = tmpNeighbours[0];
1972 2068 : move16();
1973 2068 : j = 1;
1974 2068 : move16();
1975 :
1976 29352 : FOR( i = 1; i < k; ++i )
1977 : {
1978 27284 : test();
1979 27284 : IF( NE_16( tmpNeighbours[i], tmpNeighbours[i - 1] ) &&
1980 : NE_16( tmpNeighbours[i], vtxIdx ) )
1981 : {
1982 7716 : neighbours[j] = tmpNeighbours[i];
1983 7716 : move16();
1984 7716 : j = add( j, 1 );
1985 : }
1986 : }
1987 :
1988 : /* Output, length of neighbours */
1989 2068 : return j;
1990 : }
1991 :
1992 : /*-------------------------------------------------------------------------*
1993 : * matrix_times_row_fx()
1994 : *
1995 : * Computes the product of a matrix and a row vector
1996 : *-------------------------------------------------------------------------*/
1997 :
1998 11352 : static void matrix_times_row_fx(
1999 : Word32 mat[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF], /* i : The input matrix q31 */
2000 : const Word32 *vec, /* i : The input row vector q31*/
2001 : const Word16 L, /* i : Row length */
2002 : Word32 *out /* o : Output vector q31 */
2003 : )
2004 : {
2005 : Word16 i, j;
2006 :
2007 68112 : FOR( i = 0; i < L; ++i )
2008 : {
2009 340560 : FOR( j = 0; j < L; ++j )
2010 : {
2011 283800 : out[i] = L_add( out[i], Mpy_32_32( mat[i][j], vec[j] ) ); /*31+31-31=>31*/
2012 283800 : move32();
2013 : }
2014 : }
2015 :
2016 11352 : return;
2017 : }
2018 :
2019 : /*-------------------------------------------------------------------------*
2020 : * tri_to_poly()
2021 : *
2022 : * Combines triangles of a surface in order to create polygons
2023 : *-------------------------------------------------------------------------*/
2024 :
2025 916 : static void tri_to_poly_fx(
2026 : const EFAP_VERTEX *vtxArray, /* i : Vertex array */
2027 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
2028 : const Word16 numVtx, /* i : Number of vertices */
2029 : const Word16 numTri, /* i : Number of triangles */
2030 : Word16 sortedChan[EFAP_MAX_POLY_SET][EFAP_MAX_CHAN_NUM], /* o : The matrix that will contain the sorted channels */
2031 : Word16 *outLengthPS, /* o : The length of the sorted channels */
2032 : Word16 outLengthSorted[EFAP_MAX_POLY_SET] /* o : The number of channels for each poly (i.e. outLengthSorted[i] = length(sortedChan[i]) */
2033 : )
2034 : {
2035 : Word16 i, j;
2036 : Word16 lenPoly;
2037 : Word16 lenPolySet;
2038 : Word16 found;
2039 : Word16 replaceIdx;
2040 :
2041 : Word16 poly[EFAP_MAX_CHAN_NUM];
2042 :
2043 : Word16 sortedLengths[EFAP_MAX_POLY_SET];
2044 : Word16 sortedTri[EFAP_MAX_POLY_SET];
2045 :
2046 : Word32 dist;
2047 :
2048 916 : lenPolySet = 0;
2049 916 : move16();
2050 : /* Sorting the polygons */
2051 16932 : FOR( i = 0; i < numTri; ++i )
2052 : {
2053 : /* search for coplanar vertices and add them to the polygon */
2054 16016 : lenPoly = 0;
2055 16016 : move16();
2056 211280 : FOR( j = 0; j < numVtx; ++j )
2057 : {
2058 195264 : dist = L_abs( point_plane_distance_fx(
2059 195264 : vtxArray[triArray[i].LS[0]].pos,
2060 195264 : vtxArray[triArray[i].LS[1]].pos,
2061 195264 : vtxArray[triArray[i].LS[2]].pos,
2062 195264 : vtxArray[j].pos ) ); // Q28
2063 :
2064 195264 : IF( LT_32( dist, 268435 /* 1e-3f in Q28 */ ) )
2065 : {
2066 49550 : assert( lenPoly < EFAP_MAX_CHAN_NUM && "EFAP: exceeded max polygon vertices!" );
2067 49550 : poly[lenPoly] = j;
2068 49550 : move16();
2069 49550 : lenPoly = add( lenPoly, 1 );
2070 : }
2071 : }
2072 :
2073 : /* search existing polygons to determine whether the new one already exists/is a subset or is a superset */
2074 16016 : found = 0;
2075 16016 : move16();
2076 16016 : replaceIdx = -1;
2077 16016 : move16();
2078 167277 : FOR( j = 0; j < lenPolySet; ++j )
2079 : {
2080 152012 : found = compare_poly_fx( sortedChan[j], sortedLengths[j], poly, lenPoly );
2081 :
2082 152012 : IF( found > 0 )
2083 : {
2084 751 : BREAK;
2085 : }
2086 151261 : ELSE IF( found < 0 )
2087 : {
2088 0 : replaceIdx = j;
2089 0 : move16();
2090 : }
2091 : }
2092 :
2093 16016 : IF( found == 0 )
2094 : {
2095 : /* append new poly */
2096 15265 : Copy( poly, sortedChan[lenPolySet], lenPoly );
2097 15265 : sortedTri[lenPolySet] = i;
2098 15265 : move16();
2099 15265 : sortedLengths[lenPolySet] = lenPoly;
2100 15265 : move16();
2101 15265 : lenPolySet = add( lenPolySet, 1 );
2102 : }
2103 751 : ELSE IF( EQ_16( found, -1 ) )
2104 : {
2105 : /* replace with superset */
2106 0 : Copy( poly, sortedChan[replaceIdx], lenPoly );
2107 0 : sortedTri[replaceIdx] = i;
2108 0 : move16();
2109 0 : sortedLengths[replaceIdx] = lenPoly;
2110 0 : move16();
2111 : }
2112 : }
2113 :
2114 : /* Sorting the vertex */
2115 16181 : FOR( i = 0; i < lenPolySet; ++i )
2116 : {
2117 15265 : sort_channels_vertex_fx( vtxArray, triArray, sortedChan[i], sortedLengths[i], sortedTri[i] );
2118 : }
2119 :
2120 : /* Output */
2121 916 : *outLengthPS = lenPolySet;
2122 916 : move16();
2123 916 : Copy( sortedLengths, outLengthSorted, EFAP_MAX_POLY_SET );
2124 916 : return;
2125 : }
2126 :
2127 :
2128 : /*-------------------------------------------------------------------------*
2129 : * compare_poly()
2130 : *
2131 : * Compares a newly created polygon with an existing one
2132 : *-------------------------------------------------------------------------*/
2133 :
2134 152012 : static Word16 compare_poly_fx(
2135 : Word16 *old, /* i : Existing polygon */
2136 : Word16 lenOld, /* i : Length of existing polygon */
2137 : Word16 *new, /* i : New polygon */
2138 : Word16 lenNew /* i : Length of new polygon */
2139 : )
2140 : {
2141 : Word16 i, j;
2142 : Word16 count;
2143 :
2144 152012 : count = 0;
2145 152012 : move16();
2146 :
2147 612011 : FOR( i = 0; i < lenOld; ++i )
2148 : {
2149 1600087 : FOR( j = count; j < lenNew; ++j )
2150 : {
2151 1240844 : IF( EQ_16( old[i], new[j] ) )
2152 : {
2153 100756 : count = add( count, 1 );
2154 100756 : BREAK;
2155 : }
2156 : }
2157 : }
2158 :
2159 152012 : test();
2160 152012 : test();
2161 152012 : IF( EQ_16( count, lenOld ) && LT_16( lenOld, lenNew ) )
2162 : {
2163 : /* new polygon is a superset */
2164 0 : return -1;
2165 : }
2166 152012 : ELSE IF( EQ_16( count, lenNew ) && GE_16( lenOld, lenNew ) )
2167 : {
2168 : /* found as subset or identical */
2169 751 : return 1;
2170 : }
2171 : ELSE
2172 : {
2173 : /* not found */
2174 151261 : return 0;
2175 : }
2176 : }
2177 :
2178 :
2179 : /*-------------------------------------------------------------------------*
2180 : * sort_channels_vertex()
2181 : *
2182 : * Sort the channels of a polygon set according to the vertex azimuth
2183 : *-------------------------------------------------------------------------*/
2184 :
2185 15265 : static void sort_channels_vertex_fx(
2186 : const EFAP_VERTEX *vtxArray, /* i : Vertex array */
2187 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
2188 : Word16 channels[EFAP_MAX_CHAN_NUM], /* o : Channels array to be modified */
2189 : const Word16 lengthChannels, /* i : Length of the channels array */
2190 : Word16 idxTri /* i : Index of the considered triangle */
2191 : )
2192 : {
2193 : Word16 i, j;
2194 :
2195 : Word32 P1[3], P2[3], P3[3];
2196 : Word32 tmpU[3];
2197 : Word32 U[3], V[3];
2198 : Word32 tmpV1[3], tmpV2[3], tmpV3[3];
2199 : Word32 normU, normV;
2200 : Word32 MC[3];
2201 : Word32 tmpP[3], P[3];
2202 :
2203 : Word32 x, y;
2204 :
2205 : Word32 azi[EFAP_MAX_CHAN_NUM];
2206 : Word16 order[EFAP_MAX_CHAN_NUM];
2207 :
2208 : Word16 newChannels[EFAP_MAX_CHAN_NUM];
2209 :
2210 :
2211 : /* Initializing coordinates with the vertices of the considered triangle */
2212 61060 : FOR( i = 0; i < 3; ++i )
2213 : {
2214 45795 : P1[i] = vtxArray[triArray[idxTri].LS[0]].pos[i]; // Q31
2215 45795 : move32();
2216 45795 : P2[i] = vtxArray[triArray[idxTri].LS[1]].pos[i]; // Q31
2217 45795 : move32();
2218 45795 : P3[i] = vtxArray[triArray[idxTri].LS[2]].pos[i]; // Q31
2219 45795 : move32();
2220 : }
2221 :
2222 : /* First Base Vector */
2223 15265 : v_sub_fixed( P2, P1, tmpU, 3, 1 ); // tmpU Q30
2224 15265 : Word16 exp1 = 2;
2225 15265 : move16();
2226 15265 : normU = ISqrt32( dotp_fixed( tmpU, tmpU, 3 ) /*q29*/, &exp1 ); /*q=31-exp1*/
2227 : // normU = L_shl_sat( normU, exp ); //normU Q31
2228 15265 : v_multc_fixed( tmpU, normU, U, 3 ); // U Q30 - exp1
2229 :
2230 : /* Second Base Vector */
2231 15265 : v_sub_fixed( P3, P2, tmpV1, 3, 1 ); // tmpV1 Q30
2232 15265 : v_multc_fixed( U, dotp_fixed( U, tmpV1, 3 ), tmpV2, 3 ); // tmpV2 Q28 - 2*exp1
2233 :
2234 61060 : FOR( i = 0; i < 3; i++ )
2235 : {
2236 45795 : tmpV2[i] = L_shl( tmpV2[i], add( Q2, shl( exp1, 1 ) ) ); // q30
2237 45795 : move32();
2238 : }
2239 :
2240 15265 : v_sub_fixed_no_hdrm( tmpV1, tmpV2, tmpV3, 3 ); // tmpV3 Q30
2241 15265 : Word16 exp2 = 2;
2242 15265 : move16();
2243 15265 : normV = ISqrt32( dotp_fixed( tmpV3, tmpV3, 3 ) /*q29*/, &exp2 ); // q=31-exp2
2244 :
2245 15265 : v_multc_fixed( tmpV3, normV, V, 3 ); // V Q30 - exp2
2246 :
2247 : /* Center of the first Triangle */
2248 61060 : FOR( i = 0; i < 3; ++i )
2249 : {
2250 45795 : 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
2251 45795 : move32();
2252 : }
2253 :
2254 : /* Sort Vertices */
2255 61811 : FOR( i = 0; i < lengthChannels; ++i )
2256 : {
2257 186184 : FOR( j = 0; j < 3; ++j )
2258 : {
2259 139638 : tmpP[j] = vtxArray[channels[i]].pos[j]; // q31
2260 139638 : move32();
2261 : }
2262 :
2263 46546 : v_sub_fixed( tmpP, MC, P, 3, 1 ); // P Q30
2264 :
2265 46546 : x = dotp_fixed( P, U, 3 ); // x Q29 - exp1
2266 46546 : y = dotp_fixed( P, V, 3 ); // y Q29 - exp2
2267 :
2268 : // Executing azi[i] = atan2f( y, x );
2269 46546 : azi[i] = L_shl( BASOP_util_atan2( y, x, sub( exp2, exp1 ) ), Q16 ); // azi 2Q29
2270 46546 : move32();
2271 : }
2272 :
2273 : /* Sorting the azi vec */
2274 15265 : v_sort_ind_fixed( azi, order, lengthChannels );
2275 :
2276 : /* Updating the channel array */
2277 61811 : FOR( i = 0; i < lengthChannels; ++i )
2278 : {
2279 46546 : newChannels[i] = channels[order[i]];
2280 46546 : move16();
2281 : }
2282 :
2283 : /* return Success */
2284 15265 : Copy( newChannels, channels, lengthChannels );
2285 :
2286 15265 : return;
2287 : }
2288 :
2289 : /*-------------------------------------------------------------------------*
2290 : * efap_32mod32()
2291 : *
2292 : * Modulus operation that will handle negative values the same way as matlab
2293 : *-------------------------------------------------------------------------*/
2294 :
2295 17983 : static Word32 efap_32mod32(
2296 : const Word32 x, /* i : Dividend q22*/
2297 : const Word32 y /* i : Divisor q22 */
2298 : )
2299 : {
2300 17983 : Word32 result = x % y; // q22
2301 17983 : move32();
2302 17983 : IF( result >= 0 )
2303 : {
2304 14655 : return result; // q22
2305 : }
2306 : ELSE
2307 : {
2308 3328 : return L_add( result, y ); // q22
2309 : }
2310 : }
2311 :
2312 : /*-------------------------------------------------------------------------*
2313 : * get_poly_num()
2314 : *
2315 : * Returns the index of the polygon in which the coordinate is
2316 : *-------------------------------------------------------------------------*/
2317 :
2318 791943 : static Word16 get_poly_num_fx(
2319 : const Word32 P[2], /* i : Azimuth and elevation of the point q22*/
2320 : const EFAP_POLYSET_DATA *polyData /* i : Polyset struct */
2321 : )
2322 : {
2323 : Word16 i;
2324 : Word16 num_poly, found_poly;
2325 : Word16 poly_tmp[EFAP_MAX_CHAN_NUM];
2326 : Word32 poly_dist[EFAP_MAX_CHAN_NUM];
2327 :
2328 : Word32 dist_tmp;
2329 : Word32 pos[3];
2330 :
2331 791943 : num_poly = 0;
2332 791943 : move16();
2333 :
2334 791943 : sph2cart_fx( P[0], P[1], &pos[0] ); // pos[0] q31
2335 :
2336 : /* Filter the polygon list with a fast 2d check */
2337 17175448 : FOR( i = 0; i < polyData->numPoly; ++i )
2338 : {
2339 16399109 : IF( in_poly_fx( P, polyData->polysetArray[i] ) )
2340 : {
2341 : /* select only polygons which are visible from the point */
2342 1893397 : dist_tmp = point_poly_distance_fx( polyData->polysetArray[i], pos ); // q28
2343 1893397 : IF( dist_tmp == 0 )
2344 : {
2345 15604 : return i;
2346 : }
2347 1877793 : ELSE IF( dist_tmp > 0 )
2348 : {
2349 1141622 : poly_tmp[num_poly] = i;
2350 1141622 : move16();
2351 1141622 : poly_dist[num_poly] = dist_tmp; // q28
2352 1141622 : move32();
2353 1141622 : num_poly = add( num_poly, 1 );
2354 : }
2355 : }
2356 : }
2357 776339 : IF( num_poly == 0 )
2358 : {
2359 0 : return -1;
2360 : }
2361 :
2362 : /* select the polygon with the smallest distance */
2363 776339 : found_poly = poly_tmp[0];
2364 776339 : move16();
2365 776339 : dist_tmp = poly_dist[0]; // q28
2366 776339 : move32();
2367 1141622 : FOR( i = 1; i < num_poly; i++ )
2368 : {
2369 365283 : IF( LT_32( poly_dist[i], dist_tmp ) )
2370 : {
2371 264103 : found_poly = poly_tmp[i];
2372 264103 : move16();
2373 264103 : dist_tmp = poly_dist[i]; // q28
2374 264103 : move32();
2375 : }
2376 : }
2377 :
2378 776339 : return found_poly;
2379 : }
2380 :
2381 : /*-------------------------------------------------------------------------*
2382 : * in_poly()
2383 : *
2384 : * Determines if a given point is within a polygon or not
2385 : *-------------------------------------------------------------------------*/
2386 :
2387 16399109 : static Word16 in_poly_fx( /* Angles are in Q22 */
2388 : const Word32 P[2], /* i : Azimuth and elevation of the point q22*/
2389 : const EFAP_POLYSET poly /* i : Polyset struct */
2390 : )
2391 : {
2392 : Word16 n;
2393 16399109 : Word16 numVertices = poly.numChan;
2394 : Word32 A[2];
2395 : Word32 B[2];
2396 : Word32 C[2];
2397 : Word32 P_minus_A[2];
2398 16399109 : move16();
2399 :
2400 : /* Safety check */
2401 :
2402 16399109 : IF( LT_16( numVertices, 3 ) )
2403 : {
2404 0 : return 0;
2405 : }
2406 :
2407 : /* See if the point is in one of the triangles available in the polygon */
2408 :
2409 16399109 : IF( poly.isNaN[0] )
2410 : {
2411 1096225 : A[0] = P[0]; // q22
2412 1096225 : move32();
2413 : }
2414 : ELSE
2415 : {
2416 15302884 : A[0] = poly.polyAzi[0]; // q22
2417 15302884 : move32();
2418 : }
2419 16399109 : A[1] = poly.polyEle[0]; // q22
2420 16399109 : move32();
2421 :
2422 16399109 : v_sub_fixed_no_hdrm( P, A, P_minus_A, 2 ); /* Precalculate value of (P-A) q22*/
2423 :
2424 32177879 : FOR( n = 1; n < sub( numVertices, 1 ); ++n )
2425 : {
2426 17672167 : IF( poly.isNaN[n] )
2427 : {
2428 8938198 : B[0] = P[0]; // q22
2429 8938198 : move32();
2430 : }
2431 : ELSE
2432 : {
2433 8733969 : B[0] = poly.polyAzi[n]; // q22
2434 8733969 : move32();
2435 : }
2436 17672167 : B[1] = poly.polyEle[n]; // q22
2437 17672167 : move32();
2438 :
2439 17672167 : IF( poly.isNaN[n + 1] )
2440 : {
2441 1091608 : C[0] = P[0]; // q22
2442 1091608 : move32();
2443 : }
2444 : ELSE
2445 : {
2446 16580559 : C[0] = poly.polyAzi[n + 1]; // q22
2447 16580559 : move32();
2448 : }
2449 17672167 : C[1] = poly.polyEle[n + 1]; // q22
2450 17672167 : move32();
2451 :
2452 17672167 : IF( in_tri_fx( A, B, C, P_minus_A ) )
2453 : {
2454 1893397 : return 1;
2455 : }
2456 : }
2457 :
2458 14505712 : return 0;
2459 : }
2460 :
2461 : /*-------------------------------------------------------------------------*
2462 : * in_tri()
2463 : *
2464 : * Determines if a given point is within a triangle or not
2465 : *-------------------------------------------------------------------------*/
2466 :
2467 20208590 : static Word16 in_tri_fx(
2468 : Word32 A[2], /* i : Coordinate of one apex of the triangle q22*/
2469 : Word32 B[2], /* i : Coordinate of one apex of the triangle q22*/
2470 : Word32 C[2], /* i : Coordinate of one apex of the triangle q22*/
2471 : Word32 P_minus_A[2] /* i : Value of (P - A) q22*/
2472 : )
2473 : {
2474 : Word32 tmpDot1[2], tmpDot2[2];
2475 : Word32 matInv[2][2];
2476 : Word32 invFactor;
2477 : Word16 tmp_e;
2478 : Word32 tmp32;
2479 : Word64 S[2];
2480 : /* Threshold adjusted */
2481 20208590 : Word64 thresh_int = 35184640; // 1e-6f in Q45
2482 20208590 : move64();
2483 :
2484 : /*
2485 : Not a Valid Triangle : Colinear edges
2486 : In the matlab implementation, the rcond() function is used
2487 : Since it's very complex to implement this in C
2488 : I'll just compute the determinant and if it's equal to 0, that means the two vectors are colinear
2489 : */
2490 :
2491 20208590 : v_sub_fixed_no_hdrm( B, A, tmpDot1, 2 ); // tmpDot1 q22
2492 20208590 : v_sub_fixed_no_hdrm( C, A, tmpDot2, 2 ); // tmpDot2 q22
2493 :
2494 : /* Verification of the non-colinearity : Q22 * Q22 = Q13 */
2495 : #ifdef OPT_SBA_REND_V1_BE
2496 20208590 : invFactor = Msub_32_32( Mpy_32_32( tmpDot1[0], tmpDot2[1] ), tmpDot1[1], tmpDot2[0] ); /*q22+q22-q31->q13*/
2497 : #else /* OPT_SBA_REND_V1_BE */
2498 : invFactor = L_sub( Mpy_32_32( tmpDot1[0], tmpDot2[1] ), Mpy_32_32( tmpDot1[1], tmpDot2[0] ) ); /*q22+q22-q31->q13*/
2499 : #endif /* OPT_SBA_REND_V1_BE */
2500 :
2501 20208590 : IF( invFactor == 0 )
2502 : {
2503 408 : return 0;
2504 : }
2505 :
2506 : /* invFactor = 1.f / invFactor; */
2507 20208182 : tmp32 = BASOP_Util_Divide3232_Scale_newton( ONE_IN_Q13, invFactor, &tmp_e ); /*31-tmp_e*/
2508 20208182 : invFactor = L_shl_sat( tmp32, tmp_e ); /* Q31 */
2509 :
2510 20208182 : Word16 invFactor_exp = norm_l( invFactor );
2511 20208182 : invFactor = L_shl( invFactor, invFactor_exp ); // 31+invFactor_exp
2512 :
2513 : // Q22 = Q22 * Q31
2514 20208182 : matInv[0][0] = Mpy_32_32( tmpDot2[1], invFactor ); // q=22+invFactor_exp
2515 20208182 : move32();
2516 20208182 : matInv[0][1] = Mpy_32_32( L_negate( tmpDot2[0] ), invFactor ); // q=22+invFactor_exp
2517 20208182 : move32();
2518 20208182 : matInv[1][0] = Mpy_32_32( L_negate( tmpDot1[1] ), invFactor ); // q=22+invFactor_exp
2519 20208182 : move32();
2520 20208182 : matInv[1][1] = Mpy_32_32( tmpDot1[0], invFactor ); // q=22+invFactor_exp
2521 20208182 : move32();
2522 20208182 : 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
2523 20208182 : move64();
2524 20208182 : S[0] = W_shr( S[0], invFactor_exp ); // q45
2525 20208182 : move64();
2526 20208182 : 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
2527 20208182 : move64();
2528 20208182 : S[1] = W_shr( S[1], invFactor_exp ); // q45
2529 20208182 : move64();
2530 :
2531 20208182 : test();
2532 20208182 : test();
2533 20208182 : 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 ) ) )
2534 : {
2535 15886939 : return 0;
2536 : }
2537 : ELSE
2538 : {
2539 4321243 : return 1;
2540 : }
2541 : }
2542 :
2543 : /*-------------------------------------------------------------------------*
2544 : * sph2cart_fx()
2545 : *
2546 : * Converts a vertex position to cartesian coordinates
2547 : *-------------------------------------------------------------------------*/
2548 :
2549 6481974 : static void sph2cart_fx(
2550 : const Word32 azi, /* i : Azimuth in degrees Q22 */
2551 : const Word32 ele, /* i : Elevation in degrees Q22 */
2552 : Word32 *pos /* o : Cartesian coordinates vector (x, y, z) Q31 */
2553 : )
2554 : {
2555 : Word16 azi_temp, ele_temp;
2556 :
2557 6481974 : azi_temp = extract_l( L_shr( Mpy_32_32( azi, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
2558 6481974 : ele_temp = extract_l( L_shr( Mpy_32_32( ele, ONE_BY_360_Q31 ), Q7 ) ); /* Q15 */
2559 :
2560 6481974 : pos[0] = Mpy_32_16( getCosWord16R2( azi_temp ), 0, getCosWord16R2( ele_temp ) ); /* Q31 */
2561 6481974 : move32();
2562 6481974 : pos[1] = Mpy_32_16( getSineWord16R2( azi_temp ), 0, getCosWord16R2( ele_temp ) ); /* Q31 */
2563 6481974 : move32();
2564 6481974 : pos[2] = L_shl( getSineWord16R2( ele_temp ), Q16 ); /* Q31 */
2565 6481974 : move32();
2566 :
2567 6481974 : return;
2568 : }
|
