# -*- coding: iso-8859-7 -*-
from math import pi,atan2,sqrt
from var import dpt
class ThanSpline:
"A cubic Spline."
def splfun(self, ts):
"Evaluates the spline at th eparameter ts."
def calc(x, xt):
a = 3.0 * (x[k]-x[i]) / t[k]**2 - (2.0*xt[i]+xt[k]) / t[k]
b = 2.0 * (x[i]-x[k]) / t[k]**3 + (xt[i]+xt[k]) / t[k]**2
xs = x[i] + tt * (xt[i] + tt*(a + tt*b))
xts = xt[i] + tt * (2.0*a + tt*3.0*b)
return xs, xts
t = self.t
tt = ts
if tt <= 0.0:
tt = 0.0
# i = 1
elif (tt > self.tmax):
tt = self.tmax
# i = len(self.x)-1
tor = 0.0
for i in xrange(1, len(t)):
tor = tor + t[i]
if (tt <= tor):
tt = tt + t[i] - tor
break
else: assert 0, 'impossible error in sr splfun, lib indplt'
k = i
i = i-1
xs, xts = calc(self.x, self.xt)
ys, yts = calc(self.y, self.yt)
if self.z == None: return xs, ys
zs, zts = calc(self.z, self.zt)
return xs, ys, zs
def __init__(self, ic, xs, ys, zs=None):
"""Creates a spline through some xy points.
include 'indspl.inc'
integer*4 ns, ic ! could be integer*2
real*8 xs(*), ys(*), tmax1
integer*4 i ! could be integer*2
real*8 a(nmax), b(nmax), c(nmax), d(nmax), pr, e
character cret
c-----ic = 0 : 0
c = 1 : ""
c = 2 : "":
c
"""
def calc(x):
a = [0]; b = [0]; c = [0]; d = [0]
for i in xrange(1, len(x)-1):
a.append(t[i+1])
b.append(2.0 * (t[i]+t[i+1]))
c.append(t[i])
d.append(3.0 * (t[i]**2 * (x[i+1]-x[i]) + t[i+1]**2 * (x[i]-x[i-1])) / (t[i]*t[i+1]))
if ic == 0:
b[0] = 1.0
c[0] = 0.5
d[0] = 1.5 * (x[1]-x[0]) / t[1]
a.append(2.0)
b.append(4.0)
d.append(6.0 * (x[-1]-x[-2]) / t[-1])
xt = lse(a, b, c, d, 0.0)
else:
b[0] = 2.0 + 2.0 * t[-1] / t[1]
c[0] = t[-1] / t[1]
d[0] = 3.0 * (x[1]-x[0]) * t[-1] / t[1]**2 - pr * 3.0 * (x[-2]-x[-1])/t[-1]
e = pr
xt = lse(a, b, c, d, e)
xt.append(pr * xt[0])
return xt
pr = 1.0
if ic == 2: pr = -1.0
x = tuple(xs); y = tuple(ys)
if zs == None: z = None
else: z = tuple(zs)
t = [0.0]
for i in xrange(1, len(x)):
d = (x[i]-x[i-1])**2 + (y[i]-y[i-1])**2
if z != None: d += (z[i]-z[i-1])**2
t.append(sqrt(d))
self.t = t; self.tmax = sum(t)
self.x = x; self.xt = calc(x)
self.y = y; self.yt = calc(y)
self.z = z
if z != None: self.zt = calc(z)
#==========================================================================
def lse(a,b,c,d,e):
"Solve tridiagonal system of linear equations."
n = len(a)
i = n - 1
p = [0]*n
while True:
i -= 1
b[i]=b[i]-c[i]*a[i+1] / b[i+1]
d[i]=d[i]-c[i]*d[i+1] / b[i+1]
d[1]=d[1]-e*d[i+1] / b[i+1]
e=-e*a[i+1]/b[i+1]
if (b[i] == 0.0):
p[i-1]=d[i]/a[i]
i=i-1
d[i]=d[i]-b[i]*p[i]
if i <= 0:
p[1]=(d[0]-b[0]*p[0])/(b[0]+e)
i=0
break
i-=1
d[i]=d[i]-c[i]*p[i+1]
d[1]=d[1]-e*d[i+1]/c[i+1]
e=-e*a[i+1]/c[i+1]
if i <= 0:
p[0]=d[0]/(b[0]+e)
i=0
break
while True:
i += 1
if i < n-1:
if b[i+1] == 0.0:
p[i+1]=(d[i]-a[i]*p[i-1])/c[i]
i+=2
p[i]=(d[i]-a[i]*p[i-1])/b[i]
if i >= n-1: break
return p
#==========================================================================
def test2():
s = ThanSpline(0, (1,2,3,4,5,6,7,8,9), (1,2,4,9,16,9,4,2,1))
print "tmax=", s.tmax
tt = range(int(s.tmax))
xx = []; yy = []
for t in tt:
x, y = s.splfun(t)
xx.append(x); yy.append(y)
from p_gchart import ThanChart,vis
ch = ThanChart()
ch.curveAdd(xx, yy)
vis(ch)
def test3():
s = ThanSpline(0, (1001,1002,1003,1004,1005,1006,1007,1008,1009), (1,2,4,9,16,9,4,2,1), (1,2.5,3,4.5,5,6.5,7,8.5,9))
print "tmax=", s.tmax
tt = range(int(s.tmax))
xx = []; yy = []; zz = []
for t in tt:
x, y, z = s.splfun(t)
xx.append(x); yy.append(y); zz.append(z)
print x, y, z
from p_gchart import ThanChart,vis
ch = ThanChart()
ch.curveAdd(xx, yy)
ch.curveAdd(xx, zz, color="yellow")
vis(ch)
if __name__ == "__main__": test2(); test3()
|