Initial Commit

.gitignore

@@ -0,0 +1,13 @@
+tests/simple/hmdtest
+tests/opengl/hmd-opengl
+tests/unittests/unittests
+*.exe
+*.dll
+*.o
+*.lo
+*.a
+*.la
+*.libs
+*.deps
+.cproject
+.project

LICENSE

@@ -0,0 +1,23 @@
+Boost Software License - Version 1.0 - August 17th, 2003
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.

Makefile.am

@@ -0,0 +1,4 @@
+AUTOMAKE_OPTIONS = foreign
+SUBDIRS = src tests
+pkgconfigdir = $(libdir)/$(PKG_CONFIG_EXTRA_PATH)pkgconfig
+pkgconfig_DATA = pkg-config/openhmd.pc

README

@@ -0,0 +1,40 @@
+=== OpenHMD ===
+This project aims to provide a Free and Open Source API and drivers for immersive technology, such as head mounted displays with built in head tracking.
+
+=== License ===
+OpenHMD is released under the permissive Boost Software License (see LICENSE for more information), to make sure it can be linked and distributed with both free and non-free software. While it doesn't require contribution from the users, it is still very appriciated.
+
+=== Supported Devices ===
+  * Oculus Rift (SDK)
+
+== Supported Platforms ===
+  * Linux
+  * Windows
+  * Mac Os X
+
+=== Requirements ===
+  * HIDAPI
+    * http://www.signal11.us/oss/hidapi/
+    * https://github.com/signal11/hidapi/
+
+=== Compiling and Installing ===
+./configure
+make
+sudo make install
+
+== Cross compiling for windows using mingw ==
+export PREFIX=/usr/i686-w64-mingw32/ (or whatever your mingw path is)
+PKG_CONFIG_LIBDIR=$PREFIX/lib/pkgconfig ./configure --build=`gcc -dumpmachine` --host=i686-w64-mingw32 --prefix=$PREFIX
+make
+the library will end up in the .lib directory, you can use microsoft's lib.exe to make a .lib file for it
+
+== Static linking on windows ==
+If you're linking statically with OpenHMD using windows/mingw you have to make sure the macro OHMD_STATIC is set before including openhmd.h. In GCC this can be done by adding the compiler flag -DOHMD_STATIC, and with msvc it can be done using /DOHMD_STATIC.
+
+Note that this is *only* if you're linking statically! If you're using the DLL then you *must not* define OHMD_STATIC. (If you're not sure then you're probably linking dynamically and won't have to worry about this).
+
+=== Pre-built packages ===
+...
+
+=== Using OpenHMD ===
+The distribution provides a number of usage examples under the examples/ subdirectory.

autogen.sh

@@ -0,0 +1,2 @@
+#!/bin/sh
+autoreconf --install --force

configure.ac

@@ -0,0 +1,82 @@
+name='openhmd'
+version='0.0.1'
+library_interface_version='0:0:0'
+email='noname@nurd.se'
+
+m4_define([OH_VERSION_STRING], [0.0.1])
+m4_define([OH_NAME], [openhmd])
+
+AC_PREREQ([2.13])
+AC_INIT([OH_NAME], [OH_VERSION_STRING], [noname@nurd.se])
+AM_INIT_AUTOMAKE([foreign -Wall -Werror])
+AM_PROG_AR
+LT_INIT
+AC_CANONICAL_HOST
+
+m4_ifdef([AM_SILENT_RULES], [AM_SILENT_RULES([yes])])
+
+hidapi="hidapi"
+
+AC_SUBST(PKG_CONFIG_EXTRA_PATH, "")
+AC_SUBST(EXTRA_LD_FLAGS, "")
+
+AC_MSG_CHECKING([operating system])
+case "$host" in
+*-linux*)
+	AC_MSG_RESULT([$host (Linux)])
+	hidapi="hidapi-libusb"
+
+	#link with realtime lib on linux for clock_gettime
+	AC_SUBST(EXTRA_LD_FLAGS, "-lrt")
+	;;
+*-freebsd*)
+	AC_SUBST(PKG_CONFIG_EXTRA_PATH, "libdata/")
+	;;
+esac
+
+PKG_CHECK_MODULES([hidapi], [$hidapi] >= 0.0.5)
+
+# Do we build OpenGL test?
+AC_ARG_ENABLE([opengltest],
+        [AS_HELP_STRING([--enable-opengltest],
+                [enable building of OpenGL test (default n)])],
+        [opengltest_enabled=$enableval],
+        [opengltest_enabled='no'])
+
+AM_CONDITIONAL([BUILD_OPENGL_TEST], [test "x$opengltest_enabled" != "xno"])
+
+# Libs required by OpenGL test
+if test "x$opengltest_enabled" != "xno"; then
+	PKG_CHECK_MODULES([sdl], [sdl])
+
+	# Try to find OpenGL with pkg-config
+	PKG_CHECK_MODULES([GL], [gl], [], 
+			# and try to find which lib to link to, -lGL first
+			[AC_CHECK_LIB(GL, glBegin, [GL_LIBS=-lGL], 
+
+				# if that fails, try -lopengl32 (win32)
+				[AC_CHECK_LIB(opengl32, main, [GL_LIBS=-lopengl32], 
+					AC_MSG_ERROR([GL not found])
+				)]
+			)]
+	)
+
+	AC_SUBST(GL_LIBS)
+
+	# Try to find GLEW with pkg-config
+	PKG_CHECK_MODULES([GLEW], [glew], [],
+		# if that fails, check if there's a glew header
+		[AC_CHECK_HEADER([GL/glew.h], [GLEW_LIBS=-lGLEW; GLEW_CFLAGS=-DGLEW_STATIC], AC_MSG_ERROR([GLEW not found]))]
+	)
+
+	AC_SUBST(GLEW_LIBS)
+	AC_SUBST(GLEW_CFLAGS)
+fi
+
+AC_PROG_CC
+AC_PROG_CC_C99
+
+AC_CONFIG_HEADERS([config.h])
+
+AC_OUTPUT([Makefile src/Makefile tests/Makefile tests/simple/Makefile tests/unittests/Makefile tests/opengl/Makefile])
+AC_OUTPUT 

include/openhmd.h

@@ -0,0 +1,68 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+#ifndef OPENHMD_H
+#define OPENHMD_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef _WIN32
+#ifdef DLL_EXPORT
+#define OHMD_APIENTRY __cdecl __declspec( dllexport )
+#else
+#ifdef OHMD_STATIC
+#define OHMD_APIENTRY __cdecl
+#else
+#define OHMD_APIENTRY __cdecl __declspec( dllimport )
+#endif
+#endif
+#else
+#define OHMD_APIENTRY
+#endif
+
+#define OHMD_STR_SIZE 256
+
+typedef enum {
+	OHMD_VENDOR,
+	OHMD_PRODUCT,
+	OHMD_PATH,
+} ohmd_string_value;
+
+typedef enum {
+	OHMD_ROTATION_EULER,
+	OHMD_ROTATION_QUAT,
+
+	OHMD_MAT4X4_LEFT_EYE_GL_MODELVIEW,
+	OHMD_MAT4X4_RIGHT_EYE_GL_MODELVIEW,
+
+	OHMD_MAT4X4_LEFT_EYE_GL_PROJECTION,
+	OHMD_MAT4X4_RIGHT_EYE_GL_PROJECTION,
+
+	OHMD_POSITION_VEC
+} ohmd_float_value;
+
+typedef struct ohmd_context ohmd_context;
+typedef struct ohmd_device ohmd_device;
+
+OHMD_APIENTRY ohmd_context* ohmd_ctx_create();
+OHMD_APIENTRY void ohmd_ctx_destroy(ohmd_context* ctx);
+OHMD_APIENTRY const char* ohmd_ctx_get_error(ohmd_context* ctx);
+OHMD_APIENTRY void ohmd_ctx_update(ohmd_context* ctx);
+OHMD_APIENTRY int ohmd_ctx_probe(ohmd_context* ctx);
+
+OHMD_APIENTRY const char* ohmd_list_gets(ohmd_context* ctx, int index, ohmd_string_value type);
+OHMD_APIENTRY ohmd_device* ohmd_list_open_device(ohmd_context* ctx, int index);
+
+OHMD_APIENTRY int ohmd_device_getf(ohmd_device* device, ohmd_float_value type, float* out);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

pkg-config/openhmd.pc

@@ -0,0 +1,11 @@
+prefix=${pcfiledir}/../..
+libdir=${prefix}/lib
+includedir=${prefix}/include/openhmd
+
+Name: openhmd
+Description: API and drivers for immersive technology devices such as HMDs
+Version: 0.0.1
+Requires: 
+Conflicts:
+Libs: -L${libdir}
+Cflags: -I${includedir}

src/Makefile.am

@@ -0,0 +1,15 @@
+library_includedir=$(includedir)/openhmd
+library_include_HEADERS = $(top_srcdir)/include/openhmd.h
+
+lib_LTLIBRARIES = libopenhmd.la
+libopenhmd_la_SOURCES = \
+	openhmd.c \
+	platform-win32.c \
+	drv_oculus_rift/rift.c \
+	drv_oculus_rift/packet.c \
+	omath.c \
+	platform-posix.c \
+	fusion.c
+
+libopenhmd_la_LDFLAGS = $(hidapi_LIBS) -no-undefined -version-info 0:0:0 $(EXTRA_LD_FLAGS)
+libopenhmd_la_CPPFLAGS = -fPIC -I$(top_srcdir)/include $(hidapi_CFLAGS) -Wall -Werror

src/drv_oculus_rift/packet.c

