formatting and removing redudant stuff
Fixed agent visualization issues with quaternions, thanks to @palmieri
and added manifold angle subtraction utils