Add Matrix4

Some other improvements
Signed-off-by: Aleksi Sapon <QcTechs@gmail.com>