Added threading.
Math optimizations
Minor uninitialized variable fix