3D Cubic Splines from Irregularly Sampled Timepoints

Interpolating trajectories with cubic splines.

Motivation

Imagine a rigid body moving in 3D space, subject to forces. Then imagine integrating this system with an ODE solver and getting the output states as a list of timepoints, positions and velocities. Optionally also orientations and angular velocities, but we can ignore those for now.

Adaptive ODE solvers are relatively common, which could result in states at irregularly sampled timepoints. This means the deltatime between timepoints isn’t constant, which is again often problematic when doing signal processing or other forms of data analysis, typically being solved with various interpolation schemes.

Let’s say we now have two rigid bodies, and we want to check if they collide when simulating the systems. Also assume that we are interested in checking if the two objects are going to collide at some future timepoint, and then finding the time of collision. One option is to have a low enough step size, and just check every iteration, as game engines often do. Here is an alternative approach relying on spline interpolations for irregularly sampled timepoints.

Let’s first construct a continuous representation of the trajectories using splines.

Splines

Given a set of $n$ states:

\begin{align*} & (t_0, \bm{p}_0, \bm{v}_0) \\ & (t_1, \bm{p}_1, \bm{v}_1) \\ & \dots \\ & (t_{n - 1}, \bm{p}_{n - 1}, \bm{v}_{n - 1}) \end{align*}

here assumed to be 3-dimensional, find a function $\bm{f} : \mathbb{R} \rightarrow \mathbb{R}^3$ that perfectly hits each of these states:

\begin{align*} \bm{f}(t_i) &= \bm{p}_i \\ \bm{f}'(t_i) &= \bm{v}_i \end{align*}

for $i \in [0, n - 1]$ .

As an intermediary step, let’s start by assuming that the states are actually 1-dimensional. Using a polynomial $f$ to interpolate points is relatively easy. Given $n$ points, a polynomial of degree $n - 1$ can be used to perfectly fit each of these points. But higher-degree polynomials have a lot of problems (poor generalizability outside the points, numerical instability from high exponents, high variance to noise in the dataset). A spline is defined as a set of piecewise polynomials, consisting of many polynomials with a lower degree instead of one single high degree polynomial. Splines will often fix a lot of the mentioned issues, and can be considered a much more robust way to interpolate.

For $n$ states, a spline would then be represented as $n - 1$ different polynomials $f_i$ defined on each interval: $[t_i, t_{i + 1}]$ such that:

\begin{align*} f_i(t_i) &= p_i \\ f_i(t_{i + 1}) &= p_{i + 1} \end{align*}

This means that the spline segments are continuous on the knot points $p_i$ . If the velocity constraints are added in:

\begin{align*} f_i'(t_i) &= v_i \\ f_i'(t_{i + 1}) &= v_{i + 1} \end{align*}

then the splines also have a continuous first derivative on the knot points.

Cubic Splines

There are a lot of different types of splines, and ways to construct them.

Many physical systems modeled with Newton’s second law will typically have a continuous state and first derivative. The second derivative (like acceleration) will not necessarily be continuous, as that depends on the active forces.

This then motivates the use of spline segments with cubic polynomials:

f_i(t) = a_i t^3 + b_i t^2 + c_i t + d_i

which means that acceleration will be continuous within each spline segment, but can be discontinuous between them.

A cubic polynomial has 4 coefficients. Given positions and velocities at 2 knot points, this results in $4$ equations:

\begin{align*} f_i(t_i) &= a_i t_i^3 + b_i t_i^2 + c_i t_i + d_i &= p_i \\ f_i(t_{i + 1}) &= a_i t_{i + 1}^3 + b_i t_{i + 1}^2 + c_i t_{i + 1} + d_i &= p_{i + 1} \\ f_i'(t_i) &= 3 a_i t_i^2 + 2 b_i t_i + c_i &= v_i \\ f_i'(t_{i + 1}) &= 3 a_i t_{i + 1}^2 + 2 b_i t_{i + 1} + c_i &= v_{i + 1} \end{align*}

and $4$ unknowns. Rewrite as a linear system:

\begin{bmatrix} t_i^3 & t_i^2 & t_i & 1 \\ t_{i + 1}^3 & t_{i + 1}^2 & t_{i + 1} & 1 \\ 3 t_i^2 & 2 t_i & 1 & 0 \\ 3 t_{i + 1}^2 & 2 t_{i + 1} & 1 & 0 \end{bmatrix} \begin{bmatrix} a_i \\ b_i \\ c_i \\ d_i \end{bmatrix} = \begin{bmatrix} p_i \\ p_{i + 1} \\ v_i \\ v_{i + 1} \end{bmatrix}

which is solvable as long as $t_i \neq t_{i + 1}$ , and can be solved symbolically to get explicit expressions for the coefficients of each spline segment. Alternatively with numerical solvers.

