请求高手，帮忙看一下这个程序好忙？

aaachwei

程序有点长，麻烦各位了。这个错误真不知道怎么改了，请赐教!谢谢

clear all;
clc;
tic;
alpha=0;beta=0;gamma=0;
A10=[-400/2;320/2;0];    %运动平台上各球铰点在动坐标系下的矢量表示
A20=[-400/2;-320/2;0];
A30=[0;-400/2;320/2];
A40=[0;-400/2;-320/2];
A50=[320/2;0;-400/2];
A60=[-320/2;0;-400/2];
T=[ cos(alpha)*cos(beta)  sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma)  cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma);
    cos(alpha)*sin(beta)  sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma)  cos(alpha)*sin(beta)*sin(gamma)-sin(gamma)*cos(alpha);
        -sin(beta)                              cos(beta)*sin(gamma)                        cos(gamma)*cos(beta)                      ]
C1=[-(400/2+500+400);320/2;0];        %各分支与基座连接点在定坐标系下的矢量表示
C2=[-(400/2+500+400);-320/2;0];
C3=[0;-(400/2+500+400);320/2];
C4=[0;-(400/2+500+400);-320/2];
C5=[320/2;0;-(400/2+500+400)];
C6=[-320/2;0;-(400/2+500+400)];
k=1;
xdata=0;
ydata=0;
zdata=0;
    for r=0:50:300
        for a=0:2*pi/100:2*pi
      
        x=r*cos(a);
        y=r*sin(a);
        z=0;
         
A1=T*A10+[x;y;z];
A2=T*A20+[x;y;z];
A3=T*A30+[x;y;z];
A4=T*A40+[x;y;z];
A5=T*A50+[x;y;z];
A6=T*A60+[x;y;z];
l1=500+x-sqrt(500^2-y^2-z^2);
l2=500+x-sqrt(500^2-y^2-z^2);
l3=500+y-sqrt(500^2-x^2-z^2);
l4=500+y-sqrt(500^2-x^2-z^2);
l5=500+z-sqrt(500^2-x^2-y^2);
l6=500+z-sqrt(500^2-x^2-y^2);

