//---------------------------------------------
function my_eventhandler(win,x,y,ibut)
       global u x y z sortie ech;
       xinfo('Pour bouger u : x,y,z,X,Y,Z; Réglage Zoom : +,-; Fin : F;')
       if ascii(ibut)=='x' then x=x+1; u=[x/sqrt((x^2)+(y^2)+(z^2));y/sqrt((x^2)+(y^2)+(z^2));z/sqrt((x^2)+(y^2)+(z^2))]; end
       if ascii(ibut)=='y' then y=y+1; u=[x/sqrt((x^2)+(y^2)+(z^2));y/sqrt((x^2)+(y^2)+(z^2));z/sqrt((x^2)+(y^2)+(z^2))]; end
       if ascii(ibut)=='z' then z=z+1; u=[x/sqrt((x^2)+(y^2)+(z^2));y/sqrt((x^2)+(y^2)+(z^2));z/sqrt((x^2)+(y^2)+(z^2))]; end

       if ascii(ibut)=='X' then x=x-1; u=[x/sqrt((x^2)+(y^2)+(z^2));y/sqrt((x^2)+(y^2)+(z^2));z/sqrt((x^2)+(y^2)+(z^2))]; end
       if ascii(ibut)=='Y' then y=y-1; u=[x/sqrt((x^2)+(y^2)+(z^2));y/sqrt((x^2)+(y^2)+(z^2));z/sqrt((x^2)+(y^2)+(z^2))]; end
       if ascii(ibut)=='Z' then z=z-1; u=[x/sqrt((x^2)+(y^2)+(z^2));y/sqrt((x^2)+(y^2)+(z^2));z/sqrt((x^2)+(y^2)+(z^2))]; end
       

       if ascii(ibut)=='+' then ech=ech*0.95; end
       if ascii(ibut)=='-' then ech=ech/0.95; end

       if ascii(ibut)=='F' then sortie=%T;    end   
endfunction
//---------------------------------------------
function R=Rot(a,i);
    R=eye(4,4);
    J=modulo([i,i+1],3)+[1,1];
    R(J,J)=[cos(a) -sin(a);sin(a) cos(a)];  
endfunction
//---------------------------------------------
function T=Trans(v);
    T=eye(4,4);
    T(1:3,4)=v;
endfunction
//---------------------------------------------
function draw(euler1,euler2,u)
   Base=[5  5 -5 -5 5;
         5 -5 -5  5 5;
         0  0  0  0 0;
         1  1  1  1 1];
         
   RepGlob=[0 10 0  0 0  0;
            0  0 0 10 0  0;
            0  0 0  0 0 10
            1  1 1  1 1  1];



   M=[0 1 1 0 0 0 1 1 0 0 0 0 1 1 1 1;
      0 0 0 0 0 1 1 1 1 1 1 0 0 1 1 0;
      0 0 1 1 0 0 0 1 1 0 1 1 1 1 0 0;
      1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1] ;
   M=Trans([-0.5;-0.5;0])*M; //Pan&Tilt au centre

   M1=diag([1,1,3,1])*M;
   M2=diag([2,1,1,1])*M;

   Punto=[0 u(1)*5;
          0 u(2)*5;
          0 u(3)*5];

   R1=Rot(euler1(3),3)*Rot(euler1(2),2)*Rot(euler1(1),1);
   R2=Rot(euler2(3),3)*Rot(euler2(2),2)*Rot(euler2(1),1);
   R3=Rot(euler2(3),3);




   //Rglob=Rot(-%pi/4,1)*Rot(-%pi/4,2)*Rot(-%pi/4,3);

   //RepGlob=Rglob*RepGlob;

   T21=Trans([0.5;0;0]);
   T22=Trans([0;0;3]);

   //Base=Rglob*R1*Base;
   //M1=Rglob*R1*M1;
   //M2=Rglob*R1*T2*R2*M2;
   Base=R1*Base;
   M1=R1*R3*M1;
   M2=R1*T22*R2*T21*M2;

   xbasc();xset('thickness',2);
   isoview(-10,10,-10,10)
   //xpoly(M1(1,:),M1(3,:)); xpoly(M2(1,:),M2(3,:));xpoly(Base(1,:),Base(3,:));xpoly(RepGlob(1,:),RepGlob(3,:))
   //xpoly(M1(1,:),M1(3,:)); xpoly(M2(1,:),M2(3,:));xpoly(Base(1,:),Base(3,:));xpoly(RepGlob(1,:),RepGlob(3,:))
   xpoly(M1(1,:),M1(2,:)); xpoly(M2(1,:),M2(2,:));xpoly(Base(1,:),Base(2,:));xpoly(RepGlob(1,:),RepGlob(2,:))
   xpoly(Punto(1,:),Punto(2,:))
