2D Pathfinding: Grid A*, Any-Angle Search, and Visibility Graphs in Practice

A character runs toward a health pack in a game. A warehouse robot tries to reach a shelf. A drone or ground vehicle looks for a landing or parking spot.

Underneath all of these sits the same core problem:

In a 2D environment with obstacles, how do we find a path from a start point to one (or many) goals that is short enough, smooth enough, and fast enough to compute?

This article walks through three practical families of algorithms you can actually implement:

• grid-based A* (with realistic tweaks),
• grid any-angle search (in the spirit of the ANYA algorithm),
• visibility graph + A* (polygon-based planning),

first in the single-goal setting, then in the multi-goal setting. At the end, there is a small numerical comparison on real data to see how they behave in terms of path length, runtime and search effort.

Before talking about grids and graphs, it helps to fix one basic concept that everything else builds on: the difference between Dijkstra and A*.

1. From Dijkstra to A*: Giving the Search a Sense of Direction

Both Dijkstra and A* work on a graph: you have nodes (states), edges (possible moves), and non-negative edge weights (costs). The question is how to explore that graph efficiently.

Dijkstra’s algorithm finds the shortest path from a single source to every node. It maintains a distance dist[v] for each node, starts with dist[start] = 0, and always expands the node with the smallest distance so far. Conceptually, it is like dropping a stone into water: the “wavefront” expands in all directions with equal curiosity. If you stop as soon as you pop the target node from the priority queue, you get the optimal path to that target, but the exploration up to that point has no notion of “direction” other than pure distance.

A keeps Dijkstra’s core idea but adds a heuristic. Instead of ordering nodes only by g(n) (distance from the start), it orders them by f(n)=g(n)+h(n) where h(n) estimates the remaining distance from n to the goal. If this estimate never overestimates the true cost, A still guarantees an optimal path, but the search is guided: nodes that “look closer” to the goal are tried earlier. In other words, Dijkstra is just A* with h(n) = 0 everywhere.

That single modification — adding a reasonable heuristic — makes a massive difference in practice. Almost every pathfinding method in this article uses A (or an A-like idea) on top of some underlying representation: a grid, any-angle intervals, or a visibility graph.

2. Grid-Based A*

The most familiar representation is a grid. Continuous space is discretized into square cells, each either free or blocked. It’s a simple model and, with the right tricks, surprisingly strong.

2.1. Building the grid

To turn geometry into a grid, you first decide a planning region (a bounding box in world coordinates) and a cell size in meters. From there you compute the number of rows and columns:

int rows = (int) Math.ceil(heightMeters / cellSizeMeters);
int cols = (int) Math.ceil(widthMeters  / cellSizeMeters);

For each cell, you compute its center in world coordinates and check whether that point lies inside any obstacle polygon. The result is a blocked[r][c] mask:

boolean[][] blocked = new boolean[rows][cols];

for (int r = 0; r < rows; r++) {
    for (int c = 0; c < cols; c++) {
        Point2D center = cellCenter(r, c, originX, originY, cellSizeMeters);
        blocked[r][c] = isInsideAnyObstacle(center, obstaclePolygons);
    }
}

If you allow diagonal moves, you typically forbid “cutting corners”: a diagonal step is only allowed if the two adjacent orthogonal cells are free. That one rule eliminates many subtle collision issues.

Once this is done, the grid is a graph: each free cell is a node, and edges connect neighboring free cells.

2.2. A minimal grid A* and the octile heuristic

On this grid, an 8-connected A* looks fairly standard. You keep a g matrix for the cost so far, closed flags, and parent pointers to reconstruct the path:

class Cell {
    final int r, c;
    Cell(int r, int c) { this.r = r; this.c = c; }
}

double[][] g = new double[rows][cols];
boolean[][] closed = new boolean[rows][cols];
Cell[][] parent = new Cell[rows][cols];

for (double[] row : g) Arrays.fill(row, Double.POSITIVE_INFINITY);

PriorityQueue<Cell> open = new PriorityQueue<>(Comparator.comparingDouble(
    n -> g[n.r][n.c] + heuristic(n, goal)
));

