diff --git a/python/helicalscan.py b/python/helicalscan.py index 4aa7b57..add2ea3 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -14,8 +14,83 @@ import os, sys, json import numpy as np import matplotlib as mpl import matplotlib.pyplot as plt +import mpl_toolkits.mplot3d as plt3d +import matplotlib.animation as anim + from utilities import * +#ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y') +#plot coordinates: X Y Z +#data Z X Y + +class Trf: + #https://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector + @staticmethod + def rotZ(rad): + """ Return matrix for rotating about the z-axis by 'radians' radians """ + c = np.cos(rad) + s = np.sin(rad) + m=np.identity(4) + m[0:2,0:2]=((c, -s),(s, c)) + return m + + @staticmethod + def rotY(rad): + """ Return matrix for rotating about the z-axis by 'radians' radians """ + c = np.cos(rad) + s = np.sin(rad) + m=np.identity(4) + m[np.ix_((0,2),(0,2))]=((c, -s),(s, c)) + return m + + @staticmethod + def trans(v): + m=np.identity(4) + m[0:3, 3] = v + return m + +class ExtAxes():#mpl.axes._subplots.Axes3DSubplot): + def __init__(self,ax): + self.ax=ax + ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y') + ax.view_init(elev=14., azim=10) + + def setCenter(self,v,l): + ax=self.ax + #v=center vector, l= length of each axis + l2=l/2. + ax.set_xlim(v[2]-l2, v[2]+l2); + ax.set_ylim(v[0]-l2, v[0]+l2); + ax.set_zlim(v[1]-l2, v[1]+l2) + + def pltOrig(self,m): + ax=self.ax + # m is a 4x4 matrix. the transformed matrix + r=m[:3,0] #1st + g=m[:3,1] #2nd + b=m[:3,2] #3rd + o=m[:3,3] #origin + hr=ax.plot((o[2],o[2]+r[2]), (o[0],o[0]+r[0]), (o[1],o[1]+r[1]), 'r') + hg=ax.plot((o[2],o[2]+g[2]), (o[0],o[0]+g[0]), (o[1],o[1]+g[1]), 'g') + hb=ax.plot((o[2],o[2]+b[2]), (o[0],o[0]+b[0]), (o[1],o[1]+b[1]), 'b') + return hr+hg+hb # this is a list + +def my_anim_func(idx,horig): + print('anim') + a=idx*.01*2*np.pi + m=Trf.rotY(a) + r = m[:3, 0] # 1st + g = m[:3, 1] # 2nd + b = m[:3, 2] # 3rd + o = m[:3, 3] # origin + hr,hg,hb=horig + hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0])) + hr.set_3d_properties((o[1], o[1] + r[1])) + hg.set_data((o[2], o[2] + g[2]), (o[0], o[0] + g[0])) + hg.set_3d_properties((o[1], o[1] + g[1])) + hb.set_data((o[2], o[2] + b[2]), (o[0], o[0] + b[0])) + hb.set_3d_properties((o[1], o[1] + b[1])) + class HelicalScan: def __init__(self,args): if args.cfg: @@ -53,61 +128,89 @@ class HelicalScan: eval('self.' + cmd) def test_coord_trf(self): - n = 3.; per = 1.; t = np.arange(n) - p=((2.3,2.31,4.12,24.6),(6.2,2.74,32.1,3.28)) #(y, bias, ampl, phi) + d2r=2*np.pi/360 + plt.ion() + fig = plt.figure() + #ax = fig.gca(projection='3d') + ax = fig.add_subplot(1,1,1,projection='3d') + extAx=ExtAxes(ax) + extAx.setCenter((0,5,15),10) + n = 3.; per = 1.; w = 2*np.pi*per/n*np.arange(n) + p=((2.3,0.71,4.12,10.6*d2r),(6.2,.45,3.2,45.28*d2r)) #(y, bias, ampl, phi) self.param=param=np.ndarray((len(p),5)) - z=4.5 # fix z position + z=14.5 # fix z position + pt=np.ndarray((4,3)) for i in range(2): (y, bias, ampl, phi) =p[i] - x= ampl * np.cos(2 * np.pi * (per / n * t + phi / 360.)) + bias + x= ampl * np.cos(w+phi) + bias print('yMeas_%d='%i+str(y)+' xMeas_%d='%i+str(x)) #param[i]=(z_i, y_i, x_i, r_i,phi_i) - param[i][0] =z - param[i][1] =y - param[i][2:]=HelicalScan.meas_rot_ctr(x) #(bias,ampl,phase) - pass + param[i,0] =z + param[i,1] =y + param[i,2:]=HelicalScan.meas_rot_ctr(x) #(bias,ampl,phase) + (bias, ampl, phase)=param[i][2:] + pt[i]=(bias, y, z) + pt[i+2]=(bias+ampl*np.cos(phase),y, z+ampl*np.sin(phase)) + obj = mpl.patches.Circle((z,bias), ampl, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3)) + ax.add_patch(obj) + plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z") + ax.scatter(pt[:,2], pt[:,0], pt[:,1]) + ax.plot(pt[2:,2], pt[2:,0], pt[2:,1], label='zs=0, zdir=z') print param - #self.fwd_transform(param[0][1],0.,param[0][2],param[0][1]) + #plt.show() + #m=np.identity(4); horig=extAx.pltOrig(m) + m=Trf.trans((0,0,z)); horig=extAx.pltOrig(m) + #self.fwd_transform(y ,w ,cx ,cz) + # y_0 ,0deg ,x_0 ,z_0) + m=self.fwd_transform(param[0][1],0,pt[2][0],pt[2][2],extAx) + m=self.fwd_transform(param[0][1],0,pt[2][0],pt[2][2],extAx) + + m=self.fwd_transform(param[0][1],20*d2r,pt[2][0],pt[2][2]) + extAx.pltOrig(m) + + #my_anim_func(0,horig) + a=anim.FuncAnimation(fig,my_anim_func,25,fargs=(horig,),interval=50,blit=False) + plt.show() # y_0 ,120deg ,x_0 ,z_0) self.fwd_transform(param[0][1],2*np.pi/3.,param[0][2],param[0][0]) #self.fwd_transform(param[1][1],0.,param[1][2],param[1][3]) - - - def fwd_transform(self,y,w,cx,cz): + def fwd_transform(self,y,w,cx,cz,extAx): #cx,cy: coarse stage + #input: y,w,cx,cz + #output: y,w,dx,dz + m=Trf.trans((cx,y,cz)) + m=m.dot(Trf.rotY(w)) + extAx.pltOrig(m) + extAx.setCenter(m[0:3,3],1) #TODO: NOT WORKING AT ALL NOW... param=self.param # param[i]=(z_i, y_i, x_i, r_i,phi_i) p=np.ndarray((param.shape[0], 3)) for i in range(2): - #p[i][0]=param[i][2]+param[i][3]*np.cos(param[i][4]+w) # x= x_i+r_i*cos(phi_i*w)+cx - p[i][0]=cx+param[i][3]*np.cos(param[i][4]+w) # x= x_i+r_i*cos(phi_i*w)+cx - p[i][1]=param[i][1] # y= y_i - #p[i][2]=param[i][2]+param[i][3]*np.sin(param[i][4]+w) # z= z_i+r_i*sin(phi_i*w) - p[i][2] =cz + param[i][3] * np.sin(param[i][4] + w) # z= z_i+r_i*sin(phi_i*w) + (z_i, y_i, x_i, r_i, phi_i)=param[i] + p[i,0]=x_i+r_i*np.cos(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx + p[i,1]=y_i # y= y_i + p[i,2] =z_i+r_i*np.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w) print p v=p[1]-p[0] v=v/np.sqrt(v.dot(v)) # v/|v| - v=v*(y-param[0][1])/(param[1][1]-param[0][1]) # v(y)=v*(v-y_0)/(y_1-y_0) + y_0=param[0][1] + y_1=param[1][1] + v=v*(y-y_0)/(y_1-y_0) # v(y)=v*(v-y_0)/(y_1-y_0) v=p[0]+v + cz + cx #v=v/abs(v) print v - - # - # - # - - - - #x,y,z - #returns y,w,dx,dz - pass + return m def inv_transform(y,phi,dx=0,dz=0): + #input: y,w,dx,dz + #output: y,w,cx,cz + m=np.identity(4) #dx,dy: deviation from cristal center line #ps= #x,y,z #returns y,phi,cx,cz @@ -135,9 +238,11 @@ class HelicalScan: # phi phase # bias bias value # ampl amplitude + d2r=2*np.pi/360 t = np.arange(n) - y=ampl*np.cos(2*np.pi*(per/n*t+phi/360.))+bias + w=2*np.pi*per/n*t + y=ampl*np.cos(w+phi*d2r)+bias plt.figure(1) plt.subplot(311) plt.plot(t,y,'b.-') @@ -155,7 +260,7 @@ class HelicalScan: t2 = np.linspace(0,2*np.pi,64) y2=ampl*np.cos(t2+phase)+bias plt.plot(t2,y2,'g-') - plt.stem(t/n*2*np.pi*per,y,'b-') + plt.stem(w,y,'b-') plt.show() diff --git a/python/helicalscan1.svg b/python/helicalscan1.svg index 21b37e3..d266f78 100644 --- a/python/helicalscan1.svg +++ b/python/helicalscan1.svg @@ -133,11 +133,11 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="0.91503512" - inkscape:cx="61.236951" - inkscape:cy="572.69075" + inkscape:zoom="1.3382855" + inkscape:cx="183.81592" + inkscape:cy="755.54223" inkscape:document-units="px" - inkscape:current-layer="g3894" + inkscape:current-layer="layer1" showgrid="false" inkscape:window-width="1871" inkscape:window-height="1176" @@ -172,317 +172,319 @@ x + x="574.12115" + y="280.73386">x y + x="217.59642" + y="87.74752">y z + x="55.3997" + y="452.32724">z φφs ppe pps φe + style="font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;font-size:64.99999762%;font-family:MathJax_Size1;-inkscape-font-specification:MathJax_Size1;baseline-shift:sub">e yys yye v crystal + x="3.9073553" + y="484.16422">v crystal rrs rre + d="m 85.856928,445.65589 0,-261.31492" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.54521042;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" /> + style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:1.40831339;stroke-linecap:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" + r="0.7958433" + inkscape:transform-center-x="-4.2929873" + inkscape:transform-center-y="-0.39627575" /> + cx="145.47098" + cy="374.18094" + r="0.7958433" /> xe + style="font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;font-size:64.99999762%;font-family:MathJax_Size1;-inkscape-font-specification:MathJax_Size1;baseline-shift:sub">e xs + style="font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;font-size:64.99999762%;font-family:MathJax_Size1;-inkscape-font-specification:MathJax_Size1;baseline-shift:sub">s + d="m 73.992127,433.67296 484.652183,0" + style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.7425012;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" /> + id="flowPara3858" />