bug 11519 : Do not invert the curves order.
[scilab.git] / scilab / modules / cacsd / macros / nyquist.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) 1984-2011 - INRIA - Serge STEER
3 // This file must be used under the terms of the CeCILL.
4 // This source file is licensed as described in the file COPYING, which
5 // you should have received as part of this distribution.  The terms
6 // are also available at
7 // http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt
8
9 function nyquist(varargin)
10 // Nyquist plot
11 //!
12   rhs=size(varargin);
13
14   if rhs == 0 then
15     //Hall chart as a grid for nyquist
16     s=poly(0,'s');
17     Plant=syslin('c',16000/((s+1)*(s+10)*(s+100)));
18     //two degree of freedom PID
19     tau=0.2;xsi=1.2;
20     PID=syslin('c',(1/(2*xsi*tau*s))*(1+2*xsi*tau*s+tau^2*s^2));
21     nyquist([Plant;Plant*PID],0.5,100,["Plant";"Plant and PID corrector"]);
22     hallchart(colors=color('light gray')*[1 1])
23     //move the caption in the lower rigth corner
24     ax=gca();Leg=ax.children(1);
25     Leg.legend_location="in_upper_left";
26     return;
27   end
28
29   symmetry=%t
30   if type(varargin(rhs))==4 then //symmetrization flag
31     symmetry=varargin(rhs)
32     rhs=rhs-1
33   end
34   if type(varargin(rhs))==10 then
35     comments=varargin(rhs);
36     rhs=rhs-1;
37   else
38     comments=[];
39   end
40   fname="nyquist";//for error messages
41   fmax=[];
42   if or(typeof(varargin(1))==["state-space" "rational"]) then
43     //sys,fmin,fmax [,pas] or sys,frq
44     refdim=1; //for error message
45     sltyp=varargin(1).dt;
46     if rhs==1 then
47       [frq,repf,splitf]=repfreq(varargin(1),1d-3,1d3);
48     elseif rhs==2 then //sys,frq
49       if size(varargin(2),2)<2 then
50         error(msprintf(_("%s: Wrong size for input argument #%d: A row vector with length>%d expected.\n"),fname,2,1))
51       end
52       [frq,repf]=repfreq(varargin(1:rhs));
53     elseif or(rhs==(3:4)) then //sys,fmin,fmax [,pas]
54       [frq,repf,splitf]=repfreq(varargin(1:rhs));
55     else
56       error(msprintf(_("%s: Wrong number of input arguments: %d to %d expected.\n"),fname,1,5))
57     end
58   elseif  type(varargin(1))==1 then
59     //frq,db,phi [,comments] or frq, repf [,comments]
60     refdim=2;
61     sltyp="x";
62     splitf=[];
63     splitf=1;
64     select rhs
65     case 2 then //frq,repf
66       frq=varargin(1);
67       repf=varargin(2);
68       if size(frq,2)<2 then
69         error(msprintf(_("%s: Wrong size for input argument #%d: A row vector with length>%d expected.\n"),fname,1,1))
70       end
71       if size(frq,2)<>size(varargin(2),2) then
72         error(msprintf(_("%s: Incompatible input arguments #%d and #%d: Same column dimensions expected.\n"),fname,1,2))
73       end
74
75     case 3 then  //frq,db,phi
76       frq=varargin(1);
77       if size(frq,2)<>size(varargin(2),2) then
78         error(msprintf(_("%s: Incompatible input arguments #%d and #%d: Same column dimensions expected.\n"),fname,1,2));
79       end
80       if size(frq,2)<>size(varargin(3),2) then
81         error(msprintf(_("%s: Incompatible input arguments #%d and #%d: Same column dimensions expected.\n"),fname,1,3));
82       end
83       repf=exp(log(10)*varargin(2)/20 + %pi*%i/180*varargin(3));
84
85     else
86       error(msprintf(_("%s: Wrong number of input arguments: %d to %d expected.\n"),fname,2,4))
87     end
88   else
89     error(msprintf(_("%s: Wrong type for input argument #%d: Linear dynamical system or row vector of floats expected.\n"),fname,1));
90   end;
91   if size(frq,1)==1 then
92     ilf=0;
93   else
94     ilf=1;
95   end
96
97   [mn,n]=size(repf);
98   if and(size(comments,"*")<>[0 mn]) then
99     error(msprintf(_("%s: Incompatible input arguments #%d and #%d: Same number of elements expected.\n"),fname,refdim,rhs+1));
100   end
101   //
102
103   repi=imag(repf);
104   repf=real(repf);
105
106   // computing bounds of graphic window
107   mnx=min(-1,min(repf));// to make the critical point visible
108   mxx=max(-1,max(repf));
109
110   if symmetry then
111     mxy=max(0,max(abs(repi)));
112     mny=min(0,-mxy);
113   else
114     mxy=max(0,max(repi));
115     mny=min(0,min(repi));
116   end
117   dx=(mxx-mnx)/30;
118   dy=(mxy-mny)/30;
119   rect=[mnx-dx,mny-dy;mxx+dx,mxy+dy];
120
121   fig=gcf();
122   immediate_drawing=fig.immediate_drawing;
123   fig.immediate_drawing="off";
124
125   ax=gca();
126   if ax.children==[] then
127     ax.data_bounds=rect;
128     ax.axes_visible="on";
129     ax.grid=color("lightgrey")*ones(1,3)
130     ax.title.text=_("Nyquist plot");
131     if sltyp=="c" then
132       ax.x_label.text=_("Re(h(2iπf))");
133       ax.y_label.text=_("Im(h(2iπf))");
134     elseif sltyp=="x" then
135       ax.x_label.text=_("Re");
136       ax.y_label.text=_("Im");
137     else
138       ax.x_label.text=_("Re(h(exp(2iπf*dt)))");
139       ax.y_label.text=_("Im(h(exp(2iπf*dt)))");
140     end
141   else
142     rect= ax.data_bounds
143     mnx=rect(1,1);
144     mxx=rect(2,1)
145     mny=rect(1,2)
146     mxy=rect(2,2)
147   end
148
149   // drawing the curves
150   splitf($+1)=n+1;
151
152   ksplit=1;sel=splitf(ksplit):splitf(ksplit+1)-1;
153   R=[repf(:,sel)];  I=[repi(:,sel)];
154   F=frq(:,sel);
155   for ksplit=2:size(splitf,"*")-1
156     sel=splitf(ksplit):splitf(ksplit+1)-1;
157     R=[R %nan(ones(mn,1)) repf(:,sel)];
158     I=[I %nan(ones(mn,1)) repi(:,sel)];
159     F=[F %nan(ones(size(frq,1),1)) frq(:,sel)];
160   end
161   Curves=[]
162
163   kf=1
164   if symmetry then
165     for k=1:mn
166       xpoly([R(k,:) R(k,$:-1:1)],[I(k,:) -I(k,$:-1:1)]);
167       e=gce();e.foreground=k;
168       datatipInitStruct(e,"formatfunction","formatNyquistTip","freq",[F(kf,:) F(kf,$:-1:1)])
169       Curves=[Curves,e];
170       kf=kf+ilf;
171     end
172   else
173     for k=1:mn
174       xpoly(R(k,:),I(k,:));
175       e=gce();e.foreground=k;
176       datatipInitStruct(e,"formatfunction","formatNyquistTip","freq",F(kf,:))
177       Curves=[Curves,e];
178       kf=kf+ilf;
179     end
180   end
181   clear R I
182
183   kk=1;p0=[repf(:,kk) repi(:,kk)];ks=1;d=0;
184   dx=rect(2,1)-rect(1,1);
185   dy=rect(2,2)-rect(1,2);
186   dx2=dx^2;
187   dy2=dy^2;
188
189   // collect significant frequencies along the curve
190   //-------------------------------------------------------
191   Ic=min(cumsum(sqrt((diff(repf,1,"c").^2)/dx2+ (diff(repi,1,"c").^2)/dy2),2),"r");
192   kk=1;
193   L=0;
194   DIc=0.2;
195   while %t
196     ksup=find(Ic-L>DIc);
197     if ksup==[] then break,end
198     kk1=min(ksup);
199     L=Ic(kk1);
200     Ic(1:kk1)=[];
201     kk=kk+kk1;
202
203     if min(abs(frq(:,ks($))-frq(:,kk))./abs(frq(:,kk)))>0.001 then
204       if min(sqrt(((repf(:,ks)-repf(:,kk)*ones(ks)).^2)/dx2+..
205                   ((repi(:,ks)-repi(:,kk)*ones(ks)).^2)/dy2)) >DIc then
206         ks=[ks kk];
207         d=0;
208       end
209     end
210   end
211   if ks($)~=n then
212     if min(((repf(:,ks(1))-repf(:,n))^2)/dx2+((repi(:,ks(1))-repi(:,n))^2)/dy2)>0.01 then
213       ks=[ks n];
214     end
215   end
216   // display of parametrization (frequencies along the curve)
217   //-------------------------------------------------------
218   kf=1
219   if ks($)<size(repf,2) then last=$;else last=$-1;end
220   for k=1:mn,
221     L=[];
222     for kks=ks
223       xstring(repf(k,kks),repi(k,kks),msprintf("%-0.3g",frq(kf,kks)),0);
224       e=gce();e.font_foreground=k;
225       L=[e L];
226       if symmetry&(abs(repi(k,kks))>mxy/20) then //not to overlap labels
227         xstring(repf(k,kks),-repi(k,kks),msprintf("%-0.3g",-frq(kf,kks)),0);
228         e=gce();e.font_foreground=k;
229         L=[e L];
230       end
231     end
232     L=glue(L);
233     A=[];
234
235     if size(ks,"*")>1 then
236       dr=repf(k,ks(1:last)+1)-repf(k,ks(1:last));
237       di=repi(k,ks(1:last)+1)-repi(k,ks(1:last));
238       dd=1500*sqrt((dr/dx).^2+(di/dy).^2);
239       dr=dr./dd;
240       di=di./dd;
241       // we should use xarrows or xsegs here.
242       // However their displayed arrow size depends
243       // on the data bounds and we want to avoid this
244       if symmetry then
245         xx=[repf(k,ks(1:last))         repf(k,ks(last:-1:1))+dr($:-1:1) ;
246             repf(k,ks(1:last))+dr      repf(k,ks(last:-1:1))]
247         yy=[repi(k,ks(1:last))        -repi(k,ks(last:-1:1))-di($:-1:1) ;
248             repi(k,ks(1:last))+di     -repi(k,ks(last:-1:1))]
249       else
250         xx=[repf(k,ks(1:last))    ;
251             repf(k,ks(1:last))+dr]
252         yy=[repi(k,ks(1:last));
253             repi(k,ks(1:last))+di]
254       end
255       xpolys(xx,yy)
256       //xarrows([repf(k,ks(1:last));repf(k,ks(1:last))+dr],..
257       //    [repi(k,ks(1:last));repi(k,ks(1:last))+di],1.5)
258       A=gce();
259       A.children.arrow_size_factor = 1.5;
260       A.children.polyline_style = 4;
261       A.children.foreground=k;
262     end
263
264     kf=kf+ilf;
265     glue([Curves(k) glue([L A])]);
266
267   end;
268
269   if comments<>[] then
270     legend(Curves, comments);
271   end
272   fig.immediate_drawing=immediate_drawing;
273 endfunction
274
275 function str=formatNyquistTip(curve,pt,index)
276 //This function is called by the datatip mechanism to format the tip
277 //string for the nyquist curves.
278   ud=datatipGetStruct(curve);
279   if index<>[] then
280     f=ud.freq(index);
281   else //interpolated
282     [d,ptp,i,c]=orthProj(curve.data,pt);
283     f=ud.freq(i)+(ud.freq(i+1)-ud.freq(i))*c;
284   end
285   str=msprintf("%.4g%+.4gi\n%.4g"+_("Hz"), pt,f);
286 endfunction