Performance increases

Optimized code called in tight loops for increased performance.