g[start.r][start.c] = 0.0;
open.add(start);

while (!open.isEmpty()) {
    Cell cur = open.poll();
    if (closed[cur.r][cur.c]) continue;
    closed[cur.r][cur.c] = true;

    if (cur.equals(goal)) {
        return reconstructPath(parent, cur);
    }

    for (int k = 0; k < 8; k++) {
        int nr = cur.r + dRow[k];
        int nc = cur.c + dCol[k];
        if (!inBounds(nr, nc)) continue;
        if (blocked[nr][nc]) continue;

        double cand = g[cur.r][cur.c] + stepCost(k); // 1 or √2 times cell size
        if (cand < g[nr][nc]) {
            g[nr][nc] = cand;
            parent[nr][nc] = cur;
            open.add(new Cell(nr, nc));
        }
    }
}

For 8-neighbor movement, a very common heuristic is the octile distance, which approximates the length of the best path that uses horizontal, vertical, and diagonal steps:

double octile(Cell a, Cell b, double cellMeters) {
    int dr = Math.abs(a.r - b.r);
    int dc = Math.abs(a.c - b.c);
    int min = Math.min(dr, dc), max = Math.max(dr, dc);
    return (min * Math.sqrt(2.0) + (max - min)) * cellMeters;
}

With this heuristic, A* remains admissible and consistent. You get optimal paths with reasonable performance. But on real maps, two issues often show up: the search spreads more sideways than you’d like, and the resulting paths exhibit visible zigzag noise.

2.3. Heuristic weighting: trading a bit of optimality for speed

One widely used trick is to give the heuristic a little more influence than g. Instead of f = g + h, you use f(n) = g(n) + w⋅h(n), w>1

final double HEURISTIC_WEIGHT = 1.5;

PriorityQueue<Cell> open = new PriorityQueue<>((a, b) -> {
    double ha = heuristic(a, goal);
    double hb = heuristic(b, goal);
    double fa = g[a.r][a.c] + HEURISTIC_WEIGHT * ha;
    double fb = g[b.r][b.c] + HEURISTIC_WEIGHT * hb;
    return Double.compare(fa, fb);
});

When w = 1.0, this is classic A*. As you increase w, the search becomes more “greedy” toward the goal; it explores fewer nodes and typically runs faster, but you give up strict optimality. In many practical maps, values around 1.2–1.5 reduce node expansions dramatically while changing the path length only by a few percent. Very large weights push you toward greedy best-first search and can clearly hurt path quality.

You can think of this weight as a knob between mathematical optimality and real-time responsiveness.

2.4. Zigzag, string pulling, and turn penalties

Even with a good heuristic, grid paths tend to zigzag. The root cause is that continuous straight lines are approximated by a staircase of discrete moves, and many patterns have equal or nearly equal cost. Small numerical differences or tie-breaking rules can push the search into visually noisy behaviour.

Two small techniques help a lot in practice: a line-of-sight “string pulling” pass after the search, and a tiny turn penalty during the search itself.

The string-pulling idea is simple. Once you have a path, you walk along it with a moving “anchor”. Starting from the first node, you try to connect this anchor directly to later nodes on the path. As long as there is clear line of sight between anchor and candidate, you do nothing. The moment line of sight fails, you accept the last valid node as a new waypoint and move the anchor forward. The implementation looks roughly like this:

List<Cell> smooth(List<Cell> path) {
    if (path.size() <= 2) return path;

    List<Cell> result = new ArrayList<>();
    result.add(path.get(0));
    int anchorIndex = 0;

    for (int i = 1; i < path.size() - 1; i++) {
        Cell anchor = path.get(anchorIndex);
        Cell next   = path.get(i + 1);

        if (!hasLineOfSight(anchor, next)) {
            result.add(path.get(i));
            anchorIndex = i;
        }
    }

    result.add(path.get(path.size() - 1));
    return result;
}

The hasLineOfSight function walks between the two cell centers with a Bresenham-style loop, rejecting the segment if it hits blocked cells or illegal diagonals. This post-processing tends to shorten the path and makes it visually smoother.

