From e1d6dbebce7b8674e62993f6234e1e0c74a620bf Mon Sep 17 00:00:00 2001
From: Sam Hocevar <sam@hocevar.net>
Date: Fri, 18 Feb 2011 00:09:18 +0000
Subject: [PATCH] Implement float4x4::rotate().

---
 src/matrix.cpp | 32 ++++++++++++++++++++++++++++++++
 src/matrix.h   |  1 +
 2 files changed, 33 insertions(+)

diff --git a/src/matrix.cpp b/src/matrix.cpp
index 7cad978d..c7b5b92d 100644
--- a/src/matrix.cpp
+++ b/src/matrix.cpp
@@ -116,3 +116,35 @@ template<> float4x4 float4x4::translate(float x, float y, float z)
     return ret;
 }
 
+template<> float4x4 float4x4::rotate(float theta, float x, float y, float z)
+{
+    float st = sinf(theta);
+    float ct = cosf(theta);
+
+    float len = sqrtf(x * x + y * y + z * z);
+    float invlen = len ? 1.0f / len : 0.0f;
+    x *= invlen;
+    y *= invlen;
+    z *= invlen;
+
+    float mtx = (1.0f - ct) * x;
+    float mty = (1.0f - ct) * y;
+    float mtz = (1.0f - ct) * z;
+
+    float4x4 ret(1.0f);
+
+    ret[0][0] = x * mtx + ct;
+    ret[0][1] = x * mty + st * z;
+    ret[0][2] = x * mtz - st * y;
+
+    ret[1][0] = y * mtx - st * z;
+    ret[1][1] = y * mty + ct;
+    ret[1][2] = y * mtz + st * x;
+
+    ret[2][0] = z * mtx + st * y;
+    ret[2][1] = z * mty - st * x;
+    ret[2][2] = z * mtz + ct;
+
+    return ret;
+}
+
diff --git a/src/matrix.h b/src/matrix.h
index 0f3b31ba..aec3e077 100644
--- a/src/matrix.h
+++ b/src/matrix.h
@@ -199,6 +199,7 @@ template <typename T> struct Mat4
     static Mat4<T> frustum(T left, T right, T bottom, T top, T near, T far);
     static Mat4<T> perspective(T theta, T width, T height, T near, T far);
     static Mat4<T> translate(T x, T y, T z);
+    static Mat4<T> rotate(T theta, T x, T y, T z);
 
     inline Mat4<T> operator +(Mat4<T> const val) const
     {