VAMatlabTracking.cpp 8.75 KB
Newer Older
1 2 3 4 5 6
/*
 *  --------------------------------------------------------------------------------------------
 *
 *    VVV        VVV A           Virtual Acoustics (VA) | http://www.virtualacoustics.org
 *     VVV      VVV AAA          Licensed under the Apache License, Version 2.0
 *      VVV    VVV   AAA
7
 *       VVV  VVV     AAA        Copyright 2015-2018
8 9 10 11 12 13
 *        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
			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 ] ) );

64 65 66 67 68
				// 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 );
69

70 71
					VistaQuaternion qHATOOrient = qOrientRaw * qOrientRigidBody;
					VAQuat qFinalHATO = VAQuat( qHATOOrient[ Vista::X ], qHATOOrient[ Vista::Y ], qHATOOrient[ Vista::Z ], qHATOOrient[ Vista::W ] );
72

73 74
					pVACore->SetSoundReceiverHeadAboveTorsoOrientation( iTrackedReceiverID, qFinalHATO );
				}
75 76 77 78
			}
			catch( ... )
			{
			}
79
		}
80

81
	}
82

83
	// Real-world sound receiver
84
	int iTrackedRealWorldSoundReceiverID = pVAMatlabTracker->iTrackedRealWorldSoundReceiverID;
85 86
	int iTrackedRealWorldSoundReceiverHeadRigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverHeadRigidBodyIndex;
	int iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex;
87

88
	if( iTrackedRealWorldSoundReceiverID != -1 )
89
	{
90
		// Real world receiver tracking
91
		if ((iTrackedRealWorldSoundReceiverHeadRigidBodyIndex <= sFrame->nRigidBodies) && (iTrackedRealWorldSoundReceiverHeadRigidBodyIndex > 0))
92
		{
93 94 95 96 97
			try
			{
				VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedRealWorldSoundReceiverTranslation;
				VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedRealWorldSoundReceiverRotation;

98
				const sRigidBodyData& oBodyData(sFrame->RigidBodies[iTrackedRealWorldSoundReceiverHeadRigidBodyIndex - 1]);
99 100 101 102 103 104 105 106 107 108 109 110 111
				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 ] ) );


112
				// Real world receiver HATO orientation
113
				if ((iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex <= sFrame->nRigidBodies) && (iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex > 0))
114 115
				{

116
					const sRigidBodyData& oBodyData(sFrame->RigidBodies[iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex - 1]);
117 118 119 120 121 122 123 124
					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 );

				}
125 126 127 128
			}
			catch( ... )
			{
			}
129 130 131 132
		}
	}

	// Source
133 134
	int iTrackedSourceID = pVAMatlabTracker->iTrackedSoundSourceID;
	int iTrackedSourceRigidBodyIndex = pVAMatlabTracker->iTrackedSoundSourceRigidBodyIndex;
135 136 137 138 139

	if( ( iTrackedSourceID != -1 ) && ( iTrackedSourceRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedSourceRigidBodyIndex > 0 ) )
	{
		try
		{
140 141
			VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedSoundSourceTranslation;
			VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedSoundSourceRotation;
142 143 144 145 146 147 148 149 150 151 152 153

			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;

154 155
			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 ] ) );
156 157 158 159
		}
		catch( ... )
		{
		}
160
	}
161 162

	return;
163 164 165 166 167 168 169 170 171 172 173 174 175
}

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

void CVAMatlabTracker::Reset()
{
176
	iTrackedSoundReceiverID = -1;
177 178
	iTrackedSoundReceiverHeadRigidBodyIndex = 1;
	iTrackedSoundReceiverTorsoRigidBodyIndex = 1;
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
179
	vTrackedSoundReceiverTranslation.SetToZeroVector();
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
180
	qTrackedSoundReceiverRotation.SetToNeutralQuaternion();
181

182
	iTrackedRealWorldSoundReceiverID = -1;
183
	iTrackedRealWorldSoundReceiverHeadRigidBodyIndex = 1;
184
	iTrackedRealWorldSoundReceiverTorsoRigidBodyIndex = 1;
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
185 186
	vTrackedRealWorldSoundReceiverTranslation.SetToZeroVector();
	qTrackedRealWorldSoundReceiverRotation.SetToNeutralQuaternion();
187

188 189 190 191
	iTrackedSoundSourceID = -1;
	iTrackedSoundSourceRigidBodyIndex = 1;
	vTrackedSoundSourceTranslation.SetToZeroVector();
	qTrackedSoundSourceRotation.SetToNeutralQuaternion();
192 193 194 195
}

bool CVAMatlabTracker::Initialize( std::string sServerAdress, std::string sLocalAdress )
{
196 197
	char* pcLocalAdress = &sLocalAdress[ 0 ];
	char* pcServerAdress = &sServerAdress[ 0 ];
198 199 200 201 202 203 204 205 206 207 208 209 210 211 212
	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;
213
}