Finished rotation matrix methods
Creates a lot of classes for the dynamics