@@ -0,0 +1,219 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Oculus Rift Driver - Packet Decoding and Utilities */
+
+#include <stdio.h>
+#include "rift.h"
+
+#define SKIP_CMD (buffer++)
+#define READ8 *(buffer++);
+#define READ16 *buffer | (*(buffer + 1) << 8); buffer += 2;
+#define READ32 *buffer | (*(buffer + 1) << 8) | (*(buffer + 2) << 16) | (*(buffer + 3) << 24); buffer += 4;
+#define READFLOAT ((float)(*buffer)); buffer += 4;
+#define READFIXED (float)(*buffer | (*(buffer + 1) << 8) | (*(buffer + 2) << 16) | (*(buffer + 3) << 24)) / 1000000.0f; buffer += 4;
+
+#define WRITE8(_val) *(buffer++) = (_val);
+#define WRITE16(_val) WRITE8((_val) & 0xff); WRITE8(((_val) >> 8) & 0xff);
+#define WRITE32(_val) WRITE16((_val) & 0xffff) *buffer; WRITE16(((_val) >> 16) & 0xffff);
+
+
+bool decode_sensor_range(pkt_sensor_range* range, const unsigned char* buffer, int size)
+{
+	if(size != 8){
+		LOGE("invalid packet size");
+		return false;
+	}
+
+	SKIP_CMD;
+	range->command_id = READ16;
+	range->accel_scale = READ8;
+	range->gyro_scale = READ16;
+	range->mag_scale = READ16;
+
+	return true;
+}
+
+bool decode_sensor_display_info(pkt_sensor_display_info* info, const unsigned char* buffer, int size)
+{
+	if(size != 56){
+		LOGE("invalid packet size");
+		return false;
+	}
+
+	SKIP_CMD;
+	info->command_id = READ16;
+	info->distortion_type = READ8;
+	info->h_resolution = READ16;
+	info->v_resolution = READ16;
+	info->h_screen_size = READFIXED;
+	info->v_screen_size = READFIXED;
+	info->v_center = READFIXED;
+	info->lens_separation = READFIXED;
+	info->eye_to_screen_distance[0] = READFIXED;
+	info->eye_to_screen_distance[1] = READFIXED;
+	
+	info->distortion_type_opts = 0;	
+
+	for(int i = 0; i < 6; i++)
+		info->distortion_k[i] = READFLOAT;
+
+	return true;
+}
+
+bool decode_sensor_config(pkt_sensor_config* config, const unsigned char* buffer, int size)
+{
+	if(size != 7){
+		LOGE("invalid packet size");
+		return false;
+	}
+
+	SKIP_CMD;
+	config->command_id = READ16;
+	config->flags = READ8;
+	config->packet_interval = READ8;
+	config->keep_alive_interval = READ16;
+
+	return true;
+}
+
+static void decode_sample(const unsigned char* buffer, int32_t* smp)
+{
+	/*
+	 * Decode 3 tightly packed 21 bit values from 4 bytes.
+	 * We unpack them in the higher 21 bit values first and then shift
+	 * them down to the lower in order to get the sign bits correct.
+	 */
+
+	int x = (buffer[0] << 24)          | (buffer[1] << 16) | ((buffer[2] & 0xF8) << 8);
+	int y = ((buffer[2] & 0x07) << 29) | (buffer[3] << 21) | (buffer[4] << 13) | ((buffer[5] & 0xC0) << 5);
+	int z = ((buffer[5] & 0x3F) << 26) | (buffer[6] << 18) | (buffer[7] << 10);
+
+	smp[0] = x >> 11;
+	smp[1] = y >> 11;
+	smp[2] = z >> 11;
+}
+
+bool decode_tracker_sensor_msg(pkt_tracker_sensor* msg, const unsigned char* buffer, int size)
+{
+	if(size != 62){
+		LOGE("invalid packet size");
+		return false;
+	}
+
+	SKIP_CMD;
+	msg->num_samples = READ8;
+	msg->timestamp = READ16;
+	msg->last_command_id = READ16;
+	msg->temperature = READ16;
+
+	int actual = OHMD_MIN(msg->num_samples, 3);
+	for(int i = 0; i < actual; i++){
+		decode_sample(buffer, msg->samples[i].accel);
+		buffer += 8;
+
+		decode_sample(buffer, msg->samples[i].gyro);
+		buffer += 8;
+	}
+
+	// Skip empty samples
+	buffer += (3 - actual) * 16;
+	for(int i = 0; i < 3; i++){
+		msg->mag[i] = READ16;
+	}
+
+	return true;
+}
+
+// TODO do we need to consider HMD vs sensor "centric" values
+void vec3f_from_rift_vec(const int32_t* smp, vec3f* out_vec)
+{
+	out_vec->x = (float)smp[0] * 0.0001f;
+	out_vec->y = (float)smp[1] * 0.0001f;
+	out_vec->z = (float)smp[2] * 0.0001f;
+}
+
+int encode_sensor_config(unsigned char* buffer, const pkt_sensor_config* config)
+{
+	WRITE8(RIFT_CMD_SENSOR_CONFIG);
+	WRITE16(config->command_id);
+	WRITE8(config->flags);
+	WRITE8(config->packet_interval);
+	WRITE16(config->keep_alive_interval);
+	return 7; // sensor config packet size
+}
+
+int encode_keep_alive(unsigned char* buffer, const pkt_keep_alive* keep_alive)
+{
+	WRITE8(RIFT_CMD_KEEP_ALIVE);
+	WRITE16(keep_alive->command_id);
+	WRITE16(keep_alive->keep_alive_interval);
+	return 5; // keep alive packet size
+}
+
+void dump_packet_sensor_range(const pkt_sensor_range* range)
+{
+	(void)range;
+
+	LOGD("sensor range\n");
+	LOGD("  command id:  %d", range->command_id);
+	LOGD("  accel scale: %d", range->accel_scale);
+	LOGD("  gyro scale:  %d", range->gyro_scale);
+	LOGD("  mag scale:   %d", range->mag_scale);
+}
+
+void dump_packet_sensor_display_info(const pkt_sensor_display_info* info)
+{
+	(void)info;
+
+	LOGD("display info");
+	LOGD("  command id:             %d", info->command_id);
+	LOGD("  distortion_type:        %d", info->distortion_type);
+	LOGD("  resolution:             %d x %d", info->h_resolution, info->v_resolution);
+	LOGD("  screen size:            %f x %f", info->h_screen_size, info->v_screen_size);
+	LOGD("  vertical center:        %f", info->v_center);
+	LOGD("  lens_separation:        %f", info->lens_separation);
+	LOGD("  eye_to_screen_distance: %f, %f", info->eye_to_screen_distance[0], info->eye_to_screen_distance[1]);
+	LOGD("  distortion_k:           %f, %f, %f, %f, %f, %f",
+		info->distortion_k[0], info->distortion_k[1], info->distortion_k[2],
+		info->distortion_k[3], info->distortion_k[4], info->distortion_k[5]);
+}
+
+void dump_packet_sensor_config(const pkt_sensor_config* config)
+{
+	(void)config;
+
+	LOGD("sensor config");
+	LOGD("  command id:          %u", config->command_id);
+	LOGD("  flags:               %02x", config->flags);
+	LOGD("    raw mode:                  %d", !!(config->flags & RIFT_SCF_RAW_MODE));
+	LOGD("    calibration test:          %d", !!(config->flags & RIFT_SCF_CALIBRATION_TEST));
+	LOGD("    use calibration:           %d", !!(config->flags & RIFT_SCF_USE_CALIBRATION));
+	LOGD("    auto calibration:          %d", !!(config->flags & RIFT_SCF_AUTO_CALIBRATION));
+	LOGD("    motion keep alive:         %d", !!(config->flags & RIFT_SCF_MOTION_KEEP_ALIVE));
+	LOGD("    motion command keep alive: %d", !!(config->flags & RIFT_SCF_COMMAND_KEEP_ALIVE));
+	LOGD("    sensor coordinates:        %d", !!(config->flags & RIFT_SCF_SENSOR_COORDINATES));
+	LOGD("  packet interval:     %u", config->packet_interval);
+	LOGD("  keep alive interval: %u", config->keep_alive_interval);
+}
+
+void dump_packet_tracker_sensor(const pkt_tracker_sensor* sensor)
+{
+	(void)sensor;
+
+	LOGD("tracker sensor:");
+	LOGD("  last command id: %u", sensor->last_command_id);
+	LOGD("  timestamp:       %u", sensor->timestamp);
+	LOGD("  temperature:     %d", sensor->temperature);
+	LOGD("  num samples:     %u", sensor->num_samples);
+	LOGD("  magnetic field:  %i %i %i", sensor->mag[0], sensor->mag[1], sensor->mag[2]);
+
+	for(int i = 0; i < OHMD_MIN(sensor->num_samples, 3); i++){
+		LOGD("    accel: %d %d %d", sensor->samples[i].accel[0], sensor->samples[i].accel[1], sensor->samples[i].accel[2]);
+		LOGD("    gyro:  %d %d %d", sensor->samples[i].gyro[0], sensor->samples[i].gyro[1], sensor->samples[i].gyro[2]);
+	}
+}

src/drv_oculus_rift/rift.c

