[37] | 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. |
---|