update helicalscan
This commit is contained in:
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user