Two Dimensional Path Planning with Static Polygon Obstacles Avoidance