Sub-Goal Trees -- a Framework for Goal-Directed Trajectory Prediction and Optimization