Optimized the drawing loop and cleaned up the math code.