16.09.2022

Last edited 18.09.2022

```
%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
^{01}
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`

.

## 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
^{02}
. We imagine the object's
global
^{03}
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
^{04}
.

For polygons we cache the *edges* and also their
normal vectors
^{05}
. 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 `i`

th 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
^{06}
. 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
^{07}
.

### 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 convinient 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()
```

```
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 3and 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
^{08}
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
^{09}
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!

`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
^{10}
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 $$

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
^{11}
.

```
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
^{11}
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
^{12}
but there's a great talk by a Blizzard game developer 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 :POne 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 understading 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

- 01
- 02
- 03
- 04
- 05
- 06
- 07
- 08
- 09
- 10
- 11
- 12