foo

[smouse] / src / vmath.inl

diff --git a/src/vmath.inl b/src/vmath.inl
new file mode 100644 (file)
index 0000000..49b5a2f
--- /dev/null
+++ b/src/vmath.inl
@@ -0,0 +1,68 @@
+/* vector functions */
+static inline vec3_t v3_cons(float x, float y, float z)
+{
+       vec3_t res;
+       res.x = x;
+       res.y = y;
+       res.z = z;
+       return res;
+}
+
+static inline vec3_t quat_vec(quat_t q)
+{
+       vec3_t v;
+       v.x = q.x;
+       v.y = q.y;
+       v.z = q.z;
+       return v;
+}
+
+static inline float v3_dot(vec3_t v1, vec3_t v2)
+{
+       return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
+}
+
+/* quaternion functions */
+static inline quat_t quat_cons(float s, float x, float y, float z)
+{
+       quat_t q;
+       q.x = x;
+       q.y = y;
+       q.z = z;
+       q.w = s;
+       return q;
+}
+
+static inline quat_t quat_mul(quat_t q1, quat_t q2)
+{
+       quat_t res;
+       vec3_t v1 = quat_vec(q1);
+       vec3_t v2 = quat_vec(q2);
+
+       res.w = q1.w * q2.w - v3_dot(v1, v2);
+       res.x = v2.x * q1.w + v1.x * q2.w + (v1.y * v2.z - v1.z * v2.y);
+       res.y = v2.y * q1.w + v1.y * q2.w + (v1.z * v2.x - v1.x * v2.z);
+       res.z = v2.z * q1.w + v1.z * q2.w + (v1.x * v2.y - v1.y * v2.x);
+       return res;
+}
+
+static inline void quat_to_mat(mat4_t res, quat_t q)
+{
+       m4_cons(res,    1.0 - 2.0 * q.y*q.y - 2.0 * q.z*q.z,    2.0 * q.x * q.y + 2.0 * q.w * q.z,              2.0 * q.z * q.x - 2.0 * q.w * q.y, 0,
+                                       2.0 * q.x * q.y - 2.0 * q.w * q.z,              1.0 - 2.0 * q.x*q.x - 2.0 * q.z*q.z,    2.0 * q.y * q.z + 2.0 * q.w * q.x, 0,
+                                       2.0 * q.z * q.x + 2.0 * q.w * q.y,              2.0 * q.y * q.z - 2.0 * q.w * q.x,              1.0 - 2.0 * q.x*q.x - 2.0 * q.y*q.y, 0,
+                                       0, 0, 0, 1);
+}
+
+/* matrix functions */
+static inline void m4_cons(mat4_t m,
+               float m11, float m12, float m13, float m14,
+               float m21, float m22, float m23, float m24,
+               float m31, float m32, float m33, float m34,
+               float m41, float m42, float m43, float m44)
+{
+       m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
+       m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
+       m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
+       m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
+}