diff --git a/lib/main/math/test/testpimathmatrixt.cpp b/lib/main/math/test/testpimathmatrixt.cpp
new file mode 100644
index 00000000..1d343237
--- /dev/null
+++ b/lib/main/math/test/testpimathmatrixt.cpp
@@ -0,0 +1,991 @@
+#include "gtest/gtest.h"
+#include "pimathmatrix.h"
+
+const uint rows = 3;
+const uint cols = 3;
+PIMathMatrixT<rows, cols, double> matr;
+PIMathVectorT<rows, double> vect;
+
+TEST(PIMathMatrixT_Test, identity)
+{
+    PIMathMatrixT<rows, cols, double> matrix;
+    double d;
+    double i = 1.0;
+    bool a;
+    bool output;
+    matrix = matr.identity();
+    d = matrix.determinant();
+    uint j;
+    for(j = 0; j < cols; j++)
+    {
+        if(matrix.at(i, i) == 1.0) a = true;
+        else
+        {
+            a = false;
+            break;
+        }
+    }
+    if((i == d) && (a == true)){
+        output = true;
+    }
+    else{
+        output = false;
+    }
+    ASSERT_TRUE(output);
+}
+
+TEST(PIMathMatrixT_Test, at)
+{
+    uint i;
+    bool b;
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    matrix1 = matr.identity();
+    matrix2 = matr.identity();
+    for(i = 0; i < rows; i++)
+    {
+        if(matrix1.at(i,i) == 1.0)
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+TEST(PIMathMatrixT_Test, filled)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    double g = 1.0;
+    matrix2 = matr.fill(g);
+    uint j = 0, i = 0;
+    for(i = 0; i < cols; i++)
+    {
+        for(j = 0; j < rows; j++)
+        {
+            matrix1.at(j,i) = g;
+        }
+    }
+    ASSERT_TRUE(matrix2 == matrix1);
+}
+
+TEST(PIMathMatrixT_Test, cols)
+{
+    ASSERT_EQ(cols,matr.cols());
+}
+
+TEST(PIMathMatrixT_Test, rows)
+{
+    ASSERT_EQ(rows,matr.rows());
+}
+
+TEST(PIMathMatrixT_Test, col)
+{
+    uint i;
+    uint g = 2;
+    bool b;
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    vect = matr.col(g);
+    for(i = 0; i < matr.cols(); i++)
+    {
+        if(matr.at(i,g) == vect.at(i))
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, row)
+{
+    uint i;
+    uint g = 2;
+    bool b;
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    vect = matr.row(g);
+    for(i = 0; i < matr.rows(); i++)
+    {
+        if(matr.at(g,i) == vect.at(i))
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, setCol)
+{
+    vect.at(0) = 1.0;
+    vect.at(1) = 3.0;
+    vect.at(2) = 5.0;
+    uint g = 1;
+    uint i = 0;
+    bool b;
+    matr.setCol(g, vect);
+    for(i = 0; i < vect.size(); i++)
+    {
+        if(matr.at(i,g) == vect.at(i))
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, setRow)
+{
+    vect.at(0) = 1.0;
+    vect.at(1) = 3.0;
+    vect.at(2) = 5.0;
+    uint g = 1;
+    uint i = 0;
+    bool b;
+    matr.setRow(g, vect);
+    for(i = 0; i < vect.size(); i++)
+    {
+        if(matr.at(g,i) == vect.at(i))
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, swapCols)
+{
+    PIMathVectorT<rows, double> before_Vect1;
+    PIMathVectorT<rows, double> before_Vect2;
+    PIMathVectorT<rows, double> after_Vect1;
+    PIMathVectorT<rows, double> after_Vect2;
+    int g1 = 1, g2 = 2;
+    bool b;
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    before_Vect1 = matr.col(g1);
+    before_Vect2 = matr.col(g2);
+    matr.swapCols(g1,g2);
+    after_Vect1 = matr.col(g1);
+    after_Vect2 = matr.col(g2);
+    if((before_Vect1 == after_Vect2) && (before_Vect2 == after_Vect1))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, swapRows)
+{
+    PIMathVectorT<rows, double> before_Vect1;
+    PIMathVectorT<rows, double> before_Vect2;
+    PIMathVectorT<rows, double> after_Vect1;
+    PIMathVectorT<rows, double> after_Vect2;
+    int g1 = 1, g2 = 2;
+    bool b;
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    before_Vect1 = matr.row(g1);
+    before_Vect2 = matr.row(g2);
+    matr.swapRows(g1,g2);
+    after_Vect1 = matr.row(g1);
+    after_Vect2 = matr.row(g2);
+    if((before_Vect1 == after_Vect2) && (before_Vect2 == after_Vect1))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, fill)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    double g = 1.0;
+    matr.fill(g);
+    uint j = 0, i = 0;
+    for(i = 0; i < cols; i++)
+    {
+        for(j = 0; j < rows; j++)
+        {
+            matrix1.at(j,i) = g;
+        }
+    }
+    ASSERT_TRUE(matr == matrix1);
+}
+
+TEST(PIMathMatrixT_Test, isSquare)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    const uint new_Cols = 4;
+    PIMathMatrixT<rows, new_Cols, double> matrix2;
+    bool b;
+    if((matrix1.isSquare() == true) && (matrix2.isSquare() == false))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, isIdentity)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    matrix1 = matr.identity();
+    matrix2 = matr.filled(3.67);
+    if((matrix1.isIdentity() == true) && (matrix2.isIdentity() == false))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, isNull)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    matrix2 = matr.filled(3.67);
+    if((matrix1.isNull() == true) && (matrix2.isNull() == false))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Assignment)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    bool b;
+    matrix2.fill(6.72);
+    matrix1 = matrix2;
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(matrix1.at(j,k) == 6.72)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Equal)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    matrix1 = matr;
+    matrix2 = matr;
+    matrix2.at(2, 2) = 232;
+    if(((matr == matrix1) == true) && ((matr == matrix2) == false))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Not_Equal)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    matrix1 = matr;
+    matrix2 = matr;
+    matrix2.at(2, 2) = 232;
+    if(((matr != matrix1) == false) && ((matr != matrix2) == true))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Addition_Aassignment)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    matrix2.fill(6.72);
+    matrix1.fill(1.0);
+    matrix1 += matrix2;
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(matrix1.at(j,k) == 7.72)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Subtraction_Assignment)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    matrix2.fill(6.72);
+    matrix1.fill(1.0);
+    matrix1 -= matrix2;
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(matrix1.at(j,k) == -5.72)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Multiplication_Assignment)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    bool b;
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    matrix1.fill(6.72);
+    matrix1 *= 2.0;
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(matrix1.at(j,k) == 13.44)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Division_Assignment)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    bool b;
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    matrix1.fill(6.72);
+    matrix1 /= 2.0;
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(matrix1.at(j,k) == 3.36)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Addition)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    matrix1.fill(6.72);
+    matrix2.fill(8.28);
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if((matrix1 + matrix2).at(j,k) == 15.0)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Subtraction)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    matrix1.fill(6.72);
+    matrix2.fill(8.28);
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(round((matrix1 - matrix2).at(j,k) * 10000) / 10000 == -1.56)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Multiplication)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    matrix1.fill(6.72);
+    matrix2 = matrix1*4.0;
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(matrix2.at(j,k) == 26.88)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    ASSERT_TRUE(b);
+}
+TEST(PIMathMatrixT_Test, operator_Division)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    matrix1.fill(6.72);
+    matrix2 = matrix1/4.0;
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(matrix2.at(j,k) == 1.68)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    ASSERT_TRUE(b);
+}
+
+
+TEST(PIMathMatrixT_Test, determinant)
+{
+    double d;
+    double i = 59.0;
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    d = matr.determinant();
+    ASSERT_DOUBLE_EQ(i, d);
+}
+
+TEST(PIMathMatrixT_Test, invert)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    PIMathMatrixT<rows, cols, double> matrix3;
+    double d1, d2;
+    bool b;
+    matrix1 = matr.identity();
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    matrix2 = matr;
+    matr.invert();
+    d1 = matr.determinant();
+    d2 = matrix2.determinant();
+    matrix3 = matrix1;
+    matrix1.invert();
+    if((matrix1 == matrix3) && (d1 == 1/d2))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, inverted)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    PIMathMatrixT<rows, cols, double> matrix3;
+    double d1, d2;
+    bool b;
+    matrix1 = matr.identity();
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    matrix2 = matr.inverted();
+    d1 = matr.determinant();
+    d2 = matrix2.determinant();
+    matrix3 = matrix1.inverted();
+    if((matrix1 == matrix3) && (round((1/d1)*10000)/10000 == round(d2*10000)/10000))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, toUpperTriangular)
+{
+    PIMathMatrixT<rows, cols, double> matrix;
+    double d1, d2 = 1;
+    uint i;
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    matrix = matr.toUpperTriangular();
+    d1 = matrix.determinant();
+    for(i = 0; i < cols; i++)
+    {
+        d2 = d2*matrix.at(i,i);
+    }
+    ASSERT_DOUBLE_EQ(d1, d2);
+}
+
+TEST(PIMathMatrixT_Test, transposed)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    double d1, d2;
+    bool b;
+    matr.at(0,0) = 3;
+    matr.at(0,1) = 6;
+    matr.at(0,2) = 8;
+    matr.at(1,0) = 2;
+    matr.at(1,1) = 1;
+    matr.at(1,2) = 4;
+    matr.at(2,0) = 6;
+    matr.at(2,1) = 2;
+    matr.at(2,2) = 5;
+    d1 = matr.determinant();
+    matrix1 = matr.transposed();
+    d2 = matrix1.determinant();
+    matrix2 = matrix1.transposed();
+    if((d1 == d2) && (matr == matrix2))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, rotation)
+{
+    double angle = 1.0;
+    bool b;
+    PIMathMatrixT<2u, 2u, double> matrix = PIMathMatrixT<2u, 2u, double>::rotation(angle);
+    double c = cos(angle);
+    double s = sin(angle);
+    if((c == matrix.at(1u,1u)) && (c == matrix.at(0u,0u)) && (-s == matrix.at(0u,1u)) && (s == matrix.at(1u,0u)))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, scaleX_two)
+{
+    double factor = 5.64;
+    bool b;
+    PIMathMatrixT<2u, 2u, double> matrix = PIMathMatrixT<2u, 2u, double>::scaleX(factor);
+
+    if((1.0 == matrix.at(1u,1u)) && (factor == matrix.at(0u,0u)))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, scaleY_two)
+{
+    double factor = 5.64;
+    bool b;
+    PIMathMatrixT<2u, 2u, double> matrix = PIMathMatrixT<2u, 2u, double>::scaleY(factor);
+
+    if((factor == matrix.at(1u,1u)) && (1.0 == matrix.at(0u,0u)))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, rotationX)
+{
+    double angle = 1.0;
+    bool b;
+    double c = cos(angle);
+    double s = sin(angle);
+    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::rotationX(angle);
+
+    if((1.0 == matrix.at(0u,0u)) && (c == matrix.at(1u,1u)) && (c == matrix.at(2u,2u)) && (s == matrix.at(2u,1u)) && (-s == matrix.at(1u,2u)))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, rotationY)
+{
+    double angle = 1.0;
+    bool b;
+    double c = cos(angle);
+    double s = sin(angle);
+    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::rotationY(angle);
+
+    if((1.0 == matrix.at(1u,1u)) && (c == matrix.at(0u,0u)) && (c == matrix.at(2u,2u)) && (s == matrix.at(0u,2u)) && (-s == matrix.at(2u,0u)))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, rotationZ)
+{
+    double angle = 1.0;
+    bool b;
+    double c = cos(angle);
+    double s = sin(angle);
+    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::rotationZ(angle);
+
+    if((1.0 == matrix.at(2u,2u)) && (c == matrix.at(0u,0u)) && (c == matrix.at(1u,1u)) && (s == matrix.at(1u,0u)) && (-s == matrix.at(0u,1u)))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, scaleX_three)
+{
+    double factor = 23.65;
+    bool b;
+    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::scaleX(factor);
+    if((1.0 == matrix.at(2u,2u)) && (factor == matrix.at(0u,0u)) && (1.0 == matrix.at(1u,1u)))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, scaleY_three)
+{
+    double factor = 23.65;
+    bool b;
+    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::scaleY(factor);
+    if((1.0 == matrix.at(2u,2u)) && (1.0 == matrix.at(0u,0u)) && (factor == matrix.at(1u,1u)))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, scaleZ_three)
+{
+    double factor = 23.65;
+    bool b;
+    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::scaleZ(factor);
+    if((factor == matrix.at(2u,2u)) && (1.0 == matrix.at(0u,0u)) && (1.0 == matrix.at(1u,1u)))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+