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.

- [ ] Pass sin and cos of latitude around
- [ ] Benchmark
- [ ] All tests pass
- [ ] Changelog entry
- [ ] Can be merged