%matplotlib inline

import numpy as np
import matplotlib.pyplot as plt
import math
import time

Want to know how to do a full-blown physics simulation of rigid bodies ⁰¹ [ Bodies that does not deform on contact. A completely rigid body doesn't exist in real life since all bodies break under high enough pressure. For the purpose of simulation we assume they are unbreakable. ] and collisions between them? In this in-depth tutorial I share the knowledge I gained from the time I wondered the same...

When simulating physics we show graphically the interaction between objects, trying to model real-life phenomena - mass, acceleration/velocity, forces, fluids, heat, soft/rigid bodies, etc. This is very useful and has enormous practical applications for engineering, physics, chemistry and of course for games and entertainment!

Here we'll explore how you can take equations of motion from physics and apply them to bodies, the math behind checking for intersecting objects, different types of collision handling. Are you up for the challenge?

I'll assume you have basic knowledge of Classical Mechanics though - forces, Newton's laws, torque.. so I won't take time to fully explain every detail. If you are struggling with the formulas, though, I suggest quickly looking through some materials, e.g. Khan Academy has a good course on Physics! :)

Visualization ^↑

• At first I tried making a 60 fps interactive application in Jupyter notebook, but that, as intuition should've told me, didn't go well. I decided to use a graphics library I've been working on and just interface C++ with Python. So the resulting physics code is entirely written in Python while the graphics is sent to C++ and rendered with DirectX.

• The demos which you can run (including the source code for the entire physics engine) is available in the zip.

In case you are not on Windows, I'm very sorry I can't provide a port to you, but you can see the final result in the video below and follow along with a graphics library of your choice (PyGame is a good option).

When you extract the .zip. All of the engine's source code is available in data/scripts. In the scripts you'll see that we are using a custom lstdgraphics Python module that is built from the C++ side, and which enables us to call drawing functions:

import lstdgraphics as g
def frame():
    g.line(a, b, color = ..., thickness = ...)

I won't explain the Python to C++ and DirectX interfacing part here, just because I want the focus here to be on the physics and math. It would be much better suited for a separate article.

Each demo has a main file which gets loaded by the engine. The engine searches for file names that begin with "demo_", like this: data/scripts/demo_grabbing.py, data/scripts/demo_normals.py, ...

Open run_demos.exe.

Table of Contents

• Visualization
• Shapes
• Bodies
  • Model Matrix
  • Mass
  • Centroid
  • Rotational Inertia
• Forces and Impulses
  • Using Forces in a Demo
    • Point vs. Shape Intersection
    • Grabbing Bodies with the Mouse
    • Air Drag
• Discrete Collision Detection
  • Intersection
    • Circle vs. Circle
    • Polygon vs. Polygon
    • Polygon vs. Circle
  • Response
    • Collision Contact Points
      • Failed (?) first try
      • The better method
      • Quick word about circles
    • Impulse Based Collision Resolution
    • Iterations
    • Friction
  • Continuous Collision Detection
• Practical Applications
• Further Reading
• Some References Which Made this Possible

Shapes ^↑

The first essential thing we need to look at is shapes. Shapes represent the volume which an object occupies (2D or 3D) and can be really simple (circles) or really complex (many polygons linked together).

The vertices that are part of the shape of our body are in local space ⁰² [ Each individual body has it's own virtual origin (0, 0) in which vertices are placed relative to. This is called local space. Moving/rotating the object doesn't affect it. ] . We imagine the object's global ⁰³ [ The space in which all bodies get placed in when drawing is called global space (or world space). ] position to be the same as the origin of our local space and so vertices become "offsets" from that. This allows us to use 1 shape for many objects and those objects can have different positions and rotations.

We define the shape's space origin to be at (0,0), although it's geometric center (centroid) may not lie on the origin (we later see how we can correct that).

We define two kinds of shapes in the engine: circles and convex polygons. Convex means that there are no angles bigger than 180 inside of the polygon - this assumption is very common in physics engines, because the calculations we do rely on that being the case ⁰⁴ [ Note that as a workaround, a concave shape can be represented with a bunch of convex polygons. ] .

For polygons we cache the edges and also their normal vectors ⁰⁵ [ A normal/orthogonal/perpendicular vector is a vector which forms a 90 degree angle (clockwise or counter-clockwise according to convention) with another vector. ] . These are often used, so pre-calculating them makes sense. The convention we use is that the polygon's vertices are ordered counter-clockwise. The edges and normals arrays contain the edge/normal between vertex i and vertex i + 1 in the ith slot.

def edges(vertices):
    """
    This is a helper function which returns an array of edges from given vertices.
    e.g: [[0, 1], [1, 1], [1, 0]] 
         -> 
         [[[0, 1], [1, 1]],
          [[1, 1], [1, 0]],
          [[1, 0], [0, 1]]]
    """
    # First we make an array with the required length and shape
    es = np.empty((len(vertices), 2, 2))
    # We save the first vertex and start iterating from 1
    a = vertices[0]
    for i in range(1, len(vertices) + 1):
        # At the end this module operation cycles us back to the first vertex
        b = vertices[i % len(vertices)]
        # An edge is defined by it's two vertices
        es[i - 1] = np.array([a, b])
        # Update the previous vertex
        a = b
    return es

Let's discuss how we calculate the normals for the edges. If we have a vector u = [x, y], we can rotate it 90 degrees in two ways. Clockwise or counter-clockwise. These correspond to the vectors [y, -x] and [-y, x]. To see why this is true let's plot them:

def plot_vectors(starts, ends, colors):
    vectors = []
    for i, s in enumerate(starts):
        e = np.array(ends[i])
        # Transform end coords into an offset because that's what quiver() expects..
        offset = e - s
        vectors.append([s[0], s[1], offset[0], offset[1]])
    soa = np.array(vectors)
    x, y, u, v = zip(*soa)
    plt.quiver(x, y, u, v, angles = "xy", scale_units = "xy", scale = 1, color = colors)
def plot_vector(start, end, color):
    plot_vectors([start], [end], colors = [color])
def plot_add_grid(ticks = range(-3, 4)):
    plt.axis("scaled")
    plt.xticks(ticks)
    plt.yticks(ticks)
    plt.grid()

x, y = 1.5, 2.5
plot_vectors([[0, 0], [0, 0], [0, 0]], [[x, y], [y, -x], [-y, x]], colors = ["r", "g", "b"])
plot_add_grid()
plt.show()

Since the vertices are ordered in counter-clockwise fashion, for each edge we choose the clockwise ([y, -x]) vector (it points outwards from the shape) and normalize it ⁰⁶ [ Normalizing a vector just means dividing by its length. The resulting vector has length exactly 1 ($x^2 + y^2 = 1$). ] . Here is the algorithm:

for i, e in enumerate(self.edges):
    self.normals[i] = normalized(orthogonal(e[1] - e[0]))

def magnitude(v):
    """
    Returns the length of vector v.
    """
    return np.sqrt(np.dot(v, v))
def normalized(v):
    """
    Normalizes the input - returns a vector with length 1, keeps the direction.
    """
    return v / magnitude(v)
def orthogonal(v):
    """
    Returns the clockwise perpendicular vector.
    """
    return np.array([v[1], -v[0]])

Bodies ^↑

Shapes were the first part in describing the bodies in our engine! However each body needs to have certain other properties. First of all, each body has a position and rotation in space.

This is a good place to mention that, although everything we do is in 2D, it's not much more different in 3D. (Rotation is one thing that gets very complicated in 3D though). The important thing here is to understand the basics principles of a physics simulation which are the same in 2D, 3D or whatever-D.

For the sake of simplicity when creating a body we specify it's shape and uniform density. Bodies with different density in different areas exist in real life but in our simulation we assume it is is evenly spread out ⁰⁷ [ I hope you can see how complicated you can create your engine: you can add support for mapping a non-uniform density, or you can create joints between bodies with different densities to create a large body that approximates your real-life example. However, our aim is not to create the perfect most general physics engine (because that can easily require an ungodly amount of work). ] .

Model Matrix ^↑

When drawing our bodies they shouldn't appear at the same place. When we talked about shapes we said that our bodies have position and rotation, and the shape's vertices get transformed. How do we do that?

Well probably the most convenient way is by using matrices. For 2D linear transformations we need 2x2 matrices. We can't describe translation in a 2x2 matrix (translation is not a linear transformation because it moves the origin).

But we can encode it by using a third column:

$$ T = \begin{bmatrix} 1 & 0 & v_x \\ 0 & 1 & v_y \\ 0 & 0 & 1 \\ \end{bmatrix} $$

When multiplying by our 2D coordinates for the vertex we add a third component which is equal to one. For example if we want to multiply the vertex position $(1.4, 8)$ by our matrix, we represent it like: $(1.4, 8, 1)$ and do the dot product:

$$ P = \begin{bmatrix} 1.4 \\ 8 \\ 1 \\ \end{bmatrix} \begin{bmatrix} 1 & 0 & v_x \\ 0 & 1 & v_y \\ 0 & 0 & 1 \\ \end{bmatrix} $$

A rotation matrix for an angle $\theta$ is represented like this: $$ R = \begin{bmatrix} \cos(\theta) & -\sin(\theta) & 0 \\ \sin(\theta) & \cos(\theta) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} $$

If you want to see how to derive this, there are great videos/articles on the internet.

So our final transformation for every body is just $M = TR$. We first rotate and then we translate (remember that in computer graphics we multiply matrices right to left). A good rule of thumb is that to get the proper final transformation we SRT - scale, rotate, translate, because we usually want scaling to happen along the axis of the object and rotation about the center of the object. In our engine we don't support scaling for the sake of simplicity.

Mass ^↑

We have a shape and density. How can we calculate the mass of our object? Well, we need to find its area (volume in 3D).

A circle's area is really simple to find - it's just $ \pi r^2 $.

For polygons we're going to use the Shoelace formula. To explain the formula let's write a quick method to plot polygons in a coordinate plane.

def plot_polygon(vertices, color = None):
    vertices = np.append(vertices, vertices[0]).reshape((len(vertices) + 1, 2))
    plt.plot(vertices[:, 0], vertices[:, 1], color = color)

vertices_house = np.array([
    [3, 1],
    [3, 3],
    [2, 4],
    [1, 3],
    [1, 1]
])
plot_add_grid(range(-1, 6))
plot_polygon(vertices_house, color = "r")
plt.scatter(0, 0, color = "g")
plt.show()

