Optimisation + Bug Fix.

Removed unnecessary vector normalisations.