Clean up Math

Thou shalt not use pointers
Thou shalt use const references