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