VAMatlabTracking.cpp 6.14 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12
#include "VAMatlabTracking.h"
#include "VAMatlabConnection.h"

#include <VACore.h>
#include <VAException.h>

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

// Tracking callback function
void TrackerDataHandler( sFrameOfMocapData* sFrame, void* pUserData )
13 14 15
{
	CVAMatlabTracker* pVAMatlabTracker = ( CVAMatlabTracker* ) pUserData;

16 17 18 19 20 21 22
	if( !pVAMatlabTracker )
		return;

	IVACore* pVACore = pVAMatlabTracker->pVACore;
	if( !pVACore )
		return;

23
	// Listener
24
	int iTrackedListenerID = pVAMatlabTracker->iTrackedListenerID;
25
	int iTrackedListenerRigidBodyIndex = pVAMatlabTracker->iTrackedListenerRigidBodyIndex;
26

27
	if( ( iTrackedListenerID != -1 ) && ( iTrackedListenerRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedListenerRigidBodyIndex > 0 ) )
28
	{
29
		try
30
		{
31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46
			VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedListenerTranslation;
			VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedListenerRotation;

			const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedListenerRigidBodyIndex - 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->SetListenerPosition( iTrackedListenerID, vPosRigidBody[ 0 ], vPosRigidBody[ 1 ], vPosRigidBody[ 2 ] );
			pVACore->SetListenerOrientationVU( iTrackedListenerID, vViewRigidBody[ 0 ], vViewRigidBody[ 1 ], vViewRigidBody[ 2 ], vUpRigidBody[ 0 ], vUpRigidBody[ 1 ], vUpRigidBody[ 2 ] );
47
		}
48
		catch( ... )
49 50 51
		{
		}
	}
52 53 54

	// Real-world listener
	int iTrackedRealWorldListenerID = pVAMatlabTracker->iTrackedRealWorldListenerID;
55
	int iTrackedRealWorldListenerRigidBodyIndex = pVAMatlabTracker->iTrackedRealWorldListenerRigidBodyIndex;
56 57

	if( ( iTrackedRealWorldListenerID != -1 ) && ( iTrackedRealWorldListenerRigidBodyIndex <= sFrame->nRigidBodies ) && ( iTrackedRealWorldListenerRigidBodyIndex > 0 ) )
58
	{
59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
		try
		{
			VistaVector3D vPosOffsetLocalCoordinateSystem = pVAMatlabTracker->vTrackedRealWorldListenerTranslation;
			VistaQuaternion qOrientRotation = pVAMatlabTracker->qTrackedRealWorldListenerRotation;

			const sRigidBodyData& oBodyData( sFrame->RigidBodies[ iTrackedRealWorldListenerRigidBodyIndex - 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->SetListenerRealWorldHeadPositionOrientationVU( iTrackedRealWorldListenerID, vPosRigidBody[ 0 ], vPosRigidBody[ 1 ], vPosRigidBody[ 2 ], vViewRigidBody[ 0 ], vViewRigidBody[ 1 ], vViewRigidBody[ 2 ], vUpRigidBody[ 0 ], vUpRigidBody[ 1 ], vUpRigidBody[ 2 ] );
		}
		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, vPosRigidBody[ 0 ], vPosRigidBody[ 1 ], vPosRigidBody[ 2 ] );
			pVACore->SetSoundSourceOrientationVU( iTrackedSourceID, vViewRigidBody[ 0 ], vViewRigidBody[ 1 ], vViewRigidBody[ 2 ], vUpRigidBody[ 0 ], vUpRigidBody[ 1 ], vUpRigidBody[ 2 ] );
		}
		catch( ... )
		{
		}
110
	}
111 112

	return;
113 114 115 116 117 118 119 120 121 122 123 124 125 126
}

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

void CVAMatlabTracker::Reset()
{
	iTrackedListenerID = -1;
127 128 129 130 131 132 133 134 135
	iTrackedListenerRigidBodyIndex = 1;
	vTrackedListenerTranslation.SetToZeroVector();
	qTrackedListenerRotation.SetToNeutralQuaternion();

	iTrackedRealWorldListenerID = -1;
	iTrackedRealWorldListenerRigidBodyIndex = 1;
	vTrackedRealWorldListenerTranslation.SetToZeroVector();
	qTrackedRealWorldListenerRotation.SetToNeutralQuaternion();

136
	iTrackedSourceID = -1;
137 138 139
	iTrackedSourceRigidBodyIndex = 1;
	vTrackedSourceTranslation.SetToZeroVector();
	qTrackedSourceRotation.SetToNeutralQuaternion();
140 141 142 143
}

bool CVAMatlabTracker::Initialize( std::string sServerAdress, std::string sLocalAdress )
{
144 145
	char* pcLocalAdress = &sLocalAdress[ 0 ];
	char* pcServerAdress = &sServerAdress[ 0 ];
146 147 148 149 150 151 152 153 154 155 156 157 158 159 160
	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;
161
}