ovrScope.py

import pygame
import sys
from pygame.locals import *
from sys import exit
from random import *
import ovr

import math

ovr.initialize(None)
session, luid = ovr.create()
hmdDesc = ovr.getHmdDesc(session)
print hmdDesc.ProductName

white = (255,255,255)
black = (0,0,0)
red = (255,0,0)
green = (0,255,0)
blue = (0,0,255)
red2 = (255,255,0)
green2 = (0,255,255)
blue2 = (255,0,255)
gray = (40,40,40)
height = 1000
width = 1000
vSeparation = 150
gridx = 50
gridy = 50
startX = 40     # How far from left to start scope to leave room for labels

lines = [0.0,0.0,0.0,0.0,0.0,0.0]
lineScales = [0.0,0.0,0.0,0.0,0.0,0.0]
lineNames = ["X", "Y", "Z", "Pitch", "Yaw", "Roll"]
avgLines = [0.0,0.0,0.0,0.0,0.0,0.0]
lineColors = [red, green, blue, red2, green2, blue2]

pygame.init()
screen = pygame.display.set_mode((width,height))

font = pygame.font.SysFont("comicsansms", 14)

def randomGrey():
	g = randint(25,75)
	return (g, g, g)

def quaternion_to_euler_angle(w, x, y, z):
    ysqr = y * y
    
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + ysqr)
    X = math.degrees(math.atan2(t0, t1))
    
    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    Y = math.degrees(math.asin(t2))
    
    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (ysqr + z * z)
    Z = math.degrees(math.atan2(t3, t4))
    
    return X, Y, Z

def drawScope():
    for y in range(0, height/50):
        pygame.draw.lines(screen, black, False, [(0, y * gridy),(width, y * gridy)], 1)   
    for i in range(0, len(lines)):
        text = font.render(lineNames[i], True, lineColors[i])
        screen.blit(text, (1, (i + 1) * vSeparation - text.get_height())) 

def readOculus():
    ts  = ovr.getTrackingState(session, ovr.getTimeInSeconds(), True)
    if ts.StatusFlags & (ovr.Status_OrientationTracked | ovr.Status_PositionTracked):
        pose = ts.HeadPose
        return pose

def shutdownOculus():
    ovr.destroy(session)
    ovr.shutdown()

x = startX
firstSample = True
while True:
    for event in pygame.event.get():
        if event.type in (QUIT, KEYDOWN):
            shutdownOculus()
            sys.exit();
    if x == startX:
        screen.fill(gray);
        drawScope();
    pose = readOculus()
    if pose:
        euler = quaternion_to_euler_angle(pose.ThePose.Orientation.w, pose.ThePose.Orientation.x, pose.ThePose.Orientation.y, pose.ThePose.Orientation.z)
        lines[0] = pose.ThePose.Position.x 
        lines[1] = pose.ThePose.Position.y 
        lines[2] = pose.ThePose.Position.z 
        lineScales[0] = lineScales[1] = lineScales[2] = 1000        #  1000 pixels per meter for translation
        lines[3] = euler[0] 
        lines[4] = euler[1] 
        lines[5] = euler[2]
        lineScales[3] = lineScales[4] = lineScales[5] = 10          #  10 pixels per angular degree for rotation
        if firstSample:
            for i in range(0, len(lines)):
                avgLines[i] = lines[i]
            firstSample = False 
        for i in range(0, len(lines)):
            y = vSeparation * (i + 1) + (lines[i] - avgLines[i]) * lineScales[i];
            pygame.draw.lines(screen, lineColors[i], False, [(x,y), (x,y)], 1)

    pygame.display.update()
    pygame.time.delay(11)
    x = x + 1
    if x == width:
        readOculus()
        x = startX