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 "common_api_types.h"
34 : #include <stdint.h>
35 : #include "options.h"
36 : #include "ivas_prot_rend_fx.h"
37 : #include "ivas_cnst.h"
38 : #include <math.h>
39 : #include <assert.h>
40 : #include "wmc_auto.h"
41 : #include "prot_fx.h"
42 :
43 : /*------------------------------------------------------------------------------------------*
44 : * Local constants
45 : *------------------------------------------------------------------------------------------*/
46 :
47 : #define COS_ONE_TENTH_DEGREE_FX 2147480320 // Q31
48 :
49 :
50 : /*------------------------------------------------------------------------------------------*
51 : * Local functions
52 : *------------------------------------------------------------------------------------------*/
53 :
54 : /*------------------------------------------------------------------------------------------*
55 : * IdentityQuaternion()
56 : *
57 : *
58 : *------------------------------------------------------------------------------------------*/
59 :
60 0 : static IVAS_QUATERNION IdentityQuaternion_fx(
61 : void )
62 : {
63 : IVAS_QUATERNION q;
64 :
65 0 : q.w_fx = ONE_IN_Q31;
66 0 : move32();
67 0 : q.x_fx = q.y_fx = q.z_fx = 0;
68 0 : move32();
69 0 : move32();
70 0 : move32();
71 0 : q.q_fact = Q31;
72 0 : move16();
73 :
74 0 : return q;
75 : }
76 :
77 : /*------------------------------------------------------------------------------------------*
78 : * QuaternionProduct()
79 : *
80 : * Quaternion product
81 : *------------------------------------------------------------------------------------------*/
82 :
83 192273 : void QuaternionProduct_fx(
84 : const IVAS_QUATERNION q1,
85 : const IVAS_QUATERNION q2,
86 : IVAS_QUATERNION *const r )
87 : {
88 : IVAS_QUATERNION tmp;
89 :
90 : // once verify//
91 192273 : tmp.w_fx = L_sub( ( L_sub( Mpy_32_32( q1.w_fx, q2.w_fx ), Mpy_32_32( q1.x_fx, q2.x_fx ) ) ), ( L_add( Mpy_32_32( q1.y_fx, q2.y_fx ), Mpy_32_32( q1.z_fx, q2.z_fx ) ) ) ); // ( q1.q_fact + q2.q_fact ) - 31
92 192273 : move32();
93 192273 : tmp.x_fx = L_add( ( L_add( Mpy_32_32( q1.w_fx, q2.x_fx ), Mpy_32_32( q1.x_fx, q2.w_fx ) ) ), ( L_sub( Mpy_32_32( q1.y_fx, q2.z_fx ), Mpy_32_32( q1.z_fx, q2.y_fx ) ) ) ); // ( q1.q_fact + q2.q_fact ) - 31
94 192273 : move32();
95 192273 : tmp.y_fx = L_add( ( L_sub( Mpy_32_32( q1.w_fx, q2.y_fx ), Mpy_32_32( q1.x_fx, q2.z_fx ) ) ), ( L_add( Mpy_32_32( q1.y_fx, q2.w_fx ), Mpy_32_32( q1.z_fx, q2.x_fx ) ) ) ); // ( q1.q_fact + q2.q_fact ) - 31
96 192273 : move32();
97 192273 : tmp.z_fx = L_sub( ( L_add( Mpy_32_32( q1.w_fx, q2.z_fx ), Mpy_32_32( q1.x_fx, q2.y_fx ) ) ), ( L_sub( Mpy_32_32( q1.y_fx, q2.x_fx ), Mpy_32_32( q1.z_fx, q2.w_fx ) ) ) ); // ( q1.q_fact + q2.q_fact ) - 31
98 192273 : move32();
99 192273 : tmp.q_fact = sub( add( q1.q_fact, q2.q_fact ), 31 );
100 192273 : move16();
101 :
102 192273 : *r = tmp;
103 192273 : return;
104 : }
105 :
106 : /*------------------------------------------------------------------------------------------*
107 : * QuaternionDotProduct()
108 : *
109 : * Quaternion dot product
110 : *------------------------------------------------------------------------------------------*/
111 :
112 613572 : static Word32 QuaternionDotProduct_fx(
113 : const IVAS_QUATERNION q1,
114 : const IVAS_QUATERNION q2,
115 : Word16 *q_fact )
116 : {
117 :
118 613572 : Word32 result = 0;
119 613572 : move32();
120 :
121 613572 : result = L_add( ( L_add( Mpy_32_32( q1.x_fx, q2.x_fx ), Mpy_32_32( q1.y_fx, q2.y_fx ) ) ), ( L_add( Mpy_32_32( q1.z_fx, q2.z_fx ), Mpy_32_32( q1.w_fx, q2.w_fx ) ) ) ); // ( q1.q_fact + q2.q_fact ) - 31
122 :
123 613572 : *q_fact = sub( add( q1.q_fact, q2.q_fact ), 31 );
124 613572 : move16();
125 :
126 613572 : return result;
127 : }
128 :
129 : /*------------------------------------------------------------------------------------------*
130 : * QuaternionDivision()
131 : *
132 : * Divides a quaternion by a scalar
133 : *------------------------------------------------------------------------------------------*/
134 :
135 537749 : static void QuaternionDivision_fx(
136 : const IVAS_QUATERNION q,
137 : const Word32 d,
138 : IVAS_QUATERNION *const r,
139 : Word16 den_e )
140 : {
141 537749 : Word16 scale_e, result_e = 0, w_q, x_q, y_q, z_q, result_q;
142 :
143 537749 : r->w_fx = BASOP_Util_Divide3232_Scale_newton( ( q.w_fx ), d, &scale_e );
144 537749 : move32();
145 537749 : result_e = add( scale_e, sub( sub( Q31, q.q_fact ), den_e ) ); // e+e1-e2//
146 537749 : w_q = sub( Q31, result_e );
147 :
148 537749 : r->x_fx = BASOP_Util_Divide3232_Scale_newton( ( q.x_fx ), d, &scale_e );
149 537749 : move32();
150 537749 : result_e = add( scale_e, sub( sub( Q31, q.q_fact ), den_e ) );
151 537749 : x_q = sub( Q31, result_e );
152 :
153 537749 : r->y_fx = BASOP_Util_Divide3232_Scale_newton( ( q.y_fx ), d, &scale_e );
154 537749 : move32();
155 537749 : result_e = add( scale_e, sub( sub( Q31, q.q_fact ), den_e ) );
156 537749 : y_q = sub( Q31, result_e );
157 :
158 537749 : r->z_fx = BASOP_Util_Divide3232_Scale_newton( ( q.z_fx ), d, &scale_e );
159 537749 : move32();
160 537749 : result_e = add( scale_e, sub( sub( Q31, q.q_fact ), den_e ) );
161 537749 : z_q = sub( Q31, result_e );
162 :
163 537749 : result_q = sub( s_min( s_min( w_q, x_q ), s_min( y_q, z_q ) ), 1 ); // gaurdbits//
164 :
165 537749 : r->w_fx = L_shr( r->w_fx, sub( w_q, result_q ) ); // result_q
166 537749 : move32();
167 537749 : r->x_fx = L_shr( r->x_fx, sub( x_q, result_q ) ); // result_q
168 537749 : move32();
169 537749 : r->y_fx = L_shr( r->y_fx, sub( y_q, result_q ) ); // result_q
170 537749 : move32();
171 537749 : r->z_fx = L_shr( r->z_fx, sub( z_q, result_q ) ); // result_q
172 537749 : move32();
173 537749 : r->q_fact = result_q;
174 537749 : move16();
175 537749 : }
176 :
177 :
178 : /*------------------------------------------------------------------------------------------*
179 : * QuaternionNormalize()
180 : *
181 : * Normalizes a quaternion
182 : *------------------------------------------------------------------------------------------*/
183 :
184 296303 : static void QuaternionNormalize_fx(
185 : const IVAS_QUATERNION q_fx,
186 : IVAS_QUATERNION *const r_fx )
187 : {
188 296303 : Word16 q_dot, sqrt_e = 0;
189 296303 : move16();
190 : Word32 sqrt_fx;
191 296303 : Word32 dot_prod_fx = QuaternionDotProduct_fx( q_fx, q_fx, &q_dot );
192 296303 : sqrt_e = sub( Q31, q_dot );
193 296303 : sqrt_fx = Sqrt32( dot_prod_fx, &sqrt_e );
194 296303 : QuaternionDivision_fx( q_fx, sqrt_fx, r_fx, sqrt_e );
195 296303 : return;
196 : }
197 :
198 :
199 : /*------------------------------------------------------------------------------------------*
200 : * QuaternionSlerp()
201 : *
202 : * Computes a spherical linear interpolation between two quaternions
203 : *------------------------------------------------------------------------------------------*/
204 :
205 97433 : void QuaternionSlerp_fx(
206 : const IVAS_QUATERNION q1_fx,
207 : const IVAS_QUATERNION q2_fx,
208 : const Word32 t_fx,
209 : IVAS_QUATERNION *const r_fx )
210 : {
211 : IVAS_QUATERNION r1, r2, tmp_quat;
212 : Word32 sinPhi, cosPhi, temp_32;
213 : Word16 q_min, sin_e, phi, s1, s2, temp_16;
214 :
215 97433 : QuaternionNormalize_fx( q1_fx, &r1 );
216 97433 : QuaternionNormalize_fx( q2_fx, &r2 );
217 :
218 97433 : Word16 q_dot = 0;
219 97433 : move16();
220 97433 : cosPhi = QuaternionDotProduct_fx( r1, r2, &q_dot );
221 :
222 97433 : q_min = s_min( r1.q_fact, r2.q_fact );
223 97433 : r1.w_fx = L_shr( r1.w_fx, sub( r1.q_fact, q_min ) ); // q_min
224 97433 : move32();
225 97433 : r1.x_fx = L_shr( r1.x_fx, sub( r1.q_fact, q_min ) ); // q_min
226 97433 : move32();
227 97433 : r1.y_fx = L_shr( r1.y_fx, sub( r1.q_fact, q_min ) ); // q_min
228 97433 : move32();
229 97433 : r1.z_fx = L_shr( r1.z_fx, sub( r1.q_fact, q_min ) ); // q_min
230 97433 : move32();
231 97433 : r2.w_fx = L_shr( r2.w_fx, sub( r2.q_fact, q_min ) ); // q_min
232 97433 : move32();
233 97433 : r2.x_fx = L_shr( r2.x_fx, sub( r2.q_fact, q_min ) ); // q_min
234 97433 : move32();
235 97433 : r2.y_fx = L_shr( r2.y_fx, sub( r2.q_fact, q_min ) ); // q_min
236 97433 : move32();
237 97433 : r2.z_fx = L_shr( r2.z_fx, sub( r2.q_fact, q_min ) ); // q_min
238 97433 : move32();
239 97433 : r1.q_fact = r2.q_fact = q_min;
240 97433 : move16();
241 97433 : move16();
242 :
243 97433 : IF( cosPhi < 0 )
244 : {
245 7143 : cosPhi = L_negate( cosPhi );
246 7143 : r2.w_fx = L_negate( r2.w_fx );
247 7143 : move32();
248 7143 : r2.x_fx = L_negate( r2.x_fx );
249 7143 : move32();
250 7143 : r2.y_fx = L_negate( r2.y_fx );
251 7143 : move32();
252 7143 : r2.z_fx = L_negate( r2.z_fx );
253 7143 : move32();
254 : }
255 :
256 :
257 : /* Angle less than one degree, use linear interpolation */
258 97433 : IF( GE_32( cosPhi, L_shr( COS_ONE_TENTH_DEGREE_FX, sub( 31, q_dot ) ) ) )
259 : {
260 32375 : r_fx->w_fx = L_add( L_shr( r1.w_fx, 1 ), Mpy_32_32( L_sub( r2.w_fx, r1.w_fx ), t_fx ) ); // q_min-1
261 32375 : move32();
262 32375 : r_fx->x_fx = L_add( L_shr( r1.x_fx, 1 ), Mpy_32_32( L_sub( r2.x_fx, r1.x_fx ), t_fx ) ); // q_min-1
263 32375 : move32();
264 32375 : r_fx->y_fx = L_add( L_shr( r1.y_fx, 1 ), Mpy_32_32( L_sub( r2.y_fx, r1.y_fx ), t_fx ) ); // q_min-1
265 32375 : move32();
266 32375 : r_fx->z_fx = L_add( L_shr( r1.z_fx, 1 ), Mpy_32_32( L_sub( r2.z_fx, r1.z_fx ), t_fx ) ); // q_min-1
267 32375 : move32();
268 :
269 32375 : r_fx->q_fact = sub( q_min, 1 );
270 32375 : move16();
271 : }
272 : ELSE
273 : {
274 65058 : temp_32 = L_sub( L_shr( ONE_IN_Q31, sub( 62, shl( q_dot, 1 ) ) ), ( Mpy_32_32( cosPhi, cosPhi ) ) ); // exp: sin_e
275 65058 : sin_e = sub( 62, shl( q_dot, 1 ) );
276 65058 : sinPhi = Sqrt32( temp_32, &sin_e );
277 :
278 65058 : phi = BASOP_util_atan2( sinPhi, cosPhi, sub( sin_e, sub( 31, q_dot ) ) ); // Q13
279 :
280 65058 : temp_32 = L_shl( Mpy_32_16_1( L_sub( ONE_IN_Q30, t_fx ), phi ), 1 ); // Q29
281 :
282 65058 : temp_16 = extract_h( temp_32 ); // Q13
283 65058 : s1 = getSineWord16R2( mult( temp_16, 20860 ) ); // Q15
284 : // 2 / pi in Q15 -> 20860
285 :
286 65058 : temp_32 = L_shl( Mpy_32_16_1( t_fx, phi ), 1 ); // Q29
287 65058 : temp_16 = extract_h( temp_32 ); // Q13
288 65058 : s2 = getSineWord16R2( mult( temp_16, 20860 ) ); // Q15
289 : // 2 / pi in Q15 -> 20860
290 :
291 65058 : tmp_quat.w_fx = L_add( Mpy_32_16_1( r1.w_fx, s1 ), Mpy_32_16_1( r2.w_fx, s2 ) ); // q_min
292 65058 : move32();
293 65058 : tmp_quat.x_fx = L_add( Mpy_32_16_1( r1.x_fx, s1 ), Mpy_32_16_1( r2.x_fx, s2 ) ); // q_min
294 65058 : move32();
295 65058 : tmp_quat.y_fx = L_add( Mpy_32_16_1( r1.y_fx, s1 ), Mpy_32_16_1( r2.y_fx, s2 ) ); // q_min
296 65058 : move32();
297 65058 : tmp_quat.z_fx = L_add( Mpy_32_16_1( r1.z_fx, s1 ), Mpy_32_16_1( r2.z_fx, s2 ) ); // q_min
298 65058 : move32();
299 65058 : tmp_quat.q_fact = q_min;
300 65058 : move16();
301 :
302 65058 : QuaternionDivision_fx( tmp_quat, sinPhi, r_fx, sin_e );
303 : }
304 :
305 97433 : QuaternionNormalize_fx( *r_fx, r_fx );
306 97433 : return;
307 : }
308 :
309 : /*------------------------------------------------------------------------------------------*
310 : * QuaternionConjugate()
311 : *
312 : * Computes a quaternion conjugate
313 : *------------------------------------------------------------------------------------------*/
314 :
315 219836 : static void QuaternionConjugate_fx(
316 : const IVAS_QUATERNION q,
317 : IVAS_QUATERNION *const r )
318 : {
319 219836 : r->w_fx = q.w_fx;
320 219836 : move32();
321 219836 : r->x_fx = L_negate( q.x_fx );
322 219836 : move32();
323 219836 : r->y_fx = L_negate( q.y_fx );
324 219836 : move32();
325 219836 : r->z_fx = L_negate( q.z_fx );
326 219836 : move32();
327 219836 : r->q_fact = q.q_fact;
328 219836 : move16();
329 :
330 219836 : return;
331 : }
332 :
333 : /*------------------------------------------------------------------------------------------*
334 : * QuaternionAngle()
335 : *
336 : * Computes an angle between two quaternions
337 : *------------------------------------------------------------------------------------------*/
338 :
339 43448 : static Word32 QuaternionAngle_fx(
340 : const IVAS_QUATERNION q1,
341 : const IVAS_QUATERNION q2 )
342 : {
343 : IVAS_QUATERNION q12;
344 :
345 43448 : QuaternionConjugate_fx( q1, &q12 );
346 43448 : QuaternionProduct_fx( q12, q2, &q12 ); // q12:Q25, q2:Q29, q1: Q27//
347 :
348 43448 : Word32 temp = 0;
349 43448 : move32();
350 43448 : Word16 q_dot, result_e = 0;
351 43448 : move16();
352 43448 : temp = q12.w_fx;
353 43448 : move32();
354 43448 : q12.w_fx = 0;
355 43448 : move32();
356 43448 : Word32 result = 0;
357 43448 : move32();
358 43448 : result = QuaternionDotProduct_fx( q12, q12, &q_dot );
359 43448 : result_e = sub( 31, q_dot );
360 43448 : result = Sqrt32( result, &result_e );
361 43448 : q12.w_fx = temp;
362 43448 : move32();
363 :
364 43448 : Word16 tan_result = BASOP_util_atan2( result, q12.w_fx, sub( result_e, sub( 31, q12.q_fact ) ) );
365 43448 : result = L_deposit_h( tan_result ); // Q29
366 43448 : return result;
367 : }
368 :
369 : /*------------------------------------------------------------------------------------------*
370 : * QuaternionInverse()
371 : *
372 : * Computes an inverse quaternion
373 : *------------------------------------------------------------------------------------------*/
374 :
375 176388 : void QuaternionInverse_fx(
376 : const IVAS_QUATERNION q,
377 : IVAS_QUATERNION *const r )
378 : {
379 : Word32 dot_product;
380 176388 : Word16 dot_q = 0;
381 176388 : move16();
382 176388 : dot_product = QuaternionDotProduct_fx( q, q, &dot_q );
383 176388 : QuaternionConjugate_fx( q, r );
384 176388 : QuaternionDivision_fx( *r, dot_product, r, sub( Q31, dot_q ) );
385 :
386 176388 : return;
387 : }
388 :
389 :
390 : /*------------------------------------------------------------------------------------------*
391 : * VectorSubtract()
392 : *
393 : * Computes the difference of two vectors
394 : *------------------------------------------------------------------------------------------*/
395 :
396 4004 : static IVAS_VECTOR3 VectorSubtract_fx(
397 : const IVAS_VECTOR3 p1,
398 : const IVAS_VECTOR3 p2 )
399 : {
400 : IVAS_VECTOR3 result;
401 4004 : Word16 e_result = 0, x_qfact, y_qfact, z_qfact, q_result;
402 4004 : move16();
403 4004 : result.x_fx = BASOP_Util_Add_Mant32Exp( p1.x_fx, sub( Q31, p1.q_fact ), ( L_negate( p2.x_fx ) ), sub( Q31, p2.q_fact ), &e_result );
404 4004 : move32();
405 4004 : x_qfact = sub( 31, e_result );
406 :
407 4004 : result.y_fx = BASOP_Util_Add_Mant32Exp( p1.y_fx, sub( Q31, p1.q_fact ), ( L_negate( p2.y_fx ) ), sub( Q31, p2.q_fact ), &e_result );
408 4004 : move32();
409 4004 : y_qfact = sub( 31, e_result );
410 :
411 4004 : result.z_fx = BASOP_Util_Add_Mant32Exp( p1.z_fx, sub( Q31, p1.q_fact ), ( L_negate( p2.z_fx ) ), sub( Q31, p2.q_fact ), &e_result );
412 4004 : move32();
413 4004 : z_qfact = sub( 31, e_result );
414 :
415 4004 : q_result = sub( s_min( s_min( x_qfact, y_qfact ), z_qfact ), 1 ); // guardbit//
416 4004 : result.x_fx = L_shr( result.x_fx, sub( x_qfact, q_result ) ); // q_result
417 4004 : move32();
418 4004 : result.y_fx = L_shr( result.y_fx, sub( y_qfact, q_result ) ); // q_result
419 4004 : move32();
420 4004 : result.z_fx = L_shr( result.z_fx, sub( z_qfact, q_result ) ); // q_result
421 4004 : move32();
422 4004 : result.q_fact = q_result;
423 4004 : move16();
424 :
425 4004 : return result;
426 : }
427 :
428 :
429 : /*------------------------------------------------------------------------------------------*
430 : * VectorCrossProduct()
431 : *
432 : * Computes the cross product of two vectors
433 : *------------------------------------------------------------------------------------------*/
434 :
435 4004 : static IVAS_VECTOR3 VectorCrossProduct_fx(
436 : const IVAS_VECTOR3 p1,
437 : const IVAS_VECTOR3 p2 )
438 : {
439 : IVAS_VECTOR3 result_fx;
440 :
441 4004 : result_fx.x_fx = L_sub( Mpy_32_32( p1.y_fx, p2.z_fx ), Mpy_32_32( p1.z_fx, p2.y_fx ) ); // Q: ( p1.q_fact + p2.q_fact ) - 31
442 4004 : move32();
443 4004 : result_fx.y_fx = L_sub( Mpy_32_32( p1.z_fx, p2.x_fx ), Mpy_32_32( p1.x_fx, p2.z_fx ) ); // Q: ( p1.q_fact + p2.q_fact ) - 31
444 4004 : move32();
445 4004 : result_fx.z_fx = L_sub( Mpy_32_32( p1.x_fx, p2.y_fx ), Mpy_32_32( p1.y_fx, p2.x_fx ) ); // Q: ( p1.q_fact + p2.q_fact ) - 31
446 4004 : move32();
447 4004 : result_fx.q_fact = sub( add( p1.q_fact, p2.q_fact ), 31 );
448 4004 : move16();
449 :
450 4004 : return result_fx;
451 : }
452 :
453 : /*------------------------------------------------------------------------------------------*
454 : * VectorDotProduct(
455 : *
456 : * Computes the dot product of two vectors
457 : *------------------------------------------------------------------------------------------*/
458 :
459 4004 : static Word32 VectorDotProduct_fx(
460 : const IVAS_VECTOR3 p1,
461 : const IVAS_VECTOR3 p2,
462 : Word16 *q_fact )
463 : {
464 4004 : Word32 result_fx = 0;
465 4004 : move32();
466 :
467 4004 : result_fx = L_add( L_add( Mpy_32_32( p1.x_fx, p2.x_fx ), Mpy_32_32( p1.y_fx, p2.y_fx ) ), Mpy_32_32( p1.z_fx, p2.z_fx ) ); // // Q: ( p1.q_fact + p2.q_fact ) - 31
468 4004 : *q_fact = sub( add( p1.q_fact, p2.q_fact ), 31 );
469 4004 : move16();
470 :
471 4004 : return result_fx;
472 : }
473 :
474 : /*------------------------------------------------------------------------------------------*
475 : * VectorLength()
476 : *
477 : * Computes the length of a vector
478 : *------------------------------------------------------------------------------------------*/
479 :
480 12012 : static Word32 VectorLength_fx(
481 : IVAS_VECTOR3 p,
482 : Word16 *q_fact )
483 : {
484 12012 : Word32 result_fx = 0;
485 12012 : move32();
486 12012 : result_fx = L_add( L_add( Mpy_32_32( p.x_fx, p.x_fx ), Mpy_32_32( p.y_fx, p.y_fx ) ), Mpy_32_32( p.z_fx, p.z_fx ) ); // // Q: ( p1.q_fact + p2.q_fact ) - 31
487 :
488 12012 : *q_fact = sub( add( p.q_fact, p.q_fact ), 31 );
489 12012 : move16();
490 12012 : return result_fx;
491 : }
492 :
493 : /*------------------------------------------------------------------------------------------*
494 : * VectorNormalize()
495 : *
496 : * Normalizes a vector
497 : *------------------------------------------------------------------------------------------*/
498 :
499 8008 : static IVAS_VECTOR3 VectorNormalize_fx(
500 : const IVAS_VECTOR3 p )
501 : {
502 : IVAS_VECTOR3 result_fx;
503 : Word32 length_fx;
504 8008 : Word16 q_len, scale = 0, x_qfact, y_qfact, z_qfact, q_result;
505 8008 : move16();
506 8008 : length_fx = VectorLength_fx( p, &q_len );
507 :
508 8008 : result_fx.x_fx = BASOP_Util_Divide3232_Scale_newton( p.x_fx, length_fx, &scale );
509 8008 : move32();
510 8008 : x_qfact = sub( Q31, add( scale, sub( q_len, p.q_fact ) ) ); // e+(e1-e2)//
511 :
512 8008 : result_fx.y_fx = BASOP_Util_Divide3232_Scale_newton( p.y_fx, length_fx, &scale );
513 8008 : move32();
514 8008 : y_qfact = sub( Q31, add( scale, sub( q_len, p.q_fact ) ) );
515 :
516 8008 : result_fx.z_fx = BASOP_Util_Divide3232_Scale_newton( p.z_fx, length_fx, &scale );
517 8008 : move32();
518 8008 : z_qfact = sub( Q31, add( scale, sub( q_len, p.q_fact ) ) );
519 :
520 8008 : q_result = s_min( s_min( x_qfact, y_qfact ), z_qfact );
521 8008 : result_fx.x_fx = L_shr( result_fx.x_fx, sub( x_qfact, q_result ) ); // Q: q_result
522 8008 : move32();
523 8008 : result_fx.y_fx = L_shr( result_fx.y_fx, sub( y_qfact, q_result ) ); // Q: q_result
524 8008 : move32();
525 8008 : result_fx.z_fx = L_shr( result_fx.z_fx, sub( z_qfact, q_result ) ); // Q: q_result
526 8008 : move32();
527 8008 : result_fx.q_fact = q_result;
528 8008 : move16();
529 :
530 8008 : return result_fx;
531 : }
532 :
533 : /*------------------------------------------------------------------------------------------*
534 : * VectorRotationToQuaternion()
535 : *
536 : * Computes a quaternion representing the rotation from vector p1 to vector p2
537 : *------------------------------------------------------------------------------------------*/
538 :
539 4004 : static void VectorRotationToQuaternion_fx(
540 : const IVAS_VECTOR3 p1,
541 : const IVAS_VECTOR3 p2,
542 : IVAS_QUATERNION *const r )
543 : {
544 4004 : IVAS_VECTOR3 cross_product_fx, p1_normalized_fx = { 0 }, p2_normalized_fx = { 0 };
545 : Word32 dot_product_fx;
546 : Word16 q_dot, e_add, q_result;
547 :
548 4004 : p1_normalized_fx = VectorNormalize_fx( p1 );
549 4004 : p2_normalized_fx = VectorNormalize_fx( p2 );
550 4004 : cross_product_fx = VectorCrossProduct_fx( p1_normalized_fx, p2_normalized_fx );
551 4004 : dot_product_fx = VectorDotProduct_fx( p1_normalized_fx, p2_normalized_fx, &q_dot );
552 : // dot & cross product are same q//
553 :
554 4004 : Word32 comp_fx = -2147481472; //-0.999999f in Q31
555 4004 : move32();
556 4004 : Word16 comp_e = 0, check_flag;
557 4004 : move16();
558 4004 : IF( dot_product_fx > 0 )
559 : {
560 1984 : check_flag = 0;
561 1984 : move16();
562 : }
563 : ELSE
564 : {
565 2020 : check_flag = BASOP_Util_Cmp_Mant32Exp( comp_fx, comp_e, dot_product_fx, sub( 31, q_dot ) );
566 : }
567 4004 : IF( EQ_16( check_flag, 1 ) )
568 : {
569 20 : r->w_fx = 0;
570 20 : move32();
571 20 : r->x_fx = 0;
572 20 : move32();
573 20 : r->y_fx = 0;
574 20 : move32();
575 20 : r->z_fx = ONE_IN_Q31;
576 20 : move32();
577 20 : r->q_fact = Q31;
578 20 : move16();
579 : }
580 : ELSE
581 : {
582 : /* all regular cases */
583 3984 : r->x_fx = cross_product_fx.x_fx;
584 3984 : move32();
585 3984 : r->y_fx = cross_product_fx.y_fx;
586 3984 : move32();
587 3984 : r->z_fx = cross_product_fx.z_fx;
588 3984 : move32();
589 3984 : r->w_fx = BASOP_Util_Add_Mant32Exp( dot_product_fx, sub( 31, q_dot ), ONE_IN_Q31, 0, &e_add );
590 3984 : move32();
591 3984 : q_result = sub( s_min( sub( 31, e_add ), q_dot ), 1 ); // gaurd bits//
592 3984 : r->x_fx = L_shr( r->x_fx, sub( q_dot, q_result ) );
593 3984 : move32();
594 3984 : r->y_fx = L_shr( r->y_fx, sub( q_dot, q_result ) );
595 3984 : move32();
596 3984 : r->z_fx = L_shr( r->z_fx, sub( q_dot, q_result ) );
597 3984 : move32();
598 3984 : r->w_fx = L_shr( r->w_fx, sub( sub( 31, e_add ), q_result ) );
599 3984 : move32();
600 :
601 3984 : r->q_fact = q_result;
602 3984 : move16();
603 : }
604 4004 : QuaternionNormalize_fx( *r, r );
605 :
606 4004 : return;
607 : }
608 :
609 : /*-------------------------------------------------------------------*
610 : * ivas_orient_trk_Init()
611 : *
612 : *
613 : *-------------------------------------------------------------------*/
614 :
615 739 : ivas_error ivas_orient_trk_Init_fx(
616 : ivas_orient_trk_state_t *pOTR ) /* i/o : orientation tracker handle */
617 : {
618 : IVAS_QUATERNION identity_fx;
619 :
620 739 : IF( pOTR == NULL )
621 : {
622 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
623 : }
624 739 : identity_fx.w_fx = ONE_IN_Q31;
625 739 : move32();
626 739 : identity_fx.x_fx = identity_fx.y_fx = identity_fx.z_fx = 0;
627 739 : move32();
628 739 : move32();
629 739 : move32();
630 739 : identity_fx.q_fact = Q31;
631 739 : move16();
632 :
633 : /* configuration parameters */
634 739 : pOTR->centerAdaptationRate_fx = C_ADP_RATE_Q31;
635 739 : move32();
636 739 : pOTR->offCenterAdaptationRate_fx = OFF_C_ADP_RATE_Q31;
637 739 : move32();
638 739 : pOTR->adaptationAngle_fx = PI_OVER_4_Q29; /* Excursion angle relative to center at which maximum adaptation rate shall be applied */
639 739 : move32();
640 :
641 : /* initial adaptivity filter coefficient, will be auto-adapted */
642 : // pOTR->alpha = sinf( PI2 * pOTR->offCenterAdaptationRate / OTR_UPDATE_RATE ); /* start adaptation at off-center rate = fastest rate */
643 739 : pOTR->alpha_fx = 33731208;
644 739 : move32(); // Q31
645 739 : pOTR->Q_alpha = Q31;
646 739 : move16();
647 739 : pOTR->trkRot = identity_fx;
648 739 : pOTR->absAvgRot = identity_fx;
649 : /* Use frontal and horiontal orientation as reference orientation, unless/until overridden */
650 739 : pOTR->refRot = identity_fx;
651 :
652 : // this part still float//
653 : /* set safe default tracking mode */
654 739 : pOTR->orientation_tracking = IVAS_HEAD_ORIENT_TRK_NONE;
655 739 : move16();
656 :
657 739 : return IVAS_ERR_OK;
658 : }
659 :
660 : /*-------------------------------------------------------------------*
661 : * ivas_orient_trk_SetTrackingType()
662 : *
663 : *
664 : *-------------------------------------------------------------------*/
665 :
666 739 : ivas_error ivas_orient_trk_SetTrackingType_fx(
667 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
668 : const IVAS_HEAD_ORIENT_TRK_T orientation_tracking /* i : orientation tracking type */
669 : )
670 : {
671 739 : IF( pOTR == NULL )
672 : {
673 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
674 : }
675 :
676 739 : pOTR->orientation_tracking = orientation_tracking;
677 739 : move16();
678 :
679 739 : return IVAS_ERR_OK;
680 : }
681 :
682 : /*-------------------------------------------------------------------*
683 : * ivas_orient_trk_SetReferenceRotation()
684 : *
685 : *
686 : *-------------------------------------------------------------------*/
687 :
688 0 : ivas_error ivas_orient_trk_SetReferenceRotation_fx(
689 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
690 : const IVAS_QUATERNION refRot /* i : reference rotation */
691 : )
692 : {
693 0 : IF( pOTR == NULL )
694 : {
695 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
696 : }
697 :
698 : /* check for Euler angle signaling */
699 0 : IF( EQ_32( refRot.w_fx, -1610612736 /* -3.0f in Q29 */ ) )
700 : {
701 0 : Euler2Quat_fx( deg2rad_fx( refRot.x_fx ), deg2rad_fx( refRot.y_fx ), deg2rad_fx( refRot.z_fx ), &pOTR->refRot );
702 : }
703 0 : pOTR->refRot = refRot;
704 :
705 0 : return IVAS_ERR_OK;
706 : }
707 :
708 : /*-------------------------------------------------------------------*
709 : * ivas_orient_trk_GetMainOrientation()
710 : *
711 : *
712 : *-------------------------------------------------------------------*/
713 :
714 0 : ivas_error ivas_orient_trk_GetMainOrientation_fx(
715 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
716 : IVAS_QUATERNION *pOrientation /* i/o: average/reference orientation */
717 : )
718 : {
719 0 : test();
720 0 : IF( pOTR == NULL || pOrientation == NULL )
721 : {
722 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
723 : }
724 :
725 0 : SWITCH( pOTR->orientation_tracking )
726 : {
727 0 : case IVAS_HEAD_ORIENT_TRK_NONE:
728 0 : *pOrientation = IdentityQuaternion_fx();
729 0 : BREAK;
730 0 : case IVAS_HEAD_ORIENT_TRK_REF_VEC:
731 : case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV:
732 : case IVAS_HEAD_ORIENT_TRK_REF:
733 0 : *pOrientation = pOTR->refRot;
734 0 : BREAK;
735 0 : case IVAS_HEAD_ORIENT_TRK_AVG:
736 0 : *pOrientation = pOTR->absAvgRot;
737 0 : BREAK;
738 : }
739 :
740 0 : return IVAS_ERR_OK;
741 : }
742 :
743 : /*-------------------------------------------------------------------*
744 : * ivas_orient_trk_GetTrackedRotation()
745 : *
746 : *
747 : *-------------------------------------------------------------------*/
748 :
749 0 : ivas_error ivas_orient_trk_GetTrackedRotation_fx(
750 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
751 : IVAS_QUATERNION *pRotation /* i/o: processed rotation */
752 : )
753 : {
754 0 : test();
755 0 : IF( pOTR == NULL || pRotation == NULL )
756 : {
757 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
758 : }
759 :
760 0 : *pRotation = pOTR->trkRot;
761 :
762 0 : return IVAS_ERR_OK;
763 : }
764 :
765 : /*-------------------------------------------------------------------*
766 : * ivas_orient_trk_SetReferenceVector()
767 : *
768 : *
769 : *-------------------------------------------------------------------*/
770 :
771 4004 : ivas_error ivas_orient_trk_SetReferenceVector_fx(
772 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
773 : const IVAS_VECTOR3 listenerPos, /* i : Listener position */
774 : const IVAS_VECTOR3 refPos /* i : Reference position */
775 : )
776 : {
777 : IVAS_VECTOR3 acousticFrontVector, ivasForwardVector;
778 : IVAS_VECTOR3 listenerPosLevel, refPosLevel;
779 : Word32 acousticFrontVectorLength;
780 :
781 4004 : IF( pOTR == NULL )
782 : {
783 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
784 : }
785 :
786 4004 : SWITCH( pOTR->orientation_tracking )
787 : {
788 2002 : case IVAS_HEAD_ORIENT_TRK_NONE:
789 : case IVAS_HEAD_ORIENT_TRK_REF:
790 : case IVAS_HEAD_ORIENT_TRK_AVG:
791 : case IVAS_HEAD_ORIENT_TRK_REF_VEC:
792 2002 : acousticFrontVector = VectorSubtract_fx( listenerPos, refPos );
793 2002 : BREAK;
794 2002 : case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV:
795 : /* ignore the height difference between listener position and reference position */
796 2002 : listenerPosLevel.z_fx = refPosLevel.z_fx = listenerPos.z_fx;
797 2002 : move32();
798 2002 : listenerPosLevel.x_fx = listenerPos.x_fx;
799 2002 : move32();
800 2002 : listenerPosLevel.y_fx = listenerPos.y_fx;
801 2002 : move32();
802 2002 : listenerPosLevel.q_fact = listenerPos.q_fact;
803 2002 : move16();
804 :
805 2002 : refPosLevel.x_fx = refPos.x_fx;
806 2002 : move32();
807 2002 : refPosLevel.y_fx = refPos.y_fx;
808 2002 : move32();
809 2002 : Word16 q_min = s_min( listenerPos.q_fact, refPos.q_fact );
810 2002 : refPosLevel.x_fx = L_shr( refPosLevel.x_fx, sub( refPos.q_fact, q_min ) );
811 2002 : move32();
812 2002 : refPosLevel.y_fx = L_shr( refPosLevel.y_fx, sub( refPos.q_fact, q_min ) );
813 2002 : move32();
814 2002 : refPosLevel.z_fx = L_shr( refPosLevel.z_fx, sub( refPos.q_fact, q_min ) );
815 2002 : move32();
816 2002 : refPosLevel.q_fact = q_min;
817 2002 : move16();
818 :
819 2002 : acousticFrontVector = VectorSubtract_fx( listenerPosLevel, refPosLevel );
820 2002 : BREAK;
821 0 : default:
822 0 : return IVAS_ERR_WRONG_PARAMS;
823 : }
824 :
825 4004 : Word16 accoustic_q = acousticFrontVector.q_fact;
826 4004 : move16();
827 4004 : acousticFrontVectorLength = VectorLength_fx( acousticFrontVector, &acousticFrontVector.q_fact );
828 4004 : acousticFrontVector.q_fact = accoustic_q;
829 4004 : move16();
830 : /* if the length is zero, the user has entered insensible listener and reference positions */
831 4004 : IF( LE_32( acousticFrontVectorLength, 0 ) )
832 : {
833 0 : return IVAS_ERR_WRONG_PARAMS;
834 : }
835 :
836 4004 : ivasForwardVector.x_fx = L_negate( ONE_IN_Q31 );
837 4004 : move32();
838 4004 : ivasForwardVector.y_fx = 0;
839 4004 : move32();
840 4004 : ivasForwardVector.z_fx = 0;
841 4004 : move32();
842 4004 : ivasForwardVector.q_fact = Q31;
843 4004 : move16();
844 4004 : VectorRotationToQuaternion_fx( ivasForwardVector, acousticFrontVector, &pOTR->refRot );
845 :
846 4004 : return IVAS_ERR_OK;
847 : }
848 :
849 :
850 : /*-------------------------------------------------------------------*
851 : * ivas_orient_trk_Process()
852 : *
853 : *
854 : *-------------------------------------------------------------------*/
855 :
856 565780 : ivas_error ivas_orient_trk_Process_fx(
857 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
858 : IVAS_QUATERNION absRot, /* i : absolute head rotation */
859 : Word32 updateRate_fx, /* i : rotation update rate [Hz] */
860 : IVAS_QUATERNION *pTrkRot /* o : tracked rotation */
861 : )
862 : {
863 : ivas_error result;
864 : Word32 rateRange_fx;
865 : Word32 cutoffFrequency_fx, cutoff_prod;
866 565780 : Word16 q_cutoff_prod = 0;
867 565780 : move16();
868 565780 : Word32 alpha_fx = L_shl( pOTR->alpha_fx, sub( Q30, pOTR->Q_alpha ) ); // Q30
869 :
870 565780 : test();
871 565780 : IF( pOTR == NULL || pTrkRot == NULL )
872 : {
873 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
874 : }
875 :
876 565780 : result = IVAS_ERR_OK;
877 565780 : move32();
878 :
879 565780 : SWITCH( pOTR->orientation_tracking )
880 : {
881 506316 : case IVAS_HEAD_ORIENT_TRK_NONE:
882 506316 : pOTR->trkRot = absRot;
883 506316 : BREAK;
884 16016 : case IVAS_HEAD_ORIENT_TRK_REF:
885 : case IVAS_HEAD_ORIENT_TRK_REF_VEC:
886 : case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV:
887 : /* Reset average orientation */
888 16016 : pOTR->absAvgRot = absRot;
889 :
890 : Word16 scale_e;
891 : Word32 div;
892 16016 : div = L_deposit_h( BASOP_Util_Divide3232_Scale( pOTR->centerAdaptationRate_fx, updateRate_fx, &scale_e ) );
893 :
894 16016 : scale_e = sub( scale_e, 8 ); // e+e1-e2//
895 : // here div value is less so we can use sandwitch rule of sine//
896 16016 : pOTR->alpha_fx = div;
897 16016 : move32();
898 : /* Compute relative orientation = (absolute orientation) - (reference orientation) */
899 16016 : QuaternionInverse_fx( pOTR->refRot, &pOTR->trkRot );
900 16016 : QuaternionProduct_fx( pOTR->trkRot, absRot, &pOTR->trkRot );
901 16016 : BREAK;
902 :
903 43448 : case IVAS_HEAD_ORIENT_TRK_AVG:
904 : /* Compute average (low-pass filtered) absolute orientation */
905 43448 : QuaternionSlerp_fx( pOTR->absAvgRot, absRot, alpha_fx, &pOTR->absAvgRot );
906 :
907 : /* Compute relative orientation = (absolute orientation) - (average absolute orientation) */
908 43448 : QuaternionInverse_fx( pOTR->absAvgRot, &pOTR->trkRot );
909 43448 : Word32 angle_fx, relativeOrientationRate_fx = 0;
910 43448 : move32();
911 43448 : QuaternionProduct_fx( pOTR->trkRot, absRot, &pOTR->trkRot );
912 43448 : angle_fx = QuaternionAngle_fx( absRot, pOTR->trkRot ); // Q29
913 43448 : Word16 result_e = 0;
914 43448 : move16();
915 43448 : Word16 temp_result = BASOP_Util_Divide3232_Scale( angle_fx, pOTR->adaptationAngle_fx, &result_e );
916 43448 : relativeOrientationRate_fx = L_deposit_h( temp_result );
917 43448 : Word32 one_fx = ONE_IN_Q30;
918 43448 : move32();
919 :
920 43448 : IF( GT_32( relativeOrientationRate_fx, one_fx ) )
921 : {
922 1444 : relativeOrientationRate_fx = 1;
923 1444 : move32();
924 : }
925 :
926 : /* Compute range of the adaptation rate between center = lower rate and off-center = higher rate */
927 43448 : rateRange_fx = L_sub( pOTR->offCenterAdaptationRate_fx, pOTR->centerAdaptationRate_fx );
928 : /* 'if' assumed to perform comparison to 0 */
929 43448 : if ( 0 > rateRange_fx )
930 : {
931 0 : rateRange_fx = 0;
932 0 : move32();
933 : }
934 43448 : IF( EQ_32( relativeOrientationRate_fx, 1 ) )
935 : {
936 1444 : cutoff_prod = rateRange_fx;
937 1444 : move32();
938 1444 : q_cutoff_prod = Q31;
939 1444 : move16();
940 : }
941 : ELSE
942 : {
943 42004 : cutoff_prod = Mpy_32_32( relativeOrientationRate_fx, rateRange_fx );
944 42004 : q_cutoff_prod = add( Q31, sub( sub( 31, result_e ), 31 ) );
945 : }
946 : Word16 temp_diff;
947 43448 : temp_diff = sub( 31, q_cutoff_prod );
948 43448 : cutoff_prod = L_shl( cutoff_prod, temp_diff );
949 : /* Compute adaptivity cutoff frequency: interpolate between minimum (center) and maximum (off-center) values */
950 43448 : cutoffFrequency_fx = L_add( pOTR->centerAdaptationRate_fx, cutoff_prod ); // Q31
951 43448 : cutoff_prod = Mpy_32_32( cutoffFrequency_fx, PI2_C_Q28 );
952 43448 : q_cutoff_prod = ( ( 31 + 28 ) - 31 );
953 43448 : temp_result = BASOP_Util_Divide3232_Scale( cutoff_prod, updateRate_fx, &result_e );
954 43448 : result_e = add( result_e, sub( 23, q_cutoff_prod ) );
955 43448 : pOTR->alpha_fx = L_deposit_h( temp_result );
956 43448 : move32();
957 43448 : pOTR->Q_alpha = sub( Q31, result_e );
958 43448 : move16();
959 43448 : BREAK;
960 0 : default:
961 0 : result = IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "Non-supported orientation tracking adaptation type" );
962 0 : BREAK;
963 : }
964 :
965 565780 : if ( result == IVAS_ERR_OK )
966 : {
967 565780 : *pTrkRot = pOTR->trkRot;
968 : }
969 565780 : return result;
970 : }
|
