Added Matrix class

Matrix is a 4x4 matrix