# HG changeset patch
# User jwe
# Date 912809303 0
# Node ID 1e7532b9173b48ffc43367f1c0f15cff0d1a0e0b
# Parent  3ee04ff37b3e9ec8b0499d8f4a63743794c3d8f3
[project @ 1998-12-04 22:08:23 by jwe]

diff --git a/scripts/quaternion/demoquat.m b/scripts/quaternion/demoquat.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/demoquat.m
@@ -0,0 +1,233 @@
+
+# Thanks to Mr. Charles Hall, Dr. Don Krupp and Dr. Larry Mullins at
+#  NASA's Marshall Space Flight Center for notes and instruction in
+# use and conventions with quaternions.    - A. S. Hodel
+
+function opt = demoquat()
+
+opt = 0;
+quitopt = 5;
+while(opt != quitopt)
+  opt = menu("Quaternion function demo (c) 1998 A. S. Hodel, a.s.hodel@eng.auburn.edu",
+	"quaternion construction/data extraction",
+	"simple quaternion functions",
+	"transformation functions",
+	"body-inertial frame demo",
+	"Quit");
+
+  switch(opt)
+  case(1),
+    printf("Quaternion construction/data extraction\n");
+    help quaternion
+    prompt
+    cmd = "q = quaternion(1,2,3,4)";
+    run_cmd
+    disp("This format stores the i,j,k parts of the quaternion first;")
+    disp("the real part is stored last.")
+    prompt
+    disp(" ")
+    disp("i, j, and k are all square roots of -1; however they do not")
+    disp("commute under multiplication (discussed further with the function")
+    disp("qmult).  Therefore quaternions do not commute under multiplcation:")
+    disp("    q1*q2 != q2*q1 (usually)")
+    prompt
+
+    disp("Quaternions as rotations: unit quaternion to represent")
+    disp("rotation of 45 degrees about the vector [1 1 1]")
+    cmd = "degrees = pi/180; q1 = quaternion([1 1 1],45*degrees)";
+    run_cmd
+    prompt
+    cmd = "real_q = cos(45*degrees/2)";
+    run_cmd
+    printf("The real part of the quaternion q(4) is cos(theta/2).\n----\n\n");
+    cmd = "imag_q = sin(45*degrees/2)*[1 1 1]/norm([1 1 1])"
+    run_cmd
+    disp("The imaginary part of the quaternion is sin(theta/2)*unit vector");
+    disp("The constructed quaternion is a unit quaternion.");
+    prompt
+    disp("Can also extract both forms of the quaternion:")
+    disp("Vector/angle form of 1i + 2j + 3k + 4:")
+    cmd = "[vv,th] = quaternion(q)";
+    run_cmd
+    cmd = "vv_norm = norm(vv)";
+    run_cmd
+    disp("Returns the eigenaxis as a 3-d unit vector");
+    disp("Check values: ")
+    cmd = "th_deg = th*180/pi";
+    run_cmd
+    disp("")
+    disp("This concludes the quaternion construction/extraction demo.");
+    prompt
+
+  case(2),
+    printf("Simple quaternion functions\n");
+    cmd = "help qconj";
+    run_cmd
+    cmd = "degrees = pi/180; q1 = quaternion([1 1 1],45*degrees)";
+    run_cmd
+    cmd = "q2 = qconj(q1)";
+    run_cmd
+    disp("The conjugate changes the sign of the complex part of the")
+    printf("quaternion.\n\n");
+    prompt
+    printf("\n\n\nMultiplication of quaternions:\n");
+    cmd = "help qmult";
+    run_cmd
+    cmd = "help qinv"
+    run_cmd
+    disp("Inverse quaternion: q*qi = qi*q = 1:")
+    cmd = "q1i = qinv(q1)";
+    run_cmd
+    cmd = "one = qmult(q1,q1i)";
+    run_cmd
+    
+    printf("Conclusion of simple quaternion functions");
+    prompt
+  case(3),
+    printf("Transformation functions\n");
+    disp("A problem with the discussion of coordinate transformations is that");
+    disp("one must be clear on what is being transformed: does a rotation of");
+    disp("theta degrees mean that you're rotating the VECTOR by theta degrees,");
+    disp("also called the 'active convention,' or does it mean that you rotate ");
+    disp("the COORDINATE FRAME by theta degrees, also called the 'passive convention,' ");
+    disp("which is equivalent to rotating the VECTOR by (-theta) degrees.  The");
+    disp("functions in this demo use the active convention.  I'll point out where");
+    disp("this changes the code as the demo runs.");
+    disp("    -- The author");
+    prompt
+    printf("\n\n");
+    disp("Sequences of rotations:")
+    printf("\n\nRotation of a vector by 90 degrees about the reference z axis\n");
+    cmd = "qz = quaternion([0 0 1], pi/2);";
+    disp(cmd) ; eval(cmd);
+    printf("\n\nRotation of a vector by 90 degrees about the reference y axis\n");
+    cmd="qy = quaternion([0 1 0], pi/2);";
+    disp(cmd) ; eval(cmd);
+    printf("\n\nRotation of a vector by 90 degrees about the reference x axis\n");
+    cmd="qx = quaternion([1 0 0], pi/2);";
+    run_cmd
+    printf("\n\nSequence of three rotations: 90 degrees about x, then 90 degrees\n");
+    disp("about y, then 90 degrees about z (all axes specified in the reference frame):");
+    qchk = qmult(qz,qmult(qy,qx));
+    cmd = "[vv,th] = quaternion(qchk), th_deg = th*180/pi";
+    run_cmd
+    disp("The sequence of the three rotations above is equivalent to a single rotation")
+    disp("of 90 degrees about the y axis. Check:");
+    cmd = "err = norm(qchk - qy)";
+    run_cmd
+    
+    disp("Transformation of a quaternion by a quaternion:")
+    disp("The three quaternions above were rotations specified about")
+    disp("a single reference frame.  It is often convenient to specify the");
+    disp("eigenaxis of a rotation in a different frame (e.g., when computing");
+    disp("the transformation rotation in terms of the Euler angles yaw-pitch-roll).");
+    cmd = "help qtrans";
+    run_cmd
+    disp("")
+    disp("NOTE: If the passive convention is used, then the above");
+    disp("formula changes to   v = qinv(q)*v*q  instead of ")
+    disp("v = q*v*qinv(q).")
+    prompt
+    disp("")
+    disp("Example: Vectors in Frame 2 are obtained by rotating them from ")
+    disp("   from Frame 1 by 90 degrees about the x axis (quaternion qx)")
+    disp("   A quaternion in Frame 2 rotates a vector by 90 degrees about")
+    disp("   the Frame 2 y axis (quaternion qy).  The equivalent rotation")
+    disp("   in the reference frame is:")
+    cmd = "q_eq = qtrans(qy,qx); [vv,th] = quaternion(q_eq)";
+    run_cmd
+    disp("The rotation is equivalent to rotating about the reference z axis")
+    disp("by 90 degrees (quaternion qz)")
+    prompt
+
+    disp("Transformation of a vector by a quaternion");
+    cmd = "help qtransv";
+    run_cmd
+
+    disp("NOTE: the above formula changes if the passive quaternion ")
+    disp("is used; the cross product term is subtracted instead of added.");
+    prompt
+    disp("Example: rotate the vector [1,1,1] by 90 degrees about the y axis");
+    cmd = "vec_r = qtransv([1,1,1],qy)";
+    run_cmd
+    prompt
+    disp("Equivalently, one may multiply by qtransvmat:")
+    cmd = "help qtransvmat";
+    run_cmd
+    disp("NOTE: the passive quaternion convention would use the transpose")
+    disp("(inverse) of the orthogonal matrix returned by qtransvmat.");
+    prompt
+    cmd = "vec_r_2 = qtransvmat(qy)*[1;1;1]; vec_err = norm(vec_r - vec_r_2)";
+    run_cmd
+
+    disp("")
+    disp("The last transformation function is the derivative of a quaternion")
+    disp("Given rotation rates about the reference x, y, and z axes.");
+    cmd = "help qderivmat";
+    run_cmd
+    disp("")
+    disp("Example:")
+    disp("Frame is rotating about the z axis at 1 rad/s")
+    cmd = "Omega = [0,0,1]; Dmat = qderivmat(Omega)";
+    run_cmd
+    disp("Notice that Dmat is skew symmetric, as it should be.")
+    disp("expm(Dmat*t) is orthogonal, so that unit quaternions remain")
+    disp("unit quaternions as the rotating frame precesses.");
+    disp(" ")
+    disp("This concludes the transformation demo.");
+    prompt;
+  case(4),
+    printf("Body-inertial frame demo: Look at the source code for\n");
+    printf("demoquat.m and qcoordinate_plot.m to see how it's done.\n");
+    
+    # i,j,k units
+    iv = quaternion(1,0,0,0); jv = quaternion(0,1,0,0);
+    kv = quaternion(0,0,1,0);
+    
+    # construct quaternion to desired view.
+    degrees = pi/180; daz = 45*degrees; del = -30*degrees;
+    qazimuth = quaternion([0,0,1],daz);
+    qelevation = quaternion([cos(daz),sin(daz),0],del);
+    qview = qmult(qelevation,qazimuth);
+    
+    # inertial frame i, j, k axes.
+    iif = iv; jf = qtrans(jv,iv); kf = qtrans(kv,iv);
+    
+    # rotation steps
+    th = 0:5:20; ov = ones(size(th)); myth = [th,max(th)*ov ; 0*ov,th];
+    
+    # construct yaw-pitch-roll cartoon
+    for kk=1:length(myth(1,:))
+      figure(kk)
+      thy = myth(1,kk);
+      thp = myth(2,kk);
+     
+      qyaw = quaternion([0,0,1],thy*pi/180);
+      [jvy,th] = quaternion(qtrans(jf,qyaw));
+      qpitch = quaternion(jvy(1:3),thp*pi/180);
+      qb = qmult(qpitch, qyaw);
+      qi = quaternion([1 0 0],180*degrees);
+    
+      printf("yaw=%8.4f, pitch=%8.4f, \n    qbi = (%8.4f)i + (%8.4e)j + (%8.4f)k + (%8.4f)\n",thy,thp, ...
+    	qb(1), qb(2), qb(3), qb(4));
+      [vv,th] = quaternion(qb);
+      printf("      = (vector) = [%8.4f %8.4f %8.4f], th=%5.2f deg\n", ...
+    	vv(1), vv(2), vv(3), th*180/pi);
+    
+      qb = qmult(qb,qi);
+      title(sprintf("yaw=%5.2f deg, pitch=%5.2f deg",thy,thp))
+      qcoordinate_plot(qi,qb,qview);
+      # uncomment the next four lines to get eps output
+      #gset terminal postscript eps 
+      #eval(sprintf("gset output 'fig%d.eps'",kk));
+      #replot
+      #gset terminal x11
+      #prompt
+    endfor
+  case(quitopt),
+    printf("Exiting quaternion demo\n");
+  otherwise,
+    error(sprintf("Illegal option %f",opt));
+  endswitch    
+endwhile
+endfunction
diff --git a/scripts/quaternion/qconj.m b/scripts/quaternion/qconj.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/qconj.m
@@ -0,0 +1,11 @@
+
+function retval = qconj(q)
+  # function retval = qconj(q)
+  # conjugate of a quaternion
+  #  q = [w,x,y,z] = w*i + x*j + y*k + z
+  # qconj(q) = -w*i -x*j -y*k + z
+
+  [a,b,c,d] = quaternion(q);
+  retval = quaternion(-a,-b,-c,d);
+endfunction
+
diff --git a/scripts/quaternion/qcoordinate_plot.m b/scripts/quaternion/qcoordinate_plot.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/qcoordinate_plot.m
@@ -0,0 +1,64 @@
+function qcoordinate_plot(qf,qb,qv)
+# function qcoordinate_plot(qf,qb,qv)
+# plot in the current figure a set of coordinate axes as viewed from 
+# the orientation specified by quaternion qv.  Inertial axes are
+# also plotted
+# qf: quaternion from reference (x,y,z) to inertial
+# qb: quaternion from reference to body 
+# qv: quaternion from reference to view angle 
+
+degrees = pi/180;
+d180 = 180*degrees;
+
+# construct coordinate transformation to view frame
+cm = qtransvmat(qv);
+p1 = [-1,-1,1]; p2 = [-1,-1,-1]; p3 = [1,-1,-1]; p4 = [ 1,-1, 1];
+p5 = [-1, 1,1]; p6 = [ 1, 1, 1]; p7 = [1, 1,-1]; p8 = [-1, 1,-1];
+# outline positive quadrant
+box1 = cm*[p4; p6; p5; p6; p7]';
+# outline rest of the box
+box2 =cm*[p7; p8; p5; p1; p4; p3; p7; p3; p2; p1; p2; p8]';
+
+# compute inertial to body rotation eigenaxis
+# qb = qbf*qf => qbf = qb/qf
+#
+# need to use inverse quaternion to rotate axes
+qbf = qinv(qmult(qb,qinv(qf)));
+
+[eaxv,th_eig] = quaternion(qbf);
+
+# draw 1/3 circle in x-y plane around a unit z axis
+th = (0:-12:-120)*degrees*sign(th_eig);    lth = length(th);
+cpts = [0 0 0.1*cos(th) ; 0 0 0.1*sin(th); 0 1 1*ones(1,lth)];
+
+# rotate the 1/3 circle around eigenaxis of inertial to body rotation
+# qez = qe/qz = rotation to get from z axis to eigenaxis.
+# This rotates the 1/3 circle from x-y plane to the plane normal to
+# eigenaxis
+qez = qmult(qbf,qinv(quaternion(0,0,1,0)));
+eig_xm = qtransvmat(qez);
+cpts = cm*eig_xm*cpts;
+
+# transform inertial and body quaternions to view coordinates (rotate
+# by azimuth, elevation)
+qfm = qtransvmat(qf);                 qbm = qtransvmat(qf);
+qf = qmult(qv,qf);                 qb = qmult(qv,qb);
+
+# get coordinate axes in inertial and reference frame
+jnk = qtransvmat(qf); ifv = jnk(:,1); jfv = jnk(:,2); kfv = jnk(:,3);
+jnk = qtransvmat(qb); ibv = jnk(:,1); jbv = jnk(:,2); kbv = jnk(:,3);
+
+gset size square
+axis([-2,2,-2,2]);
+[vv,theta] = quaternion(qb);
+xlabel(sprintf("rotate about eigenaxis %5.2f deg",th_eig/degrees));
+plot( [ibv(1),0],[ibv(3),0],"-@11;x (body);", ...
+	[0,jbv(1)],[0,jbv(3)],"-@21;y (body);", ...
+	[0,kbv(1)],[0,kbv(3)],"-@32;z (body);", ...
+	[ifv(1),0],[ifv(3),0],"-@13;x (inertial);", ...
+	[0,jfv(1)],[0,jfv(3)],"-@23;y (inertial);", ...
+	[0,kfv(1)],[0,kfv(3)],"-@34;z (inertial);", ...
+	cpts(1,:), cpts(3,:),".-6 ;eigenaxis;", ...
+	box2(1,:),box2(3,:),"-4;;", ...
+	box1(1,:),box1(3,:),"-5;;");
+endfunction
diff --git a/scripts/quaternion/qderiv.m b/scripts/quaternion/qderiv.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/qderiv.m
@@ -0,0 +1,23 @@
+function Dmat = qderivmat(Omega)
+# function Dmat = qderivmat(Omega)
+# Derivative of a quaternion.
+#   Let Q be a quaternion to transform a vector from a fixed frame to
+#      a rotating frame.  If the rotating frame is rotating about the 
+#      [x,y,z] axes at angular rates [wx, wy, wz], then the derivative
+#      of Q is given by
+#   Q' = qderivmat(Omega)*Q
+#
+#   If the passive convention is used (rotate the frame, not the vector),
+#   then Q' = -qderivmat(Omega)*Q; see the accompanying document
+#   quaternion.ps for details.
+
+Omega = vec(Omega);
+if(length(Omega) != 3)
+   error("qderivmat: Omega must be a length 3 vector.");
+endif
+
+Dmat = 0.5*[ 0.0,  Omega(3), -Omega(2),  Omega(1); ...
+ 	  -Omega(3),      0.0,  Omega(1),  Omega(2); ...
+  	  Omega(2), -Omega(1),      0.0,  Omega(3); ...
+ 	  -Omega(1), -Omega(2), -Omega(3),      0.0 ];
+endfunction
diff --git a/scripts/quaternion/qderivmat.m b/scripts/quaternion/qderivmat.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/qderivmat.m
@@ -0,0 +1,23 @@
+function Dmat = qderivmat(Omega)
+# function Dmat = qderivmat(Omega)
+# Derivative of a quaternion.
+#   Let Q be a quaternion to transform a vector from a fixed frame to
+#      a rotating frame.  If the rotating frame is rotating about the 
+#      [x,y,z] axes at angular rates [wx, wy, wz], then the derivative
+#      of Q is given by
+#   Q' = qderivmat(Omega)*Q
+#
+#   If the passive convention is used (rotate the frame, not the vector),
+#   then Q' = -qderivmat(Omega)*Q; see the accompanying document
+#   quaternion.ps for details.
+
+Omega = vec(Omega);
+if(length(Omega) != 3)
+   error("qderivmat: Omega must be a length 3 vector.");
+endif
+
+Dmat = 0.5*[ 0.0,  Omega(3), -Omega(2),  Omega(1); ...
+ 	  -Omega(3),      0.0,  Omega(1),  Omega(2); ...
+  	  Omega(2), -Omega(1),      0.0,  Omega(3); ...
+ 	  -Omega(1), -Omega(2), -Omega(3),      0.0 ];
+endfunction
diff --git a/scripts/quaternion/qinv.m b/scripts/quaternion/qinv.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/qinv.m
@@ -0,0 +1,12 @@
+function retval = qinv(q)
+# function b = qinv(q)
+# return the inverse of a quaternion 
+#       q =  [w,x,y,z] = w*i + x*j + y*k + z
+#  qmult(q,qinv(q)) = 1 = [0 0 0 1]
+
+if(norm(q) != 0)
+  retval = qconj(q) /sum(q .* q);
+else
+  error("qinv: zero quaternion passed!");
+end
+endfunction
diff --git a/scripts/quaternion/qmult.m b/scripts/quaternion/qmult.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/qmult.m
@@ -0,0 +1,21 @@
+function retval = qmult(a,b)
+  # function retval = qmult(a,b)
+  # multiply two quaternions 
+  #  [w,x,y,z] = w*i + x*j + y*k + z
+  #  identities:
+  #    i^2 = j^2 = k^2 = -1
+  #    ij = k                    jk = i
+  #    ki = j                    kj = -i
+  #    ji = -k                   ik = -j
+  
+  [a1,b1,c1,d1] = quaternion(a);
+  [a2,b2,c2,d2] = quaternion(b);
+  
+  ri = b1*c2 - c1*b2 + d1*a2 + a1*d2;
+  rj = c1*a2 - a1*c2 + d1*b2 + b1*d2;
+  rk = a1*b2 - b1*a2 + d1*c2 + c1*d2;
+  rr = -(a1*a2 + b1*b2 + c1*c2) + d1*d2;
+  
+  retval = quaternion(ri,rj,rk,rr);
+endfunction
+
diff --git a/scripts/quaternion/qtrans.m b/scripts/quaternion/qtrans.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/qtrans.m
@@ -0,0 +1,13 @@
+function v = qtrans(v,q)
+# function v = qtrans(v,q)
+# transform the unit quaternion v by the unit quaternion q;
+# v = [w x y z], q = transformation quaternion
+# returns v = q*v/q
+
+if(!is_vector(v) | length(v) != 4)
+  error(sprintf("qtrans: v(%d,%d) must be a quaternion",rows(v),columns(v)))
+elseif(!is_vector(q) | length(q) != 4)
+  error(sprintf("qtrans: q(%d,%d) must be a quaternion",rows(q),columns(q)))
+endif
+
+v = qmult(q,qmult(v,qinv(q)));
diff --git a/scripts/quaternion/qtransv.m b/scripts/quaternion/qtransv.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/qtransv.m
@@ -0,0 +1,23 @@
+function vi = qtransv(vb,qib)
+# function vi = qtransv(vb,q)
+# transform the 3-D vector v by the unit quaternion q;
+# v = [w x y z], q = transformation quaternion
+# returns vi = column vector
+#    vi = (2*real(q)^2 - 1)*vb + 2*imag(q)*(imag(q)'*vb) 
+#      + 2*real(q)*cross(imag(q),vb)
+#    where imag(q) is a column vector of length 3.
+
+if(!is_vector(vb) | length(vb) != 3)
+  error(sprintf("qtransv: v(%d,%d) must be a 3-D vector",rows(vb),columns(vb)))
+elseif(!is_vector(qib) | length(qib) != 4)
+  error(sprintf("qtransv: q(%d,%d) must be a quaternion",rows(qib),columns(qib)))
+elseif(max(abs(imag(vb))) + max(abs(imag(qib))) != 0)
+  vb
+  qib
+  error("qtransv: input values must be real.");
+endif
+
+qr = qib(4);  qimag = vec(qib(1:3));   vb = vec(vb);
+vi = (2*qr^2 - 1)*vb + 2*qimag*(qimag'*vb) + 2*qr*cross(qimag,vb);
+
+endfunction
diff --git a/scripts/quaternion/qtransvmat.m b/scripts/quaternion/qtransvmat.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/qtransvmat.m
@@ -0,0 +1,24 @@
+function Aib = qtransvmat(qib)
+# function Aib = qtransvmat(qib)
+# construct 3x3  transformation matrix from quaternion qib
+# Aib is equivalent to rotation of th radians about axis vv, where
+#    [vv,th] = quaternion(qib)
+
+if(!is_vector(qib) | length(qib) != 4)
+  error(sprintf("qtransvmat: q(%d,%d) must be a quaternion",rows(qib),columns(qib)))
+elseif(max(abs(imag(qib))) != 0)
+  qib
+  error("qtransvmat: input values must be real.");
+endif
+
+Aib = [ (2.*(qib(1)^2 + qib(4)^2) -1.), ...
+	  (2.*(qib(1)*qib(2)-qib(3)*qib(4))), ...
+	  (2.*(qib(1)*qib(3)+qib(2)*qib(4))); ...
+	(2.*(qib(1)*qib(2)+qib(3)*qib(4))), ...
+	  (2.*(qib(2)*qib(2)+qib(4)*qib(4))-1.), ...
+	  (2.*(qib(2)*qib(3)-qib(1)*qib(4))); ...
+	(2.*(qib(1)*qib(3)-qib(2)*qib(4))), ...
+	  (2.*(qib(2)*qib(3)+qib(1)*qib(4))), ...
+	  (2.*(qib(3)*qib(3)+qib(4)*qib(4))-1.)];
+endfunction
+
diff --git a/scripts/quaternion/quaternion.m b/scripts/quaternion/quaternion.m
new file mode 100644
--- /dev/null
+++ b/scripts/quaternion/quaternion.m
@@ -0,0 +1,86 @@
+function [a,b,c,d] = quaternion(w,x,y,z)
+# quaternion: construct or extract a quaternion
+#  w = a*i + b*j + c*k + d from given data.
+#
+# calling formats:
+# [a,b,c,d]   = quaternion(w)		-or-
+# [vv,theta] = quaternion(w)
+# w           = quaternion(a,b,c,d)
+# w           = quaternion(vv,theta)
+
+switch(nargin)
+case(1),					# extract information
+  if(!(is_vector(w) & length(w) == 4) )
+    error("input vector must be of length 4)")
+  endif
+  # extract data
+  switch(nargout)
+  case(4),
+    a = w(1);
+    b = w(2);
+    c = w(3);
+    d = w(4);
+  case(2),
+    if(abs(norm(w)-1) > 1e-12)
+      warning(sprintf("quaternion: ||w||=%e, setting=1 for vv, theta",norm(w)))
+      w = w/norm(w);
+    endif
+    [a,b,c,d] = quaternion(w);
+    theta = acos(d)*2;
+    if(abs(theta) > pi)
+      theta = theta - sign(theta)*pi;
+    endif
+    sin_th_2 = norm([a b c]);
+
+    if(sin_th_2 != 0)
+      vv = [a,b,c]/sin_th_2;
+    else
+      vv = [a b c];
+    endif
+    a = vv;
+    b = theta;
+  otherwise,
+    usage("[a,b,c,d] = quaternion(w) or [vv,theta] = quaternion(w)");
+  endswitch
+
+case(2),
+  if(nargout != 1)
+    usage("w = quaterion(vv,theta)");
+  endif
+  vv = w;
+  theta = x;
+
+  if(!is_vector(vv) | length(vv) != 3)
+    error("vv must be a length three vector")
+  elseif(!is_scalar(theta))
+    error("theta must be a scalar");
+  elseif(norm(vv) == 0)
+    error("quaternion: vv is zero.") 
+  elseif(abs(norm(vv)-1) > 1e-12)
+    warning("quaternion: ||vv|| != 1, normalizing")
+    vv = vv/norm(vv);
+  endif
+
+  if(abs(theta) > 2*pi)
+    warning("quaternion: |theta| > 2 pi, normalizing")
+    theta = rem(theta,2*pi);
+  endif
+  vv = vv*sin(theta/2);
+  d = cos(theta/2);
+  a = quaternion(vv(1), vv(2), vv(3), d); 
+
+case(4),
+  if(nargout != 1)
+    usage("w = quaterion(a,b,c,d)");
+  endif
+  if ( !(is_scalar(w) & is_scalar(x) & is_scalar(y) & is_scalar(z)) )
+    error("input values must be scalars.")
+  endif
+  a = [w x y z];
+
+otherwise,
+  error("usage: [a,b,c,d] = quaternion(w), a = quaternion(w,x,y,z)")
+
+endswitch
+
+endfunction