@@ -0,0 +1,399 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Oculus Rift Driver - HID/USB Driver Implementation */
+
+#include <stdlib.h>
+#include <hidapi.h>
+#include <string.h>
+#include <stdio.h>
+#include <time.h>
+#include <assert.h>
+
+#include "rift.h"
+
+#define TICK_LEN (1.0f / 1000.0f) // 1000 Hz ticks
+#define KEEP_ALIVE_VALUE (10 * 1000)
+#define SETFLAG(_s, _flag, _val) (_s) = ((_s) & ~(_flag)) | ((_val) ? (_flag) : 0)
+
+
+typedef struct {
+	ohmd_device base;
+
+	hid_device* handle;
+	pkt_sensor_range sensor_range;
+	pkt_sensor_display_info display_info;
+	rift_coordinate_frame coordinate_frame, hw_coordinate_frame;
+	pkt_sensor_config sensor_config;
+	pkt_tracker_sensor sensor;
+	double last_keep_alive;
+	fusion sensor_fusion;
+	vec3f raw_mag, raw_accel, raw_gyro;
+
+	// These values are derived from the display_info struct and
+	// from user provided values.
+	struct {
+		float idp; // inter-pupillary distance, user provided.
+
+		float znear; // depth near value, user provided.
+		float zfar; // depth far value, user provided.
+
+		float proj_offset; // lens offset on screen
+		mat4x4f proj_base; // base projection matrix
+		mat4x4f proj_left; // adjusted projection matrix for left screen
+		mat4x4f proj_right; // adjusted projection matrix for right screen
+
+		float full_ratio; // screen ratio for the entire device
+
+		float stereo_fov; // horizontal fov for one sub screen
+		float stereo_ratio; // screen ratio for one sub screen
+	} calc_values;
+} rift_priv;
+
+static rift_priv* rift_priv_get(ohmd_device* device)
+{
+	return (rift_priv*)device;
+}
+
+
+static int get_feature_report(rift_priv* priv, rift_sensor_feature_cmd cmd, unsigned char* buf)
+{
+	memset(buf, 0, FEATURE_BUFFER_SIZE);
+	buf[0] = (unsigned char)cmd;
+	return hid_get_feature_report(priv->handle, buf, FEATURE_BUFFER_SIZE);
+}
+
+static int send_feature_report(rift_priv* priv, const unsigned char *data, size_t length)
+{
+	return hid_send_feature_report(priv->handle, data, length);
+}
+
+static void set_coordinate_frame(rift_priv* priv, rift_coordinate_frame coordframe)
+{
+	priv->coordinate_frame = coordframe;
+
+	// set the RIFT_SCF_SENSOR_COORDINATES in the sensor config to match whether coordframe is hmd or sensor
+	SETFLAG(priv->sensor_config.flags, RIFT_SCF_SENSOR_COORDINATES, coordframe == RIFT_CF_SENSOR);
+
+	// encode send the new config to the Rift 
+	unsigned char buf[FEATURE_BUFFER_SIZE];
+	int size = encode_sensor_config(buf, &priv->sensor_config);
+	if(send_feature_report(priv, buf, size) == -1){
+		ohmd_set_error(priv->base.ctx, "send_feature_report failed in set_coordinate frame");
+		return;
+	}
+
+	// read the state again, set the hw_coordinate_frame to match what
+	// the hardware actually is set to just incase it doesn't stick.
+	size = get_feature_report(priv, RIFT_CMD_SENSOR_CONFIG, buf);
+	if(size <= 0){
+		LOGW("could not set coordinate frame");
+		priv->hw_coordinate_frame = RIFT_CF_HMD;
+		return;
+	}
+
+	decode_sensor_config(&priv->sensor_config, buf, size);
+	priv->hw_coordinate_frame = (priv->sensor_config.flags & RIFT_SCF_SENSOR_COORDINATES) ? RIFT_CF_SENSOR : RIFT_CF_HMD;
+
+	if(priv->hw_coordinate_frame != coordframe) {
+		LOGW("coordinate frame didn't stick");
+	}
+}
+
+static void handle_tracker_sensor_msg(rift_priv* priv, unsigned char* buffer, int size)
+{
+	if(!decode_tracker_sensor_msg(&priv->sensor, buffer, size)){
+		LOGE("couldn't decode tracker sensor message");
+	}
+
+	pkt_tracker_sensor* s = &priv->sensor;
+
+	dump_packet_tracker_sensor(s);
+
+	// TODO handle missed samples etc.
+
+	float dt = s->num_samples > 3 ? (s->num_samples - 2) * TICK_LEN : TICK_LEN;
+
+	int32_t mag32[] = { s->mag[0], s->mag[1], s->mag[2] };
+	vec3f_from_rift_vec(mag32, &priv->raw_mag);
+
+	for(int i = 0; i < OHMD_MIN(s->num_samples, 3); i++){
+		vec3f_from_rift_vec(s->samples[i].accel, &priv->raw_accel);
+		vec3f_from_rift_vec(s->samples[i].gyro, &priv->raw_gyro);
+
+		ofusion_update(&priv->sensor_fusion, dt, &priv->raw_gyro, &priv->raw_accel, &priv->raw_mag);
+
+		// reset dt to tick_len for the last samples if there were more than one sample
+		dt = TICK_LEN;
+	}
+}
+
+static void update_device(ohmd_device* device)
+{
+	rift_priv* priv = rift_priv_get(device);
+	unsigned char buffer[FEATURE_BUFFER_SIZE];
+
+	// Handle keep alive messages
+	double t = ohmd_get_tick();
+	if(t - priv->last_keep_alive >= (double)priv->sensor_config.keep_alive_interval / 1000.0 - .2){
+		// send keep alive message
+		pkt_keep_alive keep_alive = { 0, priv->sensor_config.keep_alive_interval };
+		int ka_size = encode_keep_alive(buffer, &keep_alive);
+		send_feature_report(priv, buffer, ka_size);
+
+		// Update the time of the last keep alive we have sent.
+		priv->last_keep_alive = t;
+	}
+
+	// Read all the messages from the device.
+	while(true){
+		int size = hid_read(priv->handle, buffer, FEATURE_BUFFER_SIZE);
+		if(size < 0){
+			LOGE("error reading from device");
+			return;
+		} else if(size == 0) {
+			return; // No more messages, return.
+		}
+
+		// currently the only message type the hardware supports (I think)
+		if(buffer[0] == RIFT_IRQ_SENSORS){
+			handle_tracker_sensor_msg(priv, buffer, size);
+		}else{
+			LOGE("unknown message type: %u", buffer[0]);
+		}
+	}
+}
+
+static void calc_derived_values(rift_priv *priv)
+{
+	priv->calc_values.idp = 0.061f; // TODO settable.
+	priv->calc_values.znear = 0.1f; // TODO settable.
+	priv->calc_values.zfar = 1000.0f; // TODO settable.
+
+	priv->calc_values.stereo_fov = DEG_TO_RAD(125.5144f); // TODO calculate.
+
+
+	// Calculate the screen ratio of each subscreen.
+	float full_ratio = (float)priv->display_info.h_resolution /
+	                   (float)priv->display_info.v_resolution;
+	float ratio = full_ratio / 2.0f;
+
+	priv->calc_values.stereo_ratio = ratio;
+
+
+	// Calculate where the lens is on each screen,
+	// and with the given value offset the projection matrix.
+	float screen_center = priv->display_info.h_screen_size / 4.0f;
+	float lens_shift = screen_center - priv->display_info.lens_separation / 2.0f;
+	float proj_offset = 4.0f * lens_shift / priv->display_info.h_screen_size;
+
+	priv->calc_values.proj_offset = proj_offset;
+
+
+	// Setup the base projection matrix. Each eye mostly have the
+	// same projection matrix with the exception of the offset.
+	omat4x4f_init_perspective(&priv->calc_values.proj_base,
+	                          priv->calc_values.stereo_fov,
+	                          priv->calc_values.stereo_ratio,
+	                          priv->calc_values.znear,
+	                          priv->calc_values.zfar);
+
+
+	// Setup the two adjusted projection matricies. Each is setup to deal
+	// with the fact that the lens is not in the center of the screen.
+	// These matrices only change of the hardware changes, so static.
+	mat4x4f translate;
+
+	omat4x4f_init_translate(&translate, proj_offset, 0, 0);
+	omat4x4f_mult(&translate,
+	              &priv->calc_values.proj_base,
+	              &priv->calc_values.proj_left);
+
+	omat4x4f_init_translate(&translate, -proj_offset, 0, 0);
+	omat4x4f_mult(&translate,
+	              &priv->calc_values.proj_base,
+	              &priv->calc_values.proj_right);
+}
+	
+static int getf(ohmd_device* device, ohmd_float_value type, float* out)
+{
+	rift_priv* priv = rift_priv_get(device);
+
+	switch(type){
+	case OHMD_ROTATION_QUAT: {
+			*(quatf*)out = priv->sensor_fusion.orient;
+			break;
+		}
+	case OHMD_MAT4X4_LEFT_EYE_GL_MODELVIEW: {
+			vec3f point = {{0, 0, 0}};
+			mat4x4f orient, world_shift, result;
+			omat4x4f_init_look_at(&orient, &priv->sensor_fusion.orient, &point);
+			omat4x4f_init_translate(&world_shift, +(priv->calc_values.idp / 2.0f), 0, 0);
+			omat4x4f_mult(&world_shift, &orient, &result);
+			omat4x4f_transpose(&result, (mat4x4f*)out);
+			break;
+		}
+	case OHMD_MAT4X4_RIGHT_EYE_GL_MODELVIEW: {
+			vec3f point = {{0, 0, 0}};
+			mat4x4f orient, world_shift, result;
+			omat4x4f_init_look_at(&orient, &priv->sensor_fusion.orient, &point);
+			omat4x4f_init_translate(&world_shift, -(priv->calc_values.idp / 2.0f), 0, 0);
+			omat4x4f_mult(&world_shift, &orient, &result);
+			omat4x4f_transpose(&result, (mat4x4f*)out);
+			break;
+		}
+	case OHMD_MAT4X4_LEFT_EYE_GL_PROJECTION: {
+			omat4x4f_transpose(&priv->calc_values.proj_left, (mat4x4f*)out);
+			break;
+		}
+	case OHMD_MAT4X4_RIGHT_EYE_GL_PROJECTION: {
+			omat4x4f_transpose(&priv->calc_values.proj_right, (mat4x4f*)out);
+			break;
+		}
+	default:
+		ohmd_set_error(priv->base.ctx, "invalid type given to getf (%d)", type);
+		return -1;
+		break;
+	}
+
+	return 0;
+}
+
+static void close_device(ohmd_device* device)
+{
+	LOGD("closing device");
+	rift_priv* priv = rift_priv_get(device);
+	hid_close(priv->handle);
+	free(priv);
+}
+
+static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
+{
+	rift_priv* priv = ohmd_alloc(driver->ctx, sizeof(rift_priv));
+	if(!priv)
+		goto cleanup;
+
+	priv->base.ctx = driver->ctx;
+
+	// Open the HID device
+	priv->handle = hid_open_path(desc->path);
+
+	if(!priv->handle)
+		goto cleanup;
+	
+	if(hid_set_nonblocking(priv->handle, 1) == -1){
+		ohmd_set_error(driver->ctx, "failed to set non-blocking on device");
+		goto cleanup;
+	}
+
+	unsigned char buf[FEATURE_BUFFER_SIZE];
+	
+	int size;
+
+	// Read and decode the sensor range
+	size = get_feature_report(priv, RIFT_CMD_RANGE, buf);
+	decode_sensor_range(&priv->sensor_range, buf, size);
+	dump_packet_sensor_range(&priv->sensor_range);
+
+	// Read and decode display information
+	size = get_feature_report(priv, RIFT_CMD_DISPLAY_INFO, buf);
+	decode_sensor_display_info(&priv->display_info, buf, size);
+	dump_packet_sensor_display_info(&priv->display_info);
+
+	// Read and decode the sensor config
+	size = get_feature_report(priv, RIFT_CMD_SENSOR_CONFIG, buf);
+	decode_sensor_config(&priv->sensor_config, buf, size);
+	dump_packet_sensor_config(&priv->sensor_config);
+
+	// if the sensor has display info data, use HMD coordinate frame
+	priv->coordinate_frame = priv->display_info.distortion_type != RIFT_DT_NONE ? RIFT_CF_HMD : RIFT_CF_SENSOR;
+
+	// apply sensor config
+	set_coordinate_frame(priv, priv->coordinate_frame);
+
+	// set keep alive interval to n seconds
+	pkt_keep_alive keep_alive = { 0, KEEP_ALIVE_VALUE };
+	size = encode_keep_alive(buf, &keep_alive);
+	send_feature_report(priv, buf, size);
+
+	// Update the time of the last keep alive we have sent.
+	priv->last_keep_alive = ohmd_get_tick();
+
+	// update sensor settings with new keep alive value
+	// (which will have been ignored in favor of the default 1000 ms one)
+	size = get_feature_report(priv, RIFT_CMD_SENSOR_CONFIG, buf);
+	decode_sensor_config(&priv->sensor_config, buf, size);
+	dump_packet_sensor_config(&priv->sensor_config);
+
+
+	calc_derived_values(priv);
+
+	priv->base.update = update_device;
+	priv->base.close = close_device;
+	priv->base.getf = getf;
+
+	ofusion_init(&priv->sensor_fusion);
+
+	return &priv->base;
+
+cleanup:
+	if(priv)
+		free(priv);
+
+	return NULL;
+}
+
+static void get_device_list(ohmd_driver* driver, ohmd_device_list* list)
+{
+	// enumerate HID devices and add any Rifts found to the device list
+#define OCULUS_VR_INC_ID 0x2833
+#define RIFT_DEVKIT_ID 0x0001
+
+	struct hid_device_info* devs = hid_enumerate(OCULUS_VR_INC_ID, RIFT_DEVKIT_ID);
+	struct hid_device_info* cur_dev = devs;
+
+	if(devs == NULL)
+		return;
+
+	while (cur_dev) {
+		ohmd_device_desc* desc = &list->devices[list->num_devices++];
+
+		strcpy(desc->driver, "OpenHMD Rift Driver");
+		strcpy(desc->vendor, "Oculus VR, Inc.");
+		strcpy(desc->product, "Rift (Devkit)");
+
+		strcpy(desc->path, cur_dev->path);
+
+		desc->driver_ptr = driver;
+
+		cur_dev = cur_dev->next;
+	}
+	hid_free_enumeration(devs);
+}
+
+static void destroy_driver(ohmd_driver* drv)
+{
+	LOGD("shutting down driver");
+	hid_exit();
+	free(drv);
+}
+
+ohmd_driver* ohmd_create_oculus_rift_drv(ohmd_context* ctx)
+{
+	ohmd_driver* drv = ohmd_alloc(ctx, sizeof(ohmd_driver));
+	if(drv == NULL)
+		return NULL;
+
+	drv->get_device_list = get_device_list;
+	drv->open_device = open_device;
+	drv->ctx = ctx;
+	drv->get_device_list = get_device_list;
+	drv->open_device = open_device;
+	drv->destroy = destroy_driver;
+
+	return drv;
+}

src/drv_oculus_rift/rift.h

@@ -0,0 +1,111 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Oculus Rift Driver Internal Interface */
+
+#ifndef RIFT_H
+#define RIFT_H
+
+#include "../openhmdi.h"
+
+#define FEATURE_BUFFER_SIZE 256
+
+typedef enum {
+	RIFT_CMD_SENSOR_CONFIG = 2,
+	RIFT_CMD_RANGE = 4,
+	RIFT_CMD_KEEP_ALIVE = 8,
+	RIFT_CMD_DISPLAY_INFO = 9
+} rift_sensor_feature_cmd;
+
+typedef enum {
+	RIFT_CF_SENSOR,
+	RIFT_CF_HMD
+} rift_coordinate_frame;
+
+typedef enum {
+	RIFT_IRQ_SENSORS = 1
+} rift_irq_cmd;
+
+typedef enum {
+	RIFT_DT_NONE,
+	RIFT_DT_SCREEN_ONLY,
+	RIFT_DT_DISTORTION
+} rift_distortion_type;
+
+// Sensor config flags
+#define RIFT_SCF_RAW_MODE           0x01
+#define RIFT_SCF_CALIBRATION_TEST   0x02
+#define RIFT_SCF_USE_CALIBRATION    0x04
+#define RIFT_SCF_AUTO_CALIBRATION   0x08
+#define RIFT_SCF_MOTION_KEEP_ALIVE  0x10
+#define RIFT_SCF_COMMAND_KEEP_ALIVE 0x20
+#define RIFT_SCF_SENSOR_COORDINATES 0x40
+
+
+
+typedef struct {
+	uint16_t command_id;
+	uint16_t accel_scale;
+	uint16_t gyro_scale;
+	uint16_t mag_scale;
+} pkt_sensor_range;
+
+typedef struct {
+	int32_t accel[3];
+	int32_t gyro[3];
+} pkt_tracker_sample;
+
+typedef struct {
+	uint8_t num_samples;
+	uint16_t timestamp;
+	uint16_t last_command_id;
+	int16_t temperature;
+	pkt_tracker_sample samples[3];
+	int16_t mag[3];
+} pkt_tracker_sensor;
+
+typedef struct {
+    uint16_t command_id;
+    uint8_t flags;
+    uint16_t packet_interval;
+    uint16_t keep_alive_interval; // in ms
+} pkt_sensor_config;
+
+typedef struct {
+	uint16_t command_id;
+	rift_distortion_type distortion_type;
+	uint8_t distortion_type_opts;
+	uint16_t h_resolution, v_resolution;
+	float h_screen_size, v_screen_size;
+	float v_center;
+	float lens_separation;
+	float eye_to_screen_distance[2];
+	float distortion_k[6];
+} pkt_sensor_display_info;
+
+typedef struct {
+	uint16_t command_id;
+	uint16_t keep_alive_interval;
+} pkt_keep_alive;
+
+
+bool decode_sensor_range(pkt_sensor_range* range, const unsigned char* buffer, int size);
+bool decode_sensor_display_info(pkt_sensor_display_info* info, const unsigned char* buffer, int size);
+bool decode_sensor_config(pkt_sensor_config* config, const unsigned char* buffer, int size);
+bool decode_tracker_sensor_msg(pkt_tracker_sensor* msg, const unsigned char* buffer, int size);
+
+void vec3f_from_rift_vec(const int32_t* smp, vec3f* out_vec);
+
+int encode_sensor_config(unsigned char* buffer, const pkt_sensor_config* config);
+int encode_keep_alive(unsigned char* buffer, const pkt_keep_alive* keep_alive);
+
+void dump_packet_sensor_range(const pkt_sensor_range* range);
+void dump_packet_sensor_config(const pkt_sensor_config* config);
+void dump_packet_sensor_display_info(const pkt_sensor_display_info* info);
+void dump_packet_tracker_sensor(const pkt_tracker_sensor* sensor);
+
+#endif