endfunction;
//---------------------------------------------

global u x y z sortie ech ;
x=-1;
y=-1;
//x=1;
//y=0;
z=0;
sortie=%F;
ech=2;

u=[x/sqrt((x^2)+(y^2)+(z^2));y/sqrt((x^2)+(y^2)+(z^2));z/sqrt((x^2)+(y^2)+(z^2))]; // u=[x;y;z] le point à observer

euler1=[0;0;0];  // Cond. Init. Centrale
euler2=[0;0;0];  // Cond. Init. Pan & Tilt
dt=0.001;

w1=[%pi/4;%pi/4;%pi/4]; // Orientation de la centrale w1=[roll,pitch,yaw]
//w1=[0;0;0];
alpha1=0.01; // Coef. regulateur proportionel pour animation de la centrale intertielle

seteventhandler('my_eventhandler');
while sortie==%F,

      // Calcul de la'orientation du Pan & Tilt pour pointer vers u

      Theta2=asin(-u(3)*cos(w1(1))*cos(w1(2))-u(1)*(sin(w1(1))*sin(w1(3))+cos(w1(1))*cos(w1(3))*sin(w1(2)))-u(2)*(cos(w1(1))*sin(w1(3))*sin(w1(2))-cos(w1(3))*sin(w1(1))));
      Psi2 = atan( ( u(3)*cos(w1(2))*sin(w1(1))+u(1)*(cos(w1(3))*sin(w1(1))*sin(w1(2))-cos(w1(1))*sin(w1(3)))+u(2)*(cos(w1(1))*cos(w1(3))+sin(w1(1))*sin(w1(2))*sin(w1(3))) ) , (u(1)*cos(w1(3))*cos(w1(2))+u(2)*cos(w1(2))*sin(w1(3))-u(3)*sin(w1(2))) );

      // avec Rmn Motion Node
      //Theta2=asin(-u(2)*cos(w1(1))*cos(w1(2))+u(1)*( sin(w1(1))*sin(w1(3))+cos(w1(1))*cos(w1(3))*sin(w1(2)))-u(3)*( cos(w1(1))*sin(w1(3))*sin(w1(2))-cos(w1(3))*sin(w1(1))));
      //Psi2 avec singularitées
      //Psi2=atan( u(2)*cos(w1(2))*sin(w1(1))+u(1)*( cos(w1(1))*sin(w1(3))-cos(w1(3))*sin(w1(1))*sin(w1(2)))+u(3)*( cos(w1(1))*cos(w1(3))+sin(w1(1))*sin(w1(3))*sin(w1(2))) , -u(1)*cos(w1(3))*cos(w1(1))+u(3)*cos(w1(2))*sin(w1(3))-u(2)*sin(w1(2)) );
      //Psi2 avec singularitées
      //Psi2=acos((-u(1)*cos(w1(3))*cos(w1(2))+u(3)*cos(w1(2))*sin(w1(3))-u(2)*sin(w1(2)))/(cos(Theta2)));
      //Psi2=asin((u(2)*cos(w1(2))*sin(w1(1))+u(1)*( cos(w1(1))*sin(w1(3))-cos(w1(3))*sin(w1(1))*sin(w1(2)) )+u(3)*( cos(w1(1)*cos(w1(3))+sin(w1(1)*sin(w1(3))*sin(w1(1)))) ) )/cos(Theta2));
      // avec Rmn Motion Node

      //w2=[0;0.52;-2.49]; // Modele directe
      w2=[0;Theta2;Psi2];  // Modele indirecte
      alpha2=0.01; // Coef. regulateur proportionel pour animation du pan & tilt

      euler1=euler1+alpha1*(w1-euler1);
      euler2=euler2+alpha2*(w2-euler2);
      draw(euler1,euler2,u);
end;
seteventhandler('') //suppress the event handler
xdel();
//---------------------------------------------