A method of planning a path of a vehicle by defining a first plurality of constraints representing dynamics of the vehicle; defining a second plurality of constraints representing collisions with obstacles; defining a third plurality of constraints representing visibility to threats; and using the first, second and third plurality of constraints with mixed integer linear programming to generate a trajectory for a vehicle from a starting point to a destination point in an environment containing the obstacles and the threats.