src/fusion.c

@@ -0,0 +1,124 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Sensor Fusion Implementation */
+
+#include <string.h>
+#include "openhmdi.h"
+
+void ofusion_init(fusion* me)
+{
+	memset(me, 0, sizeof(fusion));
+	me->orient.w = 1.0f;
+
+	ofq_init(&me->mag_fq, 20);
+	ofq_init(&me->accel_fq, 20);
+	ofq_init(&me->ang_vel_fq, 20);
+
+	me->flags = FF_USE_GRAVITY;
+	me->grav_gain = 0.05f;
+}
+
+void ofusion_update(fusion* me, float dt, const vec3f* ang_vel, const vec3f* accel, const vec3f* mag)
+{
+	me->ang_vel = *ang_vel;
+	me->accel = *accel;
+	me->raw_mag = *mag;
+
+	me->mag = *mag;
+
+	vec3f world_accel;
+	oquatf_get_rotated(&me->orient, accel, &world_accel);
+
+	me->iterations += 1;
+	me->time += dt;
+
+	ofq_add(&me->mag_fq, mag);
+	ofq_add(&me->accel_fq, &world_accel);
+	ofq_add(&me->ang_vel_fq, ang_vel);
+
+	float ang_vel_length = ovec3f_get_length(ang_vel);
+
+	if(ang_vel_length > 0.0001f){
+		vec3f rot_axis = 
+			{{ ang_vel->x / ang_vel_length, ang_vel->y / ang_vel_length, ang_vel->z / ang_vel_length }};
+
+		float rot_angle = ang_vel_length * dt;
+
+		quatf delta_orient;
+		oquatf_init_axis(&delta_orient, &rot_axis, rot_angle);
+
+		oquatf_mult_me(&me->orient, &delta_orient);
+	}
+
+	// gravity correction
+	if(me->flags & FF_USE_GRAVITY){
+		const float gravity_tolerance = .4f, ang_vel_tolerance = .1f;
+		const float min_tilt_error = 0.05f, max_tilt_error = 0.01f;
+
+		// if the device is within tolerance levels, count this as the device is level and add to the counter
+		// otherwise reset the counter and start over
+
+		me->device_level_count = 
+			fabsf(ovec3f_get_length(accel) - 9.82f) < gravity_tolerance && ang_vel_length < ang_vel_tolerance
+			? me->device_level_count + 1 : 0;
+
+		// device has been level for long enough, grab mean from the accelerometer filter queue (last n values)
+		// and use for correction
+
+		if(me->device_level_count > 50){
+			me->device_level_count = 0;
+
+			vec3f accel_mean;
+			ofq_get_mean(&me->accel_fq, &accel_mean);	
+
+			// Calculate a cross product between what the device
+			// thinks is up and what gravity indicates is down.
+			// The values are optimized of what we would get out
+			// from the cross product.
+			vec3f tilt = {{accel_mean.z, 0, -accel_mean.x}};
+
+			ovec3f_normalize_me(&tilt);
+			ovec3f_normalize_me(&accel_mean);
+
+			vec3f up = {{0, 1.0f, 0}};
+			float tilt_angle = ovec3f_get_angle(&up, &accel_mean);
+
+			if(tilt_angle > max_tilt_error){
+				me->grav_error_angle = tilt_angle;
+				me->grav_error_axis = tilt;
+			}
+		}
+
+		// preform gravity tilt correction
+		if(me->grav_error_angle > min_tilt_error){
+			float use_angle;
+			// if less than 2000 iterations have passed, set the up axis to the correction value outright
+			if(me->grav_error_angle > gravity_tolerance && me->iterations < 2000){
+				use_angle = -me->grav_error_angle;
+				me->grav_error_angle = 0;
+			}
+
+			// otherwise try to correct
+			else {
+				use_angle = -me->grav_gain * me->grav_error_angle * 0.005f * (5.0f * ang_vel_length + 1.0f); 
+				me->grav_error_angle += use_angle;
+			}
+
+			// perform the correction
+			quatf corr_quat, old_orient;
+			oquatf_init_axis(&corr_quat, &me->grav_error_axis, use_angle);
+			old_orient = me->orient;
+
+			oquatf_mult(&corr_quat, &old_orient, &me->orient);
+		}
+	}
+
+	// mitigate drift due to floating point
+	// inprecision with quat multiplication.
+	oquatf_normalize_me(&me->orient);
+}

src/fusion.h

@@ -0,0 +1,44 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Sensor Fusion */
+
+#ifndef FUSION_H
+#define FUSION_H
+
+#include "omath.h"
+
+#define FF_USE_GRAVITY 1
+
+typedef struct {
+	int state;
+
+	quatf orient;   // orientation
+	vec3f accel;    // acceleration
+	vec3f ang_vel;  // angular velocity
+	vec3f mag;      // magnetometer
+	vec3f raw_mag;  // raw magnetometer values
+
+	int iterations;
+	float time;
+
+	int flags;
+
+	// filter queues for magnetometer, accelerometers and angular velocity
+	filter_queue mag_fq, accel_fq, ang_vel_fq;
+
+	// gravity correction
+	int device_level_count;
+	float grav_error_angle;
+	vec3f grav_error_axis;
+	float grav_gain; // amount of correction
+} fusion;
+
+void ofusion_init(fusion* me);
+void ofusion_update(fusion* me, float dt, const vec3f* ang_vel, const vec3f* accel, const vec3f* mag_field);
+
+#endif

src/log.h

@@ -0,0 +1,35 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Logging and Error Handling */
+
+#ifndef LOG_H
+#define LOG_H
+
+void* ohmd_allocfn(ohmd_context* ctx, char* e_msg, size_t size);
+#define ohmd_alloc(_ctx, _size) ohmd_allocfn(_ctx, "could not allocate " #_size " bytes of RAM @ " __FILE__ ":" OHMD_STRINGIFY(__LINE__), _size)
+
+#ifndef LOGLEVEL
+#define LOGLEVEL 2
+#endif
+
+#define LOG(_level, _levelstr, ...) do{ if(_level >= LOGLEVEL){ printf("[%s] ", (_levelstr)); printf(__VA_ARGS__); puts(""); } } while(0)
+
+#if LOGLEVEL == 0
+#define LOGD(...) LOG(0, "DD", __VA_ARGS__)
+#else
+#define LOGD(...)
+#endif
+
+#define LOGV(...) LOG(1, "VV", __VA_ARGS__)
+#define LOGI(...) LOG(2, "II", __VA_ARGS__)
+#define LOGW(...) LOG(3, "WW", __VA_ARGS__)
+#define LOGE(...) LOG(4, "EE", __VA_ARGS__)
+
+#define ohmd_set_error(_ctx, ...) { snprintf((_ctx)->error_msg, OHMD_STR_SIZE, __VA_ARGS__); LOGE(__VA_ARGS__); }
+
+#endif

src/omath.c

