Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
executable file 381 lines (280 sloc) 11.6 KB
#!/usr/bin/env python
# Author: Andrew Jewett (jewett.aij at g mail)
# License: MIT License (See LICENSE.md)
# Copyright (c) 2017, California Institute of Technology
# All rights reserved.
g_program_name = __file__.split('/')[-1] # = 'interpolate_curve.py'
g_version_str = '0.3.0'
g_date_str = '2019-12-12'
g_usage_str = """
Usage:
""" + g_program_name + """ Ndesired [scale] [alpha] < coords_orig.raw > coords.raw
Example:
""" + g_program_name + """ 30117 3.0 0.5 < coords_orig.raw > coords.raw
"""
import sys
from math import *
import numpy as np
import bisect
## Tri Diagonal Matrix Algorithm(a.k.a Thomas algorithm) solver stolen from:
# https://gist.github.com/cbellei/8ab3ab8551b8dfc8b081c518ccd9ada9
# (also taken from https://gist.github.com/ofan666/1875903)
import numpy as np
## Tri Diagonal Matrix Algorithm(a.k.a Thomas algorithm) solver
def TDMAsolver(a, b, c, d):
'''
TDMA solver, a b c d can be NumPy array type or Python list type.
Solves a (non-cyclic) system of equations of the form:
a_i*x_{i-1} + b_i*x_i + c_i*x_{i+1} = d_i
where a_1=0, and c_n=0
refer to http://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm
and to http://www.cfd-online.com/Wiki/Tridiagonal_matrix_algorithm_-_TDMA_(Thomas_algorithm)
'''
nf = len(d) # number of equations
ac, bc, cc, dc = map(np.array, (a, b, c, d)) # copy arrays
for it in range(1, nf):
mc = ac[it-1]/bc[it-1]
bc[it] = bc[it] - mc*cc[it-1]
dc[it] = dc[it] - mc*dc[it-1]
xc = bc
xc[-1] = dc[-1]/bc[-1]
for il in range(nf-2, -1, -1):
xc[il] = (dc[il]-cc[il]*xc[il+1])/bc[il]
return xc
def CalcNaturalCubicSplineCoeffs(r, spline_exponent_alpha=0.5):
N = len(r)
assert(N >= 4)
D = len(r[0])
tcontrol = np.zeros(N)
# e_d2rdt2[d][i] is the second derivative of the spline at the ith
# control point (and in the dth direction)
#
# Once we have figured out e_d2rdt2[d][i], we can calculate the spline
# anywhere using:
# SplineEval(t, h_i,
# e_{i+1} / (6*h_i),
# e_i / (6*h_{i+1}),
# r_{i+1}/h_i - e_{i+1}*h_i,
# r_i/h_i - e_i*h_i)
e_d2rdt2 = np.zeros((D,N))
# We want to solve this system of equations for e_1, e_2, ..., e_{n-2}:
# (note: e_i is shorthand for e_d2rdt2[d][i])
#
# h_{i-1}*e_{i-1} + u_i*e_i + h_{i+1} * e_{i+1} = v_i
# where h_i, u_i and v_i are shorthand for:
#
# h_i = change in "time" parameter for the ith interval, which is usually:
# = |r_{i+1} - r_i |^alpha (alpha varies depending on settings)
# and
# u_i = 2 * (h_{i-1} + h_i )
# v_i = 6 * (b_i - b_{i-1})
# b_i = (r_i - r_{i-1}) / h_i
#
# ...subject to the constraints that the curve is straight at the endpoints:
# e_0 = 0 <-- first control point (indexing begins at 0)
# e_{n-1} = 0 <-- this is the last control point (indexing begins at 0)
h_dt = np.zeros(N-1) # h_dt[i] is the i'th time interval
# in the parameterization
b_drdt = np.zeros((N-1,D)) # b_drdt is a discrete version of the derivative
t_total=0.0
for i in range(0, N-1):
tcontrol[i] = t_total
sqr_distance_i_ip1 = 0.0
for d in range(0, D):
sqr_distance_i_ip1 += (r[i+1][d] - r[i][d])**2
h_dt[i] = sqr_distance_i_ip1**(0.5*spline_exponent_alpha)
for d in range(0, D):
b_drdt[i][d] = (r[i+1][d] - r[i][d]) / h_dt[i]
t_total += h_dt[i]
tcontrol[N-1] = t_total
a_coeff = np.zeros(N)
b_coeff = np.zeros(N)
c_coeff = np.zeros(N)
d_coeff = np.zeros((D,N))
for i in range(1, N-1):
# h_dt[i] is the difference in "time" in the parametric curve
# between pairs of control points. If spline_exponenent_alpha is 0
# then the time interval between control points is uniform. (Typically,
# "spline_exponent_alpha" is 0.5 for centripital Catmull-Rom splines.)
a_coeff[i] = h_dt[i-1]
b_coeff[i] = 2.0*(h_dt[i-1] + h_dt[i])
c_coeff[i] = h_dt[i]
for d in range(0, D):
d_coeff[d][i] = 6.0*(b_drdt[i][d] - b_drdt[i-1][d])
a_coeff[0] = 0.0
b_coeff[0] = 1.0
c_coeff[0] = 0.0
for d in range(0, D):
d_coeff[d][0] = 0.0
a_coeff[N-1] = 0.0
b_coeff[N-1] = 1.0
c_coeff[N-1] = 0.0
for d in range(0, D):
d_coeff[d][N-1] = 0.0
for d in range(0, D):
e_d2rdt2[d] = TDMAsolver(a_coeff, b_coeff, c_coeff, d_coeff[d])
# alternately, if that fails, try the matrix inverter that comes with numpy:
#M = np.zeros((N,N))
#for i in range(0,N):
# if i-1>=0:
# M[i][i-1] = a_coeff[i]
# M[i][i] = b_coeff[i]
# if i+1 < N:
# M[i][i+1] = c_coeff[i]
#for d in range(0, D):
# e_d2rdt2[d] = np.linalg.solve(M, d_coeff[d])
e_d2rdt2 = e_d2rdt2.transpose()
c3a = np.zeros((N, D))
c3b = np.zeros((N, D))
c1a = np.zeros((N, D))
c1b = np.zeros((N, D))
# faster to precompute these coefficients in advance:
for i in range(0, N-1):
# c3a = e_{i+1} / (6*h_i)
# c3b = e_i / (6*h_{i+1})
# c1a = r_{i+1}/h_i - e_{i+1}*h_i
# c1b = r_i / h_i - e_i*h_i)
c3a[i] = e_d2rdt2[i+1] / (6*h_dt[i])
c3b[i] = e_d2rdt2[i] / (6*h_dt[i])
c1a[i] = r[i+1]/h_dt[i] - e_d2rdt2[i+1]*h_dt[i]/6.0
c1b[i] = r[i]/h_dt[i] - e_d2rdt2[i]*h_dt[i]/6.0
# Return these spline coefficients to the caller.
# We can use these to quickly evaluate the spline repetatively later on
return c3a, c3b, c1a, c1b, tcontrol
def SplineEval(t, t_interval, c3a, c3b, c1a, c1b):
ta = t
tb = t_interval - t
return (c3a*ta*ta + c1a)*ta + (c3b*tb*tb + c1b)*tb
def SplineEvalD1(t, t_interval, c3a, c3b, c1a, c1b):
ta = t
tb = t_interval - t
return c3a*ta*ta*3.0 + c1a - (c3b*tb*tb*3.0 + c1b)
def SplineEvalD2(t, t_interval, c3a, c3b, c1a, c1b):
ta = t
tb = t_interval - t
return c3a*ta*6.0 + c3b*tb*6.0
def SplineInterpEval(t, c3a, c3b, c1a, c1b, tcontrol):
i = bisect.bisect(tcontrol, t) - 1
if i < 0: i = 0
if i >= len(tcontrol)-1: i = len(tcontrol)-2
return SplineEval(t-tcontrol[i], tcontrol[i+1]-tcontrol[i],
c3a[i], c3b[i], c1a[i], c1b[i])
def SplineInterpEvalD1(t, c3a, c3b, c1a, c1b, tcontrol):
i = bisect.bisect(tcontrol, t) - 1
if i < 0: i = 0
if i >= len(tcontrol)-1: i = len(tcontrol)-2
return SplineEvalD1(t-tcontrol[i], tcontrol[i+1]-tcontrol[i],
c3a[i], c3b[i], c1a[i], c1b[i])
def SplineInterpEvalD2(t, c3a, c3b, c1a, c1b, tcontrol):
i = bisect.bisect(tcontrol, t) - 1
if i < 0: i = 0
if i >= len(tcontrol)-1: i = len(tcontrol)-2
return SplineEvalD2(t-tcontrol[i], tcontrol[i+1]-tcontrol[i],
c3a[i], c3b[i], c1a[i], c1b[i])
def SplineCurvature2D(t, t_interval, c3a, c3b, c1a, c1b):
# first derivatives
x1,y1 = SplineEvalD1(t, t_interval, c3a, c3b, c1a, c1b)
# second derivatives
x2,y2 = SplineEvalD2(t, t_interval, c3a, c3b, c1a, c1b)
denom = pow((x1*x1 + y1*y1), 1.5)
curvature = (x1*y2 - x2*y1) / denom
return curvature
def SplineInterpCurvature2D(t, c3a, c3b, c1a, c1b, tcontrol):
i = bisect.bisect(tcontrol, t) - 1
if i < 0: i = 0
if i >= len(tcontrol)-1: i = len(tcontrol)-2
return SplineCurvature2D(t-tcontrol[i], tcontrol[i+1]-tcontrol[i],
c3a[i], c3b[i], c1a[i], c1b[i])
def ResampleCurve(x_orig, num_points, alpha=0.5):
"""
Given a list (or numpy array) of points in n-dimensional space that lie
along some curve, this function returns a new list of "num_points" points
which are uniformly distributed along the original curve. This is done
using cubic spline interpolation (which is tuned by the "alpha" parameter).
https://en.wikipedia.org/wiki/Cubic_Hermite_spline#Catmull%E2%80%93Rom_spline
Here "uniformly" means distributed at even intervals in the spline-
parameter-space, not in physical distance. (If the original points were
not uniformly distributed along the curve, then new points won't be either.)
This function has not been optimized for speed.
"""
assert(len(x_orig) >= 4)
num_dimensions = len(x_orig[0])
x_new = np.zeros((num_points, num_dimensions))
c3a, c3b, c1a, c1b, tcontrol = \
CalcNaturalCubicSplineCoeffs(x_orig, alpha)
tmin = 0.0
tmax = tcontrol[-1]
for i in range(0, num_points):
t = tmin + i*((tmax - tmin) / (num_points-1))
x_new[i] = SplineInterpEval(t, c3a, c3b, c1a, c1b, tcontrol)
return x_new
class InputError(Exception):
""" A generic exception object containing a string for error reporting.
(Raising this exception implies that the caller has provided
a faulty input file or argument.)
"""
def __init__(self, err_msg):
self.err_msg = err_msg
def __str__(self):
return self.err_msg
def __repr__(self):
return str(self)
def main():
try:
####### Main Code Below: #######
sys.stderr.write(g_program_name+' v'+g_version_str+' '+g_date_str+'\n')
spline_exponent_alpha = 0.5
use_linear_interpolation = False
# Parse the argument list:
if len(sys.argv) <= 1:
raise InputError("Missing arguments\n"+g_usage_str+"\n")
n_new = int(sys.argv[1])
if len(sys.argv) > 2:
scale = float(sys.argv[2])
else:
scale = 1.0
if len(sys.argv) > 3:
spline_exponent_alpha = float(sys.argv[3])
coords_orig = []
lines = sys.stdin.readlines()
for line in lines:
tokens = line.split()
if (len(tokens) > 0):
coords_orig.append(list(map(float, tokens)))
g_dim = len(tokens)
n_orig = len(coords_orig)
x_orig = np.array(coords_orig)
if n_orig < 2:
raise InputError("Input file contains less than two lines of coordinates.")
if n_orig < 4:
use_linear_interpolation = True
if n_new < 2:
raise InputError("Output file will contain less than two lines of coordinates.")
#coords_new = [[0.0 for d in range(0, g_dim)] for i in range(0, n_new)]
x_new = np.zeros((n_new, g_dim))
if use_linear_interpolation:
for i_new in range(0, n_new):
I_orig = (i_new) * (float(n_orig-1) / float(n_new-1))
i_orig = int(floor(I_orig))
i_remainder = I_orig - i_orig
if (i_new < n_new-1):
for d in range(0, g_dim):
x_new[i_new][d] = scale*(x_orig[i_orig][d]
+
i_remainder*(x_orig[i_orig+1][d]-
x_orig[i_orig][d]))
else:
for d in range(0, g_dim):
x_new[i_new][d] = scale*x_orig[n_orig-1][d]
else:
x_new = ResampleCurve(x_orig, n_new, spline_exponent_alpha)
# print the coordates
for i in range(0, n_new):
for d in range(0, g_dim-1):
sys.stdout.write(str(x_new[i][d]*scale) + ' ')
sys.stdout.write(str(x_new[i][g_dim-1]*scale) + "\n")
except (ValueError, InputError) as err:
sys.stderr.write('\n' + 'Error:\n\n' + str(err) + '\n')
sys.exit(1)
if __name__ == '__main__':
main()
You can’t perform that action at this time.