Merge pull request #33 from OpenHMD/set-orientation

Position and Orientation Correction/Reset

examples/opengl/main.c

@@ -133,6 +133,14 @@ int main(int argc, char** argv)
 				case SDLK_F1:
 					SDL_WM_ToggleFullScreen(gl.screen);
 					break;
+				case SDLK_F2:
+					{
+						// reset rotation and position
+						float zero[] = {0, 0, 0, 1};
+						ohmd_device_setf(hmd, OHMD_ROTATION_QUAT, zero);
+						ohmd_device_setf(hmd, OHMD_POSITION_VECTOR, zero);
+					}
+					break;
 				default:
 					break;
 				}

src/omath.c

@@ -98,6 +98,30 @@ 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);
 }
 
+float oquatf_get_dot(const quatf* me, const quatf* q)
+{
+	return me->x * q->x + me->y * q->y + me->z * q->z + me->w * q->w;
+}
+
+void oquatf_inverse(quatf* me)
+{
+	float dot = oquatf_get_dot(me, me);
+
+	// conjugate
+	for(int i = 0; i < 3; i++)
+		me->arr[i] = -me->arr[i];
+	
+	for(int i = 0; i < 4; i++)
+		me->arr[i] /= dot;
+}
+
+void oquatf_diff(const quatf* me, const quatf* q, quatf* out_q)
+{
+	quatf inv = *me;
+	oquatf_inverse(&inv);
+	oquatf_mult(&inv, q, out_q);
+}
+
 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;

src/omath.h

@@ -50,8 +50,11 @@ 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_diff(const quatf* me, const quatf* q, quatf* out_q);
 void oquatf_normalize_me(quatf* me);
 float oquatf_get_length(const quatf* me);
+float oquatf_get_dot(const quatf* me, const quatf* q);
+void oquatf_inverse(quatf* me);
 
 void oquatf_get_mat4x4(const quatf* me, const vec3f* point, float mat[4][4]);
 

src/openhmd.c

@@ -88,6 +88,8 @@ ohmd_device* OHMD_APIENTRY ohmd_list_open_device(ohmd_context* ctx, int index)
 		ohmd_driver* driver = (ohmd_driver*)desc->driver_ptr;
 		ohmd_device* device = driver->open_device(driver, desc);
 
+		device->rotation_correction.w = 1;
+
 		if (device == NULL)
 			return NULL;
 
