Added Util and Matrix

Added Util folder, with a c++ ported version of the XNA Matrix and
Vector3 class.