HDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
UT_DualQuaternion.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_DualQuaternion library (C++)
7  *
8  * COMMENTS: This class implements dual quaternions.
9  *
10  *
11  */
12 
13 
14 #ifndef __UT_DualQuaternion_h__
15 #define __UT_DualQuaternion_h__
16 
17 #include "UT_API.h"
18 #include "UT_Assert.h"
19 #include "UT_Matrix3.h"
20 #include "UT_Matrix4.h"
21 #include "UT_Quaternion.h"
22 #include "UT_Vector3.h"
23 #include <SYS/SYS_Inline.h>
24 #include <SYS/SYS_Math.h>
25 #include <SYS/SYS_TypeDecorate.h>
26 #include <SYS/SYS_Types.h>
27 #include <iosfwd>
28 #include <limits>
29 
30 
31 ///
32 /// Dual quaternion class
33 ///
34 /// @note This class only supports the subspace of dual quaternions for use
35 /// as rigid transforms. As such, its real and dual components must be
36 /// orthogonal.
37 ///
38 template <typename T>
40 {
41 public:
42  /// Trivial constructor. If called, it initializes to all zeroes.
44  UT_DualQuaternionT() = default;
45 
46  /// Construct from orthogonal real and dual parts
47  inline
49  const UT_QuaternionT<T> &dual);
50 
51  /// Construct from components of orthogonal real and dual parts
52  inline
53  UT_DualQuaternionT(T real_qx, T real_qy, T real_qz, T real_qw,
54  T dual_qx, T dual_qy, T dual_qz, T dual_qw);
55 
56  /// Construct from rotation unit quaternion and translation vector
57  inline
59 
60  /// Construct from a transform matrix
61  inline
62  UT_DualQuaternionT(const UT_Matrix4T<T> &xform);
63 
64  /// Default copy constructor
66  UT_DualQuaternionT(const UT_DualQuaternionT &dq) = default;
67 
68  /// Default assignment
71 
72  /// Assign from UT_Matrix4
73  inline void updateFromXform(const UT_Matrix4T<T> &xform);
74 
75  /// Assign from UT_Matrix3
76  inline void updateFromXform(const UT_Matrix3T<T> &xform);
77 
78  inline void updateFromRotTransPair(const UT_QuaternionT<T> &r,
79  const UT_Vector3T<T> &t);
80  inline void updateFromRotTransPair(const UT_QuaternionT<T> &r,
81  T tx, T ty, T tz);
82 
83  /////////////////////////////////
84  /// Methods
85  /////////////////////////////////
86 
87  /// Length squared
88  inline T length2() const;
89 
90  /// Length
91  inline T length() const;
92 
93  /// Normalizes the dual quaternion to have length of 1
94  inline void normalize();
95 
96  /// Changes dual quaternion into its conjugate
97  inline void conjugate();
98 
99  /// Invert this dual quaternion. Does not exist if getReal() is zero.
100  inline void invert();
101 
102  /// Invert this dual quaternion, assuming that its already normalized.
103  /// Does not exist if getReal() is zero.
104  inline void invertNormalized();
105 
106  /// Change dual quaternion into an 0 rotation and 0 translation components
107  inline void identity();
108 
109  /// Returns a vector that is transformed by the dual quaternion
110  /// Requires this is normalized
111  inline void transform(const UT_Vector3T<T> &vec, UT_Vector3T<T> &dst) const;
112 
113  /// Returns a vector that has only the rotation component applied
114  /// Requires this is normalized
115  inline void rotate(const UT_Vector3T<T> &vec, UT_Vector3T<T> &dst) const;
116 
117  /// Returns a xform that is represented by the dual quaternion
118  /// Requires this is normalized
119  inline UT_Matrix4T<T> convertToXform() const;
120 
121 
122  //////////////////////////////////////
123  /// Operators
124  //////////////////////////////////////
125 
126  inline UT_DualQuaternionT<T> &operator*=(T scalar);
130  inline bool operator==(const UT_DualQuaternionT<T> &dq) const;
131  inline bool operator!=(const UT_DualQuaternionT<T> &dq) const;
132  // todo: hash
133 
134  /// Like operator==() but using a tolerance
135  inline bool isEqual(const UT_DualQuaternionT<T> &quat,
136  T tol = T(SYS_FTOLERANCE)) const;
137 
138  friend std::ostream &
139  operator<<(std::ostream &os, const UT_DualQuaternionT<T> &dq)
140  {
141  dq.outTo(os);
142  return os;
143  }
144 
145  //////////////////////////////////
146  /// Getters
147  //////////////////////////////////
148 
149  const T * realData() const { return myReal.data(); }
150  T * realData() { return myReal.data(); }
151 
152  const T * dualData() const { return myDual.data(); }
153  T * dualData() { return myDual.data(); }
154 
155  const UT_QuaternionT<T> &getReal() const { return myReal; }
156  const UT_QuaternionT<T> &getDual() const { return myDual; }
157  const UT_QuaternionT<T> &getTranslation() const { return myDual; }
158  const UT_QuaternionT<T> &getRotation() const { return myReal; }
159 
160 
161 protected:
162  UT_API void outTo(std::ostream &os) const;
163 
164 private:
165 
166  /// Real or non-dual part of the dual quaternion.
167  /// It represents the rotation component of an xform
168  UT_QuaternionT<T> myReal;
169 
170  /// Dual part of the dual quaternion.
171  /// It represents the translation components
172  UT_QuaternionT<T> myDual;
173 
174 };
175 
176 
177 ///////////////////////////////////////
178 /// Typedefs
179 ///////////////////////////////////////
180 
185 
188 
189 ///////////////////////////////////////
190 /// Construct implementations
191 ///////////////////////////////////////
192 
193 template <typename T>
194 inline
196  const UT_QuaternionT<T> &dual)
197  : myReal(real)
198  , myDual(dual)
199 {
200 }
201 
202 template <typename T>
203 inline
205  T real_qx, T real_qy, T real_qz, T real_qw,
206  T dual_qx, T dual_qy, T dual_qz, T dual_qw)
207  : myReal(real_qx, real_qy, real_qz, real_qw)
208  , myDual(dual_qx, dual_qy, dual_qz, dual_qw)
209 {
210 }
211 
212 template <typename T>
213 inline
215  const UT_QuaternionT<T> &r,
216  const UT_Vector3T<T> &t)
217 {
218  updateFromRotTransPair(r, t);
219 }
220 
221 template <typename T>
222 inline
224 {
225  updateFromXform(xform);
226 }
227 
228 template <typename T>
229 inline void
231 {
232  UT_Matrix3T<T> rotXform;
233  xform.extractRotate(rotXform);
234  myReal.updateFromRotationMatrix(rotXform);
235 
236  UT_Vector3T<T> translate;
237  xform.getTranslates(translate);
238 
239  updateFromRotTransPair(myReal, translate);
240 }
241 
242 template <typename T>
243 inline void
245 {
246  myReal.updateFromRotationMatrix(xform);
247  updateFromRotTransPair(myReal, 0.0f, 0.0f, 0.0f);
248 }
249 
250 template <typename T>
251 inline void
253  T tx, T ty, T tz)
254 {
255  T w = -0.5f * ( tx*r.x() + ty*r.y() + tz*r.z() );
256  T x = 0.5f * ( r.w()*tx + ty*r.z() - r.y()*tz );
257  T y = 0.5f * ( r.w()*ty + tz*r.x() - r.z()*tx );
258  T z = 0.5f * ( r.w()*tz + tx*r.y() - r.x()*ty );
259 
260  myReal = r;
261  myDual.assign(x, y, z, w);
262  UT_ASSERT_P(SYSequalZero(dot(myReal, myDual)));
263  UT_ASSERT_P(SYSisEqual(myReal.normal(), T(1.0)));
264 }
265 
266 
267 ///////////////////////////////////////
268 /// Method implementations
269 ///////////////////////////////////////
270 
271 template <typename T>
272 inline void
274  const UT_Vector3T<T> &vec, UT_Vector3T<T> &dst) const
275 {
276  // The actual equation is p' = qpq* where p' and p are quaternions that
277  // represent the points while q and q* are conjugate quaternions of each
278  // other q = qt x qr, which is the operation rotation followed by
279  // translation
280 
281  UT_Vector3T<T> real_vec( myReal.x(), myReal.y(), myReal.z() );
282  UT_Vector3T<T> dual_vec( myDual.x(), myDual.y(), myDual.z() );
283 
284  UT_Vector3T<T> cross_vec = cross(real_vec, dual_vec);
286  t = (dual_vec*myReal.w() - real_vec*myDual.w() + cross_vec) * T(2.0);
287 
288  dst = myReal.rotate(vec) + t;
289 }
290 
291 template <typename T>
292 inline void
294  const UT_Vector3T<T> &vec, UT_Vector3T<T> &dst) const
295 {
296  UT_ASSERT(SYSisEqual(myReal.normal(), T(1.0)));
297  UT_Quaternion rot_quat = myReal;
298  dst = rot_quat.rotate(vec);
299 }
300 
301 template <typename T>
302 inline UT_Matrix4T<T>
304 {
305  UT_Matrix3T<T> rotMat;
306  myReal.getRotationMatrix(rotMat);
307 
308  UT_Matrix4T<T> xform;
309  xform = rotMat;
310 
311  UT_QuaternionT<T> real_conjugate = myReal;
312  real_conjugate.conjugate();
313  UT_QuaternionT<T> t_quat = myDual * real_conjugate;
314  t_quat *= T(2.0);
315 
316  xform.setTranslates(UT_Vector3T<T>(t_quat.x(), t_quat.y(), t_quat.z()));
317 
318  return xform;
319 }
320 
321 template <typename T>
322 inline void
324  const UT_Vector3T<T> &t)
325 {
326  updateFromRotTransPair(r, t.x(), t.y(), t.z());
327 }
328 
329 template <typename T>
330 inline T
332 {
333  UT_ASSERT_P(SYSequalZero(dot(myReal, myDual)));
334  return myReal.normal();
335 }
336 
337 template <typename T>
338 inline T
340 {
341  return SYSsqrt(length2());
342 }
343 
344 template <typename T>
345 inline void
347 {
348  // This assertion doesn't seem to hold true in general when we're doing a
349  // weighted average of dual quaternions. This kind of brings up a problem
350  // as to whether this is correct thing at all. But the operations here
351  // follow what is done in "Skinning with Dual Quaternions" by Ladislav et
352  // al. They give a rationale why it's ok to do the normalize() this way in
353  // the paper nut I'm not sure I buy it.
354  //UT_ASSERT_P(SYSequalZero(dot(myReal, myDual)));
355 
356  T norm = myReal.normal();
357  if(norm > std::numeric_limits<T>::min() &&
358  norm != 1.0)
359  {
360  norm = SYSsqrt(norm);
361  myReal /= norm;
362  myDual /= norm;
363  }
364 }
365 
366 template <typename T>
367 inline void
369 {
370  myReal.conjugate();
371  myDual.conjugate();
372 }
373 
374 template <typename T>
375 inline void
377 {
378  UT_ASSERT_P(!SYSequalZero(myReal.normal()));
379  UT_ASSERT_P(SYSequalZero(dot(myReal, myDual)));
380  myReal.invert();
381  myDual = (myReal * myDual * myReal) * T(-1.0);
382 }
383 
384 template <typename T>
385 inline void
387 {
388  UT_ASSERT_P(SYSisEqual(myReal.normal(), 1.0));
389  UT_ASSERT_P(SYSequalZero(dot(myReal, myDual)));
390  myReal.conjugate();
391  myDual = (myReal * myDual * myReal) * T(-1.0);
392 }
393 
394 template <typename T>
395 inline void
397 {
398  myReal.identity();
399  myDual.assign(0, 0, 0, 0);
400 }
401 
402 
403 ///////////////////////////////////////
404 /// Operator implementations
405 ///////////////////////////////////////
406 
407 template <typename T>
408 inline UT_DualQuaternionT<T> &
410 {
411  myDual += dq.getDual();
412  myReal += dq.getReal();
413  return (*this);
414 }
415 
416 template <typename T>
417 inline UT_DualQuaternionT<T> &
419 {
420  myDual *= scalar;
421  myReal *= scalar;
422  return (*this);
423 }
424 
425 template <typename T>
426 inline UT_DualQuaternionT<T> &
428 {
429  myDual = myReal * dq.getDual() + myDual * dq.getReal();
430  myReal *= dq.getReal();
431  return (*this);
432 }
433 
434 template <typename T>
435 inline UT_DualQuaternionT<T> &
437 {
438  myDual = myDual - dq.getDual();
439  myReal = myReal - dq.getReal();
440  return (*this);
441 }
442 
443 template <typename T>
444 inline bool
446 {
447  return ( myDual == dq.getDual() ) &&
448  ( myReal == dq.getReal() );
449 }
450 
451 template <typename T>
452 inline bool
454 {
455  return SYSisEqual(myDual(0), dq.getDual()(0), tol)
456  && SYSisEqual(myDual(1), dq.getDual()(1), tol)
457  && SYSisEqual(myDual(2), dq.getDual()(2), tol)
458  && SYSisEqual(myDual(3), dq.getDual()(3), tol)
459  && SYSisEqual(myReal(0), dq.getReal()(0), tol)
460  && SYSisEqual(myReal(1), dq.getReal()(1), tol)
461  && SYSisEqual(myReal(2), dq.getReal()(2), tol)
462  && SYSisEqual(myReal(3), dq.getReal()(3), tol);
463 }
464 
465 template <typename T>
466 inline bool
468 {
469  return !(*this == dq);
470 }
471 
472 ///////////////////////////////////////
473 /// External operators
474 ///////////////////////////////////////
475 
476 template <typename T>
477 inline UT_DualQuaternionT<T>
479 {
480  return UT_DualQuaternionT<T>(dq1.getReal()-dq2.getReal(),
481  dq1.getDual()-dq2.getDual());
482 }
483 
484 template <typename T>
487 {
488  return UT_DualQuaternionT<T>(dq1.getReal()+dq2.getReal(),
489  dq1.getDual()+dq2.getDual());
490 }
491 
492 template <typename T>
494 operator*(const UT_DualQuaternionT<T> &dq1, const T scalar)
495 {
496  return UT_DualQuaternionT<T>(dq1.getReal()*scalar,
497  dq1.getDual()*scalar);
498 }
499 
500 template <typename T>
503 {
504  return UT_DualQuaternionT<T>(
505  dq1.getReal() * dq2.getReal(),
506  dq1.getReal() * dq2.getDual() + dq1.getDual() * dq2.getReal());
507 }
508 
509 template<> UT_API
510 void UT_DualQuaternionT<fpreal32>::outTo(std::ostream &os) const;
511 template<> UT_API
512 void UT_DualQuaternionT<fpreal64>::outTo(std::ostream &os) const;
513 
514 #endif // __UT_DualQuaternion_h__
UT_DualQuaternionT< fpreal32 > UT_DualQuaternion
Typedefs.
void extractRotate(UT_Matrix3T< S > &dst) const
const UT_QuaternionT< T > & getDual() const
Mat3< typename promote< S, T >::type > operator*(S scalar, const Mat3< T > &m)
Multiply each element of the given matrix by scalar and return the result.
Definition: Mat3.h:609
T length2() const
Methods.
UT_DualQuaternionT< T > & operator+=(const UT_DualQuaternionT< T > &dq)
Operator implementations.
SYS_FORCE_INLINE UT_DualQuaternionT< T > & operator=(const UT_DualQuaternionT &v)=default
Default assignment.
T length() const
Length.
const GLdouble * v
Definition: glcorearb.h:836
Mat3< typename promote< T0, T1 >::type > operator+(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Add corresponding elements of m0 and m1 and return the result.
Definition: Mat3.h:625
void updateFromRotTransPair(const UT_QuaternionT< T > &r, const UT_Vector3T< T > &t)
GLdouble GLdouble GLdouble z
Definition: glcorearb.h:847
const UT_QuaternionT< T > & getTranslation() const
#define UT_API
Definition: UT_API.h:13
GLint y
Definition: glcorearb.h:102
void rotate(const UT_Vector3T< T > &vec, UT_Vector3T< T > &dst) const
const T * realData() const
Getters.
SYS_FORCE_INLINE T & x(void)
Definition: UT_Vector3.h:498
3D Vector class.
UT_DualQuaternionT< T > & operator*=(T scalar)
Operators.
SYS_FORCE_INLINE T & z(void)
Definition: UT_Vector3.h:502
UT_DualQuaternionT< fpreal64 > UT_DualQuaternionD
UT_DualQuaternionT< fpreal > UT_DualQuaternionR
GLfloat f
Definition: glcorearb.h:1925
SYS_FORCE_INLINE UT_DualQuaternionT()=default
Trivial constructor. If called, it initializes to all zeroes.
bool operator==(const UT_DualQuaternionT< T > &dq) const
bool isEqual(const UT_DualQuaternionT< T > &quat, T tol=T(SYS_FTOLERANCE)) const
Like operator==() but using a tolerance.
Mat3< typename promote< T0, T1 >::type > operator-(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Subtract corresponding elements of m0 and m1 and return the result.
Definition: Mat3.h:635
void identity()
Change dual quaternion into an 0 rotation and 0 translation components.
UT_API void outTo(std::ostream &os) const
#define UT_ASSERT_P(ZZ)
Definition: UT_Assert.h:125
const UT_QuaternionT< T > & getRotation() const
void invert()
Invert this dual quaternion. Does not exist if getReal() is zero.
fpreal64 dot(const CE_VectorT< T > &a, const CE_VectorT< T > &b)
Definition: CE_Vector.h:218
#define SYS_FORCE_INLINE
Definition: SYS_Inline.h:45
void updateFromXform(const UT_Matrix4T< T > &xform)
Assign from UT_Matrix4.
void normalize()
Normalizes the dual quaternion to have length of 1.
void conjugate()
Changes dual quaternion into its conjugate.
UT_DualQuaternionT< fpreal32 > UT_DualQuaternionF
void setTranslates(const UT_Vector3T< S > &translates)
Definition: UT_Matrix4.h:1306
void transform(const UT_Vector3T< T > &vec, UT_Vector3T< T > &dst) const
Method implementations.
bool operator!=(const UT_DualQuaternionT< T > &dq) const
GLenum GLenum dst
Definition: glcorearb.h:1792
SYS_FORCE_INLINE T & y(void)
Definition: UT_Vector3.h:500
Quaternion class.
Definition: UT_Quaternion.h:51
const UT_QuaternionT< T > & getReal() const
SYS_DECLARE_IS_POD(UT_DualQuaternionF)
UT_Matrix4T< T > convertToXform() const
GLint GLenum GLint x
Definition: glcorearb.h:408
#define SYS_FTOLERANCE
Definition: SYS_Types.h:203
GLubyte GLubyte GLubyte GLubyte w
Definition: glcorearb.h:856
#define UT_ASSERT(ZZ)
Definition: UT_Assert.h:126
UT_DualQuaternionT< T > & operator-=(const UT_DualQuaternionT< T > &dq)
const T * dualData() const
GLboolean r
Definition: glcorearb.h:1221
UT_Vector3T< T > rotate(const UT_Vector3T< T > &) const
const std::enable_if<!VecTraits< T >::IsVec, T >::type & min(const T &a, const T &b)
Definition: Composite.h:129
SIM_DerVector3 cross(const SIM_DerVector3 &lhs, const SIM_DerVector3 &rhs)
void getTranslates(UT_Vector3T< S > &translates) const
Definition: UT_Matrix4.h:1296