Finding Shortest Path in the Plane with Obstacles

17.5K views

Among the games I have developed on CodinGame, one of my favorites is Ghost in the Cell because it gave me the opportunity to solve a very interesting problem: finding the shortest path in a plane while avoiding obstacles. This is an algorithm I've been taught during a robot motion planning class when I was a student but I never had the opportunity to implement it.

In this article, I will show you how to compute a path from one factory to another while avoiding other factories. A factory is simply a round obstacle.

Expected Result

The algorithm will be divided in 3 steps:

computing polygons for each round obstacle
construction of the visibility graph (that's the tricky part)
computing the shortest path

Computation of the Polygons

The obstacles are round, however we need a discrete amount of vertices to build the visibility graph. A circle can easily be approximated by a regular polygon.

First, some vocabulary:

A regular polygon is a polygon equiangular and equilateral
The apothem ( $$a$$ ) is the distance from the center to the midpoint of one of its sides
The circumradius ( $$R$$ ) is the distance from the center to one of its vertices
The inscribed circle of a regular polygon is a circle with the same center and radius $$a$$

We are looking for a polygon for which the inscribed circle is the radius of a factory, plus a gap to forbid a path to go too close to an obstacle (too dangerous). The apothem of the polygon is : $$a = obstacle.radius + extraSpace$$ . The circumradius can be calculated from the apothem and the number of edges: $$R = \frac{a}{cos(\pi / n)}$$ .

The coordinates of the vertices of a polygon of $$n$$ edges are:

$$\left \{ \begin{matrix}
x = obstacle.x + R \cdot cos(t) \\
y = obstacle.y + R \cdot sin(t)
\end{matrix} \right .\quad\text{with}\quad t \in [0, \frac{2\pi}{n}, 2\frac{2\pi}{n}, 3\frac{2\pi}{n}, ..., (n - 1)\frac{2\pi}{n}]
$$

Here's the implementation of the polygon function:

function polygon (src) {
  const res = []
  const n = 8 // Change me!
  const r = (src.radius + extraSpace) / Math.cos(Math.PI / n)
  for (let t = 0; t < n; t++) {
    res.push({
      x: src.x + Math.round((r + 0.5) * Math.cos((2 * Math.PI * t) / n)),
      y: src.y + Math.round((r + 0.5) * Math.sin((2 * Math.PI * t) / n))
    })
  }
  return res
}

Feel free to play with the number of edges.

Visibility Graph

From the set of vertices of all the polygons, we create a graph called Visibility Graph. Each vertex is connected to all the vertices that can be reached by a direct line without crossing any obstacle.

For this, we need a function to detect an intersection between two segments. We'll also use a function to detect an intersection with a circle but it was not strictly necessary (this simplifies the code). The function directPath returns true when two points can be connected without intersecting a polygon.

The function buildGraph iterates over all the vertices of all the polygons and create a bidirectional edge when directPath returns true. We also add a unidirectional edge from each center (these edges only allows to exit a factory).

function lineIntersects (p0, p1, p2, p3) {
  var s1x = p1.x - p0.x
  var s1y = p1.y - p0.y
  var s2x = p3.x - p2.x
  var s2y = p3.y - p2.y
  var s = (-s1y * (p0.x - p2.x) + s1x * (p0.y - p2.y)) / (-s2x * s1y + s1x * s2y)
  var t = (s2x * (p0.y - p2.y) - s2y * (p0.x - p2.x)) / (-s2x * s1y + s1x * s2y)
  return s > 0 && s < 1 && t > 0 && t < 1
}
function linePolygonIntersection (src, dst, o) {
  var c = polygon(o)
  for (var k = 0; k < c.length; k++) {
    if (lineIntersects(c[k], c[(k + 1) % c.length], src, dst)) {
      return true
    }
  }
  return false
}
function lineCircleIntersection (src, dst, o) {
  const dx = dst.x - src.x
  const dy = dst.y - src.y
  const dr2 = dx * dx + dy * dy
  const vect = (src.x - o.x) * (dst.y - o.y) - (dst.x - o.x) * (src.y - o.y)
  const discr = (o.radius + extraSpace) * (o.radius + extraSpace) * dr2 - vect * vect
  if (discr > 0) {
    var sy = dy > 0 ? 1 : -1
    var sqrtDiscr = Math.sqrt(discr)
    var collision1 = {
      x: (vect * dy - sy * dx * sqrtDiscr) / dr2 + o.x,
      y: (-vect * dx - Math.abs(dy) * sqrtDiscr) / dr2 + o.y
    }
    var collision2 = {
      x: (vect * dy + sy * dx * sqrtDiscr) / dr2 + o.x,
      y: (-vect * dx + Math.abs(dy) * sqrtDiscr) / dr2 + o.y
    }
    var mx = Math.min(src.x, dst.x)
    var mmx = Math.max(src.x, dst.x)
    var my = Math.min(src.y, dst.y)
    var mmy = Math.max(src.y, dst.y)
    if ((mx <= collision1.x && collision1.x <= mmx && my <= collision1.y && collision1.y <= mmy) ||
          (mx <= collision2.x && collision2.x <= mmx && my <= collision2.y && collision2.y <= mmy)) {
      return true
    }
  }

Shortest Path

Any conventional shortest path algorithm can be used to compute the path from a factory to another using the visibility graph. Here's the implementation of an A*. For the sake of simplicity, I've used a sorted array instead of a priority queue, feel free to improve that part.

function distance (a, b) {
  return Math.sqrt(squareDistance(a, b))
}
function squareDistance (a, b) {
  return (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y)
}
function shortestPath (graph, srcNode, dstNode) {
  var q = [{ x: srcNode.x, y: srcNode.y, path: [srcNode], currentLength: 0, heuristic: 0 }]
  var visited = {}
  while (q.length > 0) {
    q.sort((a, b) => a.heuristic - b.heuristic) // this should be a queue
    const el = q.shift()
    if (directPath(dstNode, el)) {
      return [...el.path, dstNode]
    }
    visited[`${el.x} ${el.y}`] = true
    const successors = graph[`${el.x} ${el.y}`]
    if (!successors) {
      continue
    }
    successors
      .filter(succ => !visited[`${succ.x} ${succ.y}`])
      .forEach(succ => {
        const distToSucc = distance(succ, el)
        const newPath = el.path.slice()
        const currentLength = el.currentLength + distToSucc
        const heuristic = currentLength + distance(succ, dstNode)
        newPath.push({ x: succ.x, y: succ.y })
        q.push({ x: succ.x, y: succ.y, path: newPath, currentLength, heuristic })
      })
  }
}

By increasing the number of edges for each polygon, the path become smoother.

Create your playground on Tech.io

This playground was created on Tech.io, our hands-on, knowledge-sharing platform for developers.

Go to tech.io

ABOUT US CAREERS PRIVACY FAQ BLOG

The #1 tech hiring platform

SCREENING INTERVIEWING

Computation of the Polygons

Visibility Graph

Shortest Path

Exercice Javascript

Module 288 : Utilisation des techniques de programmation dans le Frontend Web

JavaScript Array Methods

JavaScript promises, mastering the asynchronous