HDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
UT_Quaternion.h
Go to the documentation of this file.
1 /*
2  * PROPRIETARY INFORMATION. This software is proprietary to
3  * Side Effects Software Inc., and is not to be reproduced,
4  * transmitted, or disclosed in any way without written permission.
5  *
6  * NAME: UT library (C++)
7  *
8  * COMMENTS: This class implements quaternions.
9  *
10  * WARNING:
11  * This class should NOT contain any virtual methods, nor should it
12  * define more member data. The size of UT_QuaternionF must always be
13  * 16 bytes (4 floats).
14  *
15  */
16 
17 #pragma once
18 
19 #ifndef __UT_Quaternion_h__
20 #define __UT_Quaternion_h__
21 
22 #include "UT_API.h"
23 #include "UT_FixedVector.h"
24 #include "UT_Vector3.h"
25 #include "UT_Vector4.h"
26 #include "UT_VectorTypes.h"
27 
28 #include <SYS/SYS_Math.h>
29 #include <limits>
30 #include <stddef.h>
31 
32 class UT_IStream;
33 
34 // Forward declaration
35 template <typename T> class UT_API UT_QuaternionT;
36 
37 /// Perform component-wise SYSlerp of two quaternions
38 template <typename T>
39 inline UT_QuaternionT<T>
40 SYSlerp(const UT_QuaternionT<T> &q1, const UT_QuaternionT<T> &q2, T t);
41 
42 /// Quaternion class
43 template <typename T>
45 {
46 public:
47 
48  typedef T value_type;
49  static const int tuple_size = 4;
50 
51  UT_QuaternionT(T qx=0, T qy=0,
52  T qz=0, T qw=0)
53  {
54  vec[0] = qx; vec[1] = qy;
55  vec[2] = qz; vec[3] = qw;
56  }
57  UT_QuaternionT(const fpreal32 v[tuple_size])
58  {
59  vec[0] = v[0]; vec[1] = v[1];
60  vec[2] = v[2]; vec[3] = v[3];
61  }
62  UT_QuaternionT(const fpreal64 v[tuple_size])
63  {
64  vec[0] = v[0]; vec[1] = v[1];
65  vec[2] = v[2]; vec[3] = v[3];
66  }
68  {
69  vec[0] = v[0]; vec[1] = v[1];
70  vec[2] = v[2]; vec[3] = v[3];
71  }
72  /// Arbitrary UT_FixedVector of the same size
73  template <typename S>
75  {
76  vec[0] = v[0]; vec[1] = v[1];
77  vec[2] = v[2]; vec[3] = v[3];
78  }
80  const UT_Vector3T<T> &axis,
81  int donormalize=1);
83  {
84  vec[0] = q.vec[0];
85  vec[1] = q.vec[1];
86  vec[2] = q.vec[2];
87  vec[3] = q.vec[3];
88  }
90  const UT_XformOrder &order)
91  {
92  updateFromEuler(rot, order);
93  }
94 
95  // Default assignment operator is fine.
96  template <typename S>
98  { vec[0] = v.x(); vec[1] = v.y(); vec[2] = v.z(); vec[3] = v.w(); }
99  template <typename S>
101  { vec[0] = v.x(); vec[1] = v.y(); vec[2] = v.z(); vec[3] = v.w(); return *this; }
102 
104  inline UT_QuaternionT<T> &operator*=(T scalar);
105  inline UT_QuaternionT<T> &operator/=(const UT_QuaternionT<T> &quat);
106  inline UT_QuaternionT<T> &operator/=(T scalar);
107  inline UT_QuaternionT<T> &operator+=(const UT_QuaternionT<T> &quat);
108  bool operator==(const UT_QuaternionT<T> &quat) const;
109  bool operator!=(const UT_QuaternionT<T> &quat) const;
110  T operator()(int idx) const
111  { return vec[idx]; }
112  T &operator()(int idx)
113  { return vec[idx]; }
114  T operator[](int idx) const
115  { return vec[idx]; }
116  T &operator[](int idx)
117  { return vec[idx]; }
118 
119  /// Does a comparison with a tolerance. This also returns true if
120  /// quat.negated() is equal to us, unlike operator==().
121  bool isEqual(const UT_QuaternionT<T> &quat,
122  T tol = T(SYS_FTOLERANCE)) const;
123 
124  // The rotation this quaternion represents as a matrix
125  void getRotationMatrix(UT_Matrix3 &mat) const;
126  void getRotationMatrix(UT_DMatrix3 &mat) const;
127  void getInverseRotationMatrix(UT_Matrix3 &mat) const;
128  void getInverseRotationMatrix(UT_DMatrix3 &mat) const;
129 
130  void getTransformMatrix(UT_Matrix4 &mat) const;
131  void getTransformMatrix(UT_DMatrix4 &mat) const;
132 
133  /// Interpolates between this quat (t==0) and the target (t==1)
134  UT_QuaternionT<T> interpolate(const UT_QuaternionT<T> &target,
135  T t, T b = 0.0f) const;
136  /// Interpolates between the n quaternions in q, with weights w,
137  /// to within tolerance tol.
138  /// NOTE: The q's must be normalized, and the weights may need to sum to 1.
139  void interpolate(const UT_QuaternionT<T> *q, const T *w, exint n, T tol = T(1e-6));
140 
141  /// Do component-wise lerp between this quat (t=0) and the target (t=1).
142  void lerp(const UT_QuaternionT<T> &target, T t)
143  {
144  vec[0] = SYSlerp(vec[0], target.vec[0], t);
145  vec[1] = SYSlerp(vec[1], target.vec[1], t);
146  vec[2] = SYSlerp(vec[2], target.vec[2], t);
147  vec[3] = SYSlerp(vec[3], target.vec[3], t);
148  }
149  /// Do component-wise lerp between this src (t=0) and dst (t=1).
151  const UT_QuaternionT<T> &dst,
152  T t)
153  {
154  vec[0] = SYSlerp(src.vec[0], dst.vec[0], t);
155  vec[1] = SYSlerp(src.vec[1], dst.vec[1], t);
156  vec[2] = SYSlerp(src.vec[2], dst.vec[2], t);
157  vec[3] = SYSlerp(src.vec[3], dst.vec[3], t);
158  }
159 
160  void assign(T qx, T qy,
161  T qz, T qw)
162  {
163  vec[0] = qx; vec[1] = qy;
164  vec[2] = qz; vec[3] = qw;
165  }
166  void identity()
167  {
168  vec[0] = vec[1] = vec[2] = 0.0f;
169  vec[3] = 1.0f;
170  }
171  void conjugate()
172  {
173  vec[0] = -vec[0];
174  vec[1] = -vec[1];
175  vec[2] = -vec[2];
176  }
177  void negate()
178  {
179  vec[0] = -vec[0];
180  vec[1] = -vec[1];
181  vec[2] = -vec[2];
182  vec[3] = -vec[3];
183  }
184  T normal() const
185  {
186  return vec[0] * vec[0] +
187  vec[1] * vec[1] +
188  vec[2] * vec[2] +
189  vec[3] * vec[3];
190  }
191  void normalize()
192  {
193  T dn = normal();
194  if (dn > std::numeric_limits<T>::min()
195  && dn != 1.0)
196  {
197  dn = SYSsqrt(dn);
198  *this /= dn;
199  }
200  }
201  T length() const
202  {
203  return SYSsqrt(normal());
204  }
205  void invert()
206  {
207  T n = normal();
208  if (n > std::numeric_limits<T>::min())
209  {
210  n = 1.0 / n;
211  conjugate();
212  vec[0] *= n;
213  vec[1] *= n;
214  vec[2] *= n;
215  vec[3] *= n;
216  }
217  }
218 
219  UT_QuaternionT<T> exp() const;
220  UT_QuaternionT<T> ln() const;
221  inline UT_QuaternionT<T> log() const
222  {
223  return ln();
224  }
225 
226  // Form the quaternion which takes v1 and rotates it to v2.
227  // v1 and v2 are assumed normalized
228  void updateFromVectors(const UT_Vector3T<T> &v1,
229  const UT_Vector3T<T> &v2);
230 
231  /// Form the quaternion from the rotation component of an
232  /// arbitrary 3x3 matrix.
233  void updateFromArbitraryMatrix(const UT_Matrix3 &);
234  void updateFromArbitraryMatrix(const UT_Matrix3D &);
235 
236  /// Form the quaternion from a rotation matrix
237  /// WARNING: This will produce incorrect results if given
238  /// a non-rotation matrix! Use updateFromArbitraryMatrix
239  /// if you may have a non-rotation matrix.
240  void updateFromRotationMatrix(const UT_Matrix3 &);
241  void updateFromRotationMatrix(const UT_Matrix3D &);
242 
243  // Form the quaternion from an angle/axis
244  void updateFromAngleAxis(T angle,
245  const UT_Vector3T<T> &axis,
246  int normalize=1);
247 
248  void getAngleAxis(T &angle,
249  UT_Vector3T<T> &axis) const;
250 
251  void updateFromLogMap(const UT_Vector3T<T> &v);
252  void getLogMap(UT_Vector3T<T> &v) const;
253 
254  // Form the quaternion from euler rotation angles (given in radians)
255  void updateFromEuler(const UT_Vector3T<T> &rot,
256  const UT_XformOrder &order);
257 
258  // Given the angular velocity omega, compute our derivative into q_prime
259  void computeDerivative(const UT_Vector3T<T> &omega,
260  UT_QuaternionT<T> &q_prime);
261 
262  // Returns the angular velocity required to move from the rotation of
263  // this quaternion to the destination quaternion in a given time.
264  UT_Vector3T<T> computeAngVel(const UT_QuaternionT<T> &dest,
265  T time) const;
266 
267  // Integrates this quaternion by the given angular velocity and time
268  // step. There are two appraoches to doing this. For small
269  // angular velocity/timesteps, one can compute the derivative
270  // implied by the angular velocity, apply linearly, and renormalize.
271  // Alternatively, one can construct the proper quaternion for the given
272  // angular velocity and rotate by that. Which method is controlled
273  // by the accurate flag.
274  void integrate(const UT_Vector3T<T> &angvel,
275  T timestep,
276  bool accurate = true);
277 
278  // Returns the rx/ry/rz euler rotation representation of the quaternion.
279  // The returned rotations are in radians.
280  UT_Vector3T<T> computeRotations(const UT_XformOrder &) const;
281 
282  /// Rotates a vector by this quaternion
283  /// Requires that this is normalized.
284  UT_Vector3T<T> rotate(const UT_Vector3T<T> &) const;
285 
286  /// rotates a vector by the inverse of this quaternion.
287  /// Requires that this is normalized.
288  UT_Vector3T<T> rotateInverse(const UT_Vector3T<T> &) const;
289 
290  // Multiply this quarternion's "real world" Euler angles by the given
291  // scalar s. That is, if this quaternion is
292  // [ cos(a), n1 sin(a), n2 sin(a), n3 sin(a) ] it is replaced by
293  // [ cos(a*s), n1 sin(a*s), n2 sin(a*s), n3 sin(a*s) ]
294  void multAngle( T s );
295 
296  T &x() { return vec[0]; }
297  T &y() { return vec[1]; }
298  T &z() { return vec[2]; }
299  T &w() { return vec[3]; }
300 
301  T x() const { return vec[0]; }
302  T y() const { return vec[1]; }
303  T z() const { return vec[2]; }
304  T w() const { return vec[3]; }
305 
306  void save(std::ostream &os, int binary=0) const;
307  bool load(UT_IStream &is);
308 
309  /// @{
310  /// Methods to serialize to a JSON stream. The vector is stored as an
311  /// array of 4 reals.
312  bool save(UT_JSONWriter &w) const;
313  bool save(UT_JSONValue &v) const;
314  bool load(UT_JSONParser &p);
315  /// @}
316 
317  const T *data(void) const { return &vec[0]; }
318  T *data(void) { return &vec[0]; }
319 
320  static int entries() { return tuple_size; }
321 
322  /// Compute a hash
323  unsigned hash() const { return SYSvector_hash(data(), tuple_size); }
324 
325 protected:
326  void initialize(T qx = 0, T qy = 0,
327  T qz = 0, T qw = 0)
328  {
329  vec[0] = qx; vec[1] = qy;
330  vec[2] = qz; vec[3] = qw;
331  }
332 private:
333  // I/O friends:
334  friend std::ostream &operator<<(std::ostream &os, const UT_QuaternionT<T> &v)
335  {
336  v.save(os);
337  return os;
338  }
339  T vec[tuple_size];
340 };
341 
342 template <typename T>
343 UT_API size_t format(char *buf, size_t buf_size, const UT_QuaternionT<T> &q);
344 
345 template <typename T>
346 inline UT_QuaternionT<T>
348 {
349  UT_QuaternionT<T> product = q1;
350 
351  product *= q2;
352 
353  return UT_QuaternionT<T>(product);
354 }
355 
356 template <typename T>
357 inline UT_QuaternionT<T>
359 {
360  return UT_QuaternionT<T>(q1.x() + q2.x(),
361  q1.y() + q2.y(),
362  q1.z() + q2.z(),
363  q1.w() + q2.w());
364 }
365 
366 template <typename T>
367 inline UT_QuaternionT<T> &
369 {
370  vec[0] += quat.vec[0];
371  vec[1] += quat.vec[1];
372  vec[2] += quat.vec[2];
373  vec[3] += quat.vec[3];
374 
375  return *this;
376 }
377 
378 template <typename T>
379 inline UT_QuaternionT<T>
381 {
382  return UT_QuaternionT<T>(q1.x() - q2.x(),
383  q1.y() - q2.y(),
384  q1.z() - q2.z(),
385  q1.w() - q2.w());
386 }
387 
388 template <typename T>
389 inline UT_QuaternionT<T>
391 {
392  return UT_QuaternionT<T>(-q.x(),
393  -q.y(),
394  -q.z(),
395  -q.w());
396 }
397 
398 template <typename T>
399 inline UT_QuaternionT<T>
400 operator*(const UT_QuaternionT<T> &q, T scalar)
401 {
402  return UT_QuaternionT<T>(q.x() * scalar,
403  q.y() * scalar,
404  q.z() * scalar,
405  q.w() * scalar);
406 }
407 
408 template <typename T>
409 inline UT_QuaternionT<T>
410 operator*(T scalar, const UT_QuaternionT<T> &q)
411 {
412  return UT_QuaternionT<T>(q.x() * scalar,
413  q.y() * scalar,
414  q.z() * scalar,
415  q.w() * scalar);
416 }
417 
418 template <typename T>
419 inline UT_QuaternionT<T> &
421 {
422  UT_Vector3T<T> v1(vec[0], vec[1], vec[2]);
423  UT_Vector3T<T> v2(q.vec[0], q.vec[1], q.vec[2]);
425  T s1 = vec[3], s2 = q.vec[3];
426 
427  vec[3] = s1*s2 - v1.dot(v2);
428  v3 = s1*v2 + s2*v1 + cross(v1, v2);
429  vec[0] = v3[0];
430  vec[1] = v3[1];
431  vec[2] = v3[2];
432 
433  return *this;
434 }
435 
436 template <typename T>
437 inline UT_QuaternionT<T> &
439 {
440  vec[0] *= scalar;
441  vec[1] *= scalar;
442  vec[2] *= scalar;
443  vec[3] *= scalar;
444 
445  return *this;
446 }
447 
448 template <typename T>
449 inline UT_QuaternionT<T>
451 {
452  UT_QuaternionT<T> a1 = a;
453  UT_QuaternionT<T> b1 = b;
454 
455  b1.invert();
456  a1 *= b1;
457 
458  return UT_QuaternionT<T>(a1);
459 }
460 
461 template <typename T>
462 inline UT_QuaternionT<T>
463 operator/(const UT_QuaternionT<T> &q, T scalar)
464 {
465  T d = 1.0/scalar;
466  return UT_QuaternionT<T>(q.x()*d, q.y()*d, q.z()*d, q.w()*d);
467 }
468 
469 template <typename T>
470 inline UT_QuaternionT<T> &
472 {
473  UT_QuaternionT<T> q = quat;
474 
475  q.invert();
476  operator*=(q);
477 
478  return *this;
479 }
480 
481 template <typename T>
482 inline UT_QuaternionT<T> &
484 {
485  T d = 1.0F/scalar;
486 
487  vec[0] *= d;
488  vec[1] *= d;
489  vec[2] *= d;
490  vec[3] *= d;
491 
492  return *this;
493 }
494 
495 template <typename T>
496 inline T
497 dot( const UT_QuaternionT<T> &q1, const UT_QuaternionT<T> &q2 )
498 {
499  return q1.x()*q2.x() + q1.y()*q2.y() + q1.z()*q2.z() + q1.w()*q2.w();
500 }
501 
502 template <typename T>
503 inline size_t
505 {
506  return val.hash();
507 }
508 
513 
514 template<typename T>
516 {
518  typedef T DataType;
519  static const exint TupleSize = 4;
520  static const bool isVectorType = true;
521 };
522 
523 ///////////////////////////////////////////////////////////////////////////////
524 //
525 // Implementations
526 //
527 
528 template <typename T>
529 inline UT_QuaternionT<T>
530 SYSlerp(const UT_QuaternionT<T> &q1, const UT_QuaternionT<T> &q2, T t)
531 {
532  return UT_QuaternionT<T>( SYSlerp(q1.x(), q2.x(), t)
533  , SYSlerp(q1.y(), q2.y(), t)
534  , SYSlerp(q1.z(), q2.z(), t)
535  , SYSlerp(q1.w(), q2.w(), t)
536  );
537 }
538 
539 #endif
T & operator[](int idx)
Mat3< typename promote< S, T >::type > operator*(S scalar, const Mat3< T > &m)
Returns M, where for .
Definition: Mat3.h:615
T length() const
UT_QuaternionT< T > & operator/=(const UT_QuaternionT< T > &quat)
UT_QuaternionT(const UT_Vector3T< T > &rot, const UT_XformOrder &order)
Definition: UT_Quaternion.h:89
UT_QuaternionT< T > & operator=(const UT_QuaternionT< S > &v)
png_voidp s1
Definition: png.h:2193
T z() const
const hboost::disable_if_c< VecTraits< T >::IsVec, T >::type & min(const T &a, const T &b)
Definition: Composite.h:128
UT_QuaternionT< fpreal > UT_QuaternionR
const GLdouble * v
Definition: glcorearb.h:836
Mat3< typename promote< T0, T1 >::type > operator+(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Returns M, where for .
Definition: Mat3.h:631
const GLuint GLenum const void * binary
Definition: glcorearb.h:1923
GA_API const UT_StringHolder rot
T & operator()(int idx)
void assign(T qx, T qy, T qz, T qw)
GLboolean GLboolean GLboolean GLboolean a
Definition: glcorearb.h:1221
JSON reader class which handles parsing of JSON or bJSON files.
Definition: UT_JSONParser.h:72
#define UT_API
Definition: UT_API.h:12
Class which writes ASCII or binary JSON streams.
Definition: UT_JSONWriter.h:32
static const exint TupleSize
GLfloat GLfloat GLfloat v2
Definition: glcorearb.h:817
3D Vector class.
GLfloat GLfloat GLfloat GLfloat v3
Definition: glcorearb.h:818
4D Vector class.
Definition: UT_Vector4.h:152
static int entries()
size_t hash_value(const UT_QuaternionT< T > &val)
UT_QuaternionT< T > operator/(const UT_QuaternionT< T > &a, const UT_QuaternionT< T > &b)
SYS_FORCE_INLINE UT_StorageNum< typename UT_StorageBetter< T, S >::Type >::AtLeast32Bit dot(const UT_FixedVector< S, SIZE, S_INSTANTIATED > &that) const
bool operator==(const BaseDimensions< T > &a, const BaseDimensions< Y > &b)
Definition: Dimensions.h:137
UT_API size_t format(char *buf, size_t buf_size, const UT_QuaternionT< T > &q)
GLdouble n
Definition: glcorearb.h:2007
GLfloat f
Definition: glcorearb.h:1925
UT_QuaternionT< fpreal32 > UT_QuaternionF
Vec3< T > & operator*=(Vec3< T > &_v, const Mat3< MT > &_m)
Returns v, where for .
Definition: Mat3.h:690
Mat3< typename promote< T0, T1 >::type > operator-(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Returns M, where for .
Definition: Mat3.h:641
T x() const
int64 exint
Definition: SYS_Types.h:109
UT_QuaternionT< fpreal64 > UT_QuaternionD
double fpreal64
Definition: SYS_Types.h:185
UT_QuaternionT(T qx=0, T qy=0, T qz=0, T qw=0)
Definition: UT_Quaternion.h:51
static const bool isVectorType
const T * data(void) const
#define SYS_FORCE_INLINE
Definition: SYS_Inline.h:45
SYS_API double exp(double x)
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:480
GLenum target
Definition: glcorearb.h:1666
UT_QuaternionT< T > & operator+=(const UT_QuaternionT< T > &quat)
T operator()(int idx) const
GLboolean * data
Definition: glcorearb.h:130
GridType::Ptr normalize(const GridType &grid, bool threaded, InterruptT *interrupt)
Normalize the vectors of the given vector-valued grid.
const Vec2< S > & operator*=(Vec2< S > &v, const Matrix33< T > &m)
Definition: ImathMatrix.h:3330
GLboolean GLboolean GLboolean b
Definition: glcorearb.h:1221
UT_QuaternionT(const UT_Vector4T< T > &v)
Definition: UT_Quaternion.h:67
void lerp(const UT_QuaternionT< T > &src, const UT_QuaternionT< T > &dst, T t)
Do component-wise lerp between this src (t=0) and dst (t=1).
T dot(const UT_QuaternionT< T > &q1, const UT_QuaternionT< T > &q2)
UT_QuaternionT< T > SYSlerp(const UT_QuaternionT< T > &q1, const UT_QuaternionT< T > &q2, T t)
Perform component-wise SYSlerp of two quaternions.
UT_QuaternionT< T > & operator*=(const UT_QuaternionT< T > &q)
GLenum GLuint GLenum GLsizei const GLchar * buf
Definition: glcorearb.h:2539
class UT_API UT_QuaternionT
Definition: UT_Quaternion.h:35
UT_QuaternionT< T > log() const
GLenum GLenum dst
Definition: glcorearb.h:1792
SYS_FORCE_INLINE UT_QuaternionT(const UT_FixedVector< S, tuple_size > &v)
Arbitrary UT_FixedVector of the same size.
Definition: UT_Quaternion.h:74
Quaternion class.
Definition: UT_Quaternion.h:44
png_voidp png_voidp s2
Definition: png.h:2193
UT_QuaternionT(const fpreal64 v[tuple_size])
Definition: UT_Quaternion.h:62
T y() const
UT_QuaternionT< fpreal32 > UT_Quaternion
T w() const
T * data(void)
GLfloat GLfloat v1
Definition: glcorearb.h:816
GLuint GLfloat * val
Definition: glcorearb.h:1607
T operator[](int idx) const
Class to store JSON objects as C++ objects.
Definition: UT_JSONValue.h:75
UT_QuaternionT(const fpreal32 v[tuple_size])
Definition: UT_Quaternion.h:57
void lerp(const UT_QuaternionT< T > &target, T t)
Do component-wise lerp between this quat (t=0) and the target (t=1).
#define SYS_FTOLERANCE
Definition: SYS_Types.h:196
GLubyte GLubyte GLubyte GLubyte w
Definition: glcorearb.h:856
bool operator!=(const BaseDimensions< T > &a, const BaseDimensions< Y > &b)
Definition: Dimensions.h:165
T normal() const
void initialize(T qx=0, T qy=0, T qz=0, T qw=0)
unsigned hash() const
Compute a hash.
float fpreal32
Definition: SYS_Types.h:184
SIM_DerVector3 cross(const SIM_DerVector3 &lhs, const SIM_DerVector3 &rhs)
GLenum src
Definition: glcorearb.h:1792
UT_QuaternionT(const UT_QuaternionT< T > &q)
Definition: UT_Quaternion.h:82