Select Git revision
VAMatlabTracking.cpp
-
Dipl.-Ing. Jonas Stienen authoredDipl.-Ing. Jonas Stienen authored
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
VAMatlabTracking.cpp 8.70 KiB
/*
* --------------------------------------------------------------------------------------------
*
* VVV VVV A Virtual Acoustics (VA) | http://www.virtualacoustics.org
* VVV VVV AAA Licensed under the Apache License, Version 2.0
* VVV VVV AAA
* VVV VVV AAA Copyright 2015-2017
* VVVVVV AAA Institute of Technical Acoustics (ITA)
* VVVV AAA RWTH Aachen University
*
* --------------------------------------------------------------------------------------------
*/
#include "VAMatlabTracking.h"
#include "VAMatlabConnection.h"
#include <VA.h>
// NatNet includes
#include <NatNetTypes.h>
#include <NatNetClient.h>
// Tracking callback function
void TrackerDataHandler( sFrameOfMocapData* sFrame, void* pUserData )
{
CVAMatlabTracker* pVAMatlabTracker = ( CVAMatlabTracker* ) pUserData;
if( !pVAMatlabTracker )
return;
IVAInterface* pVACore = pVAMatlabTracker->pVACore;
if( !pVACore )
return;
// SoundReceiver
int iTrackedReceiverID = pVAMatlabTracker->iTrackedSoundReceiverID;
int iTrackedReceiverRigidBodyIndex = pVAMatlabTracker->iTrackedSoundReceiverHeadRigidBodyIndex;
int iTrackedReceiverHATORigidBodyIndex = pVAMatlabTracker->iTrackedSoundReceiverTorsoRigidBodyIndex;
if( iTrackedReceiverID != -1 )
{
// Tracked receiver
if( ( iTrackedReceiverRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedReceiverRigidBodyIndex > 0 ) )
{
try
{
VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedSoundReceiverTranslation;
VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedSoundReceiverRotation;
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedReceiverRigidBodyIndex - 1 ] );
VistaVector3D vPosPivotPoint( oBodyData.x, oBodyData.y, oBodyData.z );
VistaQuaternion qOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
VistaQuaternion qOrientRigidBody = qOrientRotation * qOrientRaw;
VistaVector3D vViewRigidBody = qOrientRigidBody.GetViewDir();
VistaVector3D vUpRigidBody = qOrientRigidBody.GetUpDir();
VistaVector3D vPosOffsetGlobalCoordinateSystem = qOrientRigidBody.Rotate( vPosOffsetLocalCoordinateSystem );
VistaVector3D vPosRigidBody = vPosPivotPoint + vPosOffsetGlobalCoordinateSystem;
pVACore->SetSoundReceiverPosition( iTrackedReceiverID, VAVec3( vPosRigidBody[ 0 ], vPosRigidBody[ 1 ], vPosRigidBody[ 2 ] ) );
pVACore->SetSoundReceiverOrientationVU( iTrackedReceiverID, VAVec3( vViewRigidBody[ 0 ], vViewRigidBody[ 1 ], vViewRigidBody[ 2 ] ), VAVec3( vUpRigidBody[ 0 ], vUpRigidBody[ 1 ], vUpRigidBody[ 2 ] ) );
// HATO orientation
if( ( iTrackedReceiverHATORigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedReceiverHATORigidBodyIndex > 0 ) )
{
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedReceiverHATORigidBodyIndex - 1 ] );
VistaQuaternion qOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
VistaQuaternion qHATOOrient = qOrientRaw * qOrientRigidBody;
VAQuat qFinalHATO = VAQuat( qHATOOrient[ Vista::X ], qHATOOrient[ Vista::Y ], qHATOOrient[ Vista::Z ], qHATOOrient[ Vista::W ] );
pVACore->SetSoundReceiverHeadAboveTorsoOrientation( iTrackedReceiverID, qFinalHATO );
}
}
catch( ... )
{
}
}
}
// Real-world sound receiver
int iTrackedRealWorldSoundReceiverID = pVAMatlabTracker->iTrackedRealWorldSoundReceiverID;
int iTrackedRealWorldSoundReceiverRigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverRigidBodyIndex;
int iTrackedRealWorldSoundReceiverHATORigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex;
if( iTrackedRealWorldSoundReceiverID != -1 )
{
// Real world receiver tracking
if( ( iTrackedRealWorldSoundReceiverRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedRealWorldSoundReceiverRigidBodyIndex > 0 ) )
{
try
{
VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedRealWorldSoundReceiverTranslation;
VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedRealWorldSoundReceiverRotation;
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedRealWorldSoundReceiverRigidBodyIndex - 1 ] );
VistaVector3D vPosPivotPoint( oBodyData.x, oBodyData.y, oBodyData.z );
VistaQuaternion qOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
VistaQuaternion qOrientRigidBody = qOrientRotation * qOrientRaw;
VistaVector3D vViewRigidBody = qOrientRigidBody.GetViewDir();
VistaVector3D vUpRigidBody = qOrientRigidBody.GetUpDir();
VistaVector3D vPosOffsetGlobalCoordinateSystem = qOrientRigidBody.Rotate( vPosOffsetLocalCoordinateSystem );
VistaVector3D vPosRigidBody = vPosPivotPoint + vPosOffsetGlobalCoordinateSystem;
pVACore->SetSoundReceiverRealWorldPositionOrientationVU( iTrackedRealWorldSoundReceiverID, VAVec3( vPosRigidBody[ 0 ], vPosRigidBody[ 1 ], vPosRigidBody[ 2 ] ), VAVec3( vViewRigidBody[ 0 ], vViewRigidBody[ 1 ], vViewRigidBody[ 2 ] ), VAVec3( vUpRigidBody[ 0 ], vUpRigidBody[ 1 ], vUpRigidBody[ 2 ] ) );
// Real world receiver HATO orientation
if( ( iTrackedRealWorldSoundReceiverHATORigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedRealWorldSoundReceiverHATORigidBodyIndex > 0 ) )
{
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedRealWorldSoundReceiverHATORigidBodyIndex - 1 ] );
const VistaQuaternion qTorsoOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
const VistaQuaternion qTorsoOrient = qOrientRotation * qOrientRigidBody;
const VAQuat qFinalOrient( qTorsoOrient[ Vista::X ], qTorsoOrient[ Vista::Y ], qTorsoOrient[ Vista::Z ], qTorsoOrient[ Vista::W ] );
pVACore->SetSoundReceiverRealWorldHeadAboveTorsoOrientation( iTrackedReceiverID, qFinalOrient );
}
}
catch( ... )
{
}
}
}
// Source
int iTrackedSourceID = pVAMatlabTracker->iTrackedSourceID;
int iTrackedSourceRigidBodyIndex = pVAMatlabTracker->iTrackedSourceRigidBodyIndex;
if( ( iTrackedSourceID != -1 ) && ( iTrackedSourceRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedSourceRigidBodyIndex > 0 ) )
{
try
{
VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedSourceTranslation;
VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedSourceRotation;
const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedSourceRigidBodyIndex - 1 ] );
VistaVector3D vPosPivotPoint( oBodyData.x, oBodyData.y, oBodyData.z );
VistaQuaternion qOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );
VistaQuaternion qOrientRigidBody = qOrientRotation * qOrientRaw;
VistaVector3D vViewRigidBody = qOrientRigidBody.GetViewDir();
VistaVector3D vUpRigidBody = qOrientRigidBody.GetUpDir();
VistaVector3D vPosOffsetGlobalCoordinateSystem = qOrientRigidBody.Rotate( vPosOffsetLocalCoordinateSystem );
VistaVector3D vPosRigidBody = vPosPivotPoint + vPosOffsetGlobalCoordinateSystem;
pVACore->SetSoundSourcePosition( iTrackedSourceID, VAVec3( vPosRigidBody[ 0 ], vPosRigidBody[ 1 ], vPosRigidBody[ 2 ] ) );
pVACore->SetSoundSourceOrientationVU( iTrackedSourceID, VAVec3( vViewRigidBody[ 0 ], vViewRigidBody[ 1 ], vViewRigidBody[ 2 ] ), VAVec3( vUpRigidBody[ 0 ], vUpRigidBody[ 1 ], vUpRigidBody[ 2 ] ) );
}
catch( ... )
{
}
}
return;
}
CVAMatlabTracker::CVAMatlabTracker()
{
pVACore = NULL;
m_pTrackerClient = new NatNetClient( ConnectionType_Multicast );
m_pTrackerClient->SetDataCallback( TrackerDataHandler, this );
Reset();
m_bConnected = false;
}
void CVAMatlabTracker::Reset()
{
iTrackedSoundReceiverID = -1;
iTrackedSoundReceiverHeadRigidBodyIndex = 1;
iTrackedSoundReceiverTorsoRigidBodyIndex = 1;
vTrackedSoundReceiverTranslation.SetToZeroVector();
qTrackedSoundReceiverRotation.SetToNeutralQuaternion();
iTrackedRealWorldSoundReceiverID = -1;
iTrackedRealWorldSoundReceiverRigidBodyIndex = 1;
iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex = 1;
vTrackedRealWorldSoundReceiverTranslation.SetToZeroVector();
qTrackedRealWorldSoundReceiverRotation.SetToNeutralQuaternion();
iTrackedSourceID = -1;
iTrackedSourceRigidBodyIndex = 1;
vTrackedSourceTranslation.SetToZeroVector();
qTrackedSourceRotation.SetToNeutralQuaternion();
}
bool CVAMatlabTracker::Initialize( std::string sServerAdress, std::string sLocalAdress )
{
char* pcLocalAdress = &sLocalAdress[ 0 ];
char* pcServerAdress = &sServerAdress[ 0 ];
int iError = m_pTrackerClient->Initialize( pcLocalAdress, pcServerAdress );
m_bConnected = ( iError == ErrorCode_OK );
return m_bConnected;
}
bool CVAMatlabTracker::Uninitialize()
{
int iError = m_pTrackerClient->Uninitialize();
m_bConnected = false;
return ( iError == ErrorCode_OK );
}
bool CVAMatlabTracker::IsConnected() const
{
return m_bConnected;
}