Performance improvements and code simplifications

And more mathematical functions