The green dot is the origin.

The algorithm of the Shoelace formula is as follows: for every edge, take a look at the triangle formed by it and the origin. To find the area of that triangle we use the cross product of the vertices (we imagine the vertices as vectors from the origin). The cross product gives us the area of a parallelogram which we need to divide by two. We sum up all the areas of all these triangles and in the end we take the absolute value.

The negative cross products cancels out with the overlapping area and gives us just the area of the polygon. It works like magic.

plot_add_grid(range(-1, 6))
plot_polygon(vertices_house, color = "r")
plt.scatter(0, 0, color = "g")
for e in edges(vertices_house):
    plot_polygon([e[0], e[1], [0, 0]], color = "b")
plt.show()

Notice that every time the origin and the two vertices are ordered counter-clockwise - the cross product will be positive. And every time they are ordered clockwise - negative.

def shoelace(vertices):
    area = 0
    for e in edges(vertices):
        area += 0.5 * np.cross(e[0], e[1])
    return area
shoelace(vertices_house)

5.0

To test if we've implemented the algorithm correctly, let's consider a simple triangle:

vertices_triangle = np.array([
    [4, 1],
    [2, 4],
    [1, 1]
])
plot_add_grid(range(-1, 6))
plot_polygon(vertices_triangle, color = "r")
shoelace(vertices_triangle)

4.5

The correct answer is indeed $4.5$ (base is $3$ and height is $3$, so $ A = \frac{3 \cdot 3}{2} = 4.5 $).

Now that we have methods of finding the area of our shapes, the mass of a body is simply $m = A\rho$ (area times density).

Centroid ^↑

When we define a shape, each vertex's coordinates are relative to the origin of the local space of the body. It's going to be really nice for calculations if that origin is also the centroid ⁰⁸ [ center of mass ] of the shape!

To do that we need to find the coordinates of the centroid and shift every vertex so the centroid of the shape essentially overlaps with the origin.

For example, here is a pentagon which has a different center of mass than the origin. The blue figure is the result after we translate each vertex by the centroid. The green dot is now both the origin of the local space and the center of mass of the shape.

plot_add_grid(range(-3, 4))
plt.scatter(0, 0, color = "g") # Origin
vertices_5gon = np.array([
    [2, -1],
    [2.2, 1],
    [1.5, 2],
    [-1.2, 1],
    [-1, -1]
])
plot_polygon(vertices_5gon, color = "r")
centroid_5gon = [0.56995885, 0.2962963] # We know this in advance, we will see how to calculate it later!
plot_polygon(vertices_5gon - centroid_5gon, color = "b")
plt.show()

Finding the center of mass of a circle is simple enough. We define a circle with only its radius around the origin, so the centroid is just $(0,0)$.

The centroid of a triangle ABC (its barycenter) is the average of each point's coordinates: $$ G = (\frac{x_a + x_b + x_c}{3}; \frac{y_a + y_b + y_c}{3})$$

You can see this is true if you add the vectors from the vertices to G and see that their sum is zero: $ \vec{GA} + \vec{GB} + \vec{GC} = 0 $

Regular polygons also have this property of getting the center by just taking the average of the coordinates, however that's not true for any convex polygon. The mass of a polygon is uniformly distributed over its entire surface, not only at the vertices. Note that for simple shapes, such as triangles or regular polygons, the mass being evenly distributed over the surface is equivalent to the mass being at the vertices only.

When calculating the centroid for a polygon we need to triangulate ⁰⁹ [ divide a polygon up into smaller triangles ] it and take the weighted average of the triangle's centroids. As visualized in the figure below, the area of the triangle measures how much of the entire mass of the polygon is contained in the triangle, thus by how much the barycenter of that triangle influences the location of the centroid of the polygon.

plot_add_grid(range(-3, 4))
plt.scatter(0, 0, color = "g") # Origin
plot_polygon(vertices_5gon, color = "r")
for e in edges(vertices_5gon):
    plot_polygon([e[0], e[1], [0, 0]], color = "b")
    # The average of the x and y coordinates (the third vertex is the origin - 0,0)
    barycenter = (e[0] + e[1]) / 3
    plt.scatter(barycenter[0], barycenter[1], color = "g") # Each triangle's barycenter
plt.scatter(centroid_5gon[0], centroid_5gon[1])
plt.show()

def calculate_centroid(vertices):
    centroid = np.array([0.0, 0.0])
    # While going around calculating the cross product, we get the area for free, so we don't need to calculate it at the end
    area_sum = 0.0
    for e in edges(vertices):
        tri_area = 0.5 * np.cross(e[0], e[1]) # The area of each triangle
        area_sum += tri_area
        barycenter = (e[0] + e[1]) / 3
        centroid += barycenter * tri_area # Weighted barycenter
    # Divide by the area in the end (see below why this is equivalent to using the ratio of the areas)
    return centroid / area_sum
calculate_centroid(vertices_5gon)

array([0.56995885, 0.2962963 ])

We got the right answer (which we used it earlier - centroid3). To verify our function works let's plot the centroid of another polygon:

vertices_6gon = np.array([
    [3, 1],
    [3, 2],
    [2.2, 3],
    [1.5, 3],
    [0.8, 2],
    [1, 1]
]) + 2.5
centroid_6gon = calculate_centroid(vertices_6gon)
plot_add_grid(range(-1, 8))
plot_polygon(vertices_6gon, color = "r")
plt.scatter(centroid_6gon[0], centroid_6gon[1], color = "g")
plt.show()

The code is essentially evaluating this expression: $$ G = \frac{A_1}{A}G_1 + \frac{A_2}{A}G_2 + \dots + \frac{A_n}{A}G_n = \frac{G_1A_1 + G_2A_2 + \dots + G_nA_n}{A} = \frac{G_1A_1 + G_2A_2 + \dots + G_nA_n}{A} $$ where $$ A = A_1 + A_2 + \dots + A_n $$

and $A_n$ are the areas of the smaller triangles inside the polygon and $G_n$ are their barycenters. In the end we divide by the total area of the polygon (which equals the sum of the smaller triangles).

Rotational Inertia ^↑

Rotational intertia is analogous to mass in that it is a measure of the resistance a body offers to torque or rotational motion. Like mass, inertia is additive and the inertia of any shape can be calculated by summing the inertia of smaller shapes that form it.

The inertia of a circle about a perpendicular axis passing through its center is $\frac{1}{2}mr^2$

The inertia of an arbitrary convex polygon does not have a fixed formula, but like mass we can compute the inertia by subdividing the polygon into triangles with a vertex at the origin and finding their inertia.

The inertia of a triangle with a vertex at the origin and side vectors $a$ and $b$ about a perpendicular axis passing through the origin is given by: $$\frac{1}{6}m(|\vec{a}|^2 + |\vec{a}|^2 + A \cdot B) $$ The derivation of this formula is non-trivial and it will not be included here for the sake of brevity :D.

The rotational inertia of a polygon can then be computed as follows:

rot_inertia = 0
density = 1
# The dot product of a vector with itself gives us it's squared length!
# Proof for 2D vector:
#     a . b = ax * bx + ay * by
#       If a = b:
#     a . a = ax * ax + ay * ay = ax^2 + ay^2
def sqr_magnitude(v):
    return np.dot(v, v)
for e in edges(vertices_6gon):
    a, b = e[0], e[1]
    tri_mass = density * 0.5 * np.abs(np.cross(a, b))
    # We just apply the formula, nothing interesting here :(
    tri_inertia = tri_mass * (sqr_magnitude(a) + sqr_magnitude(b) + np.dot(a, b)) / 6
    rot_inertia += tri_inertia
rot_inertia

315.3639166666666

We also have a special case for static (immovable) objects. Static objects don't exist in real life but in the simulation they help us build rooms with floors and ceiling that don't get affected by other bodies or gravity. We think of them as objects with infinite mass (although we represent their mass as 0 - because it works out with the formulas. When we apply an impulse for example we take into account the mass of the object and impulse * 0 = 0).

Putting all the data together looks something like this:

class Shape:
    def __init__(self, color = 0xfff00ff):
        self.color = color
class Circle(Shape):
    def __init__(self, radius, color = 0xffff00ff):
        self.radius = float(radius)
        self.area = radius ** 2 * np.pi
        self.color = color
class ConvexPolygon(Shape):
    def __init__(self, vertices, color = 0xffff00ff, recenter = True):
        self.vertices = np.array(vertices).astype(float)
        self.normals = np.empty((len(vertices), 2)).astype(float)
        self.edges = np.empty((len(vertices), 2, 2)).astype(float)
        es = edges(self.vertices)
        self.area = shoelace(self.vertices)
        self.centroid = calculate_centroid(self.vertices)
        if recenter:
            self.vertices -= self.centroid
        # Recalculate edges after translating the vertices
        self.edges = edges(self.vertices)
        for i, e in enumerate(self.edges):
            self.normals[i] = normalized(orthogonal(e[1] - e[0]))
        self.color = color

class Body:
    def __init__(self, shape, density, static = False):
        self.pos = np.array([0.0, 0.0])
        self.rot = 0.0
        self.vel = np.array([0.0, 0.0])
        self.ang_vel  = 0.0
        self.force = np.array([0.0, 0.0])
        self.torque = 0.0
        self.shape = shape
        self.static = static
        if not static:
            self.mass = density * shape.area
            # We use inverse mass quite often (it just appears in the formulas that way).
            # Some physics engines don't even store raw mass..
            self.inv_mass = 1 / self.mass
            # Here we check the type of the shape
            if isinstance(shape, sh.Circle):
                self.rot_inertia = 0.5 * self.mass * (shape.radius ** 2)
            elif isinstance(shape, sh.ConvexPolygon):
                self.rot_inertia = 0
                for e in shape.edges:
                    a, b = e[0], e[1]
                    tri_mass = density * 0.5 * np.abs(np.cross(a, b))
                    tri_inertia = tri_mass * (sqr_magnitude(a) + sqr_magnitude(b) + np.dot(a, b)) / 6
                    self.rot_inertia += tri_inertia
            self.inv_rot_inertia = 1 / self.rot_inertia
        else:
            self.mass = 0
            self.inv_mass = 0
            self.rot_inertia = 0
            self.inv_rot_inertia = 0

Big note: The above code is not exactly what we use in our demos. The reason for that is performance! For example isinstance is very slow in Python by nature so instead we use an enum for the shape type (makes the code a little bit longer). We also cache a model matrix and a transformed shape with each body so we don't do unnecessary computations during the time step. If you want to look at those things, take a look at the source code!

