From 25e18626d89d1333a908eface1f5c8663dd59e85 Mon Sep 17 00:00:00 2001 From: josch Date: Mon, 31 Mar 2014 08:39:09 +0200 Subject: [PATCH] initial commit --- fitbspline.py | 233 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 233 insertions(+) create mode 100644 fitbspline.py diff --git a/fitbspline.py b/fitbspline.py new file mode 100644 index 0000000..f48792c --- /dev/null +++ b/fitbspline.py @@ -0,0 +1,233 @@ +#!/usr/bin/env python + +import sys +from math import sqrt +import numpy as np +import matplotlib.pyplot as plt +from scipy import interpolate +from itertools import tee, izip +from matplotlib.patches import Polygon +from matplotlib.collections import PatchCollection +import matplotlib + +def pairwise(iterable): + "s -> (s0,s1), (s1,s2), (s2,s3), ..." + a, b = tee(iterable, 2) + next(b, None) + return izip(a, b) + +def triplewise(iterable): + "s -> (s0,s1,s2), (s1,s2,s3), (s2,s3,s4), ..." + a,b,c = tee(iterable, 3) + next(b, None) + next(c, None) + next(c, None) + return izip(a,b,c) + +# using barycentric coordinates +def ptInTriangle(p, p0, p1, p2): + A = 0.5 * (-p1[1] * p2[0] + p0[1] * (-p1[0] + p2[0]) + p0[0] * (p1[1] - p2[1]) + p1[0] * p2[1]); + sign = -1 if A < 0 else 1; + s = (p0[1] * p2[0] - p0[0] * p2[1] + (p2[1] - p0[1]) * p[0] + (p0[0] - p2[0]) * p[1]) * sign; + t = (p0[0] * p1[1] - p0[1] * p1[0] + (p0[1] - p1[1]) * p[0] + (p1[0] - p0[0]) * p[1]) * sign; + return s >= 0 and t >= 0 and (s + t) <= 2 * A * sign; + +def getxing(p0, p1, p2, p3): + ux = p1[0]-p0[0] + uy = p1[1]-p0[1] + vx = p2[0]-p3[0] + vy = p2[1]-p3[1] + # get multiplicity of u at which u meets v + a = vy*ux-vx*uy + if a == 0: + # lines are parallel and never meet + return None + s = (vy*(p3[0]-p0[0])+vx*(p0[1]-p3[1]))/a + if s < 1.0: + return (p0[0]+s*ux, p0[1]+s*uy) + else: + return None + +# the line p0-p1 is the upper normal to the path +# the line p2-p3 is the lower normal to the path +# +# | | | +# p0--------|--------p1 +# | | | +# | | | +# p3--------|--------p2 +# | | | +def ptInQuadrilateral(p, p0, p1, p2, p3): + # it might be that the two normals cross at some point + # in that case the two triangles are created differently + cross = getxing(p0, p1, p2, p3) + #if cross: + # return ptInTriangle(p, p0, cross, p3) or ptInTriangle(p, p2, cross, p1) + #else: + # return ptInTriangle(p, p0, p1, p2) or ptInTriangle(p, p2, p3, p0) + return ptInTriangle(p, p0, p1, p2) or ptInTriangle(p, p2, p3, p0) + +def find_coeffs(pa, pb): + matrix = [] + for p1, p2 in zip(pa, pb): + matrix.append([p1[0], p1[1], 1, 0, 0, 0, -p2[0]*p1[0], -p2[0]*p1[1]]) + matrix.append([0, 0, 0, p1[0], p1[1], 1, -p2[1]*p1[0], -p2[1]*p1[1]]) + + A = np.matrix(matrix, dtype=np.float) + B = np.array(pb).reshape(8) + + #res = np.dot(np.linalg.inv(A), B) + res = np.dot(np.linalg.inv(A.T * A) * A.T, B) + return np.array(res).reshape(8) + +def main(): + width = 2/5.0 + halfwidth = width/2.0 + x = [] + y = [] + with open(sys.argv[1]) as f: + for l in f: + a,b = l.split() + x.append(float(a)) + y.append(float(b)) + tck,u = interpolate.splprep([x,y],s=10) + unew = np.arange(0,1.1,0.1) + out = interpolate.splev(unew,tck) + heights = [] + offs = [] + height = 0.0 + for (ax,ay),(bx,by) in pairwise(zip(*out)): + s = ax-bx + t = ay-by + l = sqrt(s*s+t*t) + offs.append(height) + height += l + heights.append(l) + # the border of the first segment is just perpendicular to the path + cx = -out[1][1]+out[1][0] + cy = out[0][1]-out[0][0] + cl = sqrt(cx*cx+cy*cy)/halfwidth + dx = out[1][1]-out[1][0] + dy = -out[0][1]+out[0][0] + dl = sqrt(dx*dx+dy*dy)/halfwidth + px = [out[0][0]+cx/cl] + py = [out[1][0]+cy/cl] + qx = [out[0][0]+dx/dl] + qy = [out[1][0]+dy/dl] + for (ubx,uby),(ux,uy),(uax,uay) in triplewise(zip(*out)): + # get adjacent line segment vectors + ax = ux-ubx + ay = uy-uby + bx = uax-ux + by = uay-uy + # normalize length + al = sqrt(ax*ax+ay*ay) + bl = sqrt(bx*bx+by*by) + ax = ax/al + ay = ay/al + bx = bx/bl + by = by/bl + # get vector perpendicular to sum + cx = -ay-by + cy = ax+bx + cl = sqrt(cx*cx+cy*cy)/halfwidth + px.append(ux+cx/cl) + py.append(uy+cy/cl) + # and in the other direction + dx = ay+by + dy = -ax-bx + dl = sqrt(dx*dx+dy*dy)/halfwidth + qx.append(ux+dx/dl) + qy.append(uy+dy/dl) + # the border of the last segment is just perpendicular to the path + cx = -out[1][-1]+out[1][-2] + cy = out[0][-1]-out[0][-2] + cl = sqrt(cx*cx+cy*cy)/halfwidth + dx = out[1][-1]-out[1][-2] + dy = -out[0][-1]+out[0][-2] + dl = sqrt(dx*dx+dy*dy)/halfwidth + px.append(out[0][-1]+cx/cl) + py.append(out[1][-1]+cy/cl) + qx.append(out[0][-1]+dx/dl) + qy.append(out[1][-1]+dy/dl) + quads = [] + patches = [] + #for (p0x,p0y,p1x,p1y),(p3x,p3y,p2x,p2y) in pairwise(zip(px,py,qx,qy)): + for (p3x,p3y,p2x,p2y),(p0x,p0y,p1x,p1y) in pairwise(zip(px,py,qx,qy)): + quads.append(((p0x,p0y),(p1x,p1y),(p2x,p2y),(p3x,p3y))) + polygon = Polygon(((p0x,p0y),(p1x,p1y),(p2x,p2y),(p3x,p3y)), True) + patches.append(polygon) + containingquad = [] + for pt in zip(x,y): + # for each point, find the quadrilateral that contains it + found = [] + for i,(p0,p1,p2,p3) in enumerate(quads): + if ptInQuadrilateral(pt,p0,p1,p2,p3): + found.append(i) + if found: + if len(found) > 2: + print found + containingquad.append(found) + else: + print "can't find quad for point" + containingquad.append(None) + #exit(1) + print containingquad + trans = [] + print width, height + for off,h,srcquad in zip(offs,heights,quads): + #targetquad = ((0,height-off),(width,height-off),(width,height-off-h),(0,height-off-h)) + targetquad = ((0,off+h),(width,off+h),(width,off),(0,off)) + trans.append(find_coeffs(srcquad,targetquad)) + patches.append(Polygon(targetquad,True)) + tx = [] + ty = [] + #targetquad = (0,height),(width,height),(width,0),(0,0) + #srcquad = (min(x),max(y)),(max(x),max(y)),(max(x),min(y)),(min(x),min(y)) + #trans = find_coeffs(srcquad,targetquad) + #for (rx,ry) in zip(x,y): + # a,b,c,d,e,f,g,h = trans + # u = (a*rx + b*ry + c)/(g*rx + h*ry + 1) + # v = (d*rx + e*ry + f)/(g*rx + h*ry + 1) + # tx.append(u) + # ty.append(v) + assert len(containingquad) == len(x) == len(y) + assert len(out[0]) == len(out[1]) == len(px) == len(py) == len(qx) == len(qy) == len(quads)+1 == len(heights)+1 == len(offs)+1 == len(trans)+1 + for (rx,ry),l in zip(zip(x,y),containingquad): + if not l: + continue + for i in l[:1]: + a,b,c,d,e,f,g,h = trans[i] + #den = -a*e+a*h*ry+b*d-b*g*ry-d*h*rx+e*g*rx + #tx.append((-b*f+b*ry+c*e-c*h*ry-e*rx+f*h*rx)/den) + #ty.append((a*f-a*ry-c*d+c*g*ry+d*rx-f*g*rx)/den) + u = (a*rx + b*ry + c)/(g*rx + h*ry + 1) + v = (d*rx + e*ry + f)/(g*rx + h*ry + 1) + tx.append(u) + ty.append(v) + sx = [] + sy = [] + for (((ax,ay),(bx,by)),(a,b,c,d,e,f,g,h)) in zip(pairwise(zip(*out)),trans): + u = (a*ax + b*ay + c)/(g*ax + h*ay + 1) + v = (d*ax + e*ay + f)/(g*ax + h*ay + 1) + sx.append(u) + sy.append(v) + u = (a*bx + b*by + c)/(g*bx + h*by + 1) + v = (d*bx + e*by + f)/(g*bx + h*by + 1) + sx.append(u) + sy.append(v) + colors = 100*np.random.rand(len(patches)/2)+100*np.random.rand(len(patches)/2) + p = PatchCollection(patches, cmap=matplotlib.cm.jet, alpha=0.4) + p.set_array(np.array(colors)) + plt.figure() + plt.axes().set_aspect('equal') + #plt.axhspan(0, height, xmin=0, xmax=width) + fig, ax = plt.subplots() + ax.add_collection(p) + ax.set_aspect('equal') + plt.plot(x,y,out[0],out[1],px,py,qx,qy,tx,ty,sx,sy) + #plt.plot(tx,ty) + plt.show() + +if __name__ == '__main__': + main()