Made the code in setVelocity() (hopefully) a bit nicer. Also added comments so that one should understand the mathematics and calculations behind what is done.