Assignment 1 Fast Trajectory Replanning Solution