diff --git a/sw/simulator/scilab/q6d/povray/q3d.pov b/sw/simulator/scilab/q6d/povray/q3d.pov deleted file mode 100644 index f8d644aeaa..0000000000 --- a/sw/simulator/scilab/q6d/povray/q3d.pov +++ /dev/null @@ -1,2 +0,0 @@ -#include "povray/q3d.inc" -object { Q3D() rotate <-1.881324,-27.550064,-18.630888> translate <2787.790787,-1108.252014,-4762.512255>} \ No newline at end of file diff --git a/sw/simulator/scilab/q6d/povray/q3d.inc b/sw/simulator/scilab/q6d/povray/q6d.inc similarity index 53% rename from sw/simulator/scilab/q6d/povray/q3d.inc rename to sw/simulator/scilab/q6d/povray/q6d.inc index 7eb9409e39..f1901b9e81 100644 --- a/sw/simulator/scilab/q6d/povray/q3d.inc +++ b/sw/simulator/scilab/q6d/povray/q6d.inc @@ -1,4 +1,3 @@ - //POVRay include files #include "arrays.inc" #include "chars.inc" @@ -21,20 +20,79 @@ #include "textures.inc" #include "transforms.inc" +#macro AXIS_POVRAY(mac_x_ver,mac_y_ver,mac_z_ver,mac_x_rot,mac_y_rot,mac_z_rot) +object { + union { + union { + cylinder{<0.,0.,0.><2500.,0.,0.> 10} + text { ttf "timrom.ttf" "X" 1, 0 scale <2,2,2> translate(<20, 0, 0>)} + texture{ pigment{rgb<1,0,0>}} + } + union { + cylinder{<0.,0,0.><0.,2500.,0.> 10} + text { ttf "timrom.ttf" "Y" 1, 0 scale <2,2,2> translate(<0, 20, 0>)} + texture{ pigment{rgb<0,1,0>}} + } + union { + cylinder{<0.,0,0.><0.,0.,2500.> 10} + text { ttf "timrom.ttf" "Z" 1, 0 scale <2,2,2> translate(<0, 0, 20>)} + texture{ pigment{rgb<0,0,1>}} + } + translate() + rotate() + } +} +#end -#macro Q3D() -union { - box{<-250,-10,-10><250,10,10> texture{pigment{Red}}} - cylinder{<-250,10,0><-250,15,0> 125 texture{pigment{Green}}} - cylinder{<250,10,0><250,15,0> 125 texture{pigment{Green}}} - box{<-10,-10,-250><10,10,250> texture{pigment{Red}}} - cylinder{<0,10,-250><0,15,-250> 125 texture{pigment{Green}}} - cylinder{<0,10, 250><0,15, 250> 125 texture{pigment{Green}}} +#macro AXIS_NED() +object { + union { + union { + cylinder{<0.,0.,0.><2500.,0.,0.> 10} + texture{ pigment{rgb<1,0,0>}} + } + union { + cylinder{<0.,0,0.><0.,2500.,0.> 10} + texture{ pigment{rgb<0,1,0>}} + } + union { + cylinder{<0.,0,0.><0.,0.,2500.> 10} + texture{ pigment{rgb<0,0,1>}} + } + } + matrix < 1, 0, 0, + 0, 0,-1, + 0,-1, 0, + 0, 0, 0 > +} +#end + + +#macro Q6D(xned, yned, zned, phi, theta, psi) + +object { + union { + box{<-250,-10,-10><0,10,10> texture{pigment{Blue}}} + box{<0,-10,-10><250,10,10> texture{pigment{Red}}} + cylinder{<-250,0,-10><-250,0,-15> 125 texture{pigment{Green}}} + cylinder{<250, 0,-10><250,0,-15> 125 texture{pigment{Green}}} + + box{<-10,-250,-10><10,250,10> texture{pigment{Blue}}} + cylinder{<0,-250,-10><0,-250,-15> 125 texture{pigment{Green}}} + cylinder{<0, 250,-10><0, 250,-15> 125 texture{pigment{Green}}} + } + rotate + translate + matrix < 1, 0, 0, + 0, 0,-1, + 0,-1, 0, + 0, 0, 0 > } #end + #declare cam_x = 0; #declare cam_y = 5000; #declare cam_z = 20000; @@ -43,7 +101,7 @@ union { #declare cam_look_y = 0; #declare cam_look_z = 0; -#declare cam_a = 32; +#declare cam_a = 36; //#declare cam_a = 10; camera diff --git a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci index 452d14a55b..4a6a094170 100644 --- a/sw/simulator/scilab/q6d/q6d_diff_flatness.sci +++ b/sw/simulator/scilab/q6d/q6d_diff_flatness.sci @@ -79,10 +79,9 @@ function [state] = df_state_of_fo(fo) phid = sign(z2dmg)*(adypsi*av-adv*aypsi)/(aypsi^2+av^2); thetad = (adxpsi*z2dmg-z3d*axpsi)/(axpsi^2+z2dmg^2); - + cphi = cos(state(DF_REF_PHI)); sphi = sin(state(DF_REF_PHI)); - ctheta = cos(state(DF_REF_THETA)); stheta = sin(state(DF_REF_THETA)); @@ -139,7 +138,6 @@ function [inp] = df_input_of_fo(fo) adv = (axpsi*adxpsi + z2dmg*z3d)/av; z4d = fo(3,5); a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d^2)*av; - //a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d)*av; b = -adv*(axpsi*adxpsi + z2dmg*z3d); a2dv = (a+b)/av^2; @@ -147,7 +145,6 @@ function [inp] = df_input_of_fo(fo) theta = atan(axpsi/z2dmg); phid = sign(z2dmg)*(adypsi*av-adv*aypsi)/(aypsi^2+av^2); - //thetad = (adxpsi*z2dmg-z3d*aypsi)/(axpsi^2+z2dmg^2); thetad = (adxpsi*z2dmg-z3d*axpsi)/(axpsi^2+z2dmg^2); a = (a2dypsi*av-a2dv*aypsi)*(aypsi^2+av^2); diff --git a/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci b/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci index d843642115..ed28cd7fe2 100644 --- a/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci +++ b/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci @@ -1,3 +1,21 @@ +function [time_out, traj_out] = merge_traj(time_in, traj_in) + time_out = []; + for t=time_in + time_out = [time_out t]; + end + traj_out = 0; + [nb_comp, nb_order, foo] = size(traj_in(1)); + traj_out = zeros(nb_comp, nb_order, length(time_out)); + + l=1; + for i=1:length(time_in) + for j=1:length(time_in(i)) + ti = traj_in(i); + traj_out(:,:,l) = ti(:,:,j); + l=l+1; + end + end +endfunction function [fo_traj] = fo_traj_circle(time) @@ -7,23 +25,37 @@ function [fo_traj] = fo_traj_circle(time) radius = 3; omega = rad_of_deg(60); c = [0 0]'; + omega_z = rad_of_deg(105); + dz = 1.; for i=1:length(time) fo_traj(DF_FO_X,1,i) = c(1) + radius*cos(omega*time(i)); fo_traj(DF_FO_Y,1,i) = c(2) + radius*sin(omega*time(i)); + fo_traj(DF_FO_Z,1,i) = dz*sin(omega_z*time(i)); +// fo_traj(DF_FO_PSI,1,i) = omega*time(i); + fo_traj(DF_FO_X,2,i) = c(1) - omega*radius*sin(omega*time(i)); fo_traj(DF_FO_Y,2,i) = c(2) + omega*radius*cos(omega*time(i)); - + fo_traj(DF_FO_Z,2,i) = omega_z*dz*cos(omega_z*time(i)); +// fo_traj(DF_FO_PSI,2,i) = omega; + + fo_traj(DF_FO_X,3,i) = c(1) - omega^2*radius*cos(omega*time(i)); fo_traj(DF_FO_Y,3,i) = c(2) - omega^2*radius*sin(omega*time(i)); - + fo_traj(DF_FO_Z,3,i) = -omega_z^2*dz*sin(omega_z*time(i)); +// fo_traj(DF_FO_PSI,3,i) = 0; + + fo_traj(DF_FO_X,4,i) = c(1) + omega^3*radius*sin(omega*time(i)); fo_traj(DF_FO_Y,4,i) = c(2) - omega^3*radius*cos(omega*time(i)); + fo_traj(DF_FO_Z,4,i) = -omega_z^3*dz*cos(omega_z*time(i)); + fo_traj(DF_FO_X,5,i) = c(1) + omega^4*radius*cos(omega*time(i)); fo_traj(DF_FO_Y,5,i) = c(2) + omega^4*radius*sin(omega*time(i)); + fo_traj(DF_FO_Z,5,i) = omega_z^4*dz*sin(omega_z*time(i)); end diff --git a/sw/simulator/scilab/q6d/q6d_povray.sci b/sw/simulator/scilab/q6d/q6d_povray.sci index 3382cc7713..9ed6b66323 100644 --- a/sw/simulator/scilab/q6d/q6d_povray.sci +++ b/sw/simulator/scilab/q6d/q6d_povray.sci @@ -1,6 +1,7 @@ + function povray_draw( time, diff_flat_ref ) - + dt_display = 1/25; t_idx = 1; @@ -8,18 +9,19 @@ function povray_draw( time, diff_flat_ref ) t = time(t_idx); while (t_idx translate <%f,%f,%f>}",-phi,psi,-theta,x,y,z); + px = 1000*diff_flat_ref(DF_REF_X,t_idx); + py = 1000*diff_flat_ref(DF_REF_Y,t_idx); + pz = 1000*diff_flat_ref(DF_REF_Z,t_idx); + _phi = deg_of_rad(diff_flat_ref(DF_REF_PHI,t_idx)); + _theta = deg_of_rad(diff_flat_ref(DF_REF_THETA,t_idx)); + _psi = deg_of_rad(diff_flat_ref(DF_REF_PSI,t_idx)); +// printf('( %f %f %f - %f %f %f)\n', px, py, pz, alpha, beta, gamma); + fid = mopen('povray/q6d.pov', "w"); + mfprintf(fid, "#include ""povray/q6d.inc""\n"); + mfprintf(fid, "object { Q6D(%f,%f,%f,%f,%f,%f)}\n",px,py,pz,_phi,_theta,_psi); + mfprintf(fid, "object { AXIS_NED()}\n"); mclose(fid); - cmd = sprintf('povray povray/q3d.pov +Opovray/foo%04d.png Display=false +W800 +H600 +Q9 +A0.3 +R5', f_idx); + cmd = sprintf('povray povray/q6d.pov +Opovray/img%04d.png Display=false +W800 +H600 +Q9 +A0.3 +R5', f_idx); a = unix_g(cmd); while (t_idx