Add helper methods for maths
Fix incorrect calculation at Normalize