Split Math.c into smaller files

Friend said that Math.c was too big and complicated.

18 files changed, 2345 insertions, 2271 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5e0a380..d6037e5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -70,7 +70,22 @@ set(BH_SOURCE
     src/Args.c
     src/Hashmap.c
     src/IO.c
-    src/Math.c
+    src/Math/Box2f.c
+    src/Math/Box3f.c
+    src/Math/Line.c
+    src/Math/Mat3f.c
+    src/Math/Mat4f.c
+    src/Math/Misc.c
+    src/Math/Plane.c
+    src/Math/Quat4f.c
+    src/Math/Ray2f.c
+    src/Math/Ray3f.c
+    src/Math/Vec2f.c
+    src/Math/Vec2i.c
+    src/Math/Vec3f.c
+    src/Math/Vec3i.c
+    src/Math/Vec4f.c
+    src/Math/Vec4i.c
     src/Queue.c
 )
 
diff --git a/src/Math.c b/src/Math.c
deleted file mode 100644
index 82f4ec3..0000000
--- a/src/Math.c
+++ /dev/null
@@ -1,2270 +0,0 @@
-#include <BH/Math.h>
-
-#include <math.h>
-#include <string.h>
-
-
-/*#define EPSILON 0.00001f*/
-#define EPSILON 0.00001f
-#define PI      3.14159265358979323846f
-
-
-void BH_Vec4fAdd(const float *a,
-                 const float *b,
-                 float *out)
-{
-    out[0] = a[0] + b[0];
-    out[1] = a[1] + b[1];
-    out[2] = a[2] + b[2];
-    out[3] = a[3] + b[3];
-}
-
-
-void BH_Vec4fSub(const float *a,
-                 const float *b,
-                 float *out)
-{
-    out[0] = a[0] - b[0];
-    out[1] = a[1] - b[1];
-    out[2] = a[2] - b[2];
-    out[3] = a[3] - b[3];
-}
-
-
-void BH_Vec4fMul(const float *a,
-                 const float *b,
-                 float *out)
-{
-    out[0] = a[0] * b[0];
-    out[1] = a[1] * b[1];
-    out[2] = a[2] * b[2];
-    out[3] = a[3] * b[3];
-}
-
-
-void BH_Vec4fScale(const float *a,
-                   const float b,
-                   float *out)
-{
-    out[0] = a[0] * b;
-    out[1] = a[1] * b;
-    out[2] = a[2] * b;
-    out[3] = a[3] * b;
-}
-
-
-void BH_Vec4fMulAdd(const float *a,
-                    const float *b,
-                    const float *c,
-                    float *out)
-{
-    out[0] = a[0] * b[0] + c[0];
-    out[1] = a[1] * b[1] + c[1];
-    out[2] = a[2] * b[2] + c[2];
-    out[3] = a[3] * b[3] + c[3];
-}
-
-
-void BH_Vec4fNegate(const float *in,
-                    float *out)
-{
-    out[0] = -in[0];
-    out[1] = -in[1];
-    out[2] = -in[2];
-    out[3] = -in[3];
-}
-
-
-float BH_Vec4fDot(const float *a,
-                  const float *b)
-{
-    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
-}
-
-
-float BH_Vec4fLength(const float *in)
-{
-    return sqrtf(BH_Vec4fDot(in, in));
-}
-
-
-void BH_Vec4fNormal(const float *in,
-                    float *out)
-{
-    BH_Vec4fScale(in, 1.0f / BH_Vec4fLength(in), out);
-}
-
-
-float BH_Vec4fNormalEx(const float *in,
-                       float *out)
-{
-    float length;
-
-    length = BH_Vec4fLength(in);
-    BH_Vec4fScale(in, 1.0f / length, out);
-    return length;
-}
-
-
-void BH_Vec4fMin(const float *a,
-                 const float *b,
-                 float *out)
-{
-    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
-    if (a[2] < b[2]) out[2] = a[2]; else out[2] = b[2];
-    if (a[3] < b[3]) out[3] = a[3]; else out[3] = b[3];
-}
-
-
-void BH_Vec4fMax(const float *a,
-                 const float *b,
-                 float *out)
-{
-    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
-    if (a[2] > b[2]) out[2] = a[2]; else out[2] = b[2];
-    if (a[3] > b[3]) out[3] = a[3]; else out[3] = b[3];
-}
-
-
-void BH_Vec4fLerp(const float *a,
-                  const float *b,
-                  float t,
-                  float *out)
-{
-    float tmp[4];
-
-    BH_Vec4fSub(b, a, tmp);
-    BH_Vec4fScale(tmp, t, tmp);
-    BH_Vec4fAdd(a, tmp, out);
-}
-
-
-void BH_Vec4fProject(const float *a,
-                     const float *b,
-                     float *out)
-{
-    float amount;
-
-    amount = BH_Vec4fDot(a, b) / BH_Vec4fDot(b, b);
-    BH_Vec4fScale(b, amount, out);
-}
-
-
-void BH_Vec4fBarycentric(const float *a,
-                         const float *b,
-                         const float *c,
-                         float v,
-                         float w,
-                         float *out)
-{
-    float tmp1[4], tmp2[4];
-    float u;
-
-    u = 1.0f - v - w;
-    tmp1[0] = tmp1[1] = tmp1[2] = tmp1[3] = u; BH_Vec4fMul(a, tmp1, tmp2);
-    tmp1[0] = tmp1[1] = tmp1[2] = tmp1[3] = v; BH_Vec4fMulAdd(b, tmp1, tmp2, tmp2);
-    tmp1[0] = tmp1[1] = tmp1[2] = tmp1[3] = w; BH_Vec4fMulAdd(c, tmp1, tmp2, out);
-}
-
-
-void BH_Vec3fAdd(const float *a,
-                 const float *b,
-                 float *out)
-{
-    out[0] = a[0] + b[0];
-    out[1] = a[1] + b[1];
-    out[2] = a[2] + b[2];
-}
-
-
-void BH_Vec3fSub(const float *a,
-                 const float *b,
-                 float *out)
-{
-    out[0] = a[0] - b[0];
-    out[1] = a[1] - b[1];
-    out[2] = a[2] - b[2];
-}
-
-
-void BH_Vec3fMul(const float *a,
-                 const float *b,
-                 float *out)
-{
-    out[0] = a[0] * b[0];
-    out[1] = a[1] * b[1];
-    out[2] = a[2] * b[2];
-}
-
-
-void BH_Vec3fScale(const float *a,
-                   const float b,
-                   float *out)
-{
-    out[0] = a[0] * b;
-    out[1] = a[1] * b;
-    out[2] = a[2] * b;
-}
-
-
-void BH_Vec3fMulAdd(const float *a,
-                    const float *b,
-                    const float *c,
-                    float *out)
-{
-    out[0] = a[0] * b[0] + c[0];
-    out[1] = a[1] * b[1] + c[1];
-    out[2] = a[2] * b[2] + c[2];
-}
-
-
-void BH_Vec3fNegate(const float *in,
-                    float *out)
-{
-    out[0] = -in[0];
-    out[1] = -in[1];
-    out[2] = -in[2];
-}
-
-
-float BH_Vec3fDot(const float *a,
-                  const float *b)
-{
-    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
-}
-
-
-void BH_Vec3fCross(const float *a,
-                   const float *b,
-                   float *out)
-{
-    float tmp[3];
-
-    tmp[0] = a[1] * b[2] - a[2] * b[1];
-    tmp[1] = a[2] * b[0] - a[0] * b[2];
-    tmp[2] = a[0] * b[1] - a[1] * b[0];
-    memcpy(out, tmp, sizeof(tmp));
-}
-
-
-float BH_Vec3fLength(const float *in)
-{
-    return sqrtf(BH_Vec3fDot(in, in));
-}
-
-
-void BH_Vec3fNormal(const float *in,
-                    float *out)
-{
-    BH_Vec3fScale(in, 1.0f / BH_Vec3fLength(in), out);
-}
-
-
-float BH_Vec3fNormalEx(const float *in,
-                       float *out)
-{
-    float length;
-
-    length = BH_Vec3fLength(in);
-    BH_Vec3fScale(in, 1.0f / length, out);
-    return length;
-}
-
-
-void BH_Vec3fMin(const float *a,
-                 const float *b,
-                 float *out)
-{
-    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
-    if (a[2] < b[2]) out[2] = a[2]; else out[2] = b[2];
-}
-
-
-void BH_Vec3fMax(const float *a,
-                 const float *b,
-                 float *out)
-{
-    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
-    if (a[2] > b[2]) out[2] = a[2]; else out[2] = b[2];
-}
-
-
-void BH_Vec3fLerp(const float *a,
-                  const float *b,
-                  float t,
-                  float *out)
-{
-    float tmp[3];
-
-    BH_Vec3fSub(b, a, tmp);
-    BH_Vec3fScale(tmp, t, tmp);
-    BH_Vec3fAdd(a, tmp, out);
-}
-
-
-void BH_Vec3fProject(const float *a,
-                     const float *b,
-                     float *out)
-{
-    float amount;
-
-    amount = BH_Vec3fDot(a, b) / BH_Vec3fDot(b, b);
-    BH_Vec3fScale(b, amount, out);
-}
-
-
-void BH_Vec3fBarycentric(const float *a,
-                         const float *b,
-                         const float *c,
-                         float v,
-                         float w,
-                         float *out)
-{
-    float tmp1[3], tmp2[3];
-    float u;
-
-    u = 1.0f - v - w;
-    tmp1[0] = tmp1[1] = tmp1[2] = u; BH_Vec3fMul(a, tmp1, tmp2);
-    tmp1[0] = tmp1[1] = tmp1[2] = v; BH_Vec3fMulAdd(b, tmp1, tmp2, tmp2);
-    tmp1[0] = tmp1[1] = tmp1[2] = w; BH_Vec3fMulAdd(c, tmp1, tmp2, out);
-}
-
-
-void BH_Vec2fAdd(const float *a,
-                 const float *b,
-                 float *out)
-{
-    out[0] = a[0] + b[0];
-    out[1] = a[1] + b[1];
-}
-
-
-void BH_Vec2fSub(const float *a,
-                 const float *b,
-                 float *out)
-{
-    out[0] = a[0] - b[0];
-    out[1] = a[1] - b[1];
-}
-
-
-void BH_Vec2fMul(const float *a,
-                 const float *b,
-                 float *out)
-{
-    out[0] = a[0] * b[0];
-    out[1] = a[1] * b[1];
-}
-
-
-void BH_Vec2fScale(const float *a,
-                   const float b,
-                   float *out)
-{
-    out[0] = a[0] * b;
-    out[1] = a[1] * b;
-}
-
-
-void BH_Vec2fMulAdd(const float *a,
-                    const float *b,
-                    const float *c,
-                    float *out)
-{
-    out[0] = a[0] * b[0] + c[0];
-    out[1] = a[1] * b[1] + c[1];
-}
-
-
-void BH_Vec2fNegate(const float *in,
-                    float *out)
-{
-    out[0] = -in[0];
-    out[1] = -in[1];
-}
-
-
-float BH_Vec2fDot(const float *a,
-                  const float *b)
-{
-    return a[0] * b[0] + a[1] * b[1];
-}
-
-
-float BH_Vec2fCross(const float *a,
-                    const float *b)
-{
-    return a[0] * b[1] - a[1] * b[0];
-}
-
-
-float BH_Vec2fLength(const float *in)
-{
-    return sqrtf(BH_Vec2fDot(in, in));
-}
-
-
-void BH_Vec2fNormal(const float *in,
-                    float *out)
-{
-    BH_Vec2fScale(in, 1.0f / BH_Vec2fLength(in), out);
-}
-
-
-float BH_Vec2fNormalEx(const float *in,
-                       float *out)
-{
-    float length;
-
-    length = BH_Vec2fLength(in);
-    BH_Vec2fScale(in, 1.0f / length, out);
-    return length;
-}
-
-
-void BH_Vec2fMin(const float *a,
-                 const float *b,
-                 float *out)
-{
-    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
-}
-
-
-void BH_Vec2fMax(const float *a,
-                 const float *b,
-                 float *out)
-{
-    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
-}
-
-
-void BH_Vec2fLerp(const float *a,
-                  const float *b,
-                  float t,
-                  float *out)
-{
-    float tmp[2];
-
-    BH_Vec2fSub(b, a, tmp);
-    BH_Vec2fScale(tmp, t, tmp);
-    BH_Vec2fAdd(a, tmp, out);
-}
-
-
-void BH_Vec2fProject(const float *a,
-                     const float *b,
-                     float *out)
-{
-    float amount;
-
-    amount = BH_Vec2fDot(a, b) / BH_Vec2fDot(b, b);
-    BH_Vec2fScale(b, amount, out);
-}
-
-
-void BH_Vec2fBarycentric(const float *a,
-                         const float *b,
-                         const float *c,
-                         float v,
-                         float w,
-                         float *out)
-{
-    float tmp1[2], tmp2[2];
-    float u;
-
-    u = 1.0f - v - w;
-    tmp1[0] = tmp1[1] = u; BH_Vec2fMul(a, tmp1, tmp2);
-    tmp1[0] = tmp1[1] = v; BH_Vec2fMulAdd(b, tmp1, tmp2, tmp2);
-    tmp1[0] = tmp1[1] = w; BH_Vec2fMulAdd(c, tmp1, tmp2, out);
-}
-
-
-float BH_Lerpf(float a, float b, float t)
-{
-    return a + (b - a) * t;
-}
-
-
-void BH_Vec4iAdd(const int *a,
-                 const int *b,
-                 int *out)
-{
-    out[0] = a[0] + b[0];
-    out[1] = a[1] + b[1];
-    out[2] = a[2] + b[2];
-    out[3] = a[3] + b[3];
-}
-
-
-void BH_Vec4iSub(const int *a,
-                 const int *b,
-                 int *out)
-{
-    out[0] = a[0] - b[0];
-    out[1] = a[1] - b[1];
-    out[2] = a[2] - b[2];
-    out[3] = a[3] - b[3];
-}
-
-
-void BH_Vec4iMul(const int *a,
-                 const int *b,
-                 int *out)
-{
-    out[0] = a[0] * b[0];
-    out[1] = a[1] * b[1];
-    out[2] = a[2] * b[2];
-    out[3] = a[3] * b[3];
-}
-
-
-void BH_Vec4iScale(const int *a,
-                   int b,
-                   int *out)
-{
-    out[0] = a[0] * b;
-    out[1] = a[1] * b;
-    out[2] = a[2] * b;
-    out[3] = a[3] * b;
-}
-
-
-void BH_Vec4iMulAdd(const int *a,
-                    const int *b,
-                    const int *c,
-                    int *out)
-{
-    out[0] = a[0] * b[0] + c[0];
-    out[1] = a[1] * b[1] + c[1];
-    out[2] = a[2] * b[2] + c[2];
-    out[3] = a[3] * b[3] + c[3];
-}
-
-
-void BH_Vec4iNegate(const int *in,
-                    int *out)
-{
-    out[0] = -in[0];
-    out[1] = -in[1];
-    out[2] = -in[2];
-    out[3] = -in[3];
-}
-
-
-void BH_Vec4iMin(const int *a,
-                 const int *b,
-                 int *out)
-{
-    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
-    if (a[2] < b[2]) out[2] = a[2]; else out[2] = b[2];
-    if (a[3] < b[3]) out[3] = a[3]; else out[3] = b[3];
-}
-
-
-void BH_Vec4iMax(const int *a,
-                 const int *b,
-                 int *out)
-{
-    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
-    if (a[2] > b[2]) out[2] = a[2]; else out[2] = b[2];
-    if (a[3] > b[3]) out[3] = a[3]; else out[3] = b[3];
-}
-
-
-void BH_Vec3iAdd(const int *a,
-                 const int *b,
-                 int *out)
-{
-    out[0] = a[0] + b[0];
-    out[1] = a[1] + b[1];
-    out[2] = a[2] + b[2];
-}
-
-
-void BH_Vec3iSub(const int *a,
-                 const int *b,
-                 int *out)
-{
-    out[0] = a[0] - b[0];
-    out[1] = a[1] - b[1];
-    out[2] = a[2] - b[2];
-}
-
-
-void BH_Vec3iMul(const int *a,
-                 const int *b,
-                 int *out)
-{
-    out[0] = a[0] * b[0];
-    out[1] = a[1] * b[1];
-    out[2] = a[2] * b[2];
-}
-
-
-void BH_Vec3iScale(const int *a,
-                   int b,
-                   int *out)
-{
-    out[0] = a[0] * b;
-    out[1] = a[1] * b;
-    out[2] = a[2] * b;
-}
-
-
-void BH_Vec3iMulAdd(const int *a,
-                    const int *b,
-                    const int *c,
-                    int *out)
-{
-    out[0] = a[0] * b[0] + c[0];
-    out[1] = a[1] * b[1] + c[1];
-    out[2] = a[2] * b[2] + c[2];
-}
-
-
-void BH_Vec3iNegate(const int *in,
-                    int *out)
-{
-    out[0] = -in[0];
-    out[1] = -in[1];
-    out[2] = -in[2];
-}
-
-
-void BH_Vec3iMin(const int *a,
-                 const int *b,
-                 int *out)
-{
-    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
-    if (a[2] < b[2]) out[2] = a[2]; else out[2] = b[2];
-}
-
-
-void BH_Vec3iMax(const int *a,
-                 const int *b,
-                 int *out)
-{
-    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
-    if (a[2] > b[2]) out[2] = a[2]; else out[2] = b[2];
-}
-
-
-void BH_Vec2iAdd(const int *a,
-                 const int *b,
-                 int *out)
-{
-    out[0] = a[0] + b[0];
-    out[1] = a[1] + b[1];
-}
-
-
-void BH_Vec2iSub(const int *a,
-                 const int *b,
-                 int *out)
-{
-    out[0] = a[0] - b[0];
-    out[1] = a[1] - b[1];
-}
-
-
-void BH_Vec2iMul(const int *a,
-                 const int *b,
-                 int *out)
-{
-    out[0] = a[0] * b[0];
-    out[1] = a[1] * b[1];
-}
-
-
-void BH_Vec2iScale(const int *a,
-                   int b,
-                   int *out)
-{
-    out[0] = a[0] * b;
-    out[1] = a[1] * b;
-}
-
-
-void BH_Vec2iMulAdd(const int *a,
-                    const int *b,
-                    const int *c,
-                    int *out)
-{
-    out[0] = a[0] * b[0] + c[0];
-    out[1] = a[1] * b[1] + c[1];
-}
-
-
-void BH_Vec2iNegate(const int *in,
-                    int *out)
-{
-    out[0] = -in[0];
-    out[1] = -in[1];
-}
-
-
-void BH_Vec2iMin(const int *a,
-                 const int *b,
-                 int *out)
-{
-    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
-}
-
-
-void BH_Vec2iMax(const int *a,
-                 const int *b,
-                 int *out)
-{
-    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
-    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
-}
-
-
-void BH_Quat4fIdentity(float *out)
-{
-    static const float ident[4] = {0.0f, 0.0f, 0.0f, 1.0f};
-    memcpy(out, ident, sizeof(ident));
-}
-
-
-void BH_Quat4fConjugate(const float *in,
-                        float *out)
-{
-    out[0] = -in[0];
-    out[1] = -in[1];
-    out[2] = -in[2];
-    out[3] = in[3];
-}
-
-
-void BH_Quat4fInverse(const float *in,
-                      float *out)
-{
-    float dot;
-
-    dot = BH_Vec4fDot(in, in);
-    BH_Quat4fConjugate(in, out);
-    BH_Quat4fScale(out, 1.0f / dot, out);
-}
-
-
-void BH_Quat4fMul(const float *a,
-                  const float *b,
-                  float *out)
-{
-    float tmp1[4], tmp2[4], tmp3[4];
-    float w;
-
-    w = a[3] * b[3] - BH_Vec3fDot(a, b);
-    BH_Vec4fScale(a, b[3], tmp1);
-    BH_Vec4fScale(b, a[3], tmp2);
-    BH_Vec3fCross(a, b, tmp3);
-    BH_Vec4fAdd(tmp1, tmp2, out);
-    BH_Vec4fAdd(tmp3, out, out);
-    out[3] = w;
-}
-
-
-void BH_Quat4fSlerp(const float *a,
-                    const float *b,
-                    float t,
-                    float *out)
-{
-    float angle, denom;
-    float from[4], to[4];
-
-    angle = acosf(BH_Vec4fDot(a, b));
-    if (fabsf(angle) < EPSILON)
-    {
-        BH_Vec4fLerp(a, b, t, out);
-        return;
-    }
-
-    denom = 1.0f / sinf(angle);
-    BH_Vec4fScale(a, sinf((1 - t) * angle) * denom, from);
-    BH_Vec4fScale(b, sinf(t * angle) * denom, to);
-    BH_Vec4fAdd(from, to, out);
-}
-
-
-void BH_Quat4fFromEuler(float roll,
-                        float pitch,
-                        float yaw,
-                        float *out)
-{
-    float cr, cp, cy, sr, sp, sy;
-
-    cr = cosf(roll / 2.0f);
-    cp = cosf(pitch / 2.0f);
-    cy = cosf(yaw / 2.0f);
-    sr = sinf(roll / 2.0f);
-    sp = sinf(pitch / 2.0f);
-    sy = sinf(yaw / 2.0f);
-
-    out[0] = sr * cp * cy - cr * sp * sy;
-    out[1] = cr * sp * cy + sr * cp * sy;
-    out[2] = cr * cp * sy - sr * sp * cy;
-    out[3] = cr * cp * cy + sr * sp * sy;
-}
-
-
-void BH_Quat4fFromAxis(const float *axis,
-                       float angle,
-                       float *out)
-{
-    float c, s;
-
-    c = cosf(angle / 2.0f);
-    s = sinf(angle / 2.0f);
-
-    out[0] = axis[0] * s;
-    out[1] = axis[1] * s;
-    out[2] = axis[2] * s;
-    out[3] = c;
-}
-
-
-void BH_Quat4fToEuler(const float *in,
-                      float *roll,
-                      float *pitch,
-                      float *yaw)
-{
-    float ww, xw, yw, zw, xx, xy, xz, yy, yz, zz, angle;
-
-    xx = in[0] * in[0];
-    xy = in[0] * in[1];
-    xz = in[0] * in[2];
-    xw = in[0] * in[3];
-    yy = in[1] * in[1];
-    yz = in[1] * in[2];
-    yw = in[1] * in[3];
-    zz = in[2] * in[2];
-    zw = in[2] * in[3];
-    ww = in[3] * in[3];
-
-    angle = 2.0f * (yw - xz);
-    if (angle > 1.0f)
-        angle = 1.0f;
-    if (angle < -1.0f)
-        angle = -1.0f;
-
-    *pitch = asinf(angle);
-
-    if (fabsf(*pitch - (PI / 2.0f)) < EPSILON)
-    {
-        *roll = 0.0f;
-        *yaw = -2.0f * atan2f(in[0], in[3]);
-    }
-    else if (fabsf(*pitch - (PI / -2.0f)) < EPSILON)
-    {
-        *roll = 0.0f;
-        *yaw = 2.0f * atan2f(in[0], in[3]);
-    }
-    else
-    {
-        *roll = atan2f(2.0f * (xw + yz), ww - xx - yy + zz);
-        *yaw = atan2f(2.0f * (zw + xy), ww + xx - yy - zz);
-    }
-}
-
-
-void BH_Quat4fToAxis(const float *in,
-                     float *axis,
-                     float *angle)
-{
-    *angle = 2.0f * acosf(in[3]);
-
-    if (fabsf(*angle) < EPSILON)
-    {
-        axis[0] = 1.0f;
-        axis[1] = 0.0f;
-        axis[2] = 0.0f;
-    }
-    else
-    {
-        float tmp;
-
-        tmp = sqrtf(1.0f - in[3] * in[3]);
-        axis[0] = in[0] / tmp;
-        axis[1] = in[1] / tmp;
-        axis[2] = in[2] / tmp;
-    }
-}
-
-
-void BH_Quat4fToMat4f(const float *in,
-                      float *out)
-{
-    float xx, xy, xz, xw, yy, yz, yw, zz, zw;
-
-    xx = in[0] * in[0];
-    xy = in[0] * in[1];
-    xz = in[0] * in[2];
-    xw = in[0] * in[3];
-    yy = in[1] * in[1];
-    yz = in[1] * in[2];
-    yw = in[1] * in[3];
-    zz = in[2] * in[2];
-    zw = in[2] * in[3];
-
-    BH_Mat4fIdentity(out);
-    out[0] = 1.0f - 2.0f * (yy + zz);
-    out[1] = 2.0f * (xy + zw);
-    out[2] = 2.0f * (xz - yw);
-    out[4] = 2.0f * (xy - zw);
-    out[5] = 1.0f - 2.0f * (xx + zz);
-    out[6] = 2.0f * (yz + xw);
-    out[8] = 2.0f * (xz + yw);
-    out[9] = 2.0f * (yz - xw);
-    out[10] = 1.0f - 2.0f * (xx + yy);
-}
-
-
-void BH_Mat4fIdentity(float *out)
-{
-    const float ident[16] =
-    {
-        1.0f, 0.0f, 0.0f, 0.0f,
-        0.0f, 1.0f, 0.0f, 0.0f,
-        0.0f, 0.0f, 1.0f, 0.0f,
-        0.0f, 0.0f, 0.0f, 1.0f
-    };
-
-    memcpy(out, ident, sizeof(ident));
-}
-
-
-void BH_Mat4fAdd(const float *a,
-                 const float *b,
-                 float *out)
-{
-    BH_Vec4fAdd(&a[0], &b[0], &out[0]);
-    BH_Vec4fAdd(&a[4], &b[4], &out[4]);
-    BH_Vec4fAdd(&a[8], &b[8], &out[8]);
-    BH_Vec4fAdd(&a[12], &b[12], &out[12]);
-}
-
-
-void BH_Mat4fSub(const float *a,
-                 const float *b,
-                 float *out)
-{
-    BH_Vec4fSub(&a[0], &b[0], &out[0]);
-    BH_Vec4fSub(&a[4], &b[4], &out[4]);
-    BH_Vec4fSub(&a[8], &b[8], &out[8]);
-    BH_Vec4fSub(&a[12], &b[12], &out[12]);
-}
-
-
-void BH_Mat4fMul(const float *a,
-                 const float *b,
-                 float *out)
-{
-    float tmp[16], row[4];
-
-    row[0] = row[1] = row[2] = row[3] = b[0]; BH_Vec4fMul(&a[0], row, &tmp[0]);
-    row[0] = row[1] = row[2] = row[3] = b[1]; BH_Vec4fMulAdd(&a[4], row, &tmp[0], &tmp[0]);
-    row[0] = row[1] = row[2] = row[3] = b[2]; BH_Vec4fMulAdd(&a[8], row, &tmp[0], &tmp[0]);
-    row[0] = row[1] = row[2] = row[3] = b[3]; BH_Vec4fMulAdd(&a[12], row, &tmp[0], &tmp[0]);
-
-    row[0] = row[1] = row[2] = row[3] = b[4]; BH_Vec4fMul(&a[0], row, &tmp[4]);
-    row[0] = row[1] = row[2] = row[3] = b[5]; BH_Vec4fMulAdd(&a[4], row, &tmp[4], &tmp[4]);
-    row[0] = row[1] = row[2] = row[3] = b[6]; BH_Vec4fMulAdd(&a[8], row, &tmp[4], &tmp[4]);
-    row[0] = row[1] = row[2] = row[3] = b[7]; BH_Vec4fMulAdd(&a[12], row, &tmp[4], &tmp[4]);
-
-    row[0] = row[1] = row[2] = row[3] = b[8]; BH_Vec4fMul(&a[0], row, &tmp[8]);
-    row[0] = row[1] = row[2] = row[3] = b[9]; BH_Vec4fMulAdd(&a[4], row, &tmp[8], &tmp[8]);
-    row[0] = row[1] = row[2] = row[3] = b[10]; BH_Vec4fMulAdd(&a[8], row, &tmp[8], &tmp[8]);
-    row[0] = row[1] = row[2] = row[3] = b[11]; BH_Vec4fMulAdd(&a[12], row, &tmp[8], &tmp[8]);
-
-    row[0] = row[1] = row[2] = row[3] = b[12]; BH_Vec4fMul(&a[0], row, &tmp[12]);
-    row[0] = row[1] = row[2] = row[3] = b[13]; BH_Vec4fMulAdd(&a[4], row, &tmp[12], &tmp[12]);
-    row[0] = row[1] = row[2] = row[3] = b[14]; BH_Vec4fMulAdd(&a[8], row, &tmp[12], &tmp[12]);
-    row[0] = row[1] = row[2] = row[3] = b[15]; BH_Vec4fMulAdd(&a[12], row, &tmp[12], &tmp[12]);
-
-    memcpy(out, tmp, sizeof(tmp));
-}
-
-
-void BH_Mat4fScale(const float *a,
-                   float b,
-                   float *out)
-{
-    BH_Vec4fScale(&a[0], b, &out[0]);
-    BH_Vec4fScale(&a[4], b, &out[4]);
-    BH_Vec4fScale(&a[8], b, &out[8]);
-    BH_Vec4fScale(&a[12], b, &out[12]);
-}
-
-
-void BH_Mat4fTranspose(const float *in,
-                       float *out)
-{
-    float tmp[16];
-
-    tmp[0] = in[0]; tmp[4] = in[1]; tmp[8] = in[2]; tmp[12] = in[3];
-    tmp[1] = in[4]; tmp[5] = in[5]; tmp[9] = in[6]; tmp[13] = in[7];
-    tmp[2] = in[8]; tmp[6] = in[9]; tmp[10] = in[10]; tmp[14] = in[11];
-    tmp[3] = in[12]; tmp[7] = in[13]; tmp[11] = in[14]; tmp[15] = in[15];
-
-    memcpy(out, tmp, sizeof(tmp));
-}
-
-
-float BH_Mat4fTrace(const float *in)
-{
-    return in[0] + in[5] + in[10] + in[15];
-}
-
-
-float BH_Mat4fDet(const float *in)
-{
-    float a, b, c, d, e, f, result;
-
-    a = in[2]  * in[7]  - in[3]  * in[6];
-    b = in[2]  * in[11] - in[3]  * in[10];
-    c = in[2]  * in[15] - in[3]  * in[14];
-    d = in[6]  * in[11] - in[7]  * in[10];
-    e = in[6]  * in[15] - in[7]  * in[14];
-    f = in[10] * in[15] - in[11] * in[14];
-
-    result = 0.0f;
-    result += in[0]  * (in[5] * f - in[9] * e + in[13] * d);
-    result -= in[4]  * (in[1] * f - in[9] * c + in[13] * b);
-    result += in[8]  * (in[1] * e - in[5] * c + in[13] * a);
-    result -= in[12] * (in[1] * d - in[5] * b + in[9]  * a);
-
-    return result;
-}
-
-
-int BH_Mat4fInverse(const float *in,
-                    float *out)
-{
-    float a, b, c, d, e, f, det;
-    float tmp[16];
-
-    a = in[2]  * in[7]  - in[3]  * in[6];
-    b = in[2]  * in[11] - in[3]  * in[10];
-    c = in[2]  * in[15] - in[3]  * in[14];
-    d = in[6]  * in[11] - in[7]  * in[10];
-    e = in[6]  * in[15] - in[7]  * in[14];
-    f = in[10] * in[15] - in[11] * in[14];
-
-    tmp[0] =  (in[5] * f - in[9] * e + in[13] * d);
-    tmp[1] = -(in[1] * f - in[9] * c + in[13] * b);
-    tmp[2] =  (in[1] * e - in[5] * c + in[13] * a);
-    tmp[3] = -(in[1] * d - in[5] * b + in[9]  * a);
-
-    det = 0.0f;
-    det += in[0]  * tmp[0];
-    det += in[4]  * tmp[1];
-    det += in[8]  * tmp[2];
-    det += in[12] * tmp[3];
-
-    if (fabsf(det) < EPSILON)
-        return BH_ERROR;
-
-    tmp[4] = -(in[4] * f - in[8] * e + in[12] * d);
-    tmp[5] =  (in[0] * f - in[8] * c + in[12] * b);
-    tmp[6] = -(in[0] * e - in[4] * c + in[12] * a);
-    tmp[7] =  (in[0] * d - in[4] * b + in[8]  * a);
-
-    a = in[1] * in[7]  - in[3]  * in[5];
-    b = in[1] * in[11] - in[3]  * in[9];
-    c = in[1] * in[15] - in[3]  * in[13];
-    d = in[5] * in[11] - in[7]  * in[9];
-    e = in[5] * in[15] - in[7]  * in[13];
-    f = in[9] * in[15] - in[11] * in[13];
-
-    tmp[8] =   (in[4] * f - in[8] * e + in[12] * d);
-    tmp[9] =  -(in[0] * f - in[8] * c + in[12] * b);
-    tmp[10] =  (in[0] * e - in[4] * c + in[12] * a);
-    tmp[11] = -(in[0] * d - in[4] * b + in[8]  * a);
-
-    a = in[1] * in[6]  - in[2]  * in[5];
-    b = in[1] * in[10] - in[2]  * in[9];
-    c = in[1] * in[14] - in[2]  * in[13];
-    d = in[5] * in[10] - in[6]  * in[9];
-    e = in[5] * in[14] - in[6]  * in[13];
-    f = in[9] * in[14] - in[10] * in[13];
-
-    tmp[12] = -(in[4] * f - in[8] * e + in[12] * d);
-    tmp[13] =  (in[0] * f - in[8] * c + in[12] * b);
-    tmp[14] = -(in[0] * e - in[4] * c + in[12] * a);
-    tmp[15] =  (in[0] * d - in[4] * b + in[8]  * a);
-
-    BH_Mat4fScale(tmp, 1.0f / det, out);
-    return BH_OK;
-}
-
-
-void BH_Mat4fFromScale(float x,
-                       float y,
-                       float z,
-                       float *out)
-{
-    BH_Mat4fIdentity(out);
-    out[0] = x;
-    out[5] = y;
-    out[10] = z;
-}
-
-
-void BH_Mat4fFromTranslation(float x,
-                             float y,
-                             float z,
-                             float *out)
-{
-    BH_Mat4fIdentity(out);
-    out[12] = x;
-    out[13] = y;
-    out[14] = z;
-}
-
-
-void BH_Mat4fFromRotationX(float angle,
-                           float *out)
-{
-    float c, s;
-
-    c = cosf(angle);
-    s = sinf(angle);
-
-    BH_Mat4fIdentity(out);
-    out[5] = c;
-    out[6] = s;
-    out[9] = -s;
-    out[10] = c;
-}
-
-
-void BH_Mat4fFromRotationY(float angle,
-                           float *out)
-{
-    float c, s;
-
-    c = cosf(angle);
-    s = sinf(angle);
-
-    BH_Mat4fIdentity(out);
-    out[0] = c;
-    out[2] = -s;
-    out[8] = s;
-    out[10] = c;
-}
-
-
-void BH_Mat4fFromRotationZ(float angle,
-                           float *out)
-{
-    float c, s;
-
-    c = cosf(angle);
-    s = sinf(angle);
-
-    BH_Mat4fIdentity(out);
-    out[0] = c;
-    out[1] = s;
-    out[4] = -s;
-    out[5] = c;
-}
-
-
-void BH_Mat4fFromAxis(const float *axis,
-                      float angle,
-                      float *out)
-{
-    float x, y, z, length;
-    float c, s, moc, xx, xy, xz, yy, yz, zz;
-
-    length = BH_Vec3fLength(axis);
-    BH_Mat4fIdentity(out);
-
-    if (fabsf(length) < EPSILON)
-        return;
-
-    x = axis[0] / length;
-    y = axis[1] / length;
-    z = axis[2] / length;
-
-    c = cosf(angle);
-    s = sinf(angle);
-    moc = 1.0f - c;
-
-    xx = x * x;
-    xy = x * y;
-    xz = x * z;
-    yy = y * y;
-    yz = y * z;
-    zz = z * z;
-
-    out[0] = c + xx * moc;
-    out[1] = xy * moc + z * s;
-    out[2] = xz * moc - y * s;
-
-    out[4] = xy * moc - z * s;
-    out[5] = c + yy * moc;
-    out[6] = yz * moc + x * s;
-
-    out[8] = xz * moc + y * s;
-    out[9] = yz * moc - x * s;
-    out[10] = c + zz * moc;
-}
-
-
-void BH_Mat4fFromEuler(float roll,
-                       float pitch,
-                       float yaw,
-                       float *out)
-{
-    float rs, rc, ys, yc, ps, pc;
-
-    rs = sinf(roll);
-    rc = cosf(roll);
-    ps = sinf(pitch);
-    pc = cosf(pitch);
-    ys = sinf(yaw);
-    yc = cosf(yaw);
-
-    BH_Mat4fIdentity(out);
-    out[0] = pc * yc;
-    out[1] = pc * ys;
-    out[2] = -ps;
-    out[4] = ps * rs * yc - rc * ys;
-    out[5] = ps * rs * ys + rc * yc;
-    out[6] = pc * rs;
-    out[8] = rs * ys + ps * rc * yc;
-    out[9] = ps * rc * ys - rs * yc;
-    out[10] = pc * rc;
-}
-
-
-void BH_Mat4fFromQuat4f(const float *in,
-                        float *out)
-{
-    BH_Quat4fToMat4f(in, out);
-}
-
-
-void BH_Mat4fFromOrtho(float xMin,
-                       float xMax,
-                       float yMin,
-                       float yMax,
-                       float zMin,
-                       float zMax,
-                       float *out)
-{
-    float dx, dy, dz;
-
-    dx = xMax - xMin;
-    dy = yMax - yMin;
-    dz = zMax - zMin;
-
-    BH_Mat4fIdentity(out);
-
-    out[0] = 2.0f / dx;
-    out[5] = 2.0f / dy;
-    out[10] = -2.0f / dz;
-    out[12] = -(xMax + xMin) / dx;
-    out[13] = -(yMax + yMin) / dy;
-    out[14] = -(zMax + zMin) / dz;
-}
-
-
-void BH_Mat4fFromFrustum(float fov,
-                         float aspect,
-                         float zMin,
-                         float zMax,
-                         float *out)
-{
-    float t, dz;
-
-    dz = zMax - zMin;
-    t = tanf(fov / 2.0f);
-
-    BH_Mat4fIdentity(out);
-
-    out[0] = 1.0f / (aspect * t);
-    out[5] = 1.0f / t;
-    out[10] = -(zMax + zMin) / dz;
-    out[11] = -1.0f;
-    out[14] = -(2.0f * zMax * zMin) / dz;
-    out[15] = 0.0f;
-}
-
-
-void BH_Mat4fFromLookAt(const float *position,
-                        const float *at,
-                        const float *up,
-                        float *out)
-{
-    float cameraDir[3], cameraRight[3], cameraUp[3];
-
-    BH_Vec3fSub(position, at, cameraDir);
-    BH_Vec3fNormal(cameraDir, cameraDir);
-    BH_Vec3fCross(up, cameraDir, cameraRight);
-    BH_Vec3fNormal(cameraRight, cameraRight);
-    BH_Vec3fCross(cameraDir, cameraRight, cameraUp);
-
-    out[0] = cameraRight[0];
-    out[1] = cameraUp[0];
-    out[2] = cameraDir[0];
-    out[3] = 0.0f;
-
-    out[4] = cameraRight[1];
-    out[5] = cameraUp[1];
-    out[6] = cameraDir[1];
-    out[7] = 0.0f;
-
-    out[8] = cameraRight[2];
-    out[9] = cameraUp[2];
-    out[10] = cameraDir[2];
-    out[11] = 0.0f;
-
-    out[12] = -BH_Vec3fDot(cameraRight, position);
-    out[13] = -BH_Vec3fDot(cameraUp, position);
-    out[14] = -BH_Vec3fDot(cameraDir, position);
-    out[15] = 1.0f;
-}
-
-
-void BH_Mat4fApplyVec4f(const float *a,
-                        const float *b,
-                        float *out)
-{
-    float tmp[4], row[4];
-
-    row[0] = row[1] = row[2] = row[3] = b[0]; BH_Vec4fMul(&a[0], row, tmp);
-    row[0] = row[1] = row[2] = row[3] = b[1]; BH_Vec4fMulAdd(&a[4], row, tmp, tmp);
-    row[0] = row[1] = row[2] = row[3] = b[2]; BH_Vec4fMulAdd(&a[8], row, tmp, tmp);
-    row[0] = row[1] = row[2] = row[3] = b[3]; BH_Vec4fMulAdd(&a[12], row, tmp, tmp);
-
-    memcpy(out, tmp, sizeof(tmp));
-}
-
-
-void BH_Mat4fApplyVec3f(const float *a,
-                        const float *b,
-                        float *out)
-{
-    float tmp[4], row[4];
-
-    row[0] = row[1] = row[2] = row[3] = b[0]; BH_Vec4fMul(&a[0], row, tmp);
-    row[0] = row[1] = row[2] = row[3] = b[1]; BH_Vec4fMulAdd(&a[4], row, tmp, tmp);
-    row[0] = row[1] = row[2] = row[3] = b[2]; BH_Vec4fMulAdd(&a[8], row, tmp, tmp);
-    row[0] = row[1] = row[2] = row[3] = 1.0f; BH_Vec4fMulAdd(&a[12], row, tmp, tmp);
-
-    memcpy(out, tmp, sizeof(float) * 3);
-}
-
-
-void BH_Mat3fIdentity(float *out)
-{
-    static const float ident[9] =
-    {
-        1.0f, 0.0f, 0.0f,
-        0.0f, 1.0f, 0.0f,
-        0.0f, 0.0f, 1.0f
-    };
-
-    memcpy(out, ident, sizeof(ident));
-}
-
-
-void BH_Mat3fAdd(const float *a,
-                 const float *b,
-                 float *out)
-{
-    BH_Vec3fAdd(&a[0], &b[0], &out[0]);
-    BH_Vec3fAdd(&a[3], &b[3], &out[3]);
-    BH_Vec3fAdd(&a[6], &b[6], &out[6]);
-}
-
-
-void BH_Mat3fSub(const float *a,
-                 const float *b,
-                 float *out)
-{
-    BH_Vec3fSub(&a[0], &b[0], &out[0]);
-    BH_Vec3fSub(&a[3], &b[3], &out[3]);
-    BH_Vec3fSub(&a[6], &b[6], &out[6]);
-}
-
-
-void BH_Mat3fMul(const float *a,
-                 const float *b,
-                 float *out)
-{
-    float tmp[9], row[3];
-
-    row[0] = row[1] = row[2] = b[0]; BH_Vec3fMul(&a[0], row, &tmp[0]);
-    row[0] = row[1] = row[2] = b[1]; BH_Vec3fMulAdd(&a[3], row, &tmp[0], &tmp[0]);
-    row[0] = row[1] = row[2] = b[2]; BH_Vec3fMulAdd(&a[6], row, &tmp[0], &tmp[0]);
-
-    row[0] = row[1] = row[2] = b[3]; BH_Vec3fMul(&a[0], row, &tmp[3]);
-    row[0] = row[1] = row[2] = b[4]; BH_Vec3fMulAdd(&a[3], row, &tmp[3], &tmp[3]);
-    row[0] = row[1] = row[2] = b[5]; BH_Vec3fMulAdd(&a[6], row, &tmp[3], &tmp[3]);
-
-    row[0] = row[1] = row[2] = b[6]; BH_Vec3fMul(&a[0], row, &tmp[6]);
-    row[0] = row[1] = row[2] = b[7]; BH_Vec3fMulAdd(&a[3], row, &tmp[6], &tmp[6]);
-    row[0] = row[1] = row[2] = b[8]; BH_Vec3fMulAdd(&a[6], row, &tmp[6], &tmp[6]);
-
-    memcpy(out, tmp, sizeof(tmp));
-}
-
-
-void BH_Mat3fScale(const float *a,
-                   float b,
-                   float *out)
-{
-    BH_Vec3fScale(&a[0], b, &out[0]);
-    BH_Vec3fScale(&a[3], b, &out[3]);
-    BH_Vec3fScale(&a[6], b, &out[6]);
-}
-
-
-void BH_Mat3fTranspose(const float *in,
-                       float *out)
-{
-    float tmp[9];
-
-    tmp[0] = in[0]; tmp[3] = in[1]; tmp[6] = in[2];
-    tmp[1] = in[3]; tmp[4] = in[4]; tmp[7] = in[5];
-    tmp[2] = in[6]; tmp[5] = in[7]; tmp[8] = in[8];
-
-    memcpy(out, tmp, sizeof(tmp));
-}
-
-
-float BH_Mat3fTrace(const float *in)
-{
-    return in[0] + in[4] + in[8];
-}
-
-
-float BH_Mat3fDet(const float *in)
-{
-    float a, b, c, result;
-
-    a = in[4] * in[8] - in[7] * in[5];
-    b = in[1] * in[8] - in[7] * in[2];
-    c = in[1] * in[5] - in[4] * in[2];
-
-    result = 0.0f;
-    result += in[0] * a;
-    result -= in[3] * b;
-    result += in[6] * c;
-
-    return result;
-}
-
-
-int BH_Mat3fInverse(const float *in,
-                    float *out)
-{
-    float a, b, c, det;
-    float tmp[16];
-
-    a = in[4] * in[8] - in[7] * in[5];
-    b = in[1] * in[8] - in[7] * in[2];
-    c = in[1] * in[5] - in[4] * in[2];
-
-    tmp[0] =  a;
-    tmp[1] = -b;
-    tmp[2] =  c;
-
-    det = 0.0f;
-    det += in[0] * tmp[0];
-    det += in[3] * tmp[1];
-    det += in[6] * tmp[2];
-
-    if (fabsf(det) < EPSILON)
-        return BH_ERROR;
-
-    a = in[3] * in[8] - in[6] * in[5];
-    b = in[0] * in[8] - in[6] * in[2];
-    c = in[0] * in[5] - in[3] * in[2];
-
-    tmp[3] = -a;
-    tmp[4] =  b;
-    tmp[5] = -c;
-
-    a = in[3] * in[7] - in[6] * in[4];
-    b = in[0] * in[7] - in[6] * in[1];
-    c = in[0] * in[4] - in[3] * in[1];
-
-    tmp[6] =  a;
-    tmp[7] = -b;
-    tmp[8] =  c;
-
-    BH_Mat3fScale(tmp, 1.0f / det, out);
-    return BH_OK;
-}
-
-
-void BH_Mat3fFromScale(float x,
-                       float y,
-                       float *out)
-{
-    BH_Mat3fIdentity(out);
-    out[0] = x;
-    out[4] = y;
-}
-
-
-void BH_Mat3fFromTranslation(float x,
-                             float y,
-                             float *out)
-{
-    BH_Mat3fIdentity(out);
-    out[6] = x;
-    out[7] = y;
-}
-
-
-void BH_Mat3fFromRotation(float angle,
-                          float *out)
-{
-    float c, s;
-
-    c = cosf(angle);
-    s = sinf(angle);
-
-    BH_Mat3fIdentity(out);
-    out[0] = c;
-    out[1] = s;
-    out[3] = -s;
-    out[4] = c;
-}
-
-
-void BH_Mat3fApplyVec3f(float *a,
-                        float *b,
-                        float *out)
-{
-    float tmp[3], row[3];
-
-    row[0] = row[1] = row[2] = b[0]; BH_Vec3fMul(&a[0], row, tmp);
-    row[0] = row[1] = row[2] = b[1]; BH_Vec3fMulAdd(&a[3], row, tmp, tmp);
-    row[0] = row[1] = row[2] = b[2]; BH_Vec3fMulAdd(&a[6], row, tmp, tmp);
-
-    memcpy(out, tmp, sizeof(tmp));
-}
-
-
-void BH_Mat3fApplyVec2f(float *a,
-                        float *b,
-                        float *out)
-{
-    float tmp[3], row[3];
-
-    row[0] = row[1] = row[2] = b[0]; BH_Vec3fMul(&a[0], row, tmp);
-    row[0] = row[1] = row[2] = b[1]; BH_Vec3fMulAdd(&a[3], row, tmp, tmp);
-    row[0] = row[1] = row[2] = 1.0f; BH_Vec3fMulAdd(&a[6], row, tmp, tmp);
-
-    memcpy(out, tmp, sizeof(float) * 2);
-}
-
-
-int BH_PlaneFromPoints(const float *a,
-                       const float *b,
-                       const float *c,
-                       float *out)
-{
-    float tmp1[3], tmp2[3];
-
-    BH_Vec3fSub(b, a, tmp1);
-    BH_Vec3fSub(c, a, tmp2);
-    BH_Vec3fCross(tmp2, tmp1, tmp1);
-    if (BH_Vec3fNormalEx(tmp1, tmp1) < EPSILON)
-        return BH_ERROR;
-
-    out[3] = BH_Vec3fDot(a, tmp1);
-    memcpy(out, tmp1, sizeof(tmp1));
-    return BH_OK;
-}
-
-
-float BH_PlaneDistance(const float *plane,
-                       const float *point)
-{
-    return BH_Vec3fDot(plane, point) - plane[3];
-}
-
-
-void BH_PlaneClosestPoint(const float *plane,
-                          const float *point,
-                          float *out)
-{
-    float tmp[3];
-
-    BH_Vec3fScale(plane, BH_PlaneDistance(plane, point), tmp);
-    BH_Vec3fSub(point, tmp, out);
-}
-
-
-int BH_Ray3fIntersectPlane(const float *start,
-                           const float *direction,
-                           const float *plane,
-                           float *t,
-                           float *out)
-{
-    float tmp1[3];
-    float denom, time;
-
-    /* Calculate intersection time */
-    denom = BH_Vec3fDot(direction, plane);
-    time = (plane[3] - BH_Vec3fDot(plane, start)) / denom;
-
-    /* Check for ray/plane parallel to each other or point is behing the ray. */
-    if (fabsf(denom) < EPSILON || time < 0.0f)
-        return BH_ERROR;
-
-    /* Compute intersection point */
-    BH_Vec3fScale(direction, time, tmp1);
-    BH_Vec3fAdd(start, tmp1, out);
-    *t = time;
-    return BH_OK;
-}
-
-
-int BH_Ray3fIntersectTriangle(const float *start,
-                              const float *direction,
-                              const float *a,
-                              const float *b,
-                              const float *c,
-                              float *t,
-                              float *out)
-{
-    float plane[4];
-    float tmp1[3], tmp2[3], tmp3[3];
-    float time;
-
-    /* Compute plane */
-    if (BH_PlaneFromPoints(a, b, c, plane))
-        return BH_ERROR;
-
-    /* Compute intersection point in ray against plane */
-    if (BH_Ray3fIntersectPlane(start, direction, plane, &time, tmp3))
-        return BH_ERROR;
-
-    /* Check if point inside rectangle */
-    BH_Vec3fSub(b, a, tmp1);
-    BH_Vec3fSub(tmp3, a, tmp2);
-    BH_Vec3fCross(tmp2, tmp1, tmp1);
-    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
-        return BH_ERROR;
-
-    BH_Vec3fSub(c, b, tmp1);
-    BH_Vec3fSub(tmp3, b, tmp2);
-    BH_Vec3fCross(tmp2, tmp1, tmp1);
-    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
-        return BH_ERROR;
-
-    BH_Vec3fSub(a, c, tmp1);
-    BH_Vec3fSub(tmp3, c, tmp2);
-    BH_Vec3fCross(tmp2, tmp1, tmp1);
-    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
-        return BH_ERROR;
-
-    /* Copy intersection point */
-    memcpy(out, tmp3, sizeof(tmp3));
-    *t = time;
-    return BH_OK;
-}
-
-
-int BH_Segment3fIntersectPlane(const float *start,
-                               const float *end,
-                               const float *plane,
-                               float *t,
-                               float *out)
-{
-    float tmp[3];
-    float denom, time;
-
-    /* Calculate intersection time */
-    BH_Vec3fSub(end, start, tmp);
-    denom = BH_Vec3fDot(tmp, plane);
-    time = (plane[3] - BH_Vec3fDot(plane, start)) / denom;
-
-    /* Check for ray/plane parallel to each other or point is behing the ray. */
-    if (fabsf(denom) < EPSILON || time < 0.0f || time > 1.0f)
-        return BH_ERROR;
-
-    /* Compute intersection point */
-    BH_Vec3fScale(tmp, time, tmp);
-    BH_Vec3fAdd(start, tmp, out);
-    *t = time;
-    return BH_OK;
-}
-
-
-int BH_Segment3fIntersectTriangle(const float *start,
-                                  const float *end,
-                                  const float *a,
-                                  const float *b,
-                                  const float *c,
-                                  float *t,
-                                  float *out)
-{
-    float plane[4];
-    float tmp1[3], tmp2[3], tmp3[3];
-    float time;
-
-    /* Compute plane */
-    if (BH_PlaneFromPoints(a, b, c, plane))
-        return BH_ERROR;
-
-    /* Compute intersection point in ray against plane */
-    if (BH_Segment3fIntersectPlane(start, end, plane, &time, tmp3))
-        return BH_ERROR;
-
-    /* Check if point inside rectangle */
-    BH_Vec3fSub(b, a, tmp1);
-    BH_Vec3fSub(tmp3, a, tmp2);
-    BH_Vec3fCross(tmp2, tmp1, tmp1);
-    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
-        return BH_ERROR;
-
-    BH_Vec3fSub(c, b, tmp1);
-    BH_Vec3fSub(tmp3, b, tmp2);
-    BH_Vec3fCross(tmp2, tmp1, tmp1);
-    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
-        return BH_ERROR;
-
-    BH_Vec3fSub(a, c, tmp1);
-    BH_Vec3fSub(tmp3, c, tmp2);
-    BH_Vec3fCross(tmp2, tmp1, tmp1);
-    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
-        return BH_ERROR;
-
-    /* Copy intersection point */
-    memcpy(out, tmp3, sizeof(tmp3));
-    *t = time;
-    return BH_OK;
-}
-
-
-void BH_Triangle3fBarycentric(const float *a,
-                              const float *b,
-                              const float *c,
-                              const float *point,
-                              float *out)
-{
-    float tmp1[3], tmp2[3], tmp3[3];
-    float t11, t12, t22, t31, t32, denom;
-
-    BH_Vec3fSub(b, a, tmp1);
-    BH_Vec3fSub(c, a, tmp2);
-    BH_Vec3fSub(point, a, tmp3);
-
-    t11 = BH_Vec3fDot(tmp1, tmp1);
-    t12 = BH_Vec3fDot(tmp1, tmp2);
-    t22 = BH_Vec3fDot(tmp2, tmp2);
-    t31 = BH_Vec3fDot(tmp3, tmp1);
-    t32 = BH_Vec3fDot(tmp3, tmp2);
-    denom = 1.0f / (t11 * t22 - t12 * t12);
-
-    out[1] = (t22 * t31 - t12 * t32) * denom;
-    out[2] = (t11 * t32 - t12 * t31) * denom;
-    out[0] = 1.0f - out[1] - out[2];
-}
-
-
-int BH_LineFromPoints(const float *a,
-                      const float *b,
-                      float *out)
-{
-    float tmp[2];
-
-    tmp[0] = a[1] - b[1];
-    tmp[1] = b[0] - a[0];
-    if (BH_Vec2fNormalEx(tmp, tmp) < EPSILON)
-        return BH_ERROR;
-
-    out[2] = BH_Vec2fDot(tmp, a);
-    memcpy(out, tmp, sizeof(tmp));
-    return BH_OK;
-}
-
-
-float BH_LineDistance(const float *line,
-                      const float *point)
-{
-    return BH_Vec2fDot(line, point) - line[2];
-}
-
-
-void BH_LineClosestPoint(const float *line,
-                         const float *point,
-                         float *out)
-{
-    float tmp[2];
-
-    BH_Vec2fScale(line, BH_LineDistance(line, point), tmp);
-    BH_Vec2fSub(point, tmp, out);
-}
-
-
-int BH_Ray2fIntersectLine(const float *start,
-                          const float *direction,
-                          const float *line,
-                          float *t,
-                          float *out)
-{
-    float tmp1[2];
-    float denom, time;
-
-    /* Calculate intersection time */
-    denom = BH_Vec2fDot(direction, line);
-    time = (line[2] - BH_Vec2fDot(line, start)) / denom;
-
-    /* Check for ray/plane parallel to each other or point is behing the ray. */
-    if (fabsf(denom) < EPSILON || time < 0.0f)
-        return BH_ERROR;
-
-    /* Compute intersection point */
-    BH_Vec2fScale(direction, time, tmp1);
-    BH_Vec2fAdd(start, tmp1, out);
-    *t = time;
-    return BH_OK;
-}
-
-
-int BH_Ray2fIntersectTime(const float *aStart,
-                          const float *aDirection,
-                          const float *bStart,
-                          const float *bDirection,
-                          float *time1,
-                          float *time2)
-{
-    float tmp1[2], tmp2[2], tmp3[2];
-    float denom;
-
-    /* Rotate directions by 90 degrees and caluclate denom */
-    tmp1[0] = -aDirection[1]; tmp1[1] = aDirection[0];
-    tmp2[0] = -bDirection[1]; tmp2[1] = bDirection[0];
-    denom = BH_Vec2fDot(tmp1, bDirection);
-
-    if (fabsf(denom) < EPSILON)
-        return BH_ERROR;
-
-    /* Calculate segments offset and intersection times */
-    BH_Vec2fSub(aStart, bStart, tmp3);
-    *time1 = BH_Vec2fDot(tmp3, tmp2) / denom;
-    *time2 = BH_Vec2fDot(tmp3, tmp1) / denom;
-
-    return BH_OK;
-}
-
-
-int BH_Ray2fIntersectRay(const float *aStart,
-                         const float *aDirection,
-                         const float *bStart,
-                         const float *bDirection,
-                         float *t,
-                         float *out)
-{
-    float tmp[2];
-    float time1, time2;
-
-    if (BH_Ray2fIntersectTime(aStart, aDirection, bStart, bDirection, &time1, &time2))
-        return BH_ERROR;
-
-    if (time1 < 0.0f || time2 < 0.0f)
-        return BH_ERROR;
-
-    BH_Vec2fScale(aDirection, time1, tmp);
-    BH_Vec2fAdd(aStart, tmp, out);
-    *t = time1;
-    return BH_OK;
-}
-
-
-int BH_Ray2fIntersectSegment(const float *aStart,
-                             const float *aDirection,
-                             const float *bStart,
-                             const float *bEnd,
-                             float *t,
-                             float *out)
-{
-    float tmp[2];
-    float time1, time2;
-
-    BH_Vec2fSub(bEnd, bStart, tmp);
-    if (BH_Ray2fIntersectTime(aStart, aDirection, bStart, tmp, &time1, &time2))
-        return BH_ERROR;
-
-    if (time1 < 0.0f || time2 < 0.0f || time2 > 1.0f)
-        return BH_ERROR;
-
-    BH_Vec2fScale(aDirection, time1, tmp);
-    BH_Vec2fAdd(aStart, tmp, out);
-    *t = time1;
-    return BH_OK;
-}
-
-
-int BH_Segment2fIntersectLine(const float *start,
-                              const float *end,
-                              const float *line,
-                              float *t,
-                              float *out)
-{
-    float tmp[2];
-    float denom, time;
-
-    /* Calculate intersection time */
-    BH_Vec2fSub(end, start, tmp);
-    denom = BH_Vec2fDot(tmp, line);
-    time = (line[2] - BH_Vec2fDot(line, start)) / denom;
-
-    /* Check for ray/plane parallel to each other or point is behing the ray. */
-    if (fabsf(denom) < EPSILON || time < 0.0f || time > 1.0f)
-        return BH_ERROR;
-
-    /* Compute intersection point */
-    BH_Vec2fScale(tmp, time, tmp);
-    BH_Vec2fAdd(start, tmp, out);
-    *t = time;
-    return BH_OK;
-}
-
-
-int BH_Segment2fIntersectSegment(const float *aStart,
-                                 const float *aEnd,
-                                 const float *bStart,
-                                 const float *bEnd,
-                                 float *t,
-                                 float *out)
-{
-    float tmp1[2], tmp2[2];
-    float time1, time2;
-
-    BH_Vec2fSub(aEnd, aStart, tmp1);
-    BH_Vec2fSub(bEnd, bStart, tmp2);
-    if (BH_Ray2fIntersectTime(aStart, tmp1, bStart, tmp2, &time1, &time2))
-        return BH_ERROR;
-
-    if (time1 < 0.0f || time1 > 1.0f || time2 < 0.0f || time2 > 1.0f)
-        return BH_ERROR;
-
-    BH_Vec2fLerp(aStart, aEnd, time1, out);
-    *t = time1;
-
-    return BH_OK;
-}
-
-
-void BH_Box3fUnion(const float *aMin,
-                   const float *aMax,
-                   const float *bMin,
-                   const float *bMax,
-                   float *outMin,
-                   float *outMax)
-{
-    BH_Vec3fMin(aMin, bMin, outMin);
-    BH_Vec3fMax(aMax, bMax, outMax);
-}
-
-
-int BH_Box3fIntersect(const float *aMin,
-                      const float *aMax,
-                      const float *bMin,
-                      const float *bMax,
-                      float *outMin,
-                      float *outMax)
-{
-    BH_Vec3fMax(aMin, bMin, outMin);
-    BH_Vec3fMin(aMax, bMax, outMax);
-
-    if (outMin[0] >= outMax[0] || outMin[1] >= outMax[1] || outMin[2] >= outMax[2])
-        return BH_ERROR;
-
-    return BH_OK;
-}
-
-
-int BH_Box3fContains(const float *aMin,
-                     const float *aMax,
-                     const float *point)
-{
-    if (point[0] < aMin[0] || point[1] < aMin[1] || point[2] < aMin[2])
-        return BH_ERROR;
-
-    if (point[0] > aMax[0] || point[1] > aMax[1] || point[2] > aMax[2])
-        return BH_ERROR;
-
-    return BH_OK;
-}
-
-
-int BH_Box3fEnclose(const float *points,
-                    size_t size,
-                    float *outMin,
-                    float *outMax)
-{
-    float tmp1[3], tmp2[3];
-    size_t i;
-
-    if (!size)
-        return BH_ERROR;
-
-    memcpy(tmp1, points, sizeof(tmp1));
-    memcpy(tmp2, points, sizeof(tmp2));
-
-    for (i = 1; i < size; i++)
-    {
-        BH_Vec3fMin(tmp1, points + i * 3, tmp1);
-        BH_Vec3fMax(tmp2, points + i * 3, tmp2);
-    }
-
-    memcpy(outMin, tmp1, sizeof(tmp1));
-    memcpy(outMax, tmp2, sizeof(tmp2));
-    return BH_OK;
-}
-
-
-void BH_Box2fUnion(const float *aMin,
-                   const float *aMax,
-                   const float *bMin,
-                   const float *bMax,
-                   float *outMin,
-                   float *outMax)
-{
-    BH_Vec2fMin(aMin, bMin, outMin);
-    BH_Vec2fMax(aMax, bMax, outMax);
-}
-
-
-int BH_Box2fIntersect(const float *aMin,
-                      const float *aMax,
-                      const float *bMin,
-                      const float *bMax,
-                      float *outMin,
-                      float *outMax)
-{
-    BH_Vec2fMax(aMin, bMin, outMin);
-    BH_Vec2fMin(aMax, bMax, outMax);
-
-    if (outMin[0] >= outMax[0] || outMin[1] >= outMax[1])
-        return BH_ERROR;
-
-    return BH_OK;
-}
-
-
-int BH_Box2fContains(const float *aMin,
-                     const float *aMax,
-                     const float *point)
-{
-    if (point[0] < aMin[0] || point[1] < aMin[1])
-        return BH_ERROR;
-
-    if (point[0] > aMax[0] || point[1] > aMax[1])
-        return BH_ERROR;
-
-    return BH_OK;
-}
-
-
-int BH_Box2fEnclose(const float *points,
-                    size_t size,
-                    float *outMin,
-                    float *outMax)
-{
-    float tmp1[2], tmp2[2];
-    size_t i;
-
-    if (!size)
-        return BH_ERROR;
-
-    memcpy(tmp1, points, sizeof(tmp1));
-    memcpy(tmp2, points, sizeof(tmp2));
-
-    for (i = 1; i < size; i++)
-    {
-        BH_Vec2fMin(tmp1, points + i * 2, tmp1);
-        BH_Vec2fMax(tmp2, points + i * 2, tmp2);
-    }
-
-    memcpy(outMin, tmp1, sizeof(tmp1));
-    memcpy(outMax, tmp2, sizeof(tmp2));
-    return BH_OK;
-}
-
-
-int BH_Ray3fIntersectBox3f(const float *aStart,
-                           const float *aDirection,
-                           const float *bMin,
-                           const float *bMax,
-                           float *t,
-                           float *out)
-{
-    float timeNear, timeFar, hitNear, hitFar, denom, tmp;
-    int i;
-
-    timeNear = -1.0f / 0.0f;
-    timeFar  =  1.0f / 0.0f;
-
-    /* Check if origin inside box */
-    if (!BH_Box3fContains(bMin, bMax, aStart))
-    {
-        memcpy(out, aStart, sizeof(float) * 3);
-        *t = 0.0f;
-        return BH_OK;
-    }
-
-    /* Check each axis for the minimal and maximum intersection time */
-    for (i = 0; i < 3; i++)
-    {
-        if (fabsf(aDirection[i]) < EPSILON)
-        {
-            if (aStart[i] < bMin[i] || aStart[i] > bMax[i])
-                return BH_ERROR;
-            continue;
-        }
-
-        denom = 1.0f / aDirection[i];
-        hitNear = (bMin[i] - aStart[i]) * denom;
-        hitFar  = (bMax[i] - aStart[i]) * denom;
-
-        if (hitNear > hitFar)
-        {
-            tmp = hitNear;
-            hitNear = hitFar;
-            hitFar = tmp;
-        }
-
-        if (hitNear > timeNear)
-            timeNear = hitNear;
-        if (hitFar < timeFar)
-            timeFar = hitFar;
-
-        if (timeNear > timeFar || timeFar < 0.0f)
-            return BH_ERROR;
-    }
-
-    out[0] = aStart[0] + aDirection[0] * timeNear;
-    out[1] = aStart[1] + aDirection[1] * timeNear;
-    out[2] = aStart[2] + aDirection[2] * timeNear;
-    *t = timeNear;
-
-    return BH_OK;
-}
-
-
-int BH_Segment3fIntersectBox3f(const float *aStart,
-                               const float *aEnd,
-                               const float *bMin,
-                               const float *bMax,
-                               float *t,
-                               float *out)
-{
-    float tmp[3];
-    float time;
-
-    BH_Vec3fSub(aEnd, aStart, tmp);
-    if (BH_Ray3fIntersectBox3f(aStart, tmp, bMin, bMax, &time, out))
-        return BH_ERROR;
-
-    if (time > 1.0f)
-        return BH_ERROR;
-
-    *t = time;
-    return BH_OK;
-}
-
-
-int BH_Ray2fIntersectBox2f(const float *aStart,
-                           const float *aDirection,
-                           const float *bMin,
-                           const float *bMax,
-                           float *t,
-                           float *out)
-{
-    float timeNear, timeFar, hitNear, hitFar, denom, tmp;
-    int i;
-
-    timeNear = -1.0f / 0.0f;
-    timeFar  =  1.0f / 0.0f;
-
-    /* Check if origin inside box */
-    if (!BH_Box2fContains(bMin, bMax, aStart))
-    {
-        memcpy(out, aStart, sizeof(float) * 2);
-        *t = 0.0f;
-        return BH_OK;
-    }
-
-    /* Check each axis for the minimal and maximum intersection time */
-    for (i = 0; i < 2; i++)
-    {
-        if (fabsf(aDirection[i]) < EPSILON)
-        {
-            if (aStart[i] < bMin[i] || aStart[i] > bMax[i])
-                return BH_ERROR;
-            continue;
-        }
-
-        denom = 1.0f / aDirection[i];
-        hitNear = (bMin[i] - aStart[i]) * denom;
-        hitFar  = (bMax[i] - aStart[i]) * denom;
-
-        if (hitNear > hitFar)
-        {
-            tmp = hitNear;
-            hitNear = hitFar;
-            hitFar = tmp;
-        }
-
-        if (hitNear > timeNear)
-            timeNear = hitNear;
-        if (hitFar < timeFar)
-            timeFar = hitFar;
-
-        if (timeNear > timeFar || timeFar < 0.0f)
-            return BH_ERROR;
-    }
-
-    out[0] = aStart[0] + aDirection[0] * timeNear;
-    out[1] = aStart[1] + aDirection[1] * timeNear;
-    *t = timeNear;
-
-    return BH_OK;
-}
-
-
-int BH_Segment2fIntersectBox2f(const float *aStart,
-                               const float *aEnd,
-                               const float *bMin,
-                               const float *bMax,
-                               float *t,
-                               float *out)
-{
-    float tmp[3];
-    float time;
-
-    BH_Vec2fSub(aEnd, aStart, tmp);
-    if (BH_Ray2fIntersectBox2f(aStart, tmp, bMin, bMax, &time, out))
-        return BH_ERROR;
-
-    if (time > 1.0f)
-        return BH_ERROR;
-
-    *t = time;
-    return BH_OK;
-}
diff --git a/src/Math/Box2f.c b/src/Math/Box2f.c
new file mode 100644
index 0000000..bd6eb73
--- /dev/null
+++ b/src/Math/Box2f.c
@@ -0,0 +1,75 @@
+#include <BH/Math.h>
+#include <string.h>
+
+
+#define EPSILON 0.00001f
+#define PI      3.14159265358979323846f
+
+
+void BH_Box2fUnion(const float *aMin,
+                   const float *aMax,
+                   const float *bMin,
+                   const float *bMax,
+                   float *outMin,
+                   float *outMax)
+{
+    BH_Vec2fMin(aMin, bMin, outMin);
+    BH_Vec2fMax(aMax, bMax, outMax);
+}
+
+
+int BH_Box2fIntersect(const float *aMin,
+                      const float *aMax,
+                      const float *bMin,
+                      const float *bMax,
+                      float *outMin,
+                      float *outMax)
+{
+    BH_Vec2fMax(aMin, bMin, outMin);
+    BH_Vec2fMin(aMax, bMax, outMax);
+
+    if (outMin[0] >= outMax[0] || outMin[1] >= outMax[1])
+        return BH_ERROR;
+
+    return BH_OK;
+}
+
+
+int BH_Box2fContains(const float *aMin,
+                     const float *aMax,
+                     const float *point)
+{
+    if (point[0] < aMin[0] || point[1] < aMin[1])
+        return BH_ERROR;
+
+    if (point[0] > aMax[0] || point[1] > aMax[1])
+        return BH_ERROR;
+
+    return BH_OK;
+}
+
+
+int BH_Box2fEnclose(const float *points,
+                    size_t size,
+                    float *outMin,
+                    float *outMax)
+{
+    float tmp1[2], tmp2[2];
+    size_t i;
+
+    if (!size)
+        return BH_ERROR;
+
+    memcpy(tmp1, points, sizeof(tmp1));
+    memcpy(tmp2, points, sizeof(tmp2));
+
+    for (i = 1; i < size; i++)
+    {
+        BH_Vec2fMin(tmp1, points + i * 2, tmp1);
+        BH_Vec2fMax(tmp2, points + i * 2, tmp2);
+    }
+
+    memcpy(outMin, tmp1, sizeof(tmp1));
+    memcpy(outMax, tmp2, sizeof(tmp2));
+    return BH_OK;
+}
diff --git a/src/Math/Box3f.c b/src/Math/Box3f.c
new file mode 100644
index 0000000..ee09dad
--- /dev/null
+++ b/src/Math/Box3f.c
@@ -0,0 +1,75 @@
+#include <BH/Math.h>
+#include <string.h>
+
+
+#define EPSILON 0.00001f
+#define PI      3.14159265358979323846f
+
+
+void BH_Box3fUnion(const float *aMin,
+                   const float *aMax,
+                   const float *bMin,
+                   const float *bMax,
+                   float *outMin,
+                   float *outMax)
+{
+    BH_Vec3fMin(aMin, bMin, outMin);
+    BH_Vec3fMax(aMax, bMax, outMax);
+}
+
+
+int BH_Box3fIntersect(const float *aMin,
+                      const float *aMax,
+                      const float *bMin,
+                      const float *bMax,
+                      float *outMin,
+                      float *outMax)
+{
+    BH_Vec3fMax(aMin, bMin, outMin);
+    BH_Vec3fMin(aMax, bMax, outMax);
+
+    if (outMin[0] >= outMax[0] || outMin[1] >= outMax[1] || outMin[2] >= outMax[2])
+        return BH_ERROR;
+
+    return BH_OK;
+}
+
+
+int BH_Box3fContains(const float *aMin,
+                     const float *aMax,
+                     const float *point)
+{
+    if (point[0] < aMin[0] || point[1] < aMin[1] || point[2] < aMin[2])
+        return BH_ERROR;
+
+    if (point[0] > aMax[0] || point[1] > aMax[1] || point[2] > aMax[2])
+        return BH_ERROR;
+
+    return BH_OK;
+}
+
+
+int BH_Box3fEnclose(const float *points,
+                    size_t size,
+                    float *outMin,
+                    float *outMax)
+{
+    float tmp1[3], tmp2[3];
+    size_t i;
+
+    if (!size)
+        return BH_ERROR;
+
+    memcpy(tmp1, points, sizeof(tmp1));
+    memcpy(tmp2, points, sizeof(tmp2));
+
+    for (i = 1; i < size; i++)
+    {
+        BH_Vec3fMin(tmp1, points + i * 3, tmp1);
+        BH_Vec3fMax(tmp2, points + i * 3, tmp2);
+    }
+
+    memcpy(outMin, tmp1, sizeof(tmp1));
+    memcpy(outMax, tmp2, sizeof(tmp2));
+    return BH_OK;
+}
diff --git a/src/Math/Line.c b/src/Math/Line.c
new file mode 100644
index 0000000..938775f
--- /dev/null
+++ b/src/Math/Line.c
@@ -0,0 +1,41 @@
+#include <BH/Math.h>
+#include <string.h>
+
+
+#define EPSILON 0.00001f
+#define PI      3.14159265358979323846f
+
+
+int BH_LineFromPoints(const float *a,
+                      const float *b,
+                      float *out)
+{
+    float tmp[2];
+
+    tmp[0] = a[1] - b[1];
+    tmp[1] = b[0] - a[0];
+    if (BH_Vec2fNormalEx(tmp, tmp) < EPSILON)
+        return BH_ERROR;
+
+    out[2] = BH_Vec2fDot(tmp, a);
+    memcpy(out, tmp, sizeof(tmp));
+    return BH_OK;
+}
+
+
+float BH_LineDistance(const float *line,
+                      const float *point)
+{
+    return BH_Vec2fDot(line, point) - line[2];
+}
+
+
+void BH_LineClosestPoint(const float *line,
+                         const float *point,
+                         float *out)
+{
+    float tmp[2];
+
+    BH_Vec2fScale(line, BH_LineDistance(line, point), tmp);
+    BH_Vec2fSub(point, tmp, out);
+}
diff --git a/src/Math/Mat3f.c b/src/Math/Mat3f.c
new file mode 100644
index 0000000..e78b646
--- /dev/null
+++ b/src/Math/Mat3f.c
@@ -0,0 +1,215 @@
+#include <BH/Math.h>
+#include <string.h>
+#include <math.h>
+
+
+#define EPSILON 0.00001f
+#define PI      3.14159265358979323846f
+
+
+void BH_Mat3fIdentity(float *out)
+{
+    static const float ident[9] =
+    {
+        1.0f, 0.0f, 0.0f,
+        0.0f, 1.0f, 0.0f,
+        0.0f, 0.0f, 1.0f
+    };
+
+    memcpy(out, ident, sizeof(ident));
+}
+
+
+void BH_Mat3fAdd(const float *a,
+                 const float *b,
+                 float *out)
+{
+    BH_Vec3fAdd(&a[0], &b[0], &out[0]);
+    BH_Vec3fAdd(&a[3], &b[3], &out[3]);
+    BH_Vec3fAdd(&a[6], &b[6], &out[6]);
+}
+
+
+void BH_Mat3fSub(const float *a,
+                 const float *b,
+                 float *out)
+{
+    BH_Vec3fSub(&a[0], &b[0], &out[0]);
+    BH_Vec3fSub(&a[3], &b[3], &out[3]);
+    BH_Vec3fSub(&a[6], &b[6], &out[6]);
+}
+
+
+void BH_Mat3fMul(const float *a,
+                 const float *b,
+                 float *out)
+{
+    float tmp[9], row[3];
+
+    row[0] = row[1] = row[2] = b[0]; BH_Vec3fMul(&a[0], row, &tmp[0]);
+    row[0] = row[1] = row[2] = b[1]; BH_Vec3fMulAdd(&a[3], row, &tmp[0], &tmp[0]);
+    row[0] = row[1] = row[2] = b[2]; BH_Vec3fMulAdd(&a[6], row, &tmp[0], &tmp[0]);
+
+    row[0] = row[1] = row[2] = b[3]; BH_Vec3fMul(&a[0], row, &tmp[3]);
+    row[0] = row[1] = row[2] = b[4]; BH_Vec3fMulAdd(&a[3], row, &tmp[3], &tmp[3]);
+    row[0] = row[1] = row[2] = b[5]; BH_Vec3fMulAdd(&a[6], row, &tmp[3], &tmp[3]);
+
+    row[0] = row[1] = row[2] = b[6]; BH_Vec3fMul(&a[0], row, &tmp[6]);
+    row[0] = row[1] = row[2] = b[7]; BH_Vec3fMulAdd(&a[3], row, &tmp[6], &tmp[6]);
+    row[0] = row[1] = row[2] = b[8]; BH_Vec3fMulAdd(&a[6], row, &tmp[6], &tmp[6]);
+
+    memcpy(out, tmp, sizeof(tmp));
+}
+
+
+void BH_Mat3fScale(const float *a,
+                   float b,
+                   float *out)
+{
+    BH_Vec3fScale(&a[0], b, &out[0]);
+    BH_Vec3fScale(&a[3], b, &out[3]);
+    BH_Vec3fScale(&a[6], b, &out[6]);
+}
+
+
+void BH_Mat3fTranspose(const float *in,
+                       float *out)
+{
+    float tmp[9];
+
+    tmp[0] = in[0]; tmp[3] = in[1]; tmp[6] = in[2];
+    tmp[1] = in[3]; tmp[4] = in[4]; tmp[7] = in[5];
+    tmp[2] = in[6]; tmp[5] = in[7]; tmp[8] = in[8];
+
+    memcpy(out, tmp, sizeof(tmp));
+}
+
+
+float BH_Mat3fTrace(const float *in)
+{
+    return in[0] + in[4] + in[8];
+}
+
+
+float BH_Mat3fDet(const float *in)
+{
+    float a, b, c, result;
+
+    a = in[4] * in[8] - in[7] * in[5];
+    b = in[1] * in[8] - in[7] * in[2];
+    c = in[1] * in[5] - in[4] * in[2];
+
+    result = 0.0f;
+    result += in[0] * a;
+    result -= in[3] * b;
+    result += in[6] * c;
+
+    return result;
+}
+
+
+int BH_Mat3fInverse(const float *in,
+                    float *out)
+{
+    float a, b, c, det;
+    float tmp[16];
+
+    a = in[4] * in[8] - in[7] * in[5];
+    b = in[1] * in[8] - in[7] * in[2];
+    c = in[1] * in[5] - in[4] * in[2];
+
+    tmp[0] =  a;
+    tmp[1] = -b;
+    tmp[2] =  c;
+
+    det = 0.0f;
+    det += in[0] * tmp[0];
+    det += in[3] * tmp[1];
+    det += in[6] * tmp[2];
+
+    if (fabsf(det) < EPSILON)
+        return BH_ERROR;
+
+    a = in[3] * in[8] - in[6] * in[5];
+    b = in[0] * in[8] - in[6] * in[2];
+    c = in[0] * in[5] - in[3] * in[2];
+
+    tmp[3] = -a;
+    tmp[4] =  b;
+    tmp[5] = -c;
+
+    a = in[3] * in[7] - in[6] * in[4];
+    b = in[0] * in[7] - in[6] * in[1];
+    c = in[0] * in[4] - in[3] * in[1];
+
+    tmp[6] =  a;
+    tmp[7] = -b;
+    tmp[8] =  c;
+
+    BH_Mat3fScale(tmp, 1.0f / det, out);
+    return BH_OK;
+}
+
+
+void BH_Mat3fFromScale(float x,
+                       float y,
+                       float *out)
+{
+    BH_Mat3fIdentity(out);
+    out[0] = x;
+    out[4] = y;
+}
+
+
+void BH_Mat3fFromTranslation(float x,
+                             float y,
+                             float *out)
+{
+    BH_Mat3fIdentity(out);
+    out[6] = x;
+    out[7] = y;
+}
+
+
+void BH_Mat3fFromRotation(float angle,
+                          float *out)
+{
+    float c, s;
+
+    c = cosf(angle);
+    s = sinf(angle);
+
+    BH_Mat3fIdentity(out);
+    out[0] = c;
+    out[1] = s;
+    out[3] = -s;
+    out[4] = c;
+}
+
+
+void BH_Mat3fApplyVec3f(float *a,
+                        float *b,
+                        float *out)
+{
+    float tmp[3], row[3];
+
+    row[0] = row[1] = row[2] = b[0]; BH_Vec3fMul(&a[0], row, tmp);
+    row[0] = row[1] = row[2] = b[1]; BH_Vec3fMulAdd(&a[3], row, tmp, tmp);
+    row[0] = row[1] = row[2] = b[2]; BH_Vec3fMulAdd(&a[6], row, tmp, tmp);
+
+    memcpy(out, tmp, sizeof(tmp));
+}
+
+
+void BH_Mat3fApplyVec2f(float *a,
+                        float *b,
+                        float *out)
+{
+    float tmp[3], row[3];
+
+    row[0] = row[1] = row[2] = b[0]; BH_Vec3fMul(&a[0], row, tmp);
+    row[0] = row[1] = row[2] = b[1]; BH_Vec3fMulAdd(&a[3], row, tmp, tmp);
+    row[0] = row[1] = row[2] = 1.0f; BH_Vec3fMulAdd(&a[6], row, tmp, tmp);
+
+    memcpy(out, tmp, sizeof(float) * 2);
+}
diff --git a/src/Math/Mat4f.c b/src/Math/Mat4f.c
new file mode 100644
index 0000000..c7f6d14
--- /dev/null
+++ b/src/Math/Mat4f.c
@@ -0,0 +1,446 @@
+#include <BH/Math.h>
+#include <string.h>
+#include <math.h>
+
+
+#define EPSILON 0.00001f
+#define PI      3.14159265358979323846f
+
+
+void BH_Mat4fIdentity(float *out)
+{
+    const float ident[16] =
+    {
+        1.0f, 0.0f, 0.0f, 0.0f,
+        0.0f, 1.0f, 0.0f, 0.0f,
+        0.0f, 0.0f, 1.0f, 0.0f,
+        0.0f, 0.0f, 0.0f, 1.0f
+    };
+
+    memcpy(out, ident, sizeof(ident));
+}
+
+
+void BH_Mat4fAdd(const float *a,
+                 const float *b,
+                 float *out)
+{
+    BH_Vec4fAdd(&a[0], &b[0], &out[0]);
+    BH_Vec4fAdd(&a[4], &b[4], &out[4]);
+    BH_Vec4fAdd(&a[8], &b[8], &out[8]);
+    BH_Vec4fAdd(&a[12], &b[12], &out[12]);
+}
+
+
+void BH_Mat4fSub(const float *a,
+                 const float *b,
+                 float *out)
+{
+    BH_Vec4fSub(&a[0], &b[0], &out[0]);
+    BH_Vec4fSub(&a[4], &b[4], &out[4]);
+    BH_Vec4fSub(&a[8], &b[8], &out[8]);
+    BH_Vec4fSub(&a[12], &b[12], &out[12]);
+}
+
+
+void BH_Mat4fMul(const float *a,
+                 const float *b,
+                 float *out)
+{
+    float tmp[16], row[4];
+
+    row[0] = row[1] = row[2] = row[3] = b[0]; BH_Vec4fMul(&a[0], row, &tmp[0]);
+    row[0] = row[1] = row[2] = row[3] = b[1]; BH_Vec4fMulAdd(&a[4], row, &tmp[0], &tmp[0]);
+    row[0] = row[1] = row[2] = row[3] = b[2]; BH_Vec4fMulAdd(&a[8], row, &tmp[0], &tmp[0]);
+    row[0] = row[1] = row[2] = row[3] = b[3]; BH_Vec4fMulAdd(&a[12], row, &tmp[0], &tmp[0]);
+
+    row[0] = row[1] = row[2] = row[3] = b[4]; BH_Vec4fMul(&a[0], row, &tmp[4]);
+    row[0] = row[1] = row[2] = row[3] = b[5]; BH_Vec4fMulAdd(&a[4], row, &tmp[4], &tmp[4]);
+    row[0] = row[1] = row[2] = row[3] = b[6]; BH_Vec4fMulAdd(&a[8], row, &tmp[4], &tmp[4]);
+    row[0] = row[1] = row[2] = row[3] = b[7]; BH_Vec4fMulAdd(&a[12], row, &tmp[4], &tmp[4]);
+
+    row[0] = row[1] = row[2] = row[3] = b[8]; BH_Vec4fMul(&a[0], row, &tmp[8]);
+    row[0] = row[1] = row[2] = row[3] = b[9]; BH_Vec4fMulAdd(&a[4], row, &tmp[8], &tmp[8]);
+    row[0] = row[1] = row[2] = row[3] = b[10]; BH_Vec4fMulAdd(&a[8], row, &tmp[8], &tmp[8]);
+    row[0] = row[1] = row[2] = row[3] = b[11]; BH_Vec4fMulAdd(&a[12], row, &tmp[8], &tmp[8]);
+
+    row[0] = row[1] = row[2] = row[3] = b[12]; BH_Vec4fMul(&a[0], row, &tmp[12]);
+    row[0] = row[1] = row[2] = row[3] = b[13]; BH_Vec4fMulAdd(&a[4], row, &tmp[12], &tmp[12]);
+    row[0] = row[1] = row[2] = row[3] = b[14]; BH_Vec4fMulAdd(&a[8], row, &tmp[12], &tmp[12]);
+    row[0] = row[1] = row[2] = row[3] = b[15]; BH_Vec4fMulAdd(&a[12], row, &tmp[12], &tmp[12]);
+
+    memcpy(out, tmp, sizeof(tmp));
+}
+
+
+void BH_Mat4fScale(const float *a,
+                   float b,
+                   float *out)
+{
+    BH_Vec4fScale(&a[0], b, &out[0]);
+    BH_Vec4fScale(&a[4], b, &out[4]);
+    BH_Vec4fScale(&a[8], b, &out[8]);
+    BH_Vec4fScale(&a[12], b, &out[12]);
+}
+
+
+void BH_Mat4fTranspose(const float *in,
+                       float *out)
+{
+    float tmp[16];
+
+    tmp[0] = in[0]; tmp[4] = in[1]; tmp[8] = in[2]; tmp[12] = in[3];
+    tmp[1] = in[4]; tmp[5] = in[5]; tmp[9] = in[6]; tmp[13] = in[7];
+    tmp[2] = in[8]; tmp[6] = in[9]; tmp[10] = in[10]; tmp[14] = in[11];
+    tmp[3] = in[12]; tmp[7] = in[13]; tmp[11] = in[14]; tmp[15] = in[15];
+
+    memcpy(out, tmp, sizeof(tmp));
+}
+
+
+float BH_Mat4fTrace(const float *in)
+{
+    return in[0] + in[5] + in[10] + in[15];
+}
+
+
+float BH_Mat4fDet(const float *in)
+{
+    float a, b, c, d, e, f, result;
+
+    a = in[2]  * in[7]  - in[3]  * in[6];
+    b = in[2]  * in[11] - in[3]  * in[10];
+    c = in[2]  * in[15] - in[3]  * in[14];
+    d = in[6]  * in[11] - in[7]  * in[10];
+    e = in[6]  * in[15] - in[7]  * in[14];
+    f = in[10] * in[15] - in[11] * in[14];
+
+    result = 0.0f;
+    result += in[0]  * (in[5] * f - in[9] * e + in[13] * d);
+    result -= in[4]  * (in[1] * f - in[9] * c + in[13] * b);
+    result += in[8]  * (in[1] * e - in[5] * c + in[13] * a);
+    result -= in[12] * (in[1] * d - in[5] * b + in[9]  * a);
+
+    return result;
+}
+
+
+int BH_Mat4fInverse(const float *in,
+                    float *out)
+{
+    float a, b, c, d, e, f, det;
+    float tmp[16];
+
+    a = in[2]  * in[7]  - in[3]  * in[6];
+    b = in[2]  * in[11] - in[3]  * in[10];
+    c = in[2]  * in[15] - in[3]  * in[14];
+    d = in[6]  * in[11] - in[7]  * in[10];
+    e = in[6]  * in[15] - in[7]  * in[14];
+    f = in[10] * in[15] - in[11] * in[14];
+
+    tmp[0] =  (in[5] * f - in[9] * e + in[13] * d);
+    tmp[1] = -(in[1] * f - in[9] * c + in[13] * b);
+    tmp[2] =  (in[1] * e - in[5] * c + in[13] * a);
+    tmp[3] = -(in[1] * d - in[5] * b + in[9]  * a);
+
+    det = 0.0f;
+    det += in[0]  * tmp[0];
+    det += in[4]  * tmp[1];
+    det += in[8]  * tmp[2];
+    det += in[12] * tmp[3];
+
+    if (fabsf(det) < EPSILON)
+        return BH_ERROR;
+
+    tmp[4] = -(in[4] * f - in[8] * e + in[12] * d);
+    tmp[5] =  (in[0] * f - in[8] * c + in[12] * b);
+    tmp[6] = -(in[0] * e - in[4] * c + in[12] * a);
+    tmp[7] =  (in[0] * d - in[4] * b + in[8]  * a);
+
+    a = in[1] * in[7]  - in[3]  * in[5];
+    b = in[1] * in[11] - in[3]  * in[9];
+    c = in[1] * in[15] - in[3]  * in[13];
+    d = in[5] * in[11] - in[7]  * in[9];
+    e = in[5] * in[15] - in[7]  * in[13];
+    f = in[9] * in[15] - in[11] * in[13];
+
+    tmp[8] =   (in[4] * f - in[8] * e + in[12] * d);
+    tmp[9] =  -(in[0] * f - in[8] * c + in[12] * b);
+    tmp[10] =  (in[0] * e - in[4] * c + in[12] * a);
+    tmp[11] = -(in[0] * d - in[4] * b + in[8]  * a);
+
+    a = in[1] * in[6]  - in[2]  * in[5];
+    b = in[1] * in[10] - in[2]  * in[9];
+    c = in[1] * in[14] - in[2]  * in[13];
+    d = in[5] * in[10] - in[6]  * in[9];
+    e = in[5] * in[14] - in[6]  * in[13];
+    f = in[9] * in[14] - in[10] * in[13];
+
+    tmp[12] = -(in[4] * f - in[8] * e + in[12] * d);
+    tmp[13] =  (in[0] * f - in[8] * c + in[12] * b);
+    tmp[14] = -(in[0] * e - in[4] * c + in[12] * a);
+    tmp[15] =  (in[0] * d - in[4] * b + in[8]  * a);
+
+    BH_Mat4fScale(tmp, 1.0f / det, out);
+    return BH_OK;
+}
+
+
+void BH_Mat4fFromScale(float x,
+                       float y,
+                       float z,
+                       float *out)
+{
+    BH_Mat4fIdentity(out);
+    out[0] = x;
+    out[5] = y;
+    out[10] = z;
+}
+
+
+void BH_Mat4fFromTranslation(float x,
+                             float y,
+                             float z,
+                             float *out)
+{
+    BH_Mat4fIdentity(out);
+    out[12] = x;
+    out[13] = y;
+    out[14] = z;
+}
+
+
+void BH_Mat4fFromRotationX(float angle,
+                           float *out)
+{
+    float c, s;
+
+    c = cosf(angle);
+    s = sinf(angle);
+
+    BH_Mat4fIdentity(out);
+    out[5] = c;
+    out[6] = s;
+    out[9] = -s;
+    out[10] = c;
+}
+
+
+void BH_Mat4fFromRotationY(float angle,
+                           float *out)
+{
+    float c, s;
+
+    c = cosf(angle);
+    s = sinf(angle);
+
+    BH_Mat4fIdentity(out);
+    out[0] = c;
+    out[2] = -s;
+    out[8] = s;
+    out[10] = c;
+}
+
+
+void BH_Mat4fFromRotationZ(float angle,
+                           float *out)
+{
+    float c, s;
+
+    c = cosf(angle);
+    s = sinf(angle);
+
+    BH_Mat4fIdentity(out);
+    out[0] = c;
+    out[1] = s;
+    out[4] = -s;
+    out[5] = c;
+}
+
+
+void BH_Mat4fFromAxis(const float *axis,
+                      float angle,
+                      float *out)
+{
+    float x, y, z, length;
+    float c, s, moc, xx, xy, xz, yy, yz, zz;
+
+    length = BH_Vec3fLength(axis);
+    BH_Mat4fIdentity(out);
+
+    if (fabsf(length) < EPSILON)
+        return;
+
+    x = axis[0] / length;
+    y = axis[1] / length;
+    z = axis[2] / length;
+
+    c = cosf(angle);
+    s = sinf(angle);
+    moc = 1.0f - c;
+
+    xx = x * x;
+    xy = x * y;
+    xz = x * z;
+    yy = y * y;
+    yz = y * z;
+    zz = z * z;
+
+    out[0] = c + xx * moc;
+    out[1] = xy * moc + z * s;
+    out[2] = xz * moc - y * s;
+
+    out[4] = xy * moc - z * s;
+    out[5] = c + yy * moc;
+    out[6] = yz * moc + x * s;
+
+    out[8] = xz * moc + y * s;
+    out[9] = yz * moc - x * s;
+    out[10] = c + zz * moc;
+}
+
+
+void BH_Mat4fFromEuler(float roll,
+                       float pitch,
+                       float yaw,
+                       float *out)
+{
+    float rs, rc, ys, yc, ps, pc;
+
+    rs = sinf(roll);
+    rc = cosf(roll);
+    ps = sinf(pitch);
+    pc = cosf(pitch);
+    ys = sinf(yaw);
+    yc = cosf(yaw);
+
+    BH_Mat4fIdentity(out);
+    out[0] = pc * yc;
+    out[1] = pc * ys;
+    out[2] = -ps;
+    out[4] = ps * rs * yc - rc * ys;
+    out[5] = ps * rs * ys + rc * yc;
+    out[6] = pc * rs;
+    out[8] = rs * ys + ps * rc * yc;
+    out[9] = ps * rc * ys - rs * yc;
+    out[10] = pc * rc;
+}
+
+
+void BH_Mat4fFromQuat4f(const float *in,
+                        float *out)
+{
+    BH_Quat4fToMat4f(in, out);
+}
+
+
+void BH_Mat4fFromOrtho(float xMin,
+                       float xMax,
+                       float yMin,
+                       float yMax,
+                       float zMin,
+                       float zMax,
+                       float *out)
+{
+    float dx, dy, dz;
+
+    dx = xMax - xMin;
+    dy = yMax - yMin;
+    dz = zMax - zMin;
+
+    BH_Mat4fIdentity(out);
+
+    out[0] = 2.0f / dx;
+    out[5] = 2.0f / dy;
+    out[10] = -2.0f / dz;
+    out[12] = -(xMax + xMin) / dx;
+    out[13] = -(yMax + yMin) / dy;
+    out[14] = -(zMax + zMin) / dz;
+}
+
+
+void BH_Mat4fFromFrustum(float fov,
+                         float aspect,
+                         float zMin,
+                         float zMax,
+                         float *out)
+{
+    float t, dz;
+
+    dz = zMax - zMin;
+    t = tanf(fov / 2.0f);
+
+    BH_Mat4fIdentity(out);
+
+    out[0] = 1.0f / (aspect * t);
+    out[5] = 1.0f / t;
+    out[10] = -(zMax + zMin) / dz;
+    out[11] = -1.0f;
+    out[14] = -(2.0f * zMax * zMin) / dz;
+    out[15] = 0.0f;
+}
+
+
+void BH_Mat4fFromLookAt(const float *position,
+                        const float *at,
+                        const float *up,
+                        float *out)
+{
+    float cameraDir[3], cameraRight[3], cameraUp[3];
+
+    BH_Vec3fSub(position, at, cameraDir);
+    BH_Vec3fNormal(cameraDir, cameraDir);
+    BH_Vec3fCross(up, cameraDir, cameraRight);
+    BH_Vec3fNormal(cameraRight, cameraRight);
+    BH_Vec3fCross(cameraDir, cameraRight, cameraUp);
+
+    out[0] = cameraRight[0];
+    out[1] = cameraUp[0];
+    out[2] = cameraDir[0];
+    out[3] = 0.0f;
+
+    out[4] = cameraRight[1];
+    out[5] = cameraUp[1];
+    out[6] = cameraDir[1];
+    out[7] = 0.0f;
+
+    out[8] = cameraRight[2];
+    out[9] = cameraUp[2];
+    out[10] = cameraDir[2];
+    out[11] = 0.0f;
+
+    out[12] = -BH_Vec3fDot(cameraRight, position);
+    out[13] = -BH_Vec3fDot(cameraUp, position);
+    out[14] = -BH_Vec3fDot(cameraDir, position);
+    out[15] = 1.0f;
+}
+
+
+void BH_Mat4fApplyVec4f(const float *a,
+                        const float *b,
+                        float *out)
+{
+    float tmp[4], row[4];
+
+    row[0] = row[1] = row[2] = row[3] = b[0]; BH_Vec4fMul(&a[0], row, tmp);
+    row[0] = row[1] = row[2] = row[3] = b[1]; BH_Vec4fMulAdd(&a[4], row, tmp, tmp);
+    row[0] = row[1] = row[2] = row[3] = b[2]; BH_Vec4fMulAdd(&a[8], row, tmp, tmp);
+    row[0] = row[1] = row[2] = row[3] = b[3]; BH_Vec4fMulAdd(&a[12], row, tmp, tmp);
+
+    memcpy(out, tmp, sizeof(tmp));
+}
+
+
+void BH_Mat4fApplyVec3f(const float *a,
+                        const float *b,
+                        float *out)
+{
+    float tmp[4], row[4];
+
+    row[0] = row[1] = row[2] = row[3] = b[0]; BH_Vec4fMul(&a[0], row, tmp);
+    row[0] = row[1] = row[2] = row[3] = b[1]; BH_Vec4fMulAdd(&a[4], row, tmp, tmp);
+    row[0] = row[1] = row[2] = row[3] = b[2]; BH_Vec4fMulAdd(&a[8], row, tmp, tmp);
+    row[0] = row[1] = row[2] = row[3] = 1.0f; BH_Vec4fMulAdd(&a[12], row, tmp, tmp);
+
+    memcpy(out, tmp, sizeof(float) * 3);
+}
diff --git a/src/Math/Misc.c b/src/Math/Misc.c
new file mode 100644
index 0000000..3d09c2f
--- /dev/null
+++ b/src/Math/Misc.c
@@ -0,0 +1,33 @@
+#include <BH/Math.h>
+
+
+float BH_Lerpf(float a, float b, float t)
+{
+    return a + (b - a) * t;
+}
+
+
+void BH_Triangle3fBarycentric(const float *a,
+                              const float *b,
+                              const float *c,
+                              const float *point,
+                              float *out)
+{
+    float tmp1[3], tmp2[3], tmp3[3];
+    float t11, t12, t22, t31, t32, denom;
+
+    BH_Vec3fSub(b, a, tmp1);
+    BH_Vec3fSub(c, a, tmp2);
+    BH_Vec3fSub(point, a, tmp3);
+
+    t11 = BH_Vec3fDot(tmp1, tmp1);
+    t12 = BH_Vec3fDot(tmp1, tmp2);
+    t22 = BH_Vec3fDot(tmp2, tmp2);
+    t31 = BH_Vec3fDot(tmp3, tmp1);
+    t32 = BH_Vec3fDot(tmp3, tmp2);
+    denom = 1.0f / (t11 * t22 - t12 * t12);
+
+    out[1] = (t22 * t31 - t12 * t32) * denom;
+    out[2] = (t11 * t32 - t12 * t31) * denom;
+    out[0] = 1.0f - out[1] - out[2];
+}
diff --git a/src/Math/Plane.c b/src/Math/Plane.c
new file mode 100644
index 0000000..23711c5
--- /dev/null
+++ b/src/Math/Plane.c
@@ -0,0 +1,43 @@
+#include <BH/Math.h>
+#include <string.h>
+
+
+#define EPSILON 0.00001f
+#define PI      3.14159265358979323846f
+
+
+int BH_PlaneFromPoints(const float *a,
+                       const float *b,
+                       const float *c,
+                       float *out)
+{
+    float tmp1[3], tmp2[3];
+
+    BH_Vec3fSub(b, a, tmp1);
+    BH_Vec3fSub(c, a, tmp2);
+    BH_Vec3fCross(tmp2, tmp1, tmp1);
+    if (BH_Vec3fNormalEx(tmp1, tmp1) < EPSILON)
+        return BH_ERROR;
+
+    out[3] = BH_Vec3fDot(a, tmp1);
+    memcpy(out, tmp1, sizeof(tmp1));
+    return BH_OK;
+}
+
+
+float BH_PlaneDistance(const float *plane,
+                       const float *point)
+{
+    return BH_Vec3fDot(plane, point) - plane[3];
+}
+
+
+void BH_PlaneClosestPoint(const float *plane,
+                          const float *point,
+                          float *out)
+{
+    float tmp[3];
+
+    BH_Vec3fScale(plane, BH_PlaneDistance(plane, point), tmp);
+    BH_Vec3fSub(point, tmp, out);
+}
diff --git a/src/Math/Quat4f.c b/src/Math/Quat4f.c
new file mode 100644
index 0000000..45e9b32
--- /dev/null
+++ b/src/Math/Quat4f.c
@@ -0,0 +1,207 @@
+#include <BH/Math.h>
+#include <string.h>
+#include <math.h>
+
+
+#define EPSILON 0.00001f
+#define PI      3.14159265358979323846f
+
+
+void BH_Quat4fIdentity(float *out)
+{
+    static const float ident[4] = {0.0f, 0.0f, 0.0f, 1.0f};
+    memcpy(out, ident, sizeof(ident));
+}
+
+
+void BH_Quat4fConjugate(const float *in,
+                        float *out)
+{
+    out[0] = -in[0];
+    out[1] = -in[1];
+    out[2] = -in[2];
+    out[3] = in[3];
+}
+
+
+void BH_Quat4fInverse(const float *in,
+                      float *out)
+{
+    float dot;
+
+    dot = BH_Vec4fDot(in, in);
+    BH_Quat4fConjugate(in, out);
+    BH_Quat4fScale(out, 1.0f / dot, out);
+}
+
+
+void BH_Quat4fMul(const float *a,
+                  const float *b,
+                  float *out)
+{
+    float tmp1[4], tmp2[4], tmp3[4];
+    float w;
+
+    w = a[3] * b[3] - BH_Vec3fDot(a, b);
+    BH_Vec4fScale(a, b[3], tmp1);
+    BH_Vec4fScale(b, a[3], tmp2);
+    BH_Vec3fCross(a, b, tmp3);
+    BH_Vec4fAdd(tmp1, tmp2, out);
+    BH_Vec4fAdd(tmp3, out, out);
+    out[3] = w;
+}
+
+
+void BH_Quat4fSlerp(const float *a,
+                    const float *b,
+                    float t,
+                    float *out)
+{
+    float angle, denom;
+    float from[4], to[4];
+
+    angle = acosf(BH_Vec4fDot(a, b));
+    if (fabsf(angle) < EPSILON)
+    {
+        BH_Vec4fLerp(a, b, t, out);
+        return;
+    }
+
+    denom = 1.0f / sinf(angle);
+    BH_Vec4fScale(a, sinf((1 - t) * angle) * denom, from);
+    BH_Vec4fScale(b, sinf(t * angle) * denom, to);
+    BH_Vec4fAdd(from, to, out);
+}
+
+
+void BH_Quat4fFromEuler(float roll,
+                        float pitch,
+                        float yaw,
+                        float *out)
+{
+    float cr, cp, cy, sr, sp, sy;
+
+    cr = cosf(roll / 2.0f);
+    cp = cosf(pitch / 2.0f);
+    cy = cosf(yaw / 2.0f);
+    sr = sinf(roll / 2.0f);
+    sp = sinf(pitch / 2.0f);
+    sy = sinf(yaw / 2.0f);
+
+    out[0] = sr * cp * cy - cr * sp * sy;
+    out[1] = cr * sp * cy + sr * cp * sy;
+    out[2] = cr * cp * sy - sr * sp * cy;
+    out[3] = cr * cp * cy + sr * sp * sy;
+}
+
+
+void BH_Quat4fFromAxis(const float *axis,
+                       float angle,
+                       float *out)
+{
+    float c, s;
+
+    c = cosf(angle / 2.0f);
+    s = sinf(angle / 2.0f);
+
+    out[0] = axis[0] * s;
+    out[1] = axis[1] * s;
+    out[2] = axis[2] * s;
+    out[3] = c;
+}
+
+
+void BH_Quat4fToEuler(const float *in,
+                      float *roll,
+                      float *pitch,
+                      float *yaw)
+{
+    float ww, xw, yw, zw, xx, xy, xz, yy, yz, zz, angle;
+
+    xx = in[0] * in[0];
+    xy = in[0] * in[1];
+    xz = in[0] * in[2];
+    xw = in[0] * in[3];
+    yy = in[1] * in[1];
+    yz = in[1] * in[2];
+    yw = in[1] * in[3];
+    zz = in[2] * in[2];
+    zw = in[2] * in[3];
+    ww = in[3] * in[3];
+
+    angle = 2.0f * (yw - xz);
+    if (angle > 1.0f)
+        angle = 1.0f;
+    if (angle < -1.0f)
+        angle = -1.0f;
+
+    *pitch = asinf(angle);
+
+    if (fabsf(*pitch - (PI / 2.0f)) < EPSILON)
+    {
+        *roll = 0.0f;
+        *yaw = -2.0f * atan2f(in[0], in[3]);
+    }
+    else if (fabsf(*pitch - (PI / -2.0f)) < EPSILON)
+    {
+        *roll = 0.0f;
+        *yaw = 2.0f * atan2f(in[0], in[3]);
+    }
+    else
+    {
+        *roll = atan2f(2.0f * (xw + yz), ww - xx - yy + zz);
+        *yaw = atan2f(2.0f * (zw + xy), ww + xx - yy - zz);
+    }
+}
+
+
+void BH_Quat4fToAxis(const float *in,
+                     float *axis,
+                     float *angle)
+{
+    *angle = 2.0f * acosf(in[3]);
+
+    if (fabsf(*angle) < EPSILON)
+    {
+        axis[0] = 1.0f;
+        axis[1] = 0.0f;
+        axis[2] = 0.0f;
+    }
+    else
+    {
+        float tmp;
+
+        tmp = sqrtf(1.0f - in[3] * in[3]);
+        axis[0] = in[0] / tmp;
+        axis[1] = in[1] / tmp;
+        axis[2] = in[2] / tmp;
+    }
+}
+
+
+void BH_Quat4fToMat4f(const float *in,
+                      float *out)
+{
+    float xx, xy, xz, xw, yy, yz, yw, zz, zw;
+
+    xx = in[0] * in[0];
+    xy = in[0] * in[1];
+    xz = in[0] * in[2];
+    xw = in[0] * in[3];
+    yy = in[1] * in[1];
+    yz = in[1] * in[2];
+    yw = in[1] * in[3];
+    zz = in[2] * in[2];
+    zw = in[2] * in[3];
+
+    BH_Mat4fIdentity(out);
+    out[0] = 1.0f - 2.0f * (yy + zz);
+    out[1] = 2.0f * (xy + zw);
+    out[2] = 2.0f * (xz - yw);
+    out[4] = 2.0f * (xy - zw);
+    out[5] = 1.0f - 2.0f * (xx + zz);
+    out[6] = 2.0f * (yz + xw);
+    out[8] = 2.0f * (xz + yw);
+    out[9] = 2.0f * (yz - xw);
+    out[10] = 1.0f - 2.0f * (xx + yy);
+}
diff --git a/src/Math/Ray2f.c b/src/Math/Ray2f.c
new file mode 100644
index 0000000..ea2572b
--- /dev/null
+++ b/src/Math/Ray2f.c
@@ -0,0 +1,236 @@
+#include <BH/Math.h>
+#include <string.h>
+#include <math.h>
+
+#define EPSILON 0.00001f
+#define PI      3.14159265358979323846f
+
+
+int BH_Ray2fIntersectLine(const float *start,
+                          const float *direction,
+                          const float *line,
+                          float *t,
+                          float *out)
+{
+    float tmp1[2];
+    float denom, time;
+
+    /* Calculate intersection time */
+    denom = BH_Vec2fDot(direction, line);
+    time = (line[2] - BH_Vec2fDot(line, start)) / denom;
+
+    /* Check for ray/plane parallel to each other or point is behing the ray. */
+    if (fabsf(denom) < EPSILON || time < 0.0f)
+        return BH_ERROR;
+
+    /* Compute intersection point */
+    BH_Vec2fScale(direction, time, tmp1);
+    BH_Vec2fAdd(start, tmp1, out);
+    *t = time;
+    return BH_OK;
+}
+
+
+int BH_Ray2fIntersectTime(const float *aStart,
+                          const float *aDirection,
+                          const float *bStart,
+                          const float *bDirection,
+                          float *time1,
+                          float *time2)
+{
+    float tmp1[2], tmp2[2], tmp3[2];
+    float denom;
+
+    /* Rotate directions by 90 degrees and caluclate denom */
+    tmp1[0] = -aDirection[1]; tmp1[1] = aDirection[0];
+    tmp2[0] = -bDirection[1]; tmp2[1] = bDirection[0];
+    denom = BH_Vec2fDot(tmp1, bDirection);
+
+    if (fabsf(denom) < EPSILON)
+        return BH_ERROR;
+
+    /* Calculate segments offset and intersection times */
+    BH_Vec2fSub(aStart, bStart, tmp3);
+    *time1 = BH_Vec2fDot(tmp3, tmp2) / denom;
+    *time2 = BH_Vec2fDot(tmp3, tmp1) / denom;
+
+    return BH_OK;
+}
+
+
+int BH_Ray2fIntersectRay(const float *aStart,
+                         const float *aDirection,
+                         const float *bStart,
+                         const float *bDirection,
+                         float *t,
+                         float *out)
+{
+    float tmp[2];
+    float time1, time2;
+
+    if (BH_Ray2fIntersectTime(aStart, aDirection, bStart, bDirection, &time1, &time2))
+        return BH_ERROR;
+
+    if (time1 < 0.0f || time2 < 0.0f)
+        return BH_ERROR;
+
+    BH_Vec2fScale(aDirection, time1, tmp);
+    BH_Vec2fAdd(aStart, tmp, out);
+    *t = time1;
+    return BH_OK;
+}
+
+
+int BH_Ray2fIntersectSegment(const float *aStart,
+                             const float *aDirection,
+                             const float *bStart,
+                             const float *bEnd,
+                             float *t,
+                             float *out)
+{
+    float tmp[2];
+    float time1, time2;
+
+    BH_Vec2fSub(bEnd, bStart, tmp);
+    if (BH_Ray2fIntersectTime(aStart, aDirection, bStart, tmp, &time1, &time2))
+        return BH_ERROR;
+
+    if (time1 < 0.0f || time2 < 0.0f || time2 > 1.0f)
+        return BH_ERROR;
+
+    BH_Vec2fScale(aDirection, time1, tmp);
+    BH_Vec2fAdd(aStart, tmp, out);
+    *t = time1;
+    return BH_OK;
+}
+
+
+int BH_Segment2fIntersectLine(const float *start,
+                              const float *end,
+                              const float *line,
+                              float *t,
+                              float *out)
+{
+    float tmp[2];
+    float denom, time;
+
+    /* Calculate intersection time */
+    BH_Vec2fSub(end, start, tmp);
+    denom = BH_Vec2fDot(tmp, line);
+    time = (line[2] - BH_Vec2fDot(line, start)) / denom;
+
+    /* Check for ray/plane parallel to each other or point is behing the ray. */
+    if (fabsf(denom) < EPSILON || time < 0.0f || time > 1.0f)
+        return BH_ERROR;
+
+    /* Compute intersection point */
+    BH_Vec2fScale(tmp, time, tmp);
+    BH_Vec2fAdd(start, tmp, out);
+    *t = time;
+    return BH_OK;
+}
+
+
+int BH_Segment2fIntersectSegment(const float *aStart,
+                                 const float *aEnd,
+                                 const float *bStart,
+                                 const float *bEnd,
+                                 float *t,
+                                 float *out)
+{
+    float tmp1[2], tmp2[2];
+    float time1, time2;
+
+    BH_Vec2fSub(aEnd, aStart, tmp1);
+    BH_Vec2fSub(bEnd, bStart, tmp2);
+    if (BH_Ray2fIntersectTime(aStart, tmp1, bStart, tmp2, &time1, &time2))
+        return BH_ERROR;
+
+    if (time1 < 0.0f || time1 > 1.0f || time2 < 0.0f || time2 > 1.0f)
+        return BH_ERROR;
+
+    BH_Vec2fLerp(aStart, aEnd, time1, out);
+    *t = time1;
+
+    return BH_OK;
+}
+
+int BH_Ray2fIntersectBox2f(const float *aStart,
+                           const float *aDirection,
+                           const float *bMin,
+                           const float *bMax,
+                           float *t,
+                           float *out)
+{
+    float timeNear, timeFar, hitNear, hitFar, denom, tmp;
+    int i;
+
+    timeNear = -1.0f / 0.0f;
+    timeFar  =  1.0f / 0.0f;
+
+    /* Check if origin inside box */
+    if (!BH_Box2fContains(bMin, bMax, aStart))
+    {
+        memcpy(out, aStart, sizeof(float) * 2);
+        *t = 0.0f;
+        return BH_OK;
+    }
+
+    /* Check each axis for the minimal and maximum intersection time */
+    for (i = 0; i < 2; i++)
+    {
+        if (fabsf(aDirection[i]) < EPSILON)
+        {
+            if (aStart[i] < bMin[i] || aStart[i] > bMax[i])
+                return BH_ERROR;
+            continue;
+        }
+
+        denom = 1.0f / aDirection[i];
+        hitNear = (bMin[i] - aStart[i]) * denom;
+        hitFar  = (bMax[i] - aStart[i]) * denom;
+
+        if (hitNear > hitFar)
+        {
+            tmp = hitNear;
+            hitNear = hitFar;
+            hitFar = tmp;
+        }
+
+        if (hitNear > timeNear)
+            timeNear = hitNear;
+        if (hitFar < timeFar)
+            timeFar = hitFar;
+
+        if (timeNear > timeFar || timeFar < 0.0f)
+            return BH_ERROR;
+    }
+
+    out[0] = aStart[0] + aDirection[0] * timeNear;
+    out[1] = aStart[1] + aDirection[1] * timeNear;
+    *t = timeNear;
+
+    return BH_OK;
+}
+
+
+int BH_Segment2fIntersectBox2f(const float *aStart,
+                               const float *aEnd,
+                               const float *bMin,
+                               const float *bMax,
+                               float *t,
+                               float *out)
+{
+    float tmp[3];
+    float time;
+
+    BH_Vec2fSub(aEnd, aStart, tmp);
+    if (BH_Ray2fIntersectBox2f(aStart, tmp, bMin, bMax, &time, out))
+        return BH_ERROR;
+
+    if (time > 1.0f)
+        return BH_ERROR;
+
+    *t = time;
+    return BH_OK;
+}
diff --git a/src/Math/Ray3f.c b/src/Math/Ray3f.c
new file mode 100644
index 0000000..e265071
--- /dev/null
+++ b/src/Math/Ray3f.c
@@ -0,0 +1,232 @@
+#include <BH/Math.h>
+#include <string.h>
+#include <math.h>
+
+
+#define EPSILON 0.00001f
+#define PI      3.14159265358979323846f
+
+
+int BH_Ray3fIntersectPlane(const float *start,
+                           const float *direction,
+                           const float *plane,
+                           float *t,
+                           float *out)
+{
+    float tmp1[3];
+    float denom, time;
+
+    /* Calculate intersection time */
+    denom = BH_Vec3fDot(direction, plane);
+    time = (plane[3] - BH_Vec3fDot(plane, start)) / denom;
+
+    /* Check for ray/plane parallel to each other or point is behing the ray. */
+    if (fabsf(denom) < EPSILON || time < 0.0f)
+        return BH_ERROR;
+
+    /* Compute intersection point */
+    BH_Vec3fScale(direction, time, tmp1);
+    BH_Vec3fAdd(start, tmp1, out);
+    *t = time;
+    return BH_OK;
+}
+
+
+int BH_Ray3fIntersectTriangle(const float *start,
+                              const float *direction,
+                              const float *a,
+                              const float *b,
+                              const float *c,
+                              float *t,
+                              float *out)
+{
+    float plane[4];
+    float tmp1[3], tmp2[3], tmp3[3];
+    float time;
+
+    /* Compute plane */
+    if (BH_PlaneFromPoints(a, b, c, plane))
+        return BH_ERROR;
+
+    /* Compute intersection point in ray against plane */
+    if (BH_Ray3fIntersectPlane(start, direction, plane, &time, tmp3))
+        return BH_ERROR;
+
+    /* Check if point inside rectangle */
+    BH_Vec3fSub(b, a, tmp1);
+    BH_Vec3fSub(tmp3, a, tmp2);
+    BH_Vec3fCross(tmp2, tmp1, tmp1);
+    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
+        return BH_ERROR;
+
+    BH_Vec3fSub(c, b, tmp1);
+    BH_Vec3fSub(tmp3, b, tmp2);
+    BH_Vec3fCross(tmp2, tmp1, tmp1);
+    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
+        return BH_ERROR;
+
+    BH_Vec3fSub(a, c, tmp1);
+    BH_Vec3fSub(tmp3, c, tmp2);
+    BH_Vec3fCross(tmp2, tmp1, tmp1);
+    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
+        return BH_ERROR;
+
+    /* Copy intersection point */
+    memcpy(out, tmp3, sizeof(tmp3));
+    *t = time;
+    return BH_OK;
+}
+
+
+int BH_Segment3fIntersectPlane(const float *start,
+                               const float *end,
+                               const float *plane,
+                               float *t,
+                               float *out)
+{
+    float tmp[3];
+    float denom, time;
+
+    /* Calculate intersection time */
+    BH_Vec3fSub(end, start, tmp);
+    denom = BH_Vec3fDot(tmp, plane);
+    time = (plane[3] - BH_Vec3fDot(plane, start)) / denom;
+
+    /* Check for ray/plane parallel to each other or point is behing the ray. */
+    if (fabsf(denom) < EPSILON || time < 0.0f || time > 1.0f)
+        return BH_ERROR;
+
+    /* Compute intersection point */
+    BH_Vec3fScale(tmp, time, tmp);
+    BH_Vec3fAdd(start, tmp, out);
+    *t = time;
+    return BH_OK;
+}
+
+
+int BH_Segment3fIntersectTriangle(const float *start,
+                                  const float *end,
+                                  const float *a,
+                                  const float *b,
+                                  const float *c,
+                                  float *t,
+                                  float *out)
+{
+    float plane[4];
+    float tmp1[3], tmp2[3], tmp3[3];
+    float time;
+
+    /* Compute plane */
+    if (BH_PlaneFromPoints(a, b, c, plane))
+        return BH_ERROR;
+
+    /* Compute intersection point in ray against plane */
+    if (BH_Segment3fIntersectPlane(start, end, plane, &time, tmp3))
+        return BH_ERROR;
+
+    /* Check if point inside rectangle */
+    BH_Vec3fSub(b, a, tmp1);
+    BH_Vec3fSub(tmp3, a, tmp2);
+    BH_Vec3fCross(tmp2, tmp1, tmp1);
+    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
+        return BH_ERROR;
+
+    BH_Vec3fSub(c, b, tmp1);
+    BH_Vec3fSub(tmp3, b, tmp2);
+    BH_Vec3fCross(tmp2, tmp1, tmp1);
+    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
+        return BH_ERROR;
+
+    BH_Vec3fSub(a, c, tmp1);
+    BH_Vec3fSub(tmp3, c, tmp2);
+    BH_Vec3fCross(tmp2, tmp1, tmp1);
+    if (BH_Vec3fDot(tmp1, plane) < 0.0f)
+        return BH_ERROR;
+
+    /* Copy intersection point */
+    memcpy(out, tmp3, sizeof(tmp3));
+    *t = time;
+    return BH_OK;
+}
+
+
+int BH_Ray3fIntersectBox3f(const float *aStart,
+                           const float *aDirection,
+                           const float *bMin,
+                           const float *bMax,
+                           float *t,
+                           float *out)
+{
+    float timeNear, timeFar, hitNear, hitFar, denom, tmp;
+    int i;
+
+    timeNear = -1.0f / 0.0f;
+    timeFar  =  1.0f / 0.0f;
+
+    /* Check if origin inside box */
+    if (!BH_Box3fContains(bMin, bMax, aStart))
+    {
+        memcpy(out, aStart, sizeof(float) * 3);
+        *t = 0.0f;
+        return BH_OK;
+    }
+
+    /* Check each axis for the minimal and maximum intersection time */
+    for (i = 0; i < 3; i++)
+    {
+        if (fabsf(aDirection[i]) < EPSILON)
+        {
+            if (aStart[i] < bMin[i] || aStart[i] > bMax[i])
+                return BH_ERROR;
+            continue;
+        }
+
+        denom = 1.0f / aDirection[i];
+        hitNear = (bMin[i] - aStart[i]) * denom;
+        hitFar  = (bMax[i] - aStart[i]) * denom;
+
+        if (hitNear > hitFar)
+        {
+            tmp = hitNear;
+            hitNear = hitFar;
+            hitFar = tmp;
+        }
+
+        if (hitNear > timeNear)
+            timeNear = hitNear;
+        if (hitFar < timeFar)
+            timeFar = hitFar;
+
+        if (timeNear > timeFar || timeFar < 0.0f)
+            return BH_ERROR;
+    }
+
+    out[0] = aStart[0] + aDirection[0] * timeNear;
+    out[1] = aStart[1] + aDirection[1] * timeNear;
+    out[2] = aStart[2] + aDirection[2] * timeNear;
+    *t = timeNear;
+
+    return BH_OK;
+}
+
+
+int BH_Segment3fIntersectBox3f(const float *aStart,
+                               const float *aEnd,
+                               const float *bMin,
+                               const float *bMax,
+                               float *t,
+                               float *out)
+{
+    float tmp[3];
+    float time;
+
+    BH_Vec3fSub(aEnd, aStart, tmp);
+    if (BH_Ray3fIntersectBox3f(aStart, tmp, bMin, bMax, &time, out))
+        return BH_ERROR;
+
+    if (time > 1.0f)
+        return BH_ERROR;
+
+    *t = time;
+    return BH_OK;
+}
diff --git a/src/Math/Vec2f.c b/src/Math/Vec2f.c
new file mode 100644
index 0000000..351415b
--- /dev/null
+++ b/src/Math/Vec2f.c
@@ -0,0 +1,153 @@
+#include <BH/Math.h>
+#include <math.h>
+
+
+void BH_Vec2fAdd(const float *a,
+                 const float *b,
+                 float *out)
+{
+    out[0] = a[0] + b[0];
+    out[1] = a[1] + b[1];
+}
+
+
+void BH_Vec2fSub(const float *a,
+                 const float *b,
+                 float *out)
+{
+    out[0] = a[0] - b[0];
+    out[1] = a[1] - b[1];
+}
+
+
+void BH_Vec2fMul(const float *a,
+                 const float *b,
+                 float *out)
+{
+    out[0] = a[0] * b[0];
+    out[1] = a[1] * b[1];
+}
+
+
+void BH_Vec2fScale(const float *a,
+                   const float b,
+                   float *out)
+{
+    out[0] = a[0] * b;
+    out[1] = a[1] * b;
+}
+
+
+void BH_Vec2fMulAdd(const float *a,
+                    const float *b,
+                    const float *c,
+                    float *out)
+{
+    out[0] = a[0] * b[0] + c[0];
+    out[1] = a[1] * b[1] + c[1];
+}
+
+
+void BH_Vec2fNegate(const float *in,
+                    float *out)
+{
+    out[0] = -in[0];
+    out[1] = -in[1];
+}
+
+
+float BH_Vec2fDot(const float *a,
+                  const float *b)
+{
+    return a[0] * b[0] + a[1] * b[1];
+}
+
+
+float BH_Vec2fCross(const float *a,
+                    const float *b)
+{
+    return a[0] * b[1] - a[1] * b[0];
+}
+
+
+float BH_Vec2fLength(const float *in)
+{
+    return sqrtf(BH_Vec2fDot(in, in));
+}
+
+
+void BH_Vec2fNormal(const float *in,
+                    float *out)
+{
+    BH_Vec2fScale(in, 1.0f / BH_Vec2fLength(in), out);
+}
+
+
+float BH_Vec2fNormalEx(const float *in,
+                       float *out)
+{
+    float length;
+
+    length = BH_Vec2fLength(in);
+    BH_Vec2fScale(in, 1.0f / length, out);
+    return length;
+}
+
+
+void BH_Vec2fMin(const float *a,
+                 const float *b,
+                 float *out)
+{
+    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
+}
+
+
+void BH_Vec2fMax(const float *a,
+                 const float *b,
+                 float *out)
+{
+    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
+}
+
+
+void BH_Vec2fLerp(const float *a,
+                  const float *b,
+                  float t,
+                  float *out)
+{
+    float tmp[2];
+
+    BH_Vec2fSub(b, a, tmp);
+    BH_Vec2fScale(tmp, t, tmp);
+    BH_Vec2fAdd(a, tmp, out);
+}
+
+
+void BH_Vec2fProject(const float *a,
+                     const float *b,
+                     float *out)
+{
+    float amount;
+
+    amount = BH_Vec2fDot(a, b) / BH_Vec2fDot(b, b);
+    BH_Vec2fScale(b, amount, out);
+}
+
+
+void BH_Vec2fBarycentric(const float *a,
+                         const float *b,
+                         const float *c,
+                         float v,
+                         float w,
+                         float *out)
+{
+    float tmp1[2], tmp2[2];
+    float u;
+
+    u = 1.0f - v - w;
+    tmp1[0] = tmp1[1] = u; BH_Vec2fMul(a, tmp1, tmp2);
+    tmp1[0] = tmp1[1] = v; BH_Vec2fMulAdd(b, tmp1, tmp2, tmp2);
+    tmp1[0] = tmp1[1] = w; BH_Vec2fMulAdd(c, tmp1, tmp2, out);
+}
diff --git a/src/Math/Vec2i.c b/src/Math/Vec2i.c
new file mode 100644
index 0000000..94601ec
--- /dev/null
+++ b/src/Math/Vec2i.c
@@ -0,0 +1,73 @@
+#include <BH/Math.h>
+
+
+void BH_Vec2iAdd(const int *a,
+                 const int *b,
+                 int *out)
+{
+    out[0] = a[0] + b[0];
+    out[1] = a[1] + b[1];
+}
+
+
+void BH_Vec2iSub(const int *a,
+                 const int *b,
+                 int *out)
+{
+    out[0] = a[0] - b[0];
+    out[1] = a[1] - b[1];
+}
+
+
+void BH_Vec2iMul(const int *a,
+                 const int *b,
+                 int *out)
+{
+    out[0] = a[0] * b[0];
+    out[1] = a[1] * b[1];
+}
+
+
+void BH_Vec2iScale(const int *a,
+                   int b,
+                   int *out)
+{
+    out[0] = a[0] * b;
+    out[1] = a[1] * b;
+}
+
+
+void BH_Vec2iMulAdd(const int *a,
+                    const int *b,
+                    const int *c,
+                    int *out)
+{
+    out[0] = a[0] * b[0] + c[0];
+    out[1] = a[1] * b[1] + c[1];
+}
+
+
+void BH_Vec2iNegate(const int *in,
+                    int *out)
+{
+    out[0] = -in[0];
+    out[1] = -in[1];
+}
+
+
+void BH_Vec2iMin(const int *a,
+                 const int *b,
+                 int *out)
+{
+    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
+}
+
+
+void BH_Vec2iMax(const int *a,
+                 const int *b,
+                 int *out)
+{
+    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
+}
diff --git a/src/Math/Vec3f.c b/src/Math/Vec3f.c
new file mode 100644
index 0000000..50a268d
--- /dev/null
+++ b/src/Math/Vec3f.c
@@ -0,0 +1,168 @@
+#include <BH/Math.h>
+#include <string.h>
+#include <math.h>
+
+
+void BH_Vec3fAdd(const float *a,
+                 const float *b,
+                 float *out)
+{
+    out[0] = a[0] + b[0];
+    out[1] = a[1] + b[1];
+    out[2] = a[2] + b[2];
+}
+
+
+void BH_Vec3fSub(const float *a,
+                 const float *b,
+                 float *out)
+{
+    out[0] = a[0] - b[0];
+    out[1] = a[1] - b[1];
+    out[2] = a[2] - b[2];
+}
+
+
+void BH_Vec3fMul(const float *a,
+                 const float *b,
+                 float *out)
+{
+    out[0] = a[0] * b[0];
+    out[1] = a[1] * b[1];
+    out[2] = a[2] * b[2];
+}
+
+
+void BH_Vec3fScale(const float *a,
+                   const float b,
+                   float *out)
+{
+    out[0] = a[0] * b;
+    out[1] = a[1] * b;
+    out[2] = a[2] * b;
+}
+
+
+void BH_Vec3fMulAdd(const float *a,
+                    const float *b,
+                    const float *c,
+                    float *out)
+{
+    out[0] = a[0] * b[0] + c[0];
+    out[1] = a[1] * b[1] + c[1];
+    out[2] = a[2] * b[2] + c[2];
+}
+
+
+void BH_Vec3fNegate(const float *in,
+                    float *out)
+{
+    out[0] = -in[0];
+    out[1] = -in[1];
+    out[2] = -in[2];
+}
+
+
+float BH_Vec3fDot(const float *a,
+                  const float *b)
+{
+    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
+}
+
+
+void BH_Vec3fCross(const float *a,
+                   const float *b,
+                   float *out)
+{
+    float tmp[3];
+
+    tmp[0] = a[1] * b[2] - a[2] * b[1];
+    tmp[1] = a[2] * b[0] - a[0] * b[2];
+    tmp[2] = a[0] * b[1] - a[1] * b[0];
+    memcpy(out, tmp, sizeof(tmp));
+}
+
+
+float BH_Vec3fLength(const float *in)
+{
+    return sqrtf(BH_Vec3fDot(in, in));
+}
+
+
+void BH_Vec3fNormal(const float *in,
+                    float *out)
+{
+    BH_Vec3fScale(in, 1.0f / BH_Vec3fLength(in), out);
+}
+
+
+float BH_Vec3fNormalEx(const float *in,
+                       float *out)
+{
+    float length;
+
+    length = BH_Vec3fLength(in);
+    BH_Vec3fScale(in, 1.0f / length, out);
+    return length;
+}
+
+
+void BH_Vec3fMin(const float *a,
+                 const float *b,
+                 float *out)
+{
+    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
+    if (a[2] < b[2]) out[2] = a[2]; else out[2] = b[2];
+}
+
+
+void BH_Vec3fMax(const float *a,
+                 const float *b,
+                 float *out)
+{
+    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
+    if (a[2] > b[2]) out[2] = a[2]; else out[2] = b[2];
+}
+
+
+void BH_Vec3fLerp(const float *a,
+                  const float *b,
+                  float t,
+                  float *out)
+{
+    float tmp[3];
+
+    BH_Vec3fSub(b, a, tmp);
+    BH_Vec3fScale(tmp, t, tmp);
+    BH_Vec3fAdd(a, tmp, out);
+}
+
+
+void BH_Vec3fProject(const float *a,
+                     const float *b,
+                     float *out)
+{
+    float amount;
+
+    amount = BH_Vec3fDot(a, b) / BH_Vec3fDot(b, b);
+    BH_Vec3fScale(b, amount, out);
+}
+
+
+void BH_Vec3fBarycentric(const float *a,
+                         const float *b,
+                         const float *c,
+                         float v,
+                         float w,
+                         float *out)
+{
+    float tmp1[3], tmp2[3];
+    float u;
+
+    u = 1.0f - v - w;
+    tmp1[0] = tmp1[1] = tmp1[2] = u; BH_Vec3fMul(a, tmp1, tmp2);
+    tmp1[0] = tmp1[1] = tmp1[2] = v; BH_Vec3fMulAdd(b, tmp1, tmp2, tmp2);
+    tmp1[0] = tmp1[1] = tmp1[2] = w; BH_Vec3fMulAdd(c, tmp1, tmp2, out);
+}
diff --git a/src/Math/Vec3i.c b/src/Math/Vec3i.c
new file mode 100644
index 0000000..c791c09
--- /dev/null
+++ b/src/Math/Vec3i.c
@@ -0,0 +1,81 @@
+#include <BH/Math.h>
+
+
+void BH_Vec3iAdd(const int *a,
+                 const int *b,
+                 int *out)
+{
+    out[0] = a[0] + b[0];
+    out[1] = a[1] + b[1];
+    out[2] = a[2] + b[2];
+}
+
+
+void BH_Vec3iSub(const int *a,
+                 const int *b,
+                 int *out)
+{
+    out[0] = a[0] - b[0];
+    out[1] = a[1] - b[1];
+    out[2] = a[2] - b[2];
+}
+
+
+void BH_Vec3iMul(const int *a,
+                 const int *b,
+                 int *out)
+{
+    out[0] = a[0] * b[0];
+    out[1] = a[1] * b[1];
+    out[2] = a[2] * b[2];
+}
+
+
+void BH_Vec3iScale(const int *a,
+                   int b,
+                   int *out)
+{
+    out[0] = a[0] * b;
+    out[1] = a[1] * b;
+    out[2] = a[2] * b;
+}
+
+
+void BH_Vec3iMulAdd(const int *a,
+                    const int *b,
+                    const int *c,
+                    int *out)
+{
+    out[0] = a[0] * b[0] + c[0];
+    out[1] = a[1] * b[1] + c[1];
+    out[2] = a[2] * b[2] + c[2];
+}
+
+
+void BH_Vec3iNegate(const int *in,
+                    int *out)
+{
+    out[0] = -in[0];
+    out[1] = -in[1];
+    out[2] = -in[2];
+}
+
+
+void BH_Vec3iMin(const int *a,
+                 const int *b,
+                 int *out)
+{
+    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
+    if (a[2] < b[2]) out[2] = a[2]; else out[2] = b[2];
+}
+
+
+void BH_Vec3iMax(const int *a,
+                 const int *b,
+                 int *out)
+{
+    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
+    if (a[2] > b[2]) out[2] = a[2]; else out[2] = b[2];
+}
diff --git a/src/Math/Vec4f.c b/src/Math/Vec4f.c
new file mode 100644
index 0000000..768f84c
--- /dev/null
+++ b/src/Math/Vec4f.c
@@ -0,0 +1,162 @@
+#include <BH/Math.h>
+#include <math.h>
+
+
+void BH_Vec4fAdd(const float *a,
+                 const float *b,
+                 float *out)
+{
+    out[0] = a[0] + b[0];
+    out[1] = a[1] + b[1];
+    out[2] = a[2] + b[2];
+    out[3] = a[3] + b[3];
+}
+
+
+void BH_Vec4fSub(const float *a,
+                 const float *b,
+                 float *out)
+{
+    out[0] = a[0] - b[0];
+    out[1] = a[1] - b[1];
+    out[2] = a[2] - b[2];
+    out[3] = a[3] - b[3];
+}
+
+
+void BH_Vec4fMul(const float *a,
+                 const float *b,
+                 float *out)
+{
+    out[0] = a[0] * b[0];
+    out[1] = a[1] * b[1];
+    out[2] = a[2] * b[2];
+    out[3] = a[3] * b[3];
+}
+
+
+void BH_Vec4fScale(const float *a,
+                   const float b,
+                   float *out)
+{
+    out[0] = a[0] * b;
+    out[1] = a[1] * b;
+    out[2] = a[2] * b;
+    out[3] = a[3] * b;
+}
+
+
+void BH_Vec4fMulAdd(const float *a,
+                    const float *b,
+                    const float *c,
+                    float *out)
+{
+    out[0] = a[0] * b[0] + c[0];
+    out[1] = a[1] * b[1] + c[1];
+    out[2] = a[2] * b[2] + c[2];
+    out[3] = a[3] * b[3] + c[3];
+}
+
+
+void BH_Vec4fNegate(const float *in,
+                    float *out)
+{
+    out[0] = -in[0];
+    out[1] = -in[1];
+    out[2] = -in[2];
+    out[3] = -in[3];
+}
+
+
+float BH_Vec4fDot(const float *a,
+                  const float *b)
+{
+    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
+}
+
+
+float BH_Vec4fLength(const float *in)
+{
+    return sqrtf(BH_Vec4fDot(in, in));
+}
+
+
+void BH_Vec4fNormal(const float *in,
+                    float *out)
+{
+    BH_Vec4fScale(in, 1.0f / BH_Vec4fLength(in), out);
+}
+
+
+float BH_Vec4fNormalEx(const float *in,
+                       float *out)
+{
+    float length;
+
+    length = BH_Vec4fLength(in);
+    BH_Vec4fScale(in, 1.0f / length, out);
+    return length;
+}
+
+
+void BH_Vec4fMin(const float *a,
+                 const float *b,
+                 float *out)
+{
+    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
+    if (a[2] < b[2]) out[2] = a[2]; else out[2] = b[2];
+    if (a[3] < b[3]) out[3] = a[3]; else out[3] = b[3];
+}
+
+
+void BH_Vec4fMax(const float *a,
+                 const float *b,
+                 float *out)
+{
+    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
+    if (a[2] > b[2]) out[2] = a[2]; else out[2] = b[2];
+    if (a[3] > b[3]) out[3] = a[3]; else out[3] = b[3];
+}
+
+
+void BH_Vec4fLerp(const float *a,
+                  const float *b,
+                  float t,
+                  float *out)
+{
+    float tmp[4];
+
+    BH_Vec4fSub(b, a, tmp);
+    BH_Vec4fScale(tmp, t, tmp);
+    BH_Vec4fAdd(a, tmp, out);
+}
+
+
+void BH_Vec4fProject(const float *a,
+                     const float *b,
+                     float *out)
+{
+    float amount;
+
+    amount = BH_Vec4fDot(a, b) / BH_Vec4fDot(b, b);
+    BH_Vec4fScale(b, amount, out);
+}
+
+
+void BH_Vec4fBarycentric(const float *a,
+                         const float *b,
+                         const float *c,
+                         float v,
+                         float w,
+                         float *out)
+{
+    float tmp1[4], tmp2[4];
+    float u;
+
+    u = 1.0f - v - w;
+    tmp1[0] = tmp1[1] = tmp1[2] = tmp1[3] = u; BH_Vec4fMul(a, tmp1, tmp2);
+    tmp1[0] = tmp1[1] = tmp1[2] = tmp1[3] = v; BH_Vec4fMulAdd(b, tmp1, tmp2, tmp2);
+    tmp1[0] = tmp1[1] = tmp1[2] = tmp1[3] = w; BH_Vec4fMulAdd(c, tmp1, tmp2, out);
+}
diff --git a/src/Math/Vec4i.c b/src/Math/Vec4i.c
new file mode 100644
index 0000000..e62ff9e
--- /dev/null
+++ b/src/Math/Vec4i.c
@@ -0,0 +1,89 @@
+#include <BH/Math.h>
+
+
+void BH_Vec4iAdd(const int *a,
+                 const int *b,
+                 int *out)
+{
+    out[0] = a[0] + b[0];
+    out[1] = a[1] + b[1];
+    out[2] = a[2] + b[2];
+    out[3] = a[3] + b[3];
+}
+
+
+void BH_Vec4iSub(const int *a,
+                 const int *b,
+                 int *out)
+{
+    out[0] = a[0] - b[0];
+    out[1] = a[1] - b[1];
+    out[2] = a[2] - b[2];
+    out[3] = a[3] - b[3];
+}
+
+
+void BH_Vec4iMul(const int *a,
+                 const int *b,
+                 int *out)
+{
+    out[0] = a[0] * b[0];
+    out[1] = a[1] * b[1];
+    out[2] = a[2] * b[2];
+    out[3] = a[3] * b[3];
+}
+
+
+void BH_Vec4iScale(const int *a,
+                   int b,
+                   int *out)
+{
+    out[0] = a[0] * b;
+    out[1] = a[1] * b;
+    out[2] = a[2] * b;
+    out[3] = a[3] * b;
+}
+
+
+void BH_Vec4iMulAdd(const int *a,
+                    const int *b,
+                    const int *c,
+                    int *out)
+{
+    out[0] = a[0] * b[0] + c[0];
+    out[1] = a[1] * b[1] + c[1];
+    out[2] = a[2] * b[2] + c[2];
+    out[3] = a[3] * b[3] + c[3];
+}
+
+
+void BH_Vec4iNegate(const int *in,
+                    int *out)
+{
+    out[0] = -in[0];
+    out[1] = -in[1];
+    out[2] = -in[2];
+    out[3] = -in[3];
+}
+
+
+void BH_Vec4iMin(const int *a,
+                 const int *b,
+                 int *out)
+{
+    if (a[0] < b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] < b[1]) out[1] = a[1]; else out[1] = b[1];
+    if (a[2] < b[2]) out[2] = a[2]; else out[2] = b[2];
+    if (a[3] < b[3]) out[3] = a[3]; else out[3] = b[3];
+}
+
+
+void BH_Vec4iMax(const int *a,
+                 const int *b,
+                 int *out)
+{
+    if (a[0] > b[0]) out[0] = a[0]; else out[0] = b[0];
+    if (a[1] > b[1]) out[1] = a[1]; else out[1] = b[1];
+    if (a[2] > b[2]) out[2] = a[2]; else out[2] = b[2];
+    if (a[3] > b[3]) out[3] = a[3]; else out[3] = b[3];
+}