Added Matrix and Vector3d into utils

include/utils/Matrix.h

@@ -0,0 +1,201 @@
+/*!
+ * \file include/utils/Matrix.h
+ * \brief 3D Matrix
+ *
+ * \author Mongoose
+ */
+
+#ifndef _UTILS_MATRIX_H_
+#define _UTILS_MATRIX_H_
+
+#include "utils/math.h"
+#include "utils/Quaternion.h"
+#include "utils/Vector3d.h"
+
+
+/*!
+ * \brief 3D Matrix
+ *
+ * Multidim map for row order encoding
+ *
+ *     ///////////////////////////////////////////////
+ *     // 0,0 - 0;   0,1 - 1;   0,2 - 2;   0,3 - 3  //
+ *     // 1,0 - 4;   1,1 - 5;   1,2 - 6;   1,3 - 7  //
+ *     // 2,0 - 8;   2,1 - 9;   2,2 - 10;  2,3 - 11 //
+ *     // 3,0 - 12;  3,1 - 13;  3,2 - 14;  3,3 - 15 //
+ *     ///////////////////////////////////////////////
+ *
+ * Multidim map for column order encoding
+ *
+ *     ///////////////////////////////////////////////
+ *     // 0,0 - 0;   0,1 - 4;   0,2 - 8;   0,3 - 12 //
+ *     // 1,0 - 1;   1,1 - 5;   1,2 - 9;   1,3 - 13 //
+ *     // 2,0 - 2;   2,1 - 6;   2,2 - 10;  2,3 - 14 //
+ *     // 3,0 - 3;   3,1 - 7;   3,2 - 11;  3,3 - 15 //
+ *     ///////////////////////////////////////////////
+ */
+class Matrix {
+public:
+
+    /*!
+     * \brief Constructs an object of Matrix
+     */
+    Matrix();
+
+    /*!
+     * \brief Constructs an object of Matrix
+     * \param mat Matrix as data source
+     */
+    Matrix(matrix_t mat);
+
+    /*!
+     * \brief Constructs an object of Matrix
+     * \param q Converts and assigns the Quaternion to the Matrix
+     */
+    Matrix(Quaternion &q);
+
+    /*!
+     * \brief Returns this matrix copy
+     * \param mat target
+     */
+    void getMatrix(matrix_t mat);
+
+    /*!
+     * \brief Returns this matrix transposed
+     * \param mat target
+     */
+    void getTransposeMatrix(matrix_t mat);
+
+    /*!
+     * \brief Returns this matrix inverted
+     * \param mat target
+     */
+    bool getInvert(matrix_t mat);
+
+    /*!
+     * \brief Multiplies two matrices
+     * \param a first matrix
+     * \param b second matrix
+     * \returns resultant matrix
+     */
+    static Matrix multiply(const Matrix &a, const Matrix &b);
+
+    /*!
+     * \brief Multiplies v vector and this matrix
+     * \param v vector
+     * \param result where the result will be stored, may be same as v
+     */
+    void multiply4v(vec4_t v, vec4_t result);
+
+    /*!
+     * \brief Multiplies v vector and this matrix
+     * \param v vector
+     * \param result where the result will be stored, may be same as v
+     */
+    void multiply3v(vec3_t v, vec3_t result);
+
+    /*!
+     * \brief Prints matrix values to stdout
+     */
+    void print();
+
+    /*!
+     * \brief Is this matrix the identity matrix?
+     * \returns true if it is identity, false otherwise
+     */
+    bool isIdentity();
+
+    /*!
+     * \brief Multiplies a and this matrix
+     * \param a matrix to multiply with
+     * \returns resultant matrix
+     */
+    Matrix operator *(const Matrix &a);
+
+    /*!
+     * \brief Multiply vector by this matrix
+     * \param v Vector to multiply with
+     * \returns resultant vector (mult)
+     */
+    Vector3d operator *(Vector3d v);
+
+    /*!
+     * \brief Sets to identity matrix
+     */
+    void setIdentity();
+
+    /*!
+     * \brief S et the matrix
+     * \fixme dangerous, scary, boo!
+     * \param mat new matrix
+     */
+    void setMatrix(matrix_t mat);
+
+    /*!
+     * \brief Rotate object in 3D space
+     * \param x x rotation in radians
+     * \param y y rotation in radians
+     * \param z z rotation in radians
+     */
+    void rotate(vec_t x, vec_t y, vec_t z);
+
+    /*!
+     * \brief Rotate object in 3D space
+     * \param xyz rotation in radians
+     */
+    void rotate(const vec_t *xyz);
+
+    /*!
+     * \brief Scale object in 3D space
+     * \param x x scaling
+     * \param y y scaling
+     * \param z z scaling
+     */
+    void scale(vec_t x, vec_t y, vec_t z);
+
+    /*!
+     * \brief Scale object in 3D space
+     * \param xyz scaling factors
+     */
+    void scale(const vec_t *xyz);
+
+    /*!
+     * \brief Translate (move) object in 3D space
+     * \param x x translation
+     * \param y y translation
+     * \param z z translation
+     */
+    void translate(vec_t x, vec_t y, vec_t z);
+
+    /*!
+     * \brief Translate (move) object in 3D space
+     * \param xyz translations
+     */
+    void translate(const vec_t *xyz);
+
+    /*!
+     * \brief Transpose this matrix
+     */
+    void transpose();
+
+    matrix_t mMatrix; //!< Data model, moved public for faster external renderer feedback use
+
+private:
+
+    /*!
+     * \brief Copys value from source to dest
+     * \param source source
+     * \param dest destination
+     */
+    static void copy(matrix_t source, matrix_t dest);
+
+    /*!
+     * \brief Multiplies matrices a and b. Neither a or b is also the result.
+     * \param a first matrix
+     * \param b second matrix
+     * \param result wil be set to resultant matrix value
+     */
+    static void multiply(const matrix_t a, const matrix_t b, matrix_t result);
+};
+
+#endif

