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