From 93d795d794edc043358c5b05072e454610a88be1 Mon Sep 17 00:00:00 2001 From: r57zone Date: Tue, 21 Apr 2020 22:47:29 +0400 Subject: [PATCH] Fix --- .../samples/driver_sample/driver_sample.cpp | 209 +++++++++--------- 1 file changed, 106 insertions(+), 103 deletions(-) diff --git a/OpenVR/UDP/samples/driver_sample/driver_sample.cpp b/OpenVR/UDP/samples/driver_sample/driver_sample.cpp index 24e7cc9..505eab3 100644 --- a/OpenVR/UDP/samples/driver_sample/driver_sample.cpp +++ b/OpenVR/UDP/samples/driver_sample/driver_sample.cpp @@ -9,7 +9,9 @@ #include #if defined( _WINDOWS ) -#include +//#include +#include +#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;