Major overhaul of angle/speed code to use velocity vectors