A small turn penalty works inside the search. Whenever a successor changes direction relative to its parent, you add a tiny cost to g:

final double TURN_PENALTY = 0.001; // in meters

double candidateCost(Cell parent, Cell cur, Cell nb, double stepCost) {
    double cost = g[cur.r][cur.c] + stepCost;
    if (parent != null) {
        int dpr = cur.r - parent.r;
        int dpc = cur.c - parent.c;
        int dnr = nb.r - cur.r;
        int dnc = nb.c - cur.c;
        if (dpr != dnr || dpc != dnc) {
            cost += TURN_PENALTY; // penalize a change of heading
        }
    }
    return cost;
}

A search without string pulling and turn penalty*

A search with string pulling and turn penalty*

This small bias nudges the search away from unnecessary zigzags even before smoothing. Among several almost equivalent options, the algorithm tends to prefer routes with fewer direction changes. In combination, a slightly weighted heuristic, LOS-based smoothing, and a turn penalty turn a textbook grid A* into a practical, stable planner that behaves well under real constraints.

3. Grid Any-Angle Search (ANYA-Style)

Grid A* treats each cell as a node and only moves along grid edges and diagonals. Any-angle algorithms take a different view. Instead of committing to fixed cell centers, they represent groups of cells as intervals and allow paths to pass through arbitrary points, as long as they remain collision-free.

A typical any-angle state has two parts:

• a root point (usually a grid vertex or cell center), and
• a horizontal interval on a row [x_left, x_right].

Conceptually, the state says: “from this root, I can see and reach any point on this interval.” The successsor generation logic moves these intervals across rows, reflecting visibility constraints, and builds new states that represent new sets of reachable points.

The search loop itself still looks like A*: states are pulled from a priority queue ordered by f = g + h, and each state generates successors via an expansion policy. Only the representation of “what a node means” is different, and the heuristics are more involved.

The heuristic in an ANYA-style algorithm cannot simply measure the distance from a single node coordinate to the target, because a node corresponds to a whole interval of coordinates. Instead, it tries to estimate the cheapest way to reach the target from the root, possibly via either endpoint of the interval or directly across it. A simplified version looks like this:

class AnyaHeuristic implements Heuristic<AnyaState> {
    EuclideanDistanceHeuristic h = new EuclideanDistanceHeuristic();

    @Override
    public double getValue(AnyaState n, AnyaState t) {
        int row    = n.interval.getRow();
        double L   = n.interval.getLeft();
        double R   = n.interval.getRight();
        double tx  = t.root.x;
        double ty  = t.root.y;
        double rx  = n.root.x;
        double ry  = n.root.y;

        // If root and target are on the same side of this row,
        // mirror the target through the row.
        if (ry < row && ty < row) {
            ty += 2 * (row - ty);
        } else if (ry > row && ty > row) {
            ty -= 2 * (ty - row);
        }

        // Project the interval endpoints onto the target row
        double riseRootToRow   = Math.abs(ry - row);
        double riseRowToTarget = Math.abs(row - t.root.y);
        double lrun = rx - L;
        double rrun = R  - rx;

        double leftProj  = L - riseRowToTarget * (lrun / riseRootToRow);
        double rightProj = R + riseRowToTarget * (rrun / riseRootToRow);

        if (tx < leftProj) {
            // must go through left endpoint
            return h.h(rx, ry, L, row) + h.h(L, row, tx, ty);
        }
        if (tx > rightProj) {
            // must go through right endpoint
            return h.h(rx, ry, R, row) + h.h(R, row, tx, ty);
        }

        // target lies “in front” of the interval interior
        return h.h(rx, ry, tx, ty);
    }
}

This kind of heuristic is still admissible but encodes more geometry than a plain Euclidean distance from a grid cell center. Together with an expansion policy that knows how to zig-zag intervals around obstacles, the algorithm can produce any-angle paths that are optimal with respect to Euclidean distance on the underlying grid.

The trade-off is clear in implementation and runtime: each state expansion is significantly more complex than in vanilla grid A. The algorithm often expands far fewer states than grid A, but each expansion costs more CPU. In practice, it is not unusual to see total runtime two to four times higher than a well-implemented grid A* on the same map, even though the resulting paths are shorter and smoother.

