From 3a372d702c68d45757063b50c08d9ce2e6d627e2 Mon Sep 17 00:00:00 2001
From: Marek Szczerba <marek.szczerba@philips.com>
Date: Wed, 17 Apr 2024 11:03:01 +0200
Subject: [PATCH] Fix for QuaternionSlerp inaccuracies in corner cases

---
 lib_com/options.h          |  1 +
 lib_rend/ivas_orient_trk.c | 46 +++++++++++++++++++++++++++++++++++++-
 2 files changed, 46 insertions(+), 1 deletion(-)

diff --git a/lib_com/options.h b/lib_com/options.h
index fe344ed7ab..039f6519ae 100644
--- a/lib_com/options.h
+++ b/lib_com/options.h
@@ -177,6 +177,7 @@
 #define NONBE_FIX_1054_NEGATIVE_LVQ_INDEX               /* Nokia: issue 1054: Input to decode_comb in deindex_lvq_SHB should be positive */
 #define NONBE_FIX_1063_DIV_BY_ZERO_SUMNRG               /* VoiceAge: issue 1063: division by zero for angle_rot feature in the UNCLR classifier */
 #define NONBE_FIX_1045_ISM_BITRATE_SWITCHING            /* Eri: Difference between ROM/File HRTF in ISM bitrate switching */
+#define NONBE_FIX_1067_QUATERNIONSLERP_INACCURACIES     /* Philips: issue 1067: QuaternionSlerp inaccuracies in corner cases */
 
 /* ##################### End NON-BE switches ########################### */
 
diff --git a/lib_rend/ivas_orient_trk.c b/lib_rend/ivas_orient_trk.c
index 81cda7dd97..0e2525adb7 100644
--- a/lib_rend/ivas_orient_trk.c
+++ b/lib_rend/ivas_orient_trk.c
@@ -49,7 +49,9 @@
  *------------------------------------------------------------------------------------------*/
 
 #define OTR_UPDATE_RATE (float) FRAMES_PER_SEC /* rate of the Process() calls [Hz]; 1x per IVAS frame */
-
+#ifdef NONBE_FIX_1067_QUATERNIONSLERP_INACCURACIES
+#define COS_ONE_TENTH_DEGREE ( 0.999998476913288f )
+#endif
 
 /*------------------------------------------------------------------------------------------*
  * Local functions
@@ -157,12 +159,53 @@ void QuaternionSlerp(
     const float t,
     IVAS_QUATERNION *const r )
 {
+#ifdef NONBE_FIX_1067_QUATERNIONSLERP_INACCURACIES
+    IVAS_QUATERNION r1, r2;
+    float phi, sinPhi, cosPhi, s1, s2;
+
+    QuaternionNormalize( q1, &r1 );
+    QuaternionNormalize( q2, &r2 );
+
+    cosPhi = QuaternionDotProduct( r1, r2 );
+
+    if ( cosPhi < 0 )
+    {
+        cosPhi = -cosPhi;
+        r2.w = -r2.w;
+        r2.x = -r2.x;
+        r2.y = -r2.y;
+        r2.z = -r2.z;
+    }
+
+    /* Angle less than one degree, use linear interpolation */
+    if ( cosPhi >= COS_ONE_TENTH_DEGREE )
+    {
+        r->w = r1.w + t * ( r2.w - r1.w );
+        r->x = r1.x + t * ( r2.x - r1.x );
+        r->y = r1.y + t * ( r2.y - r1.y );
+        r->z = r1.z + t * ( r2.z - r1.z );
+    }
+    else
+    {
+        phi = acosf( cosPhi );
+        sinPhi = sinf( phi );
+
+        s1 = sinf( ( 1 - t ) * phi );
+        s2 = sinf( t * phi );
+
+        r->w = ( s1 * r1.w + s2 * r2.w ) / sinPhi;
+        r->x = ( s1 * r1.x + s2 * r2.x ) / sinPhi;
+        r->y = ( s1 * r1.y + s2 * r2.y ) / sinPhi;
+        r->z = ( s1 * r1.z + s2 * r2.z ) / sinPhi;
+    }
+#else
     float angle, denom, s, s2;
 
     s = QuaternionDotProduct( q1, q2 );
 
     if ( fabsf( s ) >= 1.0f )
     {
+
         *r = q2;
         return;
     }
@@ -177,6 +220,7 @@ void QuaternionSlerp(
     r->z = ( q1.z * s + q2.z * s2 ) / denom;
     r->w = ( q1.w * s + q2.w * s2 ) / denom;
 
+#endif
     QuaternionNormalize( *r, r );
 
     return;
-- 
GitLab