Drake geometry util additions

Added rotation conversion functions + test. Moved angleDiff to drakeGeometryUtil.
Ah, OK, didn't know what you meant.