import numpy as np
def move_point(p0,u,d):
return p0+u*d
p0=np.mat([[1,0,0]]).T
u=np.mat([[1,0,0],[0,1,0],[0,0,1]]).T
d=10
p1=move_point(p0,u,d)
print(p1)
p0=np.mat([[35,-280.893,1.33236],[35,280.893,1.33236],[874.75,-309.9,120.35],[874.75,309.9,120.35]]).T
p0_temp=p0[:,0]
u=np.mat([[0,90,90],[90,30,120],[90,60,30]]).T
u=np.cos(u*np.pi/180)
norm_u=np.linalg.norm(u,ord=None,axis=0) #按列求模，对单位矢量模=1
d=100
p1=move_point(p0_temp,u,d)
print(u,norm_u)
print(‘p1:n’,p1)
