VAMatlabTracking.cpp 8.7 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13
/*
 *  --------------------------------------------------------------------------------------------
 *
 *    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
 *
 *  --------------------------------------------------------------------------------------------
 */

14 15 16
#include "VAMatlabTracking.h"
#include "VAMatlabConnection.h"

17
#include <VA.h>
18 19 20 21 22 23 24

// NatNet includes
#include <NatNetTypes.h>
#include <NatNetClient.h>

// Tracking callback function
void TrackerDataHandler( sFrameOfMocapData* sFrame, void* pUserData )
25 26 27
{
	CVAMatlabTracker* pVAMatlabTracker = ( CVAMatlabTracker* ) pUserData;

28 29 30
	if( !pVAMatlabTracker )
		return;

31
	IVAInterface* pVACore = pVAMatlabTracker->pVACore;
32 33 34
	if( !pVACore )
		return;

Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
35
	// SoundReceiver
36
	int iTrackedReceiverID = pVAMatlabTracker->iTrackedSoundReceiverID;
37 38
	int iTrackedReceiverRigidBodyIndex = pVAMatlabTracker->iTrackedSoundReceiverHeadRigidBodyIndex;
	int iTrackedReceiverHATORigidBodyIndex = pVAMatlabTracker->iTrackedSoundReceiverTorsoRigidBodyIndex;
39

40
	if( iTrackedReceiverID != -1 )
41
	{
42 43
		// Tracked receiver
		if( ( iTrackedReceiverRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedReceiverRigidBodyIndex > 0 ) )
44
		{
45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66
			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 ] ) );
			}
			catch( ... )
			{
			}
67
		}
68 69 70

		// HATO orientation
		if( ( iTrackedReceiverHATORigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedReceiverHATORigidBodyIndex > 0 ) )
71
		{
72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87
			try
			{
				const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedReceiverHATORigidBodyIndex - 1 ] );
				VistaQuaternion qOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );

				// Convert absolute orientation to relative HATO orientation
				VAQuat qOrient = pVACore->GetSoundReceiverOrientation( iTrackedReceiverID );
				VistaQuaternion qHeadOrient( qOrient.comp );
				VistaQuaternion qHATOOrient = qOrientRaw * qHeadOrient;
				VAQuat qFinalHATO = VAQuat( qHATOOrient[ Vista::X ], qHATOOrient[ Vista::Y ], qHATOOrient[ Vista::Z ], qHATOOrient[ Vista::W ] );

				pVACore->SetSoundReceiverHeadAboveTorsoOrientation( iTrackedReceiverID, qFinalHATO );
			}
			catch( ... )
			{
			}
88 89
		}
	}
90

91
	// Real-world sound receiver
92
	int iTrackedRealWorldSoundReceiverID = pVAMatlabTracker->iTrackedRealWorldSoundReceiverID;
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
93
	int iTrackedRealWorldSoundReceiverRigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverRigidBodyIndex;
94
	int iTrackedRealWorldSoundReceiverHATORigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex;
95

96
	if( iTrackedRealWorldSoundReceiverID != -1 ) 
97
	{
98 99
		// Real world receiver tracking
		if( ( iTrackedRealWorldSoundReceiverRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedRealWorldSoundReceiverRigidBodyIndex > 0 ) )
100
		{
101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121
			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 ] ) );
			}
			catch( ... )
			{
			}
122
		}
123 124 125

		// Real world receiver HATO orientation
		if( ( iTrackedRealWorldSoundReceiverHATORigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedRealWorldSoundReceiverHATORigidBodyIndex > 0 ) )
126
		{
127 128 129 130 131 132 133 134 135 136
			try
			{
				const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedRealWorldSoundReceiverHATORigidBodyIndex - 1 ] );
				VAQuat qOrientRaw( oBodyData.qx, oBodyData.qy, oBodyData.qz, oBodyData.qw );

				pVACore->SetSoundReceiverRealWorldTorsoOrientation( iTrackedReceiverID, qOrientRaw );
			}
			catch( ... )
			{
			}
137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161
		}
	}

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

162 163
			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 ] ) );
164 165 166 167
		}
		catch( ... )
		{
		}
168
	}
169 170

	return;
171 172 173 174 175 176 177 178 179 180 181 182 183
}

CVAMatlabTracker::CVAMatlabTracker()
{
	pVACore = NULL;
	m_pTrackerClient = new NatNetClient( ConnectionType_Multicast );
	m_pTrackerClient->SetDataCallback( TrackerDataHandler, this );
	Reset();
	m_bConnected = false;
}

void CVAMatlabTracker::Reset()
{
184
	iTrackedSoundReceiverID = -1;
185 186
	iTrackedSoundReceiverHeadRigidBodyIndex = 1;
	iTrackedSoundReceiverTorsoRigidBodyIndex = 1;
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
187
	vTrackedSoundReceiverTranslation.SetToZeroVector();
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
188
	qTrackedSoundReceiverRotation.SetToNeutralQuaternion();
189

190
	iTrackedRealWorldSoundReceiverID = -1;
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
191
	iTrackedRealWorldSoundReceiverRigidBodyIndex = 1;
192
	iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex = 1;
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
193 194
	vTrackedRealWorldSoundReceiverTranslation.SetToZeroVector();
	qTrackedRealWorldSoundReceiverRotation.SetToNeutralQuaternion();
195

196
	iTrackedSourceID = -1;
197 198 199
	iTrackedSourceRigidBodyIndex = 1;
	vTrackedSourceTranslation.SetToZeroVector();
	qTrackedSourceRotation.SetToNeutralQuaternion();
200 201 202 203
}

bool CVAMatlabTracker::Initialize( std::string sServerAdress, std::string sLocalAdress )
{
204 205
	char* pcLocalAdress = &sLocalAdress[ 0 ];
	char* pcServerAdress = &sServerAdress[ 0 ];
206 207 208 209 210 211 212 213 214 215 216 217 218 219 220
	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;
221
}