Ok =)
I am trying to test your 5 steps with this function, and I receive surprises- mistakes with function results =)
and now think, that maybe easy will be use quaternions)