#!/usr/bin/python # tzspline.py # --copyright-- Copyright 2007 (C) Tranzoa, Co. All rights reserved. Warranty: You're free and on your own here. This code is not necessarily up-to-date or of public quality. # --url-- http://www.tranzoa.net/tzpython/ # --email-- pycode is the name to send to. tranzoa.com is the place to send to. # --bodstamps-- # May 5, 2007 bar # October 25, 2007 bar get a_spliner code from the web that's working (i believe) # October 26, 2007 bar add the tree printout code # November 1, 2007 bar a_smoother # November 18, 2007 bar turn on doxygen # November 27, 2007 bar insert boilerplate copyright # December 22, 2007 bar handle vertical movement # May 17, 2008 bar email adr # September 14, 2009 bar comment # July 6, 2011 bar comment # November 29, 2011 bar pyflake cleanup # May 27, 2012 bar doxygen namespace # August 19, 2015 bar pyflakes # --eodstamps-- ## \file # \namespace tzpython.tzspline # # # # Spline stuff # # # # a_smoother may have tiny anomalies at the points - hikes.py puts out glitches - see what happens when the big points get shoulders to protect 'em. # # !!!! The old code doesn't work - the slope is wrong when crossing a point because the phoney points are extrapolated dumbly and pick up the wrong slope # # !!!! The proper control points must be generated from the points given # # Curious about another way: # # Minimize the acceleration: # # Specifically, create a mid-point between each pair of control points and adjust the mid-points up and down. # # Assume that 3 control points are named: A C E and that the mid point between A and C is b, and 'tween C and E is d. # So: AbCdE are the points. # Initialize b and d (and the other mid-points) using linear interpolation. e.g. b = (A + C) / 2 # # If the maximum acceleration (e.g. ((d - C) - (C - b)) ) of all control possible points is at C, then do one of these: # # d = ( E + C + C + C - b) / 3 # or: # b = (-d + C + C + C + A) / 3 # # Until nothing better can be done with any control point. # Then double 'em up again - until there are enough points on the smoothed line. # # Or is the idea to minimize the total acceleration? (nope - don't want any huge acceleration, anywhere) # Or the square of the acceleration? # # # Javascript spline code in this page: # http://www.akiti.ca/CubicSpline.html # # Nice small interpolation code (linear, cosine, cubic, hermite): # http://paulbourke.net/miscellaneous/interpolation/ # # import math from types import ListType, TupleType if False : def _cccc_calc(a, i, f) : v0 = a[i] v3 = a[i + 1] v1 = v0 + ((v0 - a[i - 1]) / 3.0) # extrapolate forward a 1/3 of the way from the point before this pair v2 = v3 + ((v3 - a[i + 2]) / 3.0) # extrapolate back a 1/3 of the way from the next point after this pair A = v3 - (3.0 * v2) + (3.0 * v1) - v0 B = (3.0 * v2) - (6.0 * v1) + (3.0 * v0) C = (3.0 * v1) - (3.0 * v0) D = v0 ff = f * f fff = f * ff return(A * fff + B * ff + C * f + D) # # from util\stuff\splice.bas # def _calc(a, i, f) : v0 = a[i] v3 = a[i + 1] v1 = v0 + ((v0 - a[i - 1]) / 3.0) # extrapolate forward a 1/3 of the way from the point before this pair v2 = v3 + ((v3 - a[i + 2]) / 3.0) # extrapolate back a 1/3 of the way from the next point after this pair if False : v0 = a[i - 1] v1 = a[i] v2 = a[i + 1] v3 = a[i + 2] ff = f * f f1 = 1.0 - f ff1 = f1 * f1 A = f1 * ff1 B = 3 * f * ff1 C = 3 * ff * f1 D = f * ff return(A * v0 + B * v1 + C * v2 + D * v3) def spline_point(ax, ay, fx) : """ Given an array of ax and ay values, and given a float 1.0 <= fx <= len(ay) - 2.0, return the y value at fx. """ ln = len(ay) if not ax : ax = range(ln) if (not isinstance(ax, ListType)) and (not isinstance(ax, TupleType)) : ax = [ x + ax for x in xrange(ln) ] if len(ax) != ln : raise ValueError, "Different length of ax and ay array" ifx = int(fx) if abs(fx - float(ifx)) < 0.001 : fx = float(ifx) if not (1.0 <= fx <= ln - 2.0 + 0.001) : s = "fx is out of range - not (1.0 <= %f <= %f)" % ( fx, float(ln) - 2.0 ) raise IndexError, s # if abs(float(ifx) - fx) < 0.001 : # return( ( ax[ifx], ay[ifx] ) ) fx -= float(ifx) x = _calc(ax, ifx, fx) y = _calc(ay, ifx, fx) return( ( x, y ) ) pass # # # Modified version of code snagged at http://cars9.uchicago.edu/software/python/ # http://cars.uchicago.edu/software/pub/python_epics.tar # # class a_spliner : """ Given two parallel arrays, one with X coordinates, the other with Y, create an object that gives "spline" Y values for arbitrary X values. """ def __init__(me, x_array, y_array, begin_slope = None, end_slope = None) : if len(x_array) != len(y_array) : raise "a_spliner: Arrays not same length: x[%u] y[%u]!" % ( len(x_array), len(y_array) ) me.x_vals = [ float(v) for v in x_array ] me.y_vals = [ float(v) for v in y_array ] me.begin_slope = begin_slope me.end_slope = end_slope me.calc_ypp() def calc_ypp(me): x_vals = me.x_vals y_vals = me.y_vals n = len(x_vals) y2_vals = [ 0.0 ] * n u = [ 0.0 ] * (n - 1) if me.begin_slope != None : u[0] = (3.0 / (x_vals[1] - x_vals[0])) * ((y_vals[1] - y_vals[0]) / (x_vals[1] - x_vals[0]) - me.begin_slope) y2_vals[0] = -0.5 else: u[0] = 0.0 y2_vals[0] = 0.0 # natural spline x = x_vals[0] y = y_vals[0] nx = x_vals[1] ny = y_vals[1] for i in xrange(1, n - 1) : nnx = x_vals[i + 1] nny = y_vals[i + 1] sig = (nx - x) / (nnx - x) p = (sig * y2_vals[i-1]) + 2.0 y2_vals[i] = (sig - 1.0) / p u[i] = ((nny - ny) / (nnx - nx)) - ((ny - y) / (nx - x)) u[i] = (6.0 * u[i] / (nnx - x) - (sig * u[i-1])) / p x = nx y = ny nx = nnx ny = nny if me.end_slope != None : qn = 0.5 un = (3.0 / (x_vals[n-1] - x_vals[n-2])) * (me.end_slope - (y_vals[n-1] - y_vals[n-2]) / (x_vals[n-1] - x_vals[n-2])) else: qn = 0.0 un = 0.0 # natural spline y2_vals[n-1] = (un - (qn * u[n-2])) / ((qn * y2_vals[n-1]) + 1.0) rng = range(n - 1) rng.reverse() for k in rng: # backsubstitution step y2_vals[k] = (y2_vals[k] * y2_vals[k+1]) + u[k] me.y2_vals = y2_vals # compute approximation def __call__(me, arg): if isinstance(arg, ListType) or isinstance(arg, TupleType) : return(map(me.call, arg)) else: return(me.call(arg)) pass def call(me, x): "Evaluate the spline, assuming x is a scalar." # if out of range, return endpoint if x <= me.x_vals[0] : return(me.y_vals[0]) if x >= me.x_vals[-1] : return(me.y_vals[-1]) # # binary search to find the x index above the arg # pos = 0 lo = 0 hi = len(me.x_vals) while lo < hi : pos = (lo + hi) / 2 if x >= me.x_vals[pos] : pos += 1 lo = pos else : hi = pos pass h = me.x_vals[pos] - me.x_vals[pos - 1] if h <= 0.0 : es = "a_spliner: Bad X positions : %f %f!" % ( me.x_vals[pos], me.x_vals[pos - 1] ) raise es a = ( me.x_vals[pos ] - x) / h b = (x - me.x_vals[pos - 1] ) / h return( (a * me.y_vals[pos - 1]) + (b * me.y_vals[pos ]) + ( ((a * a * a - a) * me.y2_vals[pos - 1]) + ((b * b * b - b) * me.y2_vals[pos ]) ) * h * h / 6.0 ) pass # a_spliner # # # http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/457658 # # class Interpolator: def __init__(self, name, func, points, deriv=None): self.name = name # used for naming the C function self.intervals = intervals = [ ] # Generate a cubic spline for each interpolation interval. for u, v in map(None, points[:-1], points[1:]): FU, FV = func(u), func(v) # adjust h as needed, or pass in a derivative function if deriv == None: h = 0.01 DU = (func(u + h) - FU) / h DV = (func(v + h) - FV) / h else: DU = deriv(u) DV = deriv(v) denom = (u - v)**3 A = ((-DV - DU) * v + (DV + DU) * u + 2 * FV - 2 * FU) / denom B = -((-DV - 2 * DU) * v**2 + u * ((DU - DV) * v + 3 * FV - 3 * FU) + 3 * FV * v - 3 * FU * v + (2 * DV + DU) * u**2) / denom C = (- DU * v**3 + u * ((- 2 * DV - DU) * v**2 + 6 * FV * v - 6 * FU * v) + (DV + 2 * DU) * u**2 * v + DV * u**3) / denom D = -(u *(-DU * v**3 - 3 * FU * v**2) + FU * v**3 + u**2 * ((DU - DV) * v**2 + 3 * FV * v) + u**3 * (DV * v - FV)) / denom intervals.append((u, A, B, C, D)) def __call__(self, x): def getInterval(x, intervalList): # run-time proportional to the log of the length # of the interval list n = len(intervalList) if n < 2: return intervalList[0] n2 = n / 2 if x < intervalList[n2][0]: return getInterval(x, intervalList[:n2]) else: return getInterval(x, intervalList[n2:]) # Tree-search the intervals to get coefficients. u, A, B, C, D = getInterval(x, self.intervals) # Plug coefficients into polynomial. return ((A * x + B) * x + C) * x + D def c_code(self): """Generate C code to efficiently implement this interpolator. Run the C code through 'indent' if you need it to be legible.""" def codeChoice(intervalList): n = len(intervalList) if n < 2: return ("A=%.10e;B=%.10e;C=%.10e;D=%.10e;" % intervalList[0][1:]) n2 = n / 2 return ("if (x < %.10e) {%s} else {%s}" % (intervalList[n2][0], codeChoice(intervalList[:n2]), codeChoice(intervalList[n2:]))) return ("double interpolator_%s(double x) {" % self.name + "double A,B,C,D;%s" % codeChoice(self.intervals) + "return ((A * x + B) * x + C) * x + D;}") # # # Print out a decision tree to find the maximum and minimum values that interpolated values may take # if interpolation is done to make sure that there are not gross swings away from the points # and to make sure that monotonically increasing or decreasing Y values stay that way in the interpolations. # # def play_with_something() : import re class a_choice : def __init__(me, order, h, l) : me.order = order me.h = h me.l = l def __str__(me) : return("%s %s %s" % ( me.order, me.h, me.l ) ) pass # a_choice def _unique(ca) : cnt = {} for c in ca : cnt[c.h + c.l] = True return(len(cnt)) def _without(regxs, regx) : return( [ r for r in regxs if regx.pattern != r.pattern ] ) def _split(indent, regxs, ca, tf = "\\-") : if ca : ind = indent[0:-1] print "%s|" % ( ind ) if _unique(ca) == 1 : print "%s%s %s" % ( ind, tf, str([ str(c) for c in ca ]) ) return best = len(ca) for regx in regxs : fnd = [] nfn = [] for c in ca : if regx.search(c.order) : fnd.append(c) else : nfn.append(c) pass if (len(fnd) and len(nfn)) or (len(ca) == 1) : v = abs(_unique(fnd) - _unique(nfn)) if best > v : best = v brx = regx bfnd = fnd bnfn = nfn pass pass ind = indent[0:-1] print "%s%s %s-\\" % ( ind, tf, brx.pattern) _split(indent + " |", _without(regxs, brx), bfnd, "+-T") _split(indent + " ", _without(regxs, brx), bnfn, "\-F") pass def find_cmps(ca) : lts = {} for c in ca : for i in list(c.order) : lts[i] = True pass regxs = [] for c1 in lts.keys() : for c2 in lts.keys() : if c1 != c2 : regxs.append(re.compile(c1 + ".*" + c2)) pass pass _split("", regxs, ca) # Numbers are 1 2 3 4, left to right # 2 = left middle number is limit # 3 = right middle number is limit # w = limit is 2 + 2 - 1 # e = limit is 3 + 3 - 4 # M = max(w, e) # m = min(w, e) choices = [ # max min a_choice('1234', 'h2', 'l3'), a_choice('1243', 'h2', 'lm'), a_choice('1324', 'he', 'lw'), a_choice('1342', 'he', 'lw'), a_choice('1423', 'h2', 'lm'), a_choice('1432', 'h3', 'lw'), a_choice('2134', 'hM', 'l3'), a_choice('2143', 'hw', 'le'), a_choice('2314', 'hM', 'l3'), a_choice('2341', 'hM', 'l3'), a_choice('2413', 'hw', 'le'), a_choice('2431', 'hw', 'le'), a_choice('3124', 'he', 'lw'), a_choice('3142', 'he', 'lw'), a_choice('3214', 'hM', 'l2'), a_choice('3241', 'hM', 'l2'), a_choice('3412', 'he', 'lw'), a_choice('3421', 'hM', 'l2'), a_choice('4123', 'h2', 'le'), a_choice('4132', 'h3', 'lm'), a_choice('4213', 'hw', 'le'), a_choice('4231', 'hw', 'le'), a_choice('4312', 'h3', 'lm'), a_choice('4321', 'h3', 'l2'), ] find_cmps(choices) pass def smooth_max_min_of_4_y_values(y1, y2, y3, y4) : """ Note: this works find except that it can create butterflies, 'cause it doesn't know that the angle coming in to y2 is not from y1, but rather has overshot y2 and is coming back. Oh well. Given 4 Y values, return the maximum and minimum Y values that a smooth curve going through the points should have in Y between the middle (y2 and y3) points. These max/min values can be used to minimize gross swings beyond the bounds of reality. """ return( ( max(y2, y3), min(y2, y3) ) ) if y1 >= y4 : if y1 >= y2 : if y3 >= y2 : if y3 >= y4 : return( ( y3 + y3 - y4, y2 + y2 - y1 ) ) return( ( y3, y2 + y2 - y1 ) ) elif y3 >= y4 : return( ( y2, y3 ) ) return( ( y2, min(y2 + y2 - y1, y3 + y3 - y4) ) ) elif y1 >- y3 : if y3 >= y4 : return( ( max(y2 + y2 - y1, y3 + y3 - y4), y3 ) ) return( ( y2 + y2 - y1, y3 + y3 - y4 ) ) elif y3 >= y2 : return( ( max(y2 + y2 - y1, y3 + y3 - y4), y2 ) ) return( ( max(y2 + y2 - y1, y3 + y3 - y4), y3 ) ) elif y1 >= y2 : if y1 >= y3 : if y3 >= y2 : return( ( y3, min(y2 + y2 - y1, y3 + y3 - y4) ) ) return( ( y2, y3 + y3 - y4 ) ) elif y3 >= y4 : return( ( y3 + y3 - y4, y2 + y2 - y1 ) ) return( ( y3, min(y2 + y2 - y1, y3 + y3 - y4) ) ) elif y3 >= y2 : if y3 >= y4 : return( ( max(y2 + y2 - y1, y3 + y3 - y4), y2 ) ) return( ( y3, y2 ) ) elif y3 >= y4 : return( ( max(y2 + y2 - y1, y3 + y3 - y4), y3 ) ) return( ( y2 + y2 - y1, y3 + y3 - y4 ) ) class a_smoother : """ Given two parallel arrays, one with X coordinates, the other with Y, create an object that gives "smooth" Y values for arbitrary X values. """ def __init__(me, x_array, y_array) : if len(x_array) != len(y_array) : raise "a_smoother: Arrays not same length: x[%u] y[%u]!" % ( len(x_array), len(y_array) ) me.x_vals = [] me.y_vals = [] if x_array : px = x_array[0] ty = y_array[0] tc = 1.0 for i in xrange(len(x_array)) : x = x_array[i] y = y_array[i] if x == px : ty += y # average the Y values for multiple-same X values tc += 1.0 else : me.x_vals.append(px) me.y_vals.append(ty / tc) px = x ty = y tc = 1.0 i += 1 me.x_vals.append(px) me.y_vals.append(ty / tc) me.calc_mm() me.sig_mult = (1.0 + math.exp((1.0 - 0.5) * -8.0)) def calc_mm(me): x_vals = me.x_vals y_vals = me.y_vals me.west_yd = [ 0.0 for x in x_vals ] me.east_yd = [ 0.0 for x in x_vals ] if len(x_vals) : if len(x_vals) < 2 : me.west_yd[0] = me.east_yd[0] = y_vals[0] else : x_vals.insert(0, x_vals[1] + x_vals[1] - x_vals[0]) y_vals.insert(0, y_vals[1] + y_vals[1] - y_vals[0]) x_vals.append(x_vals[-1] + x_vals[-1] - x_vals[-2]) y_vals.append(y_vals[-1] + y_vals[-1] - y_vals[-2]) y2 = y_vals[0] y3 = y_vals[1] y4 = y_vals[2] for i in xrange(1, len(y_vals) - 2) : y1 = y2 y2 = y3 y3 = y4 y4 = y_vals[i + 2] ( mx, mn ) = smooth_max_min_of_4_y_values(y1, y2, y3, y4) # print i - 1, mn, mx, y1, y2, y3, y4 d = (x_vals[i + 1] - x_vals[i]) y = y_vals[i ] + (d * ((y_vals[i ] - y_vals[i - 1]) / (x_vals[i ] - x_vals[i - 1]))) me.east_yd[i - 1] = max(mn, min(mx, y)) - y_vals[i ] y = y_vals[i + 1] - (d * ((y_vals[i + 2] - y_vals[i + 1]) / (x_vals[i + 2] - x_vals[i + 1]))) me.west_yd[i - 1] = max(mn, min(mx, y)) - y_vals[i + 1] me.x_vals = x_vals[1:-1] me.y_vals = y_vals[1:-1] pass # print me.west_yd # print me.east_yd pass pass # compute approximation def __call__(me, arg): if isinstance(arg, ListType) or isinstance(arg, TupleType) : return(map(me.call, arg)) else: return(me.call(arg)) pass def call(me, x): "Evaluate the smoother, assuming x is a scalar." # if out of range, return endpoint if x <= me.x_vals[0] : return(me.y_vals[0]) if x >= me.x_vals[-1] : return(me.y_vals[-1]) # # binary search to find the x index above the arg # pos = 0 lo = 0 hi = len(me.x_vals) while lo < hi : pos = (lo + hi) / 2 if x >= me.x_vals[pos] : pos += 1 lo = pos else : hi = pos pass h = me.x_vals[pos] - me.x_vals[pos - 1] if h <= 0.0 : es = "a_smoother: Bad X positions : %f %f!" % ( me.x_vals[pos], me.x_vals[pos - 1] ) raise es pos -= 1 b = (x - me.x_vals[pos]) / h # # Note: rather than linearly morphing from fy to by, it might be interesting to make the right hand multipliers in these statements follow a sigmoid function. # blah # # b = me.sig_mult / (1.0 + math.exp((b - 0.5) * -8.0)) fy = (me.y_vals[pos ] + (me.east_yd[pos] * b )) * (1 - b) by = (me.y_vals[pos + 1] + (me.west_yd[pos] * (1 - b))) * b return(fy + by) pass # a_smoother # # # Test code. # # if __name__ == '__main__' : import sys import TZCommandLineAtFile del(sys.argv[0]) TZCommandLineAtFile.expand_at_sign_command_line_files(sys.argv) # play_with_something() ys = [ 0, 2, 5, 2, 10, 5 ] xs = [ 0, 1, 2, 7, 8, 9 ] # xs = [ 32, 33, 40, 46, 47, 54, 61, 62, 69, 75 ] # xs = [ 32.0, 33.0, 34.0, 35.0, 36.0, 37.0, 38.0, 39.0, 40.0, 41.0 ] # ys = [ 122.3, 122.4, 122.5, 122.6, 122.601, 122.602, 122.7, 122.8, 122.85, 122.9 ] # for x in xrange(1, len(ys) - 2) : # print x, smooth_max_min_of_4_y_values(ys[x - 1], ys[x], ys[x + 1], ys[x + 2]) sp = a_smoother(xs, ys) i = xs[0] - 1.0 pv = 0.0 while i < xs[-1] + 1.0 : v = sp(i) print i, v, # if v < pv : # print "ouch", pv = v print i += 0.25 x = [ ii * 0.3 for ii in xrange(10) ] y = [ math.cos(ii * 0.3) for ii in xrange(10) ] sp = a_smoother(x, y) # , True, True) v = [ 0.5 ] if sys.argv : v = [ float(a) for a in sys.argv ] print sp(v), [ math.cos(c) for c in v ] # # # # eof