physics: optimizations, removal of slow external functions

Creating a copy of every value every single time you want to measure a distance. Not a good idea if speed is what you're after. Could have sent pointers instead, did try, but I was only using lenght once, why bother? It's about as fast as she goes now.