Refactoring common math routines into functions.

Created a header for common math routine functions and wrote a few.