d1=-200*cos(beta)*cos(alpha)+160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+x+700-l1;
d2=-200*cos(beta)*cos(alpha)-160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+x+700-l2;
d3=-200*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))+160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+y+700-l3;
d4=-200*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))-160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+y+700-l4;
d5=-160*sin(beta)-200*cos(gamma)*cos(beta)+z+700-l5;
d6=-160*sin(beta)-200*cos(gamma)*cos(beta)+z+700-l6;
e1=-200*cos(beta)*cos(alpha)+160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+x+700-l1;
e2=-200*cos(beta)*cos(alpha)-160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+x+700-l2;
e3=-200*(cos(alpha)*sin(beta)*sin(gamma)+sin(alpha)*cos(gamma))+160*(cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma))+x;
e4=-200*(cos(alpha)*sin(beta)*sin(gamma)+sin(alpha)*cos(gamma))-160*(cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma))+x;
e5=160*cos(beta)*cos(alpha)-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)+x-160;
e6=-160*cos(beta)*cos(gamma)-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)+x+160;
f1=-200*cos(beta)*sin(alpha)+160*(sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma))+y-160;
f2=-200*cos(beta)*sin(alpha)-160*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))+y+160;
f3=-200*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))+160*(sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma))+y+700-l3;
f4=-200*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))-160*(sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma))+y+700-l4;
f5=160*cos(beta)*sin(alpha)-200*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*sin(gamma)+y;
f6=-160*cos(beta)*sin(alpha)-200*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*sin(gamma)+y;
g1=200*sin(beta)+160*sin(gamma)*cos(beta)+z;
g2=200*sin(beta)-160*sin(gamma)*cos(beta)+z;
g3=-200*sin(gamma)*cos(beta)+160*cos(gamma)*cos(beta)+z-160;
g4=-200*sin(gamma)*cos(beta)-160*cos(gamma)*cos(beta)+z+160;
g5=-160*sin(beta)-200*cos(gamma)*cos(beta)+z+700-l5;
g6=160*sin(beta)-200*cos(gamma)*cos(beta)+z+700-l6;
L1=(-200*sin(alpha)*cos(beta)-160*sin(alpha)*sin(beta)*sin(gamma)-160*cos(alpha)*cos(gamma))*(x+700-l1)+(-200*cos(beta)*cos(gamma)+160*cos(alpha)*sin(beta)*sin(gamma)-160*sin(alpha)*cos(gamma))*(y-160);
L2=(-200*sin(alpha)*cos(beta)+160*sin(alpha)*sin(beta)*sin(gamma)+160*cos(alpha)*cos(gamma))*(x+700-l1)+(-200*cos(beta)*cos(gamma)-160*cos(alpha)*sin(beta)*sin(gamma)+160*sin(alpha)*cos(gamma))*(y+160);
L3=(160*cos(alpha)*sin(gamma)-160*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*cos(gamma)+200*sin(alpha)*sin(beta)*sin(gamma))*x+(-200*cos(alpha)*sin(beta)*sin(gamma)+200*sin(alpha)*cos(gamma)+160*cos(alpha)*sin(beta)*cos(gamma)+160*sin(alpha)*sin(gamma))*(y+700-l3);
L4=(-160*cos(alpha)*sin(gamma)+160*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*cos(gamma)+200*sin(alpha)*sin(beta)*sin(gamma))*x+(-200*cos(alpha)*sin(beta)*sin(gamma)+200*sin(alpha)*cos(gamma)-160*cos(alpha)*sin(beta)*cos(gamma)-160*sin(alpha)*sin(gamma))*(y+700-l4);
L5=(200*sin(alpha)*sin(beta)*cos(gamma)-200*cos(alpha)*sin(gamma)-160*sin(alpha)*cos(beta))*(x-160)+(-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)+160*cos(alpha)*cos(beta))*y;
L6=(200*sin(alpha)*sin(beta)*cos(gamma)-200*cos(alpha)*sin(gamma)+160*sin(alpha)*cos(beta))*(x+160)+(-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)-160*cos(alpha)*cos(beta))*y;
m1=(200*cos(alpha)*cos(beta)+160*cos(alpha)*cos(beta)*sin(gamma))*(x+700-l1)+(200*sin(beta)*sin(gamma)+160*sin(alpha)*cos(beta)*sin(gamma))*(y-160)+(200*cos(beta)-160*sin(gamma)*sin(beta))*z;
m2=(200*cos(alpha)*cos(beta)-160*cos(alpha)*cos(beta)*sin(gamma))*(x+700-l2)+(200*sin(beta)*sin(gamma)-160*sin(alpha)*cos(beta)*sin(gamma))*(y+160)+(200*cos(beta)+160*sin(gamma)*sin(beta))*z;
m3=(160*cos(alpha)*cos(beta)*cos(gamma)-200*cos(alpha)*cos(beta)*sin(gamma))*x+(-200*sin(alpha)*cos(beta)*sin(gamma)+160*sin(alpha)*cos(beta)*cos(gamma))*(y+700-l3)+(200*sin(gamma)*sin(beta)-160*cos(gamma)*sin(beta))*(z-160);
m4=(-160*cos(alpha)*cos(beta)*cos(gamma)-200*cos(alpha)*cos(beta)*sin(gamma))*x+(-200*sin(alpha)*cos(beta)*sin(gamma)-160*sin(alpha)*cos(beta)*cos(gamma))*(y+700-l4)+(200*sin(gamma)*sin(beta)+160*cos(gamma)*sin(beta))*(z+160);
m5=(-160*sin(beta)*cos(alpha)-200*cos(alpha)*cos(beta)*cos(gamma))*(x-160)+(-160*sin(beta)*sin(alpha)-200*sin(alpha)*cos(beta)*cos(gamma))*y+(-160*cos(beta)+200*cos(gamma)*sin(beta))*(z+700-l5);
m6=(160*sin(beta)*cos(alpha)-200*cos(alpha)*cos(beta)*cos(gamma))*(x+160)+(160*sin(beta)*sin(alpha)-200*sin(alpha)*cos(beta)*cos(gamma))*y+(160*cos(beta)+200*cos(gamma)*sin(beta))*(z+700-l6);
n1=(160*cos(alpha)*sin(beta)*cos(gamma)+160*sin(alpha)*sin(gamma))*(x+700-l1)+(160*sin(alpha)*sin(beta)*cos(gamma)-160*cos(alpha)*sin(gamma))*(y-160)+160*z*cos(beta)*cos(gamma);
n2=(-160*cos(alpha)*sin(beta)*cos(gamma)-160*sin(alpha)*sin(gamma))*(x+700-l1)+(-160*sin(alpha)*sin(beta)*cos(gamma)+160*cos(alpha)*sin(gamma))*(y+160)-160*z*cos(beta)*cos(gamma);
n3=(-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)-160*cos(alpha)*sin(beta)*sin(gamma)+160*sin(alpha)*cos(gamma))*x+(-200*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*sin(gamma)-160*sin(alpha)*sin(beta)*sin(gamma)-160*cos(alpha)*cos(gamma))*(y+700-l3)+(-200*cos(gamma)*cos(beta)-160*cos(beta)*sin(gamma))*(z-160);
n4=(-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)+160*cos(alpha)*sin(beta)*sin(gamma)-160*sin(alpha)*cos(gamma))*x+(-200*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*sin(gamma)+160*sin(alpha)*sin(beta)*sin(gamma)+160*cos(alpha)*cos(gamma))*(y+700-l4)+(-200*cos(gamma)*cos(beta)+160*cos(beta)*sin(gamma))*(z-160);
n5=(200*cos(alpha)*sin(beta)*sin(gamma)-200*cos(gamma)*sin(alpha))*(x-160)+(200*sin(alpha)*sin(beta)*sin(gamma)+200*cos(alpha)*cos(gamma))*y+200*cos(beta)*sin(gamma)*(z+700-l5);
n6=(200*cos(alpha)*sin(beta)*sin(gamma)-200*cos(gamma)*sin(alpha))*(x+160)+(200*sin(alpha)*sin(beta)*sin(gamma)+200*cos(alpha)*cos(gamma))*y+200*cos(beta)*sin(gamma)*(z+700-l6);