include/utils/Vector3d.h

@@ -0,0 +1,160 @@
+/*!
+ * \file include/utils/Vector3d.h
+ * \brief 3D Math vector
+ *
+ * \author Mongoose
+ */
+
+#ifndef _UTILS_VECTOR3D_H_
+#define _UTILS_VECTOR3D_H_
+
+#include "utils/math.h"
+
+/*!
+ * \brief 3D Math Vector
+ */
+class Vector3d {
+public:
+
+    /*!
+     * \brief Constructs an object of Vector3d
+     */
+    Vector3d();
+
+    /*!
+     * \brief Constructs an object of Vector3d
+     * \param v data to load into new Vector3d
+     */
+    Vector3d(vec3_t v);
+
+    /*!
+     * \brief Constructs an object of Vector3d
+     * \param x X part of new Vector3d
+     * \param y Y part of new Vector3d
+     * \param z Z part of new Vector3d
+     */
+    Vector3d(vec_t x, vec_t y, vec_t z);
+
+    /*!
+     * \brief Constructs an object of Vector3d
+     * \param v contents of new Vector3d
+     */
+    Vector3d(const Vector3d &v);
+
+    /*!
+     * \brief Calculate dot product
+     * \param u first argument
+     * \param v second argument
+     * \returns dot product of u and v vectors
+     */
+    static vec_t dot(const Vector3d &u, const Vector3d &v);
+
+    /*!
+     * \brief Calculate cross product
+     * \param u first argument
+     * \param v second argument
+     * \returns cross product of u and v vectors
+     */
+    static Vector3d cross(const Vector3d &u, const Vector3d &v);
+
+    /*!
+     * \brief Get Magnitude
+     * \returns magnitude of this vector
+     */
+    vec_t magnitude();
+
+    /*!
+     * \brief Normalize
+     * \returns normalized copy of this vector
+     */
+    Vector3d unit();
+
+    /*!
+     * \brief Get the Zero vector
+     * \returns (0, 0, 0) vector
+     */
+    static Vector3d zeroVector();
+
+    /*!
+     * \brief Add to this vector
+     * \param v addend
+     * \returns a vector = this vector + v
+     */
+    Vector3d operator +(const Vector3d &v);
+
+    /*!
+     * \brief Subtract from this vector
+     * \param v subtrahend
+     * \returns a vector = this vector - v
+     */
+    Vector3d operator -(const Vector3d &v);
+
+    /*!
+     * \brief Negate this vector
+     * \returns a copy of this vector, negated
+     */
+    Vector3d operator -();
+
+    /*!
+     * \brief Scale this vector
+     * \param s scaling factor
+     * \returns this vector multiplied with s
+     */
+    Vector3d operator *(vec_t s);
+
+    /*!
+     * \brief Scale this vactor
+     * \param s inverse scaling factor
+     * \returns this vector divided by s
+     */
+    Vector3d operator /(vec_t s);
+
+    /*!
+     * \brief Dot product this vector
+     * \param v second vector for dot product
+     * \returns dot product of V by this vector
+     */
+    vec_t operator *(const Vector3d &v);
+
+    /*!
+     * \brief Normalizes this vector
+     */
+    void normalize();
+
+    /*!
+     * \brief Set this vector to Zero (0, 0, 0)
+     */
+    void zero();
+
+    /*!
+     * \brief Set this vector
+     * \param v what this vector will be set to
+     * \returns this vector, now equal to v
+     */
+    Vector3d &operator =(const Vector3d &v);
+
+    /*!
+     * \brief Add to this vector, in place
+     * \param v what will be added to this vector
+     * \returns this vector, with v added
+     */
+    Vector3d &operator +=(const Vector3d &v);
+
+    /*!
+     * \brief Subtract from this vector, in place
+     * \param v what will be subtracted from this vector
+     * \returns this vector, with v subtracted
+     */
+    Vector3d &operator -=(const Vector3d &v);
+
+    /*!
+     * \brief Scale this vector, in place
+     * \param s scaling factor
+     * \returns this vactor multiplied by s
+     */
+    Vector3d &operator *=(vec_t s);
+
+    vec3_t mVec; //!< Vector data
+};
+
+#endif

