SubSim: An autonomous underwater vehicle simulation package