Rework the whole thing !

Now based on rotation matrix and vector computations only.
dot products are way way safer than angles