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" />