VAMatlabTracking.cpp

/*
 *  --------------------------------------------------------------------------------------------
 *
 *    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;
}