51 lines
1.3 KiB
Python
51 lines
1.3 KiB
Python
|
from __future__ import division
|
||
|
|
|
||
|
|
import numpy as np
|
||
|
|
from scipy.interpolate import Akima1DInterpolator
|
||
|
|
|
||
|
|
|
||
|
|
def cubic_spline_3pts(x, y, T):
|
||
|
|
"""
|
||
|
|
Apparently scipy.interpolate.interp1d does not support
|
||
|
|
cubic spline for less than 4 points.
|
||
|
|
"""
|
||
|
|
x0, x1, x2 = x
|
||
|
|
y0, y1, y2 = y
|
||
|
|
|
||
|
|
x1x0, x2x1 = x1 - x0, x2 - x1
|
||
|
|
y1y0, y2y1 = y1 - y0, y2 - y1
|
||
|
|
_x1x0, _x2x1 = 1.0 / x1x0, 1.0 / x2x1
|
||
|
|
|
||
|
|
m11, m12, m13 = 2 * _x1x0, _x1x0, 0
|
||
|
|
m21, m22, m23 = _x1x0, 2.0 * (_x1x0 + _x2x1), _x2x1
|
||
|
|
m31, m32, m33 = 0, _x2x1, 2.0 * _x2x1
|
||
|
|
|
||
|
|
v1 = 3 * y1y0 * _x1x0 * _x1x0
|
||
|
|
v3 = 3 * y2y1 * _x2x1 * _x2x1
|
||
|
|
v2 = v1 + v3
|
||
|
|
|
||
|
|
M = np.array([[m11, m12, m13], [m21, m22, m23], [m31, m32, m33]])
|
||
|
|
v = np.array([v1, v2, v3]).T
|
||
|
|
k = np.array(np.linalg.inv(M).dot(v))
|
||
|
|
|
||
|
|
a1 = k[0] * x1x0 - y1y0
|
||
|
|
b1 = -k[1] * x1x0 + y1y0
|
||
|
|
a2 = k[1] * x2x1 - y2y1
|
||
|
|
b2 = -k[2] * x2x1 + y2y1
|
||
|
|
|
||
|
|
t = T[np.r_[T >= x0] & np.r_[T <= x2]]
|
||
|
|
t1 = (T[np.r_[T >= x0] & np.r_[T < x1]] - x0) / x1x0
|
||
|
|
t2 = (T[np.r_[T >= x1] & np.r_[T <= x2]] - x1) / x2x1
|
||
|
|
t11, t22 = 1.0 - t1, 1.0 - t2
|
||
|
|
|
||
|
|
q1 = t11 * y0 + t1 * y1 + t1 * t11 * (a1 * t11 + b1 * t1)
|
||
|
|
q2 = t22 * y1 + t2 * y2 + t2 * t22 * (a2 * t22 + b2 * t2)
|
||
|
|
q = np.append(q1, q2)
|
||
|
|
|
||
|
|
return t, q
|
||
|
|
|
||
|
|
|
||
|
|
def akima(X, Y, x):
|
||
|
|
spl = Akima1DInterpolator(X, Y)
|
||
|
|
return spl(x)
|