# Implementation of SAT (Separating axis theorem)

A project I was working on required the usage of the Separating Axis Theorem to detect collisions between two convex polygons in real time. So I implemented a basic class (ConvexFrame in the code) to keep track of the information about the polygon that is necessary for the SAT algorithm. I implemented it, and it works, but it is unfortunately quite slow for the project I'm working on (which is a game by the way).

For example, testing for a collision between an 8-sided shape and a 3-sided shape takes on average 0.00015 seconds of computation time, which means that in a normal game tick (1/60s = 0.016...s) I can calculate a maximum of 100 collisions between convex polygons. I need it to be faster than this, and I'm not sure how I can optimize it. Can someone help me understand where I can optimize the code?

The code is split up into 2 main files: geometry.py (imported as geo in the other file), and physical.py. geometry.py contains basic functions for vector calculations and the likes, where physical.py contains the SAT algorithm and the ConvexFrame class. I made sure that most of the functions in the geometry file were as optimized as I could get them to be, so that shouldn't be the problem, but just incase I included the average runtime of each of the functions in geo.

geometry.py:

import math
import maths # maths is an even simpler file containing constants and other basic functions
# there is no need to include it here.

def centroid(*points):
"""Calculate the centroid from a set of points."""
# Average time for 4 points: 1.4572602962591971e-06s
x, y = zip(*points)
_len = len(x)
return [sum(x)/_len, sum(y)/_len]

def icentroid(*points):
"""Faster than normal centroid, but returns an iterator.

Since this returns an iterator, to separate it up into an
(x, y) tuple, simply say:

>>> x, y = icentroid(*points)
"""
# Average time for 4 points: 9.622882809023352e-07s
_len = len(points)
return map(lambda coords: sum(coords)/_len,
zip(*points))

def to_the_left(v1, v2, v3):
"""Check if v3 is to the left of the line between v1 and v2."""
# Average time: 3.958449703405762e-07s
vx, vy = v3
x1, y1 = v1
x2, y2 = v2
# Calculate the cross-product...
res = (x2 - x1)*(vy - y2) - (y2 - y1)*(vx - x2)
return res > 0

def rotate_vector(v, angle, anchor):
"""Rotate a vector v by the given angle, relative to the anchor point."""
# Average time: 1.5980422712460723e-06s
x, y = v

x = x - anchor[0]
y = y - anchor[1]
# Here is a compiler optimization; inplace operators are slower than
# non-inplace operators like above. This function gets used a lot, so
# performance is critical.

cos_theta = math.cos(angle)
sin_theta = math.sin(angle)

nx = x*cos_theta - y*sin_theta
ny = x*sin_theta + y*cos_theta

nx = nx + anchor[0]
ny = ny + anchor[1]
return [nx, ny]

def distance(v1, v2):
"""Calculate the distance between two points."""
# Average time: 5.752867448971415e-07s
x1, y1 = v1
x2, y2 = v2
deltax = x2 - x1
deltay = y2 - y1
return math.sqrt(deltax * deltax + deltay * deltay)

def distance_sqrd(v1, v2):
"""Calculate the squared distance between two points."""
# Average time: 3.5745887637150984e-07s
x1, y1 = v1
x2, y2 = v2
deltax = x2 - x1
deltay = y2 - y1
return deltax * deltax + deltay * deltay

def project(vector, start, end):
"""Project a vector onto a line defined by a start and an end point."""
# Average time: 1.1918602889221005e-06s
vx, vy = vector
x1, y1 = start
x2, y2 = end

if x1 == x2:
return x1, vy

deltax = x2 - x1
deltay = y2 - y1

m1 = deltay/deltax
m2 = -deltax/deltay

b1 = y1 - m1*x1
b2 = vy - m2*vx

px = (b2 - b1)/(m1 - m2)
py = m2*px + b2

return px, py

def normalize(vector):
"""Normalize a given vector."""
# Average time: 9.633639630529273e-07s
x, y = vector
magnitude = 1/math.sqrt(x*x + y*y)
return magnitude*x, magnitude*y

def perpendicular(vector):
"""Return the perpendicular vector."""
# Average time: 2.1031882874416398e-07s
x, y = vector
return y, -x

def dot_product(v1, v2):
"""Calculate the dot product of two vectors."""
# Average time: 2.617608074634745e-07s
x1, y1 = v1
x2, y2 = v2
return x1*x2 + y1*y2


physical.py:

import geometry as geo
import operator

class ConvexFrame(object):

def __init__(self, *coordinates, origin=None):
self.__original_coords = coordinates
self._origin = origin

self._offsets = []

if not self._origin:
self._origin = geo.centroid(*coordinates)
orx, ory = self._origin
append_to_offsets = self._offsets.append
for vertex in coordinates:
x, y = vertex
offx = x - orx
offy = y - ory
append_to_offsets([offx, offy])

