Outsourced matrix math code into separate header.

2016-12-20 20:30:42 +01:00
parent 3000e129e3
commit a93bdeb063
2 changed files with 107 additions and 24 deletions
@@ -0,0 +1,97 @@
+#pragma once
+
+#include <openvr.h>
+#include <cmath>
+
+
+namespace utils {
+
+inline vr::HmdMatrix34_t& initRotationMatrix(vr::HmdMatrix34_t& matrix, unsigned axisId, float angle) {
+	switch (axisId) {
+		case 0: // x-axis
+			matrix.m[0][0] = 1.0f;
+			matrix.m[0][1] = 0.0f;
+			matrix.m[0][2] = 0.0f;
+			matrix.m[0][3] = 0.0f;
+			matrix.m[1][0] = 0.0f;
+			matrix.m[1][1] = std::cos(angle);
+			matrix.m[1][2] = -std::sin(angle);
+			matrix.m[1][3] = 0.0f;
+			matrix.m[2][0] = 0.0f;
+			matrix.m[2][1] = std::sin(angle);
+			matrix.m[2][2] = std::cos(angle);
+			matrix.m[2][3] = 0.0f;
+			break;
+		case 1: // y-axis
+			matrix.m[0][0] = std::cos(angle);
+			matrix.m[0][1] = 0.0f;
+			matrix.m[0][2] = std::sin(angle);
+			matrix.m[0][3] = 0.0f;
+			matrix.m[1][0] = 0.0f;
+			matrix.m[1][1] = 1.0f;
+			matrix.m[1][2] = 0.0f;
+			matrix.m[1][3] = 0.0f;
+			matrix.m[2][0] = -std::sin(angle);
+			matrix.m[2][1] = 0.0f;
+			matrix.m[2][2] = std::cos(angle);
+			matrix.m[2][3] = 0.0f;
+			break;
+		case 2: // z-axis
+			matrix.m[0][0] = std::cos(angle);
+			matrix.m[0][1] = -std::sin(angle);
+			matrix.m[0][2] = 0.0f;
+			matrix.m[0][3] = 0.0f;
+			matrix.m[1][0] = std::sin(angle);
+			matrix.m[1][1] = std::cos(angle);
+			matrix.m[1][2] = 0.0f;
+			matrix.m[1][3] = 0.0f;
+			matrix.m[2][0] = 0.0f;
+			matrix.m[2][1] = 0.0f;
+			matrix.m[2][2] = 1.0f;
+			matrix.m[2][3] = 0.0f;
+			break;
+		default:
+			break;
+	}
+	return matrix;
+}
+
+
+inline vr::HmdMatrix34_t& matMul33(vr::HmdMatrix34_t& result, const vr::HmdMatrix34_t& a, const vr::HmdMatrix34_t& b) {
+	for (unsigned i = 0; i < 3; i++) {
+		for (unsigned j = 0; j < 3; j++) {
+			result.m[i][j] = 0.0f;
+			for (unsigned k = 0; k < 3; k++) {
+				result.m[i][j] += a.m[i][k] * b.m[k][j];
+			}
+		}
+	}
+	return result;
+}
+
+
+inline vr::HmdVector3_t& matMul33(vr::HmdVector3_t& result, const vr::HmdMatrix34_t& a, const vr::HmdVector3_t& b) {
+	for (unsigned i = 0; i < 3; i++) {
+		result.v[i] = 0.0f;
+		for (unsigned k = 0; k < 3; k++) {
+			result.v[i] += a.m[i][k] * b.v[k];
+		};
+	}
+	return result;
+}
+
+
+inline vr::HmdVector3_t& matMul33(vr::HmdVector3_t& result, const vr::HmdVector3_t& a, const vr::HmdMatrix34_t& b) {
+	for (unsigned i = 0; i < 3; i++) {
+		result.v[i] = 0.0f;
+		for (unsigned k = 0; k < 3; k++) {
+			result.v[i] +=  a.v[k] * b.m[k][i];
+		};
+	}
+	return result;
+}
+
+
+
+} // end namespace utils
+