updated 'updateVelocities' function to match the one in fraunhofer repo