This class above is not the final definition of our body by all means! We can always add more properties to our objects. For example, restitution - how much do objects bounce. Another useful thing many physics engines do is add a sleep property. If an object is at rest on the ground for example, we set it's sleep to true and skip calculations in our time step. This can lead to huge performance gains depending on the situation. For the sake of simplicity we will not be implementing such a system.

Forces and Impulses ^↑

The main interaction mechanism with a body is going to be through forces (and indirectly torque). All forces are applied per time step. That's how forces in our universe work too! (If we imagine that the time steps in real life are infinetely small). We say a force is continuous as long as it's acting on a object. If we want a continuous force to act on an object in our engine, we apply it every frame until we want it to stop.

The definition of torque is: $ \vec{\tau} = \vec{r} \times \vec{F} $. That means that if a force is applied at the center of mass, the torque generated is zero. If we apply it anywhere else though, we need to find the cross product - where $\vec{r}$ is the vector from the center to the point where the force is applied.

Impulses are large forces applied over very short periods. However, we will consider them as momentum changes and directly modify the linear and angular velocities:

def apply_force(body, force, point = [0.0, 0.0]):
    body.force += force
    body.torque += np.cross(point, force)
def apply_impulse(body, impulse, point = [0.0, 0.0]):
    body.vel += impulse * (1 / body.mass)
    body.ang_vel += np.cross(point, impulse) * (1 / body.rot_inertia)

There is no point in applying forces if we are not going to use them. At the end of each time step we need to do some integration. I assume you know that we think of velocity as a derivative of position. That means that velocity describes the rate of which position changes. Acceleration is the second derivative - it describes the rate at which velocity changes.

Since we are dealing with computers, we need to do numerical integration. We do that by considering very small time intervals for each step. Each frame (if running at 60 fps) takes $\frac{1}{60}$ seconds or a delta time of about $0.016$.

Furthermore, Newton's equation of motion gives a way to relate forces and acceleration. $$\vec{F} = m\vec{a}$$ $$\vec{a} = \frac{\vec{F}}{m}$$

With all of this we arive at this code which applies movement to each body at the end of the frame:

for b in bodies:
    if b.static: continue
    # Here we also apply '- up * gravity' to each body (9.8 m/s^2 by default) 
    acc = b.force / b.mass - np.array([0, 1]) * gravity
    b.vel += acc * dt
    b.pos += b.vel * dt
    # Integrating angular velocity is analogous to linear velocity.
    ang_acc = b.torque / b.rot_inertia
    b.ang_vel += ang_acc * dt
    b.rot += b.ang_vel * dt
    # We also reset the force and torque at the end (as we discussed above).
    b.force = [0.0, 0.0]
    b.torque = 0

Using Forces in a Demo ^↑

Now that we have a way to apply forces. Let's test it with something.. fun? (if you can call it that). We want to make a demo where you can grab objects with your mouse and throw them around.

Getting information about mouse input is handled by C++ and passed to us by functions:

def mouse_click(x, y, rightButton): pass
def mouse_release(x, y, rightButton): pass
def mouse_move(x, y): pass

The coordinates have been automatically converted to world space.

Point vs. Shape Intersection ^↑

Here comes the first challenge. How do we determine whether we clicked a body? We need a way to determine if a point is inside a shape.

plot_add_grid()
c = plt.Circle((0, 0), 2, color = "r", fill = False)
plt.gca().add_artist(c)
plt.scatter(0, 0, color = "g")
points = [[1.5, 2], [1.7, -2], [-0.5, 0.7]]
for p in points:
    plt.scatter(p[0], p[1])
    plt.plot([0, p[0]], [0, p[1]])
plt.show()

The circle case is trivial. Just check if the vector from the center to the point has a magnitude ¹⁰ [ length of a vector ] less than the radius. Here only the green segment has length less than the radius, so it is inside.

def point_in_circle(center, radius, p):
    v = np.array(p).astype(float) - center
    return sqr_magnitude(p) < radius ** 2

A simple optimization is to use the square length and square radius in order to avoid calculating square roots. Here we can see the result:

plot_add_grid()
c = plt.Circle((0, 0), 2, color = "r", fill = False)
plt.gca().add_artist(c)
plt.scatter(0, 0, color = "g")
points = [[1.5, 2], [1.7, -2], [-0.5, 0.7], [2, 0], [-np.sqrt(2) + 0.1, -np.sqrt(2) + 0.1]]
for p in points:
    c = "g" if point_in_circle([0, 0], 2, p) else "r"
    plt.scatter(p[0], p[1], color = c)
    plt.plot([0, p[0]], [0, p[1]], color = c)
plt.show()

This Ray casting algorithm is one really clever algorithm to determine if a point is inside a polygon. You test how many times a ray, starting from the point and going in any fixed direction, intersects the edges of the polygon. If the point is on the outside of the polygon the ray will intersect its edge an even number of times. If the point is on the inside of the polygon then it will intersect the edge an odd number of times.

If even works both for convex and concave polygons!

This works on theory, but in our case - not so much. I had a lot of trouble with edge cases. After implementing it I tested it thoroughly and it didn't work properly when we looked at a point on the same y-level as a vertex. I tried a lot of work-around and different implementations but none of them worked...

We will take a look at the implementation either way just in case you are curious :)

The first step is determining the intersection point of two lines. There are quick and elegant solutions on stackoverflow using np.linalg but you can think about this problem on your own - you don't need fancy linear algebra. First, use the points to get the coefficients of the line equation: $$ y = ax + b $$

$$ a = \frac{\text{rise}}{\text{run}} = \frac{y_2 - y_1}{x_2 - x_1}$$

After we know $a$: $$ b = y - ax $$

Now that we know the coefficients, just set them equal and solve for x:

$$ a_1x + b_1 = a_2x + b_2 $$

$$ a_1x - a_2x = b_2 - b_1 $$$$ x_p = \frac{b_2 - b_1}{a_1 - a_2} $$

And then get the y coordinate by using either equation:

$$ y_p = a_1x_p + b_1 = a_2x_p + b_2 $$

We also need to handle the special case for vertical lines (where $x_2 - x_1$ is $0$).

Let's implement this:

p1, p2 = line1[0], line1[1]
w1, w2 = line2[0], line2[1]
run1 = p2[0] - p1[0]
run2 = w2[0] - w1[0]

Now that we have calculated the runs of the two equations, let's check if either are $0$:

if run1 == 0 and run2 == 0:
    return None # The lines are vertical and parallel

If could also be that just one of the lines is vertical and there is a solution:

if run1 == 0:
    a2 = (w2[1] - w1[1]) / run2
    b2 = w2[1] - a2 * w2[0]
    # y = a2 * x + b2   =>   b2 = y - a2 * x
    # The first equation is x = p1[0]
    # The second equation is y = a2 * x + b2
    # Their intersection is at guaranteed to be at: x = p1[0]
    x = p1[0]
    y = a2 * x + b2
    return [x, y]

(The other case for run2 is analogous.)

a1 = (p2[1] - p1[1]) / run1
a2 = (w2[1] - w1[1]) / run2
if a1 == a2:
    return None # The lines are horizontal and parallel

If both lines have the same $a$ coefficient, they are either on top of each other or not intersecting. In both cases we say they don't have an intersection point.

Finally, we calculate the $b$ coefficients and return the intersection point:

b1 = p2[1] - a1 * p2[0]
b2 = w2[1] - a2 * w2[0]
# Here we have: a1 * x + b1 = a2 * x + b2
x = (b2 - b1) / (a1 - a2)
y = a1 * x + b1
return [x, y]

The entire algorithm:

def line_vs_line(line1, line2):
    """
    Returns the intersection point of two lines.
    A line is represented by two points: [[p1_x, p1_y], [p2_x, p2_y]].
    """
    p1, p2 = line1[0], line1[1]
    w1, w2 = line2[0], line2[1]
    run1 = p2[0] - p1[0]
    run2 = w2[0] - w1[0]
    if run1 == 0 and run2 == 0:
        return None # The lines are vertical and parallel
    if run1 == 0:
        a2 = (w2[1] - w1[1]) / run2
        b2 = w2[1] - a2 * w2[0] # y = a2 * x + b2 => b2 = y - a2 * x
        # The first equation is x = p1[0]
        # The second equation is y = a2 * x + b2
        # Their intersection is at guaranteed to be at: x = p1[0]
        x = p1[0]
        y = a2 * x + b2
        return [x, y]
    if run2 == 0:
        a1 = (p2[1] - p1[1]) / run1
        b1 = p2[1] - a1 * p2[0]
        x = w1[0]
        y = a1 * x + b1
        return [x, y]
    a1 = (p2[1] - p1[1]) / run1
    a2 = (w2[1] - w1[1]) / run2
    if a1 == a2:
        return None # The lines are horizontal and parallel
    b1 = p2[1] - a1 * p2[0]
    b2 = w2[1] - a2 * w2[0]
    # Here we have: a1 * x + b1 = a2 * x + b2
    x = (b2 - b1) / (a1 - a2)
    y = a1 * x + b1
    return [x, y]

Here we try a line vs all extended segments of a polygon:

plot_add_grid()
plot_polygon(vertices_5gon, color = "r")
# The point which we want to test
p = np.array([2.5, 1.3])
plt.scatter(p[0], p[1], color = "b")
plt.axhline(p[1])
for e in edges(vertices_5gon):
    k = line_vs_line([p, p + [1, 0]], e)
    if k is not None:
        plt.scatter(k[0], k[1], color = "orange")
plt.show()

The next steps are checking if the point actually lies on the segment (not the intersection) and if it actually lies on the ray (not its extension). These are really simple - just number comparisons.

def is_between(a, b, c):
    """
    Checks if c is between a and b
    """
    if a > b:
        a, b = b, a
    return a < c < b
def is_point_on_ray_extension(ray, k):
    """
    Checks if the point k lies on the extension of the ray (the part of the line that isn't on top of the ray).
    A ray is represented by a point and a relative direction: [[px, py], [dir_x, dir_y]].
    """
    # Get two points which represent the line that contains the ray
    b, d = ray[0], ray[0] + ray[1]
    return is_between(k[0], d[0], b[0]) or is_between(k[1], d[1], b[1])

