utility.hpp
template <class Scalar> Scalar norm_squared(const VECTOR(Scalar)& v) { return v.squaredNorm(); }