initial commit

3 files changed, 2020 insertions, 0 deletions

diff --git a/shared/Matrices.cpp b/shared/Matrices.cpp
new file mode 100644
index 0000000..582b285
--- /dev/null
+++ b/shared/Matrices.cpp
@@ -0,0 +1,581 @@
+///////////////////////////////////////////////////////////////////////////////
+// Matrice.cpp
+// ===========
+// NxN Matrix Math classes
+//
+// The elements of the matrix are stored as column major order.
+// | 0 2 |    | 0 3 6 |    |  0  4  8 12 |
+// | 1 3 |    | 1 4 7 |    |  1  5  9 13 |
+//            | 2 5 8 |    |  2  6 10 14 |
+//                         |  3  7 11 15 |
+//
+//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
+// CREATED: 2005-06-24
+// UPDATED: 2014-09-21
+//
+// Copyright (C) 2005 Song Ho Ahn
+///////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <algorithm>
+#include "Matrices.h"
+
+const float DEG2RAD = 3.141593f / 180;
+const float EPSILON = 0.00001f;
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// transpose 2x2 matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix2& Matrix2::transpose()
+{
+    std::swap(m[1],  m[2]);
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// return the determinant of 2x2 matrix
+///////////////////////////////////////////////////////////////////////////////
+float Matrix2::getDeterminant()
+{
+    return m[0] * m[3] - m[1] * m[2];
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inverse of 2x2 matrix
+// If cannot find inverse, set identity matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix2& Matrix2::invert()
+{
+    float determinant = getDeterminant();
+    if(fabs(determinant) <= EPSILON)
+    {
+        return identity();
+    }
+
+    float tmp = m[0];   // copy the first element
+    float invDeterminant = 1.0f / determinant;
+    m[0] =  invDeterminant * m[3];
+    m[1] = -invDeterminant * m[1];
+    m[2] = -invDeterminant * m[2];
+    m[3] =  invDeterminant * tmp;
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// transpose 3x3 matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix3& Matrix3::transpose()
+{
+    std::swap(m[1],  m[3]);
+    std::swap(m[2],  m[6]);
+    std::swap(m[5],  m[7]);
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// return determinant of 3x3 matrix
+///////////////////////////////////////////////////////////////////////////////
+float Matrix3::getDeterminant()
+{
+    return m[0] * (m[4] * m[8] - m[5] * m[7]) -
+           m[1] * (m[3] * m[8] - m[5] * m[6]) +
+           m[2] * (m[3] * m[7] - m[4] * m[6]);
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inverse 3x3 matrix
+// If cannot find inverse, set identity matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix3& Matrix3::invert()
+{
+    float determinant, invDeterminant;
+    float tmp[9];
+
+    tmp[0] = m[4] * m[8] - m[5] * m[7];
+    tmp[1] = m[2] * m[7] - m[1] * m[8];
+    tmp[2] = m[1] * m[5] - m[2] * m[4];
+    tmp[3] = m[5] * m[6] - m[3] * m[8];
+    tmp[4] = m[0] * m[8] - m[2] * m[6];
+    tmp[5] = m[2] * m[3] - m[0] * m[5];
+    tmp[6] = m[3] * m[7] - m[4] * m[6];
+    tmp[7] = m[1] * m[6] - m[0] * m[7];
+    tmp[8] = m[0] * m[4] - m[1] * m[3];
+
+    // check determinant if it is 0
+    determinant = m[0] * tmp[0] + m[1] * tmp[3] + m[2] * tmp[6];
+    if(fabs(determinant) <= EPSILON)
+    {
+        return identity(); // cannot inverse, make it idenety matrix
+    }
+
+    // divide by the determinant
+    invDeterminant = 1.0f / determinant;
+    m[0] = invDeterminant * tmp[0];
+    m[1] = invDeterminant * tmp[1];
+    m[2] = invDeterminant * tmp[2];
+    m[3] = invDeterminant * tmp[3];
+    m[4] = invDeterminant * tmp[4];
+    m[5] = invDeterminant * tmp[5];
+    m[6] = invDeterminant * tmp[6];
+    m[7] = invDeterminant * tmp[7];
+    m[8] = invDeterminant * tmp[8];
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// transpose 4x4 matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::transpose()
+{
+    std::swap(m[1],  m[4]);
+    std::swap(m[2],  m[8]);
+    std::swap(m[3],  m[12]);
+    std::swap(m[6],  m[9]);
+    std::swap(m[7],  m[13]);
+    std::swap(m[11], m[14]);
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inverse 4x4 matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invert()
+{
+    // If the 4th row is [0,0,0,1] then it is affine matrix and
+    // it has no projective transformation.
+    if(m[3] == 0 && m[7] == 0 && m[11] == 0 && m[15] == 1)
+        this->invertAffine();
+    else
+    {
+        this->invertGeneral();
+        /*@@ invertProjective() is not optimized (slower than generic one)
+        if(fabs(m[0]*m[5] - m[1]*m[4]) > EPSILON)
+            this->invertProjective();   // inverse using matrix partition
+        else
+            this->invertGeneral();      // generalized inverse
+        */
+    }
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// compute the inverse of 4x4 Euclidean transformation matrix
+//
+// Euclidean transformation is translation, rotation, and reflection.
+// With Euclidean transform, only the position and orientation of the object
+// will be changed. Euclidean transform does not change the shape of an object
+// (no scaling). Length and angle are reserved.
+//
+// Use inverseAffine() if the matrix has scale and shear transformation.
+//
+// M = [ R | T ]
+//     [ --+-- ]    (R denotes 3x3 rotation/reflection matrix)
+//     [ 0 | 1 ]    (T denotes 1x3 translation matrix)
+//
+// y = M*x  ->  y = R*x + T  ->  x = R^-1*(y - T)  ->  x = R^T*y - R^T*T
+// (R is orthogonal,  R^-1 = R^T)
+//
+//  [ R | T ]-1    [ R^T | -R^T * T ]    (R denotes 3x3 rotation matrix)
+//  [ --+-- ]   =  [ ----+--------- ]    (T denotes 1x3 translation)
+//  [ 0 | 1 ]      [  0  |     1    ]    (R^T denotes R-transpose)
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invertEuclidean()
+{
+    // transpose 3x3 rotation matrix part
+    // | R^T | 0 |
+    // | ----+-- |
+    // |  0  | 1 |
+    float tmp;
+    tmp = m[1];  m[1] = m[4];  m[4] = tmp;
+    tmp = m[2];  m[2] = m[8];  m[8] = tmp;
+    tmp = m[6];  m[6] = m[9];  m[9] = tmp;
+
+    // compute translation part -R^T * T
+    // | 0 | -R^T x |
+    // | --+------- |
+    // | 0 |   0    |
+    float x = m[12];
+    float y = m[13];
+    float z = m[14];
+    m[12] = -(m[0] * x + m[4] * y + m[8] * z);
+    m[13] = -(m[1] * x + m[5] * y + m[9] * z);
+    m[14] = -(m[2] * x + m[6] * y + m[10]* z);
+
+    // last row should be unchanged (0,0,0,1)
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// compute the inverse of a 4x4 affine transformation matrix
+//
+// Affine transformations are generalizations of Euclidean transformations.
+// Affine transformation includes translation, rotation, reflection, scaling,
+// and shearing. Length and angle are NOT preserved.
+// M = [ R | T ]
+//     [ --+-- ]    (R denotes 3x3 rotation/scale/shear matrix)
+//     [ 0 | 1 ]    (T denotes 1x3 translation matrix)
+//
+// y = M*x  ->  y = R*x + T  ->  x = R^-1*(y - T)  ->  x = R^-1*y - R^-1*T
+//
+//  [ R | T ]-1   [ R^-1 | -R^-1 * T ]
+//  [ --+-- ]   = [ -----+---------- ]
+//  [ 0 | 1 ]     [  0   +     1     ]
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invertAffine()
+{
+    // R^-1
+    Matrix3 r(m[0],m[1],m[2], m[4],m[5],m[6], m[8],m[9],m[10]);
+    r.invert();
+    m[0] = r[0];  m[1] = r[1];  m[2] = r[2];
+    m[4] = r[3];  m[5] = r[4];  m[6] = r[5];
+    m[8] = r[6];  m[9] = r[7];  m[10]= r[8];
+
+    // -R^-1 * T
+    float x = m[12];
+    float y = m[13];
+    float z = m[14];
+    m[12] = -(r[0] * x + r[3] * y + r[6] * z);
+    m[13] = -(r[1] * x + r[4] * y + r[7] * z);
+    m[14] = -(r[2] * x + r[5] * y + r[8] * z);
+
+    // last row should be unchanged (0,0,0,1)
+    //m[3] = m[7] = m[11] = 0.0f;
+    //m[15] = 1.0f;
+
+    return * this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inverse matrix using matrix partitioning (blockwise inverse)
+// It devides a 4x4 matrix into 4 of 2x2 matrices. It works in case of where
+// det(A) != 0. If not, use the generic inverse method
+// inverse formula.
+// M = [ A | B ]    A, B, C, D are 2x2 matrix blocks
+//     [ --+-- ]    det(M) = |A| * |D - ((C * A^-1) * B)|
+//     [ C | D ]
+//
+// M^-1 = [ A' | B' ]   A' = A^-1 - (A^-1 * B) * C'
+//        [ ---+--- ]   B' = (A^-1 * B) * -D'
+//        [ C' | D' ]   C' = -D' * (C * A^-1)
+//                      D' = (D - ((C * A^-1) * B))^-1
+//
+// NOTE: I wrap with () if it it used more than once.
+//       The matrix is invertable even if det(A)=0, so must check det(A) before
+//       calling this function, and use invertGeneric() instead.
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invertProjective()
+{
+    // partition
+    Matrix2 a(m[0], m[1], m[4], m[5]);
+    Matrix2 b(m[8], m[9], m[12], m[13]);
+    Matrix2 c(m[2], m[3], m[6], m[7]);
+    Matrix2 d(m[10], m[11], m[14], m[15]);
+
+    // pre-compute repeated parts
+    a.invert();             // A^-1
+    Matrix2 ab = a * b;     // A^-1 * B
+    Matrix2 ca = c * a;     // C * A^-1
+    Matrix2 cab = ca * b;   // C * A^-1 * B
+    Matrix2 dcab = d - cab; // D - C * A^-1 * B
+
+    // check determinant if |D - C * A^-1 * B| = 0
+    //NOTE: this function assumes det(A) is already checked. if |A|=0 then,
+    //      cannot use this function.
+    float determinant = dcab[0] * dcab[3] - dcab[1] * dcab[2];
+    if(fabs(determinant) <= EPSILON)
+    {
+        return identity();
+    }
+
+    // compute D' and -D'
+    Matrix2 d1 = dcab;      //  (D - C * A^-1 * B)
+    d1.invert();            //  (D - C * A^-1 * B)^-1
+    Matrix2 d2 = -d1;       // -(D - C * A^-1 * B)^-1
+
+    // compute C'
+    Matrix2 c1 = d2 * ca;   // -D' * (C * A^-1)
+
+    // compute B'
+    Matrix2 b1 = ab * d2;   // (A^-1 * B) * -D'
+
+    // compute A'
+    Matrix2 a1 = a - (ab * c1); // A^-1 - (A^-1 * B) * C'
+
+    // assemble inverse matrix
+    m[0] = a1[0];  m[4] = a1[2]; /*|*/ m[8] = b1[0];  m[12]= b1[2];
+    m[1] = a1[1];  m[5] = a1[3]; /*|*/ m[9] = b1[1];  m[13]= b1[3];
+    /*-----------------------------+-----------------------------*/
+    m[2] = c1[0];  m[6] = c1[2]; /*|*/ m[10]= d1[0];  m[14]= d1[2];
+    m[3] = c1[1];  m[7] = c1[3]; /*|*/ m[11]= d1[1];  m[15]= d1[3];
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// compute the inverse of a general 4x4 matrix using Cramer's Rule
+// If cannot find inverse, return indentity matrix
+// M^-1 = adj(M) / det(M)
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invertGeneral()
+{
+    // get cofactors of minor matrices
+    float cofactor0 = getCofactor(m[5],m[6],m[7], m[9],m[10],m[11], m[13],m[14],m[15]);
+    float cofactor1 = getCofactor(m[4],m[6],m[7], m[8],m[10],m[11], m[12],m[14],m[15]);
+    float cofactor2 = getCofactor(m[4],m[5],m[7], m[8],m[9], m[11], m[12],m[13],m[15]);
+    float cofactor3 = getCofactor(m[4],m[5],m[6], m[8],m[9], m[10], m[12],m[13],m[14]);
+
+    // get determinant
+    float determinant = m[0] * cofactor0 - m[1] * cofactor1 + m[2] * cofactor2 - m[3] * cofactor3;
+    if(fabs(determinant) <= EPSILON)
+    {
+        return identity();
+    }
+
+    // get rest of cofactors for adj(M)
+    float cofactor4 = getCofactor(m[1],m[2],m[3], m[9],m[10],m[11], m[13],m[14],m[15]);
+    float cofactor5 = getCofactor(m[0],m[2],m[3], m[8],m[10],m[11], m[12],m[14],m[15]);
+    float cofactor6 = getCofactor(m[0],m[1],m[3], m[8],m[9], m[11], m[12],m[13],m[15]);
+    float cofactor7 = getCofactor(m[0],m[1],m[2], m[8],m[9], m[10], m[12],m[13],m[14]);
+
+    float cofactor8 = getCofactor(m[1],m[2],m[3], m[5],m[6], m[7],  m[13],m[14],m[15]);
+    float cofactor9 = getCofactor(m[0],m[2],m[3], m[4],m[6], m[7],  m[12],m[14],m[15]);
+    float cofactor10= getCofactor(m[0],m[1],m[3], m[4],m[5], m[7],  m[12],m[13],m[15]);
+    float cofactor11= getCofactor(m[0],m[1],m[2], m[4],m[5], m[6],  m[12],m[13],m[14]);
+
+    float cofactor12= getCofactor(m[1],m[2],m[3], m[5],m[6], m[7],  m[9], m[10],m[11]);
+    float cofactor13= getCofactor(m[0],m[2],m[3], m[4],m[6], m[7],  m[8], m[10],m[11]);
+    float cofactor14= getCofactor(m[0],m[1],m[3], m[4],m[5], m[7],  m[8], m[9], m[11]);
+    float cofactor15= getCofactor(m[0],m[1],m[2], m[4],m[5], m[6],  m[8], m[9], m[10]);
+
+    // build inverse matrix = adj(M) / det(M)
+    // adjugate of M is the transpose of the cofactor matrix of M
+    float invDeterminant = 1.0f / determinant;
+    m[0] =  invDeterminant * cofactor0;
+    m[1] = -invDeterminant * cofactor4;
+    m[2] =  invDeterminant * cofactor8;
+    m[3] = -invDeterminant * cofactor12;
+
+    m[4] = -invDeterminant * cofactor1;
+    m[5] =  invDeterminant * cofactor5;
+    m[6] = -invDeterminant * cofactor9;
+    m[7] =  invDeterminant * cofactor13;
+
+    m[8] =  invDeterminant * cofactor2;
+    m[9] = -invDeterminant * cofactor6;
+    m[10]=  invDeterminant * cofactor10;
+    m[11]= -invDeterminant * cofactor14;
+
+    m[12]= -invDeterminant * cofactor3;
+    m[13]=  invDeterminant * cofactor7;
+    m[14]= -invDeterminant * cofactor11;
+    m[15]=  invDeterminant * cofactor15;
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// return determinant of 4x4 matrix
+///////////////////////////////////////////////////////////////////////////////
+float Matrix4::getDeterminant()
+{
+    return m[0] * getCofactor(m[5],m[6],m[7], m[9],m[10],m[11], m[13],m[14],m[15]) -
+           m[1] * getCofactor(m[4],m[6],m[7], m[8],m[10],m[11], m[12],m[14],m[15]) +
+           m[2] * getCofactor(m[4],m[5],m[7], m[8],m[9], m[11], m[12],m[13],m[15]) -
+           m[3] * getCofactor(m[4],m[5],m[6], m[8],m[9], m[10], m[12],m[13],m[14]);
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// compute cofactor of 3x3 minor matrix without sign
+// input params are 9 elements of the minor matrix
+// NOTE: The caller must know its sign.
+///////////////////////////////////////////////////////////////////////////////
+float Matrix4::getCofactor(float m0, float m1, float m2,
+                           float m3, float m4, float m5,
+                           float m6, float m7, float m8)
+{
+    return m0 * (m4 * m8 - m5 * m7) -
+           m1 * (m3 * m8 - m5 * m6) +
+           m2 * (m3 * m7 - m4 * m6);
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// translate this matrix by (x, y, z)
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::translate(const Vector3& v)
+{
+    return translate(v.x, v.y, v.z);
+}
+
+Matrix4& Matrix4::translate(float x, float y, float z)
+{
+    m[0] += m[3] * x;   m[4] += m[7] * x;   m[8] += m[11]* x;   m[12]+= m[15]* x;
+    m[1] += m[3] * y;   m[5] += m[7] * y;   m[9] += m[11]* y;   m[13]+= m[15]* y;
+    m[2] += m[3] * z;   m[6] += m[7] * z;   m[10]+= m[11]* z;   m[14]+= m[15]* z;
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// uniform scale
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::scale(float s)
+{
+    return scale(s, s, s);
+}
+
+Matrix4& Matrix4::scale(float x, float y, float z)
+{
+    m[0] *= x;   m[4] *= x;   m[8] *= x;   m[12] *= x;
+    m[1] *= y;   m[5] *= y;   m[9] *= y;   m[13] *= y;
+    m[2] *= z;   m[6] *= z;   m[10]*= z;   m[14] *= z;
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// build a rotation matrix with given angle(degree) and rotation axis, then
+// multiply it with this object
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::rotate(float angle, const Vector3& axis)
+{
+    return rotate(angle, axis.x, axis.y, axis.z);
+}
+
+Matrix4& Matrix4::rotate(float angle, float x, float y, float z)
+{
+    float c = cosf(angle * DEG2RAD);    // cosine
+    float s = sinf(angle * DEG2RAD);    // sine
+    float c1 = 1.0f - c;                // 1 - c
+    float m0 = m[0],  m4 = m[4],  m8 = m[8],  m12= m[12],
+          m1 = m[1],  m5 = m[5],  m9 = m[9],  m13= m[13],
+          m2 = m[2],  m6 = m[6],  m10= m[10], m14= m[14];
+
+    // build rotation matrix
+    float r0 = x * x * c1 + c;
+    float r1 = x * y * c1 + z * s;
+    float r2 = x * z * c1 - y * s;
+    float r4 = x * y * c1 - z * s;
+    float r5 = y * y * c1 + c;
+    float r6 = y * z * c1 + x * s;
+    float r8 = x * z * c1 + y * s;
+    float r9 = y * z * c1 - x * s;
+    float r10= z * z * c1 + c;
+
+    // multiply rotation matrix
+    m[0] = r0 * m0 + r4 * m1 + r8 * m2;
+    m[1] = r1 * m0 + r5 * m1 + r9 * m2;
+    m[2] = r2 * m0 + r6 * m1 + r10* m2;
+    m[4] = r0 * m4 + r4 * m5 + r8 * m6;
+    m[5] = r1 * m4 + r5 * m5 + r9 * m6;
+    m[6] = r2 * m4 + r6 * m5 + r10* m6;
+    m[8] = r0 * m8 + r4 * m9 + r8 * m10;
+    m[9] = r1 * m8 + r5 * m9 + r9 * m10;
+    m[10]= r2 * m8 + r6 * m9 + r10* m10;
+    m[12]= r0 * m12+ r4 * m13+ r8 * m14;
+    m[13]= r1 * m12+ r5 * m13+ r9 * m14;
+    m[14]= r2 * m12+ r6 * m13+ r10* m14;
+
+    return *this;
+}
+
+Matrix4& Matrix4::rotateX(float angle)
+{
+    float c = cosf(angle * DEG2RAD);
+    float s = sinf(angle * DEG2RAD);
+    float m1 = m[1],  m2 = m[2],
+          m5 = m[5],  m6 = m[6],
+          m9 = m[9],  m10= m[10],
+          m13= m[13], m14= m[14];
+
+    m[1] = m1 * c + m2 *-s;
+    m[2] = m1 * s + m2 * c;
+    m[5] = m5 * c + m6 *-s;
+    m[6] = m5 * s + m6 * c;
+    m[9] = m9 * c + m10*-s;
+    m[10]= m9 * s + m10* c;
+    m[13]= m13* c + m14*-s;
+    m[14]= m13* s + m14* c;
+
+    return *this;
+}
+
+Matrix4& Matrix4::rotateY(float angle)
+{
+    float c = cosf(angle * DEG2RAD);
+    float s = sinf(angle * DEG2RAD);
+    float m0 = m[0],  m2 = m[2],
+          m4 = m[4],  m6 = m[6],
+          m8 = m[8],  m10= m[10],
+          m12= m[12], m14= m[14];
+
+    m[0] = m0 * c + m2 * s;
+    m[2] = m0 *-s + m2 * c;
+    m[4] = m4 * c + m6 * s;
+    m[6] = m4 *-s + m6 * c;
+    m[8] = m8 * c + m10* s;
+    m[10]= m8 *-s + m10* c;
+    m[12]= m12* c + m14* s;
+    m[14]= m12*-s + m14* c;
+
+    return *this;
+}
+
+Matrix4& Matrix4::rotateZ(float angle)
+{
+    float c = cosf(angle * DEG2RAD);
+    float s = sinf(angle * DEG2RAD);
+    float m0 = m[0],  m1 = m[1],
+          m4 = m[4],  m5 = m[5],
+          m8 = m[8],  m9 = m[9],
+          m12= m[12], m13= m[13];
+
+    m[0] = m0 * c + m1 *-s;
+    m[1] = m0 * s + m1 * c;
+    m[4] = m4 * c + m5 *-s;
+    m[5] = m4 * s + m5 * c;
+    m[8] = m8 * c + m9 *-s;
+    m[9] = m8 * s + m9 * c;
+    m[12]= m12* c + m13*-s;
+    m[13]= m12* s + m13* c;
+
+    return *this;
+}
diff --git a/shared/Matrices.h b/shared/Matrices.h
new file mode 100644
index 0000000..3515f54
--- /dev/null
+++ b/shared/Matrices.h
@@ -0,0 +1,909 @@
+///////////////////////////////////////////////////////////////////////////////
+// Matrice.h
+// =========
+// NxN Matrix Math classes
+//
+// The elements of the matrix are stored as column major order.
+// | 0 2 |    | 0 3 6 |    |  0  4  8 12 |
+// | 1 3 |    | 1 4 7 |    |  1  5  9 13 |
+//            | 2 5 8 |    |  2  6 10 14 |
+//                         |  3  7 11 15 |
+//
+//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
+// CREATED: 2005-06-24
+// UPDATED: 2013-09-30
+//
+// Copyright (C) 2005 Song Ho Ahn
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef MATH_MATRICES_H
+#define MATH_MATRICES_H
+
+#include <iostream>
+#include <iomanip>
+#include "Vectors.h"
+
+///////////////////////////////////////////////////////////////////////////
+// 2x2 matrix
+///////////////////////////////////////////////////////////////////////////
+class Matrix2
+{
+public:
+    // constructors
+    Matrix2();  // init with identity
+    Matrix2(const float src[4]);
+    Matrix2(float m0, float m1, float m2, float m3);
+
+    void        set(const float src[4]);
+    void        set(float m0, float m1, float m2, float m3);
+    void        setRow(int index, const float row[2]);
+    void        setRow(int index, const Vector2& v);
+    void        setColumn(int index, const float col[2]);
+    void        setColumn(int index, const Vector2& v);
+
+    const float* get() const;
+    float       getDeterminant();
+
+    Matrix2&    identity();
+    Matrix2&    transpose();                            // transpose itself and return reference
+    Matrix2&    invert();
+
+    // operators
+    Matrix2     operator+(const Matrix2& rhs) const;    // add rhs
+    Matrix2     operator-(const Matrix2& rhs) const;    // subtract rhs
+    Matrix2&    operator+=(const Matrix2& rhs);         // add rhs and update this object
+    Matrix2&    operator-=(const Matrix2& rhs);         // subtract rhs and update this object
+    Vector2     operator*(const Vector2& rhs) const;    // multiplication: v' = M * v
+    Matrix2     operator*(const Matrix2& rhs) const;    // multiplication: M3 = M1 * M2
+    Matrix2&    operator*=(const Matrix2& rhs);         // multiplication: M1' = M1 * M2
+    bool        operator==(const Matrix2& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Matrix2& rhs) const;   // exact compare, no epsilon
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Matrix2 operator-(const Matrix2& m);                     // unary operator (-)
+    friend Matrix2 operator*(float scalar, const Matrix2& m);       // pre-multiplication
+    friend Vector2 operator*(const Vector2& vec, const Matrix2& m); // pre-multiplication
+    friend std::ostream& operator<<(std::ostream& os, const Matrix2& m);
+
+protected:
+
+private:
+    float m[4];
+
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// 3x3 matrix
+///////////////////////////////////////////////////////////////////////////
+class Matrix3
+{
+public:
+    // constructors
+    Matrix3();  // init with identity
+    Matrix3(const float src[9]);
+    Matrix3(float m0, float m1, float m2,           // 1st column
+            float m3, float m4, float m5,           // 2nd column
+            float m6, float m7, float m8);          // 3rd column
+
+    void        set(const float src[9]);
+    void        set(float m0, float m1, float m2,   // 1st column
+                    float m3, float m4, float m5,   // 2nd column
+                    float m6, float m7, float m8);  // 3rd column
+    void        setRow(int index, const float row[3]);
+    void        setRow(int index, const Vector3& v);
+    void        setColumn(int index, const float col[3]);
+    void        setColumn(int index, const Vector3& v);
+
+    const float* get() const;
+    float       getDeterminant();
+
+    Matrix3&    identity();
+    Matrix3&    transpose();                            // transpose itself and return reference
+    Matrix3&    invert();
+
+    // operators
+    Matrix3     operator+(const Matrix3& rhs) const;    // add rhs
+    Matrix3     operator-(const Matrix3& rhs) const;    // subtract rhs
+    Matrix3&    operator+=(const Matrix3& rhs);         // add rhs and update this object
+    Matrix3&    operator-=(const Matrix3& rhs);         // subtract rhs and update this object
+    Vector3     operator*(const Vector3& rhs) const;    // multiplication: v' = M * v
+    Matrix3     operator*(const Matrix3& rhs) const;    // multiplication: M3 = M1 * M2
+    Matrix3&    operator*=(const Matrix3& rhs);         // multiplication: M1' = M1 * M2
+    bool        operator==(const Matrix3& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Matrix3& rhs) const;   // exact compare, no epsilon
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Matrix3 operator-(const Matrix3& m);                     // unary operator (-)
+    friend Matrix3 operator*(float scalar, const Matrix3& m);       // pre-multiplication
+    friend Vector3 operator*(const Vector3& vec, const Matrix3& m); // pre-multiplication
+    friend std::ostream& operator<<(std::ostream& os, const Matrix3& m);
+
+protected:
+
+private:
+    float m[9];
+
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// 4x4 matrix
+///////////////////////////////////////////////////////////////////////////
+class Matrix4
+{
+public:
+    // constructors
+    Matrix4();  // init with identity
+    Matrix4(const float src[16]);
+    Matrix4(float m00, float m01, float m02, float m03, // 1st column
+            float m04, float m05, float m06, float m07, // 2nd column
+            float m08, float m09, float m10, float m11, // 3rd column
+            float m12, float m13, float m14, float m15);// 4th column
+
+    void        set(const float src[16]);
+    void        set(float m00, float m01, float m02, float m03, // 1st column
+                    float m04, float m05, float m06, float m07, // 2nd column
+                    float m08, float m09, float m10, float m11, // 3rd column
+                    float m12, float m13, float m14, float m15);// 4th column
+    void        setRow(int index, const float row[4]);
+    void        setRow(int index, const Vector4& v);
+    void        setRow(int index, const Vector3& v);
+    void        setColumn(int index, const float col[4]);
+    void        setColumn(int index, const Vector4& v);
+    void        setColumn(int index, const Vector3& v);
+
+    const float* get() const;
+    const float* getTranspose();                        // return transposed matrix
+    float        getDeterminant();
+
+    Matrix4&    identity();
+    Matrix4&    transpose();                            // transpose itself and return reference
+    Matrix4&    invert();                               // check best inverse method before inverse
+    Matrix4&    invertEuclidean();                      // inverse of Euclidean transform matrix
+    Matrix4&    invertAffine();                         // inverse of affine transform matrix
+    Matrix4&    invertProjective();                     // inverse of projective matrix using partitioning
+    Matrix4&    invertGeneral();                        // inverse of generic matrix
+
+    // transform matrix
+    Matrix4&    translate(float x, float y, float z);   // translation by (x,y,z)
+    Matrix4&    translate(const Vector3& v);            //
+    Matrix4&    rotate(float angle, const Vector3& axis); // rotate angle(degree) along the given axix
+    Matrix4&    rotate(float angle, float x, float y, float z);
+    Matrix4&    rotateX(float angle);                   // rotate on X-axis with degree
+    Matrix4&    rotateY(float angle);                   // rotate on Y-axis with degree
+    Matrix4&    rotateZ(float angle);                   // rotate on Z-axis with degree
+    Matrix4&    scale(float scale);                     // uniform scale
+    Matrix4&    scale(float sx, float sy, float sz);    // scale by (sx, sy, sz) on each axis
+
+    // operators
+    Matrix4     operator+(const Matrix4& rhs) const;    // add rhs
+    Matrix4     operator-(const Matrix4& rhs) const;    // subtract rhs
+    Matrix4&    operator+=(const Matrix4& rhs);         // add rhs and update this object
+    Matrix4&    operator-=(const Matrix4& rhs);         // subtract rhs and update this object
+    Vector4     operator*(const Vector4& rhs) const;    // multiplication: v' = M * v
+    Vector3     operator*(const Vector3& rhs) const;    // multiplication: v' = M * v
+    Matrix4     operator*(const Matrix4& rhs) const;    // multiplication: M3 = M1 * M2
+    Matrix4&    operator*=(const Matrix4& rhs);         // multiplication: M1' = M1 * M2
+    bool        operator==(const Matrix4& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Matrix4& rhs) const;   // exact compare, no epsilon
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Matrix4 operator-(const Matrix4& m);                     // unary operator (-)
+    friend Matrix4 operator*(float scalar, const Matrix4& m);       // pre-multiplication
+    friend Vector3 operator*(const Vector3& vec, const Matrix4& m); // pre-multiplication
+    friend Vector4 operator*(const Vector4& vec, const Matrix4& m); // pre-multiplication
+    friend std::ostream& operator<<(std::ostream& os, const Matrix4& m);
+
+protected:
+
+private:
+    float       getCofactor(float m0, float m1, float m2,
+                            float m3, float m4, float m5,
+                            float m6, float m7, float m8);
+
+    float m[16];
+    float tm[16];                                       // transpose m
+
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// inline functions for Matrix2
+///////////////////////////////////////////////////////////////////////////
+inline Matrix2::Matrix2()
+{
+    // initially identity matrix
+    identity();
+}
+
+
+
+inline Matrix2::Matrix2(const float src[4])
+{
+    set(src);
+}
+
+
+
+inline Matrix2::Matrix2(float m0, float m1, float m2, float m3)
+{
+    set(m0, m1, m2, m3);
+}
+
+
+
+inline void Matrix2::set(const float src[4])
+{
+    m[0] = src[0];  m[1] = src[1];  m[2] = src[2];  m[3] = src[3];
+}
+
+
+
+inline void Matrix2::set(float m0, float m1, float m2, float m3)
+{
+    m[0]= m0;  m[1] = m1;  m[2] = m2;  m[3]= m3;
+}
+
+
+
+inline void Matrix2::setRow(int index, const float row[2])
+{
+    m[index] = row[0];  m[index + 2] = row[1];
+}
+
+
+
+inline void Matrix2::setRow(int index, const Vector2& v)
+{
+    m[index] = v.x;  m[index + 2] = v.y;
+}
+
+
+
+inline void Matrix2::setColumn(int index, const float col[2])
+{
+    m[index*2] = col[0];  m[index*2 + 1] = col[1];
+}
+
+
+
+inline void Matrix2::setColumn(int index, const Vector2& v)
+{
+    m[index*2] = v.x;  m[index*2 + 1] = v.y;
+}
+
+
+
+inline const float* Matrix2::get() const
+{
+    return m;
+}
+
+
+
+inline Matrix2& Matrix2::identity()
+{
+    m[0] = m[3] = 1.0f;
+    m[1] = m[2] = 0.0f;
+    return *this;
+}
+
+
+
+inline Matrix2 Matrix2::operator+(const Matrix2& rhs) const
+{
+    return Matrix2(m[0]+rhs[0], m[1]+rhs[1], m[2]+rhs[2], m[3]+rhs[3]);
+}
+
+
+
+inline Matrix2 Matrix2::operator-(const Matrix2& rhs) const
+{
+    return Matrix2(m[0]-rhs[0], m[1]-rhs[1], m[2]-rhs[2], m[3]-rhs[3]);
+}
+
+
+
+inline Matrix2& Matrix2::operator+=(const Matrix2& rhs)
+{
+    m[0] += rhs[0];  m[1] += rhs[1];  m[2] += rhs[2];  m[3] += rhs[3];
+    return *this;
+}
+
+
+
+inline Matrix2& Matrix2::operator-=(const Matrix2& rhs)
+{
+    m[0] -= rhs[0];  m[1] -= rhs[1];  m[2] -= rhs[2];  m[3] -= rhs[3];
+    return *this;
+}
+
+
+
+inline Vector2 Matrix2::operator*(const Vector2& rhs) const
+{
+    return Vector2(m[0]*rhs.x + m[2]*rhs.y,  m[1]*rhs.x + m[3]*rhs.y);
+}
+
+
+
+inline Matrix2 Matrix2::operator*(const Matrix2& rhs) const
+{
+    return Matrix2(m[0]*rhs[0] + m[2]*rhs[1],  m[1]*rhs[0] + m[3]*rhs[1],
+                   m[0]*rhs[2] + m[2]*rhs[3],  m[1]*rhs[2] + m[3]*rhs[3]);
+}
+
+
+
+inline Matrix2& Matrix2::operator*=(const Matrix2& rhs)
+{
+    *this = *this * rhs;
+    return *this;
+}
+
+
+
+inline bool Matrix2::operator==(const Matrix2& rhs) const
+{
+    return (m[0] == rhs[0]) && (m[1] == rhs[1]) && (m[2] == rhs[2]) && (m[3] == rhs[3]);
+}
+
+
+
+inline bool Matrix2::operator!=(const Matrix2& rhs) const
+{
+    return (m[0] != rhs[0]) || (m[1] != rhs[1]) || (m[2] != rhs[2]) || (m[3] != rhs[3]);
+}
+
+
+
+inline float Matrix2::operator[](int index) const
+{
+    return m[index];
+}
+
+
+
+inline float& Matrix2::operator[](int index)
+{
+    return m[index];
+}
+
+
+
+inline Matrix2 operator-(const Matrix2& rhs)
+{
+    return Matrix2(-rhs[0], -rhs[1], -rhs[2], -rhs[3]);
+}
+
+
+
+inline Matrix2 operator*(float s, const Matrix2& rhs)
+{
+    return Matrix2(s*rhs[0], s*rhs[1], s*rhs[2], s*rhs[3]);
+}
+
+
+
+inline Vector2 operator*(const Vector2& v, const Matrix2& rhs)
+{
+    return Vector2(v.x*rhs[0] + v.y*rhs[1],  v.x*rhs[2] + v.y*rhs[3]);
+}
+
+
+
+inline std::ostream& operator<<(std::ostream& os, const Matrix2& m)
+{
+    os << std::fixed << std::setprecision(5);
+    os << "[" << std::setw(10) << m[0] << " " << std::setw(10) << m[2] << "]\n"
+       << "[" << std::setw(10) << m[1] << " " << std::setw(10) << m[3] << "]\n";
+    os << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
+    return os;
+}
+// END OF MATRIX2 INLINE //////////////////////////////////////////////////////
+
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// inline functions for Matrix3
+///////////////////////////////////////////////////////////////////////////
+inline Matrix3::Matrix3()
+{
+    // initially identity matrix
+    identity();
+}
+
+
+
+inline Matrix3::Matrix3(const float src[9])
+{
+    set(src);
+}
+
+
+
+inline Matrix3::Matrix3(float m0, float m1, float m2,
+                        float m3, float m4, float m5,
+                        float m6, float m7, float m8)
+{
+    set(m0, m1, m2,  m3, m4, m5,  m6, m7, m8);
+}
+
+
+
+inline void Matrix3::set(const float src[9])
+{
+    m[0] = src[0];  m[1] = src[1];  m[2] = src[2];
+    m[3] = src[3];  m[4] = src[4];  m[5] = src[5];
+    m[6] = src[6];  m[7] = src[7];  m[8] = src[8];
+}
+
+
+
+inline void Matrix3::set(float m0, float m1, float m2,
+                         float m3, float m4, float m5,
+                         float m6, float m7, float m8)
+{
+    m[0] = m0;  m[1] = m1;  m[2] = m2;
+    m[3] = m3;  m[4] = m4;  m[5] = m5;
+    m[6] = m6;  m[7] = m7;  m[8] = m8;
+}
+
+
+
+inline void Matrix3::setRow(int index, const float row[3])
+{
+    m[index] = row[0];  m[index + 3] = row[1];  m[index + 6] = row[2];
+}
+
+
+
+inline void Matrix3::setRow(int index, const Vector3& v)
+{
+    m[index] = v.x;  m[index + 3] = v.y;  m[index + 6] = v.z;
+}
+
+
+
+inline void Matrix3::setColumn(int index, const float col[3])
+{
+    m[index*3] = col[0];  m[index*3 + 1] = col[1];  m[index*3 + 2] = col[2];
+}
+
+
+
+inline void Matrix3::setColumn(int index, const Vector3& v)
+{
+    m[index*3] = v.x;  m[index*3 + 1] = v.y;  m[index*3 + 2] = v.z;
+}
+
+
+
+inline const float* Matrix3::get() const
+{
+    return m;
+}
+
+
+
+inline Matrix3& Matrix3::identity()
+{
+    m[0] = m[4] = m[8] = 1.0f;
+    m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0f;
+    return *this;
+}
+
+
+
+inline Matrix3 Matrix3::operator+(const Matrix3& rhs) const
+{
+    return Matrix3(m[0]+rhs[0], m[1]+rhs[1], m[2]+rhs[2],
+                   m[3]+rhs[3], m[4]+rhs[4], m[5]+rhs[5],
+                   m[6]+rhs[6], m[7]+rhs[7], m[8]+rhs[8]);
+}
+
+
+
+inline Matrix3 Matrix3::operator-(const Matrix3& rhs) const
+{
+    return Matrix3(m[0]-rhs[0], m[1]-rhs[1], m[2]-rhs[2],
+                   m[3]-rhs[3], m[4]-rhs[4], m[5]-rhs[5],
+                   m[6]-rhs[6], m[7]-rhs[7], m[8]-rhs[8]);
+}
+
+
+
+inline Matrix3& Matrix3::operator+=(const Matrix3& rhs)
+{
+    m[0] += rhs[0];  m[1] += rhs[1];  m[2] += rhs[2];
+    m[3] += rhs[3];  m[4] += rhs[4];  m[5] += rhs[5];
+    m[6] += rhs[6];  m[7] += rhs[7];  m[8] += rhs[8];
+    return *this;
+}
+
+
+
+inline Matrix3& Matrix3::operator-=(const Matrix3& rhs)
+{
+    m[0] -= rhs[0];  m[1] -= rhs[1];  m[2] -= rhs[2];
+    m[3] -= rhs[3];  m[4] -= rhs[4];  m[5] -= rhs[5];
+    m[6] -= rhs[6];  m[7] -= rhs[7];  m[8] -= rhs[8];
+    return *this;
+}
+
+
+
+inline Vector3 Matrix3::operator*(const Vector3& rhs) const
+{
+    return Vector3(m[0]*rhs.x + m[3]*rhs.y + m[6]*rhs.z,
+                   m[1]*rhs.x + m[4]*rhs.y + m[7]*rhs.z,
+                   m[2]*rhs.x + m[5]*rhs.y + m[8]*rhs.z);
+}
+
+
+
+inline Matrix3 Matrix3::operator*(const Matrix3& rhs) const
+{
+    return Matrix3(m[0]*rhs[0] + m[3]*rhs[1] + m[6]*rhs[2],  m[1]*rhs[0] + m[4]*rhs[1] + m[7]*rhs[2],  m[2]*rhs[0] + m[5]*rhs[1] + m[8]*rhs[2],
+                   m[0]*rhs[3] + m[3]*rhs[4] + m[6]*rhs[5],  m[1]*rhs[3] + m[4]*rhs[4] + m[7]*rhs[5],  m[2]*rhs[3] + m[5]*rhs[4] + m[8]*rhs[5],
+                   m[0]*rhs[6] + m[3]*rhs[7] + m[6]*rhs[8],  m[1]*rhs[6] + m[4]*rhs[7] + m[7]*rhs[8],  m[2]*rhs[6] + m[5]*rhs[7] + m[8]*rhs[8]);
+}
+
+
+
+inline Matrix3& Matrix3::operator*=(const Matrix3& rhs)
+{
+    *this = *this * rhs;
+    return *this;
+}
+
+
+
+inline bool Matrix3::operator==(const Matrix3& rhs) const
+{
+    return (m[0] == rhs[0]) && (m[1] == rhs[1]) && (m[2] == rhs[2]) &&
+           (m[3] == rhs[3]) && (m[4] == rhs[4]) && (m[5] == rhs[5]) &&
+           (m[6] == rhs[6]) && (m[7] == rhs[7]) && (m[8] == rhs[8]);
+}
+
+
+
+inline bool Matrix3::operator!=(const Matrix3& rhs) const
+{
+    return (m[0] != rhs[0]) || (m[1] != rhs[1]) || (m[2] != rhs[2]) ||
+           (m[3] != rhs[3]) || (m[4] != rhs[4]) || (m[5] != rhs[5]) ||
+           (m[6] != rhs[6]) || (m[7] != rhs[7]) || (m[8] != rhs[8]);
+}
+
+
+
+inline float Matrix3::operator[](int index) const
+{
+    return m[index];
+}
+
+
+
+inline float& Matrix3::operator[](int index)
+{
+    return m[index];
+}
+
+
+
+inline Matrix3 operator-(const Matrix3& rhs)
+{
+    return Matrix3(-rhs[0], -rhs[1], -rhs[2], -rhs[3], -rhs[4], -rhs[5], -rhs[6], -rhs[7], -rhs[8]);
+}
+
+
+
+inline Matrix3 operator*(float s, const Matrix3& rhs)
+{
+    return Matrix3(s*rhs[0], s*rhs[1], s*rhs[2], s*rhs[3], s*rhs[4], s*rhs[5], s*rhs[6], s*rhs[7], s*rhs[8]);
+}
+
+
+
+inline Vector3 operator*(const Vector3& v, const Matrix3& m)
+{
+    return Vector3(v.x*m[0] + v.y*m[1] + v.z*m[2],  v.x*m[3] + v.y*m[4] + v.z*m[5],  v.x*m[6] + v.y*m[7] + v.z*m[8]);
+}
+
+
+
+inline std::ostream& operator<<(std::ostream& os, const Matrix3& m)
+{
+    os << std::fixed << std::setprecision(5);
+    os << "[" << std::setw(10) << m[0] << " " << std::setw(10) << m[3] << " " << std::setw(10) << m[6] << "]\n"
+       << "[" << std::setw(10) << m[1] << " " << std::setw(10) << m[4] << " " << std::setw(10) << m[7] << "]\n"
+       << "[" << std::setw(10) << m[2] << " " << std::setw(10) << m[5] << " " << std::setw(10) << m[8] << "]\n";
+    os << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
+    return os;
+}
+// END OF MATRIX3 INLINE //////////////////////////////////////////////////////
+
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// inline functions for Matrix4
+///////////////////////////////////////////////////////////////////////////
+inline Matrix4::Matrix4()
+{
+    // initially identity matrix
+    identity();
+}
+
+
+
+inline Matrix4::Matrix4(const float src[16])
+{
+    set(src);
+}
+
+
+
+inline Matrix4::Matrix4(float m00, float m01, float m02, float m03,
+                        float m04, float m05, float m06, float m07,
+                        float m08, float m09, float m10, float m11,
+                        float m12, float m13, float m14, float m15)
+{
+    set(m00, m01, m02, m03,  m04, m05, m06, m07,  m08, m09, m10, m11,  m12, m13, m14, m15);
+}
+
+
+
+inline void Matrix4::set(const float src[16])
+{
+    m[0] = src[0];  m[1] = src[1];  m[2] = src[2];  m[3] = src[3];
+    m[4] = src[4];  m[5] = src[5];  m[6] = src[6];  m[7] = src[7];
+    m[8] = src[8];  m[9] = src[9];  m[10]= src[10]; m[11]= src[11];
+    m[12]= src[12]; m[13]= src[13]; m[14]= src[14]; m[15]= src[15];
+}
+
+
+
+inline void Matrix4::set(float m00, float m01, float m02, float m03,
+                         float m04, float m05, float m06, float m07,
+                         float m08, float m09, float m10, float m11,
+                         float m12, float m13, float m14, float m15)
+{
+    m[0] = m00;  m[1] = m01;  m[2] = m02;  m[3] = m03;
+    m[4] = m04;  m[5] = m05;  m[6] = m06;  m[7] = m07;
+    m[8] = m08;  m[9] = m09;  m[10]= m10;  m[11]= m11;
+    m[12]= m12;  m[13]= m13;  m[14]= m14;  m[15]= m15;
+}
+
+
+
+inline void Matrix4::setRow(int index, const float row[4])
+{
+    m[index] = row[0];  m[index + 4] = row[1];  m[index + 8] = row[2];  m[index + 12] = row[3];
+}
+
+
+
+inline void Matrix4::setRow(int index, const Vector4& v)
+{
+    m[index] = v.x;  m[index + 4] = v.y;  m[index + 8] = v.z;  m[index + 12] = v.w;
+}
+
+
+
+inline void Matrix4::setRow(int index, const Vector3& v)
+{
+    m[index] = v.x;  m[index + 4] = v.y;  m[index + 8] = v.z;
+}
+
+
+
+inline void Matrix4::setColumn(int index, const float col[4])
+{
+    m[index*4] = col[0];  m[index*4 + 1] = col[1];  m[index*4 + 2] = col[2];  m[index*4 + 3] = col[3];
+}
+
+
+
+inline void Matrix4::setColumn(int index, const Vector4& v)
+{
+    m[index*4] = v.x;  m[index*4 + 1] = v.y;  m[index*4 + 2] = v.z;  m[index*4 + 3] = v.w;
+}
+
+
+
+inline void Matrix4::setColumn(int index, const Vector3& v)
+{
+    m[index*4] = v.x;  m[index*4 + 1] = v.y;  m[index*4 + 2] = v.z;
+}
+
+
+
+inline const float* Matrix4::get() const
+{
+    return m;
+}
+
+
+
+inline const float* Matrix4::getTranspose()
+{
+    tm[0] = m[0];   tm[1] = m[4];   tm[2] = m[8];   tm[3] = m[12];
+    tm[4] = m[1];   tm[5] = m[5];   tm[6] = m[9];   tm[7] = m[13];
+    tm[8] = m[2];   tm[9] = m[6];   tm[10]= m[10];  tm[11]= m[14];
+    tm[12]= m[3];   tm[13]= m[7];   tm[14]= m[11];  tm[15]= m[15];
+    return tm;
+}
+
+
+
+inline Matrix4& Matrix4::identity()
+{
+    m[0] = m[5] = m[10] = m[15] = 1.0f;
+    m[1] = m[2] = m[3] = m[4] = m[6] = m[7] = m[8] = m[9] = m[11] = m[12] = m[13] = m[14] = 0.0f;
+    return *this;
+}
+
+
+
+inline Matrix4 Matrix4::operator+(const Matrix4& rhs) const
+{
+    return Matrix4(m[0]+rhs[0],   m[1]+rhs[1],   m[2]+rhs[2],   m[3]+rhs[3],
+                   m[4]+rhs[4],   m[5]+rhs[5],   m[6]+rhs[6],   m[7]+rhs[7],
+                   m[8]+rhs[8],   m[9]+rhs[9],   m[10]+rhs[10], m[11]+rhs[11],
+                   m[12]+rhs[12], m[13]+rhs[13], m[14]+rhs[14], m[15]+rhs[15]);
+}
+
+
+
+inline Matrix4 Matrix4::operator-(const Matrix4& rhs) const
+{
+    return Matrix4(m[0]-rhs[0],   m[1]-rhs[1],   m[2]-rhs[2],   m[3]-rhs[3],
+                   m[4]-rhs[4],   m[5]-rhs[5],   m[6]-rhs[6],   m[7]-rhs[7],
+                   m[8]-rhs[8],   m[9]-rhs[9],   m[10]-rhs[10], m[11]-rhs[11],
+                   m[12]-rhs[12], m[13]-rhs[13], m[14]-rhs[14], m[15]-rhs[15]);
+}
+
+
+
+inline Matrix4& Matrix4::operator+=(const Matrix4& rhs)
+{
+    m[0] += rhs[0];   m[1] += rhs[1];   m[2] += rhs[2];   m[3] += rhs[3];
+    m[4] += rhs[4];   m[5] += rhs[5];   m[6] += rhs[6];   m[7] += rhs[7];
+    m[8] += rhs[8];   m[9] += rhs[9];   m[10]+= rhs[10];  m[11]+= rhs[11];
+    m[12]+= rhs[12];  m[13]+= rhs[13];  m[14]+= rhs[14];  m[15]+= rhs[15];
+    return *this;
+}
+
+
+
+inline Matrix4& Matrix4::operator-=(const Matrix4& rhs)
+{
+    m[0] -= rhs[0];   m[1] -= rhs[1];   m[2] -= rhs[2];   m[3] -= rhs[3];
+    m[4] -= rhs[4];   m[5] -= rhs[5];   m[6] -= rhs[6];   m[7] -= rhs[7];
+    m[8] -= rhs[8];   m[9] -= rhs[9];   m[10]-= rhs[10];  m[11]-= rhs[11];
+    m[12]-= rhs[12];  m[13]-= rhs[13];  m[14]-= rhs[14];  m[15]-= rhs[15];
+    return *this;
+}
+
+
+
+inline Vector4 Matrix4::operator*(const Vector4& rhs) const
+{
+    return Vector4(m[0]*rhs.x + m[4]*rhs.y + m[8]*rhs.z  + m[12]*rhs.w,
+                   m[1]*rhs.x + m[5]*rhs.y + m[9]*rhs.z  + m[13]*rhs.w,
+                   m[2]*rhs.x + m[6]*rhs.y + m[10]*rhs.z + m[14]*rhs.w,
+                   m[3]*rhs.x + m[7]*rhs.y + m[11]*rhs.z + m[15]*rhs.w);
+}
+
+
+
+inline Vector3 Matrix4::operator*(const Vector3& rhs) const
+{
+    return Vector3(m[0]*rhs.x + m[4]*rhs.y + m[8]*rhs.z,
+                   m[1]*rhs.x + m[5]*rhs.y + m[9]*rhs.z,
+                   m[2]*rhs.x + m[6]*rhs.y + m[10]*rhs.z);
+}
+
+
+
+inline Matrix4 Matrix4::operator*(const Matrix4& n) const
+{
+    return Matrix4(m[0]*n[0]  + m[4]*n[1]  + m[8]*n[2]  + m[12]*n[3],   m[1]*n[0]  + m[5]*n[1]  + m[9]*n[2]  + m[13]*n[3],   m[2]*n[0]  + m[6]*n[1]  + m[10]*n[2]  + m[14]*n[3],   m[3]*n[0]  + m[7]*n[1]  + m[11]*n[2]  + m[15]*n[3],
+                   m[0]*n[4]  + m[4]*n[5]  + m[8]*n[6]  + m[12]*n[7],   m[1]*n[4]  + m[5]*n[5]  + m[9]*n[6]  + m[13]*n[7],   m[2]*n[4]  + m[6]*n[5]  + m[10]*n[6]  + m[14]*n[7],   m[3]*n[4]  + m[7]*n[5]  + m[11]*n[6]  + m[15]*n[7],
+                   m[0]*n[8]  + m[4]*n[9]  + m[8]*n[10] + m[12]*n[11],  m[1]*n[8]  + m[5]*n[9]  + m[9]*n[10] + m[13]*n[11],  m[2]*n[8]  + m[6]*n[9]  + m[10]*n[10] + m[14]*n[11],  m[3]*n[8]  + m[7]*n[9]  + m[11]*n[10] + m[15]*n[11],
+                   m[0]*n[12] + m[4]*n[13] + m[8]*n[14] + m[12]*n[15],  m[1]*n[12] + m[5]*n[13] + m[9]*n[14] + m[13]*n[15],  m[2]*n[12] + m[6]*n[13] + m[10]*n[14] + m[14]*n[15],  m[3]*n[12] + m[7]*n[13] + m[11]*n[14] + m[15]*n[15]);
+}
+
+
+
+inline Matrix4& Matrix4::operator*=(const Matrix4& rhs)
+{
+    *this = *this * rhs;
+    return *this;
+}
+
+
+
+inline bool Matrix4::operator==(const Matrix4& n) const
+{
+    return (m[0] == n[0])  && (m[1] == n[1])  && (m[2] == n[2])  && (m[3] == n[3])  &&
+           (m[4] == n[4])  && (m[5] == n[5])  && (m[6] == n[6])  && (m[7] == n[7])  &&
+           (m[8] == n[8])  && (m[9] == n[9])  && (m[10]== n[10]) && (m[11]== n[11]) &&
+           (m[12]== n[12]) && (m[13]== n[13]) && (m[14]== n[14]) && (m[15]== n[15]);
+}
+
+
+
+inline bool Matrix4::operator!=(const Matrix4& n) const
+{
+    return (m[0] != n[0])  || (m[1] != n[1])  || (m[2] != n[2])  || (m[3] != n[3])  ||
+           (m[4] != n[4])  || (m[5] != n[5])  || (m[6] != n[6])  || (m[7] != n[7])  ||
+           (m[8] != n[8])  || (m[9] != n[9])  || (m[10]!= n[10]) || (m[11]!= n[11]) ||
+           (m[12]!= n[12]) || (m[13]!= n[13]) || (m[14]!= n[14]) || (m[15]!= n[15]);
+}
+
+
+
+inline float Matrix4::operator[](int index) const
+{
+    return m[index];
+}
+
+
+
+inline float& Matrix4::operator[](int index)
+{
+    return m[index];
+}
+
+
+
+inline Matrix4 operator-(const Matrix4& rhs)
+{
+    return Matrix4(-rhs[0], -rhs[1], -rhs[2], -rhs[3], -rhs[4], -rhs[5], -rhs[6], -rhs[7], -rhs[8], -rhs[9], -rhs[10], -rhs[11], -rhs[12], -rhs[13], -rhs[14], -rhs[15]);
+}
+
+
+
+inline Matrix4 operator*(float s, const Matrix4& rhs)
+{
+    return Matrix4(s*rhs[0], s*rhs[1], s*rhs[2], s*rhs[3], s*rhs[4], s*rhs[5], s*rhs[6], s*rhs[7], s*rhs[8], s*rhs[9], s*rhs[10], s*rhs[11], s*rhs[12], s*rhs[13], s*rhs[14], s*rhs[15]);
+}
+
+
+
+inline Vector4 operator*(const Vector4& v, const Matrix4& m)
+{
+    return Vector4(v.x*m[0] + v.y*m[1] + v.z*m[2] + v.w*m[3],  v.x*m[4] + v.y*m[5] + v.z*m[6] + v.w*m[7],  v.x*m[8] + v.y*m[9] + v.z*m[10] + v.w*m[11], v.x*m[12] + v.y*m[13] + v.z*m[14] + v.w*m[15]);
+}
+
+
+
+inline Vector3 operator*(const Vector3& v, const Matrix4& m)
+{
+    return Vector3(v.x*m[0] + v.y*m[1] + v.z*m[2],  v.x*m[4] + v.y*m[5] + v.z*m[6],  v.x*m[8] + v.y*m[9] + v.z*m[10]);
+}
+
+
+
+inline std::ostream& operator<<(std::ostream& os, const Matrix4& m)
+{
+    os << std::fixed << std::setprecision(5);
+    os << "[" << std::setw(10) << m[0] << " " << std::setw(10) << m[4] << " " << std::setw(10) << m[8]  <<  " " << std::setw(10) << m[12] << "]\n"
+       << "[" << std::setw(10) << m[1] << " " << std::setw(10) << m[5] << " " << std::setw(10) << m[9]  <<  " " << std::setw(10) << m[13] << "]\n"
+       << "[" << std::setw(10) << m[2] << " " << std::setw(10) << m[6] << " " << std::setw(10) << m[10] <<  " " << std::setw(10) << m[14] << "]\n"
+       << "[" << std::setw(10) << m[3] << " " << std::setw(10) << m[7] << " " << std::setw(10) << m[11] <<  " " << std::setw(10) << m[15] << "]\n";
+    os << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
+    return os;
+}
+// END OF MATRIX4 INLINE //////////////////////////////////////////////////////
+#endif
diff --git a/shared/Vectors.h b/shared/Vectors.h
new file mode 100644
index 0000000..2efb840
--- /dev/null
+++ b/shared/Vectors.h
@@ -0,0 +1,530 @@
+///////////////////////////////////////////////////////////////////////////////
+// Vectors.h
+// =========
+// 2D/3D/4D vectors
+//
+//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
+// CREATED: 2007-02-14
+// UPDATED: 2013-01-20
+//
+// Copyright (C) 2007-2013 Song Ho Ahn
+///////////////////////////////////////////////////////////////////////////////
+
+
+#ifndef VECTORS_H_DEF
+#define VECTORS_H_DEF
+
+#include <cmath>
+#include <iostream>
+
+///////////////////////////////////////////////////////////////////////////////
+// 2D vector
+///////////////////////////////////////////////////////////////////////////////
+struct Vector2
+{
+    float x;
+    float y;
+
+    // ctors
+    Vector2() : x(0), y(0) {};
+    Vector2(float x, float y) : x(x), y(y) {};
+
+    // utils functions
+    void        set(float x, float y);
+    float       length() const;                         //
+    float       distance(const Vector2& vec) const;     // distance between two vectors
+    Vector2&    normalize();                            //
+    float       dot(const Vector2& vec) const;          // dot product
+    bool        equal(const Vector2& vec, float e) const; // compare with epsilon
+
+    // operators
+    Vector2     operator-() const;                      // unary operator (negate)
+    Vector2     operator+(const Vector2& rhs) const;    // add rhs
+    Vector2     operator-(const Vector2& rhs) const;    // subtract rhs
+    Vector2&    operator+=(const Vector2& rhs);         // add rhs and update this object
+    Vector2&    operator-=(const Vector2& rhs);         // subtract rhs and update this object
+    Vector2     operator*(const float scale) const;     // scale
+    Vector2     operator*(const Vector2& rhs) const;    // multiply each element
+    Vector2&    operator*=(const float scale);          // scale and update this object
+    Vector2&    operator*=(const Vector2& rhs);         // multiply each element and update this object
+    Vector2     operator/(const float scale) const;     // inverse scale
+    Vector2&    operator/=(const float scale);          // scale and update this object
+    bool        operator==(const Vector2& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Vector2& rhs) const;   // exact compare, no epsilon
+    bool        operator<(const Vector2& rhs) const;    // comparison for sort
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Vector2 operator*(const float a, const Vector2 vec);
+    friend std::ostream& operator<<(std::ostream& os, const Vector2& vec);
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// 3D vector
+///////////////////////////////////////////////////////////////////////////////
+struct Vector3
+{
+    float x;
+    float y;
+    float z;
+
+    // ctors
+    Vector3() : x(0), y(0), z(0) {};
+    Vector3(float x, float y, float z) : x(x), y(y), z(z) {};
+
+    // utils functions
+    void        set(float x, float y, float z);
+    float       length() const;                         //
+    float       distance(const Vector3& vec) const;     // distance between two vectors
+    Vector3&    normalize();                            //
+    float       dot(const Vector3& vec) const;          // dot product
+    Vector3     cross(const Vector3& vec) const;        // cross product
+    bool        equal(const Vector3& vec, float e) const; // compare with epsilon
+
+    // operators
+    Vector3     operator-() const;                      // unary operator (negate)
+    Vector3     operator+(const Vector3& rhs) const;    // add rhs
+    Vector3     operator-(const Vector3& rhs) const;    // subtract rhs
+    Vector3&    operator+=(const Vector3& rhs);         // add rhs and update this object
+    Vector3&    operator-=(const Vector3& rhs);         // subtract rhs and update this object
+    Vector3     operator*(const float scale) const;     // scale
+    Vector3     operator*(const Vector3& rhs) const;    // multiplay each element
+    Vector3&    operator*=(const float scale);          // scale and update this object
+    Vector3&    operator*=(const Vector3& rhs);         // product each element and update this object
+    Vector3     operator/(const float scale) const;     // inverse scale
+    Vector3&    operator/=(const float scale);          // scale and update this object
+    bool        operator==(const Vector3& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Vector3& rhs) const;   // exact compare, no epsilon
+    bool        operator<(const Vector3& rhs) const;    // comparison for sort
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Vector3 operator*(const float a, const Vector3 vec);
+    friend std::ostream& operator<<(std::ostream& os, const Vector3& vec);
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// 4D vector
+///////////////////////////////////////////////////////////////////////////////
+struct Vector4
+{
+    float x;
+    float y;
+    float z;
+    float w;
+
+    // ctors
+    Vector4() : x(0), y(0), z(0), w(0) {};
+    Vector4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {};
+
+    // utils functions
+    void        set(float x, float y, float z, float w);
+    float       length() const;                         //
+    float       distance(const Vector4& vec) const;     // distance between two vectors
+    Vector4&    normalize();                            //
+    float       dot(const Vector4& vec) const;          // dot product
+    bool        equal(const Vector4& vec, float e) const; // compare with epsilon
+
+    // operators
+    Vector4     operator-() const;                      // unary operator (negate)
+    Vector4     operator+(const Vector4& rhs) const;    // add rhs
+    Vector4     operator-(const Vector4& rhs) const;    // subtract rhs
+    Vector4&    operator+=(const Vector4& rhs);         // add rhs and update this object
+    Vector4&    operator-=(const Vector4& rhs);         // subtract rhs and update this object
+    Vector4     operator*(const float scale) const;     // scale
+    Vector4     operator*(const Vector4& rhs) const;    // multiply each element
+    Vector4&    operator*=(const float scale);          // scale and update this object
+    Vector4&    operator*=(const Vector4& rhs);         // multiply each element and update this object
+    Vector4     operator/(const float scale) const;     // inverse scale
+    Vector4&    operator/=(const float scale);          // scale and update this object
+    bool        operator==(const Vector4& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Vector4& rhs) const;   // exact compare, no epsilon
+    bool        operator<(const Vector4& rhs) const;    // comparison for sort
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Vector4 operator*(const float a, const Vector4 vec);
+    friend std::ostream& operator<<(std::ostream& os, const Vector4& vec);
+};
+
+
+
+// fast math routines from Doom3 SDK
+inline float invSqrt(float x)
+{
+    float xhalf = 0.5f * x;
+    int i = *(int*)&x;          // get bits for floating value
+    i = 0x5f3759df - (i>>1);    // gives initial guess
+    x = *(float*)&i;            // convert bits back to float
+    x = x * (1.5f - xhalf*x*x); // Newton step
+    return x;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inline functions for Vector2
+///////////////////////////////////////////////////////////////////////////////
+inline Vector2 Vector2::operator-() const {
+    return Vector2(-x, -y);
+}
+
+inline Vector2 Vector2::operator+(const Vector2& rhs) const {
+    return Vector2(x+rhs.x, y+rhs.y);
+}
+
+inline Vector2 Vector2::operator-(const Vector2& rhs) const {
+    return Vector2(x-rhs.x, y-rhs.y);
+}
+
+inline Vector2& Vector2::operator+=(const Vector2& rhs) {
+    x += rhs.x; y += rhs.y; return *this;
+}
+
+inline Vector2& Vector2::operator-=(const Vector2& rhs) {
+    x -= rhs.x; y -= rhs.y; return *this;
+}
+
+inline Vector2 Vector2::operator*(const float a) const {
+    return Vector2(x*a, y*a);
+}
+
+inline Vector2 Vector2::operator*(const Vector2& rhs) const {
+    return Vector2(x*rhs.x, y*rhs.y);
+}
+
+inline Vector2& Vector2::operator*=(const float a) {
+    x *= a; y *= a; return *this;
+}
+
+inline Vector2& Vector2::operator*=(const Vector2& rhs) {
+    x *= rhs.x; y *= rhs.y; return *this;
+}
+
+inline Vector2 Vector2::operator/(const float a) const {
+    return Vector2(x/a, y/a);
+}
+
+inline Vector2& Vector2::operator/=(const float a) {
+    x /= a; y /= a; return *this;
+}
+
+inline bool Vector2::operator==(const Vector2& rhs) const {
+    return (x == rhs.x) && (y == rhs.y);
+}
+
+inline bool Vector2::operator!=(const Vector2& rhs) const {
+    return (x != rhs.x) || (y != rhs.y);
+}
+
+inline bool Vector2::operator<(const Vector2& rhs) const {
+    if(x < rhs.x) return true;
+    if(x > rhs.x) return false;
+    if(y < rhs.y) return true;
+    if(y > rhs.y) return false;
+    return false;
+}
+
+inline float Vector2::operator[](int index) const {
+    return (&x)[index];
+}
+
+inline float& Vector2::operator[](int index) {
+    return (&x)[index];
+}
+
+inline void Vector2::set(float x_, float y_) {
+    this->x = x_; this->y = y_;
+}
+
+inline float Vector2::length() const {
+    return sqrtf(x*x + y*y);
+}
+
+inline float Vector2::distance(const Vector2& vec) const {
+    return sqrtf((vec.x-x)*(vec.x-x) + (vec.y-y)*(vec.y-y));
+}
+
+inline Vector2& Vector2::normalize() {
+    //@@const float EPSILON = 0.000001f;
+    float xxyy = x*x + y*y;
+    //@@if(xxyy < EPSILON)
+    //@@    return *this;
+
+    //float invLength = invSqrt(xxyy);
+    float invLength = 1.0f / sqrtf(xxyy);
+    x *= invLength;
+    y *= invLength;
+    return *this;
+}
+
+inline float Vector2::dot(const Vector2& rhs) const {
+    return (x*rhs.x + y*rhs.y);
+}
+
+inline bool Vector2::equal(const Vector2& rhs, float epsilon) const {
+    return fabs(x - rhs.x) < epsilon && fabs(y - rhs.y) < epsilon;
+}
+
+inline Vector2 operator*(const float a, const Vector2 vec) {
+    return Vector2(a*vec.x, a*vec.y);
+}
+
+inline std::ostream& operator<<(std::ostream& os, const Vector2& vec) {
+    os << "(" << vec.x << ", " << vec.y << ")";
+    return os;
+}
+// END OF VECTOR2 /////////////////////////////////////////////////////////////
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inline functions for Vector3
+///////////////////////////////////////////////////////////////////////////////
+inline Vector3 Vector3::operator-() const {
+    return Vector3(-x, -y, -z);
+}
+
+inline Vector3 Vector3::operator+(const Vector3& rhs) const {
+    return Vector3(x+rhs.x, y+rhs.y, z+rhs.z);
+}
+
+inline Vector3 Vector3::operator-(const Vector3& rhs) const {
+    return Vector3(x-rhs.x, y-rhs.y, z-rhs.z);
+}
+
+inline Vector3& Vector3::operator+=(const Vector3& rhs) {
+    x += rhs.x; y += rhs.y; z += rhs.z; return *this;
+}
+
+inline Vector3& Vector3::operator-=(const Vector3& rhs) {
+    x -= rhs.x; y -= rhs.y; z -= rhs.z; return *this;
+}
+
+inline Vector3 Vector3::operator*(const float a) const {
+    return Vector3(x*a, y*a, z*a);
+}
+
+inline Vector3 Vector3::operator*(const Vector3& rhs) const {
+    return Vector3(x*rhs.x, y*rhs.y, z*rhs.z);
+}
+
+inline Vector3& Vector3::operator*=(const float a) {
+    x *= a; y *= a; z *= a; return *this;
+}
+
+inline Vector3& Vector3::operator*=(const Vector3& rhs) {
+    x *= rhs.x; y *= rhs.y; z *= rhs.z; return *this;
+}
+
+inline Vector3 Vector3::operator/(const float a) const {
+    return Vector3(x/a, y/a, z/a);
+}
+
+inline Vector3& Vector3::operator/=(const float a) {
+    x /= a; y /= a; z /= a; return *this;
+}
+
+inline bool Vector3::operator==(const Vector3& rhs) const {
+    return (x == rhs.x) && (y == rhs.y) && (z == rhs.z);
+}
+
+inline bool Vector3::operator!=(const Vector3& rhs) const {
+    return (x != rhs.x) || (y != rhs.y) || (z != rhs.z);
+}
+
+inline bool Vector3::operator<(const Vector3& rhs) const {
+    if(x < rhs.x) return true;
+    if(x > rhs.x) return false;
+    if(y < rhs.y) return true;
+    if(y > rhs.y) return false;
+    if(z < rhs.z) return true;
+    if(z > rhs.z) return false;
+    return false;
+}
+
+inline float Vector3::operator[](int index) const {
+    return (&x)[index];
+}
+
+inline float& Vector3::operator[](int index) {
+    return (&x)[index];
+}
+
+inline void Vector3::set(float x_, float y_, float z_) {
+    this->x = x_; this->y = y_; this->z = z_;
+}
+
+inline float Vector3::length() const {
+    return sqrtf(x*x + y*y + z*z);
+}
+
+inline float Vector3::distance(const Vector3& vec) const {
+    return sqrtf((vec.x-x)*(vec.x-x) + (vec.y-y)*(vec.y-y) + (vec.z-z)*(vec.z-z));
+}
+
+inline Vector3& Vector3::normalize() {
+    //@@const float EPSILON = 0.000001f;
+    float xxyyzz = x*x + y*y + z*z;
+    //@@if(xxyyzz < EPSILON)
+    //@@    return *this; // do nothing if it is ~zero vector
+
+    //float invLength = invSqrt(xxyyzz);
+    float invLength = 1.0f / sqrtf(xxyyzz);
+    x *= invLength;
+    y *= invLength;
+    z *= invLength;
+    return *this;
+}
+
+inline float Vector3::dot(const Vector3& rhs) const {
+    return (x*rhs.x + y*rhs.y + z*rhs.z);
+}
+
+inline Vector3 Vector3::cross(const Vector3& rhs) const {
+    return Vector3(y*rhs.z - z*rhs.y, z*rhs.x - x*rhs.z, x*rhs.y - y*rhs.x);
+}
+
+inline bool Vector3::equal(const Vector3& rhs, float epsilon) const {
+    return fabs(x - rhs.x) < epsilon && fabs(y - rhs.y) < epsilon && fabs(z - rhs.z) < epsilon;
+}
+
+inline Vector3 operator*(const float a, const Vector3 vec) {
+    return Vector3(a*vec.x, a*vec.y, a*vec.z);
+}
+
+inline std::ostream& operator<<(std::ostream& os, const Vector3& vec) {
+    os << "(" << vec.x << ", " << vec.y << ", " << vec.z << ")";
+    return os;
+}
+// END OF VECTOR3 /////////////////////////////////////////////////////////////
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inline functions for Vector4
+///////////////////////////////////////////////////////////////////////////////
+inline Vector4 Vector4::operator-() const {
+    return Vector4(-x, -y, -z, -w);
+}
+
+inline Vector4 Vector4::operator+(const Vector4& rhs) const {
+    return Vector4(x+rhs.x, y+rhs.y, z+rhs.z, w+rhs.w);
+}
+
+inline Vector4 Vector4::operator-(const Vector4& rhs) const {
+    return Vector4(x-rhs.x, y-rhs.y, z-rhs.z, w-rhs.w);
+}
+
+inline Vector4& Vector4::operator+=(const Vector4& rhs) {
+    x += rhs.x; y += rhs.y; z += rhs.z; w += rhs.w; return *this;
+}
+
+inline Vector4& Vector4::operator-=(const Vector4& rhs) {
+    x -= rhs.x; y -= rhs.y; z -= rhs.z; w -= rhs.w; return *this;
+}
+
+inline Vector4 Vector4::operator*(const float a) const {
+    return Vector4(x*a, y*a, z*a, w*a);
+}
+
+inline Vector4 Vector4::operator*(const Vector4& rhs) const {
+    return Vector4(x*rhs.x, y*rhs.y, z*rhs.z, w*rhs.w);
+}
+
+inline Vector4& Vector4::operator*=(const float a) {
+    x *= a; y *= a; z *= a; w *= a; return *this;
+}
+
+inline Vector4& Vector4::operator*=(const Vector4& rhs) {
+    x *= rhs.x; y *= rhs.y; z *= rhs.z; w *= rhs.w; return *this;
+}
+
+inline Vector4 Vector4::operator/(const float a) const {
+    return Vector4(x/a, y/a, z/a, w/a);
+}
+
+inline Vector4& Vector4::operator/=(const float a) {
+    x /= a; y /= a; z /= a; w /= a; return *this;
+}
+
+inline bool Vector4::operator==(const Vector4& rhs) const {
+    return (x == rhs.x) && (y == rhs.y) && (z == rhs.z) && (w == rhs.w);
+}
+
+inline bool Vector4::operator!=(const Vector4& rhs) const {
+    return (x != rhs.x) || (y != rhs.y) || (z != rhs.z) || (w != rhs.w);
+}
+
+inline bool Vector4::operator<(const Vector4& rhs) const {
+    if(x < rhs.x) return true;
+    if(x > rhs.x) return false;
+    if(y < rhs.y) return true;
+    if(y > rhs.y) return false;
+    if(z < rhs.z) return true;
+    if(z > rhs.z) return false;
+    if(w < rhs.w) return true;
+    if(w > rhs.w) return false;
+    return false;
+}
+
+inline float Vector4::operator[](int index) const {
+    return (&x)[index];
+}
+
+inline float& Vector4::operator[](int index) {
+    return (&x)[index];
+}
+
+inline void Vector4::set(float x_, float y_, float z_, float w_) {
+    this->x = x_; this->y = y_; this->z = z_; this->w = w_;
+}
+
+inline float Vector4::length() const {
+    return sqrtf(x*x + y*y + z*z + w*w);
+}
+
+inline float Vector4::distance(const Vector4& vec) const {
+    return sqrtf((vec.x-x)*(vec.x-x) + (vec.y-y)*(vec.y-y) + (vec.z-z)*(vec.z-z) + (vec.w-w)*(vec.w-w));
+}
+
+inline Vector4& Vector4::normalize() {
+    //NOTE: leave w-component untouched
+    //@@const float EPSILON = 0.000001f;
+    float xxyyzz = x*x + y*y + z*z;
+    //@@if(xxyyzz < EPSILON)
+    //@@    return *this; // do nothing if it is zero vector
+
+    //float invLength = invSqrt(xxyyzz);
+    float invLength = 1.0f / sqrtf(xxyyzz);
+    x *= invLength;
+    y *= invLength;
+    z *= invLength;
+    return *this;
+}
+
+inline float Vector4::dot(const Vector4& rhs) const {
+    return (x*rhs.x + y*rhs.y + z*rhs.z + w*rhs.w);
+}
+
+inline bool Vector4::equal(const Vector4& rhs, float epsilon) const {
+    return fabs(x - rhs.x) < epsilon && fabs(y - rhs.y) < epsilon &&
+           fabs(z - rhs.z) < epsilon && fabs(w - rhs.w) < epsilon;
+}
+
+inline Vector4 operator*(const float a, const Vector4 vec) {
+    return Vector4(a*vec.x, a*vec.y, a*vec.z, a*vec.w);
+}
+
+inline std::ostream& operator<<(std::ostream& os, const Vector4& vec) {
+    os << "(" << vec.x << ", " << vec.y << ", " << vec.z << ", " << vec.w << ")";
+    return os;
+}
+// END OF VECTOR4 /////////////////////////////////////////////////////////////
+
+#endif