c++ angle axis updates

I added a statically allocated vector and matrix classes.  They are now used to do the 3d vector and matrix calculations for angle axis stuff.

I also want to implement the angle axis distances in c++.

this follows from the discussion in #53