fixed bug in calc'ing distance
nodes now avoid obstacles