Drake geometry util additions

Added rotation conversion functions + test. Moved angleDiff to drakeGeometryUtil.
would be cleaner to have the 4.0 (forcing double) on all of the elements, instead of just the one.