B1=[-(400/2+500-l1);320/2;0];        %虎可铰各点在固定坐标系下的矢量表示
B2=[ -(400/2+500-l2);-320/2;0];
B3=[0;-(400/2+500-l3);320/2];
B4=[0;-(400/2+500-l4);-320/2];
B5=[320/2;0;-(400/2+500-l5)];   
B6=[-320/2;0;-(400/2+500-l6)];
zeta1=(B1-A1)/500;
zeta2=(B2-A2)/500;
zeta3=(B3-A3)/500;      
zeta4=(B4-A4)/500;        
zeta5=(B5-A5)/500;         
zeta6=(B6-A6)/500;

eta1=(C1-B1)/abs(400+l1);         
eta2=(C2-B2)/abs(400+l2);        
eta3=(C3-B3)/abs(400+l3);         
eta4=(C4-B4)/abs(400+l4);         
eta5=(C5-B5)/abs(400+l5);
eta6=(C6-B6)/abs(400+l6);

delta1=acos(dot(zeta1,T*eta1));
delta2=acos(dot(zeta2,T*eta2));
delta3=acos(dot(zeta3,T*eta3));
delta4=acos(dot(zeta4,T*eta4));
delta5=acos(dot(zeta5,T*eta5));
delta6=acos(dot(zeta6,T*eta6));

   theta1=acos(dot(zeta1,eta1));
   theta2=acos(dot(zeta2,eta2));   
   theta3=acos(dot(zeta3,eta3));  
   theta4=acos(dot(zeta4,eta4));
   theta5=acos(dot(zeta5,eta5));
   theta6=acos(dot(zeta6,eta6));
   
