# Finding Shortest Path in the Plane with Obstacles

[CG]Maxime
8,033 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.

The algorithm will be divided in 3 steps:

## 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 () is the distance from the center to the midpoint of one of its sides
• The circumradius () 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

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 : . The circumradius can be calculated from the apothem and the number of edges: .

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

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))
})
}
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

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
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

## 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 }]
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

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