Cubic Hermite Splines (Tangent 1)

Right now the timepoints are assumed to be any value, as long as they are sorted in ascending order. Sometimes the timepoints can get relatively large, which can lead to some numerical errors. Something that would simplify some calculations and numerical problems is to normalize the time between intervals to always go from 0 to 1.

Define a normalized time $u_i$ as:

u_i(t) = \frac{t - t_i}{t_{i+1} - t_i}

for $t \in [t_i, t_{i + 1}]$ . Then define the new spline segment polynomial:

F_i(u_i) = a_i u_i^3 + b_i u_i^2 + c_i u_i + d_i

such that:

\begin{align*} f_i(t) &= F_i(u_i(t)) \\ f_i'(t) &= F_i'(u_i(t)) \cdot u_i'(t) = \frac{F_i'(u_i(t))}{t_{i + 1} - t_i} \end{align*}

Can then define:

\begin{align*} m_i = (t_{i + 1} - t_i) v_i m_{i + 1} = (t_{i + 1} - t_i) v_{i + 1} \end{align*}

for each spline segment. This leads to a simpler form:

\begin{bmatrix} 0 & 0 & 0 & 1 \\ 1 & 1 & 1 & 1 \\ 0 & 0 & 1 & 0 \\ 3 & 2 & 1 & 0 \end{bmatrix} \begin{bmatrix} a_i \\ b_i \\ c_i \\ d_i \end{bmatrix} = \begin{bmatrix} p_i \\ p_{i + 1} \\ m_i \\ m_{i + 1} \end{bmatrix}

This approach turns out be to equivalent to a cubic Hermite spline, which uses a set of basis functions to define the polynomial:

F_i(u_i) = (2 u_i^3 - 3 u_i^2 + 1) p_i + (u_i^3 - 2 u_i^2 + u_i) m_i + (-2 u_i^3 + 3 u_i^2) p_{i + 1} + (u_i^3 - u_i^2) m_{i + 1}

Expanding the above expression and collecting the terms for the coefficients can be shown to lead to the same result as solving the linear system.

Cubic Hermite splines are a relatively standard way of doing it, so mentioned here for completeness. It is often convenient to not have to rescale the timepoints, so the initial approach is going to be the one used forwards.

3D Splines

Let’s now go back to the original 3-dimensional trajectories. Extending splines to work in 3-dimensions is as easy as using a separate spline for each coordinate, and concatenating in a vector. Each spline segment is found by independently estimating the coefficients for that segment and coordinate. For cubic splines this becomes:

\bm{f}_i(t) = \begin{bmatrix} x_i(t) \\ y_i(t) \\ z_i(t) \end{bmatrix} = \begin{bmatrix} a_{x, i} t^3 + b_{x, i} t^2 + c_{x, i} t + d_{x, i} \\ a_{y, i} t^3 + b_{y, i} t^2 + c_{y, i} t + d_{y, i} \\ a_{z, i} t^3 + b_{z, i} t^2 + c_{z, i} t + d_{z, i} \end{bmatrix}

Another way to interpret 3D splines is as follows:

\bm{f}_i(t) = \begin{bmatrix} a_{x, i} \\ a_{y, i} \\ a_{z, i} \end{bmatrix} t^3 + \begin{bmatrix} b_{x, i} \\ b_{y, i} \\ b_{z, i} \end{bmatrix} t^2 + \begin{bmatrix} c_{x, i} \\ c_{y, i} \\ c_{z, i} \end{bmatrix} t + \begin{bmatrix} d_{x, i} \\ d_{y, i} \\ d_{z, i} \end{bmatrix}

meaning one cubic spline with the coefficients as vectors. This is of course the same mathematically, but can be important to consider when implementing it in code. Basically vector of splines vs spline of vectors.

Horner’s Method for Evaluating Polynomials (Tangent 2)

Let’s use a standard cubic polynomial as an example:

f(t) = a t^3 + b t^2 + c t + d

To evaluate this polynomial naively, an algorithm can look like this:

def f_naive(t, a, b, c, d):
    t2 = t * t
    t3 = t2 * t

    return a * t3 + b * t2 + c * t + d

requiring a total of 5 multiplications and 3 additions. As floating point multiplications are a lot more expensive than additions, a more efficient implementation looks like this:

def f_horner(t, a, b, c, d):
    return (((a * t) + b) * t + c) * t + d

called Horner’s method, and only requires 3 multiplications and 3 additions. For a general polynomial of degree n, the implementation could look like this:

def f_horner(t, coeffs):
    n = len(coeffs)
    f = coeffs[0]
    for i in range(1, n):
        f = f * t + coeffs[i]
    return f

End

Now that we have a way to represent trajectories as continuous functions, the followup post goes into how to use these to check for collisions between two different trajectories.