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.h"
37 : #include "ivas_prot_rend.h"
38 : #include "ivas_cnst.h"
39 : #ifdef DEBUGGING
40 : #include "debug.h"
41 : #endif
42 : #include <math.h>
43 : #include <assert.h>
44 : #include "wmc_auto.h"
45 :
46 :
47 : /*------------------------------------------------------------------------------------------*
48 : * Local constants
49 : *------------------------------------------------------------------------------------------*/
50 :
51 : #define OTR_UPDATE_RATE (float) FRAMES_PER_SEC /* rate of the Process() calls [Hz]; 1x per IVAS frame */
52 : #define COS_ONE_TENTH_DEGREE ( 0.999998476913288f )
53 :
54 : /*------------------------------------------------------------------------------------------*
55 : * Local functions
56 : *------------------------------------------------------------------------------------------*/
57 :
58 : /*------------------------------------------------------------------------------------------*
59 : * IdentityQuaternion()
60 : *
61 : *
62 : *------------------------------------------------------------------------------------------*/
63 :
64 0 : static IVAS_QUATERNION IdentityQuaternion(
65 : void )
66 : {
67 : IVAS_QUATERNION q;
68 :
69 0 : q.w = 1.0f;
70 0 : q.x = q.y = q.z = 0.0f;
71 :
72 0 : return q;
73 : }
74 :
75 :
76 : /*------------------------------------------------------------------------------------------*
77 : * QuaternionProduct()
78 : *
79 : * Quaternion product
80 : *------------------------------------------------------------------------------------------*/
81 :
82 8131753 : void QuaternionProduct(
83 : const IVAS_QUATERNION q1,
84 : const IVAS_QUATERNION q2,
85 : IVAS_QUATERNION *const r )
86 : {
87 : IVAS_QUATERNION tmp;
88 8131753 : tmp.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
89 8131753 : tmp.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
90 8131753 : tmp.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
91 8131753 : tmp.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
92 :
93 8131753 : *r = tmp;
94 :
95 8131753 : return;
96 : }
97 :
98 : /*------------------------------------------------------------------------------------------*
99 : * QuaternionDotProduct()
100 : *
101 : * Quaternion dot product
102 : *------------------------------------------------------------------------------------------*/
103 :
104 17940509 : static float QuaternionDotProduct(
105 : const IVAS_QUATERNION q1,
106 : const IVAS_QUATERNION q2 )
107 : {
108 17940509 : return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
109 : }
110 :
111 :
112 : /*------------------------------------------------------------------------------------------*
113 : * QuaternionDivision()
114 : *
115 : * Divides a quaternion by a scalar
116 : *------------------------------------------------------------------------------------------*/
117 :
118 15577283 : static void QuaternionDivision(
119 : const IVAS_QUATERNION q,
120 : const float d,
121 : IVAS_QUATERNION *const r )
122 : {
123 15577283 : r->w = q.w / d;
124 15577283 : r->x = q.x / d;
125 15577283 : r->y = q.y / d;
126 15577283 : r->z = q.z / d;
127 :
128 15577283 : return;
129 : }
130 :
131 :
132 : /*------------------------------------------------------------------------------------------*
133 : * QuaternionNormalize()
134 : *
135 : * Normalizes a quaternion
136 : *------------------------------------------------------------------------------------------*/
137 :
138 7839420 : static void QuaternionNormalize(
139 : const IVAS_QUATERNION q,
140 : IVAS_QUATERNION *const r )
141 : {
142 7839420 : QuaternionDivision( q, sqrtf( QuaternionDotProduct( q, q ) ), r );
143 :
144 7839420 : return;
145 : }
146 :
147 :
148 : /*------------------------------------------------------------------------------------------*
149 : * QuaternionSlerp()
150 : *
151 : * Computes a spherical linear interpolation between two quaternions
152 : *------------------------------------------------------------------------------------------*/
153 :
154 2363226 : void QuaternionSlerp(
155 : const IVAS_QUATERNION q1,
156 : const IVAS_QUATERNION q2,
157 : const float t,
158 : IVAS_QUATERNION *const r )
159 : {
160 : IVAS_QUATERNION r1, r2;
161 : float phi, sinPhi, cosPhi, s1, s2;
162 :
163 2363226 : QuaternionNormalize( q1, &r1 );
164 2363226 : QuaternionNormalize( q2, &r2 );
165 :
166 2363226 : cosPhi = QuaternionDotProduct( r1, r2 );
167 :
168 2363226 : if ( cosPhi < 0 )
169 : {
170 246417 : cosPhi = -cosPhi;
171 246417 : r2.w = -r2.w;
172 246417 : r2.x = -r2.x;
173 246417 : r2.y = -r2.y;
174 246417 : r2.z = -r2.z;
175 : }
176 :
177 : /* Angle less than one degree, use linear interpolation */
178 2363226 : if ( cosPhi >= COS_ONE_TENTH_DEGREE )
179 : {
180 1379736 : r->w = r1.w + t * ( r2.w - r1.w );
181 1379736 : r->x = r1.x + t * ( r2.x - r1.x );
182 1379736 : r->y = r1.y + t * ( r2.y - r1.y );
183 1379736 : r->z = r1.z + t * ( r2.z - r1.z );
184 : }
185 : else
186 : {
187 983490 : phi = acosf( cosPhi );
188 983490 : sinPhi = sinf( phi );
189 :
190 983490 : s1 = sinf( ( 1 - t ) * phi );
191 983490 : s2 = sinf( t * phi );
192 :
193 983490 : r->w = ( s1 * r1.w + s2 * r2.w ) / sinPhi;
194 983490 : r->x = ( s1 * r1.x + s2 * r2.x ) / sinPhi;
195 983490 : r->y = ( s1 * r1.y + s2 * r2.y ) / sinPhi;
196 983490 : r->z = ( s1 * r1.z + s2 * r2.z ) / sinPhi;
197 : }
198 2363226 : QuaternionNormalize( *r, r );
199 :
200 2363226 : return;
201 : }
202 :
203 :
204 : /*------------------------------------------------------------------------------------------*
205 : * QuaternionConjugate()
206 : *
207 : * Computes a quaternion conjugate
208 : *------------------------------------------------------------------------------------------*/
209 :
210 8922755 : static void QuaternionConjugate(
211 : const IVAS_QUATERNION q,
212 : IVAS_QUATERNION *const r )
213 : {
214 8922755 : r->w = q.w;
215 8922755 : r->x = -q.x;
216 8922755 : r->y = -q.y;
217 8922755 : r->z = -q.z;
218 :
219 8922755 : return;
220 : }
221 :
222 :
223 : /*------------------------------------------------------------------------------------------*
224 : * QuaternionAngle()
225 : *
226 : * Computes an angle between two quaternions
227 : *------------------------------------------------------------------------------------------*/
228 :
229 1184892 : static float QuaternionAngle(
230 : const IVAS_QUATERNION q1,
231 : const IVAS_QUATERNION q2 )
232 : {
233 : IVAS_QUATERNION q12;
234 : float angle;
235 :
236 1184892 : QuaternionConjugate( q1, &q12 );
237 1184892 : QuaternionProduct( q12, q2, &q12 );
238 1184892 : angle = 2.0f * atan2f( sqrtf( q12.x * q12.x + q12.y * q12.y + q12.z * q12.z ), q12.w );
239 :
240 1184892 : return angle;
241 : }
242 :
243 :
244 : /*------------------------------------------------------------------------------------------*
245 : * QuaternionInverse()
246 : *
247 : * Computes an inverse quaternion
248 : *------------------------------------------------------------------------------------------*/
249 :
250 7737863 : void QuaternionInverse(
251 : const IVAS_QUATERNION q,
252 : IVAS_QUATERNION *const r )
253 : {
254 : float dot_product;
255 :
256 7737863 : dot_product = QuaternionDotProduct( q, q );
257 7737863 : QuaternionConjugate( q, r );
258 7737863 : QuaternionDivision( *r, dot_product, r );
259 :
260 7737863 : return;
261 : }
262 :
263 :
264 : /*------------------------------------------------------------------------------------------*
265 : * VectorSubtract()
266 : *
267 : * Computes the difference of two vectors
268 : *------------------------------------------------------------------------------------------*/
269 :
270 749742 : static IVAS_VECTOR3 VectorSubtract(
271 : const IVAS_VECTOR3 p1,
272 : const IVAS_VECTOR3 p2 )
273 : {
274 : IVAS_VECTOR3 result;
275 :
276 749742 : result.x = p1.x - p2.x;
277 749742 : result.y = p1.y - p2.y;
278 749742 : result.z = p1.z - p2.z;
279 :
280 749742 : return result;
281 : }
282 :
283 :
284 : /*------------------------------------------------------------------------------------------*
285 : * VectorCrossProduct()
286 : *
287 : * Computes the cross product of two vectors
288 : *------------------------------------------------------------------------------------------*/
289 :
290 749742 : static IVAS_VECTOR3 VectorCrossProduct(
291 : const IVAS_VECTOR3 p1,
292 : const IVAS_VECTOR3 p2 )
293 : {
294 : IVAS_VECTOR3 result;
295 749742 : result.x = p1.y * p2.z - p1.z * p2.y;
296 749742 : result.y = p1.z * p2.x - p1.x * p2.z;
297 749742 : result.z = p1.x * p2.y - p1.y * p2.x;
298 :
299 749742 : return result;
300 : }
301 :
302 :
303 : /*------------------------------------------------------------------------------------------*
304 : * VectorDotProduct(
305 : *
306 : * Computes the dot product of two vectors
307 : *------------------------------------------------------------------------------------------*/
308 :
309 749742 : static float VectorDotProduct(
310 : const IVAS_VECTOR3 p1,
311 : const IVAS_VECTOR3 p2 )
312 : {
313 749742 : return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z;
314 : }
315 :
316 :
317 : /*------------------------------------------------------------------------------------------*
318 : * VectorLength()
319 : *
320 : * Computes the length of a vector
321 : *------------------------------------------------------------------------------------------*/
322 :
323 2249226 : static float VectorLength(
324 : const IVAS_VECTOR3 p )
325 : {
326 2249226 : return sqrtf( p.x * p.x + p.y * p.y + p.z * p.z );
327 : }
328 :
329 :
330 : /*------------------------------------------------------------------------------------------*
331 : * VectorNormalize()
332 : *
333 : * Normalizes a vector
334 : *------------------------------------------------------------------------------------------*/
335 :
336 1499484 : static IVAS_VECTOR3 VectorNormalize(
337 : const IVAS_VECTOR3 p )
338 : {
339 : IVAS_VECTOR3 result;
340 :
341 1499484 : const float length = VectorLength( p );
342 :
343 1499484 : result.x = p.x / length;
344 1499484 : result.y = p.y / length;
345 1499484 : result.z = p.z / length;
346 :
347 1499484 : return result;
348 : }
349 :
350 :
351 : /*------------------------------------------------------------------------------------------*
352 : * VectorRotationToQuaternion()
353 : *
354 : * Computes a quaternion representing the rotation from vector p1 to vector p2
355 : *------------------------------------------------------------------------------------------*/
356 :
357 749742 : static void VectorRotationToQuaternion(
358 : const IVAS_VECTOR3 p1,
359 : const IVAS_VECTOR3 p2,
360 : IVAS_QUATERNION *const r )
361 : {
362 : float dot_product;
363 : IVAS_VECTOR3 cross_product, p1_normalized, p2_normalized;
364 :
365 749742 : p1_normalized = VectorNormalize( p1 );
366 749742 : p2_normalized = VectorNormalize( p2 );
367 749742 : cross_product = VectorCrossProduct( p1_normalized, p2_normalized );
368 749742 : dot_product = VectorDotProduct( p1_normalized, p2_normalized );
369 :
370 749742 : if ( dot_product < -0.999999 )
371 : {
372 : /* happens when the p1 vector is parallel to p2, but direction is flipped */
373 1206 : r->w = 0.0f;
374 1206 : r->x = 0.0f;
375 1206 : r->y = 0.0f;
376 1206 : r->z = 1.0f;
377 : }
378 : else
379 : {
380 : /* all regular cases */
381 748536 : r->x = cross_product.x;
382 748536 : r->y = cross_product.y;
383 748536 : r->z = cross_product.z;
384 748536 : r->w = 1.0f + dot_product;
385 : }
386 :
387 749742 : QuaternionNormalize( *r, r );
388 :
389 749742 : return;
390 : }
391 :
392 : /*-------------------------------------------------------------------*
393 : * ivas_orient_trk_Init()
394 : *
395 : *
396 : *-------------------------------------------------------------------*/
397 :
398 1077 : ivas_error ivas_orient_trk_Init(
399 : ivas_orient_trk_state_t *pOTR ) /* i/o : orientation tracker handle */
400 : {
401 : IVAS_QUATERNION identity;
402 :
403 1077 : if ( pOTR == NULL )
404 : {
405 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
406 : }
407 :
408 1077 : identity.w = 1.0f;
409 1077 : identity.x = identity.y = identity.z = 0.0f;
410 :
411 : /* configuration parameters */
412 1077 : pOTR->centerAdaptationRate = 1.0f / 30.0f;
413 1077 : pOTR->offCenterAdaptationRate = 1.0f / 8.0f;
414 1077 : pOTR->adaptationAngle = PI_OVER_2; /* Excursion angle relative to center at which maximum adaptation rate shall be applied */
415 :
416 : /* initial adaptivity filter coefficient, will be auto-adapted */
417 1077 : pOTR->alpha = sinf( PI2 * pOTR->offCenterAdaptationRate / OTR_UPDATE_RATE ); /* start adaptation at off-center rate = fastest rate */
418 :
419 1077 : pOTR->trkRot = identity;
420 1077 : pOTR->absAvgRot = identity;
421 : /* Use frontal and horiontal orientation as reference orientation, unless/until overridden */
422 1077 : pOTR->refRot = identity;
423 :
424 : /* set safe default tracking mode */
425 1077 : pOTR->orientation_tracking = IVAS_HEAD_ORIENT_TRK_NONE;
426 :
427 1077 : return IVAS_ERR_OK;
428 : }
429 :
430 :
431 : /*-------------------------------------------------------------------*
432 : * ivas_orient_trk_SetTrackingType()
433 : *
434 : *
435 : *-------------------------------------------------------------------*/
436 :
437 1077 : ivas_error ivas_orient_trk_SetTrackingType(
438 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
439 : const IVAS_HEAD_ORIENT_TRK_T orientation_tracking /* i : orientation tracking type */
440 : )
441 : {
442 1077 : if ( pOTR == NULL )
443 : {
444 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
445 : }
446 :
447 1077 : pOTR->orientation_tracking = orientation_tracking;
448 :
449 1077 : return IVAS_ERR_OK;
450 : }
451 :
452 : /*-------------------------------------------------------------------*
453 : * ivas_orient_trk_SetReferenceRotation()
454 : *
455 : *
456 : *-------------------------------------------------------------------*/
457 :
458 144864 : ivas_error ivas_orient_trk_SetReferenceRotation(
459 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
460 : const IVAS_QUATERNION refRot /* i : reference rotation */
461 : )
462 : {
463 144864 : if ( pOTR == NULL )
464 : {
465 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
466 : }
467 :
468 : /* check for Euler angle signaling */
469 144864 : if ( refRot.w == -3.0f )
470 : {
471 0 : Euler2Quat( deg2rad( refRot.x ), deg2rad( refRot.y ), deg2rad( refRot.z ), &pOTR->refRot );
472 : }
473 : else
474 : {
475 144864 : pOTR->refRot = refRot;
476 : }
477 :
478 144864 : return IVAS_ERR_OK;
479 : }
480 :
481 :
482 : /*-------------------------------------------------------------------*
483 : * ivas_orient_trk_GetMainOrientation()
484 : *
485 : *
486 : *-------------------------------------------------------------------*/
487 :
488 0 : ivas_error ivas_orient_trk_GetMainOrientation(
489 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
490 : IVAS_QUATERNION *pOrientation /* i/o: average/reference orientation */
491 : )
492 : {
493 0 : if ( pOTR == NULL || pOrientation == NULL )
494 : {
495 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
496 : }
497 :
498 0 : switch ( pOTR->orientation_tracking )
499 : {
500 0 : case IVAS_HEAD_ORIENT_TRK_NONE:
501 0 : *pOrientation = IdentityQuaternion();
502 0 : break;
503 0 : case IVAS_HEAD_ORIENT_TRK_REF_VEC:
504 : case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV:
505 : case IVAS_HEAD_ORIENT_TRK_REF:
506 0 : *pOrientation = pOTR->refRot;
507 0 : break;
508 0 : case IVAS_HEAD_ORIENT_TRK_AVG:
509 0 : *pOrientation = pOTR->absAvgRot;
510 0 : break;
511 : }
512 :
513 0 : return IVAS_ERR_OK;
514 : }
515 :
516 :
517 : /*-------------------------------------------------------------------*
518 : * ivas_orient_trk_GetTrackedRotation()
519 : *
520 : *
521 : *-------------------------------------------------------------------*/
522 :
523 0 : ivas_error ivas_orient_trk_GetTrackedRotation(
524 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
525 : IVAS_QUATERNION *pRotation /* i/o: processed rotation */
526 : )
527 : {
528 0 : if ( pOTR == NULL || pRotation == NULL )
529 : {
530 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
531 : }
532 :
533 0 : *pRotation = pOTR->trkRot;
534 :
535 0 : return IVAS_ERR_OK;
536 : }
537 :
538 :
539 : /*-------------------------------------------------------------------*
540 : * ivas_orient_trk_SetReferenceVector()
541 : *
542 : *
543 : *-------------------------------------------------------------------*/
544 :
545 749742 : ivas_error ivas_orient_trk_SetReferenceVector(
546 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
547 : const IVAS_VECTOR3 listenerPos, /* i : Listener position */
548 : const IVAS_VECTOR3 refPos /* i : Reference position */
549 : )
550 : {
551 : IVAS_VECTOR3 acousticFrontVector, ivasForwardVector;
552 : IVAS_VECTOR3 listenerPosLevel, refPosLevel;
553 : float acousticFrontVectorLength;
554 :
555 749742 : if ( pOTR == NULL )
556 : {
557 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
558 : }
559 :
560 749742 : switch ( pOTR->orientation_tracking )
561 : {
562 629448 : case IVAS_HEAD_ORIENT_TRK_NONE:
563 : case IVAS_HEAD_ORIENT_TRK_REF:
564 : case IVAS_HEAD_ORIENT_TRK_AVG:
565 : case IVAS_HEAD_ORIENT_TRK_REF_VEC:
566 629448 : acousticFrontVector = VectorSubtract( refPos, listenerPos );
567 629448 : break;
568 120294 : case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV:
569 : /* ignore the height difference between listener position and reference position */
570 120294 : listenerPosLevel.z = refPosLevel.z = listenerPos.z;
571 120294 : listenerPosLevel.x = listenerPos.x;
572 120294 : listenerPosLevel.y = listenerPos.y;
573 120294 : refPosLevel.x = refPos.x;
574 120294 : refPosLevel.y = refPos.y;
575 120294 : acousticFrontVector = VectorSubtract( refPosLevel, listenerPosLevel );
576 120294 : break;
577 0 : default:
578 0 : return IVAS_ERR_WRONG_PARAMS;
579 : }
580 :
581 749742 : acousticFrontVectorLength = VectorLength( acousticFrontVector );
582 :
583 : /* if the length is zero, the user has entered insensible listener and reference positions */
584 749742 : if ( acousticFrontVectorLength < 0.0001f )
585 : {
586 0 : return IVAS_ERR_WRONG_PARAMS;
587 : }
588 :
589 749742 : ivasForwardVector.x = 1.0f;
590 749742 : ivasForwardVector.y = 0.0f;
591 749742 : ivasForwardVector.z = 0.0f;
592 749742 : VectorRotationToQuaternion( ivasForwardVector, acousticFrontVector, &pOTR->refRot );
593 :
594 749742 : return IVAS_ERR_OK;
595 : }
596 :
597 :
598 : /*-------------------------------------------------------------------*
599 : * ivas_orient_trk_Process()
600 : *
601 : *
602 : *-------------------------------------------------------------------*/
603 :
604 33208445 : ivas_error ivas_orient_trk_Process(
605 : ivas_orient_trk_state_t *pOTR, /* i/o: orientation tracker handle */
606 : IVAS_QUATERNION absRot, /* i : absolute head rotation */
607 : float updateRate, /* i : rotation update rate [Hz] */
608 : IVAS_QUATERNION *pTrkRot /* o : tracked rotation */
609 : )
610 : {
611 : float normalizedOrientation;
612 : float relativeOrientationRate;
613 : float rateRange;
614 : float cutoffFrequency;
615 33208445 : float alpha = pOTR->alpha;
616 : float ang;
617 : ivas_error result;
618 :
619 33208445 : if ( pOTR == NULL || pTrkRot == NULL )
620 : {
621 0 : return IVAS_ERR_UNEXPECTED_NULL_POINTER;
622 : }
623 :
624 33208445 : result = IVAS_ERR_OK;
625 :
626 33208445 : switch ( pOTR->orientation_tracking )
627 : {
628 28445239 : case IVAS_HEAD_ORIENT_TRK_NONE:
629 28445239 : pOTR->trkRot = absRot;
630 28445239 : break;
631 3578314 : case IVAS_HEAD_ORIENT_TRK_REF:
632 : case IVAS_HEAD_ORIENT_TRK_REF_VEC:
633 : case IVAS_HEAD_ORIENT_TRK_REF_VEC_LEV:
634 : /* Reset average orientation */
635 3578314 : pOTR->absAvgRot = absRot;
636 :
637 : /* Reset adaptation filter - start adaptation at center rate */
638 3578314 : pOTR->alpha = sinf( 2.0f * EVS_PI * pOTR->centerAdaptationRate / updateRate );
639 :
640 : /* Compute relative orientation = (absolute orientation) - (reference orientation) */
641 3578314 : QuaternionInverse( pOTR->refRot, &pOTR->trkRot );
642 3578314 : QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot );
643 3578314 : break;
644 :
645 1184892 : case IVAS_HEAD_ORIENT_TRK_AVG:
646 : /* Compute average (low-pass filtered) absolute orientation */
647 1184892 : QuaternionSlerp( pOTR->absAvgRot, absRot, alpha, &pOTR->absAvgRot );
648 :
649 : /* Compute relative orientation = (absolute orientation) - (average absolute orientation) */
650 1184892 : QuaternionInverse( pOTR->absAvgRot, &pOTR->trkRot );
651 1184892 : QuaternionProduct( pOTR->trkRot, absRot, &pOTR->trkRot );
652 :
653 : /* Adapt LPF constant based on orientation excursion relative to current mean:
654 : - low cutoff (slow adaptation) for small excursions (around center)
655 : - high cutoff (fast adaptation) for large excursions (off-center)
656 : */
657 1184892 : ang = QuaternionAngle( absRot, pOTR->trkRot );
658 1184892 : normalizedOrientation = ang * ang;
659 :
660 1184892 : relativeOrientationRate = sqrtf( normalizedOrientation ) / pOTR->adaptationAngle;
661 : /* 'if' assumed to perform comparison to 0 */
662 1184892 : if ( relativeOrientationRate > 1.0f )
663 : {
664 0 : relativeOrientationRate = 1.0f;
665 : }
666 :
667 : /* Compute range of the adaptation rate between center = lower rate and off-center = higher rate */
668 1184892 : rateRange = pOTR->offCenterAdaptationRate - pOTR->centerAdaptationRate;
669 : /* 'if' assumed to perform comparison to 0 */
670 1184892 : if ( rateRange < 0.0f )
671 : {
672 0 : rateRange = 0.0f;
673 : }
674 :
675 : /* Compute adaptivity cutoff frequency: interpolate between minimum (center) and maximum (off-center) values */
676 1184892 : cutoffFrequency = pOTR->centerAdaptationRate + ( relativeOrientationRate * rateRange );
677 :
678 : /* Compute filter coefficient corresponding to desired cutoff frequency */
679 1184892 : pOTR->alpha = sinf( 2.0f * EVS_PI * cutoffFrequency / updateRate );
680 1184892 : break;
681 0 : default:
682 0 : result = IVAS_ERROR( IVAS_ERR_INTERNAL_FATAL, "Non-supported orientation tracking adaptation type" );
683 0 : break;
684 : }
685 :
686 33208445 : if ( result == IVAS_ERR_OK )
687 : {
688 33208445 : *pTrkRot = pOTR->trkRot;
689 : }
690 :
691 33208445 : return result;
692 : }
|
