F and G updated to take care of obstacles and tested.  Calculation of RHS also updated to take care of obstacles.