Fix quaternion.
Add quaternion functions and test them.