A-star: heuristic for multiple goals

Question

Let's consider a simple grid, where any point is connected with at most 4 other points (North-East-West-South neighborhood).

I have to write program, that computes minimal route from selected initial point to any of goal points, which are connected (there is route consisting of goal points between any two goals). Of course there can be obstacles on grid.

My solution is quite simple: I'm using A* algorithm with variable heuristic function h(x) - manhattan distance from x to nearest goal point. To find nearest goal point I have to do linear search (in O(n), where n - number of goal points). Here is my question: is there any more efficient solution (heuristic function) to dynamically find nearest goal point (where time < O(n))?

Or maybe A* is not good way to solve that problem?

Answer 1

You can calculate the f score using the nearest target. As others said, for naive approach, you can directly calculate all target distance from current node and pick the minimum, if you only have few targets to search. For more than 100 targets, you can probably find the nearest by KDTree to speed up the process.

Here is a sample code in dart.

Iterable<Vector2> getPath(Vector2 from, Iterable<Vector2> targets,
      {double? maxDistance, bool useAStar = false}) {
    targets = targets.asSet();
    clearPoints();
    var projectedTargets = addPoints(targets).toSet();

    var tree = useAStar ? IKDTree(targets) : null;
    var q = PriorityQueue<Node>(_priorityQueueComparor);
    Map<Vector2, Node> visited = {};
    var node = Node(from);
    visited[from] = node;
    q.add(node);
    while (q.isNotEmpty) {
      var current = q.removeFirst();
      // developer.log(
      //     '${current.point}#${current.distance}: ${getEdges(current.point).map((e) => e.dest)}');
      for (var edge in getEdges(current.point)) {
        if (visited.containsKey(edge.dest)) continue;

        var distance = current.distance + edge.distance;

        // too far
        if (maxDistance != null && distance > maxDistance) continue;

        // it is a target
        if (projectedTargets.contains(edge.dest)) {
          return reconstructPath(visited, current, edge.dest);
        }

        // we only interested in exploring polygon node.
        if (!_polygonPoints.contains(edge.dest)) continue;

        var f = 0.0;
        if (tree != null) {
          var nearest = tree
              .nearest(edge.dest, maxDistance: maxDistance ?? double.infinity)
              .firstOrNull;
          f = nearest != null ? edge.dest.distanceToSquared(nearest) : 0.0;
        }

        node = Node(edge.dest, distance, current.count + 1, current.point, f);
        visited[edge.dest] = node;
        q.add(node);
      }
    }

    return [];
  }

  Iterable<Vector2> reconstructPath(
      Map<Vector2, Node> visited, Node prev, Vector2 point) {
    var path = <Vector2>[point];
    Node? currentNode = prev;
    while (currentNode != null) {
      path.add(currentNode.point);
      currentNode = visited[currentNode.prev];
    }
    return path.reversed;
  }

  int _priorityQueueComparor(Node p0, Node p1) {
    int r;
    if (p0.f > 0 && p1.f > 0) {
      r = ((p0.distance * p0.distance) + p0.f)
          .compareTo((p1.distance * p1.distance) + p1.f);
      if (r != 0) return r;
    }
    r = p0.distance.compareTo(p1.distance);
    if (r != 0) return r;
    return p0.count.compareTo(p1.count);
  }

and the implementation of KDTree

class IKDTree {
  final int _dimensions = 2;
  late Node? _root;

  IKDTree(Iterable<Vector2> points) {
    _root = _buildTree(points, null);
  }

  Node? _buildTree(Iterable<Vector2> points, Node? parent) {
    var list = points.asList();
    if (list.isEmpty) return null;

    var median = (list.length / 2).floor();

    // Select the longest dimension as division axis
    var axis = 0;
    var aabb = AABB.fromPoints(list);
    for (var i = 1; i < _dimensions; i++) {
      if (aabb.range[i] > aabb.range[axis]) {
        axis = i;
      }
    }
    // Divide by the division axis and recursively build.
    // var list = list.orderBy((e) => _selector(e)[axis]).asList();
    list.sort(((a, b) => a[axis].compareTo(b[axis])));
    var point = list[median];
    var node = Node(point.clone());
    node.parent = parent;
    node.left = _buildTree(list.sublist(0, median), node);
    node.right = _buildTree(list.sublist(median + 1), node);
    update(node);
    return node;
  }

  void addPoint(Vector2 point, [bool allowRebuild = true]) {
    _root = _addByPoint(_root, point, allowRebuild, 0);
  }