@@ -126,6 +128,9 @@ int OHMD_APIENTRY ohmd_device_getf(ohmd_device* device, ohmd_float_value type, f
 			vec3f point = {{0, 0, 0}};
 			quatf rot;
 			device->getf(device, OHMD_ROTATION_QUAT, (float*)&rot);
+			quatf tmp = device->rotation_correction;
+			oquatf_mult_me(&tmp, &rot);
+			rot = tmp;
 			mat4x4f orient, world_shift, result;
 			omat4x4f_init_look_at(&orient, &rot, &point);
 			omat4x4f_init_translate(&world_shift, +(device->properties.ipd / 2.0f), 0, 0);
@@ -137,6 +142,7 @@ int OHMD_APIENTRY ohmd_device_getf(ohmd_device* device, ohmd_float_value type, f
 			vec3f point = {{0, 0, 0}};
 			quatf rot;
 			device->getf(device, OHMD_ROTATION_QUAT, (float*)&rot);
+			oquatf_mult_me(&rot, &device->rotation_correction);
 			mat4x4f orient, world_shift, result;
 			omat4x4f_init_look_at(&orient, &rot, &point);
 			omat4x4f_init_translate(&world_shift, -(device->properties.ipd / 2.0f), 0, 0);
@@ -184,6 +190,33 @@ int OHMD_APIENTRY ohmd_device_getf(ohmd_device* device, ohmd_float_value type, f
 	case OHMD_PROJECTION_ZNEAR:
 		*out = device->properties.znear;
 		return 0;
+
+	case OHMD_ROTATION_QUAT:
+	{
+		int ret = device->getf(device, OHMD_ROTATION_QUAT, out);
+
+		if(ret != 0)
+			return ret;
+
+		oquatf_mult_me((quatf*)out, &device->rotation_correction);
+		quatf tmp = device->rotation_correction;
+		oquatf_mult_me(&tmp, (quatf*)out);
+		*(quatf*)out = tmp;
+		return 0;
+	}
+	case OHMD_POSITION_VECTOR:
+	{
+		int ret = device->getf(device, OHMD_POSITION_VECTOR, out);
+
+		if(ret != 0)
+			return ret;
+
+		for(int i = 0; i < 3; i++)
+			out[i] += device->position_correction.arr[i];
+
+		return 0;
+	}
+		
 	default:
 		return device->getf(device, type, out);
 	}
@@ -201,6 +234,34 @@ int OHMD_APIENTRY ohmd_device_setf(ohmd_device* device, ohmd_float_value type, f
 	case OHMD_PROJECTION_ZNEAR:
 		device->properties.znear = *in;
 		return 0;
+	case OHMD_ROTATION_QUAT:
+		{
+			// adjust rotation correction
+			quatf q;
+			int ret = device->getf(device, OHMD_ROTATION_QUAT, (float*)&q);
+
+			if(ret != 0){
+				return ret;
+			}
+
+			oquatf_diff(&q, (quatf*)in, &device->rotation_correction);
+			return 0;
+		}
+	case OHMD_POSITION_VECTOR:
+		{
+			// adjust position correction
+			vec3f v;
+			int ret = device->getf(device, OHMD_POSITION_VECTOR, (float*)&v);
+
+			if(ret != 0){
+				return ret;
+			}
+
+			for(int i = 0; i < 3; i++)
+				device->position_correction.arr[i] = in[i] - v.arr[i];
+
+			return 0;
+		}
 	default:
 		return -1;
 	}

src/openhmdi.h

@@ -72,6 +72,9 @@ typedef struct {
 struct ohmd_device {
 	ohmd_device_properties properties;
 
+	quatf rotation_correction;
+	vec3f position_correction;
+
 	int (*getf)(ohmd_device* device, ohmd_float_value type, float* out);
 	void (*update)(ohmd_device* device);
 	void (*close)(ohmd_device* device);

tests/unittests/main.c

@@ -7,6 +7,7 @@
 
 /* Unit Tests - Main */
 
+#include <string.h>
 #include "tests.h"
 
 bool float_eq(float a, float b, float t)
@@ -14,7 +15,7 @@ bool float_eq(float a, float b, float t)
 	return fabsf(a - b) < t;
 }
 
-#define Test(_t) printf("   "#_t); _t(); printf("           \tok\n");
+#define Test(_t) printf("   "#_t); _t(); printf("%*sok\n", 50 - (int)strlen(#_t), "");
 
 int main()
 {
@@ -28,6 +29,9 @@ int main()
 	printf("quatf tests\n");
 	Test(test_oquatf_init_axis);
 	Test(test_oquatf_get_rotated);
+	Test(test_oquatf_get_dot);
+	Test(test_oquatf_inverse);
+	Test(test_oquatf_diff);
 	printf("\n");
 
 	printf("high level tests\n");

tests/unittests/quat.c

@@ -94,3 +94,61 @@ void test_oquatf_get_length()
 void test_oquatf_get_mat4x4()
 {
 }
+
+typedef struct {
+	quatf q1, q2;
+	float f;
+} quat2_float;
+
+void test_oquatf_get_dot()
+{
+	// TODO add more test cases
+	quat2_float list[] = {
+		{ {{1, 2, 3, 1}}, {{4, 3, 2, .5}}, 16.5 },
+	};
+
+	int sz = sizeof(quat2_float);
+
+	for(int i = 0; i < sizeof(list) / sz; i++){
+		TAssert(float_eq(oquatf_get_dot(&list[i].q1, &list[i].q2), list[i].f, t));
+	}
+}
+
+typedef struct {
+	quatf q1, q2;
+} quat2;
+
+void test_oquatf_inverse()
+{
+	// TODO add more test cases
+	quat2 list[] = {
+		{ {{1, 2, 3, 1}}, {{-0.06666666666666667, -0.13333333333333333, -0.2, 0.06666666666666667}} },
+	};
+
+	int sz = sizeof(quat2);
+
+	for(int i = 0; i < sizeof(list) / sz; i++){
+		oquatf_inverse(&list[i].q1);
+		TAssert(quatf_eq(list[i].q1, list[i].q2, t));
+	}
+}
+
+typedef struct {
+	quatf q1, q2, q3;
+} quat3;
+
+void test_oquatf_diff()
+{
+	// TODO add more test cases
+	quat3 list[] = {
+		{ {{1, 2, 3, 1}}, {{5, 3, 2, .1}}, {{0.660000, -0.680000, 0.580000, 1.140000}} },
+	};
+
+	int sz = sizeof(quat3);
+
+	for(int i = 0; i < sizeof(list) / sz; i++){
+		quatf q;
+		oquatf_diff(&list[i].q1, &list[i].q2, &q);
+		TAssert(quatf_eq(q, list[i].q3, t));
+	}
+}

tests/unittests/tests.h

@@ -34,6 +34,9 @@ void test_oquatf_mult();
 void test_oquatf_mult_me();
 void test_oquatf_normalize();
 void test_oquatf_get_length();
+void test_oquatf_get_dot();
+void test_oquatf_inverse();
+void test_oquatf_diff();
 
 void test_oquatf_get_mat4x4();