1 | function theta = angle(q) |
---|
2 | % ANGLE or phase of quaternion. |
---|
3 | % (Quaternion overloading of standard Matlab function.) |
---|
4 | % |
---|
5 | % If q = A + mu .* B where A and B are real/complex, and mu is a unit pure |
---|
6 | % quaternion, then angle(q) = B. For pure quaternions, angle(q) = pi/2. |
---|
7 | |
---|
8 | % Copyright © 2005 Stephen J. Sangwine and Nicolas Le Bihan. |
---|
9 | % See the file : Copyright.m for further details. |
---|
10 | |
---|
11 | % Modified : 20 September 2005 to handle complexified quaternions. |
---|
12 | |
---|
13 | error(nargchk(1, 1, nargin)), error(nargoutchk(0, 1, nargout)) |
---|
14 | |
---|
15 | if ispure(q) |
---|
16 | |
---|
17 | % q is a pure quaternion, with no scalar part, and by definition, theqq |
---|
18 | % angle is a right-angle, even for complexified quaternions. Return a |
---|
19 | % matrix of the same size as q, with pi/2 at every position. |
---|
20 | % Rationale : we can construct an exact result in this case with |
---|
21 | % minimal calculation, whereas if we use the algorithm below for full |
---|
22 | % quaternions, the result may be subject to small errors. |
---|
23 | |
---|
24 | theta = ones(size(q)) .* (pi ./ 2); |
---|
25 | |
---|
26 | else |
---|
27 | |
---|
28 | % q is a full quaternion. We adopt two methods here, depending on |
---|
29 | % whether q is a real quaternion or a complexified quaternion. In |
---|
30 | % both cases we need the scalar part and the modulus of the vector |
---|
31 | % part. We extract these from unit(q) because the argument or angle |
---|
32 | % of unit(q) is the same as the argument of q, and having unit |
---|
33 | % modulus simplifies the formula in the complex case. |
---|
34 | |
---|
35 | u = unit(q); |
---|
36 | x = s(u); |
---|
37 | y = abs(v(u)); |
---|
38 | |
---|
39 | % Now establish whether x and y are real or complex (strictly whether |
---|
40 | % any imaginary part of x or y is non-zero). |
---|
41 | |
---|
42 | if isreal(x) & isreal(y) |
---|
43 | |
---|
44 | % All elements of x and y are real. Therefore we can use the |
---|
45 | % simple method of constructing a complex value from x and y |
---|
46 | % and using the Matlab angle function to compute the angle. |
---|
47 | % Because we took the absolute value (modulus) of the vector |
---|
48 | % part when we constructed y, y is never negative, and the |
---|
49 | % angle returned is always between 0 and pi inclusive. This is |
---|
50 | % normal for quaternions, since the vector part is a directionn |
---|
51 | % in 3-space. The opposite direction is represented by negating |
---|
52 | % mu (the axis of the quaternion), not by adding pi to the angle. |
---|
53 | |
---|
54 | theta = angle(complex(x, y)); |
---|
55 | else |
---|
56 | |
---|
57 | % At least one element of x and/or y is complex, so we must use |
---|
58 | % an algorithm suitable for the complex case. See the appendix |
---|
59 | % below for details of the derivation of the formula used here. |
---|
60 | |
---|
61 | theta = - i .* log(x + i.*y); |
---|
62 | end |
---|
63 | end |
---|
64 | |
---|
65 | % Appendix: calculation of arctangent(y, x) when y and x are complex. |
---|
66 | % |
---|
67 | % Algorithms for computing the arctangent (or arcsin/arccos) of a complex |
---|
68 | % value are given in some books on complex analysis. For example: |
---|
69 | % |
---|
70 | % John H. Mathews, |
---|
71 | % 'Basic Complex Variables for Mathematics and Engineering', |
---|
72 | % Allyn and Bacon, Boston, 1982. ISBN 0-205-07170-8. |
---|
73 | % |
---|
74 | % The algorithm used here can be derived using the method given in Mathews' |
---|
75 | % book, as follows. We are given x and y, both complex, and these are the |
---|
76 | % sine and cosine of theta (also complex), respectively. |
---|
77 | % Therefore, tan(theta) = y/x. The sine and cosine of a complex angle can |
---|
78 | % be written in terms of complex exponentials as (using mathematical |
---|
79 | % notation with implicit multiplication, rather than Matlab notation): |
---|
80 | % |
---|
81 | % sin(theta) = (1/2i)[exp(i theta) - exp(-i theta)] |
---|
82 | % cos(theta) = (1/2) [exp(i theta) + exp(-i theta)] |
---|
83 | % |
---|
84 | % Therefore, we can write: |
---|
85 | % |
---|
86 | % y/x = sin(theta)/cos(theta) |
---|
87 | % = [exp(i theta) - exp(-i theta)]/i[exp(i theta) + exp(-i theta)] |
---|
88 | % |
---|
89 | % Multiplying numerator and denominator on the right by exp(i theta) |
---|
90 | % this simplifies to: |
---|
91 | % |
---|
92 | % y/x = [exp(2i theta) - 1]/i[exp(2i theta) + 1] |
---|
93 | % |
---|
94 | % and cross multiplying and gathering all terms on the left, and |
---|
95 | % multiplying through by -1, we get: |
---|
96 | % |
---|
97 | % => exp(2i theta)[x - iy] - [x + iy] = 0 |
---|
98 | % |
---|
99 | % => exp(2i theta) = [x + iy]/[x - iy] |
---|
100 | % |
---|
101 | % Rationalizing the right-hand side we obtain: |
---|
102 | % |
---|
103 | % exp(2i theta) = [x+iy]^2 / [x^2 + y^2] |
---|
104 | % |
---|
105 | % Taking the square root of both sides: |
---|
106 | % |
---|
107 | % exp(i theta) = [x + iy]/sqrt[x^2 + y^2] |
---|
108 | % |
---|
109 | % Finally, taking the natural logarithm of both sides and multiplying |
---|
110 | % throughout by i, we get: |
---|
111 | % |
---|
112 | % theta = -i ln[(x + iy)/sqrt(x^2 + y^2)] |
---|
113 | % |
---|
114 | % If we normalise the quaternion before computing x and y, we can ensure |
---|
115 | % that sqrt(x^2 + y^2) = 1, and therefore we can simplify the formula to |
---|
116 | % that used in the code above. |
---|