|
3224
|
1 function [a,b,c,d] = quaternion(w,x,y,z) |
|
|
2 # quaternion: construct or extract a quaternion |
|
|
3 # w = a*i + b*j + c*k + d from given data. |
|
|
4 # |
|
|
5 # calling formats: |
|
|
6 # [a,b,c,d] = quaternion(w) -or- |
|
|
7 # [vv,theta] = quaternion(w) |
|
|
8 # w = quaternion(a,b,c,d) |
|
|
9 # w = quaternion(vv,theta) |
|
|
10 |
|
|
11 switch(nargin) |
|
|
12 case(1), # extract information |
|
|
13 if(!(is_vector(w) & length(w) == 4) ) |
|
|
14 error("input vector must be of length 4)") |
|
|
15 endif |
|
|
16 # extract data |
|
|
17 switch(nargout) |
|
|
18 case(4), |
|
|
19 a = w(1); |
|
|
20 b = w(2); |
|
|
21 c = w(3); |
|
|
22 d = w(4); |
|
|
23 case(2), |
|
|
24 if(abs(norm(w)-1) > 1e-12) |
|
|
25 warning(sprintf("quaternion: ||w||=%e, setting=1 for vv, theta",norm(w))) |
|
|
26 w = w/norm(w); |
|
|
27 endif |
|
|
28 [a,b,c,d] = quaternion(w); |
|
|
29 theta = acos(d)*2; |
|
|
30 if(abs(theta) > pi) |
|
|
31 theta = theta - sign(theta)*pi; |
|
|
32 endif |
|
|
33 sin_th_2 = norm([a b c]); |
|
|
34 |
|
|
35 if(sin_th_2 != 0) |
|
|
36 vv = [a,b,c]/sin_th_2; |
|
|
37 else |
|
|
38 vv = [a b c]; |
|
|
39 endif |
|
|
40 a = vv; |
|
|
41 b = theta; |
|
|
42 otherwise, |
|
|
43 usage("[a,b,c,d] = quaternion(w) or [vv,theta] = quaternion(w)"); |
|
|
44 endswitch |
|
|
45 |
|
|
46 case(2), |
|
|
47 if(nargout != 1) |
|
|
48 usage("w = quaterion(vv,theta)"); |
|
|
49 endif |
|
|
50 vv = w; |
|
|
51 theta = x; |
|
|
52 |
|
|
53 if(!is_vector(vv) | length(vv) != 3) |
|
|
54 error("vv must be a length three vector") |
|
|
55 elseif(!is_scalar(theta)) |
|
|
56 error("theta must be a scalar"); |
|
|
57 elseif(norm(vv) == 0) |
|
|
58 error("quaternion: vv is zero.") |
|
|
59 elseif(abs(norm(vv)-1) > 1e-12) |
|
|
60 warning("quaternion: ||vv|| != 1, normalizing") |
|
|
61 vv = vv/norm(vv); |
|
|
62 endif |
|
|
63 |
|
|
64 if(abs(theta) > 2*pi) |
|
|
65 warning("quaternion: |theta| > 2 pi, normalizing") |
|
|
66 theta = rem(theta,2*pi); |
|
|
67 endif |
|
|
68 vv = vv*sin(theta/2); |
|
|
69 d = cos(theta/2); |
|
|
70 a = quaternion(vv(1), vv(2), vv(3), d); |
|
|
71 |
|
|
72 case(4), |
|
|
73 if(nargout != 1) |
|
|
74 usage("w = quaterion(a,b,c,d)"); |
|
|
75 endif |
|
|
76 if ( !(is_scalar(w) & is_scalar(x) & is_scalar(y) & is_scalar(z)) ) |
|
|
77 error("input values must be scalars.") |
|
|
78 endif |
|
|
79 a = [w x y z]; |
|
|
80 |
|
|
81 otherwise, |
|
|
82 error("usage: [a,b,c,d] = quaternion(w), a = quaternion(w,x,y,z)") |
|
|
83 |
|
|
84 endswitch |
|
|
85 |
|
|
86 endfunction |