Multi-goal search on top of intervals is also more subtle. In a grid or visibility graph, a node means a point; if that point is a goal and the heuristic is admissible, the first goal popped by A* is guaranteed to be the best one. With intervals, a single node may contain several goal points on the same row, and it is not automatically safe to stop as soon as “any goal happens to lie inside this interval.” In practice, you need to explicitly construct final states at each goal position, compute their exact g cost, continue the search for a while, and keep track of the best goal so far. It works, but it is more complex than the very clean multi-goal trick used on grids and visibility graphs.

Any Angle (Anya) Algorithm

4. Visibility Graphs and A*: Planning on Polygons

If your environment is described by polygons — rooms, obstacles, boundaries — then a visibility graph is a very natural structure. Instead of rasterizing the world onto a grid, you build a graph whose nodes are geometrically meaningful points, and whose edges connect pairs of nodes that can see each other without intersecting an obstacle.

A typical construction starts by assembling a node set. The start position becomes one node, each goal becomes another, and all obstacle vertices join the list. In some implementations, extra sample points are added along long obstacle edges to give the planner more flexibility and avoid extreme “corner hugging”.

List<Point> nodes = new ArrayList<>();

nodes.add(start);         // start
nodes.addAll(goals);      // goals

for (Polygon poly : obstacles) {
    for (Coordinate c : poly.getCoordinates()) {
        nodes.add(new Point(c.x, c.y));
    }
}

Edges are added between pairs of nodes that are mutually visible. A naive version would try every pair of nodes, build the segment between them, and test that segment against every obstacle. This gives you a worst-case complexity of roughly O(N²·M), which quickly becomes expensive.

A more realistic implementation builds a spatial index (such as an R-tree) over prepared obstacle geometries and uses it to cut down intersection tests. For each candidate segment, you query the index with the segment’s bounding box to get a small set of nearby obstacles, and only perform accurate intersection checks against this subset. A typical visibility test looks like this:

boolean edgeVisible(Coordinate a, Coordinate b, SpatialIndex index) {
    LineString seg = gf.createLineString(new Coordinate[]{a, b});

    @SuppressWarnings("unchecked")
    List<PreparedGeometry> near = index.query(seg.getEnvelopeInternal());
    if (near.isEmpty()) return true;

    for (PreparedGeometry pg : near) {
        Geometry g = pg.getGeometry();
        if (!g.getEnvelopeInternal().intersects(seg.getEnvelopeInternal()))
            continue;
        if (!g.intersects(seg))
            continue;

        // Sample interior points to detect intersection with obstacle interior
        final int SAMPLES = 17;
        for (int s = 1; s < SAMPLES; s++) {
            double t = s / (double) SAMPLES;
            double x = a.x + t * (b.x - a.x);
            double y = a.y + t * (b.y - a.y);
            if (pg.contains(gf.createPoint(new Coordinate(x, y)))) {
                return false;
            }
        }
    }
    return true;
}

Once visibility is known, the graph is straightforward to assemble:

List<List<Edge>> adj = new ArrayList<>();
for (int i = 0; i < nodes.size(); i++) adj.add(new ArrayList<>());

for (int i = 0; i < nodes.size(); i++) {
    for (int j = i + 1; j < nodes.size(); j++) {

        double dist = nodes.get(i).distance(nodes.get(j));
        if (dist > maxEdgeLength) continue;

        if (!edgeVisible(nodes.get(i).p, nodes.get(j).p, obstacleIndex))
            continue;

        adj.get(i).add(new Edge(i, j, dist));
        adj.get(j).add(new Edge(j, i, dist)); // undirected
    }
}

At that point, you are back in a standard graph setting: a set of nodes, weighted edges, and a cost metric that is simply the Euclidean length of each segment. A (or Dijkstra) on this graph is completely conventional: the only difference compared to grid A is the structure of the graph and the geometry behind it.

The cost trade-off is also clear. Building the visibility graph is the expensive part: you pay for spatial indexing, intersection checks, and O(N²) candidate edges. After that, queries are cheap and paths are geometrically very clean, especially around polygon corners.

