Implementation of SAT (Separating axis theorem)

Question

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

Gareth Rees · Answer 1 · 2015-04-28 09:30:51Z

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):
    def __add__(self, other):
        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

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

asked	2 years ago
viewed	498 times
active	1 year ago

current community

your communities

more stack exchange communities

Implementation of SAT (Separating axis theorem)

1 Answer 1

1. Vectors

2. Projection

3. Performance

Your Answer

Not the answer you're looking for? Browse other questions tagged python performance python-3.x computational-geometry collision or ask your own question.

Hot Network Questions

current community

your communities

more stack exchange communities

Implementation of SAT (Separating axis theorem)

1 Answer 1

1. Vectors

2. Projection

3. Performance

Your Answer

Sign up or log in

Post as a guest

Not the answer you're looking for? Browse other questions tagged python performance python-3.x computational-geometry collision or ask your own question.

Related

Hot Network Questions