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.h"
39 : #include "ivas_prot.h"
40 : #include "ivas_prot_rend.h"
41 : #include "ivas_rom_rend.h"
42 : #include "ivas_stat_dec.h"
43 : #ifdef DEBUGGING
44 : #include "debug.h"
45 : #endif
46 : #include "wmc_auto.h"
47 :
48 : /*-----------------------------------------------------------------------*
49 : * Local constants
50 : *-----------------------------------------------------------------------*/
51 :
52 : #define EFAP_MAX_SIZE_TMP_BUFF 30
53 : #define EFAP_MAX_GHOST_LS 5 /* Maximum number of ghost Loudspeakers, for memory allocation purpose */
54 : #define POLY_THRESH 1e-4f
55 : #ifdef DEBUG_EFAP_POLY_TOFILE
56 : #define PANNING_AZI_RESOLUTION 2
57 : #define PANNING_ELE_RESOLUTION 5
58 : #endif
59 :
60 :
61 : /*-----------------------------------------------------------------------*
62 : * Local function prototypes
63 : *-----------------------------------------------------------------------*/
64 :
65 : static ivas_error poly_init( EFAP *efap, const int16_t efip_flag );
66 :
67 : static ivas_error sphere_triangulation( const int16_t numSpk, EFAP_VERTEX_DATA *vtxData, EFAP_POLYSET_DATA *polyData, float ***dmTranspose, int16_t *numTot, const int16_t efip_flag );
68 :
69 : static void initial_polyeder( EFAP_VERTEX_DATA *vtxData, EFAP_LS_TRIANGLE *triArray, int16_t *numTri, int16_t *vtxInHull );
70 :
71 : static void add_ghost_speakers( EFAP_VERTEX *vertexArray, int16_t *numVtx, const int16_t efip_flag );
72 :
73 : static void sort_vertices( const EFAP_VERTEX *vertexArray, const int16_t *numVtx, int16_t *order );
74 :
75 : static void add_vertex_to_convex_hull( const EFAP_VERTEX_DATA *vtxData, const int16_t vtxIdx, int16_t *vtxInHull, EFAP_LS_TRIANGLE *triArray, int16_t *szTri );
76 :
77 : static void visible_edges( const EFAP_LS_TRIANGLE *triArray, const int16_t *visible, const int16_t numSurface, int16_t *numEdges, int16_t *edges );
78 :
79 : static void flip_plane( const EFAP_VERTEX *vtxArray, int16_t *surface, const float centroid[3] );
80 :
81 : static void remap_ghosts( EFAP_VERTEX *vtxArray, EFAP_LS_TRIANGLE *triArray, int16_t numSpk, int16_t *numVertex, int16_t numTri, float **downmixMatrix );
82 :
83 : static void vertex_init( const float *aziSpk, const float *eleSpk, EFAP_VERTEX_DATA *efapVtxData );
84 :
85 : static void efap_panning( const float azi, const float ele, const EFAP_POLYSET_DATA *polyData, float *bufferL );
86 :
87 : static void get_poly_gains( const float azi, const float ele, const float aziPoly[EFAP_MAX_CHAN_NUM], const float elePoly[EFAP_MAX_CHAN_NUM], const int16_t numChan, float *buffer );
88 :
89 : static float get_tri_gain( const float A[2], const float B[2], const float C[2], const float P_minus_A[2] );
90 :
91 : #ifdef DEBUG_EFAP_POLY_TOFILE
92 : static void get_poly_select( EFAP_POLYSET_DATA *polyData );
93 : #endif
94 :
95 :
96 : /*-----------------------------------------------------------------------*
97 : * EFAP Utils
98 : *-----------------------------------------------------------------------*/
99 :
100 : static void add_vertex( EFAP_VERTEX *vtxArray, const float azi, const float ele, const int16_t pos, const EFAP_VTX_DMX_TYPE );
101 :
102 : static void efap_sort_s( int16_t *x, int16_t *idx, const int16_t len );
103 :
104 : static float vertex_distance( const EFAP_VERTEX *vtxArray, const EFAP_LS_TRIANGLE tri, const int16_t vtxIdx );
105 :
106 : static float point_plane_distance( const float P1[3], const float P2[3], const float P3[3], const float X[3] );
107 :
108 : static float point_poly_distance( const EFAP_POLYSET poly, const float X[3] );
109 :
110 : static void efap_crossp( const float *v1, const float *v2, float *v );
111 :
112 : static int16_t find_int_in_tri( const EFAP_LS_TRIANGLE *tri, const int16_t n, const int16_t r, int16_t *pos );
113 :
114 : static void remove_vertex( EFAP_VERTEX *vtxArray, const int16_t idx, const int16_t L );
115 :
116 : static int16_t get_neighbours( const EFAP_LS_TRIANGLE *triArray, const int16_t vtxIdx, const int16_t numTri, int16_t *neighbours );
117 :
118 : static void matrix_times_row( float mat[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF], const float *vec, const int16_t L, float *out );
119 :
120 : static void tri_to_poly( const EFAP_VERTEX *vtxArray, const EFAP_LS_TRIANGLE *triArray, const int16_t numVtx, const int16_t numTri, int16_t sortedChan[EFAP_MAX_POLY_SET][EFAP_MAX_CHAN_NUM], int16_t *outLengthPS, int16_t outLengthSorted[EFAP_MAX_POLY_SET] );
121 :
122 : static int16_t compare_poly( int16_t *old, int16_t lenOld, int16_t *new, int16_t lenNew );
123 :
124 : static void sort_channels_vertex( const EFAP_VERTEX *vtxArray, const EFAP_LS_TRIANGLE *triArray, int16_t channels[EFAP_MAX_CHAN_NUM], const int16_t lengthChannels, int16_t idxTri );
125 :
126 : static float efap_fmodf( const float x, const float y );
127 :
128 : static int16_t get_poly_num( const float P[2], const EFAP_POLYSET_DATA *polyData );
129 :
130 : static int16_t in_poly( const float P[2], const EFAP_POLYSET poly );
131 :
132 : static int16_t in_tri( float A[2], float B[2], float C[2], float P_minus_A[2] );
133 :
134 : static void sph2cart( const float azi, const float ele, float *pos );
135 :
136 : /*-----------------------------------------------------------------------*
137 : * Global function definitions
138 : *-----------------------------------------------------------------------*/
139 :
140 : /*-------------------------------------------------------------------------*
141 : * efap_init_data()
142 : *
143 : * Wrap the internal functions to initialize the EFAP data structure
144 : *-------------------------------------------------------------------------*/
145 :
146 2098 : ivas_error efap_init_data(
147 : EFAP_HANDLE *hEFAPdata, /* i/o: handle for EFAP data structure that will be initialized */
148 : const float *speaker_node_azi_deg, /* i : vector of speaker node azimuths (positive left) */
149 : const float *speaker_node_ele_deg, /* i : vector of speaker node elevations (positive up) */
150 : const int16_t num_speaker_nodes, /* i : number of speaker nodes in the set */
151 : const int16_t efap_mode /* i : indicates whether EFAP or EFIP is used */
152 : )
153 : {
154 : /* Handle instance declaration */
155 : int8_t polyset_size;
156 : EFAP *efap;
157 : ivas_error error;
158 :
159 2098 : error = IVAS_ERR_OK;
160 :
161 : /* Basic init checks */
162 2098 : if ( !speaker_node_azi_deg || !speaker_node_ele_deg )
163 : {
164 0 : hEFAPdata = NULL;
165 0 : return IVAS_ERROR( IVAS_ERR_WRONG_PARAMS, "EFAP requires arrays of speaker azimuths and elevations" );
166 : }
167 :
168 : /*-----------------------------------------------------------------*
169 : * Allocate memory
170 : *-----------------------------------------------------------------*/
171 :
172 : /* Memory allocation for main EFAP structure */
173 2098 : if ( ( efap = (EFAP *) malloc( sizeof( EFAP ) ) ) == NULL )
174 : {
175 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP handle\n" ) );
176 : }
177 :
178 : /* Memory allocation and update for aziSpk & eleSpk arrays*/
179 2098 : if ( ( efap->aziSpk = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL )
180 : {
181 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP speaker azimuths\n" ) );
182 : }
183 2098 : if ( ( efap->eleSpk = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL )
184 : {
185 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP speaker elevations\n" ) );
186 : }
187 :
188 : /* Memory allocation for vertexArray */
189 2098 : if ( ( efap->vtxData.vertexArray = (EFAP_VERTEX *) malloc( ( num_speaker_nodes + EFAP_MAX_GHOST_LS ) * sizeof( EFAP_VERTEX ) ) ) == NULL )
190 : {
191 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP Vertex Array\n" ) );
192 : }
193 :
194 : /* Memory allocation for the tmp buffer short */
195 2098 : if ( ( efap->bufferShort = (float *) malloc( num_speaker_nodes * sizeof( float ) ) ) == NULL )
196 : {
197 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP bufferS\n" ) );
198 : }
199 : /* get upper bound of number of polygons required */
200 2098 : polyset_size = efap_poly_limit[num_speaker_nodes - 1];
201 :
202 : /* Memory allocation for the polyset array */
203 2098 : if ( ( efap->polyData.polysetArray = (EFAP_POLYSET *) malloc( polyset_size * sizeof( EFAP_POLYSET ) ) ) == NULL )
204 : {
205 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP bufferS\n" ) );
206 : }
207 :
208 : /* Memory allocation for the triangle array */
209 2098 : if ( ( efap->polyData.triArray = (EFAP_LS_TRIANGLE *) malloc( polyset_size * sizeof( EFAP_LS_TRIANGLE ) ) ) == NULL )
210 : {
211 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP bufferS\n" ) );
212 : }
213 :
214 : /*-----------------------------------------------------------------*
215 : * Initialize values
216 : *-----------------------------------------------------------------*/
217 :
218 2098 : efap->numSpk = num_speaker_nodes;
219 :
220 : /* The number of vertex is first set to the number of LS but will evolve further */
221 2098 : efap->vtxData.numVtx = num_speaker_nodes;
222 :
223 : /* Loudspeaker configuration */
224 2098 : mvr2r( speaker_node_azi_deg, efap->aziSpk, num_speaker_nodes );
225 2098 : mvr2r( speaker_node_ele_deg, efap->eleSpk, num_speaker_nodes );
226 :
227 : /* Initialization of the vertex */
228 2098 : vertex_init( efap->aziSpk, efap->eleSpk, &efap->vtxData );
229 :
230 : /* Initialization of polygons and ghost LS */
231 2098 : if ( ( error = poly_init( efap, efap_mode ) ) != IVAS_ERR_OK )
232 : {
233 0 : return error;
234 : }
235 :
236 : /* Memory allocation for the tmp buffer long */
237 2098 : if ( ( efap->bufferLong = (float *) malloc( efap->vtxData.numVtx * sizeof( float ) ) ) == NULL )
238 : {
239 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP bufferL\n" ) );
240 : }
241 :
242 2098 : *hEFAPdata = efap;
243 2098 : return error;
244 : }
245 :
246 :
247 : /*-------------------------------------------------------------------------*
248 : * efap_determine_gains()
249 : *
250 : * Obtain amplitude panning gains for all speaker nodes based on the
251 : * given direction
252 : *-------------------------------------------------------------------------*/
253 :
254 1873554 : void efap_determine_gains(
255 : EFAP_HANDLE hEFAPdata, /* i : EFAP structure */
256 : float *gains, /* o : gain vector for speaker nodes for given direction */
257 : const float azi_deg, /* i : azimuth in degrees for panning direction (positive left) */
258 : const float ele_deg, /* i : elevation in degrees for panning direction (positive up) */
259 : const int16_t efap_mode /* i : indicates whether EFAP or EFIP is used */
260 : )
261 : {
262 : int16_t i, j;
263 : float azi_wrap, ele_wrap;
264 : float normBuffer;
265 :
266 : /* Resetting bufferShort and bufferLong */
267 1873554 : set_zero( hEFAPdata->bufferShort, hEFAPdata->numSpk );
268 1873554 : set_zero( hEFAPdata->bufferLong, hEFAPdata->vtxData.numVtx );
269 :
270 : /* Wrap angles to correct range */
271 1873554 : panning_wrap_angles( azi_deg, ele_deg, &azi_wrap, &ele_wrap );
272 :
273 : /* Panning */
274 1873554 : efap_panning( azi_wrap, ele_wrap, &hEFAPdata->polyData, hEFAPdata->bufferLong );
275 :
276 1873554 : if ( efap_mode == EFAP_MODE_EFAP )
277 : {
278 1860394 : normBuffer = 0.f;
279 16711985 : for ( j = 0; j < hEFAPdata->numSpk; ++j )
280 : {
281 14851591 : hEFAPdata->bufferShort[j] = 0.f;
282 : /* Multiplying by the downmixMatrix */
283 180727522 : for ( i = 0; i < hEFAPdata->vtxData.numVtx; ++i )
284 : {
285 165875931 : hEFAPdata->bufferShort[j] += hEFAPdata->bufferLong[i] * hEFAPdata->dmTranspose[i][j];
286 : }
287 14851591 : normBuffer = normBuffer + hEFAPdata->bufferShort[j] * hEFAPdata->bufferShort[j];
288 : }
289 1860394 : normBuffer = inv_sqrt( normBuffer );
290 :
291 16711985 : for ( j = 0; j < hEFAPdata->numSpk; ++j )
292 : {
293 14851591 : hEFAPdata->bufferShort[j] *= normBuffer;
294 : }
295 : }
296 : else
297 : {
298 13160 : normBuffer = 0.f;
299 135590 : for ( j = 0; j < hEFAPdata->numSpk; ++j )
300 : {
301 122430 : hEFAPdata->bufferShort[j] = 0.f;
302 : /* Multiplying by the downmixMatrix */
303 1602580 : for ( i = 0; i < hEFAPdata->vtxData.numVtx; ++i )
304 : {
305 1480150 : hEFAPdata->bufferShort[j] += hEFAPdata->bufferLong[i] * hEFAPdata->dmTranspose[i][j];
306 : }
307 122430 : normBuffer = normBuffer + hEFAPdata->bufferShort[j];
308 : }
309 13160 : normBuffer = 1.f / normBuffer;
310 :
311 135590 : for ( j = 0; j < hEFAPdata->numSpk; ++j )
312 : {
313 122430 : hEFAPdata->bufferShort[j] = sqrtf( hEFAPdata->bufferShort[j] * normBuffer );
314 : }
315 : }
316 :
317 : /* Copy gains to output */
318 1873554 : mvr2r( hEFAPdata->bufferShort, gains, hEFAPdata->numSpk );
319 :
320 1873554 : return;
321 : }
322 :
323 :
324 : /*-------------------------------------------------------------------------*
325 : * efap_free_data()
326 : *
327 : * Wrapper to free EFAP data structure
328 : *-------------------------------------------------------------------------*/
329 :
330 5191 : void efap_free_data(
331 : EFAP_HANDLE *hEFAPdata /* i/o: EFAP handle to be freed */
332 : )
333 : {
334 : int16_t i, dim1;
335 : void **p_dmTranspose;
336 :
337 5191 : if ( hEFAPdata == NULL || *hEFAPdata == NULL )
338 : {
339 3093 : return;
340 : }
341 :
342 2098 : dim1 = ( *hEFAPdata )->numTot;
343 :
344 : /* instance buffer members */
345 2098 : free( ( *hEFAPdata )->aziSpk );
346 2098 : ( *hEFAPdata )->aziSpk = NULL;
347 :
348 2098 : free( ( *hEFAPdata )->eleSpk );
349 2098 : ( *hEFAPdata )->eleSpk = NULL;
350 :
351 2098 : free( ( *hEFAPdata )->vtxData.vertexArray );
352 2098 : ( *hEFAPdata )->vtxData.vertexArray = NULL;
353 :
354 2098 : free( ( *hEFAPdata )->vtxData.vtxOrder );
355 2098 : ( *hEFAPdata )->vtxData.vtxOrder = NULL;
356 :
357 2098 : free( ( *hEFAPdata )->polyData.polysetArray );
358 2098 : ( *hEFAPdata )->vtxData.vtxOrder = NULL;
359 :
360 2098 : free( ( *hEFAPdata )->polyData.triArray );
361 2098 : ( *hEFAPdata )->vtxData.vtxOrder = NULL;
362 :
363 2098 : free( ( *hEFAPdata )->bufferLong );
364 2098 : ( *hEFAPdata )->bufferLong = NULL;
365 :
366 2098 : free( ( *hEFAPdata )->bufferShort );
367 2098 : ( *hEFAPdata )->bufferShort = NULL;
368 :
369 2098 : p_dmTranspose = (void **) ( *hEFAPdata )->dmTranspose;
370 25539 : for ( i = 0; i < dim1; i++ )
371 : {
372 23441 : free( p_dmTranspose[i] );
373 : }
374 2098 : free( p_dmTranspose );
375 :
376 : /* instance */
377 2098 : free( *hEFAPdata );
378 2098 : *hEFAPdata = NULL;
379 :
380 2098 : return;
381 : }
382 :
383 :
384 : /*-----------------------------------------------------------------------*
385 : * Local function definitions
386 : *-----------------------------------------------------------------------*/
387 :
388 :
389 : /*-------------------------------------------------------------------------*
390 : * poly_init()
391 : *
392 : * Main function for the Efap initialization whose purpose is to initialize
393 : * the different polygons and to add the ghost speakers
394 : *-------------------------------------------------------------------------*/
395 :
396 2098 : static ivas_error poly_init(
397 : EFAP *efap, /* i/o: A pointer to a handle to efap instance */
398 : const int16_t efip_flag /* i : flag to indicate whether initialization is for EFIP (used for ALLRAD) */
399 : )
400 : {
401 : int16_t n, m, j;
402 : int16_t finalLength, lengthTri2PolyPS;
403 : int16_t lengthTri2PolySorted[EFAP_MAX_POLY_SET];
404 : int16_t sortedChan[EFAP_MAX_POLY_SET][EFAP_MAX_CHAN_NUM];
405 : float tmpMax, tmpMin;
406 : ivas_error error;
407 :
408 2098 : error = IVAS_ERR_OK;
409 :
410 : /* Safety Check */
411 2098 : assert( efap != NULL && "EFAP: efap == NULL" );
412 :
413 115390 : for ( n = 0; n < EFAP_MAX_POLY_SET; n++ )
414 : {
415 113292 : set_s( sortedChan[n], 0, EFAP_MAX_CHAN_NUM );
416 : }
417 :
418 : /* Computing the different ghost vertex, the downmix matrix and the triangle array */
419 2098 : if ( ( error = sphere_triangulation( efap->numSpk, &efap->vtxData, &efap->polyData, &efap->dmTranspose, &efap->numTot, efip_flag ) ) != IVAS_ERR_OK )
420 : {
421 0 : return error;
422 : }
423 :
424 : /* set isNaN for ghost loudspeakers */
425 25539 : for ( n = 0; n < efap->vtxData.numVtx; ++n )
426 : {
427 23441 : if ( efap->vtxData.vertexArray[n].ele > 90.0 - 1e-6 ||
428 21343 : efap->vtxData.vertexArray[n].ele < 1e-6 - 90.0 )
429 : {
430 4196 : efap->vtxData.vertexArray[n].isNaN = true;
431 : }
432 : }
433 :
434 : /* Converting the triangle to polygon structure */
435 2098 : tri_to_poly( efap->vtxData.vertexArray, efap->polyData.triArray, efap->vtxData.numVtx, efap->polyData.numTri, sortedChan, &lengthTri2PolyPS, lengthTri2PolySorted );
436 :
437 : /* Completing the polyData Structure */
438 2098 : finalLength = -1;
439 :
440 38817 : for ( n = 0; n < lengthTri2PolyPS; ++n )
441 : {
442 36719 : m = finalLength + 1;
443 :
444 : /* Complete the fields of the polygon */
445 148647 : for ( j = 0; j < lengthTri2PolySorted[n]; ++j )
446 : {
447 111928 : efap->polyData.polysetArray[m].chan[j] = sortedChan[n][j];
448 111928 : efap->polyData.polysetArray[m].polyAzi[j] = efap->vtxData.vertexArray[sortedChan[n][j]].azi;
449 111928 : efap->polyData.polysetArray[m].polyEle[j] = efap->vtxData.vertexArray[sortedChan[n][j]].ele;
450 111928 : efap->polyData.polysetArray[m].isNaN[j] = efap->vtxData.vertexArray[sortedChan[n][j]].isNaN;
451 : }
452 :
453 36719 : efap->polyData.polysetArray[m].numChan = lengthTri2PolySorted[n];
454 :
455 : /* In case tmpMax - tmpMin > 180, wrap polygon azimuth */
456 36719 : maximum( efap->polyData.polysetArray[m].polyAzi, lengthTri2PolySorted[n], &tmpMax );
457 36719 : minimum( efap->polyData.polysetArray[m].polyAzi, lengthTri2PolySorted[n], &tmpMin );
458 :
459 36719 : if ( ( tmpMax - tmpMin ) > 180 )
460 : {
461 25071 : for ( j = 0; j < lengthTri2PolySorted[n]; ++j )
462 : {
463 19112 : assert( ( m + 2 < EFAP_MAX_POLY_SET ) && "EFAP: maximum polygons exceeded!" );
464 :
465 : /* add two new polygons with azimuths wrapped to differing bounds */
466 19112 : efap->polyData.polysetArray[m + 1].polyAzi[j] = efap_fmodf( efap->polyData.polysetArray[m].polyAzi[j], 360 );
467 19112 : efap->polyData.polysetArray[m + 2].polyAzi[j] = efap->polyData.polysetArray[m + 1].polyAzi[j] - 360;
468 :
469 : /* Copy the rest of the fields */
470 19112 : efap->polyData.polysetArray[m + 1].chan[j] = efap->polyData.polysetArray[m].chan[j];
471 19112 : efap->polyData.polysetArray[m + 1].polyEle[j] = efap->polyData.polysetArray[m].polyEle[j];
472 19112 : efap->polyData.polysetArray[m + 1].isNaN[j] = efap->polyData.polysetArray[m].isNaN[j];
473 19112 : efap->polyData.polysetArray[m + 1].numChan = lengthTri2PolySorted[n];
474 :
475 19112 : efap->polyData.polysetArray[m + 2].chan[j] = efap->polyData.polysetArray[m].chan[j];
476 19112 : efap->polyData.polysetArray[m + 2].polyEle[j] = efap->polyData.polysetArray[m].polyEle[j];
477 19112 : efap->polyData.polysetArray[m + 2].isNaN[j] = efap->polyData.polysetArray[m].isNaN[j];
478 19112 : efap->polyData.polysetArray[m + 2].numChan = lengthTri2PolySorted[n];
479 : }
480 5959 : finalLength += 2;
481 : }
482 36719 : ++finalLength;
483 : }
484 2098 : ++finalLength;
485 :
486 : /* Updating the number of polygons */
487 2098 : efap->polyData.numPoly = finalLength;
488 :
489 :
490 : #ifdef DEBUG_EFAP_POLY_TOFILE
491 : get_poly_select( &efap->polyData );
492 : #endif
493 2098 : return error;
494 : }
495 :
496 : #ifdef DEBUG_EFAP_POLY_TOFILE
497 : static void get_poly_select(
498 : EFAP_POLYSET_DATA *polyData /* o : Polygon data structure */
499 : )
500 : {
501 : int16_t azi_index, ele_index;
502 : float P[2];
503 :
504 : #ifdef DEBUG_EFAP_POLY_TOFILE
505 : /* Write polygon selection table to .csv file, modify filename according to selected loudspeaker layout! */
506 : static FILE *pF = NULL;
507 : if ( pF == NULL )
508 : pF = fopen( "./res/efap_poly_select_cicpX.csv", "w" );
509 : #endif
510 :
511 : for ( azi_index = 0; azi_index <= ( 360 / PANNING_AZI_RESOLUTION ); azi_index++ )
512 : {
513 : P[0] = (float) ( ( azi_index * PANNING_AZI_RESOLUTION ) - 180 );
514 : for ( ele_index = 0; ele_index <= ( 180 / PANNING_ELE_RESOLUTION ); ele_index++ )
515 : {
516 : P[1] = (float) ( ( ele_index * PANNING_ELE_RESOLUTION ) - 90 );
517 :
518 : #ifdef DEBUG_EFAP_POLY_TOFILE
519 : if ( pF != NULL )
520 : fprintf( pF, "%d,", get_poly_num( P, polyData ) );
521 : #endif
522 : }
523 : }
524 :
525 : #ifdef DEBUG_EFAP_POLY_TOFILE
526 : if ( pF != NULL )
527 : fclose( pF );
528 : #endif
529 :
530 : return;
531 : }
532 : #endif
533 :
534 : /*-------------------------------------------------------------------------*
535 : * sphere_triangulation()
536 : *
537 : *
538 : *-------------------------------------------------------------------------*/
539 :
540 2098 : static ivas_error sphere_triangulation(
541 : const int16_t numSpk, /* i : Number of speakers */
542 : EFAP_VERTEX_DATA *vtxData, /* i/o: Vertex data structure */
543 : EFAP_POLYSET_DATA *polyData, /* o : Polygon data structure */
544 : float ***dmTranspose, /* o : Transpose of the downmix matrix */
545 : int16_t *numTot, /* o : Number of speakers (real + ghost) */
546 : const int16_t efip_flag /* i : flag to indicate whether initialization is for EFIP (used for ALLRAD) */
547 : )
548 : {
549 : int16_t i;
550 : void **p_dmTranspose;
551 : int16_t vtxInHull[EFAP_MAX_SIZE_TMP_BUFF];
552 :
553 2098 : set_s( vtxInHull, 0, EFAP_MAX_SIZE_TMP_BUFF );
554 :
555 : /* Add Imaginary Speakers */
556 2098 : add_ghost_speakers( vtxData->vertexArray, &vtxData->numVtx, efip_flag );
557 :
558 : /* Sort the vertices according to their index */
559 2098 : if ( ( vtxData->vtxOrder = (int16_t *) malloc( vtxData->numVtx * sizeof( int16_t ) ) ) == NULL )
560 : {
561 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "Can not allocate memory for EFAP Vertex Order\n" ) );
562 : }
563 :
564 2098 : sort_vertices( vtxData->vertexArray, &vtxData->numVtx, vtxData->vtxOrder );
565 :
566 : /* Computing the initial Polyeder */
567 2098 : initial_polyeder( vtxData, polyData->triArray, &polyData->numTri, &vtxInHull[0] );
568 :
569 : /* Add the vertex to the convex hull */
570 25539 : for ( i = 0; i < vtxData->numVtx; ++i )
571 : {
572 23441 : add_vertex_to_convex_hull( vtxData, vtxData->vtxOrder[i], &vtxInHull[0], polyData->triArray, &polyData->numTri );
573 : }
574 :
575 2098 : assert( polyData->numTri != 0 && "EFAP: failed to construct convex hull!" );
576 :
577 : /* Allocate the DM matrix transpose */
578 2098 : if ( ( p_dmTranspose = malloc( vtxData->numVtx * sizeof( float * ) ) ) == NULL )
579 : {
580 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "EFAP: can not allocate memory for dmTranspose\n" ) );
581 : }
582 :
583 : /* Store the value of numVtx to be used for freeing later (numVtx will change after remap_ghosts() ) */
584 2098 : *numTot = vtxData->numVtx;
585 :
586 25539 : for ( i = 0; i < vtxData->numVtx; i++ )
587 : {
588 23441 : if ( ( p_dmTranspose[i] = malloc( numSpk * sizeof( float ) ) ) == NULL )
589 : {
590 0 : return ( IVAS_ERROR( IVAS_ERR_FAILED_ALLOC, "EFAP: can not allocate memory for dmTranspose\n" ) );
591 : }
592 : }
593 2098 : *dmTranspose = (float **) p_dmTranspose;
594 :
595 : /* Remap Ghosts */
596 2098 : remap_ghosts( vtxData->vertexArray, polyData->triArray, numSpk, &vtxData->numVtx, polyData->numTri, *dmTranspose );
597 :
598 2098 : return IVAS_ERR_OK;
599 : }
600 :
601 :
602 : /*-------------------------------------------------------------------------*
603 : * initial_polyeder()
604 : *
605 : *
606 : *-------------------------------------------------------------------------*/
607 :
608 2098 : static void initial_polyeder(
609 : EFAP_VERTEX_DATA *vtxData, /* i : Vertex data structure */
610 : EFAP_LS_TRIANGLE *triArray, /* o : Triangle array structure */
611 : int16_t *numTri, /* o : Size of triangle array */
612 : int16_t *vtxInHull /* o : Flag if the vertex was added to the inital simplex */
613 : )
614 : {
615 : int16_t i;
616 : int16_t numVtx;
617 : int16_t tmpSurface[3];
618 : int16_t tetrahedron[4];
619 : float tmp;
620 : float centroid[3];
621 : float tmpCross[3];
622 : float tmp1[3], tmp2[3], tmp3[3];
623 :
624 : /* Safety check */
625 2098 : assert( triArray != NULL && "EFAP: triArray==NULL" );
626 :
627 : /* initialize variables */
628 2098 : set_s( tmpSurface, -1, 3 );
629 2098 : set_zero( centroid, 3 );
630 2098 : set_zero( tmp1, 3 );
631 2098 : set_zero( tmp2, 3 );
632 2098 : set_zero( tmp3, 3 );
633 2098 : set_zero( tmpCross, 3 );
634 :
635 2098 : numVtx = vtxData->numVtx;
636 :
637 : /* seed vertices */
638 10490 : for ( i = 0; i < 4; i++ )
639 : {
640 8392 : tetrahedron[i] = i;
641 : }
642 :
643 : /* 1. attempt to create an edge with nonzero length */
644 2098 : while ( ( vtxData->vertexArray[tetrahedron[0]].azi == vtxData->vertexArray[tetrahedron[1]].azi ) &&
645 162 : ( vtxData->vertexArray[tetrahedron[0]].ele == vtxData->vertexArray[tetrahedron[1]].ele ) )
646 : {
647 0 : tetrahedron[1]++;
648 0 : assert( tetrahedron[1] < numVtx && "EFAP: convex hull construction failed, vertices are coincident!" );
649 : }
650 :
651 : /* 2. attempt to create a triangle with nonzero area */
652 2098 : tmp = 0.0f;
653 2098 : v_sub( vtxData->vertexArray[tetrahedron[1]].pos, vtxData->vertexArray[tetrahedron[0]].pos, tmp1, 3 );
654 2098 : while ( tetrahedron[2] < numVtx )
655 : {
656 2098 : v_sub( vtxData->vertexArray[tetrahedron[2]].pos, vtxData->vertexArray[tetrahedron[0]].pos, tmp2, 3 );
657 2098 : efap_crossp( tmp1, tmp2, tmpCross );
658 8392 : for ( i = 0; i < 3; i++ )
659 : {
660 6294 : tmp += tmpCross[i] * tmpCross[i];
661 : }
662 2098 : if ( fabsf( tmp ) > ( POLY_THRESH * POLY_THRESH ) ) /* compare tmp against POLY_THRESH^2 instead of sqrtf(tmp) */
663 : {
664 2098 : break;
665 : }
666 0 : tetrahedron[2]++;
667 : }
668 2098 : assert( tetrahedron[2] < numVtx && "EFAP: convex hull construction failed, vertices are colinear!" );
669 :
670 : /* 3. attempt to create a tetrahedron with nonzero volume */
671 2098 : tmp = 0.0f;
672 8073 : while ( tetrahedron[3] < numVtx )
673 : {
674 8073 : v_sub( vtxData->vertexArray[tetrahedron[3]].pos, vtxData->vertexArray[tetrahedron[0]].pos, tmp3, 3 );
675 8073 : tmp = dotp( tmp3, tmpCross, 3 );
676 8073 : if ( fabsf( tmp ) > POLY_THRESH )
677 : {
678 2098 : break;
679 : }
680 5975 : tetrahedron[3]++;
681 : }
682 2098 : assert( tetrahedron[3] < numVtx && "EFAP: convex hull construction failed, vertices are coplanar!" );
683 :
684 : /* 4. compute the centroid of the initial tetrahedron */
685 10490 : for ( i = 0; i < 4; i++ )
686 : {
687 8392 : vtxInHull[tetrahedron[i]] = 1; /* set vertex as added to hull*/
688 8392 : centroid[0] += vtxData->vertexArray[tetrahedron[i]].pos[0];
689 8392 : centroid[1] += vtxData->vertexArray[tetrahedron[i]].pos[1];
690 8392 : centroid[2] += vtxData->vertexArray[tetrahedron[i]].pos[2];
691 : }
692 :
693 2098 : centroid[0] /= 4;
694 2098 : centroid[1] /= 4;
695 2098 : centroid[2] /= 4;
696 :
697 : /* 5. create and orient planes */
698 2098 : tmpSurface[0] = tetrahedron[0];
699 2098 : tmpSurface[1] = tetrahedron[1];
700 2098 : tmpSurface[2] = tetrahedron[2];
701 2098 : flip_plane( vtxData->vertexArray, tmpSurface, centroid );
702 2098 : mvs2s( tmpSurface, triArray[0].LS, 3 );
703 :
704 2098 : tmpSurface[0] = tetrahedron[0];
705 2098 : tmpSurface[1] = tetrahedron[1];
706 2098 : tmpSurface[2] = tetrahedron[3];
707 2098 : flip_plane( vtxData->vertexArray, tmpSurface, centroid );
708 2098 : mvs2s( tmpSurface, triArray[1].LS, 3 );
709 :
710 2098 : tmpSurface[0] = tetrahedron[0];
711 2098 : tmpSurface[1] = tetrahedron[2];
712 2098 : tmpSurface[2] = tetrahedron[3];
713 2098 : flip_plane( vtxData->vertexArray, tmpSurface, centroid );
714 2098 : mvs2s( tmpSurface, triArray[2].LS, 3 );
715 :
716 2098 : tmpSurface[0] = tetrahedron[1];
717 2098 : tmpSurface[1] = tetrahedron[2];
718 2098 : tmpSurface[2] = tetrahedron[3];
719 2098 : flip_plane( vtxData->vertexArray, tmpSurface, centroid );
720 2098 : mvs2s( tmpSurface, triArray[3].LS, 3 );
721 :
722 : /* set numTri */
723 2098 : *numTri = 4;
724 :
725 2098 : return;
726 : }
727 :
728 :
729 : /*-------------------------------------------------------------------------*
730 : * add_ghost_speakers()
731 : *
732 : *
733 : *-------------------------------------------------------------------------*/
734 :
735 2098 : static void add_ghost_speakers(
736 : EFAP_VERTEX *vertexArray, /* i/o: Vertex array */
737 : int16_t *numVtx, /* i/o: Size of vertex array */
738 : const int16_t efip_flag /* i : flag to indicate whether initialization is for EFIP (used for ALLRAD) */
739 : )
740 : {
741 : int16_t numVertex;
742 : int16_t lengthVertGhst; /* Nb of vertical ghost added */
743 : int16_t lengthHorGhst; /* Nb of Horizontal Ghost */
744 : int16_t i, j, k, a; /* Integer for loops */
745 : int16_t num_new; /* Number of new vertices to add */
746 : float maxAngle; /* Max azimuth tolerance for extend the LS setup horizontaly */
747 : float newDiff; /* Angle differences that will help us set the extended LS setup */
748 : float newAzi; /* New azimuth for the new horizontal LS */
749 : float ele[EFAP_MAX_SIZE_TMP_BUFF];
750 : float tmpEle;
751 : float tmpAzi[EFAP_MAX_SIZE_TMP_BUFF];
752 : float tmpAngleDiff[EFAP_MAX_SIZE_TMP_BUFF]; /* tmp array of angles differences */
753 : float sectors[EFAP_MAX_SIZE_TMP_BUFF]; /* Help us determine the zone where we should extend LS */
754 : EFAP_VTX_DMX_TYPE vtxDmxType;
755 :
756 2098 : vtxDmxType = EFAP_DMX_INTENSITY;
757 2098 : numVertex = *numVtx;
758 2098 : maxAngle = 1.f / 160.0f;
759 :
760 : /* Extracting Azi and Ele for computation purposes */
761 20915 : for ( i = 0; i < numVertex; ++i )
762 : {
763 18817 : ele[i] = vertexArray[i].ele;
764 : }
765 :
766 : /* ADD VOG IF NECESSERAY (i.e. if the elevation of the highest LS is < 90 deg) */
767 2098 : a = 0;
768 2098 : maximum( ele, numVertex, &tmpEle );
769 :
770 2098 : lengthVertGhst = 0;
771 2098 : if ( tmpEle < 90.0f )
772 : {
773 2098 : if ( efip_flag )
774 : {
775 188 : if ( tmpEle > 45.0f )
776 : {
777 6 : vtxDmxType = EFAP_DMX_NONE;
778 : }
779 : else
780 : {
781 182 : vtxDmxType = EFAP_DMX_AMPLITUDE;
782 : }
783 : }
784 :
785 2098 : add_vertex( vertexArray, 0, 90, numVertex + a, vtxDmxType );
786 :
787 2098 : ++lengthVertGhst;
788 2098 : ++a;
789 : }
790 :
791 : /* ADD VOH IF NECESSERAY (i.e. if the elevation of the lowest LS is > -90 deg) */
792 2098 : minimum( ele, numVertex, &tmpEle );
793 2098 : if ( tmpEle > -90.0f )
794 : {
795 2098 : if ( efip_flag )
796 : {
797 188 : if ( tmpEle < -45.0f )
798 : {
799 6 : vtxDmxType = EFAP_DMX_NONE;
800 : }
801 : else
802 : {
803 182 : vtxDmxType = EFAP_DMX_AMPLITUDE;
804 : }
805 : }
806 :
807 2098 : add_vertex( vertexArray, 0, -90, numVertex + a, vtxDmxType );
808 :
809 2098 : ++lengthVertGhst;
810 2098 : ++a;
811 : }
812 :
813 : /* LIST ALL SURROUNDING loudspeakers */
814 2098 : k = 0;
815 :
816 20915 : for ( i = 0; i < numVertex; ++i )
817 : {
818 18817 : if ( fabsf( vertexArray[i].ele ) < 45.0f )
819 : {
820 18409 : tmpAzi[k] = vertexArray[i].azi;
821 18409 : ++k;
822 : }
823 : }
824 :
825 2098 : lengthHorGhst = 0;
826 2098 : if ( k == 0 ) /* no speakers found: add a triangle of ghost speakers */
827 : {
828 0 : add_vertex( vertexArray, 0, 0, numVertex + a, EFAP_DMX_INTENSITY );
829 0 : add_vertex( vertexArray, 120, 0, numVertex + a + 1, EFAP_DMX_INTENSITY );
830 0 : add_vertex( vertexArray, 240, 0, numVertex + a + 2, EFAP_DMX_INTENSITY );
831 0 : a += 3;
832 0 : lengthHorGhst += 3;
833 : }
834 2098 : else if ( k == 1 ) /* only one speaker found: add two ghost speakers to complete a triangle */
835 : {
836 162 : add_vertex( vertexArray, tmpAzi[0] + 120, 0, numVertex + a, EFAP_DMX_INTENSITY );
837 162 : add_vertex( vertexArray, tmpAzi[0] + 240, 0, numVertex + a + 1, EFAP_DMX_INTENSITY );
838 162 : a += 2;
839 162 : lengthHorGhst += 2;
840 : }
841 : else /* fill gaps greater than maxAngle */
842 : {
843 : /* Here, k correspond to the number of LS whose ele is < 45 deg, should be = numVertex */
844 1936 : v_sort( tmpAzi, 0, k - 1 );
845 :
846 : /* The next lines correspond to angle_diff = [azi(2:end), azi(1) + 360] - azi; in Matlab */
847 18247 : for ( i = 0; i < k - 1; ++i )
848 : {
849 16311 : tmpAngleDiff[i] = tmpAzi[i + 1] - tmpAzi[i];
850 16311 : sectors[i] = ceilf( tmpAngleDiff[i] * maxAngle );
851 :
852 16311 : if ( sectors[i] > 1 )
853 : {
854 0 : ++lengthHorGhst;
855 : }
856 : }
857 1936 : tmpAngleDiff[k - 1] = tmpAzi[0] + 360 - tmpAzi[k - 1];
858 :
859 1936 : sectors[k - 1] = ceilf( tmpAngleDiff[k - 1] * maxAngle );
860 :
861 1936 : if ( sectors[k - 1] > 1 )
862 : {
863 104 : ++lengthHorGhst;
864 : }
865 :
866 : /* Adding new virtual speakers */
867 20183 : for ( i = 0; i < k; ++i )
868 : {
869 18247 : if ( sectors[i] > 1 )
870 : {
871 104 : newDiff = tmpAngleDiff[i] / sectors[i];
872 104 : num_new = (int16_t) ( sectors[i] - 1.0f );
873 :
874 208 : for ( j = 0; j < num_new; ++j )
875 : {
876 104 : newAzi = tmpAzi[i] + ( j + 1 ) * newDiff;
877 :
878 104 : add_vertex( vertexArray, newAzi, 0, numVertex + a, EFAP_DMX_INTENSITY );
879 104 : ++a;
880 :
881 104 : if ( j > 0 )
882 : {
883 0 : ++lengthHorGhst;
884 : }
885 : }
886 : }
887 : }
888 : }
889 2098 : *numVtx = numVertex + lengthHorGhst + lengthVertGhst;
890 :
891 2098 : return;
892 : }
893 :
894 :
895 : /*-------------------------------------------------------------------------*
896 : * sort_vertices()
897 : *
898 : *
899 : *-------------------------------------------------------------------------*/
900 :
901 2098 : static void sort_vertices(
902 : const EFAP_VERTEX *vertexArray, /* i : Vertex array */
903 : const int16_t *numVtx, /* i : Size of vertex array */
904 : int16_t *order /* o : Original index positions */
905 : )
906 : {
907 : int16_t tmpIdx[EFAP_MAX_SIZE_TMP_BUFF];
908 : int16_t i;
909 :
910 : /* Initializing tmpIdx */
911 25539 : for ( i = 0; i < *numVtx; ++i )
912 : {
913 23441 : tmpIdx[i] = vertexArray[i].idx;
914 : }
915 :
916 : /* Sorting indexes */
917 2098 : efap_sort_s( tmpIdx, order, *numVtx );
918 :
919 2098 : return;
920 : }
921 :
922 :
923 : /*-------------------------------------------------------------------------*
924 : * add_vertex_to_convex_hull()
925 : *
926 : *
927 : *-------------------------------------------------------------------------*/
928 :
929 23441 : static void add_vertex_to_convex_hull(
930 : const EFAP_VERTEX_DATA *vtxData, /* i : Vertex data structure */
931 : const int16_t vtxIdx, /* i : Vertex to be added to the hull */
932 : int16_t *vtxInHull, /* i/o: Array indicating whether the vertex is part of the hull */
933 : EFAP_LS_TRIANGLE *triArray, /* i/o: Triangle array */
934 : int16_t *szTri /* i/o: Size of Triangle array */
935 : )
936 : {
937 : int16_t i, k, l;
938 : int16_t visible[EFAP_MAX_SIZE_TMP_BUFF];
939 : int16_t edges[EFAP_MAX_SIZE_TMP_BUFF];
940 : int16_t numEdges[1];
941 : int16_t surface[3];
942 : float numHullVtx;
943 : float centroid[3];
944 23441 : const float threshold = -1e-6f;
945 : float tmpDist;
946 : EFAP_LS_TRIANGLE triArrayNew[EFAP_MAX_POLY_SET];
947 :
948 : /* If the vertex is already part of the hull, nothing must be done */
949 23441 : if ( vtxInHull[vtxIdx] )
950 : {
951 8392 : return;
952 : }
953 :
954 : /* Compute the centroid of the current convex hull */
955 15049 : numHullVtx = 0;
956 15049 : set_zero( centroid, 3 );
957 206512 : for ( i = 0; i < vtxData->numVtx; i++ )
958 : {
959 191463 : if ( vtxInHull[i] )
960 : {
961 118305 : numHullVtx++;
962 118305 : centroid[0] += vtxData->vertexArray[i].pos[0];
963 118305 : centroid[1] += vtxData->vertexArray[i].pos[1];
964 118305 : centroid[2] += vtxData->vertexArray[i].pos[2];
965 : }
966 : }
967 15049 : numHullVtx = 1.0f / numHullVtx;
968 :
969 15049 : centroid[0] *= numHullVtx;
970 15049 : centroid[1] *= numHullVtx;
971 15049 : centroid[2] *= numHullVtx;
972 :
973 : /* Processing */
974 15049 : k = 0;
975 15049 : l = 0;
976 :
977 191463 : for ( i = 0; i < *szTri; ++i )
978 : {
979 176414 : tmpDist = vertex_distance( vtxData->vertexArray, triArray[i], vtxIdx );
980 :
981 176414 : if ( tmpDist > threshold )
982 : {
983 36686 : visible[k] = i;
984 36686 : ++k;
985 : }
986 : else
987 : {
988 139728 : mvs2s( triArray[i].LS, triArrayNew[l].LS, 3 );
989 139728 : ++l;
990 : }
991 : }
992 :
993 15049 : visible_edges( triArray, visible, k, numEdges, edges );
994 :
995 81833 : for ( i = 0; i < numEdges[0]; i += 2 )
996 : {
997 66784 : surface[0] = edges[i];
998 66784 : surface[1] = edges[i + 1];
999 66784 : surface[2] = vtxIdx;
1000 66784 : flip_plane( vtxData->vertexArray, surface, centroid );
1001 :
1002 66784 : mvs2s( surface, triArrayNew[l].LS, 3 );
1003 66784 : ++l;
1004 : }
1005 :
1006 : /* Outputs */
1007 221561 : for ( i = 0; i < l; i++ )
1008 : {
1009 206512 : mvs2s( triArrayNew[i].LS, triArray[i].LS, 3 );
1010 : }
1011 15049 : *szTri = l;
1012 :
1013 : /* Flag the vertex as added to the hull */
1014 15049 : vtxInHull[vtxIdx] = 1;
1015 15049 : return;
1016 : }
1017 :
1018 :
1019 : /*-------------------------------------------------------------------------*
1020 : * visible_edges()
1021 : *
1022 : *
1023 : *-------------------------------------------------------------------------*/
1024 :
1025 15049 : static void visible_edges(
1026 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
1027 : const int16_t *visible, /* i : Visible surface flag */
1028 : const int16_t numSurface, /* i : Number of surfaces */
1029 : int16_t *numEdges, /* i/o: Number of edges */
1030 : int16_t *edges /* i/o: Array of edges */
1031 : )
1032 : {
1033 : int16_t maxVertex;
1034 : int16_t i, j, k;
1035 : int16_t a, b;
1036 : int16_t tmpSurface[4];
1037 : int16_t counter[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF];
1038 : int16_t counterTranspose[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF];
1039 : float tmpMax[EFAP_MAX_SIZE_TMP_BUFF];
1040 :
1041 : /* Set counter and counterTranspose to 0 */
1042 466519 : for ( i = 0; i < EFAP_MAX_SIZE_TMP_BUFF; i++ )
1043 : {
1044 451470 : set_s( counter[i], 0, EFAP_MAX_SIZE_TMP_BUFF );
1045 451470 : set_s( counterTranspose[i], 0, EFAP_MAX_SIZE_TMP_BUFF );
1046 : }
1047 :
1048 : /* Finding the max vertex */
1049 51735 : for ( i = 0; i < numSurface; ++i )
1050 : {
1051 36686 : tmpMax[i] = (float) triArray[visible[i]].LS[0];
1052 110058 : for ( j = 1; j < 3; ++j )
1053 : {
1054 73372 : if ( tmpMax[i] < triArray[visible[i]].LS[j] )
1055 : {
1056 36894 : tmpMax[i] = (float) triArray[visible[i]].LS[j];
1057 : }
1058 : }
1059 : }
1060 15049 : maxVertex = (int16_t) tmpMax[maximum( tmpMax, numSurface, NULL )];
1061 :
1062 51735 : for ( i = 0; i < numSurface; ++i )
1063 : {
1064 36686 : tmpSurface[0] = triArray[visible[i]].LS[0];
1065 36686 : tmpSurface[1] = triArray[visible[i]].LS[1];
1066 36686 : tmpSurface[2] = triArray[visible[i]].LS[2];
1067 36686 : tmpSurface[3] = triArray[visible[i]].LS[0];
1068 :
1069 146744 : for ( j = 0; j < 3; ++j )
1070 : {
1071 110058 : a = tmpSurface[j];
1072 110058 : b = tmpSurface[j + 1];
1073 110058 : counter[a][b] = counter[a][b] + 1;
1074 110058 : counterTranspose[b][a] = counter[a][b];
1075 : }
1076 : }
1077 :
1078 192630 : for ( i = 0; i < maxVertex + 1; ++i )
1079 : {
1080 2405496 : for ( j = 0; j < maxVertex + 1; ++j )
1081 : {
1082 2227915 : counter[i][j] = counterTranspose[i][j] + counterTranspose[j][i];
1083 : }
1084 : }
1085 :
1086 : /* Finding the edges */
1087 15049 : k = 0;
1088 :
1089 177581 : for ( a = 0; a < maxVertex; ++a )
1090 : {
1091 1187699 : for ( b = a + 1; b < maxVertex + 1; ++b )
1092 : {
1093 1025167 : if ( counter[a][b] == 1 )
1094 : {
1095 66784 : edges[k] = a;
1096 66784 : edges[k + 1] = b;
1097 66784 : k += 2;
1098 : }
1099 : }
1100 : }
1101 :
1102 : /* Outputs */
1103 15049 : *numEdges = k;
1104 :
1105 15049 : return;
1106 : }
1107 :
1108 :
1109 : /*-------------------------------------------------------------------------*
1110 : * flip_plane()
1111 : *
1112 : *
1113 : *-------------------------------------------------------------------------*/
1114 :
1115 75176 : static void flip_plane(
1116 : const EFAP_VERTEX *vtxArray, /* i : Vertex array */
1117 : int16_t *surface, /* i/o: Surface/vertices to be flipped */
1118 : const float centroid[3] /* i : Centroid of convex hull from which to orient the planes outward */
1119 : )
1120 : {
1121 : int16_t tmp;
1122 : float dist;
1123 :
1124 75176 : dist = point_plane_distance(
1125 75176 : vtxArray[surface[0]].pos,
1126 75176 : vtxArray[surface[1]].pos,
1127 75176 : vtxArray[surface[2]].pos,
1128 : centroid );
1129 :
1130 75176 : if ( dist > 0 )
1131 : {
1132 : /*efap_flipLeftRight( surface, 3 );*/
1133 39827 : tmp = surface[0];
1134 39827 : surface[0] = surface[2];
1135 39827 : surface[2] = tmp;
1136 : }
1137 :
1138 75176 : return;
1139 : }
1140 :
1141 :
1142 : /*-------------------------------------------------------------------------*
1143 : * remap_ghosts()
1144 : *
1145 : *
1146 : *-------------------------------------------------------------------------*/
1147 :
1148 2098 : static void remap_ghosts(
1149 : EFAP_VERTEX *vtxArray, /* i/o: Vertex array */
1150 : EFAP_LS_TRIANGLE *triArray, /* i/o: Triangle array */
1151 : int16_t numSpk, /* i : Number of speakers */
1152 : int16_t *numVertex, /* i/o: Size of vertex array */
1153 : int16_t numTri, /* i : Size of triangle array */
1154 : float **downmixMatrixTranspose /* o : Transpose of downmix matrix */
1155 : )
1156 : {
1157 2098 : int16_t numGhst = 0;
1158 2098 : int16_t numVtx = *numVertex;
1159 : int16_t numTot;
1160 : int16_t g;
1161 : int16_t i, j;
1162 : int16_t tmpL;
1163 : float inv_tmpL;
1164 : int16_t posFound[2];
1165 : int16_t neighbours[EFAP_MAX_SIZE_TMP_BUFF];
1166 : float tmpVec[EFAP_MAX_SIZE_TMP_BUFF];
1167 : float tmpVec2[EFAP_MAX_SIZE_TMP_BUFF];
1168 : float tmpMat[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF];
1169 : float tmpNewMat[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF];
1170 : float tmpDist;
1171 2098 : const float thresh = 1e-4f;
1172 :
1173 2098 : set_f( tmpVec, 0.0f, EFAP_MAX_SIZE_TMP_BUFF );
1174 2098 : set_f( tmpVec2, 0.0f, EFAP_MAX_SIZE_TMP_BUFF );
1175 :
1176 : /* Finding unused ghosts (i.e. ghost speakers that aren't used for triangulation */
1177 6722 : for ( g = numVtx - 1; g > numSpk - 1; --g )
1178 : {
1179 : /* find(triangle_mat == ghost, 1, 'first') */
1180 4624 : if ( find_int_in_tri( triArray, g, numTri, posFound ) == 0 )
1181 : {
1182 0 : remove_vertex( vtxArray, g, numVtx );
1183 0 : --numVtx;
1184 0 : for ( i = 0; i < numTri; ++i )
1185 : {
1186 0 : for ( j = 0; j < 3; ++j )
1187 : {
1188 0 : if ( triArray[i].LS[j] > g )
1189 : {
1190 0 : triArray[i].LS[j] = g - 1;
1191 : }
1192 : }
1193 : }
1194 : }
1195 : else
1196 : {
1197 4624 : ++numGhst;
1198 : }
1199 : }
1200 :
1201 : /* Final number of LS (real + ghosts) */
1202 2098 : numTot = numSpk + numGhst;
1203 :
1204 : /* Initializing tmpMat as the identity matrix */
1205 25539 : for ( i = 0; i < numTot; ++i )
1206 : {
1207 23441 : set_f( tmpMat[i], 0.0f, numTot );
1208 23441 : set_f( tmpNewMat[i], 0.0f, numTot );
1209 :
1210 23441 : tmpMat[i][i] = 1;
1211 23441 : tmpNewMat[i][i] = 1;
1212 : }
1213 :
1214 : /* Generate initial sound energy distribution matrix */
1215 6722 : for ( i = numSpk; i < numTot; ++i )
1216 : {
1217 4624 : tmpL = get_neighbours( triArray, i, numTri, neighbours );
1218 :
1219 : /* Initializing the column to 0 */
1220 53646 : for ( j = 0; j < numTot; ++j )
1221 : {
1222 49022 : tmpMat[j][i] = 0;
1223 49022 : tmpNewMat[j][i] = 0;
1224 : }
1225 :
1226 : /* The neighbours are set to 1.0/tmpL */
1227 4624 : inv_tmpL = 1.f / tmpL;
1228 27128 : for ( j = 0; j < tmpL; ++j )
1229 : {
1230 22504 : tmpMat[neighbours[j]][i] = inv_tmpL;
1231 22504 : tmpNewMat[neighbours[j]][i] = inv_tmpL;
1232 : }
1233 : }
1234 :
1235 : /* Redistributing sound energy */
1236 25539 : for ( i = 0; i < numTot; ++i )
1237 : {
1238 308668 : for ( j = 0; j < numTot; ++j )
1239 : {
1240 285227 : tmpNewMat[i][j] = tmpMat[j][i];
1241 : }
1242 : }
1243 :
1244 6722 : for ( i = numSpk; i < numTot; ++i )
1245 : {
1246 4624 : mvr2r( tmpNewMat[i], tmpVec, numTot );
1247 :
1248 4624 : tmpDist = sum_f( &tmpVec[numSpk], numTot - numSpk );
1249 :
1250 25240 : while ( tmpDist > thresh )
1251 : {
1252 20616 : matrix_times_row( tmpMat, tmpVec, numTot, tmpVec2 );
1253 20616 : mvr2r( tmpVec2, tmpVec, numTot );
1254 20616 : set_zero( tmpVec2, numTot );
1255 20616 : tmpDist = sum_f( &tmpVec[numSpk], numTot - numSpk );
1256 : }
1257 4624 : mvr2r( tmpVec, tmpNewMat[i], numTot );
1258 : }
1259 :
1260 20915 : for ( i = 0; i < numSpk; ++i )
1261 : {
1262 : /* Applying a sqrt(2) coeff and obtaining the dmMatrix*/
1263 216856 : for ( j = 0; j < numSpk; ++j )
1264 : {
1265 198039 : downmixMatrixTranspose[j][i] = sqrtf( tmpNewMat[j][i] );
1266 : }
1267 : /* Downmix ghost loudspeakers according to dmxType */
1268 56983 : for ( ; j < numTot; ++j )
1269 : {
1270 38166 : switch ( vtxArray[j].dmxType )
1271 : {
1272 144 : case EFAP_DMX_NONE:
1273 144 : downmixMatrixTranspose[j][i] = 0.f;
1274 144 : break;
1275 3354 : case EFAP_DMX_AMPLITUDE:
1276 3354 : downmixMatrixTranspose[j][i] = tmpNewMat[j][i];
1277 3354 : break;
1278 34668 : case EFAP_DMX_INTENSITY:
1279 : default:
1280 34668 : downmixMatrixTranspose[j][i] = sqrtf( tmpNewMat[j][i] );
1281 34668 : break;
1282 : }
1283 : }
1284 : }
1285 :
1286 : /* Output */
1287 2098 : *numVertex = numTot;
1288 :
1289 2098 : return;
1290 : }
1291 :
1292 :
1293 : /*-------------------------------------------------------------------------*
1294 : * vertex_init()
1295 : *
1296 : * Initialize the vertex structures
1297 : *-------------------------------------------------------------------------*/
1298 :
1299 2098 : static void vertex_init(
1300 : const float *aziSpk, /* i : Azimuths of the LS setup */
1301 : const float *eleSpk, /* i : Elevations of the LS setup */
1302 : EFAP_VERTEX_DATA *efapVtxData /* i/o: Vertex data structure that will be updated */
1303 : )
1304 : {
1305 : int16_t i;
1306 :
1307 : /* Main Processing */
1308 20915 : for ( i = 0; i < efapVtxData->numVtx; i++ )
1309 : {
1310 18817 : add_vertex( efapVtxData->vertexArray, aziSpk[i], eleSpk[i], i, EFAP_DMX_INTENSITY );
1311 : }
1312 :
1313 2098 : return;
1314 : }
1315 :
1316 :
1317 : /*-------------------------------------------------------------------------*
1318 : * efap_panning()
1319 : *
1320 : * Compute the gain without applying the downmix Matrix and the norm of the array
1321 : *-------------------------------------------------------------------------*/
1322 :
1323 1873554 : static void efap_panning(
1324 : const float azi, /* i : Value of the azimuth */
1325 : const float ele, /* i : Value of the elevation */
1326 : const EFAP_POLYSET_DATA *polyData, /* i : Polygon data */
1327 : float *bufferL /* o : 1D array of length numSpk that will contain the tmp values */
1328 : )
1329 : {
1330 : int16_t i;
1331 : int16_t polyIdx;
1332 : int16_t numChan;
1333 : int16_t chan[EFAP_MAX_CHAN_NUM];
1334 : float aziPoly[EFAP_MAX_CHAN_NUM];
1335 : float elePoly[EFAP_MAX_CHAN_NUM];
1336 : float tmpBuff[EFAP_MAX_CHAN_NUM];
1337 : float normTmpBuff;
1338 : float P[2];
1339 :
1340 1873554 : P[0] = azi;
1341 1873554 : P[1] = ele;
1342 :
1343 : /* Finding in which polygon the point is */
1344 :
1345 1873554 : polyIdx = get_poly_num( P, polyData );
1346 :
1347 1873554 : assert( polyIdx != -1 && "EFAP: polygon not found!" );
1348 :
1349 : /* Extracting the chan, the azimuth and the ele of the considered poly */
1350 1873554 : numChan = polyData->polysetArray[polyIdx].numChan;
1351 :
1352 7574289 : for ( i = 0; i < numChan; ++i )
1353 : {
1354 5700735 : chan[i] = polyData->polysetArray[polyIdx].chan[i];
1355 5700735 : aziPoly[i] = polyData->polysetArray[polyIdx].polyAzi[i];
1356 :
1357 5700735 : if ( polyData->polysetArray[polyIdx].isNaN[i] == 1 )
1358 : {
1359 1601962 : aziPoly[i] = P[0];
1360 : }
1361 :
1362 5700735 : elePoly[i] = polyData->polysetArray[polyIdx].polyEle[i];
1363 : }
1364 :
1365 : /* Computing the gain for the polygon */
1366 1873554 : get_poly_gains( P[0], P[1], aziPoly, elePoly, numChan, tmpBuff );
1367 :
1368 : /* Computing the norm of the tmp buffer */
1369 1873554 : normTmpBuff = dotp( tmpBuff, tmpBuff, numChan );
1370 1873554 : normTmpBuff = sqrtf( normTmpBuff );
1371 :
1372 : /* Updating the buffer structure */
1373 1873554 : normTmpBuff = 1.f / normTmpBuff;
1374 7574289 : for ( i = 0; i < numChan; ++i )
1375 : {
1376 5700735 : bufferL[chan[i]] = tmpBuff[i] * normTmpBuff;
1377 : }
1378 :
1379 1873554 : return;
1380 : }
1381 :
1382 :
1383 : /*-------------------------------------------------------------------------*
1384 : * get_poly_gains()
1385 : *
1386 : * Compute the gain for a precise polygon
1387 : *-------------------------------------------------------------------------*/
1388 :
1389 1873554 : static void get_poly_gains(
1390 : const float azi, /* i : Value of the azimuth */
1391 : const float ele, /* i : Value of the elevation */
1392 : const float aziPoly[EFAP_MAX_CHAN_NUM], /* i : Azimuths of the considered polygons */
1393 : const float elePoly[EFAP_MAX_CHAN_NUM], /* i : Elevations of the considered polygons */
1394 : const int16_t numChan, /* i : Length of aziPoly & elePoly */
1395 : float *buffer /* o : 1D array of length numSpk that will contain the tmp values */
1396 : )
1397 : {
1398 : int16_t i, j;
1399 : int16_t idx1, idx2;
1400 : float P[2];
1401 : float A[2], B[2], C[2];
1402 : float P_minus_A[2];
1403 :
1404 1873554 : P[0] = azi;
1405 1873554 : P[1] = ele;
1406 :
1407 : /* Processing, we search for the triangle in which belong P, then we compute the gain */
1408 7574289 : for ( i = 1; i < numChan + 1; ++i )
1409 : {
1410 5700735 : A[0] = aziPoly[i - 1];
1411 5700735 : A[1] = elePoly[i - 1];
1412 :
1413 5700735 : v_sub( P, A, P_minus_A, 2 ); /* Precalculate value of (P-A) */
1414 :
1415 5860395 : for ( j = i; j < numChan - 2 + i; ++j )
1416 : {
1417 5860395 : idx1 = 1 + ( j % numChan );
1418 5860395 : idx2 = 1 + ( idx1 % numChan );
1419 :
1420 5860395 : B[0] = aziPoly[idx1 - 1];
1421 5860395 : B[1] = elePoly[idx1 - 1];
1422 :
1423 5860395 : C[0] = aziPoly[idx2 - 1];
1424 5860395 : C[1] = elePoly[idx2 - 1];
1425 :
1426 5860395 : if ( in_tri( A, B, C, P_minus_A ) )
1427 : {
1428 5700735 : buffer[i - 1] = get_tri_gain( A, B, C, P_minus_A );
1429 5700735 : break;
1430 : }
1431 : }
1432 : }
1433 :
1434 1873554 : return;
1435 : }
1436 :
1437 :
1438 : /*-------------------------------------------------------------------------*
1439 : * get_tri_gain()
1440 : *
1441 : * Compute the value of the gain for a given triangle
1442 : *-------------------------------------------------------------------------*/
1443 :
1444 5700735 : static float get_tri_gain(
1445 : const float A[2], /* i : Coordinate of one apex of the triangle */
1446 : const float B[2], /* i : Coordinate of one apex of the triangle */
1447 : const float C[2], /* i : Coordinate of one apex of the triangle */
1448 : const float P_minus_A[2] /* i : Value of (P - A) */
1449 : )
1450 : {
1451 : float N[2], tmpN[2];
1452 : float tmpSub1[2];
1453 : float tmpDot1, tmpDot2;
1454 : float gain;
1455 :
1456 : /* Processing */
1457 5700735 : tmpN[0] = B[1] - C[1];
1458 5700735 : tmpN[1] = C[0] - B[0];
1459 :
1460 5700735 : v_sub( B, A, tmpSub1, 2 );
1461 :
1462 5700735 : tmpDot1 = dotp( tmpN, tmpSub1, 2 );
1463 :
1464 5700735 : v_multc( tmpN, 1 / tmpDot1, N, 2 );
1465 :
1466 5700735 : tmpDot2 = dotp( P_minus_A, N, 2 );
1467 :
1468 5700735 : gain = 1 - tmpDot2;
1469 : /* Set gains <= -60dB to 0 to avoid problems in SVD */
1470 5700735 : if ( fabsf( gain ) < 1e-6 )
1471 : {
1472 1022874 : gain = 0.0f;
1473 : }
1474 5700735 : return gain;
1475 : }
1476 :
1477 :
1478 : /*-------------------------------------------------------------------------*
1479 : * add_vertex()
1480 : *
1481 : * Add a vertex to the vertex array
1482 : *-------------------------------------------------------------------------*/
1483 :
1484 23441 : static void add_vertex(
1485 : EFAP_VERTEX *vtxArray, /* i/o: Handle to the vertex array that will be updated */
1486 : const float azi, /* i : Azimuth of the vertex */
1487 : const float ele, /* i : Elevation of the vertex */
1488 : const int16_t pos, /* i : Index in the vtxArray where we want to add the vertex */
1489 : const EFAP_VTX_DMX_TYPE dmxType /* i : downmix type for the vertex */
1490 : )
1491 : {
1492 : float idxAziTmp, idxEleTmp;
1493 : float tmp;
1494 :
1495 23441 : assert( vtxArray != NULL && "EFAP: vtxArray == NULL" );
1496 :
1497 : /* Updating the vertex array */
1498 :
1499 23441 : tmp = efap_fmodf( 180.0f - azi, 360.0f );
1500 23441 : vtxArray[pos].azi = 180.0f - tmp;
1501 :
1502 23441 : tmp = ( ( 180.0f < ele ) ? 180.0f : ele );
1503 23441 : vtxArray[pos].ele = ( ( -180.0f > tmp ) ? -180.0f : tmp );
1504 :
1505 : /* Converting spherical coordinates to cartesians, assuming radius = 1 */
1506 23441 : sph2cart( vtxArray[pos].azi, vtxArray[pos].ele, &vtxArray[pos].pos[0] );
1507 :
1508 : /* Computing the index defined by idx = idxAziTmp + 181 * idxEleTmp */
1509 :
1510 : /* IdxAziTmp */
1511 23441 : tmp = fabsf( 90.0f - fabsf( vtxArray[pos].azi ) );
1512 23441 : idxAziTmp = (float) anint( tmp );
1513 :
1514 : /* IdxEleTmp */
1515 23441 : tmp = fabsf( vtxArray[pos].ele );
1516 23441 : idxEleTmp = (float) anint( tmp );
1517 23441 : idxEleTmp = 90.0f - idxEleTmp;
1518 :
1519 : /* Final Idx */
1520 23441 : vtxArray[pos].idx = (int16_t) idxAziTmp + 181 * (int16_t) idxEleTmp;
1521 :
1522 : /* Setting the nan flag to 0 */
1523 23441 : vtxArray[pos].isNaN = false;
1524 :
1525 : /* Set the default downmix type */
1526 23441 : vtxArray[pos].dmxType = dmxType;
1527 :
1528 23441 : return;
1529 : }
1530 :
1531 :
1532 : /*-------------------------------------------------------------------------*
1533 : * efap_sort_s()
1534 : *
1535 : * Sort an integer array
1536 : * (modified version of sort() to return an index array)
1537 : *-------------------------------------------------------------------------*/
1538 :
1539 6722 : static void efap_sort_s(
1540 : int16_t *x, /* i/o: Vector to be sorted */
1541 : int16_t *idx, /* o : Original index positions */
1542 : const int16_t len /* i : vector length */
1543 : )
1544 : {
1545 : int16_t i, j;
1546 : int16_t tempr, tempi;
1547 :
1548 97675 : for ( i = 0; i < len; i++ )
1549 : {
1550 90953 : idx[i] = i;
1551 : }
1552 :
1553 90953 : for ( i = len - 2; i >= 0; i-- )
1554 : {
1555 84231 : tempr = x[i];
1556 84231 : tempi = idx[i];
1557 366150 : for ( j = i + 1; ( j < len ) && ( tempr > x[j] ); j++ )
1558 : {
1559 281919 : x[j - 1] = x[j];
1560 281919 : idx[j - 1] = idx[j];
1561 : }
1562 84231 : x[j - 1] = tempr;
1563 84231 : idx[j - 1] = tempi;
1564 : }
1565 :
1566 6722 : return;
1567 : }
1568 :
1569 :
1570 : /*-------------------------------------------------------------------------*
1571 : * vertex_distance()
1572 : *
1573 : * Compute the signed distance between a vertex and a hull surface
1574 : *-------------------------------------------------------------------------*/
1575 :
1576 176414 : static float vertex_distance(
1577 : const EFAP_VERTEX *vtxArray, /* i : The considered vertex */
1578 : const EFAP_LS_TRIANGLE tri, /* i : The considered triangle */
1579 : const int16_t vtxIdx /* i : Index of the considered vertex */
1580 : )
1581 : {
1582 : float A[3], B[3], C[3], P[3];
1583 : int16_t i;
1584 :
1585 : /* Assigning the coordinates to the vector */
1586 705656 : for ( i = 0; i < 3; ++i )
1587 : {
1588 529242 : A[i] = vtxArray[tri.LS[0]].pos[i];
1589 529242 : B[i] = vtxArray[tri.LS[1]].pos[i];
1590 529242 : C[i] = vtxArray[tri.LS[2]].pos[i];
1591 :
1592 529242 : P[i] = vtxArray[vtxIdx].pos[i];
1593 : }
1594 :
1595 176414 : return point_plane_distance( A, B, C, P );
1596 : }
1597 :
1598 : /*-------------------------------------------------------------------------*
1599 : * point_poly_distance()
1600 : *
1601 : * Compute the signed distance between a point and polygon
1602 : *-------------------------------------------------------------------------*/
1603 :
1604 4736630 : static float point_poly_distance(
1605 : const EFAP_POLYSET poly, /* i : The polygon which forms a plane */
1606 : const float X[3] /* i : Cartesian coordinates of the point of interest */
1607 : )
1608 : {
1609 : float P1[3], P2[3], P3[3];
1610 :
1611 4736630 : sph2cart( poly.polyAzi[0], poly.polyEle[0], &P1[0] );
1612 4736630 : sph2cart( poly.polyAzi[1], poly.polyEle[1], &P2[0] );
1613 4736630 : sph2cart( poly.polyAzi[2], poly.polyEle[2], &P3[0] );
1614 :
1615 4736630 : return point_plane_distance( P1, P2, P3, X );
1616 : }
1617 :
1618 : /*-------------------------------------------------------------------------*
1619 : * point_plane_distance()
1620 : *
1621 : * Compute the signed distance between a point and a given plane
1622 : *-------------------------------------------------------------------------*/
1623 :
1624 5464910 : static float point_plane_distance(
1625 : const float P1[3], /* i : First point of the triangle that defines the planes */
1626 : const float P2[3], /* i : Second point of the triangle */
1627 : const float P3[3], /* i : Third point of the triangle */
1628 : const float X[3] /* i : The point of interest */
1629 : )
1630 : {
1631 : float tmpCross1[3], tmpCross2[3];
1632 : float resultCross[3];
1633 : float tmpDot1[3], tmpDot2[3];
1634 : float tmpNorm;
1635 : float dist;
1636 :
1637 : /* Check if the point already matches a triangle vertex */
1638 5464910 : if ( ( X[0] == P1[0] && X[1] == P1[1] && X[2] == P1[2] ) ||
1639 5422388 : ( X[0] == P2[0] && X[1] == P2[1] && X[2] == P2[2] ) ||
1640 5362424 : ( X[0] == P3[0] && X[1] == P3[1] && X[2] == P3[2] ) )
1641 : {
1642 147215 : return 0.0f;
1643 : }
1644 :
1645 : /* Cross Product */
1646 5317695 : v_sub( P1, P2, tmpCross1, 3 );
1647 5317695 : v_sub( P1, P3, tmpCross2, 3 );
1648 :
1649 : /* resultCross = cross(P1-P2,P1-P3) */
1650 5317695 : efap_crossp( tmpCross1, tmpCross2, resultCross );
1651 :
1652 : /* Dot Product */
1653 5317695 : tmpNorm = dotp( resultCross, resultCross, 3 );
1654 5317695 : tmpNorm = inv_sqrt( tmpNorm );
1655 5317695 : v_sub( X, P1, tmpDot1, 3 );
1656 5317695 : v_multc( resultCross, tmpNorm, tmpDot2, 3 );
1657 5317695 : dist = dotp( tmpDot1, tmpDot2, 3 );
1658 :
1659 5317695 : return dist;
1660 : }
1661 :
1662 :
1663 : /*-------------------------------------------------------------------------*
1664 : * efap_crossp()
1665 : *
1666 : * Compute the cross product between column vectors of float of size 3x1
1667 : *-------------------------------------------------------------------------*/
1668 :
1669 5319793 : static void efap_crossp(
1670 : const float *v1, /* i : First float vector */
1671 : const float *v2, /* i : Second float vector */
1672 : float *v /* o : Output vector */
1673 : )
1674 : {
1675 5319793 : v[0] = v1[1] * v2[2] - v1[2] * v2[1];
1676 5319793 : v[1] = v1[2] * v2[0] - v1[0] * v2[2];
1677 5319793 : v[2] = v1[0] * v2[1] - v1[1] * v2[0];
1678 :
1679 5319793 : return;
1680 : }
1681 :
1682 :
1683 : /*-------------------------------------------------------------------------*
1684 : * find_int_in_tri()
1685 : *
1686 : * Find an integer in triangle array of integers
1687 : *-------------------------------------------------------------------------*/
1688 :
1689 31752 : static int16_t find_int_in_tri(
1690 : const EFAP_LS_TRIANGLE *tri, /* i : Triangle array */
1691 : const int16_t n, /* i : The integer to find */
1692 : const int16_t r, /* i : Number of rows */
1693 : int16_t *pos /* o : Position of the integer */
1694 : )
1695 : {
1696 : int16_t i, j;
1697 :
1698 : /* Find the first element equal to n */
1699 293987 : for ( i = 0; i < r; ++i )
1700 : {
1701 1104646 : for ( j = 0; j < 3; ++j )
1702 : {
1703 842411 : if ( tri[i].LS[j] == n )
1704 : {
1705 27128 : pos[0] = i;
1706 27128 : pos[1] = j;
1707 27128 : return 1;
1708 : }
1709 : }
1710 : }
1711 :
1712 4624 : return 0;
1713 : }
1714 :
1715 :
1716 : /*-------------------------------------------------------------------------*
1717 : * remove_vertex()
1718 : *
1719 : * Remove a vertex from a vertex structure
1720 : *-------------------------------------------------------------------------*/
1721 :
1722 0 : static void remove_vertex(
1723 : EFAP_VERTEX *vtxArray, /* i : Vertex array */
1724 : const int16_t idx, /* i : Index of the vertex to remove */
1725 : const int16_t L /* i : Length of the Vertex array */
1726 : )
1727 : {
1728 : int16_t i;
1729 :
1730 0 : assert( idx < L && "EFAP: index out of bounds" );
1731 :
1732 : /* Shift all vertex of one position, so vtxArray[i] will be vtxArray[i+1] and so on */
1733 0 : for ( i = idx; i < L - 1; ++i )
1734 : {
1735 0 : add_vertex( vtxArray, vtxArray[i + 1].azi, vtxArray[i + 1].ele, i, EFAP_DMX_INTENSITY );
1736 : }
1737 :
1738 : /* The last vertex is set to 0 */
1739 0 : add_vertex( vtxArray, 0, 0, L - 1, EFAP_DMX_INTENSITY );
1740 :
1741 0 : return;
1742 : }
1743 :
1744 :
1745 : /*-------------------------------------------------------------------------*
1746 : * get_neighbours()
1747 : *
1748 : * Returns the neighbouring triangles of a vertex
1749 : *-------------------------------------------------------------------------*/
1750 :
1751 4624 : static int16_t get_neighbours(
1752 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
1753 : const int16_t vtxIdx, /* i : Index of the vertex */
1754 : const int16_t numTri, /* i : Number of Triangles */
1755 : int16_t *neighbours /* o : Output vector */
1756 : )
1757 : {
1758 : int16_t i, j, k;
1759 : int16_t tmpPos[2];
1760 : int16_t tmpNeighbours[EFAP_MAX_SIZE_TMP_BUFF];
1761 : int16_t dummy[EFAP_MAX_SIZE_TMP_BUFF];
1762 : EFAP_LS_TRIANGLE tmpTriArray[EFAP_MAX_POLY_SET];
1763 :
1764 : /* Processing */
1765 84172 : for ( i = 0; i < numTri; ++i )
1766 : {
1767 79548 : mvs2s( triArray[i].LS, tmpTriArray[i].LS, 3 );
1768 : }
1769 :
1770 4624 : k = 0;
1771 : while ( 1 )
1772 : {
1773 27128 : if ( find_int_in_tri( tmpTriArray, vtxIdx, numTri, tmpPos ) == 0 )
1774 : {
1775 4624 : break;
1776 : }
1777 : else
1778 : {
1779 22504 : tmpNeighbours[k] = tmpTriArray[tmpPos[0]].LS[0];
1780 22504 : tmpNeighbours[k + 1] = tmpTriArray[tmpPos[0]].LS[1];
1781 22504 : tmpNeighbours[k + 2] = tmpTriArray[tmpPos[0]].LS[2];
1782 22504 : k += 3;
1783 22504 : tmpTriArray[tmpPos[0]].LS[tmpPos[1]] = -1;
1784 : }
1785 :
1786 22504 : if ( k > 3 * numTri )
1787 : {
1788 0 : break;
1789 : }
1790 : }
1791 :
1792 : /* Sorting the neighbours vector */
1793 4624 : efap_sort_s( tmpNeighbours, dummy, k );
1794 :
1795 : /* Creating the output vector, by eliminating redundancies and also deleting the indice == vtxIdx*/
1796 4624 : neighbours[0] = tmpNeighbours[0];
1797 4624 : j = 1;
1798 :
1799 67512 : for ( i = 1; i < k; ++i )
1800 : {
1801 62888 : if ( ( tmpNeighbours[i] != tmpNeighbours[i - 1] ) &&
1802 22504 : ( tmpNeighbours[i] != vtxIdx ) )
1803 : {
1804 17880 : neighbours[j] = tmpNeighbours[i];
1805 17880 : ++j;
1806 : }
1807 : }
1808 :
1809 : /* Output, length of neighbours */
1810 4624 : return j;
1811 : }
1812 :
1813 :
1814 : /*-------------------------------------------------------------------------*
1815 : * matrix_times_row()
1816 : *
1817 : * Computes the product of a matrix and a row vector
1818 : *-------------------------------------------------------------------------*/
1819 :
1820 20616 : static void matrix_times_row(
1821 : float mat[EFAP_MAX_SIZE_TMP_BUFF][EFAP_MAX_SIZE_TMP_BUFF], /* i : The input matrix */
1822 : const float *vec, /* i : The input row vector */
1823 : const int16_t L, /* i : Row length */
1824 : float *out /* o : Output vector */
1825 : )
1826 : {
1827 : int16_t i, j;
1828 :
1829 123696 : for ( i = 0; i < L; ++i )
1830 : {
1831 618480 : for ( j = 0; j < L; ++j )
1832 : {
1833 515400 : out[i] += mat[i][j] * vec[j];
1834 : }
1835 : }
1836 :
1837 20616 : return;
1838 : }
1839 :
1840 :
1841 : /*-------------------------------------------------------------------------*
1842 : * tri_to_poly()
1843 : *
1844 : * Combines triangles of a surface in order to create polygons
1845 : *-------------------------------------------------------------------------*/
1846 :
1847 2098 : static void tri_to_poly(
1848 : const EFAP_VERTEX *vtxArray, /* i : Vertex array */
1849 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
1850 : const int16_t numVtx, /* i : Number of vertices */
1851 : const int16_t numTri, /* i : Number of triangles */
1852 : int16_t sortedChan[EFAP_MAX_POLY_SET][EFAP_MAX_CHAN_NUM], /* o : The matrix that will contain the sorted channels */
1853 : int16_t *outLengthPS, /* o : The length of the sorted channels */
1854 : int16_t outLengthSorted[EFAP_MAX_POLY_SET] /* o : The number of channels for each poly (i.e. outLengthSorted[i] = length(sortedChan[i]) */
1855 : )
1856 : {
1857 : int16_t i, j;
1858 : int16_t lenPoly;
1859 : int16_t lenPolySet;
1860 : int16_t found;
1861 : int16_t replaceIdx;
1862 :
1863 : int16_t poly[EFAP_MAX_CHAN_NUM];
1864 :
1865 : int16_t sortedLengths[EFAP_MAX_POLY_SET];
1866 : int16_t sortedTri[EFAP_MAX_POLY_SET];
1867 :
1868 : float dist;
1869 :
1870 2098 : lenPolySet = 0;
1871 : /* Sorting the polygons */
1872 40588 : for ( i = 0; i < numTri; ++i )
1873 : {
1874 : /* search for coplanar vertices and add them to the polygon */
1875 38490 : lenPoly = 0;
1876 515180 : for ( j = 0; j < numVtx; ++j )
1877 : {
1878 953380 : dist = fabsf( point_plane_distance(
1879 476690 : vtxArray[triArray[i].LS[0]].pos,
1880 476690 : vtxArray[triArray[i].LS[1]].pos,
1881 476690 : vtxArray[triArray[i].LS[2]].pos,
1882 476690 : vtxArray[j].pos ) );
1883 :
1884 476690 : if ( dist < 1e-3f )
1885 : {
1886 119012 : assert( lenPoly < EFAP_MAX_CHAN_NUM && "EFAP: exceeded max polygon vertices!" );
1887 119012 : poly[lenPoly] = j;
1888 119012 : ++lenPoly;
1889 : }
1890 : }
1891 :
1892 : /* search existing polygons to determine whether the new one already exists/is a subset or is a superset */
1893 38490 : found = 0;
1894 38490 : replaceIdx = -1;
1895 410098 : for ( j = 0; j < lenPolySet; ++j )
1896 : {
1897 373379 : found = compare_poly( sortedChan[j], sortedLengths[j], poly, lenPoly );
1898 :
1899 373379 : if ( found > 0 )
1900 : {
1901 1771 : break;
1902 : }
1903 371608 : else if ( found < 0 )
1904 : {
1905 0 : replaceIdx = j;
1906 : }
1907 : }
1908 :
1909 38490 : if ( found == 0 )
1910 : {
1911 : /* append new poly */
1912 36719 : mvs2s( poly, sortedChan[lenPolySet], lenPoly );
1913 36719 : sortedTri[lenPolySet] = i;
1914 36719 : sortedLengths[lenPolySet] = lenPoly;
1915 36719 : ++lenPolySet;
1916 : }
1917 1771 : else if ( found == -1 )
1918 : {
1919 : /* replace with superset */
1920 0 : mvs2s( poly, sortedChan[replaceIdx], lenPoly );
1921 0 : sortedTri[replaceIdx] = i;
1922 0 : sortedLengths[replaceIdx] = lenPoly;
1923 : }
1924 : }
1925 :
1926 : /* Sorting the vertex */
1927 38817 : for ( i = 0; i < lenPolySet; ++i )
1928 : {
1929 36719 : sort_channels_vertex( vtxArray, triArray, sortedChan[i], sortedLengths[i], sortedTri[i] );
1930 : }
1931 :
1932 : /* Output */
1933 2098 : *outLengthPS = lenPolySet;
1934 2098 : mvs2s( sortedLengths, outLengthSorted, EFAP_MAX_POLY_SET );
1935 :
1936 2098 : return;
1937 : }
1938 :
1939 :
1940 : /*-------------------------------------------------------------------------*
1941 : * compare_poly()
1942 : *
1943 : * Compares a newly created polygon with an existing one
1944 : *-------------------------------------------------------------------------*/
1945 :
1946 373379 : static int16_t compare_poly(
1947 : int16_t *old, /* i : Existing polygon */
1948 : int16_t lenOld, /* i : Length of existing polygon */
1949 : int16_t *new, /* i : New polygon */
1950 : int16_t lenNew /* i : Length of new polygon */
1951 : )
1952 : {
1953 : int16_t i, j;
1954 : int16_t count;
1955 :
1956 373379 : count = 0;
1957 :
1958 1502373 : for ( i = 0; i < lenOld; ++i )
1959 : {
1960 3945000 : for ( j = count; j < lenNew; ++j )
1961 : {
1962 3060301 : if ( old[i] == new[j] )
1963 : {
1964 244295 : ++count;
1965 244295 : break;
1966 : }
1967 : }
1968 : }
1969 :
1970 373379 : if ( count == lenOld && lenOld < lenNew )
1971 : {
1972 : /* new polygon is a superset */
1973 0 : return -1;
1974 : }
1975 373379 : else if ( count == lenNew && lenOld >= lenNew )
1976 : {
1977 : /* found as subset or identical */
1978 1771 : return 1;
1979 : }
1980 : else
1981 : {
1982 : /* not found */
1983 371608 : return 0;
1984 : }
1985 : }
1986 :
1987 :
1988 : /*-------------------------------------------------------------------------*
1989 : * sort_channels_vertex()
1990 : *
1991 : * Sort the channels of a polygon set according to the vertex azimuth
1992 : *-------------------------------------------------------------------------*/
1993 :
1994 36719 : static void sort_channels_vertex(
1995 : const EFAP_VERTEX *vtxArray, /* i : Vertex array */
1996 : const EFAP_LS_TRIANGLE *triArray, /* i : Triangle array */
1997 : int16_t channels[EFAP_MAX_CHAN_NUM], /* o : Channels array to be modified */
1998 : const int16_t lengthChannels, /* i : Length of the channels array */
1999 : int16_t idxTri /* i : Index of the considered triangle */
2000 : )
2001 : {
2002 : int16_t i, j;
2003 :
2004 : float P1[3], P2[3], P3[3];
2005 : float tmpU[3];
2006 : float U[3], V[3];
2007 : float tmpV1[3], tmpV2[3], tmpV3[3];
2008 : float normU, normV;
2009 : float MC[3];
2010 : float tmpP[3], P[3];
2011 :
2012 : float x, y;
2013 :
2014 : float azi[EFAP_MAX_CHAN_NUM];
2015 : int16_t order[EFAP_MAX_CHAN_NUM];
2016 :
2017 : int16_t newChannels[EFAP_MAX_CHAN_NUM];
2018 :
2019 :
2020 : /* Initializing coordinates with the vertices of the considered triangle */
2021 146876 : for ( i = 0; i < 3; ++i )
2022 : {
2023 110157 : P1[i] = vtxArray[triArray[idxTri].LS[0]].pos[i];
2024 110157 : P2[i] = vtxArray[triArray[idxTri].LS[1]].pos[i];
2025 110157 : P3[i] = vtxArray[triArray[idxTri].LS[2]].pos[i];
2026 : }
2027 :
2028 : /* First Base Vector */
2029 36719 : v_sub( P2, P1, tmpU, 3 );
2030 36719 : normU = sqrtf( dotp( tmpU, tmpU, 3 ) );
2031 36719 : v_multc( tmpU, 1 / normU, U, 3 );
2032 :
2033 : /* Second Base Vector */
2034 36719 : v_sub( P3, P2, tmpV1, 3 );
2035 36719 : v_multc( U, dotp( U, tmpV1, 3 ), tmpV2, 3 );
2036 :
2037 36719 : v_sub( tmpV1, tmpV2, tmpV3, 3 );
2038 36719 : normV = sqrtf( dotp( tmpV3, tmpV3, 3 ) );
2039 :
2040 36719 : v_multc( tmpV3, 1 / normV, V, 3 );
2041 :
2042 : /* Center of the first Triangle */
2043 146876 : for ( i = 0; i < 3; ++i )
2044 : {
2045 110157 : MC[i] = ( P1[i] + P2[i] + P3[i] ) / 3;
2046 : }
2047 :
2048 : /* Sort Vertices */
2049 148647 : for ( i = 0; i < lengthChannels; ++i )
2050 : {
2051 447712 : for ( j = 0; j < 3; ++j )
2052 : {
2053 335784 : tmpP[j] = vtxArray[channels[i]].pos[j];
2054 : }
2055 :
2056 111928 : v_sub( tmpP, MC, P, 3 );
2057 :
2058 111928 : x = dotp( P, U, 3 );
2059 111928 : y = dotp( P, V, 3 );
2060 :
2061 111928 : azi[i] = atan2f( y, x );
2062 : }
2063 :
2064 : /* Sorting the azi vec */
2065 36719 : v_sort_ind( azi, order, lengthChannels );
2066 :
2067 : /* Updating the channel array */
2068 148647 : for ( i = 0; i < lengthChannels; ++i )
2069 : {
2070 111928 : newChannels[i] = channels[order[i]];
2071 : }
2072 :
2073 : /* return Success */
2074 36719 : mvs2s( newChannels, channels, lengthChannels );
2075 :
2076 36719 : return;
2077 : }
2078 :
2079 :
2080 : /*-------------------------------------------------------------------------*
2081 : * efap_fmodf()
2082 : *
2083 : * Modulus operation that will handle negative values the same way as matlab
2084 : *-------------------------------------------------------------------------*/
2085 :
2086 42553 : static float efap_fmodf(
2087 : const float x, /* i : Dividend */
2088 : const float y /* i : Divisor */
2089 : )
2090 : {
2091 42553 : float result = fmodf( x, y );
2092 42553 : return result >= 0 ? result : result + y;
2093 : }
2094 :
2095 :
2096 : /*-------------------------------------------------------------------------*
2097 : * get_poly_num()
2098 : *
2099 : * Returns the index of the polygon in which the coordinate is
2100 : *-------------------------------------------------------------------------*/
2101 :
2102 1873554 : static int16_t get_poly_num(
2103 : const float P[2], /* i : Azimuth and elevation of the point */
2104 : const EFAP_POLYSET_DATA *polyData /* i : Polyset struct */
2105 : )
2106 : {
2107 : int16_t i;
2108 : int16_t num_poly, found_poly;
2109 : int16_t poly_tmp[EFAP_MAX_CHAN_NUM];
2110 : float poly_dist[EFAP_MAX_CHAN_NUM];
2111 :
2112 : float dist_tmp;
2113 : float pos[3];
2114 :
2115 1873554 : num_poly = 0;
2116 :
2117 1873554 : sph2cart( P[0], P[1], &pos[0] );
2118 :
2119 : /* Filter the polygon list with a fast 2d check */
2120 39642396 : for ( i = 0; i < polyData->numPoly; ++i )
2121 : {
2122 37800639 : if ( in_poly( P, polyData->polysetArray[i] ) )
2123 : {
2124 : /* select only polygons which are visible from the point */
2125 4736630 : dist_tmp = point_poly_distance( polyData->polysetArray[i], pos );
2126 4736630 : if ( dist_tmp == 0 )
2127 : {
2128 31797 : return i;
2129 : }
2130 4704833 : else if ( dist_tmp > 0 )
2131 : {
2132 2864940 : poly_tmp[num_poly] = i;
2133 2864940 : poly_dist[num_poly] = dist_tmp;
2134 2864940 : num_poly++;
2135 : }
2136 : }
2137 : }
2138 1841757 : if ( num_poly == 0 )
2139 : {
2140 0 : return -1;
2141 : }
2142 :
2143 : /* select the polygon with the smallest distance */
2144 1841757 : found_poly = poly_tmp[0];
2145 1841757 : dist_tmp = poly_dist[0];
2146 2864940 : for ( i = 1; i < num_poly; i++ )
2147 : {
2148 1023183 : if ( poly_dist[i] < dist_tmp )
2149 : {
2150 484130 : found_poly = poly_tmp[i];
2151 484130 : dist_tmp = poly_dist[i];
2152 : }
2153 : }
2154 :
2155 1841757 : return found_poly;
2156 : }
2157 :
2158 :
2159 : /*-------------------------------------------------------------------------*
2160 : * in_poly()
2161 : *
2162 : * Determines if a given point is within a polygon or not
2163 : *-------------------------------------------------------------------------*/
2164 :
2165 37800639 : static int16_t in_poly(
2166 : const float P[2], /* i : Azimuth and elevation of the point */
2167 : const EFAP_POLYSET poly /* i : Polyset struct */
2168 : )
2169 : {
2170 : int16_t n;
2171 37800639 : int16_t numVertices = poly.numChan;
2172 : float A[2];
2173 : float B[2];
2174 : float C[2];
2175 : float P_minus_A[2];
2176 :
2177 : /* Safety check */
2178 :
2179 37800639 : if ( numVertices < 3 )
2180 : {
2181 : #ifdef DEBUGGING
2182 : IVAS_ERROR( IVAS_ERR_INTERNAL, "Less than 3 channels in the polygon" );
2183 : #endif
2184 0 : return 0;
2185 : }
2186 :
2187 : /* See if the point is in one of the triangles available in the polygon */
2188 :
2189 37800639 : if ( poly.isNaN[0] )
2190 : {
2191 2716600 : A[0] = P[0];
2192 : }
2193 : else
2194 : {
2195 35084039 : A[0] = poly.polyAzi[0];
2196 : }
2197 37800639 : A[1] = poly.polyEle[0];
2198 :
2199 37800639 : v_sub( P, A, P_minus_A, 2 ); /* Precalculate value of (P-A) */
2200 :
2201 73607942 : for ( n = 1; n < numVertices - 1; ++n )
2202 : {
2203 40543933 : if ( poly.isNaN[n] )
2204 : {
2205 21082046 : B[0] = P[0];
2206 : }
2207 : else
2208 : {
2209 19461887 : B[0] = poly.polyAzi[n];
2210 : }
2211 40543933 : B[1] = poly.polyEle[n];
2212 :
2213 40543933 : if ( poly.isNaN[n + 1] )
2214 : {
2215 2710088 : C[0] = P[0];
2216 : }
2217 : else
2218 : {
2219 37833845 : C[0] = poly.polyAzi[n + 1];
2220 : }
2221 40543933 : C[1] = poly.polyEle[n + 1];
2222 :
2223 40543933 : if ( in_tri( A, B, C, P_minus_A ) )
2224 : {
2225 4736630 : return 1;
2226 : }
2227 : }
2228 :
2229 33064009 : return 0;
2230 : }
2231 :
2232 :
2233 : /*-------------------------------------------------------------------------*
2234 : * in_tri()
2235 : *
2236 : * Determines if a given point is within a triangle or not
2237 : *-------------------------------------------------------------------------*/
2238 :
2239 46404328 : static int16_t in_tri(
2240 : float A[2], /* i : Coordinate of one apex of the triangle */
2241 : float B[2], /* i : Coordinate of one apex of the triangle */
2242 : float C[2], /* i : Coordinate of one apex of the triangle */
2243 : float P_minus_A[2] /* i : Value of (P - A) */
2244 : )
2245 : {
2246 : float tmpDot1[2], tmpDot2[2];
2247 : float matInv[2][2];
2248 : float invFactor;
2249 : float S[2];
2250 46404328 : float thresh = 1e-6f;
2251 :
2252 : /*
2253 : Not a Valid Triangle : Colinear edges
2254 : In the matlab implementation, the rcond() function is used
2255 : Since it's very complex to implement this in C
2256 : I'll just compute the determinant and if it's equal to 0, that means the two vectors are colinear
2257 : */
2258 :
2259 46404328 : v_sub( B, A, tmpDot1, 2 );
2260 46404328 : v_sub( C, A, tmpDot2, 2 );
2261 :
2262 : /* Verification of the non-colinearity */
2263 46404328 : invFactor = tmpDot1[0] * tmpDot2[1] - tmpDot1[1] * tmpDot2[0];
2264 :
2265 46404328 : if ( fabsf( invFactor ) < thresh )
2266 : {
2267 1224 : return 0;
2268 : }
2269 :
2270 46403104 : invFactor = 1.f / invFactor;
2271 46403104 : matInv[0][0] = tmpDot2[1] * invFactor;
2272 46403104 : matInv[0][1] = -tmpDot2[0] * invFactor;
2273 46403104 : matInv[1][0] = -tmpDot1[1] * invFactor;
2274 46403104 : matInv[1][1] = tmpDot1[0] * invFactor;
2275 :
2276 : /* Computing S = matInv*(P-A) */
2277 46403104 : S[0] = ( matInv[0][0] * P_minus_A[0] ) + ( matInv[0][1] * P_minus_A[1] );
2278 46403104 : S[1] = ( matInv[1][0] * P_minus_A[0] ) + ( matInv[1][1] * P_minus_A[1] );
2279 :
2280 : /* Checking if we are in the triangle; For the theory, check Christian Borss article, section 3.2 */
2281 46403104 : if ( S[0] < -thresh || S[1] < -thresh || S[0] + S[1] > 1 + thresh )
2282 : {
2283 35965739 : return 0;
2284 : }
2285 : else
2286 : {
2287 10437365 : return 1;
2288 : }
2289 : }
2290 :
2291 : /*-------------------------------------------------------------------------*
2292 : * sph2cart()
2293 : *
2294 : * Converts a vertex position to cartesian coordinates
2295 : *-------------------------------------------------------------------------*/
2296 :
2297 16106885 : static void sph2cart(
2298 : const float azi, /* i : Azimuth in degrees */
2299 : const float ele, /* i : Elevation in degrees */
2300 : float *pos /* o : Cartesian coordinates vector (x, y, z) */
2301 : )
2302 : {
2303 16106885 : pos[0] = cosf( azi * PI_OVER_180 ) * cosf( ele * PI_OVER_180 );
2304 16106885 : pos[1] = sinf( azi * PI_OVER_180 ) * cosf( ele * PI_OVER_180 );
2305 16106885 : pos[2] = sinf( ele * PI_OVER_180 );
2306 :
2307 16106885 : return;
2308 : }
|