def ray_vs_segment(ray, segment):
    """
    Returns the point at which a ray and a segment intersect (or None if they don't).
    This doesn't pay attention to the ends of the segment.
    A ray is represented by a point and a relative direction: [[px, py], [dir_x, dir_y]].
    A segment is represented by two end points: [[p1_x, p1_y], [p2_x, p2_y]].
    """
    # First we get the intersection with the extended line.
    b, d = ray[0], ray[1]
    k = line_vs_line([b, b + d], segment)
    if k is None: return None
    if is_point_on_ray_extension(ray, k): # We disregard the point if it doesn't lie on the ray
        return None
    # Now we check for both ends of the segment
    a, b = segment[0], segment[1]
    min_x = min(a[0], b[0])
    max_x = max(a[0], b[0])
    if np.less(k[0], min_x) or np.isclose(k[0], min_x) or np.greater(k[0], max_x) or np.isclose(k[0], max_x):
        return None
    min_y = min(a[1], b[1])
    max_y = max(a[1], b[1])
    if np.less(k[1], min_y) or np.isclose(k[1], min_y) or np.greater(k[1], max_y) or np.isclose(k[1], max_y):
        return None
    return k

def point_in_polygon_ray_cast(p, vertices):
    """
    Returns whether a point is inside a given convex polygon (p is in local space).
    """
    p = np.array(p).astype(float)
    # We use a ray with a fixed direction (to the right).
    ray = np.array([p, [1, 0]])
    count = 0
    es = edges(vertices)
    for e in es:
        k = ray_vs_segment(ray, e)
        if k is not None:
            count += 1
    # Since ray_vs_segment doesn't pay attention to segment ends, 
    # we also count each time the ray passes through a vertex as an edge crossed.
    # Note that this still isn't perfect as we will see below.
    # I really tried a bunch of solutions before trying the vertices but most of them were even less accurate.
    for v in vertices:
        if np.isclose(p[1], v[1]) and v[0] > p[0]:
            count += 1
    return count % 2 == 1

Now we generate A LOT of points around the polygon and color the ones that pass the test orange and the one that are supposed to be outside - purple:

plot_add_grid()
plot_polygon(vertices_5gon, color = "r")
b = np.array([-2, 2.5])
e = np.array([2.5, -2])
x = b[0]
while x < e[0]:
    y = b[1]
    while y > e[1]:
        p = [x, y]
        in_poly = point_in_polygon_ray_cast(p, vertices_5gon)
        c = "orange" if in_poly else "purple"
        plt.scatter(p[0], p[1], color = c)
        y -= 0.1
    x += 0.1
plt.show()

plot_add_grid()
plot_polygon(vertices_5gon, color = "r")

As you can see, there are some points which are clearly outside but pass the test. Adding cases, rethinking everything, extending the segments just by a little bit - nothing worked and there were still some false positives.

After giving up on this algorithm I came up with my own method. If you rememeber earlier when we calculated the area of a polygon by summing up the cross product of the vectors from the origin to the ends of each edge. We can use that here. That method of finding the area relied that sometimes the area of these triangles was positive and sometimes "negative".

Our vertices are ordered counter-clockwise. So if a point is inside a polygon, going through every edge, the area will always be positive (it will always be negative if the vertices were clockwise).

So our test simply becomes:

def point_in_polygon_cross_product(p, vertices):
    """
    Returns whether a point is inside a given convex polygon (p is in local space).
    """
    p = np.array(p).astype(float)
    es = edges(vertices)
    u = es[:, 0] - p
    v = es[:, 1] - p
    # Returns False if at least one cross product is negative (or 0)
    return np.greater(np.cross(u, v), 0).all()

plot_add_grid()
plot_polygon(vertices_5gon, color = "r")
b = np.array([-2, 2.5])
e = np.array([2.5, -2])
x = b[0]
while x < e[0]:
    y = b[1]
    while y > e[1]:
        p = [x, y]
        in_poly = point_in_polygon_cross_product(p, vertices_5gon)
        c = "orange" if in_poly else "purple"
        plt.scatter(p[0], p[1], color = c)
        y -= 0.1
    x += 0.1
plt.show()

As you can see it works a lot better! :D

I didn't find this algorithm anywhere when searching for point vs polygon tests so I thought maybe it was too slow and nobody used it (I know I'm not the first one to come up with it).

So let's do some performance tests and see how bad it is compared to the previous (slightly inaccurate) one. We will again consider a large amount of points and measure both algorithms.

b = np.array([-2, 2.5])
e = np.array([2.5, -2])
tests = 0
time_ray_cast = 0.0
time_cross_product = 0.0
x = b[0]
while x < e[0]:
    begin = time.time()
    y = b[1]
    while y > e[1]:
        in_poly = point_in_polygon_ray_cast(p, vertices_5gon)
        y -= 0.1
    end = time.time()
    time_ray_cast += end - begin
    begin = time.time()
    y = b[1]
    while y > e[1]:
        in_poly = point_in_polygon_cross_product(p, vertices_5gon)
        y -= 0.1
    end = time.time()
    time_cross_product += end - begin
    x += 0.1
    tests += 1
print("Ray cast average:      ", time_ray_cast / tests, "s")
print("Cross product average: ", time_cross_product / tests, "s")

Ray cast average:       0.025290234883626302 s
Cross product average:  0.005345704820421007 s

WAIT...

It's even faster?

Well even if it was slower I was still going to use it since it's more accurate, but that's a nice surprise :D

I'm sure this is not a fair comparison.. after all I am using numpy vectorization while the first one I'm not so sure how to optimize.

Now before ditching the first algorithm all together, it did provide us with a few useful functions - testing for intersection between rays, lines and segments, etc.

Grabbing Bodies with the Mouse ^↑

When we click on the screen we need to check every body and see if any contain the mouse.

• We need to search through every body and if there are thousands of bodies this can get really slow. There are many optimization tricks for those scenarios but we won't bother since it's not a problem for now.

def mouse_click(x, y, rightButton):
    for b in bodies:
        mouse_in_local_space = np.dot(b.inv_model_mat, data.mouse)
        if point_in_shape(mouse_in_local_space, b.shape):
            # ... #
            break # Don't need to check the rest of the bodies

Where point_in_shape is a helper function which calls the functions we already defined:

def point_in_shape(p, shape):
    if isinstance(shape, Circle):
        return point_in_circle(p, [0, 0], shape.radius)
    elif isinstance(shape, ConvexPolygon):
        return point_in_polygon(p, None, shape.edges)

Now we need to store information about the body which was grabbed:

grabbed = b
grabbed_offset = data.mouse - b.pos
grabbed_was_static = b.static
# We null all forces and stop moving the object while grabbing it
set_static(b, True)

We use grabbed_offset as our pivot while moving the mouse. Otherwise the body's center will get teleported to the cursor instead of the part where we clicked initially.

When we move the mouse we save the current and the last's frame position (you will see later why) and teleport the body:

def mouse_move(x, y):
    mouse_last = data.mouse
    mouse = np.array([x, y])
    if grabbed:
        grabbed.pos = data.mouse - data.grabbed_offset

When we release the mouse button we launch the body with a force! The force direction is calculated using the last two mouse positions (which means that the faster you move the mouse, the further you launch the body):

def mouse_release(rightButton):
    if grabbed:
        # Since the distance between 2 frames of mouse movement 
        # is quite small let's multiply it by a large factor.
        F = (data.mouse - np.array(data.mouse_last)) * 5000
        apply_force(data.grabbed, F, data.grabbed_offset)
        data.grabbed = None

You can play with the demo by selecting demo_grabbing.py after running the .exe.

Air Drag ^↑

If you play around with the demo you will see that the object feels like they it is in space. That's because we don't simulate air drag. In the real life air drag depends on the shape, surface area of an object, etc.

For simplicity we can just reduce the velocity by a little bit if going slow and a lot if going fast ($1\frac{m}{s}$) and not more than that.

def clamp_magnitude(v, length):
    """
    Return a copy of v but with length clamped.
    """
    v = np.array(v).astype(float)
    sq_m = sqr_magnitude(v)
    if sq_m < length ** 2: return v
    return v * length / np.sqrt(sq_m)

We apply it in the loop after we integrate acceleration and velocity:

b.vel -= clamp_magnitude(b.vel, 1) * drag * dt
b.ang_vel -= clamp_magnitude(b.ang_vel, 1) * drag * dt

Where drag is a global constant (like gravity) between 0 and 1.

You may also notice that the floor has "collision". Yeah.. we simply check if the triangle is below the rectangle and if it is, we bring it back up and reset it's y velocity:

floor_y = floor.pos[1]
if triangle.pos[1] > floor_y:
    triangle.pos[1] = floor_y
    triangle.vel[1] = 0

We shall have something more sophisticated after the next section :D.

Discrete Collision Detection ^↑

Bodies that fly around with some force, completely ignoring each other, is cool and all, but let's take things a step further. We will detect if any two bodies intersect and if they do, we'll apply some type of collision response.

That's what we need to do! First figure out if two bodies intersect (and preferably by how much) and then somehow separate them.

Intersection ^↑

We have two types of shapes: circles and convex polygons. Let's look at each case for intersection:

Circle vs. Circle ^↑

plot_add_grid()
# Set up centers and radii
c1, c2 = np.array([-0.5, 0]), np.array([0.8, 1])
r1, r2 = 2, 1.2
plt.gca().add_artist(plt.Circle(c1, r1, color = "r", fill = False))
plt.gca().add_artist(plt.Circle(c2, r2, color = "g", fill = False))
plt.scatter(c1[0], c1[1], color = "r")
plt.scatter(c2[0], c2[1], color = "g")
plt.plot([c1[0], c2[0]], [c1[1], c2[1]], ls = ":")
plt.show()

The way we go about is by using the distance between the centers and compare it to the sum of their radii. We don't even have to get the square root, we can compare the squares (because $ x > y $ if and only if $ x^2 > y^2 $).

First we get the vector which points from the center of the first to the center of the second. That's the line which connects them.

a_to_b = b[0] - a[0]
r = (a[1] + b[1])
if sqr_magnitude(a_to_b) > r ** 2:
    # The distance is larger, they definitely don't intersect

If the comparison fails then they are colliding. Now we get the real distance and return the vector of minimum translation ¹¹ [ The vector with the least magnitude that is required to separate two overlapping shapes. ] .

d = magnitude(a_to_b)
return a_to_b / d * (r - d)

The amount we have to move is just the difference between the distance and the sum of the radii.

