Fixed equation for linear velocity
Started writing test for Angular velocity, for some reason, test crashes...