Cleaning up kabsch implementation