Implemented matrix4

This needs to be tested heavily, ideally with a proper test framework. I
can forsee plenty of pass by reference problems. The original C++
implementation passes and returns many const values. C#, unfortunately,
does not have this 'const correctness' concept.