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