One small problem though: we missed the case where the centers overlap. It's a rare case but when it happens the program crashes because we try to divide by zero. In that case without further information we can't return a direction of which the circles should move away. So we just choose arbitrarily - to the right.

Here is the final algorithm:

def circle_vs_circle(a, b):
    """
    Returns the minimum translation vector between two circles. None if they don't intersect.
    A circle is defined by a center and a radius: [[px, py], r].
    The vector returned points from a to b. So b is the circle that "should be moved".
    """
    a_to_b = b[0] - a[0]
    r = (a[1] + b[1])
    if sqr_magnitude(a_to_b) > r ** 2:
        return None # The circles don't overlap
    d = magnitude(a_to_b)
    # We can't compare floating numbers for equality reliably, so we use a function which checks if they are really close.
    if not math.isclose(d, 0):
        return a_to_b / d * (r - d)
    else:
        return np.array([1, 0]) * max(a[1], b[1]) # The amount of which we move here is just the bigger radius

c1, c2 = np.array([-0.5, 0]), np.array([0.8, 1])
r1, r2 = 2, 1.2
plot_add_grid()
plt.gca().add_artist(plt.Circle(c1, r1, color = "r", fill = False))
plt.gca().add_artist(plt.Circle(c2, r2, color = "g", fill = False))
plt.scatter(c1[0], c1[1], color = "r")
plt.scatter(c2[0], c2[1], color = "g")
mtv = circle_vs_circle([c1, r1], [c2, r2])
plot_vector(c2, c2 + mtv, color = "purple")
plt.show()

Cool! We draw the vector from the center of the second circle. If you want to later move first circle instead you need to negate the vector:

c1, c2 = np.array([-0.5, 0]), np.array([0.8, 1])
r1, r2 = 2, 1.2
mtv = circle_vs_circle([c1, r1], [c2, r2])
plot_add_grid()
plt.gca().add_artist(plt.Circle(c1, r1, color = "r", fill = False))
plt.gca().add_artist(plt.Circle(c2 + mtv, r2, color = "g", fill = False))
plt.scatter(c1[0], c1[1], color = "r")
plt.scatter(c2[0] + mtv[0], c2[1] + mtv[1], color = "g")
plot_vector(c2, c2 + mtv, color = "purple")
plt.show()

c1, c2 = np.array([-0.5, 0]), np.array([0.8, 1])
r1, r2 = 2, 1.2
mtv = circle_vs_circle([c1, r1], [c2, r2])
plot_add_grid()
plt.gca().add_artist(plt.Circle(c1 - mtv, r1, color = "r", fill = False))
plt.gca().add_artist(plt.Circle(c2, r2, color = "g", fill = False))
plt.scatter(c1[0] - mtv[0], c1[1] - mtv[1], color = "r")
plt.scatter(c2[0], c2[1], color = "g")
plot_vector(c1, c1 - mtv, color = "purple")
plt.show()

• Just moving the shapes out of each other does not seem realistic for a simulation. We will later look at a better way to handle separation.

Polygon vs. Polygon ^↑

Polygons are a LOT more complex than circles. Let's just appreciate we are not dealing with concave ones. Separating Axis Theorem is a popular way to handle the polygon vs polygon case so let's not try to reinvent the wheel. It's fast and it's really accurate. The idea is simple. If we can find an axis of separation between the polygons then they are not overlapping.

plot_add_grid(range(-4, 6))
plt.axhline(0.7, ls = "-.", color = "y")
plt.axvline(-3, ls = ":", color = "b")
vertices1 = np.array([[4, 1], [2, 4], [1, 1]])
plot_polygon(vertices1, color = "r")
vertices2 = np.array([[2, -2.5], [2.2, -0.5], [1.5, 0.5], [-1.2, -0.5], [-1, -2.5]])
plot_polygon(vertices2, color = "g")
plt.show()

The yellow line goes between the two polygons - it's called a separating line. The blue line is called the separation axis (it's the line that is perpendicular to the separating line). The theorem states that if we can find at least one of these, then the polygons are not overlapping. I mean that's kind of intuitive, right? But how do we do it in code?

We need to decide which axes we will need to test. We can't test all infinitely many directions and positions.. And here is where projection saves the day! We turn the 2D problem into a couple of 1D problems.

plot_add_grid(range(-4, 6))
plt.axhline(-3, ls = ":", color = "b")
x1 = vertices1[:, 0]
x2 = vertices2[:, 0]
min1, max1 = min(x1), max(x1)
min2, max2 = min(x2), max(x2)
plt.plot([min1, max1], [-2.8, -2.8], color = "r")
plt.plot([min2, max2], [-2.8, -2.8], color = "g")
plot_polygon(vertices1, color = "r")
vertices2 = np.array([[2, -2.5], [2.2, -0.5], [1.5, 0.5], [-1.2, -0.5], [-1, -2.5]])
plot_polygon(vertices2, color = "g")
plt.show()

You can see how we project the shapes into their "shadows" (these are also called their orthogonal projections) on the blue line. You may think that we lose information this way - and you are right. The shadows overlap in this example, but the shapes don't. We solve this problem by testing more axes:

plot_add_grid(range(-4, 6))
plt.axvline(-3, ls = ":", color = "b")
y1 = vertices1[:, 1]
y2 = vertices2[:, 1]
min1, max1 = min(y1), max(y1)
min2, max2 = min(y2), max(y2)
plt.plot([-2.8, -2.8], [min1, max1], color = "r")
plt.plot([-2.8, -2.8], [min2, max2], color = "g")
plot_polygon(vertices1, color = "r")
vertices2 = np.array([[2, -2.5], [2.2, -0.5], [1.5, 0.5], [-1.2, -0.5], [-1, -2.5]])
plot_polygon(vertices2, color = "g")
plt.show()

Here we can draw a perpendicular line in that small gap between their shadows. That line will pass between the two polygons - so they definitely aren't colliding. If the algorithm doesn't find such an axis then they are definitely colliding.

Now we need to choose a set of axes which we need to test. For 2D shapes it turns out it's enough to test each normal of each edge of the shapes. Remember that the normal is the vectors perpendicular to an edge and pointing outwards.

def plot_line_from_two_points(a, b, x, color, ls):
    a_coeff = (a[1] - b[1]) / (a[0] - b[0])
    b_coeff = a[1] - a_coeff * a[0]
    y = a_coeff * x + b_coeff
    plt.plot(x, y, color = color, ls = ls)

# Take one edge and calculate it's mid point and normal
e1, e2 = vertices2[2], vertices2[3]
# We don't need to normalize it. We do this just for the pretty picture.
n = normalized(orthogonal(e2 - e1))
mid = np.array((e2 + e1) / 2)
plot_add_grid(range(-4, 6))
# Plot a line through them
plot_line_from_two_points(mid, mid + n, np.linspace(-2, 1.5, 2), color = "b", ls = ":")
plot_polygon(vertices1, color = "r")
vertices2 = np.array([[2, -2.5], [2.2, -0.5], [1.5, 0.5], [-1.2, -0.5], [-1, -2.5]])
plot_polygon(vertices2, color = "g")
plt.show()

This blue axis is not aligned with our $x$ or $y$ axis. So we can't just min/max coordinates. We need a general way to project points onto a line. Let's do some algebra.

We have a vector $\vec{v}$ and a vector $\vec{s}$ onto which we want to project it orthogonally. The projection will be the vector $\vec{s}$ multiplied by some constant $c_{\vec{p}}$. The angle between them is $\theta$. By definition of the projection we are looking at a right angle triangle. $$ \cos\theta = \frac{|c_{\vec{p}}\vec{s}|}{|\vec{v}|}$$

$$ |c_{\vec{p}}\vec{s}| = |\vec{v}|\cos\theta$$

Let's multiply that by $\frac{|\vec{s}|}{|\vec{s}|}$.

$$ |c_{\vec{p}}\vec{s}| = \frac{|\vec{s}||\vec{v}|\cos\theta}{|\vec{s}|} $$

If you remember, the definition of the dot product includes a $\cos\theta$ term. Let's try to use that! $$ \vec{s} \cdot \vec{v} = |\vec{s}||\vec{v}|\cos\theta$$

If we replace that we get:

$$ |c_{\vec{p}}\vec{s}| = \frac{\vec{s} \cdot \vec{v}}{|\vec{s}|} $$

Now that we have the magnitude of $c_{\vec{p}}\vec{s}$ we just need to multiply it by the unit vector (normalized vector) in the direction of $\vec{s}$:

$$ c_{\vec{p}}\vec{s} = \frac{\vec{s} \cdot \vec{v}}{|\vec{s}|} \cdot \frac{\vec{s}}{|\vec{s}|} $$

Finally we get the formula for the projection:

$$ c_{\vec{p}}\vec{s} = \frac{\vec{s} \cdot \vec{v}}{|\vec{s}|^2} \vec{s} $$

def project(v, s):
    return s * np.dot(s, v) / sqr_magnitude(s)

plot_add_grid(range(-4, 6))
plot_line_from_two_points(mid, mid + n, np.linspace(-2, 1.5, 2), color = "b", ls = ":")
plot_polygon(vertices1 + 0.8, color = "r")
vertices2 = np.array([[2, -2.5], [2.2, -0.5], [1.5, 0.5], [-1.2, -0.5], [-1, -2.5]])
plot_polygon(vertices2, color = "g")
plot_vector(mid, mid + n, color = "purple")
# Take one vertex and make a vector between the midpoint of the edge and it
# We actually don't need this too since we can move vectors freely around space 
# and we just care about the coefficent of the projection at the end.
# Again, we do this for the picture below.
v = vertices2[1] - mid
plot_vector(mid, mid + v, color = "r")
projection = project(v, n)
plot_vector(mid, mid + projection, color = "black")
plt.show()

We constructed the red vertex from the vertex and the mid point of the edge and projected it onto the normal vector of the edge.

We can now find the min/max of the projections of all vertices and get the picture we had before:

def plot_projection_of_shape(vertices, axis, offset, color):
    min_proj, max_proj = float("Inf"), float("-Inf")
    for v in vertices:
        vec = v - mid
        # We actually want to find the min/max coefficent so we don't multiply by the axis here.
        proj = np.dot(axis, vec) / sqr_magnitude(axis)
        min_proj = min(proj, min_proj)
        max_proj = max(proj, max_proj)
    a, b = axis * min_proj, axis * max_proj
    plt.plot([a[0] + offset, b[0] + offset], [a[1], b[1]], color = color)