offsets = self._offsets
left = geo.to_the_left
n = len(offsets)
self.__len = n
for i in range(n):
v0 = offsets[i-1]
v1 = offsets[i]
v2 = offsets[(i+1)%n]
if not left(v0, v1, v2):
raise ValueError()

def bounding_box(self, offset=False):
offs = self._offsets
_max, _min = max, min
maxx, maxy = _max(a[0] for a in offs), _max(a[1] for a in offs)
minx, miny = _min(a[0] for a in offs), _min(a[1] for a in offs)
# As far as I can tell, there seems to be no other
# way around calling max and min twice each.
w = maxx - minx
h = maxy - miny
if not offset:
orx, ory = self._origin
return minx + orx, miny + ory, w, h
return minx, miny, w, h

def get_xoffset(self, index):
"""Retrieve the x-offset at the given index."""
return self._offsets[index][0]
def get_yoffset(self, index):
"""Retrieve the y-offset at the given index."""
return self._offsets[index][1]
def get_offset(self, index):
"""Retrieve the offset at the given index."""
return self._offsets[index]

def get_xcoord(self, index):
"""Return the x-coordinate at the given index."""
return self._offsets[index][0] + self._origin[0]
def get_ycoord(self, index):
"""Retrieve the y-coordinate at the given index."""
return self._offsets[index][1] + self._origin[1]
def get_coord(self, index):
"""Retrieve the coordinate at the given index."""
orx, ory = self._origin
x, y = self._offsets[index][0]
return x + orx, y + ory

def translate(self, x, y=None):
if y is None:
x, y = x
origin = self._origin
nx = origin[0] + x
ny = origin[1] + y
self._origin = (nx, ny)

def rotate(self, angle, anchor=(0, 0)):
# Avg runtime for 4 vertices: 6.96e-06s
orx, ory = self._origin
x, y = anchor
if x or y:
# Default values of x and y (0, 0) indicate
# for the method to use the frame origin as
# the anchor.
x = x - orx
y = y - ory
anchor = x, y
_rot = geo.rotate_vector
self._offsets = [_rot(v, angle, anchor) for v in self._offsets]

def collide(self, other):
edges = self._edges + other._edges
# The edges to test against for an axis of separation
_norm = geo.normalize
_perp = geo.perpendicular
# I store all the functions I need in local variables so
# python doesn't have to keep re-evaluating their positions
# in the for loop.

self_coords = self.coordinates
other_coords = other.coordinates

project_self = self._project
project_other = other._project

projections = [] # A list of projections in case there is a collision.
# We can use the projections to find the minimum translation vector.
append_projection = projections.append

for edge in edges:
edge = _norm(edge)
# Calculate the axis to project the shapes onto
axis = _perp(edge)

# Project the shapes onto the axis
self_projection = project_self(axis, self_coords)
other_projection = project_other(axis, other_coords)

if not (self_projection[1] > other_projection[0] and \
self_projection[0] < other_projection[1]     ): # Intersection test
# Break early if an axis has been found.
return False
overlap = self_projection[1] - other_projection[0]
append_projection((
axis[0] * overlap,
axis[1] * overlap
)) # Append the projection to the list of projections if it occurs
return projections

def _project(self, axis, coords):
_dot = geo.dot_product
projections = [_dot(v, axis) for v in coords]
return min(projections), max(projections)

@property
def _edges(self):
"""Helper property for SAT (separating axis theorem) implementation."""
edges = []
n = self.__len
offsets = self._offsets
for i in range(n):
x0, y0 = offsets[i-1]
x1, y1 = offsets[i]
edges.append((x0 - x1, y0 - y1))
return edges

@property
def coordinates(self):
coords = []
offsets = self._offsets
orx, ory = self._origin
for v in offsets:
vx, vy = v
coords.append([vx + orx, vy + ory])
return coords

@property
def offsets(self):
return self._offsets

@property
def origin(self):
return self._origin


The amount of vertices in each polygon is usually no greater than 10, meaning anything involving NumPy would actually be slower than the pure Python implementation.

I just found an optimization: I call self.project and other.project for each time in the loop, and in ConvexFrame.project I call ConvexFrame.coordinates, however the coordinates never change in the loop, thus I am wasting time recalculating the coordinates for each iteration of the loop. Fixing the code by making ConvexFrame.project take a coordinates parameter and sending in the precalculated coordinates shaves down the time to 8.776420134906875e-05s (for the same setup as in the first paragraph, an octagon vs a triangle), but I'd still like to see if it can get any faster.

