diff options
| -rw-r--r-- | maths.cpp | 72 | ||||
| -rw-r--r-- | maths.hpp | 12 |
2 files changed, 84 insertions, 0 deletions
@@ -2,6 +2,78 @@ #include <algorithm> +namespace quat { + v4f identity() { + return v4f(0.0f, 0.0f, 0.0f, 1.0f); + } + + v4f scale(const v4f& q, float s) { + return v4f(q.xyz() * s, q.w); + } + + v4f normalised(const v4f& q) { + float l = v4f::mag(q); + return v4f(q.xyz() / l, q.w); + } + + v4f conjugate(const v4f& q) { + return v4f(-q.xyz(), q.w); + } + + template <> + v4f mul<false>(const v4f& a, const v4f& b) { + return v4f( + a.x * b.w + + a.y * b.z - + a.z * b.y + + a.w * b.x, + + -a.x * b.z + + a.y * b.w + + a.z * b.x + + a.w * b.y, + + a.x * b.y - + a.y * b.x + + a.z * b.w + + a.w * b.z, + + -a.x * b.x - + a.y * b.y - + a.z * b.z + + a.w * b.w + ); + } + + template <> + v4f mul<true>(const v4f& a, const v4f& b) { + return normalised(mul<false>(a, b)); + } + + v4f mul(const v4f& a, const v4f& b) { + return mul<true>(a, b); + } + + v4f rotate(float angle, const v3f& axis) { + float h = to_rad(angle) * 0.5f; + float s = sinf(h); + return v4f( + cosf(h), + s * axis.x, + s * axis.y, + s * axis.z + ); + } + + v4f euler(const v3f& a) { + return mul(mul( + rotate(a.x, v3f(1.0f, 0.0f, 0.0f)), + rotate(a.y, v3f(0.0f, 1.0f, 0.0f))), + rotate(a.z, v3f(0.0f, 0.0f, 1.0f)) + ); + } +} + m4f::m4f() {} m4f::m4f(float d) { @@ -469,6 +469,18 @@ struct AABB { v3f min, max; }; +namespace quat { + v4f identity(); + v4f scale(const v4f& q, float s); + v4f normalised(const v4f& q); + v4f conjugate(const v4f& q); + template <bool normalise> + v4f mul(const v4f& a, const v4f& b); + v4f mul(const v4f& a, const v4f& b); + v4f rotate(float angle, const v3f& axis); + v4f euler(const v3f& a); +} + struct m4f { float m[4][4]; |