plot_add_grid(range(-5, 6))
plot_line_from_two_points(mid, mid + n, np.linspace(-1.5, 1.8, 2), color = "b", ls = ":")
plot_polygon(vertices1, color = "r")
vertices2 = np.array([[2, -2.5], [2.2, -0.5], [1.5, 0.5], [-1.2, -0.5], [-1, -2.5]])
plot_polygon(vertices2, color = "g")
plot_projection_of_shape(vertices1, n, offset = 0.4, color = "r")
plot_projection_of_shape(vertices2, n, offset = -0.1, color = "g")
plt.show()

As you can see, the projections overlap, which means that the polygons intersect along that axis. Remember that as long as there exists one axis on which the projections don't overlap, the shapes don't intersect.

Let's use all of this and create a method which checks if an axis is a separating axis of two objects:

def is_separating_axis(axis, v1, v2):
    min1, max1 = float("Inf"), float("-Inf")
    min2, max2 = float("Inf"), float("-Inf")
    # Min/max projections of v1
    for v in v1:
        proj = np.dot(v, axis) / sqr_magnitude(axis)
        min1 = min(min1, proj)
        max1 = max(max1, proj)
    # Min/max projections of v2
    for v in v2:
        proj = np.dot(v, axis) / sqr_magnitude(axis)
        min2 = min(min2, proj)
        max2 = max(max2, proj)
    return max1 >= min2 and max2 >= min1:

Now that's good and all, but like we said earlier it's good if we actually get a little more information in the case when they are overlapping. We want the minimum translation vector along that axis. That means that in each axis where the objects are colliding we are going to calculate the vector required to move one of the objects in order to separate them.

In that case we just want the amount of overlap between the two shadows:

d = min(max2 - min1, max1 - min2)

And calculating the vector using the axis:

mtv = d * normalized(axis)

So the final method looks like:

def is_separating_axis_polygons(axis, v1, v2):
    """
    Returns True and None if "axis" is a separating axis of v1 and v2 (v1 and v2 are vertices).
    Return False and the the push vector otherwise.
    """
    min1, max1 = float("Inf"), float("-Inf")
    min2, max2 = float("Inf"), float("-Inf")
    for v in v1:
        proj = np.dot(v, axis) / sqr_magnitude(axis)
        min1 = min(min1, proj)
        max1 = max(max1, proj)
    for v in v2:
        proj = np.dot(v, axis) / sqr_magnitude(axis)
        min2 = min(min2, proj)
        max2 = max(max2, proj)
    if max1 >= min2 and max2 >= min1:
        d = min(max2 - min1, max1 - min2)
        return False, d * normalized(axis)
    else:
        return True, None

Now we just need to collect all polygon normals into an array and test each axes:

def polygon_vs_polygon(polygon_a, polygon_b):
    """
    Returns the minimum push vector of two polygons (None if they don't intersect).
    "polygon_a" and "polygon_b" must be instances of ConvexPolygon.
    """
    axes = np.concatenate((polygon_a.normals, polygon_b.normals), axis = 0)
    push_vectors = []
    for axis in axes:
        separating, tv = is_separating_axis_polygons(axis, polygon_a.vertices, polygon_b.vertices)
        if separating:
            return None # The polygons don't overlap if there exists at least one axis of separation
        push_vectors.append(tv)
    mtv = min(push_vectors, key = (lambda v: sqr_magnitude(v)))
    # Our convention is that the translation vector returned pushes the second object away from the first.
    d = polygon_b.centroid - polygon_a.centroid
    if np.dot(d, mtv) < 0:
        mtv = -mtv
    return mtv

The last few lines:

d = polygon_b.centroid - polygon_a.centroid
if np.dot(d, mtv) < 0:
    mtv = -mtv
return mtv

These ensure that the vector returned pushes the second body away from the first. d is a vector which points from A to B. The dot product tells us if two vectors point generally in the same direction or in different directions - because of the negative cosine, the product is less than zero if they form an angle biggr than 180 degrees. In that case we flip the vector. This last check is very important for consistency.

Polygon vs. Circle ^↑

The last case is polygon vs circle. This one is really easy because we can use the same separating axis technique and reuse a lot of the code.