  // void removePoint(Vector2 point, [bool allowRebuild = true]) {
  //   if (node == null) return;
  //   _removeNode(node, allowRebuild);
  // }

  Node? _addByPoint(
      Node? node, Vector2 point, bool allowRebuild, int parentDim) {
    if (node == null) {
      node = Node(point.clone());
      node.dimension = (parentDim + 1) % _dimensions;
      update(node);
      return node;
    }
    _pushDown(node);

    if (point[node.dimension] < node.point[node.dimension]) {
      node.left = _addByPoint(node.left, point, allowRebuild, node.dimension);
    } else {
      node.right = _addByPoint(node.right, point, allowRebuild, node.dimension);
    }
    update(node);
    bool needRebuild = allowRebuild && criterionCheck(node);
    if (needRebuild) node = rebuild(node);
    return node;
  }

  // checked
  void _pushDown(Node? node) {
    if (node == null) return;
    if (node.needPushDownToLeft && node.left != null) {
      node.left!.treeDownsampleDeleted |= node.treeDownsampleDeleted;
      node.left!.pointDownsampleDeleted |= node.treeDownsampleDeleted;
      node.left!.treeDeleted =
          node.treeDeleted || node.left!.treeDownsampleDeleted;
      node.left!.deleted =
          node.left!.treeDeleted || node.left!.pointDownsampleDeleted;
      if (node.treeDownsampleDeleted) {
        node.left!.downDeletedNum = node.left!.treeSize;
      }
      if (node.treeDeleted) {
        node.left!.invalidNum = node.left!.treeSize;
      } else {
        node.left!.invalidNum = node.left!.downDeletedNum;
      }
      node.left!.needPushDownToLeft = true;
      node.left!.needPushDownToRight = true;
      node.needPushDownToLeft = false;
    }
    if (node.needPushDownToRight && node.right != null) {
      node.right!.treeDownsampleDeleted |= node.treeDownsampleDeleted;
      node.right!.pointDownsampleDeleted |= node.treeDownsampleDeleted;
      node.right!.treeDeleted =
          node.treeDeleted || node.right!.treeDownsampleDeleted;
      node.right!.deleted =
          node.right!.treeDeleted || node.right!.pointDownsampleDeleted;
      if (node.treeDownsampleDeleted) {
        node.right!.downDeletedNum = node.right!.treeSize;
      }
      if (node.treeDeleted) {
        node.right!.invalidNum = node.right!.treeSize;
      } else {
        node.right!.invalidNum = node.right!.downDeletedNum;
      }
      node.right!.needPushDownToLeft = true;
      node.right!.needPushDownToRight = true;
      node.needPushDownToRight = false;
    }
  }

  void _removeNode(Node? node, bool allowRebuild) {
    if (node == null || node.deleted) return;

    _pushDown(node);
    node.deleted = true;
    node.invalidNum++;
    if (node.invalidNum == node.treeSize) {
      node.treeDeleted = true;
    }

    // update and rebuild parent
    var parent = node.parent;
    if (parent != null) {
      updateAncestors(parent);
      bool needRebuild = allowRebuild && criterionCheck(parent);
      if (needRebuild) parent = rebuild(parent);
    }
  }

  void updateAncestors(Node? node) {
    if (node == null) return;
    update(node);
    updateAncestors(node.parent);
  }

  void _removeByPoint(Node? node, Vector2 point, bool allowRebuild) {
    if (node == null || node.treeDeleted) return;

    _pushDown(node);
    if (node.point == point && !node.deleted) {
      node.deleted = true;
      node.invalidNum++;
      if (node.invalidNum == node.treeSize) {
        node.treeDeleted = true;
      }
      return;
    }

    if (point[node.dimension] < node.point[node.dimension]) {
      _removeByPoint(node.left, point, false);
    } else {
      _removeByPoint(node.right, point, false);
    }
    update(node);
    bool needRebuild = allowRebuild && criterionCheck(node);
    if (needRebuild) rebuild(node);
  }

