Commit 0ea48661 authored by stoutjesdijk's avatar stoutjesdijk 🎧
Browse files

Euler processing correct in degrees 

- all internal processign in ivas_orient_trk_Process() now done in Quat
- output in Eulers in quasi-quaternion format when necessary
- Euler angles assumed in degrees, conversion to and from radians added
parent 0d67d76c

lib_rend/ivas_orient_trk.c

+29 −11
Original line number Diff line number Diff line
@@ -457,7 +457,7 @@ ivas_error ivas_orient_trk_Process( 
    float alpha = pOTR->alpha;

    float ang;

    ivas_error result;

    IVAS_QUATERNION refQuat, trkQuat;

    IVAS_QUATERNION refQuat, absQuat, trkQuat;

    IVAS_QUATERNION trkEuler;



    if ( pOTR == NULL || pTrkRot == NULL )
@@ -472,13 +472,27 @@ ivas_error ivas_orient_trk_Process( 
    /* check for Euler angle signaling */

    if ( pOTR->refRot.w == -3.0f )

    {

        Euler2Quat(pOTR->refRot.x, pOTR->refRot.y, pOTR->refRot.z, &refQuat);

        Euler2Quat(deg2rad( pOTR->refRot.x ), deg2rad( pOTR->refRot.y ), deg2rad( pOTR->refRot.z ), &refQuat);

    }

    else

    {

        refQuat = pOTR->refRot;

    }



    absQuat.w = 0.0f;

    absQuat.x = 0.0f;

    absQuat.y = 0.0f;

    absQuat.z = 0.0f;

    /* check for Euler angle signaling */

    if ( pAbsRot->w == -3.0f )

    {

        Euler2Quat(deg2rad( pAbsRot->x ), deg2rad( pAbsRot->y ), deg2rad( pAbsRot->z ), &absQuat);

    }

    else

    {

        absQuat = *pAbsRot;

    }



    trkQuat.w = 0.0f;

    trkQuat.x = 0.0f;

    trkQuat.y = 0.0f;
@@ -486,7 +500,7 @@ ivas_error ivas_orient_trk_Process( 
    /* check for Euler angle signaling */

    if ( pTrkRot->w == -3.0f )

    {

        Euler2Quat(pTrkRot->x, pTrkRot->y, pTrkRot->z, &trkQuat);

        Euler2Quat(deg2rad( pTrkRot->x ), deg2rad( pTrkRot->y ), deg2rad( pTrkRot->z ), &trkQuat);

    }

    else

    {
@@ -498,34 +512,34 @@ ivas_error ivas_orient_trk_Process( 
    switch ( pOTR->trackingType )

    {

        case OTR_TRACKING_NONE:

            trkQuat = *pAbsRot;

            trkQuat = absQuat;

            break;



        case OTR_TRACKING_REF_ORIENT:

            /* Reset average orientation   */

            pOTR->absAvgRot = *pAbsRot;

            pOTR->absAvgRot = absQuat;



            /* Reset adaptation filter - start adaptation at center rate */

            pOTR->alpha = sinf( 2.0f * EVS_PI * pOTR->centerAdaptationRate / updateRate );



            /* Compute relative orientation = (absolute orientation) - (reference orientation) */

            QuaternionInverse( refQuat, &trkQuat );

            QuaternionProduct( trkQuat, *pAbsRot, &trkQuat );

            QuaternionProduct( trkQuat, absQuat, &trkQuat );

            break;



        case OTR_TRACKING_AVG_ORIENT:

            /* Compute average (low-pass filtered) absolute orientation */

            QuaternionSlerp( pOTR->absAvgRot, *pAbsRot, alpha, &pOTR->absAvgRot );

            QuaternionSlerp( pOTR->absAvgRot, absQuat, alpha, &pOTR->absAvgRot );



            /* Compute relative orientation = (absolute orientation) - (average absolute orientation) */

            QuaternionInverse( pOTR->absAvgRot, &trkQuat );

            QuaternionProduct( trkQuat, *pAbsRot, &trkQuat );

            QuaternionProduct( trkQuat, absQuat, &trkQuat );



            /* Adapt LPF constant based on orientation excursion relative to current mean:

            - low  cutoff (slow adaptation) for small excursions (around center)

            - high cutoff (fast adaptation) for large excursions (off-center)

            */

            ang = QuaternionAngle( *pAbsRot, *pTrkRot );

            ang = QuaternionAngle( absQuat, *pTrkRot );

            normalizedOrientation = ang * ang;



            relativeOrientationRate = sqrtf( normalizedOrientation ) / pOTR->adaptationAngle;
@@ -558,10 +572,14 @@ ivas_error ivas_orient_trk_Process( 
    if ( result == IVAS_ERR_OK )

    {

        pOTR->trkRot = trkQuat;

        pOTR->refRot = refQuat;



        if ( pTrkRot->w == -3.0f )

        if ( ( pTrkRot->w == -3.0f) || ( pAbsRot->w == -3.0f ) )

        {

            Quat2Euler(trkQuat, &trkEuler.x, &trkEuler.y, &trkEuler.z);

            Quat2Euler( trkQuat, &trkEuler.z, &trkEuler.y, &trkEuler.x );

            trkEuler.x = rad2deg( trkEuler.x );

            trkEuler.y = rad2deg( trkEuler.y );

            trkEuler.z = rad2deg( trkEuler.z );

            trkEuler.w = -3.0f;

            *pTrkRot = trkEuler;

        }

lib_rend/ivas_prot_rend.h

+4 −0
Original line number Diff line number Diff line
@@ -810,6 +810,10 @@ void Euler2Quat( 
    const float roll,                                           /* i  : roll                                            */

    IVAS_QUATERNION *quat                                       /* o  : quaternion describing the rotation              */

);



float deg2rad( float degrees );



float rad2deg( float radians );

#endif



void QuatToRotMat(

lib_rend/ivas_rotation.c

+39 −14
Original line number Diff line number Diff line
@@ -127,7 +127,11 @@ void QuatToRotMat( 
     *  Euler angles instead of quaternions. In this case, all the w values must

     *  be set to -3.0 in the trajectory file to signal switching to Euler angles.

     *  The x,y, and z components of the quaternion are then interpreted as

#ifdef FIX_I109_ORIENTATION_TRACKING

     *  yaw-pitch-roll.

#else // see below: "Euler angles in R_X(roll)*R_Y(pitch)*R_Z(yaw) convention"

     *  roll-pitch-yaw.

#endif

     */

    if ( quat.w != -3.0 )

    {
@@ -179,21 +183,27 @@ void QuatToRotMat( 
/*-------------------------------------------------------------------------

 * Quat2Euler()

 *

 * Quaternion handling: calculate corresponding Euler angles

 * Quaternion handling: calculate corresponding Euler angles in radians

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



void Quat2Euler(

    const IVAS_QUATERNION quat, /* i  : quaternion describing the rotation  */

    float *yaw,                 /* o  : yaw                                 */

    float *pitch,               /* o  : pitch                               */

    float *roll                 /* o  : roll                                */

    float *yaw,                 /* o  : yaw   (z)                           */

    float *pitch,               /* o  : pitch (y)                           */

    float *roll                 /* o  : roll  (x)                           */

)

{

    if ( quat.w != -3.0 )

    {

#ifdef FIX_I109_ORIENTATION_TRACKING

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

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

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

#else // bug: x and z swapped

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

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

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

#endif

    }

    else

    {
@@ -203,15 +213,9 @@ void Quat2Euler( 
         *  pitch: rotate scene in the median plane, increase elevation with positive values

         *  roll:  rotate scene from the right ear to the top

         */

#ifdef FIX_I109_ORIENTATION_TRACKING

        *yaw   = quat.x;

        *pitch = quat.y;

        *roll  = quat.z;

#else

        *yaw   = quat.z;

        *pitch = quat.y;

        *roll  = quat.x;

#endif

    }



    return;
@@ -221,13 +225,13 @@ void Quat2Euler( 
/*-------------------------------------------------------------------------

 * Euler2Quat()

 *

 * Calculate corresponding Quaternion from Euler angles

 * Calculate corresponding Quaternion from Euler angles in radians

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



void Euler2Quat(

    const float yaw,            /* i  : yaw                                 */

    const float pitch,          /* i  : pitch                               */

    const float roll,           /* i  : roll                                */

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

    const float pitch,          /* i  : pitch (y)                           */

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

    IVAS_QUATERNION *quat       /* o  : quaternion describing the rotation  */

)

{
@@ -245,6 +249,27 @@ void Euler2Quat( 


    return;

}



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

 * rad2deg()

 *

 * Return angle in radians from degrees

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

float deg2rad( float degrees )

{

    return PI2 * degrees / 360.0f;

}



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

 * deg2rad()

 *

 * Return angle in degrees from radians

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

float rad2deg( float radians )

{

    return 360.0f * radians / PI2;

}



#endif