Added distance squared heuristic