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 "ivas_cnst.h"
      34             : #include <assert.h>
      35             : #include <stdint.h>
      36             : #include "options.h"
      37             : #include <math.h>
      38             : #include "cnst.h"
      39             : #include "prot.h"
      40             : #include "ivas_prot.h"
      41             : #ifdef DEBUGGING
      42             : #include "debug.h"
      43             : #endif
      44             : #include "wmc_auto.h"
      45             : 
      46             : /*-------------------------------------------------------------------------
      47             :  * Euler2Quat()
      48             :  *
      49             :  * Calculate corresponding Quaternion from Euler angles in radians
      50             :  *------------------------------------------------------------------------*/
      51             : 
      52           0 : void Euler2Quat(
      53             :     const float yaw,      /* i  : yaw   (x)                           */
      54             :     const float pitch,    /* i  : pitch (y)                           */
      55             :     const float roll,     /* i  : roll  (z)                           */
      56             :     IVAS_QUATERNION *quat /* o  : quaternion describing the rotation  */
      57             : )
      58             : {
      59           0 :     float cr = cosf( roll * 0.5f );
      60           0 :     float sr = sinf( roll * 0.5f );
      61           0 :     float cp = cosf( pitch * 0.5f );
      62           0 :     float sp = sinf( pitch * 0.5f );
      63           0 :     float cy = cosf( yaw * 0.5f );
      64           0 :     float sy = sinf( yaw * 0.5f );
      65           0 :     quat->w = cr * cp * cy + sr * sp * sy;
      66           0 :     quat->x = sr * cp * cy - cr * sp * sy;
      67           0 :     quat->y = sr * cp * sy + cr * sp * cy;
      68           0 :     quat->z = cr * cp * sy - sr * sp * cy;
      69             : 
      70           0 :     return;
      71             : }
      72             : 
      73             : 
      74             : /*-------------------------------------------------------------------------
      75             :  * Quat2EulerDegree()
      76             :  *
      77             :  * Quaternion handling: calculate corresponding Euler angles in degrees
      78             :  *------------------------------------------------------------------------*/
      79             : 
      80           0 : void Quat2EulerDegree(
      81             :     const IVAS_QUATERNION quat, /* i  : quaternion describing the rotation             */
      82             :     float *yaw,                 /* o  : yaw                                            */
      83             :     float *pitch,               /* o  : pitch                                          */
      84             :     float *roll                 /* o  : roll                                           */
      85             : )
      86             : {
      87           0 :     if ( quat.w != -3.0 )
      88             :     {
      89             :         float p;
      90           0 :         *yaw = atan2f( 2 * ( quat.w * quat.x + quat.y * quat.z ), 1 - 2 * ( quat.x * quat.x + quat.y * quat.y ) );
      91           0 :         p = 2 * ( quat.w * quat.y - quat.z * quat.x );
      92           0 :         p = max( -1.0f, min( 1.0f, p ) );
      93           0 :         *pitch = asinf( p );
      94           0 :         *roll = atan2f( 2 * ( quat.w * quat.z + quat.x * quat.y ), 1 - 2 * ( quat.y * quat.y + quat.z * quat.z ) );
      95           0 :         *yaw *= _180_OVER_PI;
      96           0 :         *pitch *= _180_OVER_PI;
      97           0 :         *roll *= _180_OVER_PI;
      98             :     }
      99             :     else
     100             :     {
     101             :         /* Euler angles in R_X(roll)*R_Y(pitch)*R_Z(yaw) convention
     102             :          *
     103             :          *  yaw:   rotate scene counter-clockwise in the horizontal plane
     104             :          *  pitch: rotate scene in the median plane, increase elevation with positive values
     105             :          *  roll:  rotate scene from the right ear to the top
     106             :          */
     107           0 :         *yaw = quat.z;
     108           0 :         *pitch = quat.y;
     109           0 :         *roll = quat.x;
     110             :     }
     111             : 
     112           0 :     return;
     113             : }
     114             : 
     115             : 
     116             : /*-------------------------------------------------------------------------
     117             :  * deg2rad()
     118             :  *
     119             :  * Converts degrees to normalized radians
     120             :  *------------------------------------------------------------------------*/
     121             : 
     122        3648 : float deg2rad(
     123             :     float degrees )
     124             : {
     125        3648 :     while ( degrees >= 180.0f )
     126             :     {
     127           0 :         degrees = degrees - 360.0f;
     128             :     }
     129        3648 :     while ( degrees <= -180.0f )
     130             :     {
     131           0 :         degrees = degrees + 360.0f;
     132             :     }
     133             : 
     134        3648 :     return PI_OVER_180 * degrees;
     135             : }