  // checked
  void update(Node node) {
    var left = node.left;
    var right = node.right;
    node.treeSize = (left != null ? left.treeSize : 0) +
        (right != null ? right.treeSize : 0) +
        1;
    node.invalidNum = (left != null ? left.invalidNum : 0) +
        (right != null ? right.invalidNum : 0) +
        (node.deleted ? 1 : 0);
    node.downDeletedNum = (left != null ? left.downDeletedNum : 0) +
        (right != null ? right.downDeletedNum : 0) +
        (node.pointDownsampleDeleted ? 1 : 0);
    node.treeDownsampleDeleted = (left == null || left.treeDownsampleDeleted) &&
        (right == null || right.treeDownsampleDeleted) &&
        node.pointDownsampleDeleted;
    node.treeDeleted = (left == null || left.treeDeleted) &&
        (right == null || right.treeDeleted) &&
        node.deleted;
    var minList = <Vector2>[];
    var maxList = <Vector2>[];
    if (left != null && !left.treeDeleted) {
      minList.add(left.aabb.min);
      maxList.add(left.aabb.max);
    }
    if (right != null && !right.treeDeleted) {
      minList.add(right.aabb.min);
      maxList.add(right.aabb.max);
    }
    if (!node.deleted) {
      minList.add(node.point);
      maxList.add(node.point);
    }
    if (minList.isNotEmpty && maxList.isNotEmpty) {
      node.aabb = AABB()
        ..min = minList.min()
        ..max = maxList.max();
    }

    // TODO: Radius data for search: https://github.com/hku-mars/ikd-Tree/blob/main/ikd-Tree/ikd_Tree.cpp#L1312

    if (left != null) left.parent = node;
    if (right != null) right.parent = node;

    // TODO: root alpha value for multithread
  }

  // checked
  final minimalUnbalancedTreeSize = 10;
  final deleteCriterionParam = 0.3;
  final balanceCriterionParam = 0.6;
  bool criterionCheck(Node node) {
    if (node.treeSize <= minimalUnbalancedTreeSize) return false;
    double balanceEvaluation = 0.0;
    double deleteEvaluation = 0.0;
    var child = node.left ?? node.right!;
    deleteEvaluation = node.invalidNum / node.treeSize;
    balanceEvaluation = child.treeSize / (node.treeSize - 1);
    if (deleteEvaluation > deleteCriterionParam) return true;
    if (balanceEvaluation > balanceCriterionParam ||
        balanceEvaluation < 1 - balanceCriterionParam) return true;
    return false;
  }

  void rebuildAll() {
    _root = rebuild(_root);
  }

  // checked
  Node? rebuild(Node? node) {
    if (node == null) return null;
    var parent = node.parent;
    var points = flatten(node).toList();
    // log('rebuilding: $node objects: ${objects.length}');
    deleteTreeNodes(node);
    return _buildTree(points, parent);
    // if (parent == null) {
    //   _root = newNode;
    // } else if (parent.left == node) {
    //   parent.left = newNode;
    // } else if (parent.right == node) {
    //   parent.right = newNode;
    // }
  }

  // checked
  Iterable<Vector2> flatten(Node? node) sync* {
    if (node == null) return;
    _pushDown(node);
    if (!node.deleted) yield node.point;
    yield* flatten(node.left);
    yield* flatten(node.right);
  }

  void deleteTreeNodes(Node? node) {
    if (node == null) return;
    _pushDown(node);
    deleteTreeNodes(node.left);
    deleteTreeNodes(node.right);
  }

  double _calcDist(Vector2 a, Vector2 b) {
    double dist = 0;
    for (var dim = 0; dim < _dimensions; dim++) {
      dist += math.pow(a[dim] - b[dim], 2);
    }
    return dist;
  }

  // checked
  double _calcBoxDist(Node? node, Vector2 point) {
    if (node == null) return double.infinity;
    double minDist = 0;
    for (var dim = 0; dim < _dimensions; dim++) {
      if (point[dim] < node.aabb.min[dim]) {
        minDist += math.pow(point[dim] - node.aabb.min[dim], 2);
      }
      if (point[dim] > node.aabb.max[dim]) {
        minDist += math.pow(point[dim] - node.aabb.max[dim], 2);
      }
    }
    return minDist;
  }

  void _search(Node? node, int maxNodes, Vector2 point, BinaryHeap<Result> heap,
      double maxDist) {
    if (node == null || node.treeDeleted) return;
    double curDist = _calcBoxDist(node, point);
    double maxDistSqr = maxDist * maxDist;
    if (curDist > maxDistSqr) return;
    if (node.needPushDownToLeft || node.needPushDownToRight) {
      _pushDown(node);
    }
    if (!node.deleted) {
      double dist = _calcDist(point, node.point);
      if (dist <= maxDistSqr &&
          (heap.size() < maxNodes || dist < heap.peek().distance)) {
        if (heap.size() >= maxNodes) heap.pop();
        heap.push(Result(node, dist));
      }
    }
    double distLeftNode = _calcBoxDist(node.left, point);
    double distRightNode = _calcBoxDist(node.right, point);
    if (heap.size() < maxNodes ||
        distLeftNode < heap.peek().distance &&
            distRightNode < heap.peek().distance) {
      if (distLeftNode <= distRightNode) {
        _search(node.left, maxNodes, point, heap, maxDist);
        if (heap.size() < maxNodes || distRightNode < heap.peek().distance) {
          _search(node.right, maxNodes, point, heap, maxDist);
        }
      } else {
        _search(node.right, maxNodes, point, heap, maxDist);
        if (heap.size() < maxNodes || distLeftNode < heap.peek().distance) {
          _search(node.left, maxNodes, point, heap, maxDist);
        }
      }
    } else {
      if (distLeftNode < heap.peek().distance) {
        _search(node.left, maxNodes, point, heap, maxDist);
      }
      if (distRightNode < heap.peek().distance) {
        _search(node.right, maxNodes, point, heap, maxDist);
      }
    }
  }

