Commit 0d0f0325 authored by TYAGIRIS's avatar TYAGIRIS
Browse files

euler to quat fix

parent ecd7c788

lib_com/options.h

+1 −0
Original line number Diff line number Diff line
@@ -203,6 +203,7 @@ 
/*CLDFB CODEC SWITCHES -- END*/



#define REND_STATIC_MEM_OPT

#define EUALER2QUAT_FIX



#endif

lib_rend/ivas_rotation.c

+20 −2
Original line number Diff line number Diff line
@@ -192,7 +192,6 @@ void QuatToRotMat( 
 *

 * Calculate corresponding Quaternion from Euler angles in radians

 *------------------------------------------------------------------------*/



void Euler2Quat(

    const float yaw,      /* i  : yaw   (x)                           */

    const float pitch,    /* i  : pitch (y)                           */
@@ -203,14 +202,24 @@ void Euler2Quat( 
    float cr = cosf( roll * 0.5f );

    float sr = sinf( roll * 0.5f );

    float cp = cosf( pitch * 0.5f );

#ifdef EUALER2QUAT_FIX

    float sp = sinf( pitch * 0.5f );

#else

    float sp = sinf( -pitch * 0.5f );

#endif

    float cy = cosf( yaw * 0.5f );

    float sy = sinf( yaw * 0.5f );



#ifdef EUALER2QUAT_FIX

    quat->w = cr * cp * cy + sr * sp * sy;

    quat->x = sr * cp * cy - cr * sp * sy;

    quat->y = sr * cp * sy + cr * sp * cy;

    quat->z = cr * cp * sy - sr * sp * cy;

#else

    quat->w = cr * cp * cy - sr * sp * sy;

    quat->x = sr * cp * cy + cr * sp * sy;

    quat->y = cr * sp * cy - sr * cp * sy;

    quat->z = cr * cp * sy + sr * sp * cy;

#endif



    return;

}
@@ -231,8 +240,17 @@ void Quat2EulerDegree( 
{

    if ( quat.w != -3.0 )

    {

#ifdef EUALER2QUAT_FIX

        float p;

#endif

        *yaw = atan2f( 2 * ( quat.w * quat.x + quat.y * quat.z ), 1 - 2 * ( quat.x * quat.x + quat.y * quat.y ) );

#ifdef EUALER2QUAT_FIX

        p = 2 * ( quat.w * quat.y - quat.z * quat.x );

        p = max( -1.0f, min( 1.0f, p ) );

        *pitch = asinf( p );

#else

        *pitch = asinf( 2 * ( quat.w * quat.y - quat.z * quat.x ) );

#endif

        *roll = atan2f( 2 * ( quat.w * quat.z + quat.x * quat.y ), 1 - 2 * ( quat.y * quat.y + quat.z * quat.z ) );

        *yaw *= _180_OVER_PI;

        *pitch *= _180_OVER_PI;