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 <assert.h>
35 : #include "options.h"
36 : #include "prot_fx.h"
37 : #include <math.h>
38 : #include "ivas_prot_rend_fx.h"
39 : #include "ivas_rom_rend.h"
40 : #include "ivas_cnst.h"
41 : #include "wmc_auto.h"
42 :
43 : /*---------------------------------------------------------------------*
44 : * Local function prototypes
45 : *---------------------------------------------------------------------*/
46 :
47 : static Word32 round_fixed( Word32 n, Word16 q );
48 : static void getPeriodicBSplineSampVec_fx( Word32 *BfVec_fx, Word16 *AzIdx, const Word16 NumBFs, const Word32 t_fx, Word16 *num_az_idx, const Word32 knot_interval_fx, const Word32 azimKSeq_0_fx, const Word16 azimSegSamples, const Word32 *azimBsShape_fx, const Word16 subSampFactor );
49 : static void getStandardBSplineSampVec_fx( Word32 *BfVec_fx, Word16 *NzIdx, Word16 *num_idx, const Word16 NumBFs, const Word32 t_fx, const Word32 *KSeq_fx, const Word16 SegSamples, const Word16 *BsLen, const Word16 *BsStart, const Word32 *BsShape_fx );
50 : static void GenerateFilter_fx( const Word32 elev, Word32 azim, ModelParams_t *model, ModelEval_t *modelEval );
51 : static void GenerateITD_fx( const Word32 elev, Word32 azim, ModelParamsITD_t *model, ModelEval_t *modelEval );
52 : static void SkipSmallest_ValueIndex_fx( Word16 *use_inds, const ValueIndex_t *VI, const Word16 N, const Word16 n_smallest );
53 :
54 : /*-------------------------------------------------------------------*
55 : * TDREND_REND_RenderSourceHRFilt()
56 : *
57 : * Renders each object using the HR filters
58 : --------------------------------------------------------------------*/
59 :
60 1388199 : ivas_error TDREND_REND_RenderSourceHRFilt_fx(
61 : TDREND_SRC_t *Src_p, /* i/o: The source to be rendered */
62 : Word32 *hrf_left_delta_fx, /* i/o: Left filter interpolation delta */
63 : Word16 *hrf_left_delta_e, /* i/o: Left filter interpolation delta exp */
64 : Word32 *hrf_right_delta_fx, /* i/o: Right filter interpolation delta */
65 : Word16 *hrf_right_delta_e, /* i/o: Right filter interpolation delta exp */
66 : const Word16 intp_count, /* i : Interpolation count */
67 : Word32 output_buf_fx[][L_SPATIAL_SUBFR_48k], /* o : Output buffer same Q as Src_p->InputFrame_p_fx Q11 */
68 : const Word16 subframe_length /* i : Subframe length in use */
69 : )
70 : {
71 : Word32 LeftOutputFrame_fx[L_SPATIAL_SUBFR_48k]; // will have same Q as Src_p->InputFrame_p_fx
72 : Word32 RightOutputFrame_fx[L_SPATIAL_SUBFR_48k]; // will have same Q as Src_p->InputFrame_p_fx
73 : Word16 left_filter_e;
74 : Word16 right_filter_e;
75 :
76 1388199 : TDREND_Apply_ITD_fx( Src_p->InputFrame_p_fx, LeftOutputFrame_fx, RightOutputFrame_fx,
77 1388199 : &Src_p->previtd, Src_p->itd, Src_p->mem_itd_fx, subframe_length );
78 :
79 1388199 : left_filter_e = s_max( Src_p->hrf_left_prev_e, *hrf_left_delta_e );
80 1388199 : right_filter_e = s_max( Src_p->hrf_right_prev_e, *hrf_right_delta_e );
81 :
82 : /* Scaling prev and delta HRF filter values to common Q-factor */
83 1388199 : Scale_sig32( Src_p->hrf_left_prev_fx, Src_p->filterlength, sub( Src_p->hrf_left_prev_e, left_filter_e ) );
84 1388199 : Scale_sig32( hrf_left_delta_fx, Src_p->filterlength, sub( *hrf_left_delta_e, left_filter_e ) );
85 1388199 : Scale_sig32( Src_p->hrf_right_prev_fx, Src_p->filterlength, sub( Src_p->hrf_right_prev_e, right_filter_e ) );
86 1388199 : Scale_sig32( hrf_right_delta_fx, Src_p->filterlength, sub( *hrf_right_delta_e, left_filter_e ) );
87 1388199 : Src_p->hrf_left_prev_e = left_filter_e;
88 1388199 : move16();
89 1388199 : *hrf_left_delta_e = left_filter_e;
90 1388199 : move16();
91 1388199 : Src_p->hrf_right_prev_e = right_filter_e;
92 1388199 : move16();
93 1388199 : *hrf_right_delta_e = right_filter_e;
94 1388199 : move16();
95 :
96 1388199 : TDREND_firfilt_fx( LeftOutputFrame_fx, Src_p->hrf_left_prev_fx, left_filter_e, hrf_left_delta_fx, intp_count, Src_p->mem_hrf_left_fx, subframe_length, Src_p->filterlength, Src_p->Gain_fx, Src_p->prevGain_fx );
97 1388199 : TDREND_firfilt_fx( RightOutputFrame_fx, Src_p->hrf_right_prev_fx, right_filter_e, hrf_right_delta_fx, intp_count, Src_p->mem_hrf_right_fx, subframe_length, Src_p->filterlength, Src_p->Gain_fx, Src_p->prevGain_fx );
98 :
99 1388199 : Src_p->prevGain_fx = Src_p->Gain_fx;
100 1388199 : move32();
101 :
102 : /* Copy to accumulative output frame */
103 1388199 : v_add_32( LeftOutputFrame_fx, output_buf_fx[0], output_buf_fx[0], subframe_length ); // Same Q as Src_p->InputFrame_p_fx Q11
104 1388199 : v_add_32( RightOutputFrame_fx, output_buf_fx[1], output_buf_fx[1], subframe_length ); // Same Q as Src_p->InputFrame_p_fx Q11
105 :
106 1388199 : return IVAS_ERR_OK;
107 : }
108 :
109 :
110 : /*-------------------------------------------------------------------*
111 : * GetFilterFromAngle()
112 : *
113 : * Obtain an HR filter corresponding to the input angle pair.
114 : * This version uses the HR filter model.
115 : --------------------------------------------------------------------*/
116 :
117 917003 : void GetFilterFromAngle_fx(
118 : TDREND_HRFILT_FiltSet_t *HrFiltSet_p, /* i/o: HR filter set structure */
119 : const Word32 Elev_fx, /* i : Elevation, degrees Q22 */
120 : Word32 Azim_fx, /* i : Azimuth, degrees Q22 */
121 : const Word16 filterlength, /* i : Filter length */
122 : Word32 *hrf_left_fx, /* o : Left HR filter */
123 : Word16 *hrf_left_e, /* o : Left HR filter exponent */
124 : Word32 *hrf_right_fx, /* o : Right HR filter */
125 : Word16 *hrf_right_e, /* o : Right HR filter exponent */
126 : Word16 *itd /* o : ITD value Q0 */
127 : )
128 : {
129 917003 : GenerateFilter_fx( Elev_fx, Azim_fx, &HrFiltSet_p->ModelParams, &HrFiltSet_p->ModelEval );
130 :
131 917003 : Copy32( HrFiltSet_p->ModelEval.hrfModL_fx, hrf_left_fx, filterlength );
132 917003 : *hrf_left_e = HrFiltSet_p->ModelEval.hrfModL_e;
133 917003 : move16();
134 917003 : Copy32( HrFiltSet_p->ModelEval.hrfModR_fx, hrf_right_fx, filterlength );
135 917003 : *hrf_right_e = HrFiltSet_p->ModelEval.hrfModR_e;
136 917003 : move16();
137 :
138 : /* 4. Evaluate the ITD */
139 917003 : IF( HrFiltSet_p->ModelParams.UseItdModel )
140 : {
141 917003 : GenerateITD_fx( Elev_fx, Azim_fx, &HrFiltSet_p->ModelParamsITD, &HrFiltSet_p->ModelEval );
142 917003 : *itd = extract_l( HrFiltSet_p->ModelEval.itdMod_fx ); // Q0
143 917003 : move16();
144 : }
145 : ELSE
146 : {
147 0 : *itd = 0;
148 0 : move16();
149 : }
150 :
151 917003 : return;
152 : }
153 :
154 6851491 : static Word32 round_fixed( /* o : Output value Q0 */
155 : Word32 num, /* i : Input value */
156 : Word16 q /* i : Input q-factor */
157 : )
158 : {
159 6851491 : Word32 half = L_shl( 1, sub( q, 1 ) );
160 :
161 6851491 : half = L_shr( half, 1 ); // one guard bit
162 6851491 : num = L_shr( num, 1 ); // one guard bit
163 :
164 6851491 : q = sub( q, 1 );
165 :
166 6851491 : IF( num >= 0 )
167 : {
168 6396188 : num = L_add( num, half );
169 : }
170 : ELSE
171 : {
172 455303 : num = L_sub( num, half );
173 : }
174 154737915 : WHILE( q != 0 )
175 : {
176 147886424 : num = ( num / 2 ); // using "/ 2" here because it rounds towards 0, whereas L_shr rounds towards -inf.
177 147886424 : q = sub( q, 1 );
178 : }
179 6851491 : return num;
180 : }
181 :
182 : /*-------------------------------------------------------------------*
183 : * GenerateFilter()
184 : *
185 : * Generate an HR filter using the B Spline model.
186 : --------------------------------------------------------------------*/
187 :
188 917003 : static void GenerateFilter_fx(
189 : const Word32 elev, /* i : Elevation angle, degrees Q22 */
190 : Word32 azim, /* i : Azimuth angle, degrees Q22 */
191 : ModelParams_t *model, /* i : Model parameters structure */
192 : ModelEval_t *modelEval /* i/o: Model evaluation structure */
193 : )
194 : {
195 : Word16 qp, p, k, i;
196 : Word16 AzIdx[HRTF_MODEL_BSPLINE_NUM_COEFFS][HRTF_MODEL_BSPLINE_NUM_COEFFS], EvIdx[HRTF_MODEL_BSPLINE_NUM_COEFFS]; /* non-zero basis functions */
197 : Word16 num_az_idx[HRTF_MODEL_BSPLINE_NUM_COEFFS];
198 : Word16 num_ev_idx;
199 : Word16 BM_idx[HRTF_MODEL_BSPLINE_NUM_COEFFS_SQ];
200 : Word32 knot_interval;
201 : Word32 ETotL, ETotR, ESynL, ESynR;
202 : Word16 ETotL_e, ETotR_e, ESynL_e, ESynR_e;
203 : Word32 ScaleL, ScaleR;
204 : Word16 ScaleL_e, ScaleR_e;
205 : Word16 iSec;
206 : Word16 tmp_e;
207 : Word32 tmp32;
208 : Word16 BMEnergiesR_e, BMEnergiesL_e;
209 : Word16 tmp_hrfModR_e, tmp_hrfModL_e;
210 :
211 917003 : getStandardBSplineSampVec_fx( modelEval->elevBfVec_fx, EvIdx, &num_ev_idx, model->elevDim3, elev, model->elevKSeq_fx,
212 917003 : model->elevSegSamples, model->elevBsLen, model->elevBsStart, model->elevBsShape_fx );
213 :
214 4109365 : FOR( p = 0; p < num_ev_idx; p++ )
215 : {
216 : /* Wrap the requested azimuth to the range of the BSplines */
217 3192362 : azim = L_add( azim, model->azimKSeq_fx[p][0] );
218 3192362 : WHILE( GT_32( azim, DEG_360_IN_Q22 ) )
219 : {
220 0 : azim = L_sub( azim, DEG_360_IN_Q22 ); // Q22
221 : }
222 3192362 : IF( azim < 0 )
223 : {
224 452714 : azim = L_add( azim, DEG_360_IN_Q22 ); // Q22
225 : }
226 3192362 : azim = L_sub( azim, model->azimKSeq_fx[p][0] ); // Q22
227 :
228 3192362 : IF( EQ_16( model->azimDim3[EvIdx[p]], 1 ) ) /* Constant basis function */
229 : {
230 8883 : num_az_idx[p] = 1;
231 8883 : move16();
232 8883 : AzIdx[p][0] = 0;
233 8883 : move16();
234 8883 : modelEval->azimBfVec_fx[p][0] = ONE_IN_Q30;
235 8883 : move32();
236 : }
237 : ELSE
238 : {
239 3183479 : k = EvIdx[p];
240 3183479 : move16();
241 :
242 3183479 : knot_interval = L_deposit_h( BASOP_Util_Divide3216_Scale( L_sub( model->azimKSeq_fx[k][model->azimDim3[k]], model->azimKSeq_fx[k][0] ), model->azimDim3[k], &tmp_e ) );
243 3183479 : tmp_e = add( tmp_e, 9 - 15 );
244 3183479 : knot_interval = L_shr( knot_interval, sub( 9, tmp_e ) ); // variable Q to Q22
245 :
246 3183479 : getPeriodicBSplineSampVec_fx( modelEval->azimBfVec_fx[p], AzIdx[p], model->azimDim3[k], azim, &num_az_idx[p],
247 3183479 : knot_interval, model->azimKSeq_fx[k][0], model->azimSegSamples[model->azimShapeIdx[k]],
248 3183479 : model->azimBsShape_fx[model->azimShapeIdx[k]], model->azimShapeSampFactor[k] );
249 : }
250 : }
251 :
252 : /* Compute BM */
253 917003 : qp = 0;
254 917003 : move16();
255 4109365 : FOR( p = 0; p < num_ev_idx; p++ )
256 : {
257 3192362 : Word32 expt = L_shl_sat( modelEval->elevBfVec_fx[p], 1 );
258 15503739 : FOR( i = 0; i < num_az_idx[p]; i++ )
259 : {
260 12311377 : modelEval->BM_fx[qp + i] = Mpy_32_32( expt, modelEval->azimBfVec_fx[p][i] ); /*Q30 - ( Q30 * 2 - 31 )*/ // Q30
261 12311377 : move32();
262 12311377 : BM_idx[qp + i] = add( model->azim_start_idx[EvIdx[p]], AzIdx[p][i] );
263 12311377 : move16();
264 : }
265 3192362 : qp = add( qp, num_az_idx[p] );
266 : }
267 :
268 917003 : Word16 expL = add( model->AlphaL_e, 1 );
269 917003 : Word16 expR = add( model->AlphaR_e, 1 );
270 917003 : BMEnergiesL_e = add( model->EL_e, 2 );
271 917003 : BMEnergiesR_e = add( model->ER_e, 2 );
272 :
273 : /* Compute HR filters, approximate optimized model evaluation */
274 3668012 : FOR( iSec = 0; iSec < HRTF_MODEL_N_SECTIONS; iSec++ )
275 : {
276 2751009 : Word64 temp1 = 0;
277 2751009 : move64();
278 2751009 : Word64 temp2 = 0;
279 2751009 : move64();
280 : /* Energy is precalculated part updated with square of BM value. Store index for sorting */
281 39685140 : FOR( i = 0; i < qp; i++ )
282 : {
283 36934131 : modelEval->BMEnergiesL[i].val_fx = Mpy_32_32( Mpy_32_32( modelEval->BM_fx[i], modelEval->BM_fx[i] ) /*Q29*/, model->EL_fx[( iSec * model->AlphaN ) + BM_idx[i]] ); // exp: model->EL_e + 2
284 36934131 : modelEval->BMEnergiesR[i].val_fx = Mpy_32_32( Mpy_32_32( modelEval->BM_fx[i], modelEval->BM_fx[i] ) /*Q29*/, model->ER_fx[( iSec * model->AlphaN ) + BM_idx[i]] ); // exp: model->ER_e + 2
285 36934131 : move32();
286 36934131 : move32();
287 36934131 : modelEval->BMEnergiesL[i].i = i;
288 36934131 : move16();
289 36934131 : modelEval->BMEnergiesR[i].i = i;
290 36934131 : move16();
291 36934131 : temp1 = W_add( temp1, modelEval->BMEnergiesL[i].val_fx ); // BMEnergiesL_e
292 36934131 : temp2 = W_add( temp2, modelEval->BMEnergiesR[i].val_fx ); // BMEnergiesR_e
293 : }
294 2751009 : ETotL_e = W_norm( temp1 );
295 2751009 : ETotL_e = sub( ETotL_e, 32 );
296 2751009 : ETotL = W_shl_sat_l( temp1, ETotL_e );
297 2751009 : ETotL_e = sub( BMEnergiesL_e, ETotL_e );
298 :
299 2751009 : ETotR_e = W_norm( temp2 );
300 2751009 : ETotR_e = sub( ETotR_e, 32 );
301 2751009 : ETotR = W_shl_sat_l( temp2, ETotR_e );
302 2751009 : ETotR_e = sub( BMEnergiesR_e, ETotR_e );
303 :
304 : /* Number of basis components actually used. */
305 2751009 : p = s_min( HRTF_MODEL_N_CPTS_VAR[iSec], qp );
306 2751009 : SkipSmallest_ValueIndex_fx( modelEval->UseIndsL, modelEval->BMEnergiesL, qp, sub( qp, p ) );
307 2751009 : SkipSmallest_ValueIndex_fx( modelEval->UseIndsR, modelEval->BMEnergiesR, qp, sub( qp, p ) );
308 :
309 2751009 : temp1 = 0;
310 2751009 : move64();
311 2751009 : temp2 = 0;
312 2751009 : move64();
313 :
314 : /* Account for lost energy */
315 34742954 : FOR( i = 0; i < p; i++ )
316 : {
317 31991945 : temp1 = W_add( temp1, modelEval->BMEnergiesL[modelEval->UseIndsL[i]].val_fx ); // BMEnergiesL_e
318 31991945 : temp2 = W_add( temp2, modelEval->BMEnergiesR[modelEval->UseIndsR[i]].val_fx ); // BMEnergiesR_e
319 : }
320 2751009 : ESynL_e = W_norm( temp1 );
321 2751009 : ESynL_e = sub( ESynL_e, 32 );
322 2751009 : ESynL = W_shl_sat_l( temp1, ESynL_e );
323 2751009 : ESynL_e = sub( BMEnergiesL_e, ESynL_e );
324 :
325 2751009 : ESynR_e = W_norm( temp2 );
326 2751009 : ESynR_e = sub( ESynR_e, 32 );
327 2751009 : ESynR = W_shl_sat_l( temp2, ESynR_e );
328 2751009 : ESynR_e = sub( BMEnergiesR_e, ESynR_e );
329 :
330 :
331 2751009 : tmp32 = L_deposit_h( BASOP_Util_Divide3232_Scale( ETotL, ESynL, &ScaleL_e ) );
332 2751009 : ScaleL_e = add( ScaleL_e, sub( ETotL_e, ESynL_e ) );
333 2751009 : ScaleL = Sqrt32( tmp32, &ScaleL_e );
334 2751009 : ScaleL_e = sub( ScaleL_e, 1 );
335 :
336 2751009 : tmp32 = L_deposit_h( BASOP_Util_Divide3232_Scale( ETotR, ESynR, &ScaleR_e ) );
337 2751009 : ScaleR_e = add( ScaleR_e, sub( ETotR_e, ESynR_e ) );
338 2751009 : ScaleR = Sqrt32( tmp32, &ScaleR_e );
339 2751009 : ScaleR_e = sub( ScaleR_e, 1 );
340 :
341 : /* Build using only the most energetic components. */
342 117841875 : FOR( k = model->iSecFirst[iSec]; k <= model->iSecLast[iSec]; k++ )
343 : {
344 115090866 : temp1 = 0;
345 115090866 : move64();
346 115090866 : temp2 = 0;
347 115090866 : move64();
348 :
349 1452003604 : FOR( i = 0; i < p; i++ )
350 : {
351 1336912738 : temp1 = W_add( temp1, Mpy_32_32( modelEval->BM_fx[modelEval->BMEnergiesL[modelEval->UseIndsL[i]].i], model->AlphaL_fx[BM_idx[modelEval->BMEnergiesL[modelEval->UseIndsL[i]].i] + ( model->AlphaN * k )] ) ); // add(model->AlphaL_e, 1)
352 1336912738 : temp2 = W_add( temp2, Mpy_32_32( modelEval->BM_fx[modelEval->BMEnergiesR[modelEval->UseIndsR[i]].i], model->AlphaR_fx[BM_idx[modelEval->BMEnergiesR[modelEval->UseIndsR[i]].i] + ( model->AlphaN * k )] ) ); // add(model->AlphaR_e, 1)
353 : }
354 :
355 115090866 : tmp_hrfModL_e = W_norm( temp1 );
356 115090866 : tmp_hrfModL_e = sub( tmp_hrfModL_e, 32 );
357 115090866 : modelEval->hrfModL_fx[k] = W_shl_sat_l( temp1, tmp_hrfModL_e );
358 115090866 : move32();
359 115090866 : tmp_hrfModL_e = sub( expL, tmp_hrfModL_e );
360 115090866 : if ( temp1 == 0 )
361 : {
362 1950 : tmp_hrfModL_e = 0;
363 1950 : move16();
364 : }
365 :
366 115090866 : tmp_hrfModR_e = W_norm( temp2 );
367 115090866 : tmp_hrfModR_e = sub( tmp_hrfModR_e, 32 );
368 115090866 : modelEval->hrfModR_fx[k] = W_shl_sat_l( temp2, tmp_hrfModR_e );
369 115090866 : move32();
370 115090866 : tmp_hrfModR_e = sub( expR, tmp_hrfModR_e );
371 :
372 115090866 : if ( temp2 == 0 )
373 : {
374 2677 : tmp_hrfModR_e = 0;
375 2677 : move16();
376 : }
377 :
378 : /* Account for lost energy */
379 115090866 : modelEval->hrfModL_fx[k] = Mpy_32_32( modelEval->hrfModL_fx[k], ScaleL );
380 115090866 : move32();
381 115090866 : modelEval->hrfModR_fx[k] = Mpy_32_32( modelEval->hrfModR_fx[k], ScaleR );
382 115090866 : move32();
383 :
384 : /* NOTE: Assuming that finally, hrfMod values will be <= 1. Hence making it Q30 */
385 115090866 : modelEval->hrfModL_fx[k] = L_shl( modelEval->hrfModL_fx[k], add( tmp_hrfModL_e, ScaleL_e ) ); // assuming Q30
386 115090866 : modelEval->hrfModR_fx[k] = L_shl( modelEval->hrfModR_fx[k], add( tmp_hrfModR_e, ScaleR_e ) ); // assuming Q30
387 115090866 : move32();
388 115090866 : move32();
389 : }
390 : }
391 :
392 917003 : modelEval->hrfModL_e = 1; /*Q30*/
393 917003 : move16();
394 917003 : modelEval->hrfModR_e = 1; /*Q30*/
395 917003 : move16();
396 :
397 917003 : return;
398 : }
399 :
400 :
401 : /*-------------------------------------------------------------------*
402 : * GenerateITD()
403 : *
404 : * Generates an ITD value from the B Spline model.
405 : --------------------------------------------------------------------*/
406 :
407 917003 : static void GenerateITD_fx(
408 : const Word32 elev_fx, /* i : Elevation angle, degrees Q22 */
409 : Word32 azim_fx, /* i : Azimuth angle, degrees Q22 */
410 : ModelParamsITD_t *model, /* i : ITD Model parameters structure */
411 : ModelEval_t *modelEval /* i/o: Model evaluation structure */
412 : )
413 : {
414 : Word16 qp, p, i;
415 : Word32 index;
416 : Word16 AlphaN;
417 : Word16 elev_offset;
418 : Word32 azim_itd_fx;
419 : Word16 AzIdx[HRTF_MODEL_BSPLINE_NUM_COEFFS], EvIdx[HRTF_MODEL_BSPLINE_NUM_COEFFS]; /* non-zero basis functions */
420 : Word16 num_az_idx, num_ev_idx;
421 : Word16 BM_idx[HRTF_MODEL_BSPLINE_NUM_COEFFS_SQ];
422 : Word16 itdMod_e;
423 917003 : set16_fx( AzIdx, 0, HRTF_MODEL_BSPLINE_NUM_COEFFS );
424 :
425 : /* Wrap the requested azimuth to the range of the BSplines */
426 917003 : azim_fx = L_add( azim_fx, model->azimKSeq_fx[0] );
427 917003 : WHILE( GT_32( azim_fx, DEG_360_IN_Q22 ) )
428 : {
429 0 : azim_fx = L_sub( azim_fx, DEG_360_IN_Q22 ); // Q22
430 : }
431 917003 : if ( azim_fx < 0 )
432 : {
433 452714 : azim_fx = L_add( azim_fx, DEG_360_IN_Q22 ); // Q22
434 : }
435 917003 : azim_fx = L_sub( azim_fx, model->azimKSeq_fx[0] ); // Q22
436 :
437 917003 : IF( NE_32( L_abs( elev_fx ), DEG_90_IN_Q22 ) )
438 : {
439 917003 : getStandardBSplineSampVec_fx( modelEval->elevBfVecITD_fx, EvIdx, &num_ev_idx, model->elevDim3, elev_fx, model->elevKSeq_fx,
440 917003 : model->elevSegSamples, model->elevBsLen, model->elevBsStart, model->elevBsShape_fx );
441 :
442 917003 : azim_itd_fx = azim_fx;
443 917003 : move32();
444 917003 : if ( GT_32( azim_fx, DEG_180_IN_Q22 ) )
445 : {
446 : /* Flip spline functions around 180 deg */
447 452403 : azim_itd_fx = L_sub( DEG_360_IN_Q22, azim_fx ); // Q22
448 : }
449 917003 : getStandardBSplineSampVec_fx( modelEval->azimBfVecITD_fx, AzIdx, &num_az_idx, shr( add( model->azimDim3, 1 ), 1 ), azim_itd_fx, model->azimKSeq_fx,
450 917003 : model->azimSegSamples, model->azimBsLen, model->azimBsStart, model->azimBsShape_fx );
451 917003 : IF( GT_32( azim_fx, DEG_180_IN_Q22 ) )
452 : {
453 : /* Flip spline functions around 180 deg */
454 2262015 : FOR( i = 0; i < HRTF_MODEL_BSPLINE_NUM_COEFFS; i++ )
455 : {
456 1809612 : AzIdx[i] = sub( sub( model->azimDim3, 1 ), AzIdx[i] );
457 1809612 : move16();
458 : }
459 : }
460 : }
461 : ELSE
462 : {
463 0 : num_az_idx = 1;
464 0 : move16();
465 0 : num_ev_idx = 1;
466 0 : move16();
467 :
468 0 : modelEval->elevBfVecITD_fx[0] = ONE_IN_Q30;
469 0 : move32();
470 :
471 0 : IF( LT_32( elev_fx, 0 ) )
472 : {
473 0 : EvIdx[0] = 0;
474 0 : move16();
475 : }
476 : ELSE
477 : {
478 0 : EvIdx[0] = sub( model->elevDim3, 1 );
479 0 : move16();
480 : }
481 : }
482 :
483 : /* Compute BM_ITD */
484 917003 : elev_offset = 0;
485 917003 : move16();
486 917003 : if ( EQ_32( model->elevKSeq_fx[0], -DEG_90_IN_Q22 ) )
487 : {
488 917003 : elev_offset = sub( 1, model->azimDim3 );
489 : }
490 917003 : qp = 0;
491 917003 : move16();
492 4277154 : FOR( p = 0; p < num_ev_idx; p++ )
493 : {
494 3360151 : test();
495 3360151 : test();
496 3360151 : test();
497 3360151 : test();
498 3360151 : IF( EvIdx[p] == 0 && EQ_32( model->elevKSeq_fx[EvIdx[p]], -DEG_90_IN_Q22 ) )
499 : {
500 3319 : modelEval->BM_ITD_fx[qp] = modelEval->elevBfVecITD_fx[p]; // Q30
501 3319 : move32();
502 3319 : BM_idx[qp] = imult1616( EvIdx[p], model->azimDim3 );
503 3319 : move16();
504 3319 : qp = add( qp, 1 );
505 : }
506 3356832 : ELSE IF( EQ_16( EvIdx[p], sub( model->elevDim3, 1 ) ) && EQ_32( model->elevKSeq_fx[sub( EvIdx[p], 2 )], DEG_90_IN_Q22 ) )
507 : {
508 : /* NB: -2 in if() condition above as number of knot points is numBF-2 */
509 3457 : modelEval->BM_ITD_fx[qp] = modelEval->elevBfVecITD_fx[p]; // Q30
510 3457 : move32();
511 3457 : BM_idx[qp] = add( imult1616( EvIdx[p], model->azimDim3 ), elev_offset );
512 3457 : move16();
513 3457 : qp = add( qp, 1 );
514 : }
515 : ELSE
516 : {
517 3353375 : Word16 temp_e = add( imult1616( EvIdx[p], model->azimDim3 ), elev_offset );
518 16317432 : FOR( i = 0; i < num_az_idx; i++ )
519 : {
520 12964057 : modelEval->BM_ITD_fx[qp + i] = L_shl( Mpy_32_32( modelEval->elevBfVecITD_fx[p], modelEval->azimBfVecITD_fx[i] ), 1 ); // Q30
521 12964057 : move32();
522 12964057 : BM_idx[qp + i] = add( temp_e, AzIdx[i] );
523 12964057 : move16();
524 : }
525 3353375 : qp = add( qp, num_az_idx );
526 : }
527 : }
528 :
529 : /* Compute ITD */
530 917003 : AlphaN = add( imult1616( model->elevDim3, model->azimDim3 ), elev_offset );
531 917003 : if ( EQ_32( model->elevKSeq_fx[sub( model->elevDim3, 3 )], DEG_90_IN_Q22 ) ) /* Constant azimuth basis function */
532 : {
533 917003 : AlphaN = sub( AlphaN, sub( model->azimDim3, 1 ) );
534 : }
535 :
536 : /* Matrix multiplcation (row x column) */
537 917003 : Word64 temp = 0;
538 917003 : move64();
539 917003 : Word16 res_e = add( model->W_e, 1 );
540 13887836 : FOR( i = 0; i < qp; i++ )
541 : {
542 12970833 : index = BM_idx[i];
543 12970833 : move32();
544 12970833 : temp = W_add( temp, Mpy_32_32( modelEval->BM_ITD_fx[i], model->W_fx[index] ) );
545 : }
546 917003 : itdMod_e = W_norm( temp );
547 917003 : itdMod_e = sub( itdMod_e, 32 );
548 917003 : modelEval->itdMod_fx = W_shl_sat_l( temp, itdMod_e );
549 917003 : itdMod_e = sub( res_e, itdMod_e );
550 :
551 917003 : Word32 tmp32 = Mpy_32_16_1( modelEval->itdMod_fx, model->resamp_factor_fx ); // Q = 31 - ( itdMod_e + 1 )
552 917003 : Word16 tmp_q = sub( 30, itdMod_e );
553 917003 : IF( tmp_q < 0 )
554 : {
555 0 : tmp32 = L_shr( tmp32, tmp_q );
556 0 : tmp_q = 0;
557 0 : move16();
558 : }
559 917003 : ELSE IF( GE_32( tmp_q, 32 ) )
560 : {
561 35306 : tmp32 = L_shr( tmp32, sub( tmp_q, 31 ) );
562 35306 : tmp_q = 31;
563 35306 : move16();
564 : }
565 917003 : modelEval->itdMod_fx = L_negate( round_fixed( tmp32, tmp_q ) ); // Q0
566 :
567 917003 : return;
568 : }
569 :
570 :
571 : /*-------------------------------------------------------------------*
572 : * getStandardBSplineSampVec()
573 : *
574 : * Obtain a periodic sampled B Spline basis vector.
575 : --------------------------------------------------------------------*/
576 :
577 3183479 : static void getPeriodicBSplineSampVec_fx(
578 : Word32 *BfVec_fx, /* o : values for non-zero basis functions Q30 */
579 : Word16 *AzIdx, /* o : indices of non-zero basis functions */
580 : const Word16 NumBFs, /* i : the number of basis functions = third index of Bf. */
581 : const Word32 t_fx, /* i : azimuth Q22 */
582 : Word16 *num_az_idx, /* o : Number of azimuth indices */
583 : const Word32 knot_interval_fx, /* i : The knot interval Q22 */
584 : const Word32 azimKSeq_0_fx, /* i : Knot sequence Q22 */
585 : const Word16 azimSegSamples, /* i : Samples per segment */
586 : const Word32 *azimBsShape_fx, /* i : Basis shape Q30 */
587 : const Word16 subSampFactor /* i : Subsampling factor */
588 : )
589 : {
590 : Word16 i, nI, d0, d;
591 : Word32 tmp32;
592 : Word16 tmp_e1, tmp_e2;
593 : Word16 SegSamples;
594 :
595 3183479 : SegSamples = 0;
596 3183479 : move16();
597 3183479 : IF( azimSegSamples != 0 )
598 : {
599 3183479 : SegSamples = idiv1616( azimSegSamples, subSampFactor );
600 : }
601 :
602 : /* index of closest sample point */
603 3183479 : IF( knot_interval_fx == 0 )
604 : {
605 0 : d0 = 0;
606 0 : move16();
607 : }
608 : ELSE
609 : {
610 3183479 : tmp32 = L_deposit_h( BASOP_Util_Divide3216_Scale( knot_interval_fx, SegSamples, &tmp_e1 ) );
611 3183479 : tmp_e1 = add( tmp_e1, 9 - 15 );
612 3183479 : tmp32 = L_deposit_h( BASOP_Util_Divide3232_Scale( L_sub( t_fx, azimKSeq_0_fx ), tmp32, &tmp_e2 ) );
613 3183479 : tmp_e2 = add( tmp_e2, sub( 9, tmp_e1 ) );
614 3183479 : tmp32 = L_shr( tmp32, sub( 9, tmp_e2 ) ); // Q22 (assuming tmp32 will be in range of Q22)
615 3183479 : d0 = extract_l( round_fixed( tmp32, Q22 ) );
616 : }
617 :
618 : /* find segment */
619 3183479 : nI = 0;
620 3183479 : move16();
621 3183479 : IF( d0 != 0 )
622 : {
623 3138490 : nI = idiv1616( d0, SegSamples );
624 : }
625 :
626 3183479 : *num_az_idx = HRTF_MODEL_BSPLINE_NUM_COEFFS;
627 3183479 : move16();
628 :
629 3183479 : IF( d0 % SegSamples == 0 )
630 : {
631 431422 : *num_az_idx = sub( *num_az_idx, 1 ); /* on the knot points, the last basis function is zero */
632 431422 : move16();
633 : }
634 :
635 15485973 : FOR( i = 0; i < *num_az_idx; i++ )
636 : {
637 12302494 : d = sub( d0, imult1616( ( sub( add( i, nI ), 1 ) ), SegSamples ) ); /* offset of knot_interval */
638 12302494 : d = sub( d0, imult1616( sub( add( i, nI ), 1 ), SegSamples ) );
639 12302494 : BfVec_fx[i] = azimBsShape_fx[i_mult( abs_s( d ), subSampFactor )];
640 12302494 : move32();
641 12302494 : AzIdx[i] = add( nI, i ) % NumBFs;
642 12302494 : move16();
643 : }
644 :
645 3183479 : return;
646 : }
647 :
648 : /*-------------------------------------------------------------------*
649 : * getStandardBSplineSampVec()
650 : *
651 : * Obtain a sampled B Spline basis vector.
652 : --------------------------------------------------------------------*/
653 :
654 2751009 : static void getStandardBSplineSampVec_fx(
655 : Word32 *BfVec_fx, /* o : values for non-zero basis functions Q30 */
656 : Word16 *NzIdx, /* o : indices of non-zero basis functions */
657 : Word16 *num_idx, /* i/o: number of non-zero indices */
658 : const Word16 NumBFs, /* i : the number of basis functions = third index of Bf */
659 : const Word32 t_fx, /* i : estimation point Q22 */
660 : const Word32 *KSeq_fx, /* i : knot sequence including multiplicities Q22 */
661 : const Word16 SegSamples, /* i : samples per segment */
662 : const Word16 *BsLen, /* i : lengths of basis shapes */
663 : const Word16 *BsStart, /* i : start of basis shapes */
664 : const Word32 *BsShape_fx /* i : basis shapes Q30 */
665 : )
666 : {
667 : Word16 i, nI;
668 : Word32 knot_interval_fx;
669 : Word16 d0, d, shape_idx, start_idx;
670 : Word16 tmp_e1, tmp_e2, tmp_e3;
671 : Word32 tmp32;
672 :
673 : /* assuming triple knot at the first knot */
674 2751009 : knot_interval_fx = L_deposit_h( BASOP_Util_Divide3216_Scale( L_sub( KSeq_fx[NumBFs - 3], KSeq_fx[0] ), sub( NumBFs, 3 ), &tmp_e1 ) );
675 2751009 : tmp_e1 = add( tmp_e1, 9 - 15 );
676 :
677 : /* index of closest sample point */
678 2751009 : tmp32 = L_deposit_h( BASOP_Util_Divide3216_Scale( knot_interval_fx, SegSamples, &tmp_e2 ) );
679 2751009 : tmp_e2 = add( tmp_e2, sub( tmp_e1, 15 ) );
680 2751009 : tmp32 = L_deposit_h( BASOP_Util_Divide3232_Scale( L_sub( t_fx, KSeq_fx[0] ), tmp32, &tmp_e3 ) );
681 2751009 : tmp_e3 = add( tmp_e3, sub( 9, tmp_e2 ) );
682 2751009 : tmp32 = L_shr( tmp32, sub( 9, tmp_e3 ) ); // Q22 (assuming tmp32 will be in range of Q22)
683 2751009 : d0 = extract_l( round_fixed( tmp32, 22 ) );
684 :
685 : /* find segment */
686 2751009 : nI = 0;
687 2751009 : move16();
688 2751009 : IF( d0 != 0 )
689 : {
690 2736355 : nI = idiv1616( d0, SegSamples );
691 : }
692 2751009 : *num_idx = HRTF_MODEL_BSPLINE_NUM_COEFFS;
693 2751009 : move16();
694 :
695 2751009 : IF( d0 % SegSamples == 0 )
696 : {
697 906243 : *num_idx = sub( *num_idx, 1 ); /* on the knot points, the last basis function is zero */
698 906243 : move16();
699 : }
700 :
701 12848802 : FOR( i = 0; i < *num_idx; i++ )
702 : {
703 10097793 : start_idx = s_max( 0, sub( add( i, nI ), 3 ) );
704 10097793 : shape_idx = s_min( add( i, nI ), s_min( 3, sub( sub( NumBFs, 1 ), ( add( i, nI ) ) ) ) );
705 10097793 : d = sub( d0, imult1616( start_idx, SegSamples ) );
706 :
707 10097793 : IF( GT_16( add( i, nI ), sub( NumBFs, 4 ) ) ) /* reverse full shape */
708 : {
709 481969 : d = sub( sub( BsLen[shape_idx], 1 ), d );
710 : }
711 9615824 : ELSE IF( GT_16( d, sub( BsLen[shape_idx], 1 ) ) ) /* reverse half shape */
712 : {
713 4140320 : d = sub( shl( sub( BsLen[shape_idx], 1 ), 1 ), d );
714 : }
715 10097793 : BfVec_fx[i] = BsShape_fx[add( BsStart[shape_idx], abs_s( d ) )]; /*TT, verify if abs is needed */
716 10097793 : move32();
717 10097793 : NzIdx[i] = add( nI, i );
718 10097793 : move16();
719 : }
720 :
721 2751009 : return;
722 : }
723 :
724 :
725 : /*-------------------------------------------------------------------*
726 : * HRTF_model_precalc()
727 : *
728 : * Set precalculated parameters for HRTF model
729 : --------------------------------------------------------------------*/
730 :
731 382 : void HRTF_model_precalc(
732 : ModelParams_t *model /* i/o: HRTF model parameters */
733 : )
734 : {
735 : Word16 sec_length;
736 : Word16 i;
737 382 : sec_length = mult( model->K, 10923 ); /*10923 == 2 ^ 15 / 3*/
738 1528 : FOR( i = 0; i < HRTF_MODEL_N_SECTIONS; i++ )
739 : {
740 1146 : model->iSecFirst[i] = imult1616( i, sec_length );
741 1146 : move16();
742 : }
743 1146 : FOR( i = 0; i < HRTF_MODEL_N_SECTIONS - 1; i++ )
744 : {
745 764 : model->iSecLast[i] = sub( imult1616( add( i, 1 ), sec_length ), 1 );
746 764 : move16();
747 : }
748 382 : model->iSecLast[HRTF_MODEL_N_SECTIONS - 1] = sub( model->K, 1 ); /* Final section is longer if (K % nSec) > 0 */
749 382 : move16();
750 382 : maximum_fx( model->azimDim3, model->elevDim3, &model->azimDim3Max );
751 382 : return;
752 : }
753 :
754 :
755 : /*-------------------------------------------------------------------*
756 : * BSplineModelEvalDealloc()
757 : *
758 : * Deallocate BSpline HR Filter model
759 : --------------------------------------------------------------------*/
760 405 : void BSplineModelEvalDealloc_fx(
761 : ModelParams_t *model, /* i : Model parameters */
762 : ModelEval_t *modelEval /* i : Model evaluation structure */
763 : )
764 : {
765 : /* Allocated in LoadBSplineBinary() */
766 : Word16 i;
767 :
768 405 : IF( model->modelROM )
769 : {
770 358 : free( (void *) model->azimBsShape_fx ); /* void* cast needed to please both gcc and Visual studio compilers. Deallocating const float** should be fine and gcc agrees, but Visual studio complains. */
771 5728 : FOR( i = 0; i < model->elevDim3; i++ )
772 : {
773 5370 : free( model->azimKSeq_fx[i] );
774 : }
775 358 : free( model->azimKSeq_fx );
776 358 : IF( modelEval != NULL )
777 : {
778 358 : free( modelEval->hrfModL_fx );
779 358 : free( modelEval->hrfModR_fx );
780 : }
781 : }
782 405 : return;
783 : }
784 :
785 :
786 : /*-------------------------------------------------------------------*
787 : * SkipSmallest_ValueIndex()
788 : *
789 : * Returns indices to the largest values in a ValueIndex list,
790 : * unordered (i.e. skip the n_smallest values, return the remainder).
791 : --------------------------------------------------------------------*/
792 :
793 5502018 : static void SkipSmallest_ValueIndex_fx(
794 : Word16 *use_inds, /* i/o: List of indices to use */
795 : const ValueIndex_t *VI, /* i : List of value-index items */
796 : const Word16 N, /* i : Length of list */
797 : const Word16 n_smallest /* i : Number of items to skip */
798 : )
799 : {
800 : Word16 i, j, k, flag;
801 : Word16 skip_inds[HRTF_MODEL_BSPLINE_NUM_COEFFS_SQ];
802 : Word32 candidate_max; /* Stores the max value in the shortlist (next to be kicked off) and its index */
803 : Word16 candidate_max_i;
804 :
805 : /* Initialise with first entries of VI */
806 5502018 : candidate_max = 0;
807 5502018 : move32();
808 5502018 : candidate_max_i = 0;
809 5502018 : move16();
810 15386390 : FOR( j = 0; j < n_smallest; j++ )
811 : {
812 9884372 : skip_inds[j] = j;
813 9884372 : move16();
814 9884372 : IF( LT_32( candidate_max, VI[j].val_fx ) )
815 : {
816 3446914 : candidate_max = VI[j].val_fx;
817 3446914 : move32();
818 3446914 : candidate_max_i = j;
819 3446914 : move16();
820 : }
821 : }
822 :
823 : /* Look in the remainder of the list for smaller values */
824 69485908 : FOR( i = n_smallest; i < N; i++ )
825 : {
826 163400419 : FOR( j = 0; j < n_smallest; j++ )
827 : {
828 105857551 : IF( LT_32( VI[i].val_fx, VI[skip_inds[j]].val_fx ) )
829 : {
830 : /* Found a smaller value, so it goes into the list, replacing candidate_max. */
831 6441022 : skip_inds[candidate_max_i] = i;
832 6441022 : move16();
833 6441022 : candidate_max = VI[i].val_fx;
834 6441022 : move32();
835 : /* Update candidate_max */
836 29753761 : FOR( k = 0; k < n_smallest; k++ )
837 : {
838 23312739 : IF( GT_32( VI[skip_inds[k]].val_fx, candidate_max ) )
839 : {
840 4219112 : candidate_max = VI[skip_inds[k]].val_fx;
841 4219112 : move32();
842 4219112 : candidate_max_i = k;
843 4219112 : move16();
844 : }
845 : }
846 6441022 : BREAK;
847 : }
848 : }
849 : }
850 :
851 : /* Build the list of indices that will not be skipped */
852 5502018 : k = 0;
853 5502018 : move16();
854 79370280 : FOR( j = 0; j < N; j++ )
855 : {
856 73868262 : flag = 1;
857 73868262 : move16();
858 204091536 : FOR( i = 0; i < n_smallest; i++ )
859 : {
860 140107646 : IF( EQ_16( skip_inds[i], j ) )
861 : {
862 9884372 : flag = 0;
863 9884372 : move16();
864 9884372 : BREAK;
865 : }
866 : }
867 73868262 : IF( flag )
868 : {
869 63983890 : use_inds[k] = j;
870 63983890 : move16();
871 63983890 : k = add( k, 1 );
872 : }
873 : }
874 :
875 5502018 : return;
876 : }
|
