Vectorisation

I have done a quick vectorisation of some of the routines to make them faster after profiling.