There are two things to consider. Firstly, circles don't have edges (or you can think about them having infinitely many edges and in that case infinitely many normals). We arbitrarily choose the vector described by the circle's center and the closest vertex of the polygon to it (chosing the second vertex as the polygon's center is not the most appropriate because of the strange shapes polygons can have).

c = np.array([0.5, 0])
r = 1.6
plot_add_grid(range(-3, 6))
plt.gca().add_artist(plt.Circle(c, r, color = "r", fill = False))
vertices = vertices_5gon + [1.5, 2]
plot_polygon(vertices, color = "g")
closest, closest_dist = None, float("Inf")
for v in vertices:
    # We use the squared magnitude since to avoid the square root
    u = v - c
    d = sqr_magnitude(u)
    if d < closest_dist:
            closest = u
            closest_dist = d
plt.plot([c[0], v[0]], [c[1], v[1]], ls = ":")
plt.show()

The second thing is that a circle's shadow is the same no matter what your point of view is:

plot_add_grid(range(-3, 4))
plt.axhline(-3, ls = ":", color = "b")
r = 1.6
plt.gca().add_artist(plt.Circle([0.5, 0], r, color = "r", fill = False))
plt.plot([0.5 - r, 0.5 + r], [-2.8, -2.8], color = "r")
plt.show()

The min is just the projected center minus the radius, and the max is just the projected center plus the radius.

Putting all of this together (and reusing a large part of the previous code):

def is_separating_axis_polygon_and_circle(axis, v1, center, radius):
    """
    Returns True and None if "axis" is a separating axis of v1 and a circle (v1 are vertices).
    Return False and the the translation vector otherwise.
    """
    min1, max1 = float("Inf"), float("-Inf")
    for v in v1:
        proj = np.dot(v, axis) / sqr_magnitude(axis)
        min1 = min(min1, proj)
        max1 = max(max1, proj)
    proj = np.dot(center, axis) / sqr_magnitude(axis)
    min2 = proj - radius
    max2 = proj + radius
    if n1 >= m2 and n2 >= m1:
        d = min(n2 - m1, n1 - m2)
        return False, d * normalized(axis)
    else:
        return True, None
def polygon_vs_circle(polygon, circle):
    """
    Return the minimum push vector of a polygon and a circle (None if they don't intersect).
    "polygon" and "circle" must be instances of ConvexPolygon and Circle respectively.
    """
    closest, closest_dist = None, float("Inf")
    for v in polygon.vertices:
        u = v - circle.centroid
        d = sqr_magnitude(u)
        if d < closest_dist:
            closest = u
            closest_dist = d
    axes = np.concatenate((polygon.normals, [normalized(closest)]), axis = 0)
    push_vectors = []
    for axis in axes:
        separating, tv = is_separating_axis_polygon_and_circle(axis, polygon.vertices, circle.centroid, circle.radius)
        if separating:
            return None # The polygon and circle don't overlap if there exists at least one axis of separation
        push_vectors.append(tv)
    mtv = min(push_vectors, key = (lambda v: sqr_magnitude(v)))
    d = circle.centroid - polygon.centroid
    if np.dot(d, mtv) < 0:
        mtv = -mtv
    return mtv

• Intersection testing methods in the demos are in data/scripts/hit.py.

Response ^↑

One way to handle response is just by moving the shapes directly. This is quite unnatural and very very buggy. If you launch demo_collision.py and uncheck Impulse based resolution from the side bar you can see how that looks. Since it's so bad I won't even try to make it work properly (sometimes shapes even fall through the floor).

We will instead use a more physical approach. For that, we will need a list of contact points - the points at which the objects (approximately) touched before intersecting.

Collision Contact Points ^↑

In order to apply the impulse at the proper location, we need to calculate the points at which the collision happened. Generating good contact points is crucial to predictable and life-like iteractions between bodies.

Failed (?) first try ^↑

At the beginning I thought about just projecting each vertex to the minimum translation vector and choosing the two points with the largest projection (the points that are the furthest "stuck" in the other body). If they are close together (by a given tolerance) though, the edge they form is almost perpendicular to the translation vector and we take their average (midpoint of the edge), otherwise we return the maximum point:

The implementation is really simple (after all I didn't put much thought into making an accurate algorithm :D):

n1, n2 = float("-Inf"), float("-Inf")
n1_v, n2_v = None, None
for v in shape.vertices:
    proj = np.dot(v, mta)
    if proj > n2:
        if proj >= n1:
            n1, n2 = proj, n1
            n1_v, n2_v = v, n1_v
        else:
            n2 = proj
            n2_v = v

Here we project each vertex of a polygon to the mta (minimum translation axis) and calculate the two points with the largest projection.

tolerance = 0.1
if n1 - n2 < tolerance:
    return (n1_v + n2_v) / 2
else:
    return n1_v

And at the end we return their "average".

The final code:

def get_average_contact_point(shape, mta):
    """
    Returns a very very crude approximation of the contact point of a shape when a collision occurs.
    It is used to calculate the angular velocity the shape recieves after the collision.
    Calculates the two vertices with the largest projection onto "mta" 
    and returns the max or their average using some specified tolerance. 
    For circles it returns the center (we don't even bother, you can't tell if they are rotating haha).
    """
    if shape.type == sh.Type.CIRCLE: return shape.centroid
    n1, n2 = float("-Inf"), float("-Inf")
    n1_v, n2_v = None, None
    for v in shape.vertices:
        proj = np.dot(v, mta)
        if proj > n2:
            if proj >= n1:
                n1, n2 = proj, n1
                n1_v, n2_v = v, n1_v
            else:
                n2 = proj
                n2_v = v
    tolerance = 0.1
    if n1 - n2 < tolerance:
        return (n1_v + n2_v) / 2
    else:
        return n1_v

When we are doing circles we just return the centroid:

if isinstance(shape, Circle): return shape.centroid

This is because the way we have set it up it's impossible to tell if a circle is rotating :D (we don't draw any reference points). So this is a very big hack but since I thought this algorithm wouldn't be at all sufficient I didn't bother making it work for circles.

After writing it up with impulses and everything and testing it, the results were horrible (I even tried returning both points individually instead of taking the average but it was still too bad). I instantly concluded my algorithm was way too inaccurate.

The better method ^↑

So now let's instead explore a popular way to calculate the contact points using the clipping method. The good thing is shares some parts with the algorithm above but it also introduces the concept of incident and reference edges.

The first step is to identify the two edges that are the most involved in the collision - we determine that by calculating which edge is most perpendicular to the collision normal.

We start by finding the farthest vertex in the shape. Then we look at the adjacent two edges:

def collision_find_best_edge(vertices, mtv):
    """
    Returns the edge furthest along "mtv" and most perpendicular to "mtv".
    Also returns the furthest vurtex (we will need this later :P)
    """
    # This part is familiar but instead of finding the two furthest vertices we find the one. 
    max_proj = float("-Inf")
    i_vertex, vertex = None, None
    for i, v in enumerate(vertices):
        proj = np.dot(v, mtv)
        if proj > max_proj:
            i_vertex = i
            vertex = v
            max_proj = proj
    # Using the index of the furthest vertex we look at the adjacent edges
    r_vertex = vertices[(i_vertex + 1) % len(vertices)]
    l_vertex = vertices[(i_vertex - 1) % len(vertices)]
    # Both of these must point to "vertex"!
    r = normalized(vertex - r_vertex)
    l = normalized(vertex - l_vertex)
    # See which edge is "more perpendicular". A dot product closer to 0 means angle closer to 90 degrees.
    if np.dot(r, mtv) < np.dot(l, mtv):
        # Right edge is better
        return [vertex, r_vertex], vertex
    else:
        # Left edge is better
        return [l_vertex, vertex], vertex

Here's a visual test:

plot_add_grid(range(-4, 6))
poly1 = ConvexPolygon(np.array([[4, 1], [2, 4], [1, 1]]) - 1.5, recenter = False)
plot_polygon(poly1.vertices, color = "r")
poly2 = ConvexPolygon(np.array([[2, -2.5], [2.2, -0.5], [1.5, 0.5], [-1.2, -0.5], [-1, -2.5]]), recenter = False)
plot_polygon(poly2.vertices, color = "g")
mtv = polygon_vs_polygon(poly1, poly2)
plot_vector(poly2.centroid, poly2.centroid + mtv, "r")
a_best_edge, a_max_v = collision_find_best_edge(poly1.vertices, mtv)
b_best_edge, b_max_v = collision_find_best_edge(poly2.vertices, -mtv)
plt.plot([a_best_edge[0][0], a_best_edge[1][0]], [a_best_edge[0][1], a_best_edge[1][1]], "m", lw = 3)
plt.plot([b_best_edge[0][0], b_best_edge[1][0]], [b_best_edge[0][1], b_best_edge[1][1]], "m", lw = 3)
plt.show()

Now we choose the best of these edges as a "reference" edge and the other one will be the "incident" edge. We imagine as if the incident edge crashed into the reference edge, so the collision points will be along that.

a_best_edge, a_max_v = collision_find_best_edge(a.transformed_shape.vertices, mtv)
b_best_edge, b_max_v = collision_find_best_edge(b.transformed_shape.vertices, -mtv)
# Now compare the dot products of the two best edges and determine ref/inc
if abs(dot_edge(a_best_edge, mtv)) < abs(dot_edge(b_best_edge, mtv)):
    ref = a_best_edge
    ref_max_v = a_max_v
    inc = b_best_edge
else:
    ref = b_best_edge
    ref_max_v = b_max_v
    inc = a_best_edge

Again, we save the ref_max_v as the furthest vertex (we will use it as the final clip later).

def dot_edge(edge, v):
    return np.dot(edge[1] - edge[0], v)

plot_add_grid(range(-4, 6))
plot_polygon(poly1.vertices, color = "r")
plot_polygon(poly2.vertices, color = "g")
plot_vector(poly2.centroid, poly2.centroid + mtv, color = "r")
if dot_edge(a_best_edge, mtv) < dot_edge(b_best_edge, mtv):
    ref = a_best_edge
    ref_max_v = a_max_v
    inc = b_best_edge
else:
    ref = b_best_edge
    ref_max_v = b_max_v
    inc = a_best_edge
plt.plot([inc[0][0], inc[1][0]], [inc[0][1], inc[1][1]], "m", lw = 3)
plt.plot([ref[0][0], ref[1][0]], [ref[0][1], ref[1][1]], "g", lw = 3)
plt.show()

The reference edge is green and the incident edge is magenta.

Now we will take the end points of the incident edge as our collision points. We will clip them so they lie directly above the reference edge:

plot_add_grid(range(-4, 6))
plot_polygon(poly1.vertices, color = "r")
plot_polygon(poly2.vertices, color = "g")
plot_vector(poly2.centroid, poly2.centroid + mtv, color = "r")
plt.plot([inc[0][0], inc[1][0]], [inc[0][1], inc[1][1]], "m", lw = 3)
plt.plot([ref[0][0], ref[1][0]], [ref[0][1], ref[1][1]], "g", lw = 3)
plt.axvline(-0.5, ls = ":")
plt.axvline(2.5, ls = ":")
plt.scatter(-0.5, -0.3, color = "b")
plt.scatter(1.5, 0.5, color = "b")
plt.show()

To actually clip the points, as you might guess if you think about the problem a bit, we will again use projections. Remember that the formula for the projection is: $$ c_{\vec{p}}\vec{s} = \frac{\vec{s} \cdot \vec{v}}{|\vec{s}|^2} \vec{s} $$

But since we are comparing them, we don't need to be accurate. By that I mean we can safely ignore $|\vec{s}|^2$ and $\vec{s}$, since they are shared between all points we will be projecting (and $\vec{s}$ will be our vector defined from the reference edge).

# Since we are now dealing with contact points and we need to store a lot of information about them, we will make a class.
# This allows us to keep code readable and name certain properties instead of indexing into tuples.
class Contact:
    def __init__(self, pos):
        self.pos = pos

def collision_clip_segment(p1, p2, n, offset):
    """
    Clips the segment's end points if they are past "offset" along "n".
    """
    clipped = []
    d1 = np.dot(p1, n)
    d2 = np.dot(p2, n)
    # If either point is past "offset" along "n" we keep it
    if d1 <= offset:
        clipped.append(p1)
    if d2 <= offset:
        clipped.append(p2)
    d1 -= offset
    d2 -= offset
    # Check if they are on opposing sides.
    # If you can visualize this in your head, if they are not on opposing sides, that means that
    # the segment is entirely outside of our view. In that case we return an empty array.
    if d1 * d2 < 0:
        e = p2 - p1
        # Compute the coeff along "e"
        u = d1 / (d1 - d2)
        # The new position is "u * e" but we add the original point in order to convert it into the proper space
        clipped.append(e * u + p1)
    return clipped

We now need to do three clips in total. First, clip the end points against the first vertex of the reference edge, then against the second, and finally against the edge itself (disregard points that are not inside the other shape).

The first two clips use the method above:

ref_v = normalized(ref[1] - ref[0])
o1 = -np.dot(ref_v, ref[0])
clipped = collision_clip_segment(inc[0], inc[1], -ref_v, o1)
o2 = np.dot(ref_v, ref[1])
clipped = collision_clip_segment(clipped[0], clipped[1], ref_v, o2)

The only thing we need to be careful is the signs here.

The third clip removes points that are above the reference edge - i.e they have a "negative" intersection (they are outside of the other shape). We project the points to mtv and compare them with the max vertex we saved earlier:

# Clip the final points against the reference edge
ref_max_v_depth = np.dot(ref_normal, ref_max_v)
c0_depth = np.dot(ref_normal, clipped[0]) - ref_max_v_depth
c1_depth = np.dot(ref_normal, clipped[1]) - ref_max_v_depth
# We also save the collision depth of each contact point.
# This is very important, because different contact points are in 
# different depths and we need that information later
contacts = [Contact(clipped[0]), Contact(clipped[1])]
contacts[0].collision_depth = c0_depth
contacts[1].collision_depth = c1_depth
if c1_depth < 0:
    contacts.pop(1)
if c0_depth < 0:
    contacts.pop(0)

def collision_clip_points(ref, ref_max_v, inc):
    ref_v = normalized(ref[1] - ref[0])
    o1 = -np.dot(ref_v, ref[0])
    clipped = collision_clip_segment(inc[0], inc[1], -ref_v, o1)
    o2 = np.dot(ref_v, ref[1])
    clipped = collision_clip_segment(clipped[0], clipped[1], ref_v, o2)
    ref_normal = -orthogonal(ref_v)
    ref_max_v_depth = np.dot(ref_normal, ref_max_v)
    c0_depth = np.dot(ref_normal, clipped[0]) - ref_max_v_depth
    c1_depth = np.dot(ref_normal, clipped[1]) - ref_max_v_depth
    contacts = [Contact(clipped[0]), Contact(clipped[1])]
    contacts[0].collision_depth = c0_depth
    contacts[1].collision_depth = c1_depth
    if c1_depth < 0:
        contacts.pop(1)
    if c0_depth < 0:
        contacts.pop(0)
    return contacts

Here is a test:

plot_add_grid(range(-4, 6))
poly1 = ConvexPolygon(np.array([[-1, 0], [3, 0], [3, -2], [-1, -2]]), recenter = False)
plot_polygon(poly1.vertices, color = "r")
poly2 = ConvexPolygon(np.array([[3.5, -1.666], [4.5, 0], [1.5, 2], [1.5, -0.333]]), recenter = False)
plot_polygon(poly2.vertices, color = "g")
mtv = polygon_vs_polygon(poly1, poly2)
plot_polygon(poly1.vertices, color = "r")
plot_polygon(poly2.vertices, color = "g")
plot_vector(poly2.centroid, poly2.centroid + mtv, color = "r")
a_best_edge, a_max_v = collision_find_best_edge(poly1.vertices, mtv)
b_best_edge, b_max_v = collision_find_best_edge(poly2.vertices, -mtv)
if dot_edge(a_best_edge, mtv) < dot_edge(b_best_edge, -mtv):
    ref = a_best_edge
    ref_max_v = a_max_v
    inc = b_best_edge
else:
    ref = b_best_edge
    ref_max_v = b_max_v
    inc = a_best_edge
plt.plot([ref[0][0], ref[1][0]], [ref[0][1], ref[1][1]], "g", lw = 3)
plt.plot([inc[0][0], inc[1][0]], [inc[0][1], inc[1][1]], "m", lw = 3)
for c in collision_clip_points(ref, ref_max_v, inc):
    plt.scatter(c.pos[0], c.pos[1], color = "b")
plt.show()

You can play around with the coordinates and see where the contact points get placed.

Quick word about circles ^↑

Curved shapes are easy to handle! Just calculate the point furthest along the mtv by multiplying the normalized version by the radius. In that case we don't need to do any clipping, just bail out and return the point. Our final collide method with everything combined neatly looks like this:

def collide(a, b):
    """
    Returns a list of contact points or empty if "a" and "b" are not colliding.
    """
    if not aabb_vs_aabb(a.transformed_shape.aabb, b.transformed_shape.aabb):
        return []
    mtv = minimum_translation_vector(a.transformed_shape, b.transformed_shape)
    if mtv is not None:
        d = b.transformed_shape.centroid - a.transformed_shape.centroid
        if np.dot(d, mtv) < 0:
            mtv = -mtv
        normal = normalized(mtv)
        if a.transformed_shape.type == shape.Type.CIRCLE:
            furthest_point = a.transformed_shape.centroid + a.transformed_shape.radius * normal
            c = Contact(furthest_point)
            c.normal = normal
            c.collision_depth = magnitude(mtv)
            return [c]
        a_best_edge, a_max_v = collision_find_best_edge(a.transformed_shape.vertices, mtv)
        if b.transformed_shape.type == shape.Type.CIRCLE:
            furthest_point = b.transformed_shape.centroid + b.transformed_shape.radius * normal
            c = Contact(furthest_point)
            c.normal = normal
            c.collision_depth = magnitude(mtv)
            return [c]
        b_best_edge, b_max_v = collision_find_best_edge(b.transformed_shape.vertices, -mtv)
        if abs(dot_edge(a_best_edge, mtv)) < abs(dot_edge(b_best_edge, mtv)):
            ref = a_best_edge
            ref_max_v = a_max_v
            inc = b_best_edge
        else:
            ref = b_best_edge
            ref_max_v = b_max_v
            inc = a_best_edge
        contacts = collision_clip_points(ref, ref_max_v, inc)
        # Don't forget the normals here...
        for c in contacts:
            c.normal = normal
        return contacts
    return []

We haven't taken a look at minimum_translation_vector but it's just a method which looks at the shapes and based on their type runs the algorithms for the MTV ¹¹ [ The vector with the least magnitude that is required to separate two overlapping shapes. ] we described earlier!

Now we have all needed information!!! :D

Impulse Based Collision Resolution ^↑

This is kinda the boring part, because this is just physics. We have the formulas and we need to write them in code. No problems to solve except if you got a minus wrong or some constant is messed up...

So the way we separate objects is by applying an impulse at every contact point. Impulse, remember, is an instantaneous change in velocity. Modifying acceleration or applying a force becomes too sloppy for the simulation.

First, we need to know the relative velocity at that contact point of the two objects (from A to B), which must be expressed in terms of the collision normal $\vec{n}$: $$ \Delta \vec{V} \cdot \vec{n} = (\vec{V_{B}} + \omega \times \vec{r_{2}} - \vec{V_{A}} - \omega \times \vec{r_{1}}) \cdot \vec{n} $$

Omega here means the angular velocity and $\vec{r_{1}}$ and $\vec{r_{2}}$ are the vectors from the centers of the objects to the contact point.

The following formula relates the impulse with the masses and rotational intertias of the two objects:

$$ k_n = \frac{1}{m_A} + \frac{1}{m_B} + \left[ \frac{1}{I_A}(\vec{r_1} \times \vec{n}) \times \vec{r_1} + \frac{1}{I_B}(\vec{r_2} \times \vec{n}) \times \vec{r_2} \right] \cdot \vec{n} $$

Due to floating point errors, if we implement these formulas exactly and run the simulation, our objects will be still going slowly downwards once they hit the static floor. This is because gravity causes error to accumulate and we just let it build up. To fix this, we will introduce a bias proportional to the magnitude of our mtv which we got from the intersection test earlier in order to correct the positions.

$$ \text{bias} = \frac{\text{position_bias_factor}}{\Delta t} max(0, \text{allowed_penetration} - \text{collision_depth}) $$

We set $\text{position_bias_factor}$ as a constant to $0.2$ and $\text{allowed_penetration}$ to $0.01$ while $\text{collision_depth}$ is the depth of the current contact point.

$\Delta t$ is our time step in seconds. This means that the $\text{bias}$ is measured in $\frac{m}{s}$ and we can add it to the velocity directly.

Accounting for the bias, we get our impulse formula:

$$ J = max(\frac{- \Delta \vec{V} \cdot \vec{n} + \text{bias}}{k_n}, 0) \cdot \vec{n} $$

We use max to ensure we don't get a negative impulse somehow.

In code this looks like:

def cross_scalar(s, v):
    return [-s * v[1], s * v[0]]
contacts = collide(a, b)
for c in contacts:
    r1 = c.pos - a.pos
    r2 = c.pos - b.pos
    rel_vel = b.vel + cross_scalar(b.ang_vel, r2) - a.vel - cross_scalar(a.ang_vel, r1)
    vel_along_normal = np.dot(rel_vel, c.normal)
    rn1 = np.dot(r1, c.normal)
    rn2 = np.dot(r2, c.normal)
    k = a.inv_mass + b.inv_mass
    k += a.inv_rot_inertia * (sqr_magnitude(r1) - rn1 ** 2) + b.inv_rot_inertia * (sqr_magnitude(r2) - rn2 ** 2)
    inv_k = 1 / k
    bias = -position_bias_factor * 1 / dt * min(0, -c.collision_depth + allowed_penetration)
    j = max(inv_k * (-vel_along_normal + bias), 0)
    impulse = j * c.normal
    apply_impulse(a, -impulse, r1)
    apply_impulse(b, impulse, r2)

Here we use inv_mass (and inv_rot_inertia) which is pre-calculated (1 / mass) when we create a body. For static bodies mass, inv_mass, rot_inertia, and inv_rot_inertia are $0$ so we don't need an extra case for them here. The impulse they receive is automatically zero.

The demo is demo_collision.py.

Iterations ^↑

You can increase the number of times you run the code and calculate and apply the impulse (this doesn't blow up because everytime you apply an impulse the relative velocity changes but we just don't move the objects yet). With each iteration we get closer to the true impulse (accuracy at the cost of performace). You can play with this in the demo in the side bar option.

Friction ^↑

The shapes in the demo feel really slippery :D. That's because we don't simulate friction. If you want you can incorporate that into the engine by calculating the tangent normal for each contact point and applying a friction impulse using the friction coefficient for both bodies.

Continuous Collision Detection ^↑

If you run demo_tunneling.py you will see a problem with our engine. Stuff that's moving fast can go through objects. This is not a bug in our code but instead a flaw in our entire design! It happens because we are calculating in discrete time steps.

That means that if a thing has a high enough velocity it can go straight through other objects and we can't possibly detect if the objects collided - they have already passed each other. You can increase the number of time steps but that comes with a huge cost of performance - you have to be a bit smarter.

There are methods to solve this. We won't be implementing a continuous collision solver here ¹² [ I actually really want to extend the demos and this article with an implementation, so stay tuned! ] but there's a great talk by a Blizzard aerospace, engineer at GDC about a robust solution to this problem. There is also an article by Casey Muratori's blog on how to implement the GJK algorithm.

Practical Applications ^↑

So we've successfully looked at the beginning of an implementation of a physics engine. What do people use them for?

• Firstly, extending the thing we've built to 3D is not that hard actually. An extra coordinate everywhere doesn't change the idea of the math. After all, linear algebra works the same even with vectors of infinite dimension :P

  One part becomes a lot harder though and that's rotations - in 3D we use quaternions (4D numbers) to represent orientations. It turns out that quaternions are the most robust and convenient way but also the most confusing and most rarely fully understood (just because we can't have an intuition for 4D objects). Nevertheless 3blue1brown has a great series on a 3D perspective on visualizing quaternions.