  /// Find the [maxNodes] of nearest Nodes.
  /// Distance is calculated via Metric function.
  /// Max distance can be set with [maxDistance] param
  Iterable<Vector2> nearest(Vector2 point,
      {int maxNodes = 1, double maxDistance = double.infinity}) sync* {
    var heap = BinaryHeap<Result>((e) => -e.distance);
    _search(_root, maxNodes, point, heap, maxDistance);
    var found = math.min(maxNodes, heap.content.length);

    for (var i = 0; i < found; i++) {
      yield heap.content[i].node.point;
    }
  }

  int get length => _root?.length ?? 0;

  int get height => _root?.height ?? 0;
}

class Result {
  final Node node;
  final double distance;
  const Result(this.node, this.distance);
}

class Node {
  Vector2 point;
  int dimension = 0;
  Node? parent;
  Node? left;
  Node? right;
  int treeSize = 0;
  int invalidNum = 0;
  int downDeletedNum = 0;
  bool deleted = false;
  bool treeDeleted = false;
  bool needPushDownToLeft = false;
  bool needPushDownToRight = false;
  bool treeDownsampleDeleted = false;
  bool pointDownsampleDeleted = false;

  AABB aabb = AABB();

  Node(this.point);

  int get length {
    return 1 +
        (left == null ? 0 : left!.length) +
        (right == null ? 0 : right!.length);
  }

  int get height {
    return 1 +
        math.max(
          left == null ? 0 : left!.height,
          right == null ? 0 : right!.height,
        );
  }

  int get depth {
    return 1 + (parent == null ? 0 : parent!.depth);
  }
}

Answer 2

you may consider this article If your goals not too much and want simple ways

If you want to search for any of several goals, construct a heuristic h'(x) that is the minimum of h1(x), h2(x), h3(x), ... where h1, h2, h3 are heuristics to each of the nearby spots.

One way to think about this is that we can add a new zero-cost edge from each of the goals to a new graph node. A path to that new node will necessarily go through one of the goal nodes.

If you want to search for paths to all of several goals, your best option may be Dijkstra’s Algorithm with early exit when you find all the goals. There may be a variant of A* that can calculate these paths; I don’t know.

If you want to search for spot near a single goal, ask A* search to find a path to the center of the goal area. While processing nodes from the OPEN set, exit when you pull a node that is near enough.

Answer 3

Use Dijkstra's algorithm, which has as it's output the minimal cost to all reachable points. Then you just select the goal points from the output.

Answer 4

How many goals, tens or thousands? If tens your way will work fine, if thousands then nearest neighbor search will give you ideas on setting up your data to search quickly.

The tradeoffs are obvious, spatially organizing your data to search will take time and on small sets brute force will be simpler to maintain. Since you're constantly evaluating I think that structuring the data will be worthwhile at very low numbers of points.

An alternate way to do this would be a modified flood fill algorithm that stops once it reaches a destination point during the flood.

Answer 5

First, decide whether you need to optimize, because any optimization is going to complicate your code, and for a small number of goals, your current solution is probably fine for a simple heuristic like Manhattan distance.

Before taking the first step, compute the heuristic for each goal. Remember the nearest goal as the currently selected goal, and move toward it, but subtract the maximum possible progress toward any goal from all the other distances. You can consider this second value a "meta-heuristic"; it is an optimistic estimate of the heuristic for other goals.

On subsequent steps, compute the heuristic for the current goal, and any goals with a "meta-heuristic" that is less than or equal to the heuristic. The other goals can't possibly have a better heuristic, so you don't need to compute them. The nearest goal becomes the new current goal; move toward it, subtracting the maximum possible progress from the others. Repeat until you arrive at a goal.