Fix

Author: r57zone

OpenVR/UDP/samples/driver_sample/driver_sample.cpp

@@ -9,7 +9,9 @@
 #include <chrono>
 
 #if defined( _WINDOWS )
-#include <windows.h>
+//#include <windows.h>
+#include <winsock2.h>
+#pragma comment (lib, "WSock32.Lib")
 #endif
 
 using namespace vr;
@@ -74,57 +76,35 @@ static const char * const k_pch_Sample_ScreenOffsetX_Int32 = "ScreenOffsetX";
 static const char * const k_pch_Sample_DebugMode_Bool = "DebugMode";
 
 
-#define FREETRACK_HEAP "FT_SharedMem"
-#define FREETRACK_MUTEX "FT_Mutext"
-
-/* only 6 headpose floats and the data id are filled -sh */
-typedef struct FTData__ {
-	uint32_t DataID;
-	int32_t CamWidth;
-	int32_t CamHeight;
-	/* virtual pose */
-	float  Yaw;   /* positive yaw to the left */
-	float  Pitch; /* positive pitch up */
-	float  Roll;  /* positive roll to the left */
-	float  X;
-	float  Y;
-	float  Z;
-	/* raw pose with no smoothing, sensitivity, response curve etc. */
-	float  RawYaw;
-	float  RawPitch;
-	float  RawRoll;
-	float  RawX;
-	float  RawY;
-	float  RawZ;
-	/* raw points, sorted by Y, origin top left corner */
-	float  X1;
-	float  Y1;
-	float  X2;
-	float  Y2;
-	float  X3;
-	float  Y3;
-	float  X4;
-	float  Y4;
-} volatile FTData;
-
-typedef struct FTHeap__ {
-	FTData data;
-	int32_t GameID;
-	union
-	{
-		unsigned char table[8];
-		int32_t table_ints[2];
-	};
-	int32_t GameID2;
-} volatile FTHeap;
+//OpenTrack vars
+double Yaw = 0, Pitch = 0, Roll = 0;
+double pX = 0, pY = 0, pZ = 0;
+struct TOpenTrack {
+	double X;
+	double Y;
+	double Z;
+	double Yaw;
+	double Pitch;
+	double Roll;
+};
+TOpenTrack OpenTrack;
+//WinSock
+SOCKET socketS;
+int bytes_read;
+struct sockaddr_in from;
+int fromlen;
+bool SocketActivated = false;
+bool bKeepReading = false;
 
-static HANDLE hFTMemMap = 0;
-static FTHeap *ipc_heap = 0;
-static HANDLE ipc_mutex = 0;
+std::thread *pSocketThread = NULL;
 
