最近在刀具设计中应用到坐标转换问题，出现了问题，不知道原因是什么？

clear all
clc
t=0:0.1:1;
x1=10+0*t;
y1=10+10*t;
z1=0*t;
plot3(x1,y1,z1);
xyz1=[x1;y1;z1;ones(1,length(x1))];
hold on;

phi1=pi/2;i=1;
M01=[cos(phi1(i)),-sin(phi1(i)),0,0;sin(phi1(i)),cos(phi1(i)),0,0;0,0,1,0;0,0,0,1];
xyz=M01*xyz1;
x=xyz(1,:);
y=xyz(2,:);
z=xyz(3,:);
plot3(x1,y1,z1,x,y,z,'g');

gama=pi/2;ee=0;a=100;H=0;
%坐标系s——sp转换矩阵
Mp0=[cos(gama),0,-sin(gama),ee;0,1,0,a;sin(gama),0,cos(gama),H;0,0,0,1];
 xyzp=Mp0*xyz;  
 plot3(xyzp(1,:),xyzp(2,:),xyzp(3,:),'r');

 phi2=pi/2;
 %SP——S2转换矩阵
M2p=[cos(phi2(i)),sin(phi2(i)),0,0;-sin(phi2(i)),cos(phi2(i)),0,0;0,0,1,0;0,0,0,1];
xyz2=M2p*xyzp;
plot3(xyz2(1,:),xyz2(2,:),xyz2(3,:),'k');
 
plotCircle(0,0,10)
plotCircle(0,a,10)
plotCircle(0,0,a+10)
axis equal
xlabel('X')
ylabel('Y')
zlabel('z')

旋转错误

• 最好能小小段一小段的拆分测试
• 最好能把代码封装成一个一个小函数，每个函数的功能单独测试
• 小函数测试完后再组装起来