Some cleanup to make it more 'proper' C++. Also added a cross product function.

* I'm off to work!