• Rather than look for ways to optimise, I would recommend profiling your code, for example with cProfile, to see clearly which part of your code is taking up the most time. This will save you spending time optimising parts which will not make much difference. I can't take this approach myself as you have not included the maths module, which is not part of the standard library. – trichoplax Apr 14 '14 at 15:07
• You could add a few annotations and use Cython. I'd imagine that would speed your code up quite a bit. – icktoofay Jun 8 '14 at 5:38
• What you could do is precompute some math value or use some caching techniques to avoid compute several times the same value for cos, sin, sqrt. – samuelsov Oct 17 '14 at 16:53
• check this: wiki.python.org/moin/PythonSpeed/PerformanceTips Try every bit of trick in there and you are sure to speed up your code. But as @trichoplax said, first you need to profile your code, find the bottlenecks and work there. Do not guess where the code is having a hard time, people are not good at guessing that kind of stuff. – f.rodrigues Jan 24 '15 at 5:25

### 1. Vectors

Much of this code is awkward and long-winded because points and vectors are represented by plain Python tuples. A simple operation like subtracting two points requires disassembling the points into their elements, subtracting the elements, and then reassembling the result. If the code represented points using some kind of vector data structure, then a lot of it could be simplified.

For example, here are eight lines of code for computing the offset of each vertex from the origin:

self._offsets = []
orx, ory = self._origin
append_to_offsets = self._offsets.append
for vertex in coordinates:
x, y = vertex
offx = x - orx
offy = y - ory
append_to_offsets([offx, offy])


but with a class of vectors that supported subtraction, this would be one line:

self._offsets = [v - self._origin for v in coordinates]


If you have NumPy to hand, then it would make sense to use NumPy arrays as your vectors, but if not then it's easy to write such a class. For example, you might start with something simple like this:

class Vector(tuple):
return Vector(v + w for v, w in zip(self, other))

def __sub__(self, other):
return Vector(v - w for v, w in zip(self, other))

def __mul__(self, s):
return Vector(v * s for v in self)

def __abs__(self):
return sqrt(sum(v * v for v in self))

def dot(self, other):
"""Return the dot product with the other vector."""
return sum(v * w for v, w in zip(self, other))


(See my vector.py for a full-featured implementation.)

With this class, many of your geometry functions could be simplified. For example, to calculate the distance from v1 to v2 you currently have:

x1, y1 = v1
x2, y2 = v2
deltax = x2 - x1
deltay = y2 - y1
return math.sqrt(deltax * deltax + deltay * deltay)


but using the Vector class given above, this becomes so trivial that it might not be worth defining a function for it:

return abs(v1 - v2)


This approach won't speed up your code (the same operations are being carried out) but it will make it shorter, clearer, and easier to work with, and that will help you when you do come to make performance improvements.

### 2. Projection

The algorithm in project has a bug: there's a division by zero error if start and end have the same y-coordinate:

>>> project((1, 2), (0, 0), (2, 0)) # expecting (1, 0)
Traceback (most recent call last):
m2 = -deltax/deltay
ZeroDivisionError: division by zero


In computer geometry you should always use vectors if possible: trying to work with the slope-and-intercept representation of lines leads into difficulty because of the exceptional cases.

The reliable way to project v onto the line from start to end is to use the dot product, like this:

w = end - start
return w * (w.dot(v) / w.dot(w))


This still has the possibility of division by zero, but only in the case where w is zero (that is, if start == end) and in that case no projection is possible.

### 3. Performance

1. Pure Python is always going to be a bit slow for this kind of problem, so you should look into using NumPy to speed up the calculations in collide. In this function you carry out a computation for each edge of each figure: this would be easy to vectorize so that all computations are carried out at once.

2. Because the collision test is so expensive, it's worth taking some trouble to avoid it. In particular, it would be worth storing a bounding circle with origin $o$ and radius $r$ for each polygon: namely, the smallest circle that contains all points in the polygon. Then, before doing the full polygon/polygon collide test, do a circle/circle test: if two polygons have bounding circles $(o_1, r_1)$ and $(o_2, r_2)$ then they can only collide if $\left| o_1 - o_ 2 \right| ≤ r_1 + r_2$. This should allow you to reject most collisions cheaply.

3. Consider storing your polygons in a space-partitioning data structure such as a quadtree that would let you efficiently find pairs of polygons that might collide. SciPy has an implementation in scipy.spatial.KDTree.

4. It's likely that you are going to be repeatedly testing the same set of polygons for collision (for example, in a video game you'd be doing this each frame). In that case, when you find that two polygons are separated by a particular axis, remember that axis and test it first next time. The insight is that a pair of polygons don't move very far in one time step, and so an axis that separates them at time $t$ will continue to separate them at time $t + δ$. This is the method of "caching witnesses" — see Rabbitz, "Fast Collision Detection of Moving Convex Polyhedra" in Graphics Gems IV.