Mercurial > hg > octave-nkf
diff scripts/quaternion/quaternion.m @ 3224:1e7532b9173b
[project @ 1998-12-04 22:08:23 by jwe]
author | jwe |
---|---|
date | Fri, 04 Dec 1998 22:08:23 +0000 |
parents | |
children | 41602f25d19f |
line wrap: on
line diff
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