if ((l1>=-200)&(l1<=200)&(l2>=-200)&(l2<=200)&(l3>=-200)&(l3<=200)&(l4>=-200)&(l4<=200)&(l5>=-200)&(l5<=200)&(l6>=-200)&(l6<=200)&(delta1>=0)&(delta1<=pi/6)&(delta2>=0)&(delta2<=pi/6)&(delta3>=0)&(delta3<=pi/6)&(delta4>=0)&(delta4<=pi/6)&(delta5>=0)&(delta5<=pi/6)&(delta6>=0)&(delta6<=pi/6)&(theta1>=0)&(theta1<=pi/6)&(theta2>=0)&(theta2<=pi/6)&(theta3>=0)&(theta3<=pi/6)&(theta4>=0)&(theta4<=pi/6)&(theta5>=0)&(theta5<=pi/6)&(theta6>=0)&(theta6<=pi/6))
     xdata(k)=x;
     ydata(k)=y;
   
A=[d1  0   0   0   0   0;
    0 d2   0   0   0   0;
    0  0  d3   0   0   0;
    0  0   0  d4   0   0;
    0  0   0   0  d5   0;
    0  0   0   0   0  d6];
B=[e1  f1  g1  L1  m1  n1;
   e2  f2  g2  L2  m2  n2;
   e3  f3  g3  L3  m3  n3;
   e4  f4  g4  L4  m4  n4;
   e5  f5  g5  L5  m5  n5;
   e6  f6  g6  L6  m6  n6];
if det(B)~=0
J=inv(B)*A;
Jv=[J(1,1) J(1,2) J(1,3) J(1,4) J(1,5) J(1,6);
    J(2,1) J(2,2) J(2,3) J(2,4) J(2,5) J(2,6);
    J(3,1) J(3,2) J(3,2) J(3,2) J(3,2) J(3,2)];
Kv(k)=norm(Jv);
     k=k+1;
end
end
        end
    end
toc
surf(xdata,ydata,Kv)
   
     xlabel('x/mm');ylabel('y/mm');zlabel('K')

qiyulong

T =

     1     0     0
     0     1     0
     0     0     1

Elapsed time is 1.734000 seconds.
Warning: Matrix dimensions must agree, not rendering mesh.
Warning: Matrix dimensions must agree, not rendering mesh.
Warning: Matrix dimensions must agree, not rendering mesh.
Warning: Matrix dimensions must agree, not rendering mesh.

这是运算结果，好像是不是矩阵的问题啊，咱也不懂，坐下来看高手解决吧

lpknjtu

错误在surf(xdata,ydata,Kv)函数的调用，三个变量都是一维数组。用其它作图方法试试可否满足要求。

lpknjtu

surf（x,y,z）函数的z可以理解为平面上点(x,y)对应的函数值，这里x是1X506，y是1x506矩阵，z应该是506X506矩阵。但你程序里的z(就是Kv)是1x506。

lin2009

1# aaachwei

除了surf的调用格式有误外，还有一点，就在于你的程序逻辑有误，你用for循环将所有的x,y,z都算了一遍，但没有及时保存下来。程序运行完了，也就只有最后的一组数据留下来。

aaachwei

先谢谢你啊！其他方法我也试过了，有的时候会出现下面情况“The surface Z must contain more than one row or column”，应该也是z的维数不对的关系。Kv就是关于x和y的表达式，有什么办法修改吗？

lin2009

