Polynomial Interpolation Via Point Trajectories

img.png

interpolation.py

from collections import namedtuple, OrderedDict
from typing import Dict, Callable

import numpy as np


class Values:
    def __init__(self, x, y, vx, vy):
        self.b = np.array([x, y])
        self.b_dot = np.array([vx, vy])


def build_interpolation(data: Dict[float, Values]) -> Callable:
    vias = sorted(data.items(), key=lambda x: x[0])
    polynoms = OrderedDict()
    for j in range(len(vias) - 1):
        Tj = vias[j][0]
        dTj = vias[j+1][0] - Tj

        bj = vias[j][1].b
        b_dotj = vias[j][1].b_dot
        bj1 = vias[j+1][1].b
        b_dotj1 = vias[j+1][1].b_dot

        a0 = bj
        a1 = b_dotj
        a2 = (3*bj1 - 3*bj - 2*b_dotj*dTj - b_dotj1*dTj)/(dTj**2)
        a3 = (2*bj + (b_dotj + b_dotj1)*dTj - 2*bj1)/(dTj**3)

        polynoms[Tj] = (a0.copy(), a1.copy(), a2.copy(), a3.copy())

    def function(t):
        for item in reversed(polynoms.items()):
            if t >= item[0]:
                a0, a1, a2, a3 = item[1]
                break

        return a0 + a1 * (t - item[0]) + a2 * (t - item[0]) ** 2 + a3 * (t - item[0]) ** 3

    return function

main.py

from matplotlib import pyplot as plt
import numpy as np

from interpolation import Values, build_interpolation

d = {
    0.0: Values(0, 0, 0, 0),
    1.0: Values(0, 1, 1, 0),
    2.0: Values(1, 1, 0, -1),
    3.0: Values(1, 0, 0, 0),

}


fun = build_interpolation(d)
figure, axis = plt.subplots(2, 2)
STEP = 0.01
x = np.arange(min(d), max(d), step=STEP)
y = np.array(list(map(fun, x)))

axis[0,0].plot(x, y)
axis[0,0].legend(["x", "y"])
axis[0,0].set_title("Coordinates")

nn = []
for i in range(len(x)-1):
    nn.append([(y[i+1][0] - y[i][0])/STEP, (y[i+1][1] - y[i][1])/STEP])

axis[1, 0].plot(x[:-1], nn)
axis[1, 0].legend(["x", "y"])
axis[1, 0].set_title("speed")

axis[1, 1].plot(y[:, 0], y[:, 1])
for item in d.items():
    X, Y = item[1].b[0], item[1].b[1]
    axis[1, 1].plot(X, Y, 'bo')
    axis[1, 1].annotate(f"({X}, {Y})",
                 (item[1].b[0], item[1].b[1]),
                textcoords="offset points",
                xytext=(0, 10),
                ha='center')

plt.show()