Tested GetNodeDegree, it works, but OrthogonalPathHeuristic is doing something wrong when I add another node... should investigate.