GteRotation.h
Go to the documentation of this file.
1 // David Eberly, Geometric Tools, Redmond WA 98052
2 // Copyright (c) 1998-2017
3 // Distributed under the Boost Software License, Version 1.0.
4 // http://www.boost.org/LICENSE_1_0.txt
5 // http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
6 // File Version: 3.0.0 (2016/06/19)
7 
8 #pragma once
9 
10 #include <Mathematics/GteAxisAngle.h>
11 #include <Mathematics/GteEulerAngles.h>
12 #include <Mathematics/GteMatrix.h>
13 #include <Mathematics/GteQuaternion.h>
14 #include <Mathematics/GteConstants.h>
15 
16 namespace gte
17 {
18 
19 // Conversions among various representations of rotations. The value of
20 // N must be 3 or 4. The latter case supports affine algebra when you use
21 // 4-tuple vectors (w-component is 1 for points and 0 for vector) and 4x4
22 // matrices for affine transformations. Rotation axes must be unit length.
23 // The angles are in radians. The Euler angles are in world coordinates;
24 // we have not yet added support for body coordinates.
25 
26 template <int N, typename Real>
27 class Rotation
28 {
29 public:
30  // Create rotations from various representations.
31  Rotation(Matrix<N,N,Real> const& matrix);
32  Rotation(Quaternion<Real> const& quaternion);
33  Rotation(AxisAngle<N,Real> const& axisAngle);
34  Rotation(EulerAngles<Real> const& eulerAngles);
35 
36  // Convert one representation to another.
37  operator Matrix<N,N,Real>() const;
38  operator Quaternion<Real>() const;
39  operator AxisAngle<N,Real>() const;
40  EulerAngles<Real> const& operator()(int i0, int i1, int i2) const;
41 
42 private:
43  enum RepresentationType
44  {
45  IS_MATRIX,
46  IS_QUATERNION,
47  IS_AXIS_ANGLE,
48  IS_EULER_ANGLES
49  };
50 
51  RepresentationType mType;
52  mutable Matrix<N,N,Real> mMatrix;
53  mutable Quaternion<Real> mQuaternion;
54  mutable AxisAngle<N,Real> mAxisAngle;
55  mutable EulerAngles<Real> mEulerAngles;
56 
57  // Convert a rotation matrix to a quaternion.
58  //
59  // x^2 = (+r00 - r11 - r22 + 1)/4
60  // y^2 = (-r00 + r11 - r22 + 1)/4
61  // z^2 = (-r00 - r11 + r22 + 1)/4
62  // w^2 = (+r00 + r11 + r22 + 1)/4
63  // x^2 + y^2 = (1 - r22)/2
64  // z^2 + w^2 = (1 + r22)/2
65  // y^2 - x^2 = (r11 - r00)/2
66  // w^2 - z^2 = (r11 + r00)/2
67  // x*y = (r01 + r10)/4
68  // x*z = (r02 + r20)/4
69  // y*z = (r12 + r21)/4
70  // [GTE_USE_MAT_VEC]
71  // x*w = (r21 - r12)/4
72  // y*w = (r02 - r20)/4
73  // z*w = (r10 - r01)/4
74  // [GTE_USE_VEC_MAT]
75  // x*w = (r12 - r21)/4
76  // y*w = (r20 - r02)/4
77  // z*w = (r01 - r10)/4
78  //
79  // If Q is the 4x1 column vector (x,y,z,w), the previous equations give us
80  // +- -+
81  // | x*x x*y x*z x*w |
82  // Q*Q^T = | y*x y*y y*z y*w |
83  // | z*x z*y z*z z*w |
84  // | w*x w*y w*z w*w |
85  // +- -+
86  // The code extracts the row of maximum length, normalizing it to obtain
87  // the result q.
88  static void Convert(Matrix<N,N,Real> const& r, Quaternion<Real>& q);
89 
90  // Convert a quaterion q = x*i + y*j + z*k + w to a rotation matrix.
91  // [GTE_USE_MAT_VEC]
92  // +- -+ +- -+
93  // R = | r00 r01 r02 | = | 1-2y^2-2z^2 2(xy-zw) 2(xz+yw) |
94  // | r10 r11 r12 | | 2(xy+zw) 1-2x^2-2z^2 2(yz-xw) |
95  // | r20 r21 r22 | | 2(xz-yw) 2(yz+xw) 1-2x^2-2y^2 |
96  // +- -+ +- -+
97  // [GTE_USE_VEC_MAT]
98  // +- -+ +- -+
99  // R = | r00 r01 r02 | = | 1-2y^2-2z^2 2(xy+zw) 2(xz-yw) |
100  // | r10 r11 r12 | | 2(xy-zw) 1-2x^2-2z^2 2(yz+xw) |
101  // | r20 r21 r22 | | 2(xz+yw) 2(yz-xw) 1-2x^2-2y^2 |
102  // +- -+ +- -+
103  static void Convert(Quaternion<Real> const& q, Matrix<N,N,Real>& r);
104 
105  // Convert a rotation matrix to an axis-angle pair. Let (x0,x1,x2) be the
106  // axis let t be an angle of rotation. The rotation matrix is
107  // [GTE_USE_MAT_VEC]
108  // R = I + sin(t)*S + (1-cos(t))*S^2
109  // or
110  // [GTE_USE_VEC_MAT]
111  // R = I - sin(t)*S + (1-cos(t))*S^2
112  // where I is the identity and S = {{0,-x2,x1},{x2,0,-x0},{-x1,x0,0}}
113  // where the inner-brace triples are the rows of the matrix. If t > 0,
114  // R represents a counterclockwise rotation; see the comments for the
115  // constructor Matrix3x3(axis,angle). It may be shown that cos(t) =
116  // (trace(R)-1)/2 and R - Transpose(R) = 2*sin(t)*S. As long as sin(t) is
117  // not zero, we may solve for S in the second equation, which produces the
118  // axis direction U = (S21,S02,S10). When t = 0, the rotation is the
119  // identity, in which case any axis direction is valid; we choose (1,0,0).
120  // When t = pi, it must be that R - Transpose(R) = 0, which prevents us
121  // from extracting the axis. Instead, note that (R+I)/2 = I+S^2 = U*U^T,
122  // where U is a unit-length axis direction.
123  static void Convert(Matrix<N,N,Real> const& r, AxisAngle<N,Real>& a);
124 
125  // Convert an axis-angle pair to a rotation matrix. Assuming (x0,x1,x2)
126  // is for a right-handed world (x0 to right, x1 up, x2 out of plane of
127  // page), a positive angle corresponds to a counterclockwise rotation from
128  // the perspective of an observer looking at the origin of the plane of
129  // rotation and having view direction the negative of the axis direction.
130  // The coordinate-axis rotations are the following, where
131  // unit(0) = (1,0,0), unit(1) = (0,1,0), unit(2) = (0,0,1),
132  // [GTE_USE_MAT_VEC]
133  // R(unit(0),t) = {{ 1, 0, 0}, { 0, c,-s}, { 0, s, c}}
134  // R(unit(1),t) = {{ c, 0, s}, { 0, 1, 0}, {-s, 0, c}}
135  // R(unit(2),t) = {{ c,-s, 0}, { s, c, 0}, { 0, 0, 1}}
136  // or
137  // [GTE_USE_VEC_MAT]
138  // R(unit(0),t) = {{ 1, 0, 0}, { 0, c, s}, { 0,-s, c}}
139  // R(unit(1),t) = {{ c, 0,-s}, { 0, 1, 0}, { s, 0, c}}
140  // R(unit(2),t) = {{ c, s, 0}, {-s, c, 0}, { 0, 0, 1}}
141  // where c = cos(t), s = sin(t), and the inner-brace triples are rows of
142  // the matrix. The general matrix is
143  // [GTE_USE_MAT_VEC]
144  // +- -+
145  // R = | (1-c)*x0^2 + c (1-c)*x0*x1 - s*x2 (1-c)*x0*x2 + s*x1 |
146  // | (1-c)*x0*x1 + s*x2 (1-c)*x1^2 + c (1-c)*x1*x2 - s*x0 |
147  // | (1-c)*x0*x2 - s*x1 (1-c)*x1*x2 + s*x0 (1-c)*x2^2 + c |
148  // +- -+
149  // [GTE_USE_VEC_MAT]
150  // +- -+
151  // R = | (1-c)*x0^2 + c (1-c)*x0*x1 + s*x2 (1-c)*x0*x2 - s*x1 |
152  // | (1-c)*x0*x1 - s*x2 (1-c)*x1^2 + c (1-c)*x1*x2 + s*x0 |
153  // | (1-c)*x0*x2 + s*x1 (1-c)*x1*x2 - s*x0 (1-c)*x2^2 + c |
154  // +- -+
155  static void Convert(AxisAngle<N,Real> const& a, Matrix<N,N,Real>& r);
156 
157  // Convert a rotation matrix to Euler angles. Factorization into Euler
158  // angles is not necessarily unique. If the result is ER_NOT_UNIQUE_SUM,
159  // then the multiple solutions occur because angleN2+angleN0 is constant.
160  // If the result is ER_NOT_UNIQUE_DIF, then the multiple solutions occur
161  // because angleN2-angleN0 is constant. In either type of nonuniqueness,
162  // the function returns angleN0=0.
163  static void Convert(Matrix<N,N,Real> const& r, EulerAngles<Real>& e);
164 
165  // Convert Euler angles to a rotation matrix. The three integer inputs
166  // are in {0,1,2} and correspond to world directions unit(0) = (1,0,0),
167  // unit(1) = (0,1,0), or unit(2) = (0,0,1). The triples (N0,N1,N2) must
168  // be in the following set,
169  // {(0,1,2),(0,2,1),(1,0,2),(1,2,0),(2,0,1),(2,1,0),
170  // (0,1,0),(0,2,0),(1,0,1),(1,2,1),(2,0,2),(2,1,2)}
171  // The rotation matrix is
172  // [GTE_USE_MAT_VEC]
173  // R(unit(N2),angleN2)*R(unit(N1),angleN1)*R(unit(N0),angleN0)
174  // or
175  // [GTE_USE_VEC_MAT]
176  // R(unit(N0),angleN0)*R(unit(N1),angleN1)*R(unit(N2),angleN2)
177  // The conventions of constructor Matrix3(axis,angle) apply here as well.
178  //
179  // NOTE: The reversal of order is chosen so that a rotation matrix built
180  // with one multiplication convention is the transpose of the rotation
181  // matrix built with the other multiplication convention. Thus,
182  // [GTE_USE_MAT_VEC]
183  // Matrix3x3<Real> R_mvconvention(N0,N1,N2,angleN0,angleN1,angleN2);
184  // Vector3<Real> V(...);
185  // Vector3<Real> U = R_mvconvention*V; // (u0,u1,u2) = R2*R1*R0*V
186  // [GTE_USE_VEC_MAT]
187  // Matrix3x3<Real> R_vmconvention(N0,N1,N2,angleN0,angleN1,angleN2);
188  // Vector3<Real> V(...);
189  // Vector3<Real> U = R_mvconvention*V; // (u0,u1,u2) = V*R0*R1*R2
190  // In either convention, you get the same 3-tuple U.
191  static void Convert(EulerAngles<Real> const& e, Matrix<N,N,Real>& r);
192 
193  // Convert a quaternion to an axis-angle pair, where
194  // q = sin(angle/2)*(axis[0]*i + axis[1]*j + axis[2]*k) + cos(angle/2)
195  static void Convert(Quaternion<Real> const& q, AxisAngle<N,Real>& a);
196 
197  // Convert an axis-angle pair to a quaternion, where
198  // q = sin(angle/2)*(axis[0]*i + axis[1]*j + axis[2]*k) + cos(angle/2)
199  static void Convert(AxisAngle<N,Real> const& a, Quaternion<Real>& q);
200 
201  // Convert a quaternion to Euler angles. The quaternion is converted to
202  // a matrix which is then converted to Euler angles.
203  static void Convert(Quaternion<Real> const& q, EulerAngles<Real>& e);
204 
205  // Convert Euler angles to a quaternion. The Euler angles are converted
206  // to a matrix which is then converted to a quaternion.
207  static void Convert(EulerAngles<Real> const& e, Quaternion<Real>& q);
208 
209  // Convert an axis-angle pair to Euler angles. The axis-angle pair is
210  // converted to a quaternion which is then converted to Euler angles.
211  static void Convert(AxisAngle<N,Real> const& a, EulerAngles<Real>& e);
212 
213  // Convert Euler angles to an axis-angle pair. The Euler angles are
214  // converted to a quaternion which is then converted to an axis-angle
215  // pair.
216  static void Convert(EulerAngles<Real> const& e, AxisAngle<N,Real>& a);
217 };
218 
219 
220 template <int N, typename Real>
221 Rotation<N, Real>::Rotation(Matrix<N, N, Real> const& matrix)
222  :
223  mType(IS_MATRIX),
224  mMatrix(matrix)
225 {
226  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
227 }
228 
229 template <int N, typename Real>
230 Rotation<N, Real>::Rotation(Quaternion<Real> const& quaternion)
231  :
232  mType(IS_QUATERNION),
233  mQuaternion(quaternion)
234 {
235  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
236 }
237 
238 template <int N, typename Real>
239 Rotation<N, Real>::Rotation(AxisAngle<N, Real> const& axisAngle)
240  :
241  mType(IS_AXIS_ANGLE),
242  mAxisAngle(axisAngle)
243 {
244  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
245 }
246 
247 template <int N, typename Real>
248 Rotation<N, Real>::Rotation(EulerAngles<Real> const& eulerAngles)
249  :
250  mType(IS_EULER_ANGLES),
251  mEulerAngles(eulerAngles)
252 {
253  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
254 }
255 
256 template <int N, typename Real>
257 Rotation<N, Real>::operator Matrix<N, N, Real>() const
258 {
259  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
260 
261  switch (mType)
262  {
263  case IS_MATRIX:
264  break;
265  case IS_QUATERNION:
266  Convert(mQuaternion, mMatrix);
267  break;
268  case IS_AXIS_ANGLE:
269  Convert(mAxisAngle, mMatrix);
270  break;
271  case IS_EULER_ANGLES:
272  Convert(mEulerAngles, mMatrix);
273  break;
274  }
275 
276  return mMatrix;
277 }
278 
279 template <int N, typename Real>
280 Rotation<N, Real>::operator Quaternion<Real>() const
281 {
282  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
283 
284  switch (mType)
285  {
286  case IS_MATRIX:
287  Convert(mMatrix, mQuaternion);
288  break;
289  case IS_QUATERNION:
290  break;
291  case IS_AXIS_ANGLE:
292  Convert(mAxisAngle, mQuaternion);
293  break;
294  case IS_EULER_ANGLES:
295  Convert(mEulerAngles, mQuaternion);
296  break;
297  }
298 
299  return mQuaternion;
300 }
301 
302 template <int N, typename Real>
303 Rotation<N, Real>::operator AxisAngle<N, Real>() const
304 {
305  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
306 
307  switch (mType)
308  {
309  case IS_MATRIX:
310  Convert(mMatrix, mAxisAngle);
311  break;
312  case IS_QUATERNION:
313  Convert(mQuaternion, mAxisAngle);
314  break;
315  case IS_AXIS_ANGLE:
316  break;
317  case IS_EULER_ANGLES:
318  Convert(mEulerAngles, mAxisAngle);
319  break;
320  }
321 
322  return mAxisAngle;
323 }
324 
325 template <int N, typename Real>
326 EulerAngles<Real> const& Rotation<N, Real>::operator()(int i0, int i1,
327  int i2) const
328 {
329  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
330 
331  mEulerAngles.axis[0] = i0;
332  mEulerAngles.axis[1] = i1;
333  mEulerAngles.axis[2] = i2;
334 
335  switch (mType)
336  {
337  case IS_MATRIX:
338  Convert(mMatrix, mEulerAngles);
339  break;
340  case IS_QUATERNION:
341  Convert(mQuaternion, mEulerAngles);
342  break;
343  case IS_AXIS_ANGLE:
344  Convert(mAxisAngle, mEulerAngles);
345  break;
346  case IS_EULER_ANGLES:
347  break;
348  }
349 
350  return mEulerAngles;
351 }
352 
353 template <int N, typename Real>
354 void Rotation<N, Real>::Convert(Matrix<N, N, Real> const& r,
355  Quaternion<Real>& q)
356 {
357  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
358 
359  Real r22 = r(2, 2);
360  if (r22 <= (Real)0) // x^2 + y^2 >= z^2 + w^2
361  {
362  Real dif10 = r(1, 1) - r(0, 0);
363  Real omr22 = (Real)1 - r22;
364  if (dif10 <= (Real)0) // x^2 >= y^2
365  {
366  Real fourXSqr = omr22 - dif10;
367  Real inv4x = ((Real)0.5) / sqrt(fourXSqr);
368  q[0] = fourXSqr*inv4x;
369  q[1] = (r(0, 1) + r(1, 0))*inv4x;
370  q[2] = (r(0, 2) + r(2, 0))*inv4x;
371 #if defined(GTE_USE_MAT_VEC)
372  q[3] = (r(2, 1) - r(1, 2))*inv4x;
373 #else
374  q[3] = (r(1, 2) - r(2, 1))*inv4x;
375 #endif
376  }
377  else // y^2 >= x^2
378  {
379  Real fourYSqr = omr22 + dif10;
380  Real inv4y = ((Real)0.5) / sqrt(fourYSqr);
381  q[0] = (r(0, 1) + r(1, 0))*inv4y;
382  q[1] = fourYSqr*inv4y;
383  q[2] = (r(1, 2) + r(2, 1))*inv4y;
384 #if defined(GTE_USE_MAT_VEC)
385  q[3] = (r(0, 2) - r(2, 0))*inv4y;
386 #else
387  q[3] = (r(2, 0) - r(0, 2))*inv4y;
388 #endif
389  }
390  }
391  else // z^2 + w^2 >= x^2 + y^2
392  {
393  Real sum10 = r(1, 1) + r(0, 0);
394  Real opr22 = (Real)1 + r22;
395  if (sum10 <= (Real)0) // z^2 >= w^2
396  {
397  Real fourZSqr = opr22 - sum10;
398  Real inv4z = ((Real)0.5) / sqrt(fourZSqr);
399  q[0] = (r(0, 2) + r(2, 0))*inv4z;
400  q[1] = (r(1, 2) + r(2, 1))*inv4z;
401  q[2] = fourZSqr*inv4z;
402 #if defined(GTE_USE_MAT_VEC)
403  q[3] = (r(1, 0) - r(0, 1))*inv4z;
404 #else
405  q[3] = (r(0, 1) - r(1, 0))*inv4z;
406 #endif
407  }
408  else // w^2 >= z^2
409  {
410  Real fourWSqr = opr22 + sum10;
411  Real inv4w = ((Real)0.5) / sqrt(fourWSqr);
412 #if defined(GTE_USE_MAT_VEC)
413  q[0] = (r(2, 1) - r(1, 2))*inv4w;
414  q[1] = (r(0, 2) - r(2, 0))*inv4w;
415  q[2] = (r(1, 0) - r(0, 1))*inv4w;
416 #else
417  q[0] = (r(1, 2) - r(2, 1))*inv4w;
418  q[1] = (r(2, 0) - r(0, 2))*inv4w;
419  q[2] = (r(0, 1) - r(1, 0))*inv4w;
420 #endif
421  q[3] = fourWSqr*inv4w;
422  }
423  }
424 }
425 
426 template <int N, typename Real>
427 void Rotation<N, Real>::Convert(Quaternion<Real> const& q, Matrix<N, N, Real>& r)
428 {
429  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
430 
431  r.MakeIdentity();
432 
433  Real twoX = ((Real)2)*q[0];
434  Real twoY = ((Real)2)*q[1];
435  Real twoZ = ((Real)2)*q[2];
436  Real twoXX = twoX*q[0];
437  Real twoXY = twoX*q[1];
438  Real twoXZ = twoX*q[2];
439  Real twoXW = twoX*q[3];
440  Real twoYY = twoY*q[1];
441  Real twoYZ = twoY*q[2];
442  Real twoYW = twoY*q[3];
443  Real twoZZ = twoZ*q[2];
444  Real twoZW = twoZ*q[3];
445 
446 #if defined(GTE_USE_MAT_VEC)
447  r(0, 0) = (Real)1 - twoYY - twoZZ;
448  r(0, 1) = twoXY - twoZW;
449  r(0, 2) = twoXZ + twoYW;
450  r(1, 0) = twoXY + twoZW;
451  r(1, 1) = (Real)1 - twoXX - twoZZ;
452  r(1, 2) = twoYZ - twoXW;
453  r(2, 0) = twoXZ - twoYW;
454  r(2, 1) = twoYZ + twoXW;
455  r(2, 2) = (Real)1 - twoXX - twoYY;
456 #else
457  r(0, 0) = (Real)1 - twoYY - twoZZ;
458  r(1, 0) = twoXY - twoZW;
459  r(2, 0) = twoXZ + twoYW;
460  r(0, 1) = twoXY + twoZW;
461  r(1, 1) = (Real)1 - twoXX - twoZZ;
462  r(2, 1) = twoYZ - twoXW;
463  r(0, 2) = twoXZ - twoYW;
464  r(1, 2) = twoYZ + twoXW;
465  r(2, 2) = (Real)1 - twoXX - twoYY;
466 #endif
467 }
468 
469 template <int N, typename Real>
470 void Rotation<N, Real>::Convert(Matrix<N, N, Real> const& r,
471  AxisAngle<N, Real>& a)
472 {
473  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
474 
475  Real trace = r(0, 0) + r(1, 1) + r(2, 2);
476  Real half = (Real)0.5;
477  Real cs = half*(trace - (Real)1);
478  cs = std::max(std::min(cs, (Real)1), (Real)-1);
479  a.angle = acos(cs); // The angle is in [0,pi].
480  a.axis.MakeZero();
481 
482  if (a.angle > (Real)0)
483  {
484  if (a.angle < (Real)GTE_C_PI)
485  {
486  // The angle is in (0,pi).
487 #if defined(GTE_USE_MAT_VEC)
488  a.axis[0] = r(2, 1) - r(1, 2);
489  a.axis[1] = r(0, 2) - r(2, 0);
490  a.axis[2] = r(1, 0) - r(0, 1);
491  Normalize(a.axis);
492 #else
493  a.axis[0] = r(1, 2) - r(2, 1);
494  a.axis[1] = r(2, 0) - r(0, 2);
495  a.axis[2] = r(0, 1) - r(1, 0);
496  Normalize(a.axis);
497 #endif
498  }
499  else
500  {
501  // The angle is pi, in which case R is symmetric and
502  // R+I = 2*(I+S^2) = 2*U*U^T, where U = (u0,u1,u2) is the
503  // unit-length direction of the rotation axis. Determine the
504  // largest diagonal entry of R+I and normalize the
505  // corresponding row to produce U. It does not matter the
506  // sign on u[d] for chosen diagonal d, because R(U,pi) = R(-U,pi).
507  Real one = (Real)1;
508  if (r(0, 0) >= r(1, 1))
509  {
510  if (r(0, 0) >= r(2, 2))
511  {
512  // r00 is maximum diagonal term
513  a.axis[0] = r(0, 0) + one;
514  a.axis[1] = half*(r(0, 1) + r(1, 0));
515  a.axis[2] = half*(r(0, 2) + r(2, 0));
516  }
517  else
518  {
519  // r22 is maximum diagonal term
520  a.axis[0] = half*(r(2, 0) + r(0, 2));
521  a.axis[1] = half*(r(2, 1) + r(1, 2));
522  a.axis[2] = r(2, 2) + one;
523  }
524  }
525  else
526  {
527  if (r(1, 1) >= r(2, 2))
528  {
529  // r11 is maximum diagonal term
530  a.axis[0] = half*(r(1, 0) + r(0, 1));
531  a.axis[1] = r(1, 1) + one;
532  a.axis[2] = half*(r(1, 2) + r(2, 1));
533  }
534  else
535  {
536  // r22 is maximum diagonal term
537  a.axis[0] = half*(r(2, 0) + r(0, 2));
538  a.axis[1] = half*(r(2, 1) + r(1, 2));
539  a.axis[2] = r(2, 2) + one;
540  }
541  }
542  Normalize(a.axis);
543  }
544  }
545  else
546  {
547  // The angle is 0 and the matrix is the identity. Any axis will
548  // work, so choose the Unit(0) axis.
549  a.axis[0] = (Real)1;
550  }
551 }
552 
553 template <int N, typename Real>
554 void Rotation<N, Real>::Convert(AxisAngle<N, Real> const& a,
555  Matrix<N, N, Real>& r)
556 {
557  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
558 
559  r.MakeIdentity();
560 
561  Real cs = cos(a.angle);
562  Real sn = sin(a.angle);
563  Real oneMinusCos = ((Real)1) - cs;
564  Real x0sqr = a.axis[0] * a.axis[0];
565  Real x1sqr = a.axis[1] * a.axis[1];
566  Real x2sqr = a.axis[2] * a.axis[2];
567  Real x0x1m = a.axis[0] * a.axis[1] * oneMinusCos;
568  Real x0x2m = a.axis[0] * a.axis[2] * oneMinusCos;
569  Real x1x2m = a.axis[1] * a.axis[2] * oneMinusCos;
570  Real x0Sin = a.axis[0] * sn;
571  Real x1Sin = a.axis[1] * sn;
572  Real x2Sin = a.axis[2] * sn;
573 
574 #if defined(GTE_USE_MAT_VEC)
575  r(0, 0) = x0sqr*oneMinusCos + cs;
576  r(0, 1) = x0x1m - x2Sin;
577  r(0, 2) = x0x2m + x1Sin;
578  r(1, 0) = x0x1m + x2Sin;
579  r(1, 1) = x1sqr*oneMinusCos + cs;
580  r(1, 2) = x1x2m - x0Sin;
581  r(2, 0) = x0x2m - x1Sin;
582  r(2, 1) = x1x2m + x0Sin;
583  r(2, 2) = x2sqr*oneMinusCos + cs;
584 #else
585  r(0, 0) = x0sqr*oneMinusCos + cs;
586  r(1, 0) = x0x1m - x2Sin;
587  r(2, 0) = x0x2m + x1Sin;
588  r(0, 1) = x0x1m + x2Sin;
589  r(1, 1) = x1sqr*oneMinusCos + cs;
590  r(2, 1) = x1x2m - x0Sin;
591  r(0, 2) = x0x2m - x1Sin;
592  r(1, 2) = x1x2m + x0Sin;
593  r(2, 2) = x2sqr*oneMinusCos + cs;
594 #endif
595 }
596 
597 template <int N, typename Real>
598 void Rotation<N, Real>::Convert(Matrix<N, N, Real> const& r,
599  EulerAngles<Real>& e)
600 {
601  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
602 
603  if (0 <= e.axis[0] && e.axis[0] < 3
604  && 0 <= e.axis[1] && e.axis[1] < 3
605  && 0 <= e.axis[2] && e.axis[2] < 3
606  && e.axis[1] != e.axis[0]
607  && e.axis[1] != e.axis[2])
608  {
609  if (e.axis[0] != e.axis[2])
610  {
611 #if defined(GTE_USE_MAT_VEC)
612  // Map (0,1,2), (1,2,0), and (2,0,1) to +1.
613  // Map (0,2,1), (2,1,0), and (1,0,2) to -1.
614  int parity = (((e.axis[2] | (e.axis[1] << 2)) >> e.axis[0]) & 1);
615  Real const sgn = (parity & 1 ? (Real)-1 : (Real)+1);
616 
617  if (r(e.axis[2], e.axis[0]) < (Real)1)
618  {
619  if (r(e.axis[2], e.axis[0]) > (Real)-1)
620  {
621  e.angle[2] = atan2(sgn*r(e.axis[1], e.axis[0]),
622  r(e.axis[0], e.axis[0]));
623  e.angle[1] = asin(-sgn*r(e.axis[2], e.axis[0]));
624  e.angle[0] = atan2(sgn*r(e.axis[2], e.axis[1]),
625  r(e.axis[2], e.axis[2]));
626  e.result = ER_UNIQUE;
627  }
628  else
629  {
630  e.angle[2] = (Real)0;
631  e.angle[1] = sgn*(Real)GTE_C_HALF_PI;
632  e.angle[0] = atan2(-sgn*r(e.axis[1], e.axis[2]),
633  r(e.axis[1], e.axis[1]));
634  e.result = ER_NOT_UNIQUE_DIF;
635  }
636  }
637  else
638  {
639  e.angle[2] = (Real)0;
640  e.angle[1] = -sgn*(Real)GTE_C_HALF_PI;
641  e.angle[0] = atan2(-sgn*r(e.axis[1], e.axis[2]),
642  r(e.axis[1], e.axis[1]));
643  e.result = ER_NOT_UNIQUE_SUM;
644  }
645 #else
646  // Map (0,1,2), (1,2,0), and (2,0,1) to +1.
647  // Map (0,2,1), (2,1,0), and (1,0,2) to -1.
648  int parity = (((e.axis[0] | (e.axis[1] << 2)) >> e.axis[2]) & 1);
649  Real const sgn = (parity & 1 ? (Real)+1 : (Real)-1);
650 
651  if (r(e.axis[0], e.axis[2]) < (Real)1)
652  {
653  if (r(e.axis[0], e.axis[2]) > (Real)-1)
654  {
655  e.angle[0] = atan2(sgn*r(e.axis[1], e.axis[2]),
656  r(e.axis[2], e.axis[2]));
657  e.angle[1] = asin(-sgn*r(e.axis[0], e.axis[2]));
658  e.angle[2] = atan2(sgn*r(e.axis[0], e.axis[1]),
659  r(e.axis[0], e.axis[0]));
660  e.result = ER_UNIQUE;
661  }
662  else
663  {
664  e.angle[0] = (Real)0;
665  e.angle[1] = sgn*(Real)GTE_C_HALF_PI;
666  e.angle[2] = atan2(-sgn*r(e.axis[1], e.axis[0]),
667  r(e.axis[1], e.axis[1]));
668  e.result = ER_NOT_UNIQUE_DIF;
669  }
670  }
671  else
672  {
673  e.angle[0] = (Real)0;
674  e.angle[1] = -sgn*(Real)GTE_C_HALF_PI;
675  e.angle[2] = atan2(-sgn*r(e.axis[1], e.axis[0]),
676  r(e.axis[1], e.axis[1]));
677  e.result = ER_NOT_UNIQUE_SUM;
678  }
679 #endif
680  }
681  else
682  {
683 #if defined(GTE_USE_MAT_VEC)
684  // Map (0,2,0), (1,0,1), and (2,1,2) to +1.
685  // Map (0,1,0), (1,2,1), and (2,0,2) to -1.
686  int b0 = 3 - e.axis[1] - e.axis[2];
687  int parity = (((b0 | (e.axis[1] << 2)) >> e.axis[2]) & 1);
688  Real const sgn = (parity & 1 ? (Real)+1 : (Real)-1);
689 
690  if (r(e.axis[2], e.axis[2]) < (Real)1)
691  {
692  if (r(e.axis[2], e.axis[2]) > (Real)-1)
693  {
694  e.angle[2] = atan2(r(e.axis[1], e.axis[2]),
695  sgn*r(b0, e.axis[2]));
696  e.angle[1] = acos(r(e.axis[2], e.axis[2]));
697  e.angle[0] = atan2(r(e.axis[2], e.axis[1]),
698  -sgn*r(e.axis[2], b0));
699  e.result = ER_UNIQUE;
700  }
701  else
702  {
703  e.angle[2] = (Real)0;
704  e.angle[1] = (Real)GTE_C_PI;
705  e.angle[0] = atan2(sgn*r(e.axis[1], b0),
706  r(e.axis[1], e.axis[1]));
707  e.result = ER_NOT_UNIQUE_DIF;
708  }
709  }
710  else
711  {
712  e.angle[2] = (Real)0;
713  e.angle[1] = (Real)0;
714  e.angle[0] = atan2(sgn*r(e.axis[1], b0),
715  r(e.axis[1], e.axis[1]));
716  e.result = ER_NOT_UNIQUE_SUM;
717  }
718 #else
719  // Map (0,2,0), (1,0,1), and (2,1,2) to -1.
720  // Map (0,1,0), (1,2,1), and (2,0,2) to +1.
721  int b2 = 3 - e.axis[0] - e.axis[1];
722  int parity = (((b2 | (e.axis[1] << 2)) >> e.axis[0]) & 1);
723  Real const sgn = (parity & 1 ? (Real)-1 : (Real)+1);
724 
725  if (r(e.axis[0], e.axis[0]) < (Real)1)
726  {
727  if (r(e.axis[0], e.axis[0]) > (Real)-1)
728  {
729  e.angle[0] = atan2(r(e.axis[1], e.axis[0]),
730  sgn*r(b2, e.axis[0]));
731  e.angle[1] = acos(r(e.axis[0], e.axis[0]));
732  e.angle[2] = atan2(r(e.axis[0], e.axis[1]),
733  -sgn*r(e.axis[0], b2));
734  e.result = ER_UNIQUE;
735  }
736  else
737  {
738  e.angle[0] = (Real)0;
739  e.angle[1] = (Real)GTE_C_PI;
740  e.angle[2] = atan2(sgn*r(e.axis[1], b2),
741  r(e.axis[1], e.axis[1]));
742  e.result = ER_NOT_UNIQUE_DIF;
743  }
744  }
745  else
746  {
747  e.angle[0] = (Real)0;
748  e.angle[1] = (Real)0;
749  e.angle[2] = atan2(sgn*r(e.axis[1], b2),
750  r(e.axis[1], e.axis[1]));
751  e.result = ER_NOT_UNIQUE_SUM;
752  }
753 #endif
754  }
755  }
756  else
757  {
758  // Invalid angles.
759  e.angle[0] = (Real)0;
760  e.angle[1] = (Real)0;
761  e.angle[2] = (Real)0;
762  e.result = ER_INVALID;
763  }
764 }
765 
766 template <int N, typename Real>
767 void Rotation<N, Real>::Convert(EulerAngles<Real> const& e,
768  Matrix<N, N, Real>& r)
769 {
770  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
771 
772  if (0 <= e.axis[0] && e.axis[0] < 3
773  && 0 <= e.axis[1] && e.axis[1] < 3
774  && 0 <= e.axis[2] && e.axis[2] < 3
775  && e.axis[1] != e.axis[0]
776  && e.axis[1] != e.axis[2])
777  {
778  Matrix<N, N, Real> r0, r1, r2;
779  Convert(AxisAngle<N, Real>(Vector<N, Real>::Unit(e.axis[0]),
780  e.angle[0]), r0);
781  Convert(AxisAngle<N, Real>(Vector<N, Real>::Unit(e.axis[1]),
782  e.angle[1]), r1);
783  Convert(AxisAngle<N, Real>(Vector<N, Real>::Unit(e.axis[2]),
784  e.angle[2]), r2);
785 #if defined(GTE_USE_MAT_VEC)
786  r = r2*r1*r0;
787 #else
788  r = r0*r1*r2;
789 #endif
790  }
791  else
792  {
793  // Invalid angles.
794  r.MakeIdentity();
795  }
796 }
797 
798 template <int N, typename Real>
799 void Rotation<N, Real>::Convert(Quaternion<Real> const& q,
800  AxisAngle<N, Real>& a)
801 {
802  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
803 
804  a.axis.MakeZero();
805 
806  Real axisSqrLen = q[0] * q[0] + q[1] * q[1] + q[2] * q[2];
807  if (axisSqrLen > (Real)0)
808  {
809 #if defined(GTE_USE_MAT_VEC)
810  Real adjust = ((Real)1) / sqrt(axisSqrLen);
811 #else
812  Real adjust = ((Real)-1) / sqrt(axisSqrLen);
813 #endif
814  a.axis[0] = q[0] * adjust;
815  a.axis[1] = q[1] * adjust;
816  a.axis[2] = q[2] * adjust;
817  Real cs = std::max(std::min(q[3], (Real)1), (Real)-1);
818  a.angle = ((Real)2)*acos(cs);
819  }
820  else
821  {
822  // The angle is 0 (modulo 2*pi). Any axis will work, so choose the
823  // Unit(0) axis.
824  a.axis[0] = (Real)1;
825  a.angle = (Real)0;
826  }
827 }
828 
829 template <int N, typename Real>
830 void Rotation<N, Real>::Convert(AxisAngle<N, Real> const& a,
831  Quaternion<Real>& q)
832 {
833  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
834 
835 #if defined(GTE_USE_MAT_VEC)
836  Real halfAngle = ((Real)0.5)*a.angle;
837 #else
838  Real halfAngle = ((Real)-0.5)*a.angle;
839 #endif
840  Real sn = sin(halfAngle);
841  q[0] = sn*a.axis[0];
842  q[1] = sn*a.axis[1];
843  q[2] = sn*a.axis[2];
844  q[3] = cos(halfAngle);
845 }
846 
847 template <int N, typename Real>
848 void Rotation<N, Real>::Convert(Quaternion<Real> const& q,
849  EulerAngles<Real>& e)
850 {
851  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
852 
853  Matrix<N, N, Real> r;
854  Convert(q, r);
855  Convert(r, e);
856 }
857 
858 template <int N, typename Real>
859 void Rotation<N, Real>::Convert(EulerAngles<Real> const& e,
860  Quaternion<Real>& q)
861 {
862  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
863 
864  Matrix<N, N, Real> r;
865  Convert(e, r);
866  Convert(r, q);
867 }
868 
869 template <int N, typename Real>
870 void Rotation<N, Real>::Convert(AxisAngle<N, Real> const& a,
871  EulerAngles<Real>& e)
872 {
873  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
874 
875  Quaternion<Real> q;
876  Convert(a, q);
877  Convert(q, e);
878 }
879 
880 template <int N, typename Real>
881 void Rotation<N, Real>::Convert(EulerAngles<Real> const& e,
882  AxisAngle<N, Real>& a)
883 {
884  static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
885 
886  Quaternion<Real> q;
887  Convert(e, q);
888  Convert(q, a);
889 }
890 
891 
892 }
gte::Rotation::Convert
static void Convert(Matrix< N, N, Real > const &r, Quaternion< Real > &q)
Definition: GteRotation.h:354
gte::Rotation::mEulerAngles
EulerAngles< Real > mEulerAngles
Definition: GteRotation.h:55
gte::Rotation::mType
RepresentationType mType
Definition: GteRotation.h:51
matrix
GLuint GLenum matrix
Definition: glext.h:10574
gte::Matrix::MakeIdentity
void MakeIdentity()
Definition: GteMatrix.h:462
gte::Rotation::Rotation
Rotation(Matrix< N, N, Real > const &matrix)
Definition: GteRotation.h:221
gte::Rotation::mAxisAngle
AxisAngle< N, Real > mAxisAngle
Definition: GteRotation.h:54
gte::EulerAngles::result
EulerResult result
Definition: GteEulerAngles.h:51
a
GLboolean GLboolean GLboolean GLboolean a
Definition: glcorearb.h:1217
GTE_C_PI
#define GTE_C_PI
Definition: GteConstants.h:17
gte::Rotation::mMatrix
Matrix< N, N, Real > mMatrix
Definition: GteRotation.h:52
gte::Normalize
Real Normalize(GVector< Real > &v, bool robust=false)
Definition: GteGVector.h:454
r
GLboolean r
Definition: glcorearb.h:1217
GTE_C_HALF_PI
#define GTE_C_HALF_PI
Definition: GteConstants.h:18
gte::AxisAngle::axis
Vector< N, Real > axis
Definition: GteAxisAngle.h:26
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:255
gte::Rotation::mQuaternion
Quaternion< Real > mQuaternion
Definition: GteRotation.h:53
gte::Rotation::operator()
EulerAngles< Real > const & operator()(int i0, int i1, int i2) const
Definition: GteRotation.h:326

geometric_tools_engine
Author(s): Yijiang Huang
autogenerated on Thu Jul 18 2019 04:00:01