• Games often require a physics engine because of the gameplay elements in them that are supposed to represent real world objects. First person shooters for example rely on very accurate collision detection - or the players will get very frustrated. They also require a continuous collision solver because bullets move at very fast speeds. But some games are simpler in that aspect and get away with just a discrete physics solver.

• Engineers use computer simulations to build aircraft, bridges, buildings, .. almost everything basically. It's super useful (and cheap) to test things in a simulation before building them in the real world.

• Physicists and chemists use simulations to study the structure of molecules and predict how certain compounds would react with each other. Definitely one of the more interesting areas.

I hope I've given you some basic understanding of how to implement a simulation yourself. I learned a whole bunch doing this project and I really enjoyed doing it and sharing the knowledge. Before this I've never been able to implement a working collision system.

I wrote the physics code in Python so it can be easily understood and ported to another language.

Further Reading ^↑

Here are a couple of links for more stuff related to simulating physics that's outside the scope of rigid body dynamics.

• Two Minute Papers, Training an AI to compute physics much faster than traditional methods: https://www.youtube.com/watch?v=atcKO15YVD8

• Why are cloth simulations difficult: https://www.youtube.com/watch?v=UoKXJzTYDpw&vl=en

• A cellular automaton is a collection of particles on a grid of specified shape that evolves through a number of discrete time steps according to a set of rules based on the states of neighboring cells. Turns out you can use this to simulate sand gravity and pile-forming: https://core.ac.uk/download/pdf/82379543.pdf. And here is a video: https://www.youtube.com/watch?v=AMm0gX9NXaI

• Lattice gas cellular automata (LGCA) on the other hand is a simulation where the dynamics of gas and fluids are simplied through simple local rules for the interactions between particles (cells): https://link.springer.com/referenceworkentry/10.1007%2F978-3-319-08234-9_184-1. An example: https://www.youtube.com/watch?v=odaJTL9_eCA

by Dimitar Sotirov

Some References Which Made this Possible ^↑

• Arav Singhal, Making a 2D Physics Engine, 2018, link
• Erin Catto, Crystal Dynamics, Fast and Simple Physics using Sequential Impulses, 2006, link
• Gilles Bellot, The Centroid of Convex Polygons, 2019, link
• Randy Gaul, How to Create a Custom 2D Physics Engine: The Basics and Impulse Resolution, 2013, link
• Contact Points Using Clipping, 2011, link
• Concise Tutorial for SAT (Separating Axis Theorem), 2017, link
• Orthogonal Projection onto a Line, link
• YouTube, Mathispower4u, Proving the Vector Projection Formula, link