src/utils/Matrix.cpp

@@ -0,0 +1,406 @@
+/*!
+ * \file src/utils/Matrix.cpp
+ * \brief 3D Matrix
+ *
+ * \author Mongoose
+ */
+
+#include <stdio.h>
+#include <math.h>
+
+#include "utils/Matrix.h"
+
+Matrix::Matrix() {
+    setIdentity();
+}
+
+Matrix::Matrix(matrix_t m) {
+    setMatrix(m);
+}
+
+Matrix::Matrix(Quaternion &q) {
+    matrix_t m;
+    q.getMatrix(m);
+    setMatrix(m);
+}
+
+bool Matrix::getInvert(matrix_t out) {
+    matrix_t m;
+
+#ifdef COLUMN_ORDER
+    getMatrix(m);
+#else
+    getTransposeMatrix(m);
+#endif
+
+    /* Mongoose: This code was from a Jeff Lander tutorial which was based
+       on MESA GL's InvertMatrix */
+
+    /* NB. OpenGL Matrices are COLUMN major. */
+#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
+#define MAT(m,r,c) (m)[(c)*4+(r)]
+
+    float wtmp[4][8];
+    float m0, m1, m2, m3, s;
+    float *r0, *r1, *r2, *r3;
+
+    r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
+
+    r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
+    r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
+    r0[4] = 1.0f, r0[5] = r0[6] = r0[7] = 0.0f,
+
+    r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
+    r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
+    r1[5] = 1.0f, r1[4] = r1[6] = r1[7] = 0.0f,
+
+    r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
+    r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
+    r2[6] = 1.0f, r2[4] = r2[5] = r2[7] = 0.0f,
+
+    r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
+    r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
+    r3[7] = 1.0f, r3[4] = r3[5] = r3[6] = 0.0f;
+
+    /* choose pivot - or die */
+    if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
+    if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
+    if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
+    if (0.0f == r0[0])  return false;
+
+    /* eliminate first variable     */
+    m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
+    s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
+    s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
+    s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
+    s = r0[4];
+    if (s != 0.0f) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
+    s = r0[5];
+    if (s != 0.0f) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
+    s = r0[6];
+    if (s != 0.0f) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
+    s = r0[7];
+    if (s != 0.0f) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
+
+    /* choose pivot - or die */
+    if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
+    if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
+    if (0.0f == r1[1])  return false;
+
+    /* eliminate second variable */
+    m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
+    r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
+    r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
+    s = r1[4]; if (0.0f != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
+    s = r1[5]; if (0.0f != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
+    s = r1[6]; if (0.0f != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
+    s = r1[7]; if (0.0f != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
+
+    /* choose pivot - or die */
+    if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
+    if (0.0f == r2[2])  return false;
+
+    /* eliminate third variable */
+    m3 = r3[2]/r2[2];
+    r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
+    r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
+    r3[7] -= m3 * r2[7];
+
+    /* last check */
+    if (0.0f == r3[3]) return false;
+
+    s = 1.0f/r3[3];              /* now back substitute row 3 */
+    r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
+
+    m2 = r2[3];                 /* now back substitute row 2 */
+    s  = 1.0f/r2[2];
+    r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
+    r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
+    m1 = r1[3];
+    r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
+    r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
+    m0 = r0[3];
+    r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
+    r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
+
+    m1 = r1[2];                 /* now back substitute row 1 */
+    s  = 1.0f/r1[1];
+    r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
+    r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
+    m0 = r0[2];
+    r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
+    r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
+
+    m0 = r0[1];                 /* now back substitute row 0 */
+    s  = 1.0f/r0[0];
+    r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
+    r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
+
+    MAT(out,0,0) = r0[4];
+    MAT(out,0,1) = r0[5], MAT(out,0,2) = r0[6];
+    MAT(out,0,3) = r0[7], MAT(out,1,0) = r1[4];
+    MAT(out,1,1) = r1[5], MAT(out,1,2) = r1[6];
+    MAT(out,1,3) = r1[7], MAT(out,2,0) = r2[4];
+    MAT(out,2,1) = r2[5], MAT(out,2,2) = r2[6];
+    MAT(out,2,3) = r2[7], MAT(out,3,0) = r3[4];
+    MAT(out,3,1) = r3[5], MAT(out,3,2) = r3[6];
+    MAT(out,3,3) = r3[7];
+
+    return true;
+#undef MAT
+#undef SWAP_ROWS
+}
+
+void Matrix::getMatrix(matrix_t mat) {
+    copy(mMatrix, mat);
+}
+
+void Matrix::getTransposeMatrix(matrix_t m) {
+    m[ 0]= mMatrix[0]; m[ 1]= mMatrix[4]; m[ 2]= mMatrix[ 8]; m[ 3]=mMatrix[12];
+    m[ 4]= mMatrix[1]; m[ 5]= mMatrix[5]; m[ 6]= mMatrix[ 9]; m[ 7]=mMatrix[13];
+    m[ 8]= mMatrix[2]; m[ 9]= mMatrix[6]; m[10]= mMatrix[10]; m[11]=mMatrix[14];
+    m[12]= mMatrix[3]; m[13]= mMatrix[7]; m[14]= mMatrix[11]; m[15]=mMatrix[15];
+}
+
+Matrix Matrix::multiply(const Matrix &a, const Matrix &b) {
+    Matrix c;
+    multiply(a.mMatrix, b.mMatrix, c.mMatrix);
+    return c;
+}
+
+Matrix Matrix::operator *(const Matrix &a) {
+    return multiply(a, *this);
+}
+
+Vector3d Matrix::operator *(Vector3d v) {
+    vec_t x = v.mVec[0], y = v.mVec[1], z = v.mVec[2];
+
+#ifdef COLUMN_ORDER
+    return Vector3d(mMatrix[0]*x + mMatrix[4]*y + mMatrix[ 8]*z + mMatrix[12],
+            mMatrix[1]*x + mMatrix[5]*y + mMatrix[ 9]*z + mMatrix[13],
+            mMatrix[2]*x + mMatrix[6]*y + mMatrix[10]*z + mMatrix[14]);
+#else
+    return Vector3d(mMatrix[0]*x + mMatrix[1]*y + mMatrix[ 2]*z + mMatrix[ 3],
+            mMatrix[4]*x + mMatrix[5]*y + mMatrix[ 6]*z + mMatrix[ 7],
+            mMatrix[8]*x + mMatrix[9]*y + mMatrix[10]*z + mMatrix[11]);
+#endif
+}
+
+void Matrix::multiply3v(vec3_t v, vec3_t result) {
+    vec_t x = v[0], y = v[1], z = v[2];
+
+    result[0] = mMatrix[0]*x + mMatrix[1]*y + mMatrix[ 2]*z + mMatrix[ 3];
+    result[1] = mMatrix[4]*x + mMatrix[5]*y + mMatrix[ 6]*z + mMatrix[ 7];
+    result[2] = mMatrix[8]*x + mMatrix[9]*y + mMatrix[10]*z + mMatrix[11];
+}
+
+void Matrix::multiply4v(vec4_t v, vec4_t result) {
+    vec_t x = v[0], y = v[1], z = v[2], w = v[3];
+
+    result[0] = mMatrix[ 0]*x + mMatrix[ 1]*y + mMatrix[ 2]*z + mMatrix[ 3]*w;
+    result[1] = mMatrix[ 4]*x + mMatrix[ 5]*y + mMatrix[ 6]*z + mMatrix[ 7]*w;
+    result[2] = mMatrix[ 8]*x + mMatrix[ 9]*y + mMatrix[10]*z + mMatrix[11]*w;
+    result[3] = mMatrix[12]*x + mMatrix[13]*y + mMatrix[14]*z + mMatrix[15]*w;
+}
+
+void Matrix::print() {
+    printf("{\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n}\n",
+#ifdef COLUMN_ORDER
+            mMatrix[0], mMatrix[4], mMatrix[ 8], mMatrix[12],
+            mMatrix[1], mMatrix[5], mMatrix[ 9], mMatrix[13],
+            mMatrix[2], mMatrix[6], mMatrix[10], mMatrix[14],
+            mMatrix[3], mMatrix[7], mMatrix[11], mMatrix[15]);
+#else
+            mMatrix[ 0], mMatrix[ 1], mMatrix[ 2], mMatrix[ 3],
+            mMatrix[ 4], mMatrix[ 5], mMatrix[ 6], mMatrix[ 7],
+            mMatrix[ 8], mMatrix[ 9], mMatrix[10], mMatrix[11],
+            mMatrix[12], mMatrix[13], mMatrix[14], mMatrix[15]);
+#endif
+}
+
+bool Matrix::isIdentity() {
+    // Hhhmm... floating point using direct comparisons
+    /*
+    if (mMatrix[ 0] == 1 && mMatrix[ 1] == 0 && mMatrix[ 2] == 0 &&
+            mMatrix[ 3] == 0 &&    mMatrix[ 4] == 0 && mMatrix[ 5] == 1 &&
+            mMatrix[ 6] == 0 && mMatrix[ 7] == 0 && mMatrix[ 8] == 0 &&
+            mMatrix[ 9] == 0 && mMatrix[10] == 1 && mMatrix[11] == 0 &&
+            mMatrix[12] == 0 && mMatrix[13] == 0 && mMatrix[14] == 0 &&
+            mMatrix[15] == 1)
+        return true;
+    */
+    if (equalEpsilon(mMatrix[ 0], 1.0) && equalEpsilon(mMatrix[ 1], 0.0) && equalEpsilon(mMatrix[ 2], 0.0) &&
+        equalEpsilon(mMatrix[ 3], 0.0) && equalEpsilon(mMatrix[ 4], 0.0) && equalEpsilon(mMatrix[ 5], 1.0) &&
+        equalEpsilon(mMatrix[ 6], 0.0) && equalEpsilon(mMatrix[ 7], 0.0) && equalEpsilon(mMatrix[ 8], 0.0) &&
+        equalEpsilon(mMatrix[ 9], 0.0) && equalEpsilon(mMatrix[10], 1.0) && equalEpsilon(mMatrix[11], 0.0) &&
+        equalEpsilon(mMatrix[12], 0.0) && equalEpsilon(mMatrix[13], 0.0) && equalEpsilon(mMatrix[14], 0.0) &&
+        equalEpsilon(mMatrix[15], 1.0))
+        return true;
+
+    return false;
+}
+
+void Matrix::setMatrix(matrix_t mat) {
+    copy(mat, mMatrix);
+}
+
+void Matrix::setIdentity() {
+    mMatrix[ 0] = 1; mMatrix[ 1] = 0; mMatrix[ 2] = 0; mMatrix[ 3] = 0;
+    mMatrix[ 4] = 0; mMatrix[ 5] = 1; mMatrix[ 6] = 0; mMatrix[ 7] = 0;
+    mMatrix[ 8] = 0; mMatrix[ 9] = 0; mMatrix[10] = 1; mMatrix[11] = 0;
+    mMatrix[12] = 0; mMatrix[13] = 0; mMatrix[14] = 0; mMatrix[15] = 1;
+}
+
+void Matrix::scale(const vec_t *xyz) {
+    scale(xyz[0], xyz[1], xyz[2]);
+}
+
+void Matrix::scale(vec_t sx, vec_t sy, vec_t sz) {
+    matrix_t smatrix;
+    matrix_t tmp;
+
+    smatrix[ 0] = sx; smatrix[ 1] = 0;  smatrix[ 2] = 0;  smatrix[ 3] = 0;
+    smatrix[ 4] = 0;  smatrix[ 5] = sy; smatrix[ 6] = 0;  smatrix[ 7] = 0;
+    smatrix[ 8] = 0;  smatrix[ 9] = 0;  smatrix[10] = sz; smatrix[11] = 0;
+    smatrix[12] = 0;  smatrix[13] = 0;  smatrix[14] = 0;  smatrix[15] = 1;
+
+    copy(mMatrix, tmp);
+    multiply(tmp, smatrix, mMatrix);
+}
+
+void Matrix::rotate(const vec_t *xyz) {
+    rotate(xyz[0], xyz[1], xyz[2]);
+}
+
+void Matrix::rotate(vec_t ax, vec_t ay, vec_t az) {
+    matrix_t xmat, ymat, zmat, tmp, tmp2;
+
+    xmat[ 0]=1;         xmat[ 1]=0;         xmat[ 2]=0;         xmat[ 3]=0;
+    xmat[ 4]=0;         xmat[ 5]=cosf(ax);  xmat[ 6]=sinf(ax);  xmat[ 7]=0;
+    xmat[ 8]=0;         xmat[ 9]=-sinf(ax); xmat[10]=cosf(ax);  xmat[11]=0;
+    xmat[12]=0;         xmat[13]=0;         xmat[14]=0;         xmat[15]=1;
+
+    ymat[ 0]=cosf(ay);  ymat[ 1]=0;         ymat[ 2]=-sinf(ay); ymat[ 3]=0;
+    ymat[ 4]=0;         ymat[ 5]=1;         ymat[ 6]=0;         ymat[ 7]=0;
+    ymat[ 8]=sinf(ay);  ymat[ 9]=0;         ymat[10]=cosf(ay);  ymat[11]=0;
+    ymat[12]=0;         ymat[13]=0;         ymat[14]=0;         ymat[15]=1;
+
+    zmat[ 0]=cosf(az);  zmat[ 1]=sinf(az);  zmat[ 2]=0;         zmat[ 3]=0;
+    zmat[ 4]=-sinf(az); zmat[ 5]=cosf(az);  zmat[ 6]=0;         zmat[ 7]=0;
+    zmat[ 8]=0;         zmat[ 9]=0;         zmat[10]=1;         zmat[11]=0;
+    zmat[12]=0;         zmat[13]=0;         zmat[14]=0;         zmat[15]=1;
+
+    multiply(mMatrix, ymat, tmp);
+    multiply(tmp, xmat, tmp2);
+    multiply(tmp2, zmat, mMatrix);
+}
+
+void Matrix::translate(const vec_t *xyz) {
+    translate(xyz[0], xyz[1], xyz[2]);
+}
+
+void Matrix::translate(vec_t tx, vec_t ty, vec_t tz) {
+    matrix_t tmat, tmp;
+
+    tmat[ 0]=1;  tmat[ 1]=0;  tmat[ 2]=0;  tmat[ 3]=0;
+    tmat[ 4]=0;  tmat[ 5]=1;  tmat[ 6]=0;  tmat[ 7]=0;
+    tmat[ 8]=0;  tmat[ 9]=0;  tmat[10]=1;  tmat[11]=0;
+    tmat[12]=tx; tmat[13]=ty; tmat[14]=tz; tmat[15]=1;
+
+    copy(mMatrix, tmp);
+    multiply(tmp, tmat, mMatrix);
+}
+
+void Matrix::copy(matrix_t source, matrix_t dest) {
+    for (int i = 0; i < 16; i++)
+        dest[i] = source[i];
+}
+
+void Matrix::multiply(const matrix_t a, const matrix_t b, matrix_t result) {
+    /* Generated code for matrix mult
+     * Code used:
+
+    // char order is argument
+    int i, j, k;
+    if (order == 'r') {
+        printf("// Row order\n");
+    } else {
+        printf("// Column order\n");
+    }
+    for (i = 0; i < 4; ++i) {
+        for (j = 0; j < 4; ++j) {
+            if (order == 'r') {
+                printf("result[%2i] = ", j+i*4);
+            } else {
+                printf("result[%2i] = ", j+i*4);
+            }
+            for (k = 0; k < 4; ++k) {
+                if (order == 'r') {
+                    printf("a[%2i] * b[%2i]%s",
+                    k+i*4, j+k*4, (k == 3) ? ";\n" : " + ");
+                } else {
+                    printf("a[%2i] * b[%2i]%s",
+                    i+k*4, k+j*4, (k == 3) ? ";\n" : " + ");
+                }
+                //sum+=(elements[i+k*4]*m.elements[k+j*4]);
+            }
+            //result.elements[i+j*4]=sum;
+        }
+        printf("\n");
+    }
+    printf("\n");
+    printf("// Transpose\n");
+    for(i = 0; i < 4; ++i) {
+        for (j = 0; j < 4; ++j) {
+            printf("a[%2i] = b[%2i]%s",
+            j+i*4, i+j*4, (j == 3) ? ";\n" : "; ");
+        }
+    }
+
+     * was in test/Matrix.cpp
+     */
+#ifdef COLUMN_ORDER
+    /* Column order */
+    result[ 0] = a[ 0] * b[ 0] + a[ 4] * b[ 1] + a[ 8] * b[ 2] + a[12] * b[ 3];
+    result[ 1] = a[ 0] * b[ 4] + a[ 4] * b[ 5] + a[ 8] * b[ 6] + a[12] * b[ 7];
+    result[ 2] = a[ 0] * b[ 8] + a[ 4] * b[ 9] + a[ 8] * b[10] + a[12] * b[11];
+    result[ 3] = a[ 0] * b[12] + a[ 4] * b[13] + a[ 8] * b[14] + a[12] * b[15];
+
+    result[ 4] = a[ 1] * b[ 0] + a[ 5] * b[ 1] + a[ 9] * b[ 2] + a[13] * b[ 3];
+    result[ 5] = a[ 1] * b[ 4] + a[ 5] * b[ 5] + a[ 9] * b[ 6] + a[13] * b[ 7];
+    result[ 6] = a[ 1] * b[ 8] + a[ 5] * b[ 9] + a[ 9] * b[10] + a[13] * b[11];
+    result[ 7] = a[ 1] * b[12] + a[ 5] * b[13] + a[ 9] * b[14] + a[13] * b[15];
+
+    result[ 8] = a[ 2] * b[ 0] + a[ 6] * b[ 1] + a[10] * b[ 2] + a[14] * b[ 3];
+    result[ 9] = a[ 2] * b[ 4] + a[ 6] * b[ 5] + a[10] * b[ 6] + a[14] * b[ 7];
+    result[10] = a[ 2] * b[ 8] + a[ 6] * b[ 9] + a[10] * b[10] + a[14] * b[11];
+    result[11] = a[ 2] * b[12] + a[ 6] * b[13] + a[10] * b[14] + a[14] * b[15];
+
+    result[12] = a[ 3] * b[ 0] + a[ 7] * b[ 1] + a[11] * b[ 2] + a[15] * b[ 3];
+    result[13] = a[ 3] * b[ 4] + a[ 7] * b[ 5] + a[11] * b[ 6] + a[15] * b[ 7];
+    result[14] = a[ 3] * b[ 8] + a[ 7] * b[ 9] + a[11] * b[10] + a[15] * b[11];
+    result[15] = a[ 3] * b[12] + a[ 7] * b[13] + a[11] * b[14] + a[15] * b[15];
+#else
+    /* Row order */
+    result[ 0] = a[ 0] * b[ 0] + a[ 1] * b[ 4] + a[ 2] * b[ 8] + a[ 3] * b[12];
+    result[ 1] = a[ 0] * b[ 1] + a[ 1] * b[ 5] + a[ 2] * b[ 9] + a[ 3] * b[13];
+    result[ 2] = a[ 0] * b[ 2] + a[ 1] * b[ 6] + a[ 2] * b[10] + a[ 3] * b[14];
+    result[ 3] = a[ 0] * b[ 3] + a[ 1] * b[ 7] + a[ 2] * b[11] + a[ 3] * b[15];
+
+    result[ 4] = a[ 4] * b[ 0] + a[ 5] * b[ 4] + a[ 6] * b[ 8] + a[ 7] * b[12];
+    result[ 5] = a[ 4] * b[ 1] + a[ 5] * b[ 5] + a[ 6] * b[ 9] + a[ 7] * b[13];
+    result[ 6] = a[ 4] * b[ 2] + a[ 5] * b[ 6] + a[ 6] * b[10] + a[ 7] * b[14];
+    result[ 7] = a[ 4] * b[ 3] + a[ 5] * b[ 7] + a[ 6] * b[11] + a[ 7] * b[15];
+
+    result[ 8] = a[ 8] * b[ 0] + a[ 9] * b[ 4] + a[10] * b[ 8] + a[11] * b[12];
+    result[ 9] = a[ 8] * b[ 1] + a[ 9] * b[ 5] + a[10] * b[ 9] + a[11] * b[13];
+    result[10] = a[ 8] * b[ 2] + a[ 9] * b[ 6] + a[10] * b[10] + a[11] * b[14];
+    result[11] = a[ 8] * b[ 3] + a[ 9] * b[ 7] + a[10] * b[11] + a[11] * b[15];
+
+    result[12] = a[12] * b[ 0] + a[13] * b[ 4] + a[14] * b[ 8] + a[15] * b[12];
+    result[13] = a[12] * b[ 1] + a[13] * b[ 5] + a[14] * b[ 9] + a[15] * b[13];
+    result[14] = a[12] * b[ 2] + a[13] * b[ 6] + a[14] * b[10] + a[15] * b[14];
+    result[15] = a[12] * b[ 3] + a[13] * b[ 7] + a[14] * b[11] + a[15] * b[15];
+#endif
+}
+

src/utils/Vector3d.cpp

@@ -0,0 +1,135 @@
+/*!
+ * \file src/utils/Vector3d.cpp
+ * \brief 3D Math vector
+ *
+ * \author Mongoose
+ */
+
+#include <math.h>
+
+#include "utils/Vector3d.h"
+
+Vector3d::Vector3d() {
+    mVec[0] = mVec[1] = mVec[2] = 0.0f;
+}
+
+Vector3d::Vector3d(vec3_t v) {
+    mVec[0] = v[0];
+    mVec[1] = v[1];
+    mVec[2] = v[2];
+}
+
+Vector3d::Vector3d(vec_t x, vec_t y, vec_t z) {
+    mVec[0] = x;
+    mVec[1] = y;
+    mVec[2] = z;
+}
+
+Vector3d::Vector3d(const Vector3d &v) {
+    mVec[0] = v.mVec[0];
+    mVec[1] = v.mVec[1];
+    mVec[2] = v.mVec[2];
+}
+
+vec_t Vector3d::dot(const Vector3d &u, const Vector3d &v) {
+    return (u.mVec[0]*v.mVec[0] + u.mVec[1]*v.mVec[1] + u.mVec[2]*v.mVec[2]);
+}
+
+Vector3d Vector3d::cross(const Vector3d &u, const Vector3d &v) {
+    return Vector3d(u.mVec[1] * v.mVec[2] - u.mVec[2] * v.mVec[1],
+            u.mVec[2] * v.mVec[0] - u.mVec[0] * v.mVec[2],
+            u.mVec[0] * v.mVec[1] - u.mVec[1] * v.mVec[0]);
+}
+
+vec_t Vector3d::magnitude() {
+    return sqrtf(mVec[0]*mVec[0] + mVec[1]*mVec[1] + mVec[2]*mVec[2]);
+}
+
+Vector3d Vector3d::unit() {
+    vec_t norm = magnitude();
+
+    return Vector3d(mVec[0] / norm,
+            mVec[1] / norm,
+            mVec[2] / norm);
+}
+
+Vector3d Vector3d::zeroVector() {
+    return Vector3d(0, 0, 0);
+}
+
+Vector3d Vector3d::operator +(const Vector3d &v) {
+    return Vector3d(mVec[0] + v.mVec[0],
+            mVec[1] + v.mVec[1],
+            mVec[2] + v.mVec[2]);
+}
+
+Vector3d Vector3d::operator -(const Vector3d &v) {
+    return Vector3d(mVec[0] - v.mVec[0],
+            mVec[1] - v.mVec[1],
+            mVec[2] - v.mVec[2]);
+}
+
+Vector3d Vector3d::operator -() {
+    return Vector3d(-mVec[0],
+            -mVec[1],
+            -mVec[2]);
+}
+
+Vector3d Vector3d::operator *(vec_t s) {
+    return Vector3d(s * mVec[0],
+            s * mVec[1],
+            s * mVec[2]);
+}
+
+Vector3d Vector3d::operator /(vec_t s) {
+    return Vector3d(mVec[0] / s,
+            mVec[1] / s,
+            mVec[2] / s);
+}
+
+vec_t Vector3d::operator *(const Vector3d &v) {
+    return dot(*this, v);
+}
+
+void Vector3d::normalize() {
+    vec_t norm = magnitude();
+
+    mVec[0] /= norm;
+    mVec[1] /= norm;
+    mVec[2] /= norm;
+}
+
+void Vector3d::zero() {
+    mVec[0] = 0;
+    mVec[1] = 0;
+    mVec[2] = 0;
+}
+
+Vector3d &Vector3d::operator =(const Vector3d &v) {
+    mVec[0] = v.mVec[0];
+    mVec[1] = v.mVec[1];
+    mVec[2] = v.mVec[2];
+    return *this;
+}
+
+Vector3d &Vector3d::operator +=(const Vector3d &v) {
+    mVec[0] += v.mVec[0];
+    mVec[1] += v.mVec[1];
+    mVec[2] += v.mVec[2];
+    return *this;
+}
+
+Vector3d &Vector3d::operator -=(const Vector3d &v) {
+    mVec[0] -= v.mVec[0];
+    mVec[1] -= v.mVec[1];
+    mVec[2] -= v.mVec[2];
+    return *this;
+}
+
+Vector3d &Vector3d::operator *=(vec_t s) {
+    mVec[0] *= s;
+    mVec[1] *= s;
+    mVec[2] *= s;
+    return *this;
+}
+