Remove nalgebra library search paths
Make some functions always inline
Move some functions to a separate utils module