diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 71177e1d..bebb15da 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -16,3 +16,4 @@ endmacro()
 
 # Concurrent tests
 pip_test(concurrent "")
+pip_test(math "")
diff --git a/tests/math/testpimathmatrix.cpp b/tests/math/testpimathmatrix.cpp
new file mode 100644
index 00000000..4eeccd0c
--- /dev/null
+++ b/tests/math/testpimathmatrix.cpp
@@ -0,0 +1,763 @@
+#include "gtest/gtest.h"
+#include "pimathmatrix.h"
+
+bool cmpMatrixWithValue(PIMathMatrix<double> matrix, double val)
+{
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    bool b;
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(matrix.element(j,k) == val)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    return b;
+}
+
+TEST(PIMathMatrix_Test, identity)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix;
+    int i;
+    bool b;
+    matrix = origMatr.identity(3, 3);
+    for(i = 0; i < 3; i++)
+    {
+        if(matrix[i][i] == 1.0)
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, matrixRow)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix;
+    PIMathVector<double> vector;
+    uint i;
+    bool b;
+    vector.resize(3, 3.0);
+    vector.fill(5.0);
+    matrix = origMatr.matrixRow(vector);
+    for(i = 0; i < vector.size(); i++)
+    {
+        if(matrix[0][i] == 5.0)
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, matrixCol)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix;
+    PIMathVector<double> vector;
+    uint i;
+    bool b;
+    vector.resize(3, 3.0);
+    vector.fill(5.0);
+    matrix = origMatr.matrixCol(vector);
+    for(i = 0; i < vector.size(); i++)
+    {
+        if(matrix[i][0] == 5.0)
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, setCol)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix;
+    PIMathVector<double> vector;
+    uint i;
+    bool b;
+    vector.resize(3, 3.0);
+    vector.fill(5.0);
+    matrix = origMatr.matrixCol(vector);
+    vector.fill(10.0);
+    matrix.setCol(0, vector);
+    for(i = 0; i < vector.size(); i++)
+    {
+        if(matrix[i][0] == 10.0)
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, setRow)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix;
+    PIMathVector<double> vector;
+    uint i;
+    bool b;
+    vector.resize(3, 3.0);
+    vector.fill(5.0);
+    matrix = origMatr.matrixRow(vector);
+    vector.fill(10.0);
+    matrix.setRow(0, vector);
+    for(i = 0; i < vector.size(); i++)
+    {
+        if(matrix[0][i] == 10.0)
+        {
+            b = true;
+        }
+        else
+        {
+            b = false;
+            break;
+        }
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, swapCols)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathVector<double> vector;
+    uint i1 = 0; uint i2 = 1;
+    double a1[3], a2[3], a3[3];
+    double b1[3], b2[3], b3[3];
+    bool b;
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix1 = origMatr.identity(3, 3);
+    matrix1.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix1.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix1.setCol(2, vector);
+    for(int i = 0; i < 3; i++)
+    {
+        a1[i] = matrix1.element(i, 0);
+        a2[i] = matrix1.element(i, 1);
+        a3[i] = matrix1.element(i, 2);
+    }
+    matrix1.swapCols(i1, i2);
+    for(int i = 0; i < 3; i++)
+    {
+        b1[i] = matrix1.element(i, 0);
+        b2[i] = matrix1.element(i, 1);
+        b3[i] = matrix1.element(i, 2);
+    }
+    if((memcmp(a1, b2, sizeof(b1)) == 0) && (memcmp(a2, b1, sizeof(b1)) == 0) && (memcmp(a3, b3, sizeof(b1)) == 0))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, swapRows)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathVector<double> vector;
+    uint i1 = 0; uint i2 = 1;
+    double a1[3], a2[3], a3[3];
+    double b1[3], b2[3], b3[3];
+    bool b;
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix1 = origMatr.identity(3, 3);
+    matrix1.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix1.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix1.setCol(2, vector);
+    for(int i = 0; i < 3; i++)
+    {
+        a1[i] = matrix1.element(0, i);
+        a2[i] = matrix1.element(1, i);
+        a3[i] = matrix1.element(2, i);
+    }
+    matrix1.swapRows(i1, i2);
+    for(int i = 0; i < 3; i++)
+    {
+        b1[i] = matrix1.element(0, i);
+        b2[i] = matrix1.element(1, i);
+        b3[i] = matrix1.element(2, i);
+    }
+    if((memcmp(a1, b2, sizeof(b1)) == 0) && (memcmp(a2, b1, sizeof(b1)) == 0) && (memcmp(a3, b3, sizeof(b1)) == 0))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+TEST(PIMathMatrix_Test, fill)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix;
+    bool b;
+    matrix = origMatr.identity(3, 3);
+    matrix.fill(5.0);
+    b = cmpMatrixWithValue(matrix, 5.0);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, isSquare)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(4,3);
+    if((matrix1.isSquare() == true) && (matrix2.isSquare() == false))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, isIdentity)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix2.fill(3.932);
+    if((matrix1.isIdentity() == true) && (matrix2.isIdentity() == false))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, isNull)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix1.fill(0.0);
+    matrix2.fill(3.932);
+    if((matrix1.isNull() == true) && (matrix2.isNull() == false))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, isValid)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    if((matrix1.isValid() == true) && (matrix2.isValid() == false))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Assignment)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix2.fill(6.72);
+    matrix1 = matrix2;
+    b = cmpMatrixWithValue(matrix1, 6.72);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Equal)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    PIMathMatrix<double> matrix3;
+    PIMathVector<double> vector;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix3 = origMatr.identity(3,3);
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix1.setCol(0, vector);
+    matrix2.setCol(0, vector);
+    matrix3.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix1.setCol(1, vector);
+    matrix2.setCol(1, vector);
+    matrix3.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix1.setCol(2, vector);
+    matrix2.setCol(2, vector);
+    vector.at(0) = 566.0;
+    vector.at(1) = 564.0;
+    vector.at(2) = 543.0;
+    matrix3.setCol(2, vector);
+    if(((matrix1 == matrix2) == true) && ((matrix1 == matrix3) == false))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Not_Equal)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    PIMathMatrix<double> matrix3;
+    PIMathVector<double> vector;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix3 = origMatr.identity(3,3);
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix1.setCol(0, vector);
+    matrix2.setCol(0, vector);
+    matrix3.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix1.setCol(1, vector);
+    matrix2.setCol(1, vector);
+    matrix3.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix1.setCol(2, vector);
+    matrix2.setCol(2, vector);
+    vector.at(0) = 566.0;
+    vector.at(1) = 564.0;
+    vector.at(2) = 543.0;
+    matrix3.setCol(2, vector);
+    if(((matrix1 != matrix2) == false) && ((matrix1 != matrix3) == true))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Addition_Aassignment)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix2.fill(6.72);
+    matrix1.fill(1.0);
+    matrix1 += matrix2;
+    b = cmpMatrixWithValue(matrix1, 7.72);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Subtraction_Assignment)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix2.fill(6.72);
+    matrix1.fill(1.0);
+    matrix1 -= matrix2;
+    b = cmpMatrixWithValue(matrix1, -5.72);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Multiplication_Assignment)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix1.fill(6.72);
+    matrix1 *= 2.0;
+    b = cmpMatrixWithValue(matrix1, 13.44);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Division_Assignment)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix1.fill(6.72);
+    matrix1 /= 2.0;
+    b = cmpMatrixWithValue(matrix1, 3.36);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Addition)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix1.fill(6.72);
+    matrix2.fill(8.28);
+    b = cmpMatrixWithValue(matrix1 + matrix2, 15.0);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Subtraction)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3, 3);
+    matrix2 = origMatr.identity(3, 3);
+    origMatr.fill(8.0);
+    matrix1.fill(8.28);
+    origMatr = origMatr - matrix1;
+    matrix2.fill(-0.28);
+    if(origMatr == matrix2)
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, operator_Multiplication)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix1.fill(6.72);
+    matrix2 = matrix1*4.0;
+    b = cmpMatrixWithValue(matrix2, 26.88);
+    ASSERT_TRUE(b);
+}
+TEST(PIMathMatrix_Test, operator_Division)
+{
+    PIMathMatrix<double> origMatr;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    bool b;
+    matrix1 = origMatr.identity(3,3);
+    matrix2 = origMatr.identity(3,3);
+    matrix1.fill(6.72);
+    matrix2 = matrix1/4.0;
+    b = cmpMatrixWithValue(matrix2, 1.68);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, determinant)
+{
+    PIMathMatrix<double> origMatr;
+    double d;
+    double i = 59.0;
+    PIMathMatrix<double> matrix;
+    PIMathVector<double> vector;
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix = origMatr.identity(3, 3);
+    matrix.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix.setCol(2, vector);
+    d = matrix.determinant();
+    ASSERT_DOUBLE_EQ(d, i);
+}
+
+TEST(PIMathMatrix_Test, trace)
+{
+    PIMathMatrix<double> origMatr;
+    double t;
+    double i = 9.0;
+    PIMathMatrix<double> matrix;
+    PIMathVector<double> vector;
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix = origMatr.identity(3, 3);
+    matrix.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix.setCol(2, vector);
+    t = matrix.trace();
+    ASSERT_DOUBLE_EQ(t, i);
+}
+
+TEST(PIMathMatrix_Test, toUpperTriangular)
+{
+    PIMathMatrix<double> origMatr;
+    double d1, d2 = 1;
+    int i;
+    PIMathMatrix<double> matrix;
+    PIMathVector<double> vector;
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix = origMatr.identity(3, 3);
+    matrix.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix.setCol(2, vector);
+    d1 = matrix.determinant();
+    matrix.toUpperTriangular();
+    for(i = 0; i < 3; i++)
+    {
+        d2 = d2 * matrix.element(i, i);
+    }
+    ASSERT_DOUBLE_EQ(d1, d2);
+}
+
+TEST(PIMathMatrix_Test, invert)
+{
+    PIMathMatrix<double> origMatr;
+    double d1, d2;
+    bool b;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    PIMathMatrix<double> matrix3;
+    PIMathMatrix<double> matrix4;
+    PIMathVector<double> vector;
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix1 = origMatr.identity(3, 3);
+    matrix1.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix1.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix1.setCol(2, vector);
+    d1 = matrix1.determinant();
+    matrix2 = matrix1;
+    matrix2.invert();
+    d2 = matrix2.determinant();
+    matrix3 = origMatr.identity(3, 3);
+    matrix4 = origMatr.identity(3, 3);
+    matrix4.invert();
+    if((matrix3 == matrix4) && (round((1/d1)*10000)/10000 == round(d2*10000)/10000))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, inverted)
+{
+    PIMathMatrix<double> origMatr;
+    double d1, d2;
+    bool b;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    PIMathMatrix<double> matrix3;
+    PIMathMatrix<double> matrix4;
+    PIMathVector<double> vector;
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix1 = origMatr.identity(3, 3);
+    matrix1.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix1.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix1.setCol(2, vector);
+    d1 = matrix1.determinant();
+    matrix2 = matrix1;
+    matrix1 = matrix2.invert();
+    d2 = matrix1.determinant();
+    matrix3 = origMatr.identity(3, 3);
+    matrix4 = origMatr.identity(3, 3);
+    matrix3 = matrix4.invert();
+    if((matrix3 == matrix4) && (round((1/d1)*10000)/10000 == round(d2*10000)/10000))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrix_Test, transposed)
+{
+    PIMathMatrix<double> origMatr;
+    double d1, d2;
+    bool b;
+    PIMathMatrix<double> matrix1;
+    PIMathMatrix<double> matrix2;
+    PIMathMatrix<double> matrix3;
+    PIMathVector<double> vector;
+    vector.resize(3, 3.0);
+    vector.at(0) = 3.0;
+    vector.at(1) = 6.0;
+    vector.at(2) = 8.0;
+    matrix1 = origMatr.identity(3, 3);
+    matrix1.setCol(0, vector);
+    vector.at(0) = 2.0;
+    vector.at(1) = 1.0;
+    vector.at(2) = 4.0;
+    matrix1.setCol(1, vector);
+    vector.at(0) = 6.0;
+    vector.at(1) = 2.0;
+    vector.at(2) = 5.0;
+    matrix1.setCol(2, vector);
+    d1 = matrix1.determinant();
+    matrix2 = matrix1.transposed();
+    d2 = matrix2.determinant();
+    matrix3 = matrix2.transposed();
+    if((d1 == d2) && (matrix1 == matrix3))
+    {
+        b = true;
+    }
+    else
+    {
+        b = false;
+    }
+    ASSERT_TRUE(b);
+}
diff --git a/tests/math/testpimathmatrixt.cpp b/tests/math/testpimathmatrixt.cpp
new file mode 100644
index 00000000..38976bf8
--- /dev/null
+++ b/tests/math/testpimathmatrixt.cpp
@@ -0,0 +1,833 @@
+#include "gtest/gtest.h"
+#include "pimathmatrix.h"
+
+const uint rows = 3;
+const uint cols = 3;
+
+bool cmpMatrixWithValue(PIMathMatrixT<rows, cols, double> matrix, double val)
+{
+    int i = 0;
+    int j = 0;
+    int k = 0;
+    bool b;
+    while(i < 9)
+    {
+        if(k < 3)
+        {
+            if(matrix.at(j,k) == val)
+            {
+                b = true;
+            }
+            else
+            {
+                b = false;
+                break;
+            }
+            k++;
+            if(k == 3)
+            {
+                j++;
+                k = 0;
+            }
+        }
+        i++;
+    }
+    return b;
+}
+
+
+TEST(PIMathMatrixT_Test, identity)
+{
+    PIMathMatrixT<rows, cols, double> matr;
+    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> matr;
+    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;
+    PIMathMatrixT<rows, cols, double> matr;
+    PIMathVectorT<rows, double> vect;
+    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)
+{
+    PIMathMatrixT<rows, cols, double> matr;
+    ASSERT_EQ(cols,matr.cols());
+}
+
+TEST(PIMathMatrixT_Test, rows)
+{
+    PIMathMatrixT<rows, cols, double> matr;
+    ASSERT_EQ(rows,matr.rows());
+}
+
+TEST(PIMathMatrixT_Test, col)
+{
+    PIMathMatrixT<rows, cols, double> matr;
+    PIMathVectorT<rows, double> vect;
+    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)
+{
+    PIMathMatrixT<rows, cols, double> matr;
+    PIMathVectorT<rows, double> vect;
+    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)
+{
+    PIMathMatrixT<rows, cols, double> matr;
+    PIMathVectorT<rows, double> vect;
+    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)
+{
+    PIMathMatrixT<rows, cols, double> matr;
+    PIMathVectorT<rows, double> vect;
+    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)
+{
+    PIMathMatrixT<rows, cols, double> matr;
+    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)
+{
+    PIMathMatrixT<rows, cols, double> matr;
+    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> matr;
+    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> matr;
+    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> matr;
+    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;
+    PIMathMatrixT<rows, cols, double> matr;
+    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;
+    PIMathMatrixT<rows, cols, double> matr;
+    bool b;
+    matrix2.fill(6.72);
+    matrix1 = matrix2;
+    b = cmpMatrixWithValue(matrix1, 6.72);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Equal)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    PIMathMatrixT<rows, cols, double> matr;
+    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;
+    PIMathMatrixT<rows, cols, double> matr;
+    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;
+    matrix2.fill(6.72);
+    matrix1.fill(1.0);
+    matrix1 += matrix2;
+    b = cmpMatrixWithValue(matrix1, 7.72);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Subtraction_Assignment)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    matrix2.fill(6.72);
+    matrix1.fill(1.0);
+    matrix1 -= matrix2;
+    b = cmpMatrixWithValue(matrix1, -5.72);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Multiplication_Assignment)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    bool b;
+    matrix1.fill(6.72);
+    matrix1 *= 2.0;
+    b = cmpMatrixWithValue(matrix1, 13.44);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Division_Assignment)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    bool b;
+    matrix1.fill(6.72);
+    matrix1 /= 2.0;
+    b = cmpMatrixWithValue(matrix1, 3.36);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Addition)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    matrix1.fill(6.72);
+    matrix2.fill(8.28);
+    b = cmpMatrixWithValue(matrix1 + matrix2, 15.0);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Subtraction)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    matrix1.fill(6.0);
+    matrix2.fill(5.0);
+    b = cmpMatrixWithValue(matrix1 - matrix2, 1.0);
+    ASSERT_TRUE(b);
+}
+
+TEST(PIMathMatrixT_Test, operator_Multiplication)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    matrix1.fill(6.72);
+    matrix2 = matrix1*4.0;
+    b = cmpMatrixWithValue(matrix2, 26.88);
+    ASSERT_TRUE(b);
+}
+TEST(PIMathMatrixT_Test, operator_Division)
+{
+    PIMathMatrixT<rows, cols, double> matrix1;
+    PIMathMatrixT<rows, cols, double> matrix2;
+    bool b;
+    matrix1.fill(6.72);
+    matrix2 = matrix1/4.0;
+    b = cmpMatrixWithValue(matrix2, 1.68);
+    ASSERT_TRUE(b);
+}
+
+
+TEST(PIMathMatrixT_Test, determinant)
+{
+    double d;
+    double i = 59.0;
+    PIMathMatrixT<rows, cols, double> matr;
+    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;
+    PIMathMatrixT<rows, cols, double> matr;
+    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;
+    PIMathMatrixT<rows, cols, double> matr;
+    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;
+    PIMathMatrixT<rows, cols, double> matr;
+    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;
+    PIMathMatrixT<rows, cols, double> matr;
+    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);
+}
+