# Concise tutorial for 2-dimensional convex polygons collision detection using the separating axis theorem.

## Prerequisites

This short tutorial assumes basic knowledge of linear algebra. It is especially important to know what a dot product is and it's geometric interpretation.

You can read this tutorial skipping the code bits and it should still make sense. That being said, the code is in Python and uses the numeric module `numpy`, here imported as `np`.

## Separating axis theorem (SAT)

_Relevant Wikipedia [arcticle](https://en.wikipedia.org/wiki/Hyperplane_separation_theorem)._

In simple terms, the SAT states that if we can draw a straight line between two convex shapes, they do not overlap. We'll call this line a _separating line_. More technically, two convex shapes do not overlap if there exists an axis onto which the two objects' projections do not overlap. We'll call this axis a _separating axis_. The image below illustrates this idea.


For 2-dimensional polygons, we only need to examin the axes that are orthogonal to each of the shape's edges. If the projections don't overlap in at least one of them, the shapes do not overlap. The above image shows only two of those axis, but that particular case has 9 of them (one for each edge).

To move (or push) one polygon away from the other, we also need to find a vector that, when added to the polygons position, will make the shapes not overlap. We want the minimum displacement possible, We'll call this vector the _minimum push vector_ (MPV). For 2-dimensional polygons, this vector will lie in some of the orthogonal axes. The image below illustrates that.


## The algortihm

The algorithm works as follows. For each edge in each shape, test if its orthogonal axis is a separating axis. If any of those orthogonals is, the shapes do not overlap. Otherwise, if none of them is, the shapes overlap, and the smallest push vector of each orthogonal is the MPV.

The code below uses the functions `edges_of()`, `orthogonal()`, `dot()` and `is_separating_axis()`, which will be specified in the next session, but it should be somewhat intuitive what they do.

import numpy as np

def collide(p1, p2):
    '''
    Return True and the MPV if the shapes collide. Otherwise, return False and
    None.

    p1 and p2 are lists of ordered pairs, the vertices of the polygons in the
    counterclockwise direction.
    '''

    p1 = [np.array(v, 'float64') for v in p1]
    p2 = [np.array(v, 'float64') for v in p2]

    edges = edges_of(p1)
    edges += edges_of(p2)
    orthogonals = [orthogonal(e) for e in edges]

    push_vectors = []
    for o in orthogonals:
        separates, pv = is_separating_axis(o, p1, p2)

        if separates:
            # they do not collide and there is no push vector
            return False, None
        else:
            push_vectors.append(pv)

    # they do collide and the push_vector with the smallest length is the MPV
    mpv =  min(push_vectors, key=(lambda v: np.dot(v, v)))

    # assert mpv pushes p1 away from p2
    d = centers_displacement(p1, p2) # direction from p1 to p2
    if np.dot(d, mpv) > 0: # if it's the same direction, then invert
        mpv = -mpv

    return True, mpv


def centers_displacement(p1, p2):
    """
    Return the displacement between the geometric center of p1 and p2.
    """
    # geometric center
    c1 = np.mean(np.array(p1), axis=0)
    c2 = np.mean(np.array(p2), axis=0)
    return c2 - c1

## Algorithm break down

### Finding the edges and orthogonals of a polygon: `edges_of()` and `orthogonal()`

The edge vector $\vec{e}$ is the vector that lies on the side of a polygon. The image below shows this.


In the image, the vertices are also vectors, with tails on $(0, 0)$. The edges are vectors with the tail on one vertex and head on the next. In this particular case, $\vec{e_1}$ is

$\vec{v_2} - \vec{v_1} = (3 - 4, 3 - 1) = (-1, 2)$.

For an $N$-sided polygon, $\vec{e_i}$ is

$\vec{v_j} - \vec{v_i}$, where $j = (i + 1) \mod N$

In the image, the orthogonal of $\vec{e_1}$ is $\vec{o_1}$, which has coordinates $(-\vec{e_1}.y, \vec{e_1}.x)$. You might recognize this as the 90º clockwise rotation of $\vec{e_1}$. This particular formula generalizes trivially to $\vec{o_i} = (-\vec{e_i}.y, \vec{e_i}.x)$.

With that, we have our two functions:

def edges_of(vertices):
    """
    Return the vectors for the edges of the polygon p.

    p is a polygon.
    """
    edges = []
    N = len(vertices)

    for i in range(N):
        edge = vertices[(i + 1)%N] - vertices[i]
        edges.append(edge)

    return edges

def orthogonal(v):
    """
    Return a 90 degree clockwise rotation of the vector v.
    """
    return np.array([-v[1], v[0]])

### Finding if an orthogonal vector's axis is a separating axis and calculating the MPV: `is_separating_axis()`

Remember, to be a separating axis the projections of the vertices onto the othogonal vector $\vec{o}$ mustn't overlap. To find the projections of each polygon onto the orthogonal vector's axis, we have to find the maximum and the minimum projection onto that axis among all vertices. The image below shows that.


Say the points in a polygon $P$ are a set of points $\vec{p_i}$. We define $min_P$ as the length of the smallest projection onto $\vec{o}$ among $P$'s vertices. We use the dot product for that. In math terms:

$min_P = \min_i (\vec{p_i} \cdot \vec{o}) = p_i \, o \, \cos(\angle(\vec{p_i}, \vec{o}))$

We do the same for $max_P$.

You may have noticed that $min_P$ and $max_P$ are not precisely the lengths of the the projections because we're not dotting $\vec{p_i}$ with the normalized version of $\vec{o}$. But since our objective at this point is to _compare_ those lengths, it's ok if they're all multiplied by a factor of $o$.

So, when do two projections overlap? When the intervals defined by them overlap. When does that happen? When both intervals share a constant. So that means $min_A \leq c \leq max_A$ and $min_B \leq c \leq max_B$. Simplifying that, we get $min_A \leq max_B$ and $min_B \leq max_A$. The image below might help to picture all the cases. Just switch around the names of the points and imagine the resulting projections.


All right, now, if they _do_ overlap, we must find the push vector. It's easy to see in the figure below that the smallest push vector for that axis is in the direction of the axis, and it's length is given by $max_A$ - $min_B$.


In that figure the $max_P$'s and $min_P$'s are of the right size, but remember, our $max_B$ and $min_A$ are multiplied by a factor of $o$. So, in general, the length of the push vector is

$PV = \dfrac{\min(max_B - min_A, max_A - min_B)}{o} = \dfrac{d}{o}$

The direction is the same as $\vec{o}$'s, which we'll call $\hat{o}$. Hence, the smallest push vector for this axis is

$\vec{PV} = \dfrac{d}{o}\hat{o} = \dfrac{d}{o} \dfrac{\vec{o}}{o} = \dfrac{d}{o^2}\vec{o}$

With that in mind, we have the code:

def is_separating_axis(o, p1, p2):
    """
    Return True and the push vector if o is a separating axis of p1 and p2.
    Otherwise, return False and None.
    """
    min1, max1 = float('+inf'), float('-inf')
    min2, max2 = float('+inf'), float('-inf')

    for v in p1:
        projection = np.dot(v, o)

        min1 = min(min1, projection)
        max1 = max(max1, projection)

    for v in p2:
        projection = np.dot(v, o)

        min2 = min(min2, projection)
        max2 = max(max2, projection)

    if max1 >= min2 and max2 >= min1:
        d = min(max2 - min1, max1 - min2)
        # push a bit more than needed so the shapes do not overlap in future
        # tests due to float precision
        d_over_o_squared = d/np.dot(o, o) + 1e-10
        pv = d_over_o_squared*o
        return False, pv
    else:
        return True, None