@@ -0,0 +1,324 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Math Code Implementation */
+
+#include <string.h>
+#include "openhmdi.h"
+
+// vector
+
+float ovec3f_get_length(const vec3f* me)
+{
+	return sqrtf(POW2(me->x) + POW2(me->y) + POW2(me->z));
+}
+
+void ovec3f_normalize_me(vec3f* me)
+{
+	if(me->x == 0 && me->x == 0 && me->z == 0)
+		return;
+
+	float len = ovec3f_get_length(me);
+	me->x /= len;
+	me->y /= len;
+	me->z /= len;
+}
+
+float ovec3f_get_dot(const vec3f* me, const vec3f* vec)
+{
+	return me->x * vec->x + me->y * vec->y + me->z * vec->z;
+}
+
+float ovec3f_get_angle(const vec3f* me, const vec3f* vec)
+{
+	float dot = ovec3f_get_dot(me, vec);
+	float lengths = ovec3f_get_length(me) * ovec3f_get_length(vec);
+
+	if(lengths == 0)
+		return 0;
+
+	return acosf(dot / lengths);
+}
+
+
+// quaternion
+
+void oquatf_init_axis(quatf* me, const vec3f* vec, float angle)
+{
+	vec3f norm = *vec;
+	ovec3f_normalize_me(&norm);
+
+	me->x = norm.x * sinf(angle / 2.0f);
+	me->y = norm.y * sinf(angle / 2.0f);
+	me->z = norm.z * sinf(angle / 2.0f);
+	me->w = cosf(angle / 2.0f);
+}
+
+void oquatf_get_rotated(const quatf* me, const vec3f* vec, vec3f* out_vec)
+{
+	quatf q = {{vec->x * me->w + vec->z * me->y - vec->y * me->z,
+	            vec->y * me->w + vec->x * me->z - vec->z * me->x,
+	            vec->z * me->w + vec->y * me->x - vec->x * me->y,
+	            vec->x * me->x + vec->y * me->y + vec->z * me->z}};
+
+	out_vec->x = me->w * q.x + me->x * q.w + me->y * q.z - me->z * q.y;
+	out_vec->y = me->w * q.y + me->y * q.w + me->z * q.x - me->x * q.z;
+	out_vec->z = me->w * q.z + me->z * q.w + me->x * q.y - me->y * q.x;
+}
+
+void oquatf_mult(const quatf* me, const quatf* q, quatf* out_q)
+{
+	out_q->x = me->w * q->x + me->x * q->w + me->y * q->z - me->z * q->y;
+	out_q->y = me->w * q->y - me->x * q->z + me->y * q->w + me->z * q->x;
+	out_q->z = me->w * q->z + me->x * q->y - me->y * q->x + me->z * q->w;
+	out_q->w = me->w * q->w - me->x * q->x - me->y * q->y - me->z * q->z;
+}
+
+void oquatf_mult_me(quatf* me, const quatf* q)
+{
+	quatf tmp = *me;
+	oquatf_mult(&tmp, q, me);
+}
+
+void oquatf_normalize_me(quatf* me)
+{
+	float len = oquatf_get_length(me);
+	me->x /= len;
+	me->y /= len;
+	me->z /= len;
+	me->w /= len;
+}
+
+float oquatf_get_length(const quatf* me)
+{
+	return sqrtf(me->x * me->x + me->y * me->y + me->z * me->z + me->w * me->w);
+}
+
+void oquatf_get_mat3x3(const quatf* me, float mat[9])
+{
+	float xx = me->x * me->x;
+	float xy = me->x * me->y;
+	float xz = me->x * me->z;
+	float xw = me->x * me->w;
+
+	float yy = me->y * me->y;
+	float yz = me->y * me->z;
+	float yw = me->y * me->w;
+
+	float zz = me->z * me->z;
+	float zw = me->z * me->w;
+
+	mat[0] = 1 - 2 * ( yy + zz );
+	mat[1] =     2 * ( xy - zw );
+	mat[2] =     2 * ( xz + yw );
+
+	mat[3] =     2 * ( xy + zw );
+	mat[4] = 1 - 2 * ( xx + zz );
+	mat[5] =     2 * ( yz - xw );
+
+	mat[6] =     2 * ( xz - yw );
+	mat[7] =     2 * ( yz + xw );
+	mat[8] = 1 - 2 * ( xx + yy );
+}
+
+void omat3x3_get_scales(const float mat[9], float scales[3])
+{
+	for(int i = 0; i < 3; i++)
+		scales[i] = sqrtf(mat[3 * i] * mat[3 * i] + mat[3 * i + 1] * mat[ 3 * i + 1] + mat[ 3 * i + 2] * mat[ 3 * i + 2]);
+}
+
+void omat3x3_get_euler_angles(const float mat[9], vec3f* angles)
+{
+	float scales[3];
+	omat3x3_get_scales(mat, scales);
+
+	angles->x = RAD_TO_DEG(asinf(mat[5] / scales[1]));
+	if(mat[4] != 0.0f){
+		angles->y = RAD_TO_DEG(atan2f(-mat[2] / scales[0], mat[8] / scales[2]));
+		angles->z = RAD_TO_DEG(atan2f(-mat[3] / scales[1], mat[4] / scales[1]));
+	}else{
+		angles->y = 0.0f;
+		angles->z = RAD_TO_DEG(atan2f(mat[1] / scales[0], mat[0] / scales[0]));
+	}
+}
+
+void oquatf_get_mat4x4(const quatf* me, const vec3f* point, float mat[4][4])
+{
+	mat[0][0] = 1 - 2 * me->y * me->y - 2 * me->z * me->z;
+	mat[0][1] =     2 * me->x * me->y - 2 * me->w * me->z;
+	mat[0][2] =     2 * me->x * me->z + 2 * me->w * me->y;
+	mat[0][3] = point->x;
+
+	mat[1][0] =     2 * me->x * me->y + 2 * me->w * me->z;
+	mat[1][1] = 1 - 2 * me->x * me->x - 2 * me->z * me->z;
+	mat[1][2] =     2 * me->y * me->z - 2 * me->w * me->x;
+	mat[1][3] = point->y;
+
+	mat[2][0] =     2 * me->x * me->z - 2 * me->w * me->y;
+	mat[2][1] =     2 * me->y * me->z + 2 * me->w * me->x;
+	mat[2][2] = 1 - 2 * me->x * me->x - 2 * me->y * me->y;
+	mat[2][3] = point->z;
+
+	mat[3][0] = 0;
+	mat[3][1] = 0;
+	mat[3][2] = 0;
+	mat[3][3] = 1;
+}
+
+
+// matrix
+
+void omat4x4f_init_ident(mat4x4f* me)
+{
+	memset(me, 0, sizeof(*me));
+	me->m[0][0] = 1.0f;
+	me->m[1][1] = 1.0f;
+	me->m[2][2] = 1.0f;
+	me->m[3][3] = 1.0f;
+}
+
+void omat4x4f_init_perspective(mat4x4f* me, float fovy_rad, float aspect, float znear, float zfar)
+{
+	float sine, cotangent, delta, half_fov;
+
+	half_fov = fovy_rad / 2.0f;
+	delta = zfar - znear;
+	sine = sinf(half_fov);
+
+	if ((delta == 0.0f) || (sine == 0.0f) || (aspect == 0.0f)) {
+		omat4x4f_init_ident(me);
+		return;
+	}
+
+	cotangent = cosf(half_fov) / sine;
+
+	me->m[0][0] = cotangent / aspect;
+	me->m[0][1] = 0;
+	me->m[0][2] = 0;
+	me->m[0][3] = 0;
+
+	me->m[1][0] = 0;
+	me->m[1][1] = cotangent;
+	me->m[1][2] = 0;
+	me->m[1][3] = 0;
+
+	me->m[2][0] = 0;
+	me->m[2][1] = 0;
+	me->m[2][2] = -(zfar + znear) / delta;
+	me->m[2][3] = -2.0f * znear * zfar / delta;
+
+	me->m[3][0] = 0;
+	me->m[3][1] = 0;
+	me->m[3][2] = -1.0f;
+	me->m[3][3] = 0;
+}
+
+void omat4x4f_init_look_at(mat4x4f* me, const quatf* rot, const vec3f* eye)
+{
+	quatf q;
+	vec3f p;
+
+	q.x = -rot->x;
+	q.y = -rot->y;
+	q.z = -rot->z;
+	q.w =  rot->w;
+
+	p.x = -eye->x;
+	p.y = -eye->y;
+	p.z = -eye->z;
+
+	me->m[0][0] = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
+	me->m[0][1] =     2 * q.x * q.y - 2 * q.w * q.z;
+	me->m[0][2] =     2 * q.x * q.z + 2 * q.w * q.y;
+	me->m[0][3] = p.x * me->m[0][0] + p.y * me->m[0][1] + p.z * me->m[0][2];
+
+	me->m[1][0] =     2 * q.x * q.y + 2 * q.w * q.z;
+	me->m[1][1] = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
+	me->m[1][2] =     2 * q.y * q.z - 2 * q.w * q.x;
+	me->m[1][3] = p.x * me->m[1][0] + p.y * me->m[1][1] + p.z * me->m[1][2];
+
+	me->m[2][0] =     2 * q.x * q.z - 2 * q.w * q.y;
+	me->m[2][1] =     2 * q.y * q.z + 2 * q.w * q.x;
+	me->m[2][2] = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
+	me->m[2][3] = p.x * me->m[2][0] + p.y * me->m[2][1] + p.z * me->m[2][2];
+
+	me->m[3][0] = 0;
+	me->m[3][1] = 0;
+	me->m[3][2] = 0;
+	me->m[3][3] = 1;
+}
+
+void omat4x4f_init_translate(mat4x4f* me, float x, float y, float z)
+{
+	omat4x4f_init_ident(me);
+	me->m[0][3] = x;
+	me->m[1][3] = y;
+	me->m[2][3] = z;
+}
+
+void omat4x4f_transpose(const mat4x4f* m, mat4x4f* o)
+{
+	o->m[0][0] = m->m[0][0];
+	o->m[1][0] = m->m[0][1];
+	o->m[2][0] = m->m[0][2];
+	o->m[3][0] = m->m[0][3];
+
+	o->m[0][1] = m->m[1][0];
+	o->m[1][1] = m->m[1][1];
+	o->m[2][1] = m->m[1][2];
+	o->m[3][1] = m->m[1][3];
+
+	o->m[0][2] = m->m[2][0];
+	o->m[1][2] = m->m[2][1];
+	o->m[2][2] = m->m[2][2];
+	o->m[3][2] = m->m[2][3];
+
+	o->m[0][3] = m->m[3][0];
+	o->m[1][3] = m->m[3][1];
+	o->m[2][3] = m->m[3][2];
+	o->m[3][3] = m->m[3][3];
+}
+
+void omat4x4f_mult(const mat4x4f* l, const mat4x4f* r, mat4x4f *o)
+{
+	for(int i = 0; i < 4; i++){
+		float a0 = l->m[i][0], a1 = l->m[i][1], a2 = l->m[i][2], a3 = l->m[i][3];
+		o->m[i][0] = a0 * r->m[0][0] + a1 * r->m[1][0] + a2 * r->m[2][0] + a3 * r->m[3][0];
+		o->m[i][1] = a0 * r->m[0][1] + a1 * r->m[1][1] + a2 * r->m[2][1] + a3 * r->m[3][1];
+		o->m[i][2] = a0 * r->m[0][2] + a1 * r->m[1][2] + a2 * r->m[2][2] + a3 * r->m[3][2];
+		o->m[i][3] = a0 * r->m[0][3] + a1 * r->m[1][3] + a2 * r->m[2][3] + a3 * r->m[3][3];
+	}
+}
+
+
+// filter queue
+
+void ofq_init(filter_queue* me, int size)
+{
+	memset(me, 0, sizeof(filter_queue));
+	me->size = size;
+}
+
+void ofq_add(filter_queue* me, const vec3f* vec)
+{
+	me->elems[me->at] = *vec;
+	me->at = ((me->at + 1) % me->size);
+}
+
+void ofq_get_mean(const filter_queue* me, vec3f* vec)
+{
+	vec->x = vec->y = vec->z = 0;
+	for(int i = 0; i < me->size; i++){
+		vec->x += me->elems[i].x;
+		vec->y += me->elems[i].y;
+		vec->z += me->elems[i].z;
+	}
+
+	vec->x /= (float)me->size;
+	vec->y /= (float)me->size;
+	vec->z /= (float)me->size;
+}

src/omath.h

@@ -0,0 +1,90 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Math */
+
+#ifndef OMATH_H
+#define OMATH_H
+
+#include <math.h>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+#define POW2(_x) ((_x) * (_x))
+#define RAD_TO_DEG(_r) ((_r) * 360.0f / (2.0f * (float)M_PI))
+#define DEG_TO_RAD(_d) ((_d) * (2.0f * (float)M_PI) / 360.0f)
+
+
+// vector
+
+typedef union { 
+	struct { 
+		float x, y, z; 
+	}; 
+	float arr[3]; 
+} vec3f;
+
+void ovec3f_normalize_me(vec3f* me);
+float ovec3f_get_length(const vec3f* me);
+float ovec3f_get_angle(const vec3f* me, const vec3f* vec); 
+float ovec3f_get_dot(const vec3f* me, const vec3f* vec);
+
+
+// quaternion
+
+typedef union { 
+	struct { 
+		float x, y, z, w; 
+	}; 
+	float arr[4]; 
+} quatf;
+
+void oquatf_init_axis(quatf* me, const vec3f* vec, float angle);
+
+void oquatf_get_rotated(const quatf* me, const vec3f* vec, vec3f* out_vec);
+void oquatf_mult_me(quatf* me, const quatf* q);
+void oquatf_mult(const quatf* me, const quatf* q, quatf* out_q);
+void oquatf_normalize_me(quatf* me);
+float oquatf_get_length(const quatf* me);
+
+void oquatf_get_mat4x4(const quatf* me, const vec3f* point, float mat[4][4]);
+void oquatf_get_mat3x3(const quatf* me, float mat[9]);
+
+void omat3x3_get_scales(const float mat[9], float scales[3]);
+void omat3x3_get_euler_angles(const float mat[9], vec3f* angles);
+
+
+// matrix
+
+typedef union {
+	float m[4][4];
+	float arr[16];
+} mat4x4f;
+
+void omat4x4f_init_ident(mat4x4f* me);
+void omat4x4f_init_perspective(mat4x4f* me, float fov_rad, float aspect, float znear, float zfar);
+void omat4x4f_init_look_at(mat4x4f* me, const quatf* ret, const vec3f* eye);
+void omat4x4f_init_translate(mat4x4f* me, float x, float y, float z);
+void omat4x4f_mult(const mat4x4f* left, const mat4x4f* right, mat4x4f* out_mat);
+void omat4x4f_transpose(const mat4x4f* me, mat4x4f* out_mat);
+
+
+// filter queue
+#define FILTER_QUEUE_MAX_SIZE 256
+
+typedef struct {
+	int at, size;
+	vec3f elems[FILTER_QUEUE_MAX_SIZE];
+} filter_queue;
+
+void ofq_init(filter_queue* me, int size);
+void ofq_add(filter_queue* me, const vec3f* vec);
+void ofq_get_mean(const filter_queue* me, vec3f* vec);
+
+#endif

src/openhmd.c

@@ -0,0 +1,108 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Main Lib Implemenation */
+
+#include "openhmdi.h"
+#include <stdlib.h>
+#include <string.h>
+
+OHMD_APIENTRY ohmd_context* ohmd_ctx_create()
+{
+	ohmd_context* ctx = calloc(1, sizeof(ohmd_context));
+	if(!ctx){
+		LOGE("could not allocate RAM for context");
+		return NULL;
+	}
+
+	ctx->drivers[ctx->num_drivers++] = ohmd_create_oculus_rift_drv(ctx);
+
+	return ctx;
+}
+
+OHMD_APIENTRY void ohmd_ctx_destroy(ohmd_context* ctx)
+{
+	for(int i = 0; i < ctx->num_active_devices; i++){
+		ctx->active_devices[i]->close(ctx->active_devices[i]);
+	}
+
+	for(int i = 0; i < ctx->num_drivers; i++){
+		ctx->drivers[i]->destroy(ctx->drivers[i]);
+	}
+
+	free(ctx);
+}
+
+OHMD_APIENTRY void ohmd_ctx_update(ohmd_context* ctx)
+{
+	for(int i = 0; i < ctx->num_active_devices; i++)
+		ctx->active_devices[i]->update(ctx->active_devices[i]);
+}
+
+OHMD_APIENTRY const char* ohmd_ctx_get_error(ohmd_context* ctx)
+{
+	return ctx->error_msg;
+}
+
+OHMD_APIENTRY int ohmd_ctx_probe(ohmd_context* ctx)
+{
+	memset(&ctx->list, 0, sizeof(ohmd_device_list));
+	for(int i = 0; i < ctx->num_drivers; i++){
+		ctx->drivers[i]->get_device_list(ctx->drivers[i], &ctx->list);
+	}
+
+	return ctx->list.num_devices;
+}
+
+OHMD_APIENTRY const char* ohmd_list_gets(ohmd_context* ctx, int index, ohmd_string_value type)
+{
+	if(index >= ctx->list.num_devices)
+		return NULL;
+
+	switch(type){
+	case OHMD_VENDOR:
+		return ctx->list.devices[index].vendor;
+	case OHMD_PRODUCT:
+		return ctx->list.devices[index].product;
+	case OHMD_PATH:
+		return ctx->list.devices[index].path;
+	default:
+		return NULL;
+	}
+}
+
+OHMD_APIENTRY ohmd_device* ohmd_list_open_device(ohmd_context* ctx, int index)
+{
+	if(index >= 0 && index < ctx->list.num_devices){
+
+		ohmd_device_desc* desc = &ctx->list.devices[index];
+		ohmd_driver* driver = (ohmd_driver*)desc->driver_ptr;
+		ohmd_device* device = driver->open_device(driver, desc);
+
+		if (device == NULL)
+			return NULL;
+
+		ctx->active_devices[ctx->num_active_devices++] = device;
+		return device;
+	}
+
+	ohmd_set_error(ctx, "no device with index: %d", index);
+	return NULL;
+}
+
+OHMD_APIENTRY int ohmd_device_getf(ohmd_device* device, ohmd_float_value type, float* out)
+{
+	return device->getf(device, type, out);
+}
+
+void* ohmd_allocfn(ohmd_context* ctx, char* e_msg, size_t size)
+{
+	void* ret = calloc(1, size);
+	if(!ret)
+		ohmd_set_error(ctx, "%s", e_msg);
+	return ret;
+}

