construct a 4x4 rotation matrix using a quaternion

1 files changed, 33 insertions, 0 deletions

diff --git a/maths.cpp b/maths.cpp
index 20581ca..90d0e03 100644
--- a/maths.cpp
+++ b/maths.cpp
@@ -190,6 +190,39 @@ m4f m4f::rotate(m4f m, float a, v3f v) {
 	return m * r;
 }
 
+m4f m4f::rotate(m4f r, const v4f& q) {
+	float qx, qy, qz, qw, qx2, qy2, qz2, qxqx2, qyqy2, qzqz2, qxqy2, qyqz2, qzqw2, qxqz2, qyqw2, qxqw2;
+	qx = -q.x;
+	qy = -q.y;
+	qz = -q.z;
+	qw = q.w;
+	qx2 = (qx + qx);
+	qy2 = (qy + qy);
+	qz2 = (qz + qz);
+	qxqx2 = (qx * qx2);
+	qxqy2 = (qx * qy2);
+	qxqz2 = (qx * qz2);
+	qxqw2 = (qw * qx2);
+	qyqy2 = (qy * qy2);
+	qyqz2 = (qy * qz2);
+	qyqw2 = (qw * qy2);
+	qzqz2 = (qz * qz2);
+	qzqw2 = (qw * qz2);
+	r.m[0][0] = ((1.0f - qyqy2) - qzqz2);
+	r.m[0][1] = qxqy2 - qzqw2;
+	r.m[0][2] = qxqz2 + qyqw2;
+	r.m[0][3] = 0.0f;
+	r.m[1][0] = qxqy2 + qzqw2;
+	r.m[1][1] = (1.0f - qxqx2) - qzqz2;
+	r.m[1][2] = qyqz2 - qxqw2;
+	r.m[1][3] = 0.0f;
+	r.m[2][0] = qxqz2 - qyqw2;
+	r.m[2][1] = qyqz2 + qxqw2;
+	r.m[2][2] = (1.0f - qxqx2) - qyqy2;
+	r.m[2][3] = 0.0f;
+	return r;
+}
+
 m4f m4f::scale(m4f m, v3f v) {
 	m4f r(1.0f);