Optimize the two hottest functions

Speedup roughly 25% on the unit tests.