src/openhmdi.h

@@ -0,0 +1,77 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Internal interface */
+
+#ifndef OPENHMDI_H
+#define OPENHMDI_H
+
+#include "openhmd.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#define OHMD_MAX_DEVICES 16
+
+#define OHMD_MAX(_a, _b) ((_a) > (_b) ? (_a) : (_b))
+#define OHMD_MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b))
+
+#define OHMD_STRINGIFY(_what) #_what
+
+typedef struct ohmd_driver ohmd_driver;
+
+typedef struct
+{
+	char driver[OHMD_STR_SIZE];
+	char vendor[OHMD_STR_SIZE];
+	char product[OHMD_STR_SIZE];
+	char path[OHMD_STR_SIZE];
+	ohmd_driver* driver_ptr;
+} ohmd_device_desc;
+
+typedef struct {
+	int num_devices;
+	ohmd_device_desc devices[OHMD_MAX_DEVICES];
+} ohmd_device_list;
+
+struct ohmd_driver {
+	void (*get_device_list)(ohmd_driver* driver, ohmd_device_list* list);
+	ohmd_device* (*open_device)(ohmd_driver* driver, ohmd_device_desc* desc);
+	void (*destroy)(ohmd_driver* driver);
+	ohmd_context* ctx;
+};
+
+struct ohmd_device {
+	int (*getf)(ohmd_device* device, ohmd_float_value type, float* out);
+	void (*update)(ohmd_device* device);
+	void (*close)(ohmd_device* device);
+	ohmd_context* ctx;
+};
+
+struct ohmd_context {
+	ohmd_driver* drivers[16];
+	int num_drivers;
+
+	ohmd_device_list list;
+
+	ohmd_device* active_devices[256];
+	int num_active_devices;
+
+	char error_msg[OHMD_STR_SIZE];
+};
+
+// drivers
+ohmd_driver* ohmd_create_oculus_rift_drv(ohmd_context* ctx);
+
+#include "log.h"
+#include "platform.h"
+#include "omath.h"
+#include "fusion.h"
+
+#endif

src/platform-posix.c

@@ -0,0 +1,49 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Platform Specific Functions, Unix/Posix Implementation */
+
+#if defined(__unix__) || defined(__unix) || defined(__APPLE__) || defined(__MACH__)
+
+#ifdef __CYGWIN__
+#define CLOCK_MONOTONIC (clockid_t)4
+#endif
+
+#define _POSIX_C_SOURCE 199309L
+
+#include <time.h>
+#include <sys/time.h>
+#include <stdio.h>
+
+// Use clock_gettime if the system implements posix realtime timers
+#ifndef CLOCK_MONOTONIC
+double ohmd_get_tick()
+{
+	struct timeval now;
+	gettimeofday(&now, NULL);
+	return (double)now.tv_sec * 1.0 + (double)now.tv_usec / 1000000.0;
+}
+#else
+double ohmd_get_tick()
+{
+	struct timespec now;
+	clock_gettime(CLOCK_MONOTONIC, &now);
+	return (double)now.tv_sec * 1.0 + (double)now.tv_nsec / 1000000000.0;
+}
+#endif
+
+void ohmd_sleep(double seconds)
+{
+	struct timespec sleepfor;
+
+	sleepfor.tv_sec = (time_t)seconds;
+	sleepfor.tv_nsec = (long)((seconds - sleepfor.tv_sec) * 1000000000.0);
+
+	nanosleep(&sleepfor, NULL);
+}
+
+#endif

src/platform-win32.c

@@ -0,0 +1,36 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Platform Specific Functions, Win32 Implementation */
+
+#ifdef _WIN32
+
+#define WIN32_LEAN_AND_MEAN
+#define WIN32_EXTRA_LEAN
+
+#include <windows.h>
+
+double ohmd_get_tick()
+{
+        double high, low;
+        FILETIME filetime;
+
+        GetSystemTimeAsFileTime(&filetime);
+
+        high = filetime.dwHighDateTime;
+        low = filetime.dwLowDateTime;
+
+        return (high * 4294967296.0 + low) / 10000000;
+}
+
+// TODO higher resolution
+void ohmd_sleep(double seconds)
+{
+        Sleep((DWORD)(seconds * 1000));
+}
+
+#endif

src/platform.h

@@ -0,0 +1,16 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Internal Interface for Platform Specific Functions */
+
+#ifndef PLATFORM_H
+#define PLATFORM_H
+
+double ohmd_get_tick();
+void ohmd_sleep(double seconds);
+
+#endif

tests/Makefile.am

@@ -0,0 +1,5 @@
+SUBDIRS = unittests simple
+
+if BUILD_OPENGL_TEST
+SUBDIRS += opengl
+endif

tests/opengl/Makefile.am

@@ -0,0 +1,5 @@
+bin_PROGRAMS = opengltest
+AM_CPPFLAGS = -Wall -Werror -I$(top_srcdir)/include -DOHMD_STATIC $(sdl_CFLAGS) $(GLEW_CFLAGS)
+opengltest_SOURCES = gl.c main.c
+opengltest_LDADD = $(top_srcdir)/src/libopenhmd.la -lm 
+opengltest_LDFLAGS = -static-libtool-libs $(sdl_LIBS) $(GLEW_LIBS) $(GL_LIBS)

tests/opengl/gl.c

@@ -0,0 +1,242 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* OpenGL Test - GL Helper Functions Implementation */
+
+#include "gl.h"
+#include <string.h>
+#include <math.h>
+
+#ifdef __unix
+#include <signal.h>
+#endif
+
+#ifndef M_PI
+#define M_PI 3.14159265359
+#endif
+
+void init_gl(gl_ctx* ctx, int w, int h)
+{
+	memset(ctx, 0, sizeof(gl_ctx));
+
+	// == Initialize SDL ==
+	int ret = SDL_Init(SDL_INIT_EVERYTHING);
+	if(ret < 0){
+		printf("SDL_Init failed\n");
+		exit(-1);
+	}
+
+	SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
+	SDL_GL_SetAttribute(SDL_GL_SWAP_CONTROL, 1);
+
+	ctx->screen = SDL_SetVideoMode(w, h, 0, SDL_OPENGL | SDL_GL_DOUBLEBUFFER);
+	if(ctx->screen == NULL){
+		printf("SDL_SetVideoMode failed\n");
+		exit(-1);
+	}
+
+	// Disable ctrl-c catching on linux (and OS X?) 
+	#ifdef __unix
+	signal(SIGINT, SIG_DFL);
+	#endif
+
+	// Load extensions.
+	glewInit();
+
+	printf("OpenGL Renderer: %s\n", glGetString(GL_RENDERER));
+	printf("OpenGL Vendor: %s\n", glGetString(GL_VENDOR));
+	printf("OpenGL Version: %s\n", glGetString(GL_VERSION)); 
+
+	// == Initialize OpenGL ==
+	glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
+	glClear(GL_COLOR_BUFFER_BIT);
+
+	glEnable(GL_BLEND);
+	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+	glEnable(GL_ALPHA_TEST);
+	glLoadIdentity();
+
+	glShadeModel(GL_SMOOTH);
+	glDisable(GL_DEPTH_TEST);
+	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
+	glLoadIdentity();
+
+	glMatrixMode(GL_PROJECTION);
+	glEnable(GL_POLYGON_SMOOTH); 
+	glLoadIdentity();
+
+	glViewport(0, 0, ctx->screen->w, ctx->screen->h);
+
+	perspective(ctx);
+}
+
+void ortho(gl_ctx* ctx)
+{
+	glMatrixMode(GL_PROJECTION);
+	//glPushMatrix();
+	glLoadIdentity();
+
+	glOrtho(0.0f, ctx->screen->w, ctx->screen->h, 0.0f, -1.0f, 1.0f);
+	glMatrixMode(GL_MODELVIEW);
+	//glPushMatrix();
+	glLoadIdentity();
+	
+	glDisable(GL_DEPTH_TEST);
+	glDisable(GL_DEPTH);
+
+	glDisable(GL_MULTISAMPLE);
+}
+
+void calculate_frustum(double fovy, double aspect, double znear, double zfar)
+{
+	float fh = tan(fovy / 360 * M_PI) * znear;
+	float fw = fh * aspect;
+	glFrustum(-fw, fw, -fh, fh, znear, zfar);
+}
+
+void perspective(gl_ctx* ctx)
+{
+	glViewport(0, 0, ctx->screen->w, ctx->screen->h);
+	
+	glMatrixMode(GL_PROJECTION);
+
+	glLoadIdentity();
+	calculate_frustum(90, ctx->screen->w / ctx->screen->h, .1, 5000);
+
+	glMatrixMode(GL_MODELVIEW);
+	glLoadIdentity();
+
+	glEnable(GL_DEPTH);
+	glEnable(GL_MULTISAMPLE_ARB);
+
+	glDepthFunc(GL_LEQUAL);
+	glEnable(GL_DEPTH_TEST);
+}
+
+void draw_cube()
+{
+	glBegin(GL_QUADS);
+
+	glVertex3f(  0.5f,  0.5f, -0.5f); /* Top Right Of The Quad (Top)      */
+	glVertex3f( -0.5f,  0.5f, -0.5f); /* Top Left Of The Quad (Top)       */
+	glVertex3f( -0.5f,  0.5f,  0.5f); /* Bottom Left Of The Quad (Top)    */
+	glVertex3f(  0.5f,  0.5f,  0.5f); /* Bottom Right Of The Quad (Top)   */
+
+	glVertex3f(  0.5f, -0.5f,  0.5f); /* Top Right Of The Quad (Botm)     */
+	glVertex3f( -0.5f, -0.5f,  0.5f); /* Top Left Of The Quad (Botm)      */
+	glVertex3f( -0.5f, -0.5f, -0.5f); /* Bottom Left Of The Quad (Botm)   */
+	glVertex3f(  0.5f, -0.5f, -0.5f); /* Bottom Right Of The Quad (Botm)  */
+
+	glVertex3f(  0.5f,  0.5f,  0.5f); /* Top Right Of The Quad (Front)    */
+	glVertex3f( -0.5f,  0.5f,  0.5f); /* Top Left Of The Quad (Front)     */
+	glVertex3f( -0.5f, -0.5f,  0.5f); /* Bottom Left Of The Quad (Front)  */
+	glVertex3f(  0.5f, -0.5f,  0.5f); /* Bottom Right Of The Quad (Front) */
+
+	glVertex3f(  0.5f, -0.5f, -0.5f); /* Bottom Left Of The Quad (Back)   */
+	glVertex3f( -0.5f, -0.5f, -0.5f); /* Bottom Right Of The Quad (Back)  */
+	glVertex3f( -0.5f,  0.5f, -0.5f); /* Top Right Of The Quad (Back)     */
+	glVertex3f(  0.5f,  0.5f, -0.5f); /* Top Left Of The Quad (Back)      */
+
+	glVertex3f( -0.5f,  0.5f,  0.5f); /* Top Right Of The Quad (Left)     */
+	glVertex3f( -0.5f,  0.5f, -0.5f); /* Top Left Of The Quad (Left)      */
+	glVertex3f( -0.5f, -0.5f, -0.5f); /* Bottom Left Of The Quad (Left)   */
+	glVertex3f( -0.5f, -0.5f,  0.5f); /* Bottom Right Of The Quad (Left)  */
+
+	glVertex3f(  0.5f,  0.5f, -0.5f); /* Top Right Of The Quad (Right)    */
+	glVertex3f(  0.5f,  0.5f,  0.5f); /* Top Left Of The Quad (Right)     */
+	glVertex3f(  0.5f, -0.5f,  0.5f); /* Bottom Left Of The Quad (Right)  */
+	glVertex3f(  0.5f, -0.5f, -0.5f); /* Bottom Right Of The Quad (Right) */
+
+	glEnd();
+
+}
+
+static void compile_shader_src(GLuint shader, const char* src)
+{
+	glShaderSource(shader, 1, &src, NULL);
+	glCompileShader(shader);
+
+	GLint status;
+	GLint length;
+	char log[4096] = {0};
+
+	glGetShaderiv(shader, GL_COMPILE_STATUS, &status);
+	glGetShaderInfoLog(shader, 4096, &length, log);
+	if(status == GL_FALSE){
+		printf("compile failed %s\n", log);
+	}
+}
+
+GLuint compile_shader(const char* vertex, const char* fragment)
+{
+	// Create the handels
+	GLuint vertexShader = glCreateShader(GL_VERTEX_SHADER);
+	GLuint fragmentShader = glCreateShader(GL_FRAGMENT_SHADER);
+	GLuint programShader = glCreateProgram();
+
+	// Attach the shaders to a program handel.
+	glAttachShader(programShader, vertexShader);
+	glAttachShader(programShader, fragmentShader);
+
+	// Load and compile the Vertex Shader
+	compile_shader_src(vertexShader, vertex);
+
+	// Load and compile the Fragment Shader
+	compile_shader_src(fragmentShader, fragment);
+
+	// The shader objects are not needed any more,
+	// the programShader is the complete shader to be used.
+	glDeleteShader(vertexShader);
+	glDeleteShader(fragmentShader);
+
+	glLinkProgram(programShader);
+
+	GLint status;
+	GLint length;
+	char log[4096] = {0};
+
+	glGetProgramiv(programShader, GL_LINK_STATUS, &status);
+	glGetProgramInfoLog(programShader, 4096, &length, log);
+	if(status == GL_FALSE){
+		printf("link failed %s\n", log);
+	}
+
+	return programShader;
+}
+
+void create_fbo(int eye_width, int eye_height, GLuint* fbo, GLuint* color_tex, GLuint* depth_tex)
+{
+	glGenTextures(1, color_tex);
+	glGenTextures(1, depth_tex);
+	glGenFramebuffers(1, fbo);
+
+	glBindTexture(GL_TEXTURE_2D, *color_tex);
+	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, eye_width, eye_height, 0, GL_RGBA, GL_UNSIGNED_INT, NULL);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+
+	glBindTexture(GL_TEXTURE_2D, *depth_tex);
+	glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT24, eye_width, eye_height, 0, GL_DEPTH_COMPONENT, GL_UNSIGNED_BYTE, NULL);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	glBindTexture(GL_TEXTURE_2D, 0);
+
+	glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, *fbo);
+	glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, *color_tex, 0);
+	glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, *depth_tex, 0);
+
+	GLenum status = glCheckFramebufferStatusEXT(GL_FRAMEBUFFER_EXT);
+
+	if(status != GL_FRAMEBUFFER_COMPLETE_EXT){
+		printf("failed to create fbo %x\n", status);
+	}
+	glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
+}

