home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / octa21eb.zip / octave / SCRIPTS.ZIP / scripts / quaternion / demoquat.m next >
Text File  |  1999-03-05  |  9KB  |  234 lines
  1.  
  2. # Thanks to Mr. Charles Hall, Dr. Don Krupp and Dr. Larry Mullins at
  3. #  NASA's Marshall Space Flight Center for notes and instruction in
  4. # use and conventions with quaternions.    - A. S. Hodel
  5.  
  6. function opt = demoquat()
  7.  
  8. opt = 0;
  9. quitopt = 5;
  10. while(opt != quitopt)
  11.   opt = menu("Quaternion function demo (c) 1998 A. S. Hodel, a.s.hodel@eng.auburn.edu",
  12.     "quaternion construction/data extraction",
  13.     "simple quaternion functions",
  14.     "transformation functions",
  15.     "body-inertial frame demo",
  16.     "Quit");
  17.  
  18.   switch(opt)
  19.   case(1),
  20.     printf("Quaternion construction/data extraction\n");
  21.     help quaternion
  22.     prompt
  23.     cmd = "q = quaternion(1,2,3,4)";
  24.     run_cmd
  25.     disp("This format stores the i,j,k parts of the quaternion first;")
  26.     disp("the real part is stored last.")
  27.     prompt
  28.     disp(" ")
  29.     disp("i, j, and k are all square roots of -1; however they do not")
  30.     disp("commute under multiplication (discussed further with the function")
  31.     disp("qmult).  Therefore quaternions do not commute under multiplcation:")
  32.     disp("    q1*q2 != q2*q1 (usually)")
  33.     prompt
  34.  
  35.     disp("Quaternions as rotations: unit quaternion to represent")
  36.     disp("rotation of 45 degrees about the vector [1 1 1]")
  37.     cmd = "degrees = pi/180; q1 = quaternion([1 1 1],45*degrees)";
  38.     run_cmd
  39.     prompt
  40.     cmd = "real_q = cos(45*degrees/2)";
  41.     run_cmd
  42.     printf("The real part of the quaternion q(4) is cos(theta/2).\n----\n\n");
  43.     cmd = "imag_q = sin(45*degrees/2)*[1 1 1]/norm([1 1 1])"
  44.     run_cmd
  45.     disp("The imaginary part of the quaternion is sin(theta/2)*unit vector");
  46.     disp("The constructed quaternion is a unit quaternion.");
  47.     prompt
  48.     disp("Can also extract both forms of the quaternion:")
  49.     disp("Vector/angle form of 1i + 2j + 3k + 4:")
  50.     cmd = "[vv,th] = quaternion(q)";
  51.     run_cmd
  52.     cmd = "vv_norm = norm(vv)";
  53.     run_cmd
  54.     disp("Returns the eigenaxis as a 3-d unit vector");
  55.     disp("Check values: ")
  56.     cmd = "th_deg = th*180/pi";
  57.     run_cmd
  58.     disp("")
  59.     disp("This concludes the quaternion construction/extraction demo.");
  60.     prompt
  61.  
  62.   case(2),
  63.     printf("Simple quaternion functions\n");
  64.     cmd = "help qconj";
  65.     run_cmd
  66.     cmd = "degrees = pi/180; q1 = quaternion([1 1 1],45*degrees)";
  67.     run_cmd
  68.     cmd = "q2 = qconj(q1)";
  69.     run_cmd
  70.     disp("The conjugate changes the sign of the complex part of the")
  71.     printf("quaternion.\n\n");
  72.     prompt
  73.     printf("\n\n\nMultiplication of quaternions:\n");
  74.     cmd = "help qmult";
  75.     run_cmd
  76.     cmd = "help qinv"
  77.     run_cmd
  78.     disp("Inverse quaternion: q*qi = qi*q = 1:")
  79.     cmd = "q1i = qinv(q1)";
  80.     run_cmd
  81.     cmd = "one = qmult(q1,q1i)";
  82.     run_cmd
  83.     
  84.     printf("Conclusion of simple quaternion functions");
  85.     prompt
  86.   case(3),
  87.     printf("Transformation functions\n");
  88.     disp("A problem with the discussion of coordinate transformations is that");
  89.     disp("one must be clear on what is being transformed: does a rotation of");
  90.     disp("theta degrees mean that you're rotating the VECTOR by theta degrees,");
  91.     disp("also called the 'active convention,' or does it mean that you rotate ");
  92.     disp("the COORDINATE FRAME by theta degrees, also called the 'passive convention,' ");
  93.     disp("which is equivalent to rotating the VECTOR by (-theta) degrees.  The");
  94.     disp("functions in this demo use the active convention.  I'll point out where");
  95.     disp("this changes the code as the demo runs.");
  96.     disp("    -- The author");
  97.     prompt
  98.     printf("\n\n");
  99.     disp("Sequences of rotations:")
  100.     printf("\n\nRotation of a vector by 90 degrees about the reference z axis\n");
  101.     cmd = "qz = quaternion([0 0 1], pi/2);";
  102.     disp(cmd) ; eval(cmd);
  103.     printf("\n\nRotation of a vector by 90 degrees about the reference y axis\n");
  104.     cmd="qy = quaternion([0 1 0], pi/2);";
  105.     disp(cmd) ; eval(cmd);
  106.     printf("\n\nRotation of a vector by 90 degrees about the reference x axis\n");
  107.     cmd="qx = quaternion([1 0 0], pi/2);";
  108.     run_cmd
  109.     printf("\n\nSequence of three rotations: 90 degrees about x, then 90 degrees\n");
  110.     disp("about y, then 90 degrees about z (all axes specified in the reference frame):");
  111.     qchk = qmult(qz,qmult(qy,qx));
  112.     cmd = "[vv,th] = quaternion(qchk), th_deg = th*180/pi";
  113.     run_cmd
  114.     disp("The sequence of the three rotations above is equivalent to a single rotation")
  115.     disp("of 90 degrees about the y axis. Check:");
  116.     cmd = "err = norm(qchk - qy)";
  117.     run_cmd
  118.     
  119.     disp("Transformation of a quaternion by a quaternion:")
  120.     disp("The three quaternions above were rotations specified about")
  121.     disp("a single reference frame.  It is often convenient to specify the");
  122.     disp("eigenaxis of a rotation in a different frame (e.g., when computing");
  123.     disp("the transformation rotation in terms of the Euler angles yaw-pitch-roll).");
  124.     cmd = "help qtrans";
  125.     run_cmd
  126.     disp("")
  127.     disp("NOTE: If the passive convention is used, then the above");
  128.     disp("formula changes to   v = qinv(q)*v*q  instead of ")
  129.     disp("v = q*v*qinv(q).")
  130.     prompt
  131.     disp("")
  132.     disp("Example: Vectors in Frame 2 are obtained by rotating them from ")
  133.     disp("   from Frame 1 by 90 degrees about the x axis (quaternion qx)")
  134.     disp("   A quaternion in Frame 2 rotates a vector by 90 degrees about")
  135.     disp("   the Frame 2 y axis (quaternion qy).  The equivalent rotation")
  136.     disp("   in the reference frame is:")
  137.     cmd = "q_eq = qtrans(qy,qx); [vv,th] = quaternion(q_eq)";
  138.     run_cmd
  139.     disp("The rotation is equivalent to rotating about the reference z axis")
  140.     disp("by 90 degrees (quaternion qz)")
  141.     prompt
  142.  
  143.     disp("Transformation of a vector by a quaternion");
  144.     cmd = "help qtransv";
  145.     run_cmd
  146.  
  147.     disp("NOTE: the above formula changes if the passive quaternion ")
  148.     disp("is used; the cross product term is subtracted instead of added.");
  149.     prompt
  150.     disp("Example: rotate the vector [1,1,1] by 90 degrees about the y axis");
  151.     cmd = "vec_r = qtransv([1,1,1],qy)";
  152.     run_cmd
  153.     prompt
  154.     disp("Equivalently, one may multiply by qtransvmat:")
  155.     cmd = "help qtransvmat";
  156.     run_cmd
  157.     disp("NOTE: the passive quaternion convention would use the transpose")
  158.     disp("(inverse) of the orthogonal matrix returned by qtransvmat.");
  159.     prompt
  160.     cmd = "vec_r_2 = qtransvmat(qy)*[1;1;1]; vec_err = norm(vec_r - vec_r_2)";
  161.     run_cmd
  162.  
  163.     disp("")
  164.     disp("The last transformation function is the derivative of a quaternion")
  165.     disp("Given rotation rates about the reference x, y, and z axes.");
  166.     cmd = "help qderivmat";
  167.     run_cmd
  168.     disp("")
  169.     disp("Example:")
  170.     disp("Frame is rotating about the z axis at 1 rad/s")
  171.     cmd = "Omega = [0,0,1]; Dmat = qderivmat(Omega)";
  172.     run_cmd
  173.     disp("Notice that Dmat is skew symmetric, as it should be.")
  174.     disp("expm(Dmat*t) is orthogonal, so that unit quaternions remain")
  175.     disp("unit quaternions as the rotating frame precesses.");
  176.     disp(" ")
  177.     disp("This concludes the transformation demo.");
  178.     prompt;
  179.   case(4),
  180.     printf("Body-inertial frame demo: Look at the source code for\n");
  181.     printf("demoquat.m and qcoordinate_plot.m to see how it's done.\n");
  182.     
  183.     # i,j,k units
  184.     iv = quaternion(1,0,0,0); jv = quaternion(0,1,0,0);
  185.     kv = quaternion(0,0,1,0);
  186.     
  187.     # construct quaternion to desired view.
  188.     degrees = pi/180; daz = 45*degrees; del = -30*degrees;
  189.     qazimuth = quaternion([0,0,1],daz);
  190.     qelevation = quaternion([cos(daz),sin(daz),0],del);
  191.     qview = qmult(qelevation,qazimuth);
  192.     
  193.     # inertial frame i, j, k axes.
  194.     iif = iv; jf = qtrans(jv,iv); kf = qtrans(kv,iv);
  195.     
  196.     # rotation steps
  197.     th = 0:5:20; ov = ones(size(th)); myth = [th,max(th)*ov ; 0*ov,th];
  198.     
  199.     # construct yaw-pitch-roll cartoon
  200.     for kk=1:length(myth(1,:))
  201.       figure(kk)
  202.       thy = myth(1,kk);
  203.       thp = myth(2,kk);
  204.      
  205.       qyaw = quaternion([0,0,1],thy*pi/180);
  206.       [jvy,th] = quaternion(qtrans(jf,qyaw));
  207.       qpitch = quaternion(jvy(1:3),thp*pi/180);
  208.       qb = qmult(qpitch, qyaw);
  209.       qi = quaternion([1, 0, 0],180*degrees);
  210.     
  211.       printf("yaw=%8.4f, pitch=%8.4f, \n    qbi = (%8.4f)i + (%8.4e)j + (%8.4f)k + (%8.4f)\n",thy,thp, ...
  212.         qb(1), qb(2), qb(3), qb(4));
  213.       [vv,th] = quaternion(qb);
  214.       printf("      = (vector) = [%8.4f %8.4f %8.4f], th=%5.2f deg\n", ...
  215.         vv(1), vv(2), vv(3), th*180/pi);
  216.     
  217.       qb = qmult(qb,qi);
  218.       title(sprintf("yaw=%5.2f deg, pitch=%5.2f deg",thy,thp))
  219.       qcoordinate_plot(qi,qb,qview);
  220.       # uncomment the next four lines to get eps output
  221.       #gset terminal postscript eps 
  222.       #eval(sprintf("gset output 'fig%d.eps'",kk));
  223.       #replot
  224.       #gset terminal x11
  225.       #prompt
  226.     endfor
  227.   case(quitopt),
  228.     printf("Exiting quaternion demo\n");
  229.   otherwise,
  230.     error(sprintf("Illegal option %f",opt));
  231.   endswitch    
  232. endwhile
  233. endfunction
  234.