Add vector2d class, update main loop