tests/opengl/gl.h

@@ -0,0 +1,31 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* OpenGL Test - Interface For GL Helper Functions */
+
+#ifndef GL_H
+#define GL_H
+
+#include <SDL.h>
+
+#include <GL/glew.h>
+#include <GL/gl.h>
+
+typedef struct {
+	int w, h;
+	SDL_Surface* screen;
+} gl_ctx;
+
+void ortho(gl_ctx* ctx);
+void perspective(gl_ctx* ctx);
+void init_gl(gl_ctx* ctx, int w, int h);
+void draw_cube();
+GLuint compile_shader(const char* vertex, const char* fragment);
+void create_fbo(int eye_width, int eye_height, GLuint* fbo, GLuint* color_tex, GLuint* depth_tex);
+
+
+#endif

tests/opengl/main.c

@@ -0,0 +1,238 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* OpenGL Test - Main Implementation */
+
+#include <openhmd.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <math.h>
+#include "gl.h"
+
+#define TEST_WIDTH 1280
+#define TEST_HEIGHT 800
+
+#define EYE_WIDTH (TEST_WIDTH / 2 * 2)
+#define EYE_HEIGHT (TEST_HEIGHT * 2)
+
+char* read_file(const char* filename)
+{
+	FILE* f = fopen(filename, "r");
+	fseek(f, 0, SEEK_END);
+	long len = ftell(f);
+	fseek(f, 0, SEEK_SET);
+
+	char* buffer = calloc(1, len + 1);
+	assert(buffer);
+
+	assert( fread(buffer, len, 1, f) ); 
+
+	fclose(f);
+
+	return buffer;
+}
+
+float randf()
+{
+	return (float)rand() / (float)RAND_MAX;
+}
+
+GLuint gen_cubes()
+{
+	GLuint list = glGenLists(1);
+
+	// Set the random seed.
+	srand(42);
+
+	glNewList(list, GL_COMPILE);
+
+	for(float a = 0.0f; a < 360.0f; a += 20.0f){
+		glPushMatrix();
+
+		glRotatef(a, 0, 1, 0);
+		glTranslatef(0, 0, -1);
+		glScalef(0.2, 0.2, 0.2);
+		glRotatef(randf() * 360, randf(), randf(), randf());
+
+		glColor4f(randf(), randf(), randf(), randf() * .5f + .5f);
+		draw_cube();
+
+		glPopMatrix();
+	}
+
+	// draw floor
+	glColor4f(0, 1.0f, .25f, .25f);
+	glTranslatef(0, -2.5f, 0);
+	draw_cube();
+
+	glEndList();
+
+	return list;
+}
+
+void draw_scene(GLuint list)
+{
+	// draw cubes
+	glCallList(list);
+}
+
+int main(int argc, char** argv)
+{
+	(void)argc; (void) argv;
+
+	ohmd_context* ctx = ohmd_ctx_create();
+	int num_devices = ohmd_ctx_probe(ctx);
+	if(num_devices < 0){
+		printf("failed to probe devices: %s\n", ohmd_ctx_get_error(ctx));
+		return 1;
+	}
+
+	ohmd_device* hmd = ohmd_list_open_device(ctx, 0);
+	
+	if(!hmd){
+		printf("failed to open device: %s\n", ohmd_ctx_get_error(ctx));
+		return 1;
+	}
+
+	gl_ctx gl;
+	init_gl(&gl, TEST_WIDTH, TEST_HEIGHT);
+
+	char* vertex = read_file("shaders/test1.vert.glsl");
+	char* fragment = read_file("shaders/test1.frag.glsl");
+
+	GLuint shader = compile_shader(vertex, fragment);
+	glUseProgram(shader);
+	glUniform1i(glGetUniformLocation(shader, "warpTexture"), 0);
+	glUseProgram(0);
+
+	GLuint list = gen_cubes();
+
+	GLuint left_color_tex = 0, left_depth_tex = 0, left_fbo = 0;
+	create_fbo(EYE_WIDTH, EYE_HEIGHT, &left_fbo, &left_color_tex, &left_depth_tex);
+
+	GLuint right_color_tex = 0, right_depth_tex = 0, right_fbo = 0;
+	create_fbo(EYE_WIDTH, EYE_HEIGHT, &right_fbo, &right_color_tex, &right_depth_tex);
+
+
+	bool done = false;
+	while(!done){
+		ohmd_ctx_update(ctx);
+
+		SDL_Event event;
+		while(SDL_PollEvent(&event)){
+			if(event.type == SDL_KEYDOWN){
+				switch(event.key.keysym.sym){
+				case SDLK_ESCAPE:
+					done = true;
+					break;
+				case SDLK_F1:
+					SDL_WM_ToggleFullScreen(gl.screen);
+					break;
+				default:
+					break;
+				}
+			}
+		}
+
+		// Common scene state
+		glEnable(GL_BLEND);
+		glEnable(GL_DEPTH_TEST);
+		float matrix[16];
+
+		// set hmd rotation, for left eye.
+		glMatrixMode(GL_PROJECTION);
+		ohmd_device_getf(hmd, OHMD_MAT4X4_LEFT_EYE_GL_PROJECTION, matrix);
+		glLoadMatrixf(matrix);
+
+		glMatrixMode(GL_MODELVIEW);
+		ohmd_device_getf(hmd, OHMD_MAT4X4_LEFT_EYE_GL_MODELVIEW, matrix);
+		glLoadMatrixf(matrix);
+
+		// Draw scene into framebuffer.
+		glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, left_fbo);
+		glViewport(0, 0, EYE_WIDTH, EYE_HEIGHT);
+		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+		draw_scene(list);
+		glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
+
+
+		// set hmd rotation, for right eye.
+		glMatrixMode(GL_PROJECTION);
+		ohmd_device_getf(hmd, OHMD_MAT4X4_RIGHT_EYE_GL_PROJECTION, matrix);
+		glLoadMatrixf(matrix);
+
+		glMatrixMode(GL_MODELVIEW);
+		ohmd_device_getf(hmd, OHMD_MAT4X4_RIGHT_EYE_GL_MODELVIEW, matrix);
+		glLoadMatrixf(matrix);
+
+		// Draw scene into framebuffer.
+		glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, right_fbo);
+		glViewport(0, 0, EYE_WIDTH, EYE_HEIGHT);
+		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+		draw_scene(list);
+
+		// Clean up common draw state
+		glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
+		glDisable(GL_BLEND);
+		glDisable(GL_DEPTH_TEST);
+
+
+		// Setup ortho state.
+		glUseProgram(shader);
+		glViewport(0, 0, TEST_WIDTH, TEST_HEIGHT);
+		glEnable(GL_TEXTURE_2D);
+		glColor4d(1, 1, 1, 1);
+
+		// Setup simple render state
+		glMatrixMode(GL_PROJECTION);
+		glLoadIdentity();
+		glMatrixMode(GL_MODELVIEW);
+		glLoadIdentity();
+
+		// Draw left eye
+		glBindTexture(GL_TEXTURE_2D, left_color_tex);
+		glBegin(GL_QUADS);
+		glTexCoord2d( 0,  0);
+		glVertex3d(  -1, -1, 0);
+		glTexCoord2d( 1,  0);
+		glVertex3d(   0, -1, 0);
+		glTexCoord2d( 1,  1);
+		glVertex3d(   0,  1, 0);
+		glTexCoord2d( 0,  1);
+		glVertex3d(  -1,  1, 0);
+		glEnd();
+
+		// Draw right eye
+		glBindTexture(GL_TEXTURE_2D, right_color_tex);
+		glBegin(GL_QUADS);
+		glTexCoord2d( 0,  0);
+		glVertex3d(   0, -1, 0);
+		glTexCoord2d( 1,  0);
+		glVertex3d(   1, -1, 0);
+		glTexCoord2d( 1,  1);
+		glVertex3d(   1,  1, 0);
+		glTexCoord2d( 0,  1);
+		glVertex3d(   0,  1, 0);
+		glEnd();
+
+		// Clean up state.
+		glBindTexture(GL_TEXTURE_2D, 0);
+		glDisable(GL_TEXTURE_2D);
+		glUseProgram(0);
+
+		// Da swap-dawup!
+		SDL_GL_SwapBuffers();
+		SDL_Delay(10);
+	}
+
+	ohmd_ctx_destroy(ctx);
+
+	free(vertex);
+	free(fragment);
+	
+	return 0;
+}

tests/opengl/shaders/test1.frag.glsl

@@ -0,0 +1,43 @@
+#version 120
+
+// Taken from mts3d forums, from user fredrik.
+
+uniform sampler2D warpTexture;
+
+const vec2 LeftLensCenter = vec2(0.2863248, 0.5);
+const vec2 RightLensCenter = vec2(0.7136753, 0.5);
+const vec2 LeftScreenCenter = vec2(0.25, 0.5);
+const vec2 RightScreenCenter = vec2(0.75, 0.5);
+const vec2 Scale = vec2(0.1469278, 0.2350845);
+const vec2 ScaleIn = vec2(4, 2.5);
+const vec4 HmdWarpParam   = vec4(1, 0.22, 0.24, 0);
+
+// Scales input texture coordinates for distortion.
+vec2 HmdWarp(vec2 in01, vec2 LensCenter)
+{
+	vec2 theta = (in01 - LensCenter) * ScaleIn; // Scales to [-1, 1]
+	float rSq = theta.x * theta.x + theta.y * theta.y;
+	vec2 rvector = theta * (HmdWarpParam.x + HmdWarpParam.y * rSq +
+		HmdWarpParam.z * rSq * rSq +
+		HmdWarpParam.w * rSq * rSq * rSq);
+	return LensCenter + Scale * rvector;
+}
+
+void main()
+{
+	// The following two variables need to be set per eye
+	vec2 LensCenter = gl_FragCoord.x < 640 ? LeftLensCenter : RightLensCenter;
+	vec2 ScreenCenter = gl_FragCoord.x < 640 ? LeftScreenCenter : RightScreenCenter;
+
+	vec2 oTexCoord = gl_FragCoord.xy / vec2(1280, 800);
+
+	vec2 tc = HmdWarp(oTexCoord, LensCenter);
+	if (any(bvec2(clamp(tc,ScreenCenter-vec2(0.25,0.5), ScreenCenter+vec2(0.25,0.5)) - tc)))
+	{
+		gl_FragColor = vec4(0.0, 0.0, 0.0, 1.0);
+		return;
+	}
+
+	tc.x = gl_FragCoord.x < 640 ? (2.0 * tc.x) : (2.0 * (tc.x - 0.5));
+	gl_FragColor = texture2D(warpTexture, tc);
+}

