Safe normalization

Adds
* SafeNormalize, a class to perform normalization and check its validity
* safeNormalize(), a convenient function for SafeNormalize
* powerOfTwo(), to get vector^2