├── .gitignore ├── LICENSE ├── LighthouseTracking.cpp ├── LighthouseTracking.h ├── README.md ├── build.py ├── chart.py ├── cpTime.c ├── cpTime.h ├── cylinder.cpp ├── cylinder.h ├── main.cpp ├── mirror-coords.py └── quat-test └── quat.c /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | /build/ 3 | 4 | *.pdb 5 | *.dll 6 | *.exe 7 | *.so 8 | *.bat 9 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Kevin Kellar 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /LighthouseTracking.cpp: -------------------------------------------------------------------------------- 1 | 2 | // The main file for dealing with VR specifically. See LighthouseTracking.h for descriptions of each function in the class. 3 | 4 | #include "LighthouseTracking.h" 5 | 6 | // Destructor for the LighthouseTracking object 7 | LighthouseTracking::~LighthouseTracking() 8 | { 9 | if (vr_pointer != NULL) 10 | { 11 | // VR Shutdown: https://github.com/ValveSoftware/openvr/wiki/API-Documentation#initialization-and-cleanup 12 | VR_Shutdown(); 13 | vr_pointer = NULL; 14 | } 15 | } 16 | 17 | // Constructor for the LighthouseTracking object 18 | LighthouseTracking::LighthouseTracking(InitFlags f) 19 | { 20 | flags = f; 21 | coordsBuf = new char[1024]; 22 | trackBuf = new char[1024]; 23 | rotBuf = new char[1024]; 24 | trackers = new TrackerData[16]; 25 | 26 | // Definition of the init error 27 | EVRInitError eError = VRInitError_None; 28 | 29 | /* 30 | VR_Init ( 31 | arg1: Pointer to EVRInitError type (enum defined in openvr.h) 32 | arg2: Must be of type EVRApplicationType 33 | 34 | The type of VR Applicaion. This example uses the SteamVR instance that is already running. 35 | Because of this, the init function will fail if SteamVR is not already running. 36 | 37 | Other EVRApplicationTypes include: 38 | * VRApplication_Scene - "A 3D application that will be drawing an environment."" 39 | * VRApplication_Overlay - "An application that only interacts with overlays or the dashboard."" 40 | * VRApplication_Utility 41 | */ 42 | 43 | vr_pointer = VR_Init(&eError, VRApplication_Background); 44 | 45 | // If the init failed because of an error 46 | if (eError != VRInitError_None) 47 | { 48 | vr_pointer = NULL; 49 | printf("Unable to init VR runtime: %s \n", VR_GetVRInitErrorAsEnglishDescription(eError)); 50 | exit(EXIT_FAILURE); 51 | } 52 | 53 | //If the init didn't fail, init the Cylinder object array 54 | cylinders = new Cylinder*[MAX_CYLINDERS]; 55 | for(int i = 0 ; i < MAX_CYLINDERS; i++) 56 | { 57 | cylinders[i] = new Cylinder(); 58 | } 59 | } 60 | 61 | 62 | bool LighthouseTracking::RunProcedure() 63 | { 64 | // Define a VREvent 65 | VREvent_t event; 66 | if(vr_pointer->PollNextEvent(&event, sizeof(event))) 67 | { 68 | /* 69 | ProcessVREvent is a function defined in this module. It returns false if 70 | the function determines the type of error to be fatal or signal some kind of quit. 71 | */ 72 | if (!ProcessVREvent(event)) 73 | { 74 | // If ProcessVREvent determined that OpenVR quit, print quit message 75 | printf("\nEVENT--(OpenVR) service quit"); 76 | return false; 77 | } 78 | } 79 | // ParseTrackingFrame() is where the tracking and vibration code starts 80 | ParseTrackingFrame(); 81 | return true; 82 | } 83 | 84 | bool LighthouseTracking::ProcessVREvent(const VREvent_t & event) 85 | { 86 | char* buf = new char[100]; 87 | bool ret = true; 88 | switch (event.eventType) 89 | { 90 | case VREvent_TrackedDeviceActivated: 91 | sprintf(buf, "\nEVENT--(OpenVR) Device : %d attached", event.trackedDeviceIndex); 92 | break; 93 | 94 | case VREvent_TrackedDeviceDeactivated: 95 | sprintf(buf, "\nEVENT--(OpenVR) Device : %d detached", event.trackedDeviceIndex); 96 | break; 97 | 98 | case VREvent_TrackedDeviceUpdated: 99 | sprintf(buf, "\nEVENT--(OpenVR) Device : %d updated", event.trackedDeviceIndex); 100 | break; 101 | 102 | case VREvent_DashboardActivated: 103 | sprintf(buf, "\nEVENT--(OpenVR) Dashboard activated"); 104 | break; 105 | 106 | case VREvent_DashboardDeactivated: 107 | sprintf(buf, "\nEVENT--(OpenVR) Dashboard deactivated"); 108 | break; 109 | 110 | case VREvent_ChaperoneDataHasChanged: 111 | sprintf(buf, "\nEVENT--(OpenVR) Chaperone data has changed"); 112 | break; 113 | 114 | case VREvent_ChaperoneSettingsHaveChanged: 115 | sprintf(buf, "\nEVENT--(OpenVR) Chaperone settings have changed"); 116 | break; 117 | 118 | case VREvent_ChaperoneUniverseHasChanged: 119 | sprintf(buf, "\nEVENT--(OpenVR) Chaperone universe has changed"); 120 | break; 121 | 122 | case VREvent_ApplicationTransitionStarted: 123 | sprintf(buf, "\nEVENT--(OpenVR) Application Transition: Transition has started"); 124 | break; 125 | 126 | case VREvent_ApplicationTransitionNewAppStarted: 127 | sprintf(buf, "\nEVENT--(OpenVR) Application transition: New app has started"); 128 | break; 129 | 130 | case VREvent_Quit: 131 | { 132 | sprintf(buf, "\nEVENT--(OpenVR) Received SteamVR Quit (%d%s", VREvent_Quit, ")"); 133 | ret = false; 134 | } 135 | break; 136 | 137 | case VREvent_ProcessQuit: 138 | { 139 | sprintf(buf, "\nEVENT--(OpenVR) SteamVR Quit Process (%d%s", VREvent_ProcessQuit, ")"); 140 | ret = false; 141 | } 142 | break; 143 | 144 | case VREvent_QuitAborted_UserPrompt: 145 | { 146 | sprintf(buf, "\nEVENT--(OpenVR) SteamVR Quit Aborted UserPrompt (%d%s", VREvent_QuitAborted_UserPrompt, ")"); 147 | ret = false; 148 | } 149 | break; 150 | 151 | case VREvent_QuitAcknowledged: 152 | { 153 | sprintf(buf, "\nEVENT--(OpenVR) SteamVR Quit Acknowledged (%d%s", VREvent_QuitAcknowledged, ")"); 154 | ret = false; 155 | } 156 | break; 157 | 158 | case VREvent_TrackedDeviceRoleChanged: 159 | sprintf(buf, "\nEVENT--(OpenVR) TrackedDeviceRoleChanged: %d", event.trackedDeviceIndex); 160 | break; 161 | 162 | case VREvent_TrackedDeviceUserInteractionStarted: 163 | sprintf(buf, "\nEVENT--(OpenVR) TrackedDeviceUserInteractionStarted: %d", event.trackedDeviceIndex); 164 | break; 165 | 166 | default: 167 | if (event.eventType >= 200 && event.eventType <= 203) //Button events range from 200-203 168 | dealWithButtonEvent(event); 169 | else 170 | sprintf(buf, "\nEVENT--(OpenVR) Event: %d", event.eventType); 171 | // Check entire event list starts on line #452: https://github.com/ValveSoftware/openvr/blob/master/headers/openvr.h 172 | 173 | } 174 | if(flags.printEvents) 175 | printf("%s",buf); 176 | return ret; 177 | } 178 | 179 | 180 | 181 | 182 | //This method deals exclusively with button events 183 | void LighthouseTracking::dealWithButtonEvent(VREvent_t event) 184 | { 185 | int controllerIndex; //The index of the controllers[] array that corresponds with the controller that had a buttonEvent 186 | for (int i = 0; i < 2; i++) //Iterates across the array of controllers 187 | { 188 | ControllerData* pController = &(controllers[i]); 189 | if(flags.printBEvents && event.trackedDeviceIndex == pController->deviceId) //prints the event data to the terminal 190 | printf("\nBUTTON-E--index=%d deviceId=%d hand=%d button=%d event=%d",i,pController->deviceId,pController->hand,event.data.controller.button,event.eventType); 191 | if(pController->deviceId == event.trackedDeviceIndex) //This tests to see if the current controller from the loop is the same from the event 192 | controllerIndex = i; 193 | } 194 | 195 | ControllerData* pC = &(controllers[controllerIndex]); //The pointer to the ControllerData struct 196 | 197 | if (event.data.controller.button == k_EButton_ApplicationMenu //Test if the ApplicationButton was pressed 198 | && event.eventType == VREvent_ButtonUnpress) //Test if the button is being released (the action happens on release, not press) 199 | { 200 | inDrawingMode = !inDrawingMode; 201 | doRumbleNow = true; 202 | } 203 | if(inDrawingMode) 204 | switch( event.data.controller.button ) 205 | { 206 | case k_EButton_Grip: //If it is the grip button that was... 207 | switch(event.eventType) 208 | { 209 | case VREvent_ButtonPress: // ...pressed... 210 | if(cpMillis() - gripMillis > 500) // ...and it's been half a second since the grip was last released... 211 | cylinders[cylinderIndex]->s1[1] = pC->pos.v[1]; //...then set the cylinder's y 1 to the controllers y coordinate. 212 | break; 213 | 214 | case VREvent_ButtonUnpress: // ...released... 215 | if(cpMillis() - gripMillis > 500) // ...and it's been half a second since the grip was last released... 216 | cylinders[cylinderIndex]->s2[1] = pC->pos.v[1]; //...then set the cylinder's y 2 to the controllers y coordinate. 217 | else // ...and it' hasn't been half a second since the grip was last released... 218 | { 219 | if(cylinders[cylinderIndex]->s1[1] > pC->pos.v[1]) // ...if the controller's position is **below** the starting position... 220 | cylinders[cylinderIndex]->s2[1] = -std::numeric_limits::max(); // ...set the cylinder's y 2 to negative infinity. 221 | else // ...if the controller's position is **above** the starting position... 222 | cylinders[cylinderIndex]->s2[1] = std::numeric_limits::max(); // ...set the cylinder's y 2 to positive infinity. 223 | } 224 | 225 | cylinders[cylinderIndex]->init(); 226 | gripMillis = cpMillis(); 227 | break; 228 | } 229 | break; 230 | 231 | case k_EButton_SteamVR_Trigger: 232 | switch(event.eventType) 233 | { 234 | case VREvent_ButtonPress: //If the trigger was pressed... 235 | cylinders[cylinderIndex]->s1[0] = pC->pos.v[0]; //Set the cylinder's x 1 to the controller's x 236 | cylinders[cylinderIndex]->s1[2] = pC->pos.v[2]; //Set the cylinder's z 1 to the controller's z 237 | break; 238 | 239 | case VREvent_ButtonUnpress://If the trigger was released... 240 | cylinders[cylinderIndex]->s2[0] = pC->pos.v[0]; //Set the cylinder's x 2 to the controller's x 241 | cylinders[cylinderIndex]->s2[2] = pC->pos.v[2]; //Set the cylinder's z 2 to the controller's z 242 | cylinders[cylinderIndex]->init(); 243 | break; 244 | } 245 | break; 246 | 247 | case k_EButton_SteamVR_Touchpad: 248 | switch(event.eventType) 249 | { 250 | case VREvent_ButtonPress: 251 | 252 | break; 253 | 254 | case VREvent_ButtonUnpress://If the touchpad was just pressed 255 | if(std::abs(pC->padX) > std::abs(pC->padY)) //Tests if the left or right of the pad was pressed 256 | { 257 | if (pC->padX < 0 && cylinderIndex != 0) //If left side of pad was pressed and there is a previous cylinder 258 | cylinderIndex = cylinderIndex-1; //Switch index to previous cylinder 259 | else if (pC->padX > 0 && cylinderIndex < MAX_CYLINDERS) //If the right side of the pad was pressed 260 | cylinderIndex = cylinderIndex+1; //Switch the index to the next cylinder 261 | doRumbleNow = true; 262 | } 263 | else //If the top/bottom of the pad was pressed 264 | { 265 | if (pC->padY > 0) //If the top was pressed 266 | doRumbleNow = true; 267 | else if (pC->padY < 0) //If the bottom was pressed, reset the current cylinder 268 | cylinders[cylinderIndex] = new Cylinder(); 269 | } 270 | break; 271 | } 272 | break; 273 | } 274 | 275 | } 276 | 277 | HmdVector3_t LighthouseTracking::GetPosition(HmdMatrix34_t matrix) 278 | { 279 | HmdVector3_t vector; 280 | 281 | vector.v[0] = matrix.m[0][3]; 282 | vector.v[1] = matrix.m[1][3]; 283 | vector.v[2] = matrix.m[2][3]; 284 | 285 | return vector; 286 | } 287 | long lastPRCall = 0; 288 | HmdQuaternion_t LighthouseTracking::GetRotation(HmdMatrix34_t matrix) 289 | { 290 | HmdQuaternion_t q; 291 | 292 | q.w = sqrt(fmax(0, 1 + matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2])) / 2; 293 | q.x = sqrt(fmax(0, 1 + matrix.m[0][0] - matrix.m[1][1] - matrix.m[2][2])) / 2; 294 | q.y = sqrt(fmax(0, 1 - matrix.m[0][0] + matrix.m[1][1] - matrix.m[2][2])) / 2; 295 | q.z = sqrt(fmax(0, 1 - matrix.m[0][0] - matrix.m[1][1] + matrix.m[2][2])) / 2; 296 | q.x = copysign(q.x, matrix.m[2][1] - matrix.m[1][2]); 297 | q.y = copysign(q.y, matrix.m[0][2] - matrix.m[2][0]); 298 | q.z = copysign(q.z, matrix.m[1][0] - matrix.m[0][1]); 299 | } 300 | 301 | HmdQuaternion_t LighthouseTracking::ProcessRotation(HmdQuaternion_t quat) 302 | { 303 | HmdQuaternion_t out; 304 | out.w = 2 * acos(quat.w); 305 | out.x = quat.x / sin(out.w/2); 306 | out.y = quat.y / sin(out.w/2); 307 | out.z = quat.z / sin(out.w/2); 308 | 309 | printf("\nPROCESSED w:%.3f x:%.3f y:%.3f z:%.3f",out.w,out.x,out.y,out.z); 310 | return out; 311 | } 312 | 313 | 314 | void LighthouseTracking::iterateAssignIds() 315 | { 316 | //Un-assigns the deviceIds and hands of controllers. If they are truely connected, will be re-assigned later in this function 317 | controllers[0].deviceId = -1; 318 | controllers[1].deviceId = -1; 319 | controllers[0].hand = -1; 320 | controllers[1].hand = -1; 321 | 322 | int numTrackersInitialized = 0; 323 | int numControllersInitialized = 0; 324 | 325 | for (unsigned int i = 0; i < k_unMaxTrackedDeviceCount; i++) // Iterates across all of the potential device indicies 326 | { 327 | if (!vr_pointer->IsTrackedDeviceConnected(i)) 328 | continue; //Doesn't use the id if the device isn't connected 329 | 330 | //vr_pointer points to the VRSystem that was in init'ed in the constructor. 331 | ETrackedDeviceClass trackedDeviceClass = vr_pointer->GetTrackedDeviceClass(i); 332 | 333 | //Finding the type of device 334 | if (trackedDeviceClass == ETrackedDeviceClass::TrackedDeviceClass_HMD) 335 | { 336 | hmdDeviceId = i; 337 | if(flags.printSetIds) 338 | printf("\nSETID--Assigned hmdDeviceId=%d",hmdDeviceId); 339 | } 340 | else if (trackedDeviceClass == ETrackedDeviceClass::TrackedDeviceClass_Controller && numControllersInitialized < 2) 341 | { 342 | ControllerData* pC = &(controllers[numControllersInitialized]); 343 | 344 | int sHand = -1; 345 | 346 | ETrackedControllerRole role = vr_pointer->GetControllerRoleForTrackedDeviceIndex(i); 347 | if (role == TrackedControllerRole_Invalid) //Invalid hand is actually very common, always need to test for invalid hand (lighthouses have lost tracking) 348 | sHand = 0; 349 | else if (role == TrackedControllerRole_LeftHand) 350 | sHand = 1; 351 | else if (role == TrackedControllerRole_RightHand) 352 | sHand = 2; 353 | pC->hand = sHand; 354 | pC->deviceId = i; 355 | 356 | 357 | //Used to get/store property ids for the xy of the pad and the analog reading of the trigger 358 | for(int x=0; xGetInt32TrackedDeviceProperty(pC->deviceId, 361 | (ETrackedDeviceProperty)(Prop_Axis0Type_Int32 + x)); 362 | 363 | if( prop==k_eControllerAxis_Trigger ) 364 | pC->idtrigger = x; 365 | else if( prop==k_eControllerAxis_TrackPad ) 366 | pC->idpad = x; 367 | } 368 | if(flags.printSetIds) 369 | printf("\nSETID--Assigned controllers[%d] .hand=%d .deviceId=%d .idtrigger=%d .idpad=%d",numControllersInitialized,sHand, i , pC->idtrigger, pC->idpad); 370 | numControllersInitialized++; //Increment this count so that the other controller gets initialized after initializing this one 371 | } 372 | else if(trackedDeviceClass == ETrackedDeviceClass::TrackedDeviceClass_GenericTracker) 373 | { 374 | TrackerData* pT = &(trackers[numTrackersInitialized]); 375 | pT->deviceId = i; 376 | if(flags.printSetIds) 377 | printf("\nSETID--Assigned tracker[%d] .deviceId=%d",numTrackersInitialized,pT->deviceId); 378 | numTrackersInitialized++; 379 | } 380 | 381 | } 382 | } 383 | 384 | void LighthouseTracking::setHands() 385 | { 386 | for (int z =0; z < 2; z++) 387 | { 388 | ControllerData* pC = &(controllers[z]); 389 | if (pC->deviceId < 0 || !vr_pointer->IsTrackedDeviceConnected(pC->deviceId)) 390 | continue; 391 | int sHand = -1; 392 | //Invalid hand is actually very common, always need to test for invalid hand (lighthouses have lost tracking) 393 | ETrackedControllerRole role = vr_pointer->GetControllerRoleForTrackedDeviceIndex(pC->deviceId); 394 | if (role == TrackedControllerRole_Invalid) 395 | sHand = 0; 396 | else if (role == TrackedControllerRole_LeftHand) 397 | sHand = 1; 398 | else if (role == TrackedControllerRole_RightHand) 399 | sHand = 2; 400 | pC->hand = sHand; 401 | } 402 | } 403 | 404 | void LighthouseTracking::ParseTrackingFrame() 405 | { 406 | //Runs the iterateAssignIds() method if... 407 | if(hmdDeviceId < 0 || // HMD id not yet initialized 408 | controllers[0].deviceId < 0 || // One of the controllers not yet initialized 409 | controllers[1].deviceId < 0 || 410 | controllers[0].deviceId == controllers[1].deviceId || //Both controllerData structs store the same deviceId 411 | controllers[0].hand == controllers[1].hand || //Both controllerData structs are the same hand 412 | (cpMillis() / 60000) > minuteCount) //It has been a minute since last init time 413 | { 414 | minuteCount = (cpMillis() / 60000); 415 | iterateAssignIds(); 416 | } 417 | HMDCoords(); 418 | ControllerCoords(); 419 | TrackerCoords(); 420 | if(flags.printCoords) 421 | printf("\nCOORDS-- %s",coordsBuf); 422 | if(flags.printTrack) 423 | printf("\nTRACK-- %s",trackBuf); 424 | if(flags.printRotation) 425 | printf("\nROT-- %s",rotBuf); 426 | } 427 | 428 | void LighthouseTracking::HMDCoords() 429 | { 430 | if (!vr_pointer->IsTrackedDeviceConnected(hmdDeviceId)) 431 | return; 432 | 433 | //TrackedDevicePose_t struct is a OpenVR struct. See line 180 in the openvr.h header. 434 | TrackedDevicePose_t trackedDevicePose; 435 | HmdVector3_t position; 436 | HmdQuaternion_t rot; 437 | if (vr_pointer->IsInputFocusCapturedByAnotherProcess()) 438 | printf( "\nINFO--Input Focus by Another Process"); 439 | vr_pointer->GetDeviceToAbsoluteTrackingPose(TrackingUniverseStanding, 0, &trackedDevicePose, 1); 440 | position = GetPosition(trackedDevicePose.mDeviceToAbsoluteTracking); 441 | rot = GetRotation(trackedDevicePose.mDeviceToAbsoluteTracking); 442 | sprintf(coordsBuf,"HMD %-28.28s", getPoseXYZString(trackedDevicePose,0)); 443 | sprintf(trackBuf,"HMD: %-25.25s %-7.7s " , getEnglishTrackingResultForPose(trackedDevicePose) , getEnglishPoseValidity(trackedDevicePose)); 444 | sprintf(rotBuf,"HMD: qw:%.2f qx:%.2f qy:%.2f qz:%.2f",rot.w,rot.x,rot.y,rot.z); 445 | } 446 | 447 | void LighthouseTracking::ControllerCoords() 448 | { 449 | setHands(); 450 | if(doRumbleNow) 451 | { 452 | rumbleMsOffset = cpMillis(); 453 | doRumbleNow = false; 454 | } 455 | 456 | TrackedDevicePose_t trackedDevicePose; 457 | VRControllerState_t controllerState; 458 | HmdQuaternion_t rot; 459 | 460 | //Arrays to contain information about the results of the button state sprintf call 461 | // so that the button state information can be printed all on one line for both controllers 462 | char** bufs = new char*[2]; 463 | bool* isOk = new bool[2]; 464 | 465 | //Stores the number of times 150ms have elapsed (loops with the % operator because 466 | // the "cylinder count" rumbling starts when indexN is one). 467 | int indexN = ((cpMillis()-rumbleMsOffset)/150)%(125); 468 | 469 | //Loops for each ControllerData struct 470 | for(int i = 0; i < 2; i++) 471 | { 472 | isOk[i] = false; 473 | char* buf = new char[100]; 474 | ControllerData* pC = &(controllers[i]); 475 | 476 | if (pC->deviceId < 0 || 477 | !vr_pointer->IsTrackedDeviceConnected(pC->deviceId) || 478 | pC->hand GetControllerStateWithPose(TrackingUniverseStanding, pC->deviceId, &controllerState, sizeof(controllerState), &trackedDevicePose); 482 | pC->pos = GetPosition(trackedDevicePose.mDeviceToAbsoluteTracking); 483 | rot = GetRotation(trackedDevicePose.mDeviceToAbsoluteTracking); 484 | 485 | char handString[6]; 486 | 487 | if (pC->hand == 1) 488 | sprintf(handString, "LEFT"); 489 | else if (pC->hand == 2) 490 | sprintf(handString, "RIGHT"); 491 | else if(pC->hand == 0) 492 | sprintf(handString, "INVALID"); 493 | 494 | pC->isValid =trackedDevicePose.bPoseIsValid; 495 | 496 | 497 | sprintf(coordsBuf,"%s %s: %-28.28s",coordsBuf, handString, getPoseXYZString(trackedDevicePose,pC->hand)); 498 | sprintf(trackBuf,"%s %s: %-25.25s %-7.7s" , trackBuf, handString, getEnglishTrackingResultForPose(trackedDevicePose), getEnglishPoseValidity(trackedDevicePose)); 499 | sprintf(rotBuf,"%s %s qw:%.2f qx:%.2f qy:%.2f qz:%.2f",rotBuf,handString,rot.w,rot.x,rot.y,rot.z); 500 | 501 | int t = pC->idtrigger; 502 | int p = pC->idpad; 503 | 504 | //This is the call to get analog button data from the controllers 505 | pC->trigVal = controllerState.rAxis[t].x; 506 | pC->padX = controllerState.rAxis[p].x; 507 | pC->padY = controllerState.rAxis[p].y; 508 | 509 | sprintf(buf,"hand=%s handid=%d trigger=%f padx=%f pady=%f", handString, pC->hand , pC->trigVal , pC->padX , pC->padY); 510 | bufs[i] = buf; 511 | isOk[i] = true; 512 | 513 | //The following block controlls the rumbling of the controllers 514 | if(!inDrawingMode) //Will iterate across all cylinders if in sensing mode 515 | for(int x = 0; x < MAX_CYLINDERS; x++) 516 | { 517 | Cylinder* currCy = cylinders[x]; 518 | if(currCy->hasInit && 519 | currCy->isInside(pC->pos.v[0],pC->pos.v[1],pC->pos.v[2])) 520 | vr_pointer->TriggerHapticPulse(pC->deviceId,pC->idpad,500); //Vibrates if the controller is colliding with the cylinder bounds 521 | } 522 | if (inDrawingMode && indexN % 3 == 0 && indexN < (cylinderIndex+1)*3) //Vibrates the current cylinderIndex every thirty seconds or so 523 | vr_pointer->TriggerHapticPulse(pC->deviceId,pC->idpad,300); // see the definition of indexN above before the for loop 524 | } 525 | 526 | if(flags.printAnalog && isOk[0] == true) 527 | { 528 | printf("\nANALOG-- %s", bufs[0]); 529 | if(isOk[1] == true) 530 | { 531 | printf(" %s", bufs[1]); 532 | } 533 | } 534 | } 535 | 536 | void LighthouseTracking::TrackerCoords() 537 | { 538 | TrackedDevicePose_t trackedDevicePose; 539 | VRControllerState_t controllerState; 540 | HmdQuaternion_t rot; 541 | 542 | for(int i = 0; i < 16; i++) 543 | { 544 | TrackerData* pT = &(trackers[i]); 545 | 546 | if (pT->deviceId < 0 || 547 | !vr_pointer->IsTrackedDeviceConnected(pT->deviceId) ) 548 | continue; 549 | 550 | vr_pointer->GetControllerStateWithPose(TrackingUniverseStanding, pT->deviceId, &controllerState, sizeof(controllerState), &trackedDevicePose); 551 | pT->pos = GetPosition(trackedDevicePose.mDeviceToAbsoluteTracking); 552 | rot = GetRotation(trackedDevicePose.mDeviceToAbsoluteTracking); 553 | pT->isValid =trackedDevicePose.bPoseIsValid; 554 | 555 | sprintf(coordsBuf,"%s T%d: %-28.28s",coordsBuf, i, getPoseXYZString(trackedDevicePose,0)); 556 | sprintf(trackBuf,"%s T%d: %-25.25s %-7.7s" , trackBuf, i, getEnglishTrackingResultForPose(trackedDevicePose), getEnglishPoseValidity(trackedDevicePose)); 557 | sprintf(rotBuf,"%s T%d: qw:%.2f qx:%.2f qy:%.2f qz:%.2f",rotBuf,i,rot.w,rot.x,rot.y,rot.z); 558 | } 559 | } 560 | 561 | char* LighthouseTracking::getEnglishTrackingResultForPose(TrackedDevicePose_t pose) 562 | { 563 | char* buf = new char[50]; 564 | switch (pose.eTrackingResult) 565 | { 566 | case vr::ETrackingResult::TrackingResult_Uninitialized: 567 | sprintf(buf, "Invalid tracking result"); 568 | break; 569 | case vr::ETrackingResult::TrackingResult_Calibrating_InProgress: 570 | sprintf(buf, "Calibrating in progress"); 571 | break; 572 | case vr::ETrackingResult::TrackingResult_Calibrating_OutOfRange: 573 | sprintf(buf, "Calibrating Out of range"); 574 | break; 575 | case vr::ETrackingResult::TrackingResult_Running_OK: 576 | sprintf(buf, "Running OK"); 577 | break; 578 | case vr::ETrackingResult::TrackingResult_Running_OutOfRange: 579 | sprintf(buf, "WARNING: Running Out of Range"); 580 | break; 581 | default: 582 | sprintf(buf, "Default"); 583 | break; 584 | } 585 | return buf; 586 | } 587 | 588 | char* LighthouseTracking::getEnglishPoseValidity(TrackedDevicePose_t pose) 589 | { 590 | char* buf = new char[50]; 591 | if(pose.bPoseIsValid) 592 | sprintf(buf, "Valid"); 593 | else 594 | sprintf(buf, "Invalid"); 595 | return buf; 596 | } 597 | 598 | char* LighthouseTracking::getPoseXYZString(TrackedDevicePose_t pose, int hand) 599 | { 600 | HmdVector3_t pos = GetPosition(pose.mDeviceToAbsoluteTracking); 601 | char* cB = new char[50]; 602 | if(pose.bPoseIsValid) 603 | sprintf(cB, "x:%.3f y:%.3f z:%.3f",pos.v[0], pos.v[1], pos.v[2]); 604 | else 605 | sprintf(cB, " INVALID"); 606 | if(flags.pipeCoords) 607 | for(int i = 0; i < 3; i++) 608 | if(pose.bPoseIsValid) 609 | printf("%.5f\n",pos.v[i]); 610 | else 611 | printf("invalid\n",pos.v[i]); 612 | return cB; 613 | } 614 | 615 | -------------------------------------------------------------------------------- /LighthouseTracking.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef LIGHTHOUSETRACKING_H_ 3 | #define LIGHTHOUSETRACKING_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | using namespace vr; 10 | 11 | 12 | #include "cylinder.h" 13 | #include "cpTime.h" 14 | 15 | 16 | 17 | struct _InitFlags 18 | { 19 | bool printCoords = true; 20 | bool printAnalog = true; 21 | bool printEvents = true; 22 | bool printSetIds = true; 23 | bool printBEvents = true; 24 | bool printTrack = true; 25 | bool printRotation = true; 26 | bool pipeCoords = false; 27 | }; 28 | typedef struct _InitFlags InitFlags; 29 | 30 | class LighthouseTracking 31 | { 32 | 33 | private: 34 | //The pointer to the VR system for VR function calls. Initialized in the constructor. 35 | IVRSystem* vr_pointer = NULL; 36 | 37 | //Returns xyz coordinates from the matrix that is returned by the VR system functions 38 | // see the HMDCoords() and ControllerCoords() methods for usage 39 | HmdVector3_t GetPosition(HmdMatrix34_t matrix); 40 | 41 | HmdQuaternion_t GetRotation(vr::HmdMatrix34_t matrix); 42 | 43 | HmdQuaternion_t ProcessRotation(HmdQuaternion_t quat); 44 | 45 | InitFlags flags; 46 | 47 | public: 48 | 49 | //Destructor 50 | ~LighthouseTracking(); 51 | 52 | //Initializes the VR system and the array of cylinders 53 | LighthouseTracking(InitFlags f); 54 | 55 | /* Is called in a loop from the main methods in main.cpp 56 | * Returns false if the VR system has stopped. 57 | * First polls a VR event and then runs ProcessVREvent 58 | * Returns false (to exit the program) if ProcessVREvent returns false. 59 | * If the system hasn't shut down, calls ParseTrackingFrame() 60 | */ 61 | bool RunProcedure(); 62 | 63 | /* Takes a VREvent pointer when called by RunProcedure() 64 | * Switches through the common VREvents 65 | * If it is a button event (#200-203) then calls dealWithButtonEvent 66 | * Returns false if the event should cause the application to quit (like SteamVR quitting) 67 | */ 68 | bool ProcessVREvent(const VREvent_t & event); 69 | 70 | /* First prints the button event data to the terminal 71 | * Tests to see if it was the ApplicationMenu button that was pressed. If so, switch modes 72 | * Tests that it was a grip press or unpress event and stores the y-bounds for the current cylinder if applicable 73 | * Tests if it was a trigger press or unpress and stroes the x/z bounds for the current cylinder if applicable 74 | * Finally, tests if it was a touchpad press and increments, decrements, 75 | deletes or rumbles the current cylinder depending on where the press was 76 | */ 77 | void dealWithButtonEvent(VREvent_t); 78 | 79 | /* First decides whether or not to call iterateAssignIds() see the conditions in .cpp 80 | * Then calls HMDCoords() and ControllerCoords() 81 | */ 82 | void ParseTrackingFrame(); 83 | 84 | /* Iterates across all the possible device ids (I think the number is 16 for the Vive) 85 | * For each possible id, tests to see if it is the HMD or a controller 86 | * If it is the HMD, assign hmdDeviceId to the HMD's id 87 | * If it is a controller, assign one of the controller structs' deviceId to the id 88 | then set handedness and the axis ids of the controller. 89 | */ 90 | void iterateAssignIds(); 91 | 92 | /* One of the best methods to look at if just trying to learn how to print tracking data 93 | * In only about five lines, gets the HMD pose from the VR system, converts the pose to xyz 94 | coordinates, and prints this data to the terminal 95 | */ 96 | void HMDCoords(); 97 | 98 | /* For each controller: 99 | * Gets controller state and pose from the VR system 100 | * Prints controller coords 101 | * Gets/prints button states (analog data) from the conrollers 102 | * Rumbles the controllers based on the current mode/inputs 103 | */ 104 | void ControllerCoords(); 105 | 106 | /* CURRENTLY NOT BEING CALLED BY ANY FUNCTION 107 | * If called every frame before ControllerCoords, should result in smoother 108 | contoller reconnection after leaving room bounds 109 | */ 110 | void setHands(); 111 | 112 | void TrackerCoords(); 113 | 114 | char* getEnglishTrackingResultForPose(TrackedDevicePose_t pose); 115 | char* getEnglishPoseValidity(TrackedDevicePose_t pose); 116 | char* getPoseXYZString(TrackedDevicePose_t pose, int hand); 117 | 118 | struct _ControllerData 119 | { 120 | //Fields to be initialzed by iterateAssignIds() and setHands() 121 | int deviceId = -1; // Device ID according to the SteamVR system 122 | int hand = -1; // 0=invalid 1=left 2=right 123 | int idtrigger = -1; // Trigger axis id 124 | int idpad = -1; // Touchpad axis id 125 | 126 | //Analog button data to be set in ContollerCoods() 127 | float padX; 128 | float padY; 129 | float trigVal; 130 | 131 | //Position set in ControllerCoords() 132 | HmdVector3_t pos; 133 | 134 | bool isValid; 135 | }; 136 | typedef struct _ControllerData ControllerData; 137 | 138 | struct _TrackerData 139 | { 140 | int deviceId = -1; // Device ID according to the SteamVR system 141 | HmdVector3_t pos; 142 | bool isValid; 143 | }; 144 | typedef struct _TrackerData TrackerData; 145 | 146 | //An array of ControllerData structs 147 | ControllerData controllers[2]; 148 | TrackerData* trackers; 149 | 150 | //Number of minutes that have elapsed as set in ParseTrackingFrame() 151 | // used for deciding whether or not to run iterateAssignIds() 152 | long minuteCount = 0; 153 | 154 | int hmdDeviceId = -1; 155 | 156 | //Initialized in the constuctor as an array of Cylinder object pointers 157 | Cylinder** cylinders; 158 | 159 | //If not in drawing mode, then in sensing mode 160 | // gets set in dealWithButtonEvent() and tested in ControllerCoords() 161 | bool inDrawingMode = true; 162 | 163 | //The index of the array of cylinders that is currently being edited 164 | int cylinderIndex = 0; 165 | 166 | //If true, ControllerCoords() will set rumbleMsOffset to now to trigger rumble if in drawingMode 167 | bool doRumbleNow = false; 168 | unsigned long rumbleMsOffset = 0; 169 | 170 | //The number of ms when the grip button was released 171 | unsigned long gripMillis; 172 | 173 | unsigned const int MAX_CYLINDERS = 10; 174 | 175 | char* coordsBuf; 176 | char* trackBuf; 177 | char* rotBuf; 178 | 179 | }; 180 | 181 | #endif 182 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # What is this? 2 | 3 | This project is a rapid and simple example of how to start a background process for listening to HTC Vive Lighthouse tracking data. It was mostly written to serve as example code for the [OpenVR Quick Start Guide](https://github.com/osudrl/CassieVrControls/wiki/OpenVR-Quick-Start). The project interfaces with [Valve's OpenVR SDK](https://github.com/ValveSoftware/openvr). It was developed cross platform, so the project should compile on both Windows and Linux systems. If you find that this is not the case, open an issue or contact the developer. 4 | 5 | Make sure to see [this entry](https://github.com/osudrl/CassieVrControls/wiki/Troubleshooting#unable-to-init-vr-runtime-vrclient-shared-lib-not-found-102) on the [VR Controls](https://github.com/osudrl/CassieVrControls) repository's [troubleshooting guide](https://github.com/osudrl/CassieVrControls/wiki/Troubleshooting) if you are having issues trying to interface with [OpenVR](https://github.com/ValveSoftware/openvr)/this project on Linux. 6 | 7 | ## How do I run it? 8 | 9 | First, complete the [Vive Setup Guide](https://github.com/osudrl/CassieVrControls/wiki/Vive-Setup-Guide) to ensure proper hardware/software setup of the HTC Vive/SteamVR/OpenVR SDK. 10 | 11 | After the setup is complete, this project needs to be built from source. Follow the step by step guide below to compile the project. 12 | 13 | ## How do I compile it? 14 | 15 | Make sure **g++** and **python** are installed. If using [cygwin](https://www.cygwin.com/) for Windows (recommended), these packages may need to be installed from the cygwin installer. 16 | 17 | Have [openvr](https://github.com/ValveSoftware/openvr) cloned somewhere convenient 18 | 19 | The following script should properly build and run the project: 20 | 21 | ```shell 22 | # Install Steam and SteamVR 23 | 24 | git clone https://github.com/osudrl/OpenVR-Tracking-Example.git ViveTrack 25 | cd ViveTrack 26 | python build.py ~/path/to/openvr/ 27 | bash build/compile.sh 28 | 29 | # Start up SteamVR and connect the Vive 30 | 31 | ./build/track 32 | 33 | ``` 34 | More information about compiling this project can be found on the [Compile Guide](https://github.com/osudrl/OpenVR-Tracking-Example/wiki/CompileGuide). 35 | 36 | ## How do I play it? 37 | 38 | ### Coordinates Only 39 | 40 | The easiest way to ensure that the controllers and HMD are tracking properly is to have the program print the coordinates it sees for the devices. When running the tracking example, to have it only print the coordinates of the devices use the -c option. 41 | 42 | ```shell 43 | bash build/compile.sh 44 | ./build/track -c 45 | ``` 46 | 47 | ### Coordinates Visualized in Vpython 48 | 49 | 1. Install [vpython](http://vpython.org/) on [conda](https://www.anaconda.com/what-is-anaconda/) 50 | * `conda install -v vpython vpython` 51 | 2. To run, use `./build/track -c | python mirror-coords.py`. 52 | 53 | ### Opstacle-sensing game 54 | 55 | However, to further demonstrate the tracking potential of the Vive system, the bulk of the code on this branch is for a tool where the user draws around obstacles in the room using the Vive controllers so that, when in sensing mode, the controllers can rumble when nearby the obstacles as a "warning". 56 | 57 | #### Defining a cylinder around a real-world obstacle 58 | 59 | As of now, the only shapes that can be used for collision testing are cylinders with a height in the direction of the y-axis. Here are the steps within the program to define an obstacle (cylinder): 60 | 61 | 1. Enter DrawingMode. You can toggle between DrawingMode and SensingMode with the ApplicationMenu button above the trackpad. You have entered DrawingMode when the controller rumbles breifly after pressing the ApplicationMenu button. 62 | 2. The next step defines a 2D circle in the xz plane. Hold down the trigger with the tip of the controller touching one edge of your obstacle and release while touching the opposide edge of the obstacle. The distance between the two points will be the cirle's diameter and the midpoint betwen the two will become the center of the circle. 63 | 64 | 65 | 66 | 3. By default, the y-axis bounds of the cylinder extend forever in both directions. 67 | 68 | 69 | 70 | 4. Define vertical boundaries by holding down the grip button with the controller level to the top of the obstacle and releasing level with the bottom of the obstacle. 71 | 72 | 73 | 74 | 5. Switch to SensingMode by pressing the ApplicationMenu button, and test to see if the controllers will rumble if nearby the obstacle. 75 | 76 | #### Extending vertical bounds infinitely in one direction 77 | 78 | Often, the obstacles you might want to test with are on the floor and don't have any empty space under them. To define the obstacle with infinite depth going downwards so that you don't have to rub your Vive controllers on the ground to properly define the obstacle, use the following control: 79 | 80 | 1. Follow steps 1 & 2 above. 81 | 2. Hold down the grip while the controller is level with the top of the obstacle. 82 | 3. Lower the controller an arbitrary distance below where the grip was originally heald. 83 | 4. Release the grip and rapidly press and release the grip again to extend the boundary forever in that direction. 84 | 85 | #### Drawing multiple cylinders 86 | 87 | With the current implementation, up to ten different cylinders can be drawn around obstacles. While in DrawingMode, the controller will occasionally rumble the current index of cylinder that is being edited (starting with one). To move to the next cylinder, press the right side of the touchpad. To edit a previous cylinder, press the left side. 88 | 89 | When moving to different cylinder, the controllers should vibrate the current index that has been selected. 90 | 91 | #### Controls Summary 92 | 93 | All of the following controls (except toggling modes with the ApplicationMenu button) only work in DrawingMode. 94 | 95 | | Button | Function 96 | |----------|:-------------| 97 | | Application Menu | Toggle from DrawingMode to SensingMode and back | 98 | | Trigger | Hold to define or reset the circle that is a cross-section of the cylinder in the xz plane | 99 | | Grip | Hold to define the vertical bounds of the current cylinder | 100 | | Trackpad (left) | Switch work to the previous cylinder | 101 | | Trackpad (right) | Switch work to the next cylinder | 102 | | Trackpad (up) | Press to vibrate the index of the cylinder currently being worked on | 103 | | Trackpad (down) | Reset/delete the cylinder at the current index | 104 | 105 | 106 | ## Troubleshooting: 107 | 108 | See the [Vive troubleshooting guide](https://github.com/osudrl/CassieVrControls/wiki/Troubleshooting) for Vive/SteamVR issues that were solved. 109 | 110 | 111 | ## Feedback 112 | 113 | Written by [Kevin Kellar](https://github.com/kkevlar) for Oregon State University's [Dynamic Robotics Laboratory](http://mime.oregonstate.edu/research/drl/). For issues, comments, or suggestions with this guide, contact the developer or open an issue. 114 | -------------------------------------------------------------------------------- /build.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os, fnmatch 4 | from os.path import expanduser 5 | from platform import system 6 | from shutil import copy2 7 | from subprocess import call 8 | import shlex 9 | import sys 10 | 11 | def makeWinPath(path): 12 | path = path.replace("/cygdrive/c","C:\\") 13 | path = path.replace("/","\\") 14 | path = path.replace("\\\\","\\") 15 | path = path.replace("\\","\\\\") 16 | return path 17 | 18 | 19 | nux = "nux" in system().lower() 20 | win = "win" in system().lower() 21 | found = False 22 | 23 | print("\nThis script will output a script to build the VR Track example.\n") 24 | testfile_rel_path = "src/openvr_api_public.cpp" 25 | 26 | if (len(sys.argv) > 1): 27 | ipath = sys.argv[1] 28 | ifile = os.path.join(ipath, testfile_rel_path) 29 | found = os.path.isfile(ifile) 30 | 31 | if not found: 32 | ipath = input("Enter path to the openvr repository clone or press enter\nto have the script search your home directory.\n> ") 33 | ifile = os.path.join(ipath, testfile_rel_path) 34 | found = os.path.isfile(ifile) 35 | 36 | if found: 37 | openvr_path = ifile[:-(len(testfile_rel_path)+1)] 38 | 39 | 40 | if not found: 41 | print("Attempting to search home directory for openvr sdk....") 42 | for root, dirs, files in os.walk(expanduser("~")): 43 | for name in files: 44 | absn = os.path.join(root, name) 45 | match = '*src/openvr_api_public.cpp' 46 | if fnmatch.fnmatch(absn, match): 47 | ssdist = len(match) - 0 48 | openvr_path = absn[:-ssdist] 49 | found = True 50 | break 51 | 52 | if (not found) and win: 53 | print("Attempting to search C:\\Users\\ for openvr sdk....") 54 | winroot = "C:\\Users\\" 55 | for root, dirs, files in os.walk(winroot): 56 | for name in files: 57 | absn = os.path.join(root, name) 58 | match = '*src/openvr_api_public.cpp' 59 | if fnmatch.fnmatch(absn, match): 60 | ssdist = len(match) - 0 61 | openvr_path = absn[:-ssdist] 62 | found = True 63 | break 64 | 65 | if not found: 66 | print("Failed to find the openvr sdk. Clone Valve's openvr sdk and try again.") 67 | sys.exit() 68 | 69 | if win: 70 | openvr_path = makeWinPath(openvr_path) 71 | 72 | print("Found the openvr sdk: '" + openvr_path + "'") 73 | 74 | 75 | 76 | if nux: 77 | openvr_bin = openvr_path + "/bin/linux64" 78 | if win: 79 | openvr_bin = openvr_path + "\\bin\\win64" 80 | openvr_bin = makeWinPath(openvr_bin) 81 | 82 | print("Found the openvr binaries: '" + openvr_bin + "'\n") 83 | 84 | print("Generating compile command...") 85 | 86 | comp = 'g++ -L%s -I%s -Wl,-rpath,%s -Wall -Wextra -std=c++0x -o build/track *.cpp *.c -lopenvr_api' % (openvr_bin,openvr_path,openvr_bin) 87 | 88 | print(" - Command: " + comp + "\n") 89 | 90 | os.mkdir("build") 91 | 92 | if nux: 93 | outfile = "build/compile.sh" 94 | if win: 95 | outfile = "build\\compile.bat" 96 | outfile = makeWinPath(outfile) 97 | 98 | out = open(outfile,'w+') 99 | 100 | if nux: 101 | out.write("#! /bin/sh \n") 102 | 103 | 104 | print("Writing output file to: '" + outfile + "'\n") 105 | 106 | out.write(comp + "\n") 107 | 108 | print("Finished.") 109 | 110 | -------------------------------------------------------------------------------- /chart.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | xdata=np.loadtxt('r-x.txt') 6 | ydata=np.loadtxt('r-y.txt') 7 | top = min(len(xdata), len(ydata)) 8 | X=xdata[:top] 9 | Y=ydata[:top] 10 | plt.plot(X,Y,':ro') 11 | #plt.ylim((0,55000)) 12 | plt.show() #or 13 | plt.save('figure.png') 14 | 15 | -------------------------------------------------------------------------------- /cpTime.c: -------------------------------------------------------------------------------- 1 | #include "cpTime.h" 2 | #include 3 | 4 | #if defined __linux 5 | #include 6 | #include 7 | #include 8 | bool clockDef = false; 9 | struct timespec start; 10 | #elif defined _WIN32 || defined __CYGWIN__ 11 | #include 12 | 13 | #else 14 | #error Platform not supported 15 | #endif 16 | 17 | long cpMillis() 18 | { 19 | #if defined __linux 20 | if(!clockDef) 21 | { 22 | clock_gettime(CLOCK_MONOTONIC_RAW, &start); 23 | clockDef = true; 24 | } 25 | struct timespec end; 26 | clock_gettime(CLOCK_MONOTONIC_RAW, &end); 27 | 28 | return ((end.tv_sec - start.tv_sec) * 1000000 + (end.tv_nsec - start.tv_nsec) / 1000)/1000; 29 | 30 | #elif defined _WIN32 || defined __CYGWIN__ 31 | SYSTEMTIME time; 32 | GetSystemTime(&time); 33 | return (time.wSecond * 1000) + time.wMilliseconds; 34 | 35 | #else 36 | #error Platform not supported 37 | #endif 38 | } 39 | 40 | void cpSleep(int sleepMs) 41 | { 42 | #if defined __linux 43 | usleep(sleepMs * 1000); // usleep takes sleep time in us (1 millionth of a second) 44 | 45 | #elif defined _WIN32 || defined __CYGWIN__ 46 | Sleep(sleepMs); 47 | 48 | #else 49 | #error Platform not supported 50 | #endif 51 | } 52 | 53 | 54 | -------------------------------------------------------------------------------- /cpTime.h: -------------------------------------------------------------------------------- 1 | #ifndef CPTIME_H_ 2 | #define CPTIME_H_ 3 | #include 4 | long cpMillis(); 5 | void cpSleep(int sleepMs); 6 | #endif -------------------------------------------------------------------------------- /cylinder.cpp: -------------------------------------------------------------------------------- 1 | #include "cylinder.h" 2 | 3 | //Destructor 4 | Cylinder::~Cylinder() 5 | { 6 | 7 | } 8 | 9 | //Constructor 10 | Cylinder::Cylinder() 11 | { 12 | hasInit = false; 13 | s1 = new float[3]; 14 | s2 = new float[3]; 15 | s1[1] = std::numeric_limits::max(); 16 | s2[1] = -std::numeric_limits::max(); 17 | yMin = -std::numeric_limits::max(); 18 | yMax = std::numeric_limits::max(); 19 | radius = 0; 20 | } 21 | 22 | void Cylinder::init() 23 | { 24 | //The center is set to the midpoint between the two trigger button events 25 | xOrigin = (s1[0] + s2[0])/2; 26 | zOrigin = (s1[2] + s2[2])/2; 27 | 28 | //Calculates the radius based on the distance between the two base points 29 | float deltaX = s1[0]-s2[0]; 30 | float deltaZ = s1[2]-s2[2]; 31 | radius= std::sqrt((deltaX*deltaX) + (deltaZ*deltaZ)) / 2; 32 | 33 | yMax = std::max(s1[1],s2[1]); 34 | yMin = std::min(s1[1],s2[1]); 35 | hasInit = true; 36 | } 37 | 38 | 39 | bool Cylinder::isInside(float x, float y, float z) 40 | { 41 | if(y > yMax || y < yMin) 42 | return false; 43 | float dX = x-xOrigin; 44 | float dZ = z-zOrigin; 45 | float testDist = std::sqrt(((dX*dX) + (dZ*dZ))); 46 | return (testDist <= radius); 47 | } -------------------------------------------------------------------------------- /cylinder.h: -------------------------------------------------------------------------------- 1 | #ifndef CYLINDER_H_ 2 | #define CYLINDER_H_ 3 | #include 4 | #include 5 | #include 6 | 7 | class Cylinder 8 | { 9 | public: 10 | ~Cylinder(); 11 | Cylinder(); 12 | void init(); 13 | bool isInside(float x, float y, float z); 14 | 15 | //Two points in xyz coordinates to use to make the cylinder in init() 16 | float* s1; 17 | float* s2; 18 | 19 | float xOrigin; 20 | float zOrigin; 21 | float yMin; 22 | float yMax; 23 | float radius; 24 | 25 | bool hasInit; 26 | }; 27 | #endif -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "LighthouseTracking.h" 3 | #include "cpTime.h" 4 | #include 5 | #include 6 | 7 | int main( int argc, // Number of strings in array argv 8 | char *argv[], // Array of command-line argument strings 9 | char **envp ) // Array of environment variable strings 10 | { 11 | InitFlags flags; 12 | bool isHelp = false; 13 | bool invert = false; 14 | bool hasHadFlag = false; 15 | for (int x = 0; x < argc; x++) 16 | { 17 | char* currString = argv[x]; 18 | int len = strlen(currString); 19 | bool isFlag = len > 1 && currString[0] == '-' && '-' != currString[1]; 20 | 21 | if(!hasHadFlag && isFlag) 22 | { 23 | hasHadFlag = true; 24 | invert = !invert; 25 | } 26 | 27 | if (isFlag) 28 | for(int y = 1; y < len; y++) 29 | { 30 | if(currString[y] == 'h') 31 | isHelp = true; 32 | if(currString[y] == 'c') 33 | flags.printCoords = false; 34 | if(currString[y] == 'a') 35 | flags.printAnalog = false; 36 | if(currString[y] == 'e') 37 | flags.printEvents = false; 38 | if(currString[y] == 'i') 39 | flags.printSetIds = false; 40 | if(currString[y] == 'b') 41 | flags.printBEvents = false; 42 | if(currString[y] == 't') 43 | flags.printTrack = false; 44 | if(currString[y] == 'r') 45 | flags.printRotation = false; 46 | if(currString[y] == 'V') 47 | flags.pipeCoords = true; 48 | if(currString[y] == 'O') 49 | invert = !invert; 50 | } 51 | 52 | if(!isFlag) 53 | { 54 | if( strcasecmp("--help",currString) == 0 ) 55 | isHelp = true; 56 | if( strcasecmp("--coords",currString) == 0 ) 57 | flags.printCoords = false; 58 | if( strcasecmp("--analog",currString) == 0 ) 59 | flags.printAnalog = false; 60 | if( strcasecmp("--events",currString) == 0 ) 61 | flags.printEvents = false; 62 | if( strcasecmp("--ids",currString) == 0 ) 63 | flags.printSetIds = false; 64 | if( strcasecmp("--bevents",currString) == 0 ) 65 | flags.printBEvents = false; 66 | if( strcasecmp("--track",currString) == 0 ) 67 | flags.printTrack = false; 68 | if( strcasecmp("--rot",currString) == 0 ) 69 | flags.printRotation = false; 70 | if( strcasecmp("--visual",currString) == 0 ) 71 | flags.pipeCoords = true; 72 | if( strcasecmp("--omit",currString) == 0 ) 73 | invert = !invert; 74 | } 75 | } 76 | 77 | if(invert) 78 | { 79 | flags.printCoords = !flags.printCoords; 80 | flags.printAnalog = !flags.printAnalog; 81 | flags.printEvents = !flags.printEvents; 82 | flags.printSetIds = !flags.printSetIds; 83 | flags.printBEvents = !flags.printBEvents; 84 | flags.printTrack = !flags.printTrack; 85 | flags.printRotation = !flags.printRotation; 86 | } 87 | 88 | if(isHelp) 89 | { 90 | printf("\nVive LighthouseTracking Example by Kevin Kellar.\n"); 91 | printf("Command line flags:\n"); 92 | printf(" -h --help -> Prints this help text. The \"Only Print\" flags can be combined for multiple types to both print.\n"); 93 | printf(" -a --analog -> Only print analog button data from the controllers. \n"); 94 | printf(" -b --bEvents -> Only print button event data. \n"); 95 | printf(" -c --coords -> Only print HMD/Controller coordinates. \n"); 96 | printf(" -e --events -> Only print VR events. \n"); 97 | printf(" -i --ids -> Only print the output from initAssignIds() as the devices are given ids. \n"); 98 | printf(" -r --rot -> Only print the rotation of devices. \n"); 99 | printf(" -t --track -> Only print the tracking state of devices. \n"); 100 | printf(" -O --omit -> Omits only the specified output types (a,b,c,e,i,r,t) rather than including only the specified types. Useful for hiding only a few types of output. \n"); 101 | printf(" -V --visual -> Streamlines output (coordinates) to be more easily parsed by a visual program. \n"); 102 | return EXIT_SUCCESS; 103 | } 104 | 105 | if(flags.pipeCoords) 106 | { 107 | flags.printCoords = false; 108 | flags.printAnalog = false; 109 | flags.printEvents = false; 110 | flags.printSetIds = false; 111 | flags.printBEvents = false; 112 | flags.printTrack = false; 113 | flags.printRotation = false; 114 | } 115 | 116 | // Create a new LighthouseTracking instance and parse as needed 117 | LighthouseTracking* lighthouseTracking = new LighthouseTracking(flags); 118 | if (lighthouseTracking) //null check 119 | { 120 | cpSleep(2000); 121 | while (1==1) 122 | { 123 | lighthouseTracking->RunProcedure(); 124 | cpSleep(1); 125 | } 126 | delete lighthouseTracking; 127 | } 128 | return EXIT_SUCCESS; 129 | } 130 | 131 | -------------------------------------------------------------------------------- /mirror-coords.py: -------------------------------------------------------------------------------- 1 | from vpython import * 2 | import time 3 | import threading 4 | 5 | #scene.autoscale = False 6 | 7 | print("\n") 8 | 9 | hmd = vector(0,0,0) 10 | left = vector(0,0,0) 11 | right = vector(0,0,0) 12 | 13 | devices = [hmd, left, right] 14 | 15 | def process(): 16 | xyz = ['x','y','z'] 17 | global devices 18 | while True: 19 | try: 20 | editIndex = -1; 21 | nInput = input() 22 | splits = nInput.split() 23 | except Exception as e: 24 | break 25 | else: 26 | pass 27 | finally: 28 | pass 29 | for split in splits: 30 | if "HMD" in split: 31 | editIndex = 0; 32 | if "LEFT:" in split: 33 | editIndex = 1; 34 | if "RIGHT:" in split: 35 | editIndex = 2; 36 | 37 | if editIndex < 0 or len(split) < 3 or split[1] != ':' or split[0] not in xyz: 38 | continue 39 | num = float(split[2:]) 40 | if split[0] == 'x': 41 | devices[editIndex].z = -num; 42 | elif split[0] == 'y': 43 | devices[editIndex].y = num; 44 | elif split[0] == 'z': 45 | devices[editIndex].x = -num; 46 | 47 | 48 | 49 | thread = threading.Thread(target=process) 50 | thread.start() 51 | 52 | b_hmd = box(color=color.magenta) 53 | b_left = box(color=color.blue) 54 | b_right = box(color=color.red) 55 | 56 | b_white = box(color=color.white) 57 | b_white.pos = vector(0,0,0) 58 | 59 | while True: 60 | b_hmd.pos = devices[0] * 2 61 | b_left.pos = devices[1] * 2 62 | b_right.pos = devices[2] * 2 63 | b_left.rotate(angle=.001, axis=vector(0,1,0)) 64 | 65 | 66 | -------------------------------------------------------------------------------- /quat-test/quat.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | struct _quat_t 5 | { 6 | float w; 7 | float v[3]; 8 | }; 9 | typedef struct _quat_t quat_t; 10 | 11 | float qmag(quat_t q) 12 | { 13 | float m = q.w * q.w; 14 | for(int n = 0; n < 3; n++) 15 | m += q.v[n] * q.v[n]; 16 | return sqrt(m); 17 | } 18 | 19 | quat_t qnorm(quat_t q) 20 | { 21 | float m = qmag(q); 22 | q.w = q.w/m; 23 | for(int n = 0; n < 3; n++) 24 | q.v[n] = q.v[n]/m; 25 | return q; 26 | } 27 | 28 | float vmag(float* v) 29 | { 30 | float m = 0; 31 | for(int n = 0; n < 3; n++) 32 | m += v[n] * v[n]; 33 | return sqrt(m); 34 | } 35 | 36 | void vnorm(float* v) 37 | { 38 | float m = vmag(v); 39 | for(int n = 0; n < 3; n++) 40 | v[n] = v[n]/m; 41 | } 42 | 43 | quat_t qinv(quat_t q) 44 | { 45 | q = qnorm(q); 46 | for(int n = 0; n < 3; n++) 47 | q.v[n] = -q.v[n]; 48 | return q; 49 | } 50 | 51 | void vcross( float* out, float* v, float* u) 52 | { 53 | out[0] = (v[1] * u[2]) - (v[2] * u[1]); 54 | out[1] = -( (v[0] * u[2]) - (v[2] * u[0]) ) ; 55 | out[2] = (v[0] * u[1]) - (v[1] * u[0]); 56 | } 57 | 58 | float vdot(float* v, float* u) 59 | { 60 | float dot = v[0] * u[0] + 61 | v[1] * u[1] + 62 | v[2] * u[2]; 63 | return dot; 64 | } 65 | 66 | quat_t qmult (quat_t p, quat_t q) 67 | { 68 | quat_t out; 69 | out.w = p.w*q.w - vdot(p.v,q.v); 70 | float v1[3]; 71 | float v2[3]; 72 | float v3[3]; 73 | for(int n = 0; n < 3; n++) 74 | v1[n] = q.v[n] * p.w; 75 | for(int n = 0; n < 3; n++) 76 | v2[n] = p.v[n] * q.w; 77 | vcross(v3,p.v,q.v); 78 | for(int n = 0; n < 3; n++) 79 | out.v[n] = v1[n] + v2[n] + v3[n]; 80 | return out; 81 | } 82 | 83 | void printQuat(char* label, quat_t q) 84 | { 85 | printf("Quat \"%s\" (w:%.3f i:%.3f j:%.3f k:%.3f)\n",label,q.w,q.v[0],q.v[1],q.v[2]); 86 | } 87 | 88 | int main() 89 | { 90 | printf("Quat Test: \n"); 91 | float theta = M_PI; 92 | float axis[3]; 93 | axis[0] = 0; 94 | axis[1] = 0; 95 | axis[2] = 1; 96 | 97 | quat_t rotation; 98 | rotation.w = cos(theta/(2+0.0)); 99 | for(int n = 0; n < 3; n++) 100 | rotation.v[n] = axis[n] * sin(theta/(2+0.0)); 101 | 102 | rotation = qnorm(rotation); 103 | quat_t rotinv = qinv(rotation); 104 | quat_t point; 105 | point.v[0] = 5; 106 | point.v[1] = 5; 107 | point.v[2] = 0; 108 | 109 | //rotates the xy point 5,5 around the z axis 110 | quat_t out = qmult(qmult(rotation,point),rotinv); 111 | printQuat("qreg",rotation); 112 | printQuat("point",point); 113 | printQuat("qinv",rotinv); 114 | printQuat("qout",out); 115 | 116 | //printf("Cross normed (i:%.3f j:%.3f k:%.3f)\n", out[0],out[1],out[2]); 117 | //printf("Quat (w:%.3f i:%.3f j:%.3f k:%.3f)\n",q.w,q.v[0],q.v[1],q.v[2]); 118 | //printf("Magnitude: %.3f\n", vmag(out)); 119 | } --------------------------------------------------------------------------------