sczip.m

来自「针对移动机械臂运动学」· M 代码 · 共 54 行

function d = sczip (M,v)
%SCZIP(Matrix, Option) Simplified notation: Sin(qn) = Sn and Cos(qn) = Cn
%      1 - SC0 = sczip(M)
%      2 - SC0 = sczip(M, manipulator)

%July ~ December 2005, by Reinaldo M. do Nascimento 

k=[];l=[];
O=M;
if nargin==1,v=ffx(M);
elseif isa(v,'struct'),v=v.chain.variables;
end
for i=1:length(v)
     m=0;
     c=double(char(v(i)));
     n=length(c);
     for j=1:length(k)
          if k(j)==c(n)
                 m=1;
                 break
          end
     end
     k=[k c(n)];
     if m==0
         M=subs(M,{cos(v(i)),sin(v(i))},{char([99 c(n)]),char([115 c(n)])},0);
         l=[l char([32 c(n)])];
     else
         M=subs(M,{cos(v(i)),sin(v(i))},{char([99 c(n) 48+i]),char([115 c(n) 48+i])},0);
         l=[l char([32 c(n) 48+i])];
     end
     if M==O
         l=[l char(120)];
     else
         O=M;
     end
end
%disp(v);
%disp(l);
d=M;    

function h = ffx (M)

s=findsym(M);
k=1;h=[];
for i=1:length(s)
     if s(i)==44
            h=[h sym(s(k:i-1))];
            k=i+1;
     elseif i==length(s)
             h=[h sym(s(k:length(s)))];
     end
end
h=h(length(h):-1:1);