-FTData *FreeTrack;
-bool HMDConnected = false;
-std::thread *pFTthread = NULL;
+//-----------------------------------------------------------------------------
+// Purpose:
+//-----------------------------------------------------------------------------
+
+double DegToRad(double f) {
+	return f * (3.14159265358979323846 / 180);
+}
 
 inline vr::HmdQuaternion_t EulerAngleToQuaternion(double Yaw, double Pitch, double Roll)
 {
@@ -145,43 +125,30 @@ inline vr::HmdQuaternion_t EulerAngleToQuaternion(double Yaw, double Pitch, doub
 	return q;
 }
 
-//FreeTrack implementation from OpenTrack (https://github.com/opentrack/opentrack/tree/unstable/freetrackclient)
-static BOOL impl_create_mapping(void)
+void WinSockReadFunc()
 {
-	if (ipc_heap != NULL)
-		return TRUE;
-
-	hFTMemMap = CreateFileMappingA(INVALID_HANDLE_VALUE,
-		NULL,
-		PAGE_READWRITE,
-		0,
-		sizeof(FTHeap),
-		(LPCSTR)FREETRACK_HEAP);
-
-	if (hFTMemMap == NULL)
-		return (ipc_heap = NULL), FALSE;
-
-	ipc_heap = (FTHeap*)MapViewOfFile(hFTMemMap, FILE_MAP_WRITE, 0, 0, sizeof(FTHeap));
-	ipc_mutex = CreateMutexA(NULL, FALSE, FREETRACK_MUTEX);
-
-	return TRUE;
-}
-
-void FTRead()
-{
-	while (HMDConnected) {
-		if (ipc_mutex && WaitForSingleObject(ipc_mutex, 16) == WAIT_OBJECT_0) {
-			memcpy(&FreeTrack, &ipc_heap, sizeof(FreeTrack));
-			if (ipc_heap->data.DataID > (1 << 29))
-				ipc_heap->data.DataID = 0;
-			ReleaseMutex(ipc_mutex);
+	while (SocketActivated) {
+		//Read UDP socket with OpenTrack data
+		bKeepReading = true;
+		while (bKeepReading) {
+			memset(&OpenTrack, 0, sizeof(OpenTrack));
+			bytes_read = recvfrom(socketS, (char*)(&OpenTrack), sizeof(OpenTrack), 0, (sockaddr*)&from, &fromlen);
+
+			if (bytes_read > 0) {
+				Yaw = DegToRad(OpenTrack.Yaw);
+				Pitch = DegToRad(OpenTrack.Pitch);
+				Roll = DegToRad(OpenTrack.Roll);
+				pX = OpenTrack.X;
+				pY = OpenTrack.Y;
+				pZ = OpenTrack.Z;
+			}
+			else {
+				bKeepReading = false;
+			}
 		}
 	}
 }
 
-//-----------------------------------------------------------------------------
-// Purpose:
-//-----------------------------------------------------------------------------
 class CSampleDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDisplayComponent
 {
 public:
@@ -405,12 +372,14 @@ class CSampleDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IV
 	virtual DriverPose_t GetPose() 
 	{
 		DriverPose_t pose = { 0 };
-		if (HMDConnected) {
+
+		if (SocketActivated) {
 			pose.poseIsValid = true;
 			pose.result = TrackingResult_Running_OK;
 			pose.deviceIsConnected = true;
 		}
-		else {
+		else
+		{
 			pose.poseIsValid = false;
 			pose.result = TrackingResult_Uninitialized;
 			pose.deviceIsConnected = false;
@@ -419,15 +388,13 @@ class CSampleDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IV
 		pose.qWorldFromDriverRotation = HmdQuaternion_Init(1, 0, 0, 0);
 		pose.qDriverFromHeadRotation = HmdQuaternion_Init(1, 0, 0, 0);
 
-		if (HMDConnected) {
-			//Set head tracking rotation
-			pose.qRotation = EulerAngleToQuaternion(FreeTrack->Roll, -FreeTrack->Yaw, FreeTrack->Pitch);
+		//Set head tracking rotation
+		pose.qRotation = EulerAngleToQuaternion(Roll, -Yaw, Pitch);
 
-			//Set position tracking
-			pose.vecPosition[0] = FreeTrack->X * 0.001; //millimeters to meters
-			pose.vecPosition[1] = FreeTrack->Z * 0.001; //millimeters to meters 
-			pose.vecPosition[2] = FreeTrack->Y * 0.001; //millimeters to meters 
-		}
+		//Set position tracking
+		pose.vecPosition[0] = pX * 0.01;
+		pose.vecPosition[1] = pZ * 0.01;
+		pose.vecPosition[2] = pY * 0.01;
 
 		return pose;
 	}
@@ -498,12 +465,46 @@ EVRInitError CServerDriver_Sample::Init( vr::IVRDriverContext *pDriverContext )
 	VR_INIT_SERVER_DRIVER_CONTEXT( pDriverContext );
 	//InitDriverLog( vr::VRDriverLog() );
 
-	if (impl_create_mapping() == false) {
-		HMDConnected = false;
+	//Open UDP port for receive data from OpenTrack ("UDP over network", 127.0.0.1, 4242)
+	WSADATA wsaData;
+	int iResult;
+	iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
+	if (iResult == 0) {
+		struct sockaddr_in local;
+		fromlen = sizeof(from);
+		local.sin_family = AF_INET;
+		local.sin_port = htons(4242);
+		local.sin_addr.s_addr = INADDR_ANY;
+
+		socketS = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+
+		u_long nonblocking_enabled = true;
+		ioctlsocket(socketS, FIONBIO, &nonblocking_enabled);
+
+		if (socketS != INVALID_SOCKET) {
+
+			iResult = bind(socketS, (sockaddr*)&local, sizeof(local));
+
+			if (iResult != SOCKET_ERROR) {
+				SocketActivated = true;
+				pSocketThread = new std::thread(WinSockReadFunc);
+			}
+			else {
+				WSACleanup();
+				SocketActivated = false;
+			}
+
+		}
+		else {
+			WSACleanup();
+			SocketActivated = false;
+		}
+
 	}
-	else {
-		HMDConnected = true;
-		pFTthread = new std::thread(FTRead);
+	else
+	{
+		WSACleanup();
+		SocketActivated = false;
 	}
 
 	m_pNullHmdLatest = new CSampleDeviceDriver();
@@ -514,13 +515,15 @@ EVRInitError CServerDriver_Sample::Init( vr::IVRDriverContext *pDriverContext )
 
 void CServerDriver_Sample::Cleanup() 
 {
-	if (HMDConnected) {
-		HMDConnected = false;
-		if (pFTthread) {
-			pFTthread->join();
-			delete pFTthread;
-			pFTthread = nullptr;
+	if (SocketActivated) {
+		SocketActivated = false;
+		if (pSocketThread) {
+			pSocketThread->join();
+			delete pSocketThread;
+			pSocketThread = nullptr;
 		}
+		closesocket(socketS);
+		WSACleanup();
 	}
 	//CleanupDriverLog();
 	delete m_pNullHmdLatest;