*Visibility Graph with A**

5. Multi-Goal Pathfinding: One Start, Many Targets

Now extend the original problem. Instead of a single goal, you have a set of potential targets G={g1,g2,…,gk}G = {g_1, g_2, \dots, g_k}G={g1​,g2​,…,gk​}. You want the shortest path from the start to any of these goals, and you also want to know which goal ends up being the winner.

On a grid or a visibility graph, where each state is a single point, there is a very simple fix: keep all goals in the heuristic. For each goal gig_igi​, define a per-goal heuristic hi(n)h_i(n)hi​(n) (distance from n to g_i), and then define h(n) = min​hi​(n).

If each hih_ihi​ is admissible, this combined heuristic is also admissible. A* uses f(n) = g(n) + h(n) as usual, and the stopping condition is simply “stop when the popped node is in the goal set”. The first goal popped is guaranteed to be the one with minimal cost from the start.

On a grid, the code for this looks almost trivial:

double multiGoalHeuristic(Cell n, Set<Cell> goals, double cellMeters) {
    double best = Double.POSITIVE_INFINITY;
    for (Cell g : goals) {
        double h = octile(n, g, cellMeters);
        if (h < best) best = h;
    }
    return best;
}

And the main loop is only slightly different:

if (goals.contains(cur)) {
    // cur is one of the goals
    // with h = min h_i, this is the best goal in G
    return reconstructPath(parent, cur);
}

Exactly the same trick applies to visibility graphs: replace Cell with node ID, and the octile distance with Euclidean distance between two graph nodes. The combined heuristic remains admissible, and the first goal ever removed from the open list is the cheapest of all.

This is one of the reasons why grid A and visibility graph + A are so attractive when you have multiple potential targets: multi-goal support costs almost nothing in code complexity, but saves a lot of work compared to running one separate search for each goal.

In interval-based any-angle search, things are not quite as clean. Here, a node doesn’t represent a single coordinate but a root plus an entire horizontal interval. An interval can contain several potential goal x-coordinates on the same row. If you stop the moment “some goal lies inside this interval”, there is no theoretical guarantee that this goal is globally the best one; another interval path might lead to the same or a different goal with lower overall cost. In practice, a reliable multi-goal implementation on top of ANYA-style search needs to treat goals explicitly: when an interval is found to contain a goal, you construct a final node exactly at the goal position, compute its true cost, store it, and continue searching until you are confident that no better goal remains. It works, but it doesn’t have the same elegant one-line stopping condition.

6. Choosing Between Grid A*, Any-Angle, and Visibility Graphs

By now the three approaches are concrete enough that the real question becomes: “In an actual project, which one should I use when?”

A tuned grid A* (with a mildly weighted heuristic, LOS-based smoothing, and a small turn penalty) is usually the closest thing to a default option. You discretize the world into cells, mark blocked ones, and let the search engine do its job. It is a good fit when:

• The environment can reasonably be represented at a fixed or moderate resolution (e.g., game maps, warehouse layouts, simple 2D navigation).
• You care about millisecond-level response times more than about millimeter-level geometric optimality.
• Obstacles or costs may change over time and you want to re-run searches quickly on the same grid.

With a bit of heuristic weighting, line-of-sight smoothing and a small turn penalty, grid A* often hits the “good-enough path, fast-enough, simple-enough” sweet spot. In systems that issue many queries per second (game AI, multi-agent simulations, basic robot fleets), its predictable performance and modest implementation complexity are a big deal.

The any-angle family (grid-based ANYA, navmesh-based Polyanya, and related variants) keeps the underlying representation (grid or mesh) but removes the restriction to a small fixed move set. Grid-based ANYA improves on classic grid A* by eliminating the staircase effect and producing paths that are Euclidean-optimal with respect to the discretization. Polyanya extends the same idea to polygonal navigation meshes, operating directly on faces and visibility-constrained segments. These methods make sense when:

• Your primary representation is already a grid or a navmesh and you want shorter, smoother trajectories without changing the entire map pipeline.
• Path quality matters noticeably more than raw throughput: cinematic camera control, high-speed vehicles, or visually scrutinised simulations.
• The number of queries is moderate and you are willing to pay a bit more per query to get better geometry.

The trade-off is complexity: ANYA/Polyanya-style planners require interval- or face-based states, custom heuristics, and heavier per-state geometric computations. In very high-throughput systems, a naïve any-angle implementation can be significantly slower than a well-tuned grid A* for only modest gains in path length, so you need to be clear about what you are buying.

A visibility graph + A* is a natural choice when the environment is polygonal and relatively static. Nodes are geometrically meaningful points (start, goals, obstacle vertices, sampled edge points); edges connect pairs of nodes that can see each other without intersecting any obstacle. This approach works particularly well when:

• Your world is natively polygonal (CAD drawings, GIS/map data, building floor plans, urban layouts).
• Obstacles do not change much over time; you can afford to build the graph once and reuse it for many queries.
• Geometric path quality is critical: you want something as close as possible to the true shortest path, not just “roughly in the right direction.”

Building the graph is the expensive part: there may be many vertices, many candidate edges, and lots of visibility checks, even with a spatial index. But once that cost is paid, A on the resulting graph is extremely fast, and the paths are typically shorter and cleaner than anything you can get from a grid at comparable resolution. Multi-goal pathfinding is just as straightforward as on a grid: you set the heuristic to the minimum distance to any goal and keep the standard A stopping rule.

When distances become large (for example, routes spanning many kilometres), scalability becomes a dominant concern. A uniform fine grid over a huge area quickly leads to a quadratic blow-up in the number of cells; both memory and search time can become prohibitive. In these situations you typically either:

• move to a hierarchical / multi-resolution grid, where distant regions are represented coarsely and only areas near the agent and goals are refined,
• shift to a polygonal skeleton or navmesh plus visibility / any-angle search, where the number of nodes is driven mainly by obstacle complexity rather than physical map size.

The any-angle family (ANYA, Polyanya, etc.) continues to produce high-quality trajectories over long distances, but the per-state cost does not shrink with map size, so there is no magic scalability advantage by itself. On very large maps, you often get a better balance by combining a coarse global planner (hierarchical grid or sparse visibility/navmesh graph) with local refinements near obstacles and around the agent.

Seen together, these three techniques are not really competitors; they are tools in a toolbox:

• If you want fast implementation, clean code, and high query throughput on small to medium-scale maps, a tuned grid A* is usually the sensible first choice.
• If your world is represented as a grid or navmesh and you care a lot about geometric path quality, ANYA/Polyanya-style any-angle planning is a strong upgrade.
• If you work in a polygonal, mostly static environment, have many queries and possibly large distances, a visibility graph plus A* (optionally combined with any-angle refinements) often gives you the most refined and scalable solution.

In real projects, the decision is less about which algorithm is “best in theory” and more about matching the method to the world representation (grid vs. polygons/navmesh), the size and dynamics of the environment, the throughput you need, and how much cost you are willing to pay for extra path quality.

I would like to extend my sincere appreciation to İsmail Tapan, whose insights greatly contributed to the development of this article.

REFERENCES

Harabor, D. D., Öz, D., Grastien, A., & Aksakalli, V. (2016). Optimal Any-Angle Pathfinding In Practice. Journal of Artificial Intelligence Research, 56, 89–118.

Hart, P. E., Nilsson, N. J., & Raphael, B. (1968). A Formal Basis for the Heuristic Determination of Minimum Cost Paths. IEEE Transactions on Systems Science and Cybernetics, 4(2), 100–107.

Pinter, M., 2001. Toward more realistic pathfinding. Game Developer Magazine, 8(4).

Alt, H., & Godau, M. (1988). Visibility graphs and obstacle-avoiding shortest paths. Discrete & Computational Geometry, 1, 189–202.

[embed]Bitbucket Edit descriptionbitbucket.org

https://github.com/nasa/icarous/blob/fc8dbc4d2d8e3a9ebd6d0f6fda3b5d216f1456a6/Modules/Core/TrajectoryManager/DubinsPlanner/DubinsPlanner.cpp