Constrained Quaternions Using Euler Angles David Eberly, Geometric Tools, Redmond WA 98052 https://www.geometrictools.com/ This work is licensed under the Creative Commons Attribution 4.0 International License. To view a copy of this license, visit http://creativecommons.org/licenses/by/4.0/ or send a letter to Creative Commons, PO Box 1866, Mountain View, CA 94042, USA. Created: July 29, 2008 Contents 1 Introduction 3 2 The Unconstrained Problem3 2.1 Rotation X (The Analysis).....................................3 2.2 Rotation X, Y, or Z.........................................5 2.3 Rotation XY (The Analysis).....................................6 2.3.1 Distinct Eigenvalues.....................................7 2.3.2 Equal Eigenvalues......................................8 2.4 Rotation XY, YX, YZ, ZY, ZX, XZ................................9 2.4.1 Rotation YX......................................... 10 2.4.2 Rotation ZX......................................... 10 2.4.3 Rotation XZ......................................... 11 2.4.4 Rotation YZ......................................... 11 2.4.5 Rotation ZY......................................... 12 2.5 Rotation XYZ (The Analysis).................................... 13 2.5.1 Degenerate Linear Equation................................. 14 2.5.2 Nondegenerate Linear Equation............................... 15 2.6 Rotation XYZ, XZY, YZX, YXZ, ZXY, ZYX........................... 17 2.6.1 Rotation XZY........................................ 17 2.6.2 Rotation YZX........................................ 18 2.6.3 Rotation YXZ........................................ 18 2.6.4 Rotation ZXY........................................ 19 1 2.6.5 Rotation ZYX........................................ 19 3 The Constrained Problem 20 3.1 Rotation X (The Analysis)..................................... 20 3.2 Rotation X, Y, or Z......................................... 24 3.3 Rotation XY............................................. 25 3.4 Rotation YX............................................. 28 3.5 Rotation ZX, XZ, YZ, ZY...................................... 28 4 The Test Code 28 2 1 Introduction I assume that you are familiar with quaternions, their algebraic properties, and how they represent rotations and orientations. A brief reminder of notation is in order. Unit-length quaternions are of the form q^ = w + x^{ + y|^+ zk^ (1) where w2 + x2 + y2 + z2 = 1. The quaternion may be written as a 4-tuple q = (w; x; y; z) = (q0; q1; q2; q3) (2) The last equality shows how the components of the quaternion are indexed within an array of four real numbers. The quaternionq ^ may be written also as q^ = cos(θ) + sin(θ)^u (3) ^ whereu ^ = u0^{ + u1|^+ u2k with u = (u0; u1; u2), a unit-length vector. The corresponding rotation matrix R has u as the direction of the axis of rotation and 2θ as the angle of rotation about that axis. If v = (v0; v1; v2) ^ is a vector to be rotated, definev ^ = v0^{ + v1|^+ v2k. The rotated vector Rv = (r0; r1; r2) is generated by ^ ∗ definingr ^ = r0^{ + r1|^+ r2k and computing the quaternion productr ^ =q ^v^q^ , where q^∗ = w − x^{ − y|^− zk^ (4) is the conjugate ofq ^. The quaternion −q^ represents the same rotation matrix asq ^ because − q^ = − cos(θ) − sin(θ)^u = cos(θ + π) + sin(θ + π)^u (5) The rotation axis has direction u, the same as that corresponding toq ^. The rotation angle is 2(θ+π) = 2θ+2π. The rotation angle forq ^ is 2θ. The addition of 2π does not change the results of the rotation. Also, the convention used here is that matrix-vector multiplication has the order Mv; that is, the matrix is on the left and the vector is on the right. The implication is that a product of quaternionsp ^q^ represents a product of rotations PQv. Theq ^ is applied first and thep ^ is applied second. The orientation of an object is essentially a right-handed coordinate system assigned to that object. If U0, U1, and U2 are unit-length vectors that are mutually perpendicular and for which U2 = U0 × U1, the matrix R = [U0 U1 U2] whose columns are the specified vectors is a rotation matrix and may be represented by a quaternion. This document is about specifying a quaternionp ^ and constructing a quaternionq ^ that is closest top ^, where the candidates forq ^ are constrained in some manner. The closeness is measured by the angle between the corresponding 4-tuples, p and q. The constraints are Euler-angle constraints for the rotation matrices represented byq ^. 2 The Unconstrained Problem 2.1 Rotation X (The Analysis) The quaternion that represents a rotation about the x-axis by an angle 2θ is q^(θ) = cos(θ) + sin(θ)^{ (6) 3 ^ Given a quaternionp ^ = p0 + p1^{ + p2|^+ p3k, we wish to compute the angle θ 2 [−π; π] for whichq ^(θ) is the quaternion closest top ^ on the unit hypersphere in 4D. This problem is equivalent to selecting θ for which the angle between the 4-tuples p and q(θ) is a minimum. This is equivalent to maximizing the dot product f(θ) = p · q(θ) = (p0; p1; p2; p3) · (cos(θ); sin(θ); 0; 0) = p0 cos(θ) + p1 sin(θ) (7) because the dot product is the cosine of the angle between the vectors, and the angle is minimized when the cosine is largest. The dot product may be thought of as the dot product of 2-tuples, say, f(θ) = (p0; p1) · (cos(θ); sin(θ)). Figure1 illustrates when ( p0; p1) 6= (0; 0). Figure 1. The geometric relationship between (p0; p1) and (cos(θ); sin(θ)). The maximum dot product occurs when (p0; p1) and (cos(θ); sin(θ)) are parallel and pointing in the same direction; that is, (p ; p ) (cos(θ); sin(θ)) = 0 1 (8) p 2 2 p0 + p1 p 2 2 which immediately tells you the components of q(θ). The maximum dot product is fmax = p0 + p1. If you want to know the angle, it may be computed numerically as θ = atan2(p1; p0). When (p0; p1) = (0; 0), f(θ) is identically zero, in which case all q(θ) are equally close to p. Figure2 illustrates this in 3D, say, by ignoring the k component and thinking of p = (p0; p1; p2). 4 Figure 2. Left: The case when (p0; p1) 6= (0; 0). There is a unique closest quaternion on the circle. Right: The case when (p0; p1) = (0; 0), illustrated by (p0; p1; p2) = (0; 0; 1). All quaternions on the circle are closest. Pseudocode for computing the closest quaternion is listed next. Quaternion ClosestX (Quaternion p) f Quaternion q; q [ 2 ] = 0 ; q [ 3 ] = 0 ; f l o a t sqrLength = p[0] ∗ p [ 0 ] + p [ 1 ] ∗ p [ 1 ] ; i f ( s q r L e n g t h > 0) f //A unique closest point. f l o a t invLength = 1/sqrt(sqrLength); q [ 0 ] = p [ 0 ] ∗ i n v L e n g t h ; q [ 1 ] = p [ 1 ] ∗ i n v L e n g t h ; g e l s e f // Infinitely many solutions, choose the identity(theta= 0). q [ 0 ] = 1 ; q [ 1 ] = 0 ; g r e t u r n q ; g 2.2 Rotation X, Y, or Z Analyses similar to that for quaternions representing rotations about the x-axis may be done for rotations about the y-axis or z-axis. The pseudocode is nearly identical. Using indices to denote axes, we may use a single function. Quaternion Closest (Quaternion p, i n t a x i s ) //1(x −a x i s),2(y −a x i s),3(z −a x i s) f // The appropriate nonzero components will be set later. Quaternion q(0,0,0,0); f l o a t sqrLength = p[0] ∗ p[0] + p[axis]∗ p [ a x i s ] ; i f ( s q r L e n g t h > 0) f 5 //A unique closest point. f l o a t invLength = 1/sqrt(sqrLength); q [ 0 ] = p [ 0 ] ∗ i n v L e n g t h ; q[axis] = p[axis]∗ i n v L e n g t h ; g e l s e f // Infinitely many solutions, choose the identity(theta= 0). q [ 0 ] = 1 ; g r e t u r n q ; g 2.3 Rotation XY (The Analysis) The quaternion that represents a product of rotations, one about the y-axis by an angle 2θ1 and one about the x-axis by an angle 2θ0, is ^ q^(θ0; θ1) = (c0 + s0^{)(c1 + s1|^) = c0c1 + s0c1^{ + c0s1|^+ s0s1k (9) where c` = cos(θ`) and s` = sin(θ`) for ` = 0; 1. ^ Given a quaternionp ^ = p0 + p1^{ + p2|^+ p3k, we wish to compute the angles θ0 2 [−π; π] and θ1 2 [−π; π] for whichq ^(θ0; θ1) is the quaternion closest top ^ on the unit hypersphere in 4D. This problem is equivalent to selecting θ0 and θ1 for which the angle between the 4-tuples p and q(θ0; θ1) is a minimum. This is equivalent to maximizing the dot product f(θ0; θ1) = p · q(θ0; θ1) = p0c0c1 + p1s0c1 + p2c0s1 + p3s0s1 (10) Notice that 2 3 2 3 h i p0 p1 c0 T f = c1 s1 4 5 4 5 = u1 P u0 (11) p2 p3 s0 where the last equality defines the 2 × 1 vectors u0 and u1 and the 2 × 2 matrix P . The value of f is maximized when the vectors u1 and P u0 are parallel and in the same direction. Thus, we need P u0 u1 = (12) jP u0j in which case the maximum of f and its square are 2 T T fmax = jP u0j; fmax = u0 P P u0 (13) 2 We still need to compute a vector u0 for which fmax is maximized. This is the classical problem of maximizing a quadratic form involving a unit-length vector and, in our case, one whose symmetric matrix is P TP . The T maximum occurs when u0 is a unit-length eigenvector of P P corresponding to the maximum eigenvalue λmax of the matrix.