是不是这样的图呢？
http://home.simwe.com/space-418800-do-album-picid-6949.html

aaachwei

是不是因为我把x，y用for语句来赋值，才得出的三维实体图形啊？是想要用surf绘出三维图形，并且还要求绘出等值线。不过程序怎样改才能调出图形来啊？真的万分感谢！！

lin2009

是你的程序逻辑的问题－见5＃。
每一步计算的结果都要保存下来，加了简单的6条语句，解决问题。
参考程序如下：
clear all;
clc;
tic;
alpha = 0;
beta = 0;
gamma = 0;
A10 = [-400/2;320/2;0];    %运动平台上各球铰点在动坐标系下的矢量表示
A20 = [-400/2;-320/2;0];
A30 = [0;-400/2;320/2];
A40 = [0;-400/2;-320/2];
A50 = [320/2;0;-400/2];
A60 = [-320/2;0;-400/2];
T = [ cos(alpha)*cos(beta)  sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma)  cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma);
      cos(alpha)*sin(beta)  sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma)  cos(alpha)*sin(beta)*sin(gamma)-sin(gamma)*cos(alpha);
     -sin(beta)             cos(beta)*sin(gamma)                                   cos(gamma)*cos(beta)                      ]
C1 = [-(400/2+500+400);320/2;0];        %各分支与基座连接点在定坐标系下的矢量表示
C2 = [-(400/2+500+400);-320/2;0];
C3 = [0;-(400/2+500+400);320/2];
C4 = [0;-(400/2+500+400);-320/2];
C5 = [320/2;0;-(400/2+500+400)];
C6 = [-320/2;0;-(400/2+500+400)];

