Fixed collisions, works even at speed 100

Also cleaned up physics code to make it more readable