function my_eventhandler(win,xs,ys,ibut) global sortie ech x phi v x Hm dec longpend; [xs,ys]=xchange(xs,ys,'i2f') xinfo('Velocity : v,V; Zoom : z,Z; Stairs : h,H, Length : l,L, Shift : d,D ; End : F; '+ 'xs='+string(xs)+', ys='+string(ys)+', ibut='+string(ibut)) if ascii(ibut)=='v' then v=v+0.0005; end if ascii(ibut)=='V' then v=v-0.0005; end if ascii(ibut)=='h' then Hm=Hm+0.01; end if ascii(ibut)=='H' then Hm=Hm-0.01; end if ascii(ibut)=='l' then lengthPend=lengthPend*1.01; end if ascii(ibut)=='L' then lengthPend=lengthPend/1.01; end if ascii(ibut)=='d' then dec=dec+0.1; end if ascii(ibut)=='D' then dec=dec-0.1; end if ascii(ibut)=='t' then x=x+v; end if ascii(ibut)=='z' then ech=ech*0.95; end if ascii(ibut)=='Z' then ech=ech/0.95; end if ascii(ibut)=='F' then sortie=%T; end endfunction function DrawSegments(o,v,couleur,crayon) xset('color',couleur); xset('line style',crayon); xpoly([o(1),o(1)+v(1)],[o(2),o(2)+v(2)]); xset('line style',1); endfunction function DrawArrow(o,v,couleur) xset('color',couleur); xarrows([o(1),o(1)+v(1)],[o(2),o(2)+v(2)],0.02,couleur); endfunction function DrawCircle(c,r,couleur) xset('color',couleur); xarc(-r+c(1),r+c(2),2*r,2*r,0,0); endfunction function M=IntersectCircleLine(c,r,a,b) //Circle with center c and radius r with the line y=ax+b xc=c(1);yc=c(2); a1=a^2+1;b1=2*(a*b-a*yc-xc);c1=-2*b*yc+b^2+xc^2+yc^2-r^2; delta=b1^2-4*a1*c1; M=[]; if (delta>=0) then x1=(-b1-sqrt(delta))/(2*a1); y1=a*x1+b; x2=(-b1+sqrt(delta))/(2*a1); y2=a*x2+b; M=[x1 x2;y1 y2]; end endfunction function [m,p]=IntersectTwoCircles(a,b,ra,rb) cosalpha=(rb^2-ra^2-(norm(a-b))^2)/(-2*ra*norm(a-b)); if (cosalpha<0|cosalpha>1) then m=[]; p=[]; else sinalpha=sqrt(1-cosalpha^2); m=a+(ra/norm(a-b))*[cosalpha -abs(sinalpha);abs(sinalpha) cosalpha]*(b-a); p=b; end; endfunction function R=Rot(alpha) R=[cos(alpha),-sin(alpha);sin(alpha),cos(alpha)]; endfunction function draw(Fall,cone) xbasc(); xset('thickness',1); isoview(-ech+dec,ech+dec,-ech/2,1.5*ech); xset('color',noir); xstring(dec,-0.1,["Sliding Risk coefficient = "+string(round(crit))]); xstring(dec,-0.15,["Height of Stairs = "+string(Hm)]); xstring(dec,-0.20,["Velocity = "+string(v)]); for k=1:6, DrawSegments(cone(1:2,k),5*cone(3:4,k),vert,3); end; if ((crit<0) | (rand(1)<0.5)) then xset('thickness',1); for i=1:3 DrawCircle(Pall(:,i),0.01,bleu); DrawCircle(Call(:,i),0.01,rouge); DrawArrow(Pall(:,i),0.004*Fall(:,i),bleu); end DrawArrow(Call(:,2),0.002*Fall(:,4),rouge); xset('thickness',3); for i=3:4, DrawCircle(Mall(:,i),0.02,noir); end for k=1:3, DrawCircle(Call(:,k),rayon(k),rouge); end; end; xset('color',noir); xset('thickness',2); xpoly(Call(1,:),Call(2,:)); for i=1:imax; xpoly(Stairs(1,(2*i-2)+1:(2*i-2)+2),Stairs(2,(2*i-2)+1:(2*i-2)+2)); end xpoly([Call(1,1) Mall(1,3)],[Call(2,1) Mall(2,3)]); xpoly([Call(1,3) Mall(1,4)],[Call(2,3) Mall(2,4)]); endfunction; function [c2,p2]=PositionNextWheelOneSegment(c1,r2,a,b,l1) ab=b-a; a1=ab(2)/ab(1); b1=(r2*norm(ab)+det([ab,a]))/ab(1); c2=[];p2=[]; M=IntersectCircleLine(c1,l1,a1,b1); if (M<>[]) then [xmax,kmax]=max(M(1,:)) c2=M(:,kmax); if (((ab'*(c2-a))>0) & ((-ab'*(c2-b))>0)) then p2=inv([ab(1) ab(2);ab(2) -ab(1)])*[c2'*ab;det([a,ab])]; return; end; end; c2=[]; pab=[]; [Ma,pa]=IntersectTwoCircles(c1,a,l1,r2); [Mb,pb]=IntersectTwoCircles(c1,b,l1,r2); Mab=[Ma Mb]; pab=[pa pb]; if Mab<>[] then [aaa,kmax]=max(Mab(2,:)); c2=Mab(:,kmax); p2=pab(:,kmax); end; endfunction; function PositionNextWheelAllSegments(c1,l1,r2); global Pall Call; P=[]; M=[]; for i=1:imax; a=Stairs(:,(2*i-2)+1); b=Stairs(:,(2*i-2)+2); [c2i,p2i]=[PositionNextWheelOneSegment(c1,r2,a,b,l1)] if (c2i(1)-c1(1))>-(rayon(3)-rayon(2)) then M=[M,c2i]; P=[P,p2i]; end; end [aaa,kmax]=max(M(2,:)); c2=M(:,kmax); p2=P(:,kmax); Pall=[Pall,p2]; Call=[Call,c2]; endfunction function [Fall,Cone,crit,m3x,m4x]=commande(); g=9.81;mu1=7*g;mu2=7*g;mu3=4*g;mu4=4*g; c1=Call(:,1);c2=Call(:,2);c3=Call(:,3); p1=Pall(:,1);p2=Pall(:,2);p3=Pall(:,3); m1=Mall(:,1);m2=Mall(:,2); A1=[0 0;0 0;0 0; 0 0;1 0;0 1]; c1x=c1(1);c1y=c1(2);c2x=c2(1);c2y=c2(2);c3x=c3(1);c3y=c3(2); p1x=p1(1);p1y=p1(2);p2x=p2(1);p2y=p2(2);p3x=p3(1);p3y=p3(2); m1x=m1(1);m1y=m1(2);m2x=m2(1);m2y=m2(2); b=[mu1*(m1x-p1x)-mu3*p1x;mu2*(m2x-p2x)-mu4*p2x;0;mu1+mu3;0;mu2+mu4]; A0 =[0 , 0 , 0 , 0 , 0 , 0 ,p1y-c2y,c2x-p1x,-mu3, 0 , 0 ; 0 , 0 , 0 , 0 ,p2y-p3y,p3x-p2x,c2y-p2y,p2x-c2x, 0 ,-mu4, 0 ; 1 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 ; 0 , 1 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 ; 0 , 0 , 1 , 0 , 1 , 0 , -1 , 0 , 0 , 0 , 0 ; 0 , 0 , 0 , 1 , 0 , 1 , 0 , -1 , 0 , 0 , 0 ]; U=Call-Pall; u1minus=Rot(phi)'*U(:,1);u2minus=Rot(phi)'*U(:,2);u3minus=Rot(phi)'*U(:,3); u1plus =Rot(phi) *U(:,1);u2plus =Rot(phi) *U(:,2);u3plus =Rot(phi) *U(:,3); Cone=[p1,p1,p2,p2,p3,p3;u1minus,u1plus,u2plus,u2minus,u3minus,u3plus]; U1=[u1minus(2) -u1minus(1);-u1plus(2) u1plus(1)]; U2=[u2minus(2) -u2minus(1);-u2plus(2) u2plus(1)]; U3=[u3minus(2) -u3minus(1);-u3plus(2) u3plus(1)]; C0=[[U1,zeros(2,8);zeros(2,2),U2,zeros(2,6);zeros(2,4),U3,zeros(2,4)],-ones(6,1)]; C0=[C0;zeros(2,8),[-1 0 0;1 0 0];zeros(2,8),[0 -1 0;0 1 0]]; C0=[A0;C0]; b0=[b;zeros(6,1);-(c1x-(L(3)-0.01));c1x+(L(3)-0.01);-(c3x-(L(4)-0.01));c3x+(L(4)-0.01);]; ci=-10000*ones(11,1);cs=10000*ones(11,1);me=6; Q=zeros(11,11); Q(9,9)=2;Q(10,10)=2; // à cause du 1/2 de quapro p=[zeros(8,1);-2*c1x;-2*c3x;1]; x0=quapro(Q,p,C0,b0,ci,cs,me) crit=x0(11); Fall=[x0(1:2),x0(3:4),x0(5:6),x0(7:8)]; m3x=x0(9);m4x=x0(10); endfunction //--------------------------------------------- global sortie ech x v s Pall Call Mall crit phi v Hm dec lengthPend; dec=0; rouge=5;bleu=2;blanc=8;vert=3;noir=0;magenta=6; crit=-1; v=0.01;phi=0.6; sortie=%F; Hm=0.22;Lm=0.28; x=-1.80; r0=10000;ech=1; rayon=[0.085 0.075 0.085]; // radius of the wheels lengthPend=0.35 seteventhandler('my_eventhandler'); coefvitesse=1;CallOld=zeros(2,3); while sortie==%F, L=[0.180 0.180 lengthPend lengthPend]; Slopes= [3.8 0.01 0.1 0.01 0.4 Lm 0.01 Lm 0.01 Lm 0.01 Lm 0.01 0.4 0.4 0.5 0.5 0.5 0.3 0.02 0.1 ; 0 -Hm 0 Hm -0.1 0 Hm 0 Hm 0 Hm 0 Hm 0 -0.1 -0.3 0.2 -0.2 0 1.5*Hm 0]; Stairs=[-5;0.0]; for i=1:size(Slopes,2), Stairs=[Stairs,Stairs(:,$)+Slopes(:,i),Stairs(:,$)+Slopes(:,i)]; end Stairs=[Stairs,[10;0.3]]; imax=size(Stairs,2)/2; if (abs(x)>4) then x=-x; end; if crit<0 then x=x+v*coefvitesse; end Pall=[];Call=[]; PositionNextWheelAllSegments([-r0;0],x+r0,rayon(1)); for k=2:3, PositionNextWheelAllSegments(Call(:,$),L(k-1),rayon(k)); end m1=(Call(:,1)+Call(:,2))/2; m2=(Call(:,2)+Call(:,3))/2; Mall=[m1,m2]; [Fall,cone,crit,m3x,m4x]=commande(); Mall(1,3)=m3x;Mall(1,4)=m4x; [m3,aaa]=IntersectTwoCircles([-r0;0],Call(:,1),m3x+r0,L(3)); [m4,aaa]=IntersectTwoCircles([-r0;0],Call(:,3),m4x+r0,L(4)); if (m3<>[] & m4<>[]) then Mall(2,3)=m3(2);Mall(2,4)=m4(2); draw(Fall,cone); else crit=1; xstring(dec,-0.25,["Je glisse"]); end u=Pall(:,1)-Call(:,1); coefvitesse=1-0.9*abs(u(1)/norm(u)); end; seteventhandler('') xdel();