Speedup code by moving things out of loops

There were a lot of trig functions being called inside loops. I moved some of them out of the loops for some speedups.

- [x] Benchmark
- [x] All tests pass
- [x] Changelog entry
- [x] Can be merged