public List <WaypointNode> GetRouteTo(WaypointNode dest) { List <WaypointNode> route = new List <WaypointNode>(); int insertIndex = 0; WaypointNode begin = this; WaypointNode end = dest; while (begin.RecalculateLevel() > end.RecalculateLevel()) { begin = begin.PushTo(insertIndex, route); insertIndex++; } while (begin.RecalculateLevel() < end.RecalculateLevel()) { end = end.PushTo(insertIndex, route); } while (begin.RecalculateLevel() >= 0 && begin != end) { begin = begin.PushTo(insertIndex, route); insertIndex++; end = end.PushTo(insertIndex, route); } if (begin == null) { return(null); } else { begin.PushTo(insertIndex, route); return(route); } }