tests/opengl/shaders/test1.vert.glsl

@@ -0,0 +1,8 @@
+#version 120
+
+void main(void)
+{
+	gl_TexCoord[0] = gl_MultiTexCoord0;
+	gl_Position = ftransform();
+}
+

tests/simple/Makefile.am

@@ -0,0 +1,5 @@
+bin_PROGRAMS = simple
+AM_CPPFLAGS = -Wall -Werror -I$(top_srcdir)/include -DOHMD_STATIC
+simple_SOURCES = simple.c
+simple_LDADD = $(top_srcdir)/src/libopenhmd.la -lm
+simple_LDFLAGS = -static-libtool-libs

tests/simple/simple.c

@@ -0,0 +1,57 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Simple Test */
+
+#include <openhmd.h>
+
+#include <stdio.h>
+
+void ohmd_sleep(double);
+double ohmd_get_tick();
+
+int main(int argc, char** argv)
+{
+	(void)argc; (void)argv;
+	ohmd_context* ctx = ohmd_ctx_create();
+
+	int num_devices = ohmd_ctx_probe(ctx);
+	if(num_devices < 0){
+		printf("failed to probe devices: %s\n", ohmd_ctx_get_error(ctx));
+		return -1;
+	}
+
+	printf("num devices: %d\n", num_devices);
+
+	for(int i = 0; i < num_devices; i++){
+		printf("vendor: %s\n", ohmd_list_gets(ctx, i, OHMD_VENDOR));
+		printf("product: %s\n", ohmd_list_gets(ctx, i, OHMD_PRODUCT));
+		printf("path: %s\n", ohmd_list_gets(ctx, i, OHMD_PATH));
+	}
+
+	ohmd_device* hmd = ohmd_list_open_device(ctx, 0);
+
+	if(!hmd){
+		printf("failed to open device: %s\n", ohmd_ctx_get_error(ctx));
+		return -1;
+	}
+
+	for(int i = 0; i < 10000; i++){
+		float q[4];
+
+		ohmd_ctx_update(ctx);
+
+		ohmd_device_getf(hmd, OHMD_ROTATION_QUAT, q);
+		printf("quat: % 4.4f, % 4.4f, % 4.4f, % 4.4f\n", q[0], q[1], q[2], q[3]);
+
+		ohmd_sleep(.01);
+	}
+
+	ohmd_ctx_destroy(ctx);
+	
+	return 0;
+}

tests/unittests/Makefile.am

@@ -0,0 +1,5 @@
+bin_PROGRAMS = unittests
+AM_CPPFLAGS = -Wall -Werror -I$(top_srcdir)/include -I$(top_srcdir)/src -DOHMD_STATIC
+unittests_SOURCES = main.c quat.c vec.c
+unittests_LDADD = $(top_srcdir)/src/libopenhmd.la -lm
+unittests_LDFLAGS = -static-libtool-libs

tests/unittests/main.c

@@ -0,0 +1,35 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Unit Tests - Main */
+
+#include "tests.h"
+
+bool float_eq(float a, float b, float t)
+{
+	return fabsf(a - b) < t;
+}
+
+#define Test(_t) printf("   "#_t); _t(); printf("           \tok\n");
+
+int main()
+{
+	printf("vec3f tests\n");
+	Test(test_ovec3f_normalize_me);
+	Test(test_ovec3f_get_length);
+	Test(test_ovec3f_get_angle);
+	Test(test_ovec3f_get_dot);
+	printf("\n");
+	
+	printf("quatf tests\n");
+	Test(test_oquatf_init_axis);
+	Test(test_oquatf_get_rotated);
+	printf("\n");
+
+	printf("all a-ok\n");
+	return 0;
+}

tests/unittests/quat.c

@@ -0,0 +1,102 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Unit Tests - Quaternion Tests */
+
+#include "tests.h"
+
+static const float t = 0.001;
+
+bool quatf_eq(quatf q1, quatf q2, float t)
+{
+	for(int i = 0; i < 4; i++)
+		if(!float_eq(q1.arr[i], q2.arr[i], t))
+			return false;
+
+	return true;
+}
+
+typedef struct {
+	vec3f v;
+	float f;
+	quatf q;
+} vec_float_quat;
+
+void test_oquatf_init_axis()
+{
+	//void oquatf_init_axis(quatf* me, const vec3f* vec, float angle);
+	
+	vec_float_quat list[] = {
+		{ {{0, 0, 0}}, 0, {{0, 0, 0, 1}}},
+		{ {{5, 12, 3}}, 1, {{0.1796723168488794, 0.43121356043731063, 0.10780339010932766, 0.8775825618903728}}},
+		{ {{-2, -3, 3}}, 1, {{-0.2044277365391809, -0.30664160480877134, 0.30664160480877134, 0.8775825618903728}} },
+		{ {{100, -3, 3}}, -300, {{0.7142339081165469, -0.021427017243496407, 0.021427017243496407, 0.6992508064783751}} },
+	};
+
+	int sz = sizeof(vec_float_quat);
+
+	for(int i = 0; i < sizeof(list) / sz; i++){
+		quatf q;
+		oquatf_init_axis(&q, &list[i].v, list[i].f);
+		//printf("%f %f %f %f\n", q.x, q.y, q.z, q.w);
+		TAssert(quatf_eq(q, list[i].q, t));
+	}
+}
+
+typedef struct {
+	quatf q;
+	vec3f v1, v2;
+} quat_vec2;
+
+void test_oquatf_get_rotated()
+{
+	// void oquatf_get_rotated(const quatf* me, const vec3f* vec, vec3f* out_vec);
+	
+	quat_vec2 list[] = {
+		{ {{0, 0, 0, 0}}, {{0, 0, 0}}, {{0, 0, 0}} }, 
+		{ {{0, 0, 0, 0}}, {{1, 2, 3}}, {{0, 0, 0}} }, 
+		{ {{0, 0, 0, 1}}, {{1, 2, 3}}, {{1, 2, 3}} }, 
+		{ {{.4, .2, .1, 1}}, {{2, 1, 0}}, {{2.18, 1.59, 0.2}} }, 
+		{ {{.4, .2, .1, -1}}, {{2, 1, 0}}, {{2.58, 0.79, 0.2}} }, 
+	};
+
+	int sz = sizeof(quat_vec2);
+
+	for(int i = 0; i < sizeof(list) / sz; i++){
+		vec3f vec;
+		oquatf_get_rotated(&list[i].q, &list[i].v1, &vec);
+		TAssert(vec3f_eq(vec, list[i].v2, t));
+	}
+}
+
+void test_oquatf_mult()
+{
+}
+
+void test_oquatf_normalize()
+{
+}
+
+void test_oquatf_get_length()
+{
+}
+
+void test_oquatf_get_mat4x4()
+{
+}
+
+void test_oquatf_get_mat3x3()
+{
+}
+
+void test_omat3x3_get_scales()
+{
+}
+
+void test_omat3x3_get_euler_angles()
+{
+}

tests/unittests/tests.h

@@ -0,0 +1,44 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Unit Tests - Internal Interface */
+
+#ifndef TESTS_H
+#define TESTS_H
+
+#include <stdio.h>
+#include <stdbool.h>
+#include <math.h>
+
+#include "openhmdi.h"
+
+#define TAssert(_v) if((!_v)){ printf("\ntest failed: %s @ %s:%d\n", __func__, __FILE__, __LINE__); exit(1); }
+
+bool float_eq(float a, float b, float t);
+bool vec3f_eq(vec3f v1, vec3f v2, float t);
+
+// vec3f tests
+void test_ovec3f_normalize_me();
+void test_ovec3f_get_length();
+void test_ovec3f_get_angle();
+void test_ovec3f_get_dot();
+
+// quatf tests
+void test_oquatf_init_axis();
+void test_oquatf_get_rotated();
+void test_oquatf_mult();
+void test_oquatf_mult_me();
+void test_oquatf_normalize();
+void test_oquatf_get_length();
+
+void test_oquatf_get_mat4x4();
+void test_oquatf_get_mat3x3();
+void test_omat3x3_get_scales();
+void test_omat3x3_get_euler_angles();
+
+
+#endif

tests/unittests/vec.c

@@ -0,0 +1,105 @@
+/*
+ * OpenHMD - Free and Open Source API and drivers for immersive technology.
+ * Copyright (C) 2013 Fredrik Hultin.
+ * Copyright (C) 2013 Jakob Bornecrantz.
+ * Distributed under the Boost 1.0 licence, see LICENSE for full text.
+ */
+
+/* Unit Tests - Vector3f Tests */
+
+#include "tests.h"
+
+bool vec3f_eq(vec3f v1, vec3f v2, float t)
+{
+	for(int i = 0; i < 3; i++)
+		if(!float_eq(v1.arr[i], v2.arr[i], t))
+			return false;
+
+	return true;
+}
+
+void test_ovec3f_normalize_me()
+{
+	vec3f v[][2] = {
+		{ {{1, 0, 0}}, {{1, 0, 0}} },
+		{ {{1, 2, 3}}, {{0.267261241912424, 0.534522483824849, 0.801783725737273}} },
+		{ {{-7, 13, 22}}, {{-0.264197974633739, 0.490653381462658, 0.830336491706037}} },
+		{ {{.1, .1, .1}}, {{0.577350269189626, 0.577350269189626, 0.577350269189626}} },
+		{ {{0, 0, 0}}, {{0, 0, 0}} },
+	};
+
+	int sz = sizeof(vec3f) * 2;
+	float t = 0.001;
+
+	for(int i = 0; i < sizeof(v) / sz; i++){
+		vec3f norm = v[i][0];
+		ovec3f_normalize_me(&norm);
+		TAssert(vec3f_eq(norm, v[i][1], t));
+	}
+}
+
+typedef struct {
+	vec3f vec;
+	float f;
+} vec_float;
+
+void test_ovec3f_get_length()
+{
+	vec_float vf[] = {
+		{ {{0, 0, 0}}, 0},
+		{ {{1, 0, 0}}, 1},
+		{ {{1, 2, 0}}, 2.23606797749979},
+		{ {{1, -2, 0}}, 2.23606797749979},
+		{ {{1, 2, 3}}, 3.7416573867739413},
+		{ {{-1, -2, -3}}, 3.7416573867739413},
+	};
+
+	int sz = sizeof(vec_float);
+	float t = 0.001;
+
+	for(int i = 0; i < sizeof(vf) / sz; i++){
+		TAssert(float_eq(ovec3f_get_length(&vf[i].vec), vf[i].f, t));
+	}
+}
+
+typedef struct {
+	vec3f v1, v2;
+	float f;
+} vec2_float;
+	
+void test_ovec3f_get_angle()
+{
+	vec2_float vf[] = {
+		{ {{0, 0, 0}}, {{0, 0, 0}}, 0},
+		{ {{1, 0, 0}}, {{0, 0, 0}}, 0},
+		{ {{2, 4, 3}}, {{1, 2, 3}}, 0.33940126397005316},
+		{ {{2, 4, 3}}, {{-1, 2, 3}}, 0.7311043352203973},
+	};
+
+	int sz = sizeof(vec2_float);
+	float t = 0.001;
+
+	for(int i = 0; i < sizeof(vf) / sz; i++){
+		TAssert(float_eq(ovec3f_get_angle(&vf[i].v1, &vf[i].v2), vf[i].f, t));
+	}
+}
+
+//float ovec3f_get_dot(const vec3f* me, const vec3f* vec);
+
+void test_ovec3f_get_dot()
+{
+	vec2_float vf[] = {
+		{ {{0, 0, 0}}, {{0, 0, 0}}, 0},
+		{ {{1, 2, 3}}, {{-.30, 2, 25}}, 78.7},
+		{ {{-1, -10000000, 3}}, {{-.30, 2, 25}}, -19999924.7},
+	};
+
+	int sz = sizeof(vec2_float);
+	float t = 0.001;
+
+	for(int i = 0; i < sizeof(vf) / sz; i++){
+		TAssert(float_eq(ovec3f_get_dot(&vf[i].v1, &vf[i].v2), vf[i].f, t));
+	}
+}
+
+