Commit 36f8ce42 authored by Dipl.-Ing. Jonas Stienen's avatar Dipl.-Ing. Jonas Stienen
Browse files

Progress on API change

parent 0fe6f138
......@@ -82,7 +82,7 @@ public:
{
if( m_pLastState )
{
oOrient = m_pLastState->GetOrientationYPR();
oOrient = m_pLastState->GetOrientation();
return true;
}
return false;
......
......@@ -918,8 +918,8 @@ CVABATNSoundPath* CVABinauralAirTrafficNoiseAudioRenderer::CreateSoundPath( CVAB
CVAListenerState* pListenerNew = (m_pNewSceneState ? m_pNewSceneState->GetListenerState(iListenerID) : nullptr);
if( pListenerNew != nullptr )
{
pPath->oDirSoundPath.oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetHRIRDataset();
pPath->oRefSoundPath.oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetHRIRDataset();
pPath->oDirSoundPath.oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetDirectivity();
pPath->oRefSoundPath.oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetDirectivity();
}
m_lSoundPaths.push_back( pPath );
......@@ -1150,8 +1150,8 @@ void CVABinauralAirTrafficNoiseAudioRenderer::UpdateSoundPaths()
}
else
{
pPath->oDirSoundPath.oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetHRIRDataset();
pPath->oRefSoundPath.oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetHRIRDataset();
pPath->oDirSoundPath.oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetDirectivity();
pPath->oRefSoundPath.oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetDirectivity();
}
}
......
......@@ -1171,9 +1171,9 @@ void CVABinauralArtificialReverbAudioRenderer::UpdateArtificialReverbPaths( cons
{
IVAHRIRDataset* pCurHRIR = NULL;
if( pListenerCur )
pCurHRIR = ( IVAHRIRDataset* ) pListenerCur->GetHRIRDataset();
pCurHRIR = ( IVAHRIRDataset* ) pListenerCur->GetDirectivity();
IVAHRIRDataset* pNewHRIR = ( IVAHRIRDataset* ) pListenerNew->GetHRIRDataset();
IVAHRIRDataset* pNewHRIR = ( IVAHRIRDataset* ) pListenerNew->GetDirectivity();
if( pCurHRIR != pNewHRIR )
{
......
......@@ -870,7 +870,7 @@ CVABFFSoundPath* CVABinauralFreeFieldAudioRenderer::CreateSoundPath( CVABFFSourc
CVAListenerState* pListenerNew = ( m_pNewSceneState ? m_pNewSceneState->GetListenerState( iListenerID ) : nullptr );
if( pListenerNew != nullptr )
pPath->oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetHRIRDataset();
pPath->oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetDirectivity();
// Configure DSP elements
pPath->pVariableDelayLineChL->SetAlgorithm( m_iDefaultVDLSwitchingAlgorithm );
......@@ -1113,7 +1113,7 @@ void CVABinauralFreeFieldAudioRenderer::UpdateSoundPaths()
}
else
{
pPath->oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetHRIRDataset();
pPath->oHRIRStateNew.pData = (IVAHRIRDataset*) pListenerNew->GetDirectivity();
}
}
......
......@@ -2073,7 +2073,7 @@ void CVARoomAcousticsAudioRenderer::UpdateComplexSoundPaths()
int iSourceAuraMode = pSourceStateNew->GetAuralizationMode();
CVAListenerState* pListenerStateNew = ( m_pNewSceneState ? m_pNewSceneState->GetListenerState( iListenerID ) : nullptr );
pPath->oHRIRStateNew.pData = pListenerStateNew->GetHRIRDataset();
pPath->oHRIRStateNew.pData = pListenerStateNew->GetDirectivity();
int iListenerAuraMode = pListenerStateNew->GetAuralizationMode();
pPath->iPathAuralizationMode = iSourceAuraMode & iListenerAuraMode & iGlobalAuralizationMode;
......
......@@ -861,7 +861,7 @@ CVAPTHASoundPath* CVAPTHearingAidRenderer::CreateSoundPath( Source* pSource, Lis
CVAListenerState* pListenerNew = ( m_pNewSceneState ? m_pNewSceneState->GetListenerState( iListenerID ) : nullptr );
if( pListenerNew != nullptr )
{
pPath->oHRIRStateNew.pData = ( IVAHRIRDataset* ) pListenerNew->GetHRIRDataset();
pPath->oHRIRStateNew.pData = ( IVAHRIRDataset* ) pListenerNew->GetDirectivity();
}
// ...
......@@ -1165,7 +1165,7 @@ void CVAPTHearingAidRenderer::UpdateSoundPaths()
}
else
{
pPath->oHRIRStateNew.pData = ( IVAHRIRDataset* ) pListenerNew->GetHRIRDataset();
pPath->oHRIRStateNew.pData = ( IVAHRIRDataset* ) pListenerNew->GetDirectivity();
}
it++;
}
......
......@@ -222,7 +222,7 @@ void CVAAmbisonicsBinauralMixdownReproduction::UpdateScene( CVASceneState* pNewS
if( pLIstenerState == nullptr )
return;
IVAHRIRDataset* pHRIRSet = pLIstenerState->GetHRIRDataset();
IVAHRIRDataset* pHRIRSet = pLIstenerState->GetDirectivity();
if( pHRIRSet == nullptr )
return;
......
......@@ -136,7 +136,7 @@ void CVABinauralMixdownReproduction::UpdateScene( CVASceneState* pNewState )
if( pLIstenerState == nullptr )
return;
IVAHRIRDataset* pHRIRSet = pLIstenerState->GetHRIRDataset();
IVAHRIRDataset* pHRIRSet = pLIstenerState->GetDirectivity();
if( pHRIRSet == nullptr )
return;
......
......@@ -108,7 +108,7 @@ void CVACTC4Reproduction::UpdateScene( CVASceneState* pNewState )
if( pLIstenerState == nullptr )
return;
IVAHRIRDataset* pHRIRSet = pLIstenerState->GetHRIRDataset();
IVAHRIRDataset* pHRIRSet = pLIstenerState->GetDirectivity();
if( pHRIRSet == nullptr )
return;
......
......@@ -374,7 +374,7 @@ void CVANCTCReproduction::UpdateScene( CVASceneState* pNewState )
CVAHRIRDatasetDAFF2D* pHRIRSetDAFF2D = NULL;
if( m_bTrackedListenerHRIR )
{
pHRIRSetDAFF2D = dynamic_cast< CVAHRIRDatasetDAFF2D* >( pListenerState->GetHRIRDataset() );
pHRIRSetDAFF2D = dynamic_cast< CVAHRIRDatasetDAFF2D* >( pListenerState->GetDirectivity() );
if( pHRIRSetDAFF2D == nullptr )
return;
......
......@@ -13,7 +13,7 @@
#include "VAListenerState.h"
#include <VACore.h>
#include <VA.h>
#include "../VALog.h"
#include "VASceneManager.h"
......@@ -28,10 +28,10 @@ void CVAListenerState::Initialize( double dModificationTime )
data.pMotionState = GetManager()->RequestMotionState();
data.pMotionState->Initialize( dModificationTime );
data.iAuraMode = IVACore::VA_AURAMODE_ALL;
data.iHRIRDatasetID = -1;
data.pHRIRDataset = nullptr;
data.iAuraMode = IVAInterface::VA_AURAMODE_ALL;
data.iDirectivityID = -1;
data.pDirectivity = nullptr;
SetModificationTime( dModificationTime );
}
......@@ -46,13 +46,13 @@ void CVAListenerState::Copy( const CVAListenerState* pSrc, double dModificationT
// Daten kopieren
data = pSrc->data;
SetFixed(false);
SetModificationTime(dModificationTime);
SetFixed( false );
SetModificationTime( dModificationTime );
}
void CVAListenerState::Fix() {
// Alle enthaltenen Objekte fixieren
if (data.pMotionState) data.pMotionState->Fix();
if( data.pMotionState ) data.pMotionState->Fix();
// Selbst fixieren
CVASceneStateBase::Fix();
......@@ -60,7 +60,7 @@ void CVAListenerState::Fix() {
void CVAListenerState::PreRelease() {
// Alle Referenzen auf abhngige Objekte entfernen
if (data.pMotionState) data.pMotionState->RemoveReference();
if( data.pMotionState ) data.pMotionState->RemoveReference();
// Funktion der Oberklasse aufrufen
CVASceneStateBase::PreRelease();
......@@ -70,28 +70,31 @@ const CVAMotionState* CVAListenerState::GetMotionState() const {
return data.pMotionState;
}
int CVAListenerState::GetHRIRDatasetID() const {
return data.iHRIRDatasetID;
int CVAListenerState::GetDirectivityID() const
{
return data.iDirectivityID;
}
IVAHRIRDataset* CVAListenerState::GetHRIRDataset() const {
return data.pHRIRDataset;
const CVADirectivity* CVAListenerState::GetDirectivity() const
{
return data.pDirectivity;
}
CVAMotionState* CVAListenerState::AlterMotionState() {
CVAMotionState* CVAListenerState::AlterMotionState()
{
assert( !IsFixed() );
// Falls der Zustand finalisiert => Zustand aus der Basiskonfig => Autonomen Zustand erzeugen
// Falls der Zustand nicht finalisiert => Bereits erzeugter autonomen Zustand => Diesen zurckgeben
if (data.pMotionState->IsFixed()) {
if( data.pMotionState->IsFixed() ) {
// Autonomen Zustand ableiten
CVAMotionState* pNewState = GetManager()->RequestMotionState();
// nderungszeit des bergeordneten Szenezustands bernehmen
double dCreationTime = GetModificationTime();
pNewState->Copy(data.pMotionState, dCreationTime);
pNewState->Copy( data.pMotionState, dCreationTime );
data.pMotionState->RemoveReference();
data.pMotionState = pNewState;
......@@ -106,20 +109,22 @@ int CVAListenerState::GetAuralizationMode() const
return data.iAuraMode;
}
void CVAListenerState::SetAuralizationMode(int iAuralizationMode)
void CVAListenerState::SetAuralizationMode( int iAuralizationMode )
{
assert( !IsFixed() );
data.iAuraMode = iAuralizationMode;
}
void CVAListenerState::SetHRIRDatasetID(int iHRIRDatasetID) {
void CVAListenerState::SetDirectivityID( int iID )
{
assert( !IsFixed() );
data.iHRIRDatasetID = iHRIRDatasetID;
data.iDirectivityID = iID;
}
void CVAListenerState::SetHRIRDataset(IVAHRIRDataset* pHRIRDataset) {
void CVAListenerState::SetDirectivity( const CVADirectivity* pDirectivity )
{
assert( !IsFixed() );
data.pHRIRDataset = pHRIRDataset;
data.pDirectivity = pDirectivity;
}
std::string CVAListenerState::ToString() const
......@@ -127,8 +132,8 @@ std::string CVAListenerState::ToString() const
/*
std::stringstream ss;
ss << "Sound source state {\n"
<< "
*/
<< "
*/
return "";
}
......@@ -148,28 +153,28 @@ void CVAListenerState::SetParameters( const CVAStruct& oParams )
if( oParams.HasKey( "anthroparams" ) )
{
const CVAStruct& oAnthroParams( oParams["anthroparams"] );
const CVAStruct& oAnthroParams( oParams[ "anthroparams" ] );
if( oAnthroParams.HasKey( "headwidth" ) )
data.oAnthroData.dHeadWidth = oAnthroParams["headwidth"];
data.oAnthroData.dHeadWidth = oAnthroParams[ "headwidth" ];
if( oAnthroParams.HasKey( "headheight" ) )
data.oAnthroData.dHeadHeight = oAnthroParams["headheight"];
data.oAnthroData.dHeadHeight = oAnthroParams[ "headheight" ];
if( oAnthroParams.HasKey( "headdepth" ) )
data.oAnthroData.dHeadDepth = oAnthroParams["headdepth"];
data.oAnthroData.dHeadDepth = oAnthroParams[ "headdepth" ];
}
}
CVAStruct CVAListenerState::GetParameters( const CVAStruct& oArgs ) const
CVAStruct CVAListenerState::GetParameters( const CVAStruct& ) const
{
VA_VERBOSE( "ListenerState", "Parameters requested" );
CVAStruct oRet;
oRet["auramode"] = data.iAuraMode;
oRet["hrirdatasetid"] = data.iHRIRDatasetID;
oRet[std::string("anthroparams")] = CVAStruct();
oRet[std::string("anthroparams")][std::string("headwidth")] = data.oAnthroData.dHeadWidth;
oRet[std::string("anthroparams")][std::string("headheight")] = data.oAnthroData.dHeadHeight;
oRet[std::string("anthroparams")][std::string("headdepth")] = data.oAnthroData.dHeadDepth;
oRet[ "auramode" ] = data.iAuraMode;
oRet[ "hrirdatasetid" ] = data.iDirectivityID;
oRet[ std::string( "anthroparams" ) ] = CVAStruct();
oRet[ std::string( "anthroparams" ) ][ std::string( "headwidth" ) ] = data.oAnthroData.dHeadWidth;
oRet[ std::string( "anthroparams" ) ][ std::string( "headheight" ) ] = data.oAnthroData.dHeadHeight;
oRet[ std::string( "anthroparams" ) ][ std::string( "headdepth" ) ] = data.oAnthroData.dHeadDepth;
return oRet;
}
......@@ -187,10 +192,10 @@ double CVAListenerState::CVAAnthropometricParameter::GetHeadDepthParameterFromLU
// Individual subject size
size_t b = size_t( std::round( dInvidividualDepth * 100 ) - 3 );
if( b >= m_vvdHeadDepthParameterLUT[ 0 ].size() )
b = m_vvdHeadDepthParameterLUT[ 0 ].size() - 1;
if( b <= 0 )
b = 0;
......
......@@ -19,7 +19,7 @@
#include <VAStruct.h>
class CVAMotionState;
class IVAHRIRDataset;
class CVADirectivity;
//! Class describing a human listener in a dynamic scene
class CVAListenerState : public CVASceneStateBase
......@@ -53,7 +53,7 @@ public:
{ 4.74210562780852e-05, 2.30720580074451e-05, 9.34701569421659e-06, 1.51992869845685e-06, -3.13899378800997e-06, -5.54853381928844e-06, -6.46928761560694e-06, -6.51644364002557e-06, -5.76972986376934e-06, -4.80990363083134e-06, -3.32729856888925e-06, -1.68796693156281e-06, -1.32663502405705e-09, 1.74859774437763e-06, 3.64690866805351e-06 },
{ 4.56711318729264e-05, 2.13221335998437e-05, 7.59709129150021e-06, -2.29986482858102e-07, -4.88891652339341e-06, -7.29845821589858e-06, -8.21921202920350e-06, -8.26639175577348e-06, -7.51965425771495e-06, -6.55982808583921e-06, -5.07722558185098e-06, -3.43789134071848e-06, -1.75125105572604e-06, -1.32663502405705e-09, 1.89698426178442e-06 },
{ 4.37728209564670e-05, 1.94238227038124e-05, 5.69878038825245e-06, -2.12830528611985e-06, -6.78722742442073e-06, -9.19676913668788e-06, -1.01175229385575e-05, -1.01646789770760e-05, -9.41796512832216e-06, -8.45813895411496e-06, -6.97553387785099e-06, -5.33613200681682e-06, -3.64956196374777e-06, -1.89963756069833e-06, -1.32663502405705e-09 } };
m_vvdHeadDepthParameterLUT.resize( 15 );
for( int i = 0; i < 15; i++ )
{
......@@ -91,17 +91,17 @@ public:
// Getter
const CVAMotionState* GetMotionState() const;
int GetAuralizationMode() const;
int GetHRIRDatasetID() const;
IVAHRIRDataset* GetHRIRDataset() const;
int GetDirectivityID() const;
const CVADirectivity* GetDirectivity() const;
double GetHeadDepthParameter( double dSubjDepth ) const;
//! Returns anthropometric data
const CVAAnthropometricParameter& GetAnthropometricData() const;
// Setter
void SetAuralizationMode(int iAuralizationMode);
void SetHRIRDatasetID(int iHRIRDatasetID);
void SetHRIRDataset(IVAHRIRDataset* pHRIRDataset);
void SetAuralizationMode( int iAuralizationMode );
void SetDirectivityID( int iID );
void SetDirectivity( const CVADirectivity* pDirectivity );
//! Sets anthropometric data
void SetAnthropometricData( const CVAAnthropometricParameter& oAnthroData );
......@@ -125,8 +125,8 @@ private:
{
CVAMotionState* pMotionState; //!< State of motion in 3D space
int iAuraMode; //!< Auralization flags
int iHRIRDatasetID; //!< HRIR data set index
IVAHRIRDataset* pHRIRDataset; //!< HRIR data set pointer
int iDirectivityID; //!< HRIR data set index
const CVADirectivity* pDirectivity; //!< HRIR data set pointer
CVAAnthropometricParameter oAnthroData; //!< Anthropometric data
} data; //!< Listener state internal data
};
......
......@@ -14,7 +14,7 @@
#include "VAMotionState.h"
#include "../Utils/VAUtils.h"
#include <VACore.h>
#include <VA.h>
#include <assert.h>
......@@ -23,7 +23,6 @@ void CVAMotionState::Initialize( double dModificationTime )
data.vPos.Set( 0, 0, 0 );
data.vView.Set( 0, 0, -1 );
data.vUp.Set( 0, 1, 0 );
data.vVel.Set( 0, 0, 0 );
data.oRealWorldPose.Reset();
......@@ -46,13 +45,21 @@ void CVAMotionState::Fix() { CVASceneStateBase::Fix(); }
VAVec3 CVAMotionState::GetPosition() const { return data.vPos; }
VAVec3 CVAMotionState::GetView() const { return data.vView; }
VAVec3 CVAMotionState::GetUp() const { return data.vUp; }
VAVec3 CVAMotionState::GetVelocity() const { return data.vVel; }
CVAMotionState::CVAPose CVAMotionState::GetRealWorldPose() const { return data.oRealWorldPose; }
VAOrientYPR CVAMotionState::GetOrientationYPR() const
CVAMotionState::CVAPose CVAMotionState::GetRealWorldPose() const
{
return data.oRealWorldPose;
}
void CVAMotionState::SetRealWorldPose( const CVAPose& oNewPose )
{
data.oRealWorldPose = oNewPose;
}
VAQuat CVAMotionState::GetOrientation() const
{
VAOrientYPR oOrient;
ConvertVU2YPR_DEG( data.vView, data.vUp, oOrient );
VAQuat oOrient;
ConvertViewUpToQuaternion( data.vView, data.vUp, oOrient );
return oOrient;
}
......@@ -62,16 +69,10 @@ void CVAMotionState::SetPosition( const VAVec3& vPos )
data.vPos = vPos;
}
void CVAMotionState::SetVelocity( const VAVec3& vVel )
{
assert( !IsFixed() );
data.vVel = vVel;
}
void CVAMotionState::SetOrientationYPR( const VAOrientYPR& oOrientYPR_DEG )
void CVAMotionState::SetOrientation( const VAQuat& qOrient )
{
assert( !IsFixed() );
ConvertYPR2VU_DEG( oOrientYPR_DEG, data.vView, data.vUp );
ConvertQuaternionToViewUp( qOrient, data.vView, data.vUp );
}
void CVAMotionState::SetOrientationVU( const VAVec3& vView, const VAVec3& vUp )
......
......@@ -37,11 +37,11 @@ public:
inline void Reset()
{
vPos.Set( 0.0f, 0.0f, 0.0f );
oOrientYPR_DEG.Set( 0.0f, 0.0f, 0.0f );
qOrient.Set( 0.0f, 0.0f, 0.0f, 1.0f );
};
VAVec3 vPos;
VAOrientYPR oOrientYPR_DEG;
VAQuat qOrient;
};
void Initialize( double dModificationTime );
......@@ -55,14 +55,13 @@ public:
VAVec3 GetPosition() const;
VAVec3 GetView() const;
VAVec3 GetUp() const;
VAOrientYPR GetOrientationYPR() const;
VAVec3 GetVelocity() const;
VAQuat GetOrientation() const;
void SetPosition( const VAVec3& vPos );
void SetVelocity( const VAVec3& vVel );
void SetOrientationYPR( const VAOrientYPR& oOrientYPR_DEG );
void SetOrientation( const VAQuat& qOrient );
void SetOrientationVU( const VAVec3& vView, const VAVec3& vUp );
CVAPose GetRealWorldPose() const;
void SetRealWorldPose( const CVAPose& );
private:
struct
......@@ -70,7 +69,6 @@ private:
VAVec3 vPos; //!< Position [m]
VAVec3 vView; //!< View vector
VAVec3 vUp; //!< Up vector
VAVec3 vVel; //!< Velocity [m/s]
CVAPose oRealWorldPose; //!< Pose in real world coordination system (i.e. playback room position)
} data;
};
......
......@@ -15,7 +15,7 @@
#define IW_VA_SCENE_STATE
#include "VASceneStateBase.h"
#include <VACore.h>
#include <VA.h>
#include <ITAAtomicPrimitives.h>
......
......@@ -13,8 +13,6 @@
#include "VAUtils.h"
#include <VABase.h>
#include "../Scene/VAMotionState.h"
#include <ITAConstants.h>
......@@ -24,6 +22,7 @@
#include <ITAConfigUtils.h>
#include <VistaBase/VistaTimeUtils.h>
#include <VistaBase/VistaQuaternion.h>
#include <cassert>
......@@ -79,118 +78,32 @@ CVAException convert2VAException( const VistaExceptionBase& e )
return CVAException( CVAException::UNSPECIFIED, e.GetExceptionText() );
}
void ConvertYPR2VU_RAD( const VAOrientYPR& oOrientYPRRAD, VAVec3& vView, VAVec3& vUp )
void ConvertQuaternionToViewUp( const VAQuat& qOrient, VAVec3& v3View, VAVec3& v3Up )
{
/*
* Yaw-pitch-roll (YPR) angles rotation order (referring to OpenGL axis)
*
* R := Ry(yaw) * Rx(pitch) * Rz(-roll);
*
* Note: Roll accounts to the view axis (-Z), not the Z axis (+Z). Hence inversion.
*/
const double yaw = oOrientYPRRAD.yaw;
const double pitch = oOrientYPRRAD.pitch;
const double roll = oOrientYPRRAD.roll;
VistaQuaternion qVistaQuat;
qVistaQuat[ Vista::X ] = float( qOrient.x );
qVistaQuat[ Vista::Y ] = float( qOrient.y );
qVistaQuat[ Vista::Z ] = float( qOrient.z );
qVistaQuat[ Vista::W ] = float( qOrient.w );
double sy = sin( yaw ), cy = cos( yaw );
double sp = sin( pitch ), cp = cos( pitch );
double sr = sin( roll ), cr = cos( roll );
const VistaVector3D vVistaView = qVistaQuat.GetViewDir();
v3View.Set( vVistaView[ Vista::X ], vVistaView[ Vista::Y ], vVistaView[ Vista::Z ] );
vView.x = -sy*cp;
vView.y = sp;
vView.z = -cy*cp;
vUp.x = cy*sr + sy*sp*cr;
vUp.y = cp*cr;
vUp.z = -sy*sr + cy*sp*cr;
const VistaVector3D vVistaUp = qVistaQuat.GetUpDir();
v3Up.Set( vVistaUp[ Vista::X ], vVistaUp[ Vista::Y ], vVistaUp[ Vista::Z ] );
}
void ConvertVU2YPR_RAD( const VAVec3& vView, const VAVec3& vUp, VAOrientYPR& oOrientYPRRAD )
void ConvertViewUpToQuaternion( const VAVec3& vView, const VAVec3& vUp, VAQuat& qOrient )
{
const float EPSILON = 0.0001F;
// Convenience variables
const double& vx = vView.x;
const double& vy = vView.y;
const double& vz = vView.z;
const double& ux = vUp.x;
const double& uy = vUp.y;
const double& uz = vUp.z;
double& yaw = oOrientYPRRAD.yaw;
double& pitch = oOrientYPRRAD.pitch;
double& roll = oOrientYPRRAD.roll;
// Problem: View points into north pole => Gimbal lock between yaw and roll
if( vy >= ( 1 - EPSILON ) )
{
/*
* Solution: Forget about roll. Everything is yaw.
*
* Ry(y).Rx(Pi/2).Rz(0).u0 = (sin(y) 0 cos(y))
*
* yaw = atan2(ux, uz)
*
*/
yaw = atan2( ux, uz );
pitch = ITAConstants::PI_F;
roll = 0;
return;
}
// Problem: View points into south pole => Gimbal lock between yaw and roll
if( vy <= -( 1 - EPSILON ) )
{
/*
* Solution: Forget about roll. Everything is yaw.
*
* Ry(y).Rx(Pi/2).Rz(0).u0 = (sin(y) 0 cos(y))
*
* yaw = atan2(-ux, -uz)
*
*/
yaw = atan2( -ux, -uz );
pitch = -ITAConstants::PI_F;
roll = 0;
return;
}
yaw = atan2( -vx, -vz );
pitch = asin( vy );
/*
* Problem: View does not point into a pole (see above)
* but up-vector lies within horizontal XZ plane.
*
* Solution: Roll can only be +90/-90.
* Decide by hemisphere which crossprod(v,u) falls.
* Upper hemisphere (y>=0) means -90
*
*/
// y-component of cross production v x u
double zy = vz*ux - vx*uz;
if( ( uy <= EPSILON ) && ( uy >= -EPSILON ) ) {