xdata = 0;
ydata = 0;
zdata = 0;
m = 1;
for r = 0:50:300
    n = 1;
    nn = 1;
    for a = 0:2*pi/100:2*pi
        
        x = r*cos(a);
        y = r*sin(a);
        z = 0;
        
        A1 = T*A10+[x;y;z];
        A2 = T*A20+[x;y;z];
        A3 = T*A30+[x;y;z];
        A4 = T*A40+[x;y;z];
        A5 = T*A50+[x;y;z];
        A6 = T*A60+[x;y;z];
        l1 = 500+x-sqrt(500^2-y^2-z^2);
        l2 = 500+x-sqrt(500^2-y^2-z^2);
        l3 = 500+y-sqrt(500^2-x^2-z^2);
        l4 = 500+y-sqrt(500^2-x^2-z^2);
        l5 = 500+z-sqrt(500^2-x^2-y^2);
        l6 = 500+z-sqrt(500^2-x^2-y^2);
        
        d1 = -200*cos(beta)*cos(alpha)+160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+x+700-l1;
        d2 = -200*cos(beta)*cos(alpha)-160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+x+700-l2;
        d3 = -200*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))+160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+y+700-l3;
        d4 = -200*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))-160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+y+700-l4;
        d5 = -160*sin(beta)-200*cos(gamma)*cos(beta)+z+700-l5;
        d6 = -160*sin(beta)-200*cos(gamma)*cos(beta)+z+700-l6;
        e1 = -200*cos(beta)*cos(alpha)+160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+x+700-l1;
        e2 = -200*cos(beta)*cos(alpha)-160*(cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma))+x+700-l2;
        e3 = -200*(cos(alpha)*sin(beta)*sin(gamma)+sin(alpha)*cos(gamma))+160*(cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma))+x;
        e4 = -200*(cos(alpha)*sin(beta)*sin(gamma)+sin(alpha)*cos(gamma))-160*(cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma))+x;
        e5 = 160*cos(beta)*cos(alpha)-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)+x-160;
        e6 = -160*cos(beta)*cos(gamma)-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)+x+160;
        f1 = -200*cos(beta)*sin(alpha)+160*(sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma))+y-160;
        f2 = -200*cos(beta)*sin(alpha)-160*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))+y+160;
        f3 = -200*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))+160*(sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma))+y+700-l3;
        f4 = -200*(sin(alpha)*sin(beta)*sin(gamma)-cos(alpha)*cos(gamma))-160*(sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma))+y+700-l4;
        f5 = 160*cos(beta)*sin(alpha)-200*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*sin(gamma)+y;
        f6 = -160*cos(beta)*sin(alpha)-200*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*sin(gamma)+y;
        g1 = 200*sin(beta)+160*sin(gamma)*cos(beta)+z;
        g2 = 200*sin(beta)-160*sin(gamma)*cos(beta)+z;
        g3 = -200*sin(gamma)*cos(beta)+160*cos(gamma)*cos(beta)+z-160;
        g4 = -200*sin(gamma)*cos(beta)-160*cos(gamma)*cos(beta)+z+160;
        g5 = -160*sin(beta)-200*cos(gamma)*cos(beta)+z+700-l5;
        g6 = 160*sin(beta)-200*cos(gamma)*cos(beta)+z+700-l6;
        L1 = (-200*sin(alpha)*cos(beta)-160*sin(alpha)*sin(beta)*sin(gamma)-160*cos(alpha)*cos(gamma))*(x+700-l1)+(-200*cos(beta)*cos(gamma)+160*cos(alpha)*sin(beta)*sin(gamma)-160*sin(alpha)*cos(gamma))*(y-160);
        L2 = (-200*sin(alpha)*cos(beta)+160*sin(alpha)*sin(beta)*sin(gamma)+160*cos(alpha)*cos(gamma))*(x+700-l1)+(-200*cos(beta)*cos(gamma)-160*cos(alpha)*sin(beta)*sin(gamma)+160*sin(alpha)*cos(gamma))*(y+160);
        L3 = (160*cos(alpha)*sin(gamma)-160*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*cos(gamma)+200*sin(alpha)*sin(beta)*sin(gamma))*x+(-200*cos(alpha)*sin(beta)*sin(gamma)+200*sin(alpha)*cos(gamma)+160*cos(alpha)*sin(beta)*cos(gamma)+160*sin(alpha)*sin(gamma))*(y+700-l3);
        L4 = (-160*cos(alpha)*sin(gamma)+160*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*cos(gamma)+200*sin(alpha)*sin(beta)*sin(gamma))*x+(-200*cos(alpha)*sin(beta)*sin(gamma)+200*sin(alpha)*cos(gamma)-160*cos(alpha)*sin(beta)*cos(gamma)-160*sin(alpha)*sin(gamma))*(y+700-l4);
        L5 = (200*sin(alpha)*sin(beta)*cos(gamma)-200*cos(alpha)*sin(gamma)-160*sin(alpha)*cos(beta))*(x-160)+(-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)+160*cos(alpha)*cos(beta))*y;
        L6 = (200*sin(alpha)*sin(beta)*cos(gamma)-200*cos(alpha)*sin(gamma)+160*sin(alpha)*cos(beta))*(x+160)+(-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)-160*cos(alpha)*cos(beta))*y;
        m1 = (200*cos(alpha)*cos(beta)+160*cos(alpha)*cos(beta)*sin(gamma))*(x+700-l1)+(200*sin(beta)*sin(gamma)+160*sin(alpha)*cos(beta)*sin(gamma))*(y-160)+(200*cos(beta)-160*sin(gamma)*sin(beta))*z;
        m2 = (200*cos(alpha)*cos(beta)-160*cos(alpha)*cos(beta)*sin(gamma))*(x+700-l2)+(200*sin(beta)*sin(gamma)-160*sin(alpha)*cos(beta)*sin(gamma))*(y+160)+(200*cos(beta)+160*sin(gamma)*sin(beta))*z;
        m3 = (160*cos(alpha)*cos(beta)*cos(gamma)-200*cos(alpha)*cos(beta)*sin(gamma))*x+(-200*sin(alpha)*cos(beta)*sin(gamma)+160*sin(alpha)*cos(beta)*cos(gamma))*(y+700-l3)+(200*sin(gamma)*sin(beta)-160*cos(gamma)*sin(beta))*(z-160);
        m4 = (-160*cos(alpha)*cos(beta)*cos(gamma)-200*cos(alpha)*cos(beta)*sin(gamma))*x+(-200*sin(alpha)*cos(beta)*sin(gamma)-160*sin(alpha)*cos(beta)*cos(gamma))*(y+700-l4)+(200*sin(gamma)*sin(beta)+160*cos(gamma)*sin(beta))*(z+160);
        m5 = (-160*sin(beta)*cos(alpha)-200*cos(alpha)*cos(beta)*cos(gamma))*(x-160)+(-160*sin(beta)*sin(alpha)-200*sin(alpha)*cos(beta)*cos(gamma))*y+(-160*cos(beta)+200*cos(gamma)*sin(beta))*(z+700-l5);
        m6 = (160*sin(beta)*cos(alpha)-200*cos(alpha)*cos(beta)*cos(gamma))*(x+160)+(160*sin(beta)*sin(alpha)-200*sin(alpha)*cos(beta)*cos(gamma))*y+(160*cos(beta)+200*cos(gamma)*sin(beta))*(z+700-l6);
        n1 = (160*cos(alpha)*sin(beta)*cos(gamma)+160*sin(alpha)*sin(gamma))*(x+700-l1)+(160*sin(alpha)*sin(beta)*cos(gamma)-160*cos(alpha)*sin(gamma))*(y-160)+160*z*cos(beta)*cos(gamma);
        n2 = (-160*cos(alpha)*sin(beta)*cos(gamma)-160*sin(alpha)*sin(gamma))*(x+700-l1)+(-160*sin(alpha)*sin(beta)*cos(gamma)+160*cos(alpha)*sin(gamma))*(y+160)-160*z*cos(beta)*cos(gamma);
        n3 = (-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)-160*cos(alpha)*sin(beta)*sin(gamma)+160*sin(alpha)*cos(gamma))*x+(-200*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*sin(gamma)-160*sin(alpha)*sin(beta)*sin(gamma)-160*cos(alpha)*cos(gamma))*(y+700-l3)+(-200*cos(gamma)*cos(beta)-160*cos(beta)*sin(gamma))*(z-160);
        n4 = (-200*cos(alpha)*sin(beta)*cos(gamma)-200*sin(alpha)*sin(gamma)+160*cos(alpha)*sin(beta)*sin(gamma)-160*sin(alpha)*cos(gamma))*x+(-200*sin(alpha)*sin(beta)*cos(gamma)+200*cos(alpha)*sin(gamma)+160*sin(alpha)*sin(beta)*sin(gamma)+160*cos(alpha)*cos(gamma))*(y+700-l4)+(-200*cos(gamma)*cos(beta)+160*cos(beta)*sin(gamma))*(z-160);
        n5 = (200*cos(alpha)*sin(beta)*sin(gamma)-200*cos(gamma)*sin(alpha))*(x-160)+(200*sin(alpha)*sin(beta)*sin(gamma)+200*cos(alpha)*cos(gamma))*y+200*cos(beta)*sin(gamma)*(z+700-l5);
        n6 = (200*cos(alpha)*sin(beta)*sin(gamma)-200*cos(gamma)*sin(alpha))*(x+160)+(200*sin(alpha)*sin(beta)*sin(gamma)+200*cos(alpha)*cos(gamma))*y+200*cos(beta)*sin(gamma)*(z+700-l6);
        
        B1 = [-(400/2+500-l1);320/2;0];        %虎可铰各点在固定坐标系下的矢量表示
        B2 = [ -(400/2+500-l2);-320/2;0];
        B3 = [0;-(400/2+500-l3);320/2];
        B4 = [0;-(400/2+500-l4);-320/2];
        B5 = [320/2;0;-(400/2+500-l5)];
        B6 = [-320/2;0;-(400/2+500-l6)];
        zeta1 = (B1-A1)/500;
        zeta2 = (B2-A2)/500;
        zeta3 = (B3-A3)/500;
        zeta4 = (B4-A4)/500;
        zeta5 = (B5-A5)/500;
        zeta6 = (B6-A6)/500;
        
        eta1 = (C1-B1)/abs(400+l1);
        eta2 = (C2-B2)/abs(400+l2);
        eta3 = (C3-B3)/abs(400+l3);
        eta4 = (C4-B4)/abs(400+l4);
        eta5 = (C5-B5)/abs(400+l5);
        eta6 = (C6-B6)/abs(400+l6);
        
        delta1 = acos(dot(zeta1,T*eta1));
        delta2 = acos(dot(zeta2,T*eta2));
        delta3 = acos(dot(zeta3,T*eta3));
        delta4 = acos(dot(zeta4,T*eta4));
        delta5 = acos(dot(zeta5,T*eta5));
        delta6 = acos(dot(zeta6,T*eta6));
        
        theta1 = acos(dot(zeta1,eta1));
        theta2 = acos(dot(zeta2,eta2));
        theta3 = acos(dot(zeta3,eta3));
        theta4 = acos(dot(zeta4,eta4));
        theta5 = acos(dot(zeta5,eta5));
        theta6 = acos(dot(zeta6,eta6));
        
        if ((l1 >= -200) && (l1 <= 200) && (l2 >= -200) && (l2 <= 200) && (l3 >= -200) && (l3 <= 200) && (l4 >= -200) && (l4 <= 200) && (l5 >= -200) && (l5 <= 200) && (l6 >= -200) && (l6 <= 200) && (delta1 >= 0) && (delta1 <= pi/6) && (delta2 >= 0) && (delta2 <= pi/6) && (delta3 >= 0) && (delta3 <= pi/6) && (delta4 >= 0) && (delta4 <= pi/6) && (delta5 >= 0) && (delta5 <= pi/6) && (delta6 >= 0) && (delta6 <= pi/6) && (theta1 >= 0) && (theta1 <= pi/6) && (theta2 >= 0) && (theta2 <= pi/6) && (theta3 >= 0) && (theta3 <= pi/6) && (theta4 >= 0) && (theta4 <= pi/6) && (theta5 >= 0) && (theta5 <= pi/6) && (theta6 >= 0) && (theta6 <= pi/6))
            xdata(m,n) = x;
            ydata(m,n) = y;
            n = n + 1;
            
            A = [d1  0   0   0   0   0;
                 0  d2   0   0   0   0;
                 0   0  d3   0   0   0;
                 0   0   0  d4   0   0;
                 0   0   0   0  d5   0;
                 0   0   0   0   0  d6];
            B = [e1  f1  g1  L1  m1  n1;
                 e2  f2  g2  L2  m2  n2;
                 e3  f3  g3  L3  m3  n3;
                 e4  f4  g4  L4  m4  n4;
                 e5  f5  g5  L5  m5  n5;
                 e6  f6  g6  L6  m6  n6];
            if det(B)~= 0
                J = B\A;
                Jv = [J(1,1) J(1,2) J(1,3) J(1,4) J(1,5) J(1,6);
                      J(2,1) J(2,2) J(2,3) J(2,4) J(2,5) J(2,6);
                      J(3,1) J(3,2) J(3,2) J(3,2) J(3,2) J(3,2)];
                Kv(m,nn) = norm(Jv);
                nn = nn + 1;
            end
        end
    end
    m = m + 1;
end
toc
surf(xdata,ydata,Kv)

xlabel('x/mm');ylabel('y/mm');zlabel('K')