Visualization Library 2.0.0

A lightweight C++ OpenGL middleware for 2D/3D graphics

VL     Star     Watch     Fork     Issue

[Download] [Tutorials] [All Classes] [Grouped Classes]
Quaternion.hpp
Go to the documentation of this file.
1 /**************************************************************************************/
2 /* */
3 /* Visualization Library */
4 /* http://visualizationlibrary.org */
5 /* */
6 /* Copyright (c) 2005-2020, Michele Bosi */
7 /* All rights reserved. */
8 /* */
9 /* Redistribution and use in source and binary forms, with or without modification, */
10 /* are permitted provided that the following conditions are met: */
11 /* */
12 /* - Redistributions of source code must retain the above copyright notice, this */
13 /* list of conditions and the following disclaimer. */
14 /* */
15 /* - Redistributions in binary form must reproduce the above copyright notice, this */
16 /* list of conditions and the following disclaimer in the documentation and/or */
17 /* other materials provided with the distribution. */
18 /* */
19 /* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND */
20 /* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED */
21 /* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE */
22 /* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR */
23 /* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES */
24 /* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; */
25 /* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON */
26 /* ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT */
27 /* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */
28 /* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
29 /* */
30 /**************************************************************************************/
31 
32 #ifndef Quaternion_INCLUDE_ONCE
33 #define Quaternion_INCLUDE_ONCE
34 
35 #include <vlCore/glsl_math.hpp>
36 
37 namespace vl
38 {
39  //-----------------------------------------------------------------------------
40  // Quaternion
41  //-----------------------------------------------------------------------------
43  template<typename T_Scalar>
44  class Quaternion
45  {
46  public:
47  typedef T_Scalar scalar_type;
48  //-----------------------------------------------------------------------------
51  {
52  setNoRotation();
53  }
54  //-----------------------------------------------------------------------------
56  template<typename T>
57  explicit Quaternion(const Quaternion<T>& quat)
58  {
59  mXYZW.x() = (T_Scalar)quat.xyzw().x();
60  mXYZW.y() = (T_Scalar)quat.xyzw().y();
61  mXYZW.z() = (T_Scalar)quat.xyzw().z();
62  mXYZW.w() = (T_Scalar)quat.xyzw().w();
63  }
64  //-----------------------------------------------------------------------------
66  explicit Quaternion(T_Scalar x, T_Scalar y, T_Scalar z, T_Scalar w)
67  {
68  mXYZW.x() = x;
69  mXYZW.y() = y;
70  mXYZW.z() = z;
71  mXYZW.w() = w;
72  }
73  //-----------------------------------------------------------------------------
75  explicit Quaternion(T_Scalar degrees, const Vector3<T_Scalar>& axis)
76  {
77  setFromAxisAngle(axis, degrees);
78  }
79  //-----------------------------------------------------------------------------
81  explicit Quaternion(const Vector4<T_Scalar>& v)
82  {
83  mXYZW.x() = (T_Scalar)v.x();
84  mXYZW.y() = (T_Scalar)v.y();
85  mXYZW.z() = (T_Scalar)v.z();
86  mXYZW.w() = (T_Scalar)v.w();
87  }
88  //-----------------------------------------------------------------------------
91  {
92  mXYZW.x() = q.x();
93  mXYZW.y() = q.y();
94  mXYZW.z() = q.z();
95  mXYZW.w() = q.w();
96  return *this;
97  }
98  //-----------------------------------------------------------------------------
101  {
102  mXYZW.x() = (T_Scalar)v.x();
103  mXYZW.y() = (T_Scalar)v.y();
104  mXYZW.z() = (T_Scalar)v.z();
105  mXYZW.w() = (T_Scalar)v.w();
106  return *this;
107  }
108  //-----------------------------------------------------------------------------
109  bool operator==(const Quaternion& q) const
110  {
111  return x() == q.x() && y() == q.y() && z() == q.z() && w() == q.w();
112  }
113  //-----------------------------------------------------------------------------
114  bool operator!=(const Quaternion& q) const
115  {
116  return !operator==(q);
117  }
118  //-----------------------------------------------------------------------------
120  bool operator<(const Quaternion& other) const
121  {
122  if (x() != other.x())
123  return x() < other.x();
124  if (y() != other.y())
125  return y() < other.y();
126  if (z() != other.z())
127  return z() < other.z();
128  else
129  return w() < other.w();
130  }
131  //-----------------------------------------------------------------------------
133  const Vector4<T_Scalar>& xyzw() const { return mXYZW; }
134  //-----------------------------------------------------------------------------
136  Vector4<T_Scalar>& xyzw() { return mXYZW; }
137  //-----------------------------------------------------------------------------
138  T_Scalar& x() { return mXYZW.x(); }
139  //-----------------------------------------------------------------------------
140  T_Scalar& y() { return mXYZW.y(); }
141  //-----------------------------------------------------------------------------
142  T_Scalar& z() { return mXYZW.z(); }
143  //-----------------------------------------------------------------------------
144  T_Scalar& w() { return mXYZW.w(); }
145  //-----------------------------------------------------------------------------
146  const T_Scalar& x() const { return mXYZW.x(); }
147  //-----------------------------------------------------------------------------
148  const T_Scalar& y() const { return mXYZW.y(); }
149  //-----------------------------------------------------------------------------
150  const T_Scalar& z() const { return mXYZW.z(); }
151  //-----------------------------------------------------------------------------
152  const T_Scalar& w() const { return mXYZW.w(); }
153  //-----------------------------------------------------------------------------
154  Quaternion operator*(T_Scalar val) const
155  {
156  Quaternion t = *this;
157  t.x() *= val;
158  t.y() *= val;
159  t.z() *= val;
160  t.w() *= val;
161  return t;
162  }
163  //-----------------------------------------------------------------------------
164  Quaternion& operator*=(T_Scalar val)
165  {
166  x() *= val;
167  y() *= val;
168  z() *= val;
169  w() *= val;
170  return *this;
171  }
172  //-----------------------------------------------------------------------------
173  Quaternion operator/(T_Scalar val) const
174  {
175  Quaternion t = *this;
176  val = (T_Scalar)1.0 / val;
177  t.x() *= val;
178  t.y() *= val;
179  t.z() *= val;
180  t.w() *= val;
181  return t;
182  }
183  //-----------------------------------------------------------------------------
184  Quaternion& operator/=(T_Scalar val)
185  {
186  val = (T_Scalar)1.0 / val;
187  x() *= val;
188  y() *= val;
189  z() *= val;
190  w() *= val;
191  return *this;
192  }
193  //-----------------------------------------------------------------------------
195  {
196  Quaternion t = *this;
197  t.x() += q.x();
198  t.y() += q.y();
199  t.z() += q.z();
200  t.w() += q.w();
201  return t;
202  }
203  //-----------------------------------------------------------------------------
205  {
206  x() += q.x();
207  y() += q.y();
208  z() += q.z();
209  w() += q.w();
210  return *this;
211  }
212  //-----------------------------------------------------------------------------
214  {
215  Quaternion t = *this;
216  t.x() -= q.x();
217  t.y() -= q.y();
218  t.z() -= q.z();
219  t.w() -= q.w();
220  return t;
221  }
222  //-----------------------------------------------------------------------------
224  {
225  x() -= q.x();
226  y() -= q.y();
227  z() -= q.z();
228  w() -= q.w();
229  return *this;
230  }
231  //-----------------------------------------------------------------------------
234  {
235  return Quaternion(-x(), -y(), -z(), -w());
236  }
237  //-----------------------------------------------------------------------------
240  {
241  mXYZW.x() = 0;
242  mXYZW.y() = 0;
243  mXYZW.z() = 0;
244  mXYZW.w() = 0;
245  return *this;
246  }
247  //-----------------------------------------------------------------------------
250  {
251  return Quaternion().setZero();
252  }
253  //-----------------------------------------------------------------------------
256  {
257  return q.setZero();
258  }
259  //-----------------------------------------------------------------------------
262  {
263  mXYZW.x() = 0;
264  mXYZW.y() = 0;
265  mXYZW.z() = 0;
266  mXYZW.w() = 1;
267  return *this;
268  }
269  //-----------------------------------------------------------------------------
272  {
273  return Quaternion();
274  }
275  //-----------------------------------------------------------------------------
278  {
279  return q.setNoRotation();
280  }
281  //-----------------------------------------------------------------------------
284  //-----------------------------------------------------------------------------
287  {
288  return Quaternion().setFromVectors(from, to);
289  }
290  //-----------------------------------------------------------------------------
293  {
294  return q.setFromVectors(from, to);
295  }
296  //-----------------------------------------------------------------------------
300  //-----------------------------------------------------------------------------
303  {
304  return Quaternion().setFromMatrix(m);
305  }
306  //-----------------------------------------------------------------------------
309  {
310  return q.setFromMatrix(m);
311  }
312  //-----------------------------------------------------------------------------
316  //-----------------------------------------------------------------------------
319  {
320  return Quaternion().setFromMatrix(m);
321  }
322  //-----------------------------------------------------------------------------
325  {
326  return q.setFromMatrix(m);
327  }
328  //-----------------------------------------------------------------------------
329  Quaternion& setFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ);
330  //-----------------------------------------------------------------------------
331  static Quaternion getFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ)
332  {
333  return Quaternion().setFromEulerXYZ(degX, degY, degZ);
334  }
335  //-----------------------------------------------------------------------------
336  static Quaternion& getFromEulerXYZ(Quaternion& q, T_Scalar degX, T_Scalar degY, T_Scalar degZ)
337  {
338  return q.setFromEulerXYZ(degX, degY, degZ);
339  }
340  //-----------------------------------------------------------------------------
341  Quaternion& setFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX);
342  //-----------------------------------------------------------------------------
343  static Quaternion getFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX)
344  {
345  return Quaternion().setFromEulerZYX(degZ, degY, degX);
346  }
347  //-----------------------------------------------------------------------------
348  static Quaternion& getFromEulerZYX(Quaternion& q, T_Scalar degZ, T_Scalar degY, T_Scalar degX)
349  {
350  return q.setFromEulerZYX(degZ, degY, degX);
351  }
352  //-----------------------------------------------------------------------------
353  Quaternion& setFromAxisAngle(const Vector3<T_Scalar>& axis, T_Scalar degrees);
354  //-----------------------------------------------------------------------------
355  static Quaternion getFromAxisAngle(const Vector3<T_Scalar>& axis, T_Scalar degrees)
356  {
357  return Quaternion().setFromAxisAngle(axis, degrees);
358  }
359  //-----------------------------------------------------------------------------
360  static Quaternion& getFromAxisAngle(Quaternion& q, const Vector3<T_Scalar>& axis, T_Scalar degrees)
361  {
362  return q.setFromAxisAngle(axis, degrees);
363  }
364  //-----------------------------------------------------------------------------
366  void toAxisAngle( Vector3<T_Scalar>& axis, T_Scalar& degrees ) const;
367  //-----------------------------------------------------------------------------
370  //-----------------------------------------------------------------------------
373  //-----------------------------------------------------------------------------
376  //-----------------------------------------------------------------------------
379  //-----------------------------------------------------------------------------
381  T_Scalar dot(const Quaternion& q) const
382  {
383  return x()*q.x() + y()*q.y() + z()*q.z() + w()*q.w();
384  }
385  //-----------------------------------------------------------------------------
387  T_Scalar length() const { return mXYZW.length(); }
388  //-----------------------------------------------------------------------------
391  Quaternion& normalize(T_Scalar* len=NULL) { mXYZW.normalize(len); return *this; }
392  //-----------------------------------------------------------------------------
395  Quaternion getNormalized(T_Scalar* len=NULL) const { Quaternion t = *this; t.normalize(len); return t; }
396  //-----------------------------------------------------------------------------
399  Quaternion& getNormalized(Quaternion& q, T_Scalar* len=NULL) const { q = *this; q.normalize(len); return q; }
400  //-----------------------------------------------------------------------------
402  T_Scalar lengthSquared() const
403  {
404  return x()*x() + y()*y() + z()*z() + w()*w();
405  }
406  //-----------------------------------------------------------------------------
409  {
410  return Quaternion(-x(), -y(), -z(), w());
411  }
412  //-----------------------------------------------------------------------------
415  {
416  q = Quaternion(-x(), -y(), -z(), w());
417  return q;
418  }
419  //-----------------------------------------------------------------------------
422  {
423  return getConjugate() / lengthSquared();
424  }
425  //-----------------------------------------------------------------------------
428  {
429  q = getConjugate() / lengthSquared();
430  return q;
431  }
432  //-----------------------------------------------------------------------------
436  static Quaternion getSlerp(T_Scalar t, const Quaternion& a, const Quaternion& b);
437  //-----------------------------------------------------------------------------
441  static Quaternion& getSlerp(Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& b);
442  //-----------------------------------------------------------------------------
444  static Quaternion getSquad(T_Scalar t, const Quaternion& a, const Quaternion& p, const Quaternion& q, const Quaternion& b)
445  {
446  return getSlerp((T_Scalar)2.0*t*((T_Scalar)1.0-t), getSlerp(t,a,b), getSlerp(t,p,q));
447  }
448  //-----------------------------------------------------------------------------
450  static Quaternion& getSquad(Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& p, const Quaternion& q, const Quaternion& b)
451  {
452  return getSlerp(out, (T_Scalar)2.0*t*((T_Scalar)1.0-t), getSlerp(t,a,b), getSlerp(t,p,q));
453  }
454  //-----------------------------------------------------------------------------
458  static Quaternion getNlerp( T_Scalar t, const Quaternion& a, const Quaternion& b )
459  {
460  Quaternion q = a + (b - a) * t;
461  q.normalize();
462  return q;
463  }
464  //-----------------------------------------------------------------------------
468  static Quaternion& getNlerp( Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& b )
469  {
470  out = a + (b - a) * t;
471  out.normalize();
472  return out;
473  }
474  //-----------------------------------------------------------------------------
475 
476  protected:
478  };
479  //-----------------------------------------------------------------------------
480  template<typename T_Scalar>
482  {
483  return q * r;
484  }
485  //-----------------------------------------------------------------------------
486  template<typename T_Scalar>
488  {
490  q.x() = q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y();
491  q.y() = q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z();
492  q.z() = q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x();
493  q.w() = q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z();
494  return q;
495  }
496  //-----------------------------------------------------------------------------
497  // post multiplication
498  template<typename T_Scalar>
500  {
501  // Matrix conversion formula based implementation
502  T_Scalar x2 = q.x() * q.x();
503  T_Scalar y2 = q.y() * q.y();
504  T_Scalar z2 = q.z() * q.z();
505  T_Scalar xy = q.x() * q.y();
506  T_Scalar xz = q.x() * q.z();
507  T_Scalar yz = q.y() * q.z();
508  T_Scalar wx = q.w() * q.x();
509  T_Scalar wy = q.w() * q.y();
510  T_Scalar wz = q.w() * q.z();
511 
513  r.x() = ( v.x()*(1.0f - 2.0f * (y2 + z2)) + v.y()*(2.0f * (xy - wz)) + v.z()*(2.0f * (xz + wy)) );
514  r.y() = ( v.x()*(2.0f * (xy + wz)) + v.y()*(1.0f - 2.0f * (x2 + z2)) + v.z()*(2.0f * (yz - wx)) );
515  r.z() = ( v.x()*(2.0f * (xz - wy)) + v.y()*(2.0f * (yz + wx)) + v.z()*(1.0f - 2.0f * (x2 + y2)) );
516  return r;
517  }
518  //-----------------------------------------------------------------------------
519  // post multiplication
520  template<typename T_Scalar>
522  {
523  return Vector4<T_Scalar>( q * v.xyz(), v.w() );
524  }
525  //-----------------------------------------------------------------------------
526  template<typename T_Scalar>
527  Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ )
528  {
529  *this = Quaternion<T_Scalar>(degX, Vector3<T_Scalar>(1,0,0)) * Quaternion<T_Scalar>(degY, Vector3<T_Scalar>(0,1,0)) * Quaternion<T_Scalar>(degZ, Vector3<T_Scalar>(0,0,1));
530  return *this;
531  }
532  //-----------------------------------------------------------------------------
533  template<typename T_Scalar>
534  Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX )
535  {
536  *this = Quaternion<T_Scalar>(degZ, Vector3<T_Scalar>(0,0,1)) * Quaternion<T_Scalar>(degY, Vector3<T_Scalar>(0,1,0)) * Quaternion<T_Scalar>(degX, Vector3<T_Scalar>(1,0,0));
537  return *this;
538  }
539  //-----------------------------------------------------------------------------
540  template<typename T_Scalar>
542  {
543  return setFromMatrix( m.get3x3() );
544  }
545  //-----------------------------------------------------------------------------
546  template<typename T_Scalar>
548  {
549  T_Scalar tr, s, q[4];
550 
551  int next[3] = {1, 2, 0};
552 
553  tr = m.e(0,0) + m.e(1,1) + m.e(2,2);
554 
555  // check the diagonal
556  if (tr + (T_Scalar)1.0 > 0.0)
557  {
558  s = vl::sqrt(tr + (T_Scalar)1.0);
559  w() = s / (T_Scalar)2.0;
560  s = (T_Scalar)0.5 / s;
561  x() = (m.e(2,1) - m.e(1,2)) * s;
562  y() = (m.e(0,2) - m.e(2,0)) * s;
563  z() = (m.e(1,0) - m.e(0,1)) * s;
564  }
565  else
566  {
567  // diagonal is negative
568  int i, j, k;
569  i = 0;
570  if (m.e(1,1) > m.e(0,0)) i = 1;
571  if (m.e(2,2) > m.e(i,i)) i = 2;
572  j = next[i];
573  k = next[j];
574 
575  s = vl::sqrt((m.e(i,i) - (m.e(j,j) + m.e(k,k))) + (T_Scalar)1.0);
576 
577  q[i] = s * (T_Scalar)0.5;
578 
579  if (s != 0.0)
580  s = (T_Scalar)0.5 / s;
581 
582  q[3] = (m.e(k,j) - m.e(j,k)) * s;
583  q[j] = (m.e(j,i) + m.e(i,j)) * s;
584  q[k] = (m.e(k,i) + m.e(i,k)) * s;
585 
586  x() = q[0];
587  y() = q[1];
588  z() = q[2];
589  w() = q[3];
590  }
591 
592  return *this;
593  }
594  //-----------------------------------------------------------------------------
595  template<typename T_Scalar>
597  {
598  degrees *= (T_Scalar)dDEG_TO_RAD;
599  T_Scalar sa2 = sin(degrees * (T_Scalar)0.5);
600  Vector3<T_Scalar> na = axis;
601  na.normalize();
602  mXYZW.x() = na.x() * sa2;
603  mXYZW.y() = na.y() * sa2;
604  mXYZW.z() = na.z() * sa2;
605  mXYZW.w() = cos(degrees * (T_Scalar)0.5);
606  return *this;
607  }
608  //-----------------------------------------------------------------------------
609  template<typename T_Scalar>
611  {
612  T_Scalar iscale = sqrt( x()*x() + y()*y() + z()*z() );
613  if (iscale == 0)
614  {
615  axis.x() = 0;
616  axis.y() = 0;
617  axis.z() = 0;
618  degrees = 0;
619  }
620  else
621  {
622  iscale = T_Scalar(1.0) / iscale;
623  axis.x() = x() * iscale;
624  axis.y() = y() * iscale;
625  axis.z() = z() * iscale;
626  VL_CHECK(w()>=-1.0 && w()<=+1.0)
627  T_Scalar tw = clamp(w(),(T_Scalar)-1.0,(T_Scalar)+1.0);
628  degrees = acos( tw ) * (T_Scalar)2.0 * (T_Scalar)dRAD_TO_DEG;
629  }
630  }
631  //-----------------------------------------------------------------------------
632  template<typename T_Scalar>
634  {
635  T_Scalar x2 = x() * x();
636  T_Scalar y2 = y() * y();
637  T_Scalar z2 = z() * z();
638  T_Scalar xy = x() * y();
639  T_Scalar xz = x() * z();
640  T_Scalar yz = y() * z();
641  T_Scalar wx = w() * x();
642  T_Scalar wy = w() * y();
643  T_Scalar wz = w() * z();
644 
645  return out = Matrix4<T_Scalar>(
646  (1.0f - 2.0f * (y2 + z2)), (2.0f * (xy - wz)), (2.0f * (xz + wy)), 0.0f,
647  (2.0f * (xy + wz)), (1.0f - 2.0f * (x2 + z2)), (2.0f * (yz - wx)), 0.0f,
648  (2.0f * (xz - wy)), (2.0f * (yz + wx)), (1.0f - 2.0f * (x2 + y2)), 0.0f,
649  0.0f, 0.0f, 0.0f, 1.0f );
650  }
651  //-----------------------------------------------------------------------------
652  template<typename T_Scalar>
654  {
655  Matrix4<T_Scalar> out;
656  return toMatrix4(out);
657  }
658  //-----------------------------------------------------------------------------
659  template<typename T_Scalar>
661  {
662  T_Scalar x2 = x() * x();
663  T_Scalar y2 = y() * y();
664  T_Scalar z2 = z() * z();
665  T_Scalar xy = x() * y();
666  T_Scalar xz = x() * z();
667  T_Scalar yz = y() * z();
668  T_Scalar wx = w() * x();
669  T_Scalar wy = w() * y();
670  T_Scalar wz = w() * z();
671 
672  return out = Matrix3<T_Scalar>(
673  (1.0f - 2.0f * (y2 + z2)), (2.0f * (xy + wz)), (2.0f * (xz - wy)),
674  (2.0f * (xy - wz)), (1.0f - 2.0f * (x2 + z2)), (2.0f * (yz + wx)),
675  (2.0f * (xz + wy)), (2.0f * (yz - wx)), (1.0f - 2.0f * (x2 + y2)) );
676  }
677  //-----------------------------------------------------------------------------
678  template<typename T_Scalar>
680  {
681  Matrix3<T_Scalar> out;
682  return toMatrix3(out);
683  }
684  //-----------------------------------------------------------------------------
685  template<typename T_Scalar>
687  {
689  getSlerp(q, t, a, b);
690  return q;
691  }
692  //-----------------------------------------------------------------------------
693  template<typename T_Scalar>
695  {
696  T_Scalar scale_a, scale_b, omega, sinom;
697  T_Scalar cosom = a.dot(b);
698 
699  Quaternion<T_Scalar> b2(b);
700 
701  if ( cosom < 0 )
702  {
703  cosom = -cosom;
704  b2 = -b;
705  }
706 
707  // clamp rounding errors
708  cosom = cosom > (T_Scalar)1 ? (T_Scalar)1 : cosom;
709 
710  if( cosom < (T_Scalar)1.0 )
711  {
712  omega = acos(cosom);
713  sinom = sin(omega);
714  VL_CHECK(sinom != 0)
715  scale_a = sin(((T_Scalar)1.0-t) * omega) / sinom;
716  scale_b = sin(t * omega) / sinom;
717  }
718  else
719  {
720  // linear interpolation for degenerate cases
721  scale_a = (T_Scalar)1.0 - t;
722  scale_b = t;
723  }
724 
725  return out = (a*scale_a) + (b2*scale_b);
726  }
727  //-----------------------------------------------------------------------------
728  template<typename T_Scalar>
730  {
731  Vector3<T_Scalar> a,b;
732  a = from;
733  b = to;
734  a.normalize();
735  b.normalize();
736  Vector3<T_Scalar> axis = cross(a,b);
737  T_Scalar len = 0;
738  axis.normalize(&len);
739  if(len)
740  {
741  T_Scalar cosa = vl::dot(a,b);
742  cosa = clamp(cosa, (T_Scalar)-1.0, (T_Scalar)+1.0);
743  T_Scalar alpha = acos( cosa );
744  setFromAxisAngle(axis, alpha*(T_Scalar)dRAD_TO_DEG);
745  }
746  else
747  setNoRotation();
748  return *this;
749  }
750  //-----------------------------------------------------------------------------
754  //-----------------------------------------------------------------------------
755 }
756 
757 #endif
static Quaternion getFromMatrix(const Matrix4< T_Scalar > &m)
Converts the given rotation matrix into a quaternion.
Definition: Quaternion.hpp:302
const T_Scalar & z() const
Definition: Vector4.hpp:103
float clamp(float x, float minval, float maxval)
Definition: Vector2.hpp:315
Quaternion operator/(T_Scalar val) const
Definition: Quaternion.hpp:173
const Vector3 & normalize(T_Scalar *len=NULL)
Definition: Vector3.hpp:227
Quaternion & operator+=(const Quaternion &q)
Definition: Quaternion.hpp:204
Quaternion & operator*=(T_Scalar val)
Definition: Quaternion.hpp:164
const T_Scalar & x() const
Definition: Vector4.hpp:101
T sqrt(T a)
Definition: glsl_math.hpp:592
const T_Scalar & w() const
Definition: Quaternion.hpp:152
static Quaternion getZero()
Returns the zero quaternion.
Definition: Quaternion.hpp:249
static Quaternion & getNoRotation(Quaternion &q)
Returns the no-rotation quaternion, i.e. Quaternion(0,0,0,1).
Definition: Quaternion.hpp:277
Quaternion & setFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ)
Definition: Quaternion.hpp:527
T sin(T a)
Definition: glsl_math.hpp:199
T degrees(T radians)
Definition: glsl_math.hpp:173
const T_Scalar & z() const
Definition: Vector3.hpp:91
static Quaternion getFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ)
Definition: Quaternion.hpp:331
void toAxisAngle(Vector3< T_Scalar > &axis, T_Scalar &degrees) const
Converts a quaternion to an axis-angle representation.
Definition: Quaternion.hpp:610
bool operator!=(const Quaternion &q) const
Definition: Quaternion.hpp:114
Quaternion & setFromAxisAngle(const Vector3< T_Scalar > &axis, T_Scalar degrees)
Definition: Quaternion.hpp:596
Quaternion & setZero()
Sets all the components of the quaternion to zero.
Definition: Quaternion.hpp:239
Quaternion & setNoRotation()
Set the quaternion to no-rotation, i.e. Quaternion(0,0,0,1).
Definition: Quaternion.hpp:261
Quaternion< float > fquat
Definition: Quaternion.hpp:751
Quaternion(const Quaternion< T > &quat)
Copy-constructor.
Definition: Quaternion.hpp:57
Vector4< T_Scalar > & xyzw()
Returns the internal vec4 used to contain the xyzw the quaternion components.
Definition: Quaternion.hpp:136
static Quaternion & getFromMatrix(Quaternion &q, const Matrix3< T_Scalar > &m)
Converts the given rotation matrix into a quaternion.
Definition: Quaternion.hpp:324
Quaternion< double > dquat
Definition: Quaternion.hpp:752
T_Scalar lengthSquared() const
Returns the squared length of a quaternion.
Definition: Quaternion.hpp:402
Vector3< T_Scalar > xyz() const
Definition: Vector4.hpp:131
const T_Scalar & e(int i, int j) const
Definition: Matrix3.hpp:519
Quaternion operator*(T_Scalar val) const
Definition: Quaternion.hpp:154
const double dRAD_TO_DEG
Constant to convert radian into degree using double precision.
Definition: std_types.hpp:68
Quaternion & getInverse(Quaternion &q) const
Returns the inverse of a quaternion.
Definition: Quaternion.hpp:427
The Vector4 class is a template class that implements a generic 4 components vector, see also vl::fvec4, vl::dvec4, vl::uvec4, vl::ivec4, vl::svec4, vl::usvec4, vl::bvec4, vl::ubvec4.
Definition: Vector4.hpp:44
fvec3 cross(const fvec3 &v1, const fvec3 &v2)
Definition: Vector3.hpp:277
T_Scalar & y()
Definition: Quaternion.hpp:140
Quaternion & setFromMatrix(const Matrix4< T_Scalar > &m)
Creates a quaternion representing the given rotation matrix.
Definition: Quaternion.hpp:541
const T_Scalar & x() const
Definition: Quaternion.hpp:146
static Quaternion getFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX)
Definition: Quaternion.hpp:343
Quaternion(T_Scalar x, T_Scalar y, T_Scalar z, T_Scalar w)
Constructor.
Definition: Quaternion.hpp:66
static Quaternion & getZero(Quaternion &q)
Returns the zero quaternion.
Definition: Quaternion.hpp:255
const Vector4< T_Scalar > & xyzw() const
Returns the internal vec4 used to contain the xyzw the quaternion components.
Definition: Quaternion.hpp:133
Quaternion(const Vector4< T_Scalar > &v)
Constructor from vec4.
Definition: Quaternion.hpp:81
bool operator<(const Quaternion &other) const
Lexicographic ordering.
Definition: Quaternion.hpp:120
T_Scalar scalar_type
Definition: Quaternion.hpp:47
Implements a Quaternion usually used to represent rotations and orientations.
Definition: Quaternion.hpp:44
Visualization Library main namespace.
Quaternion()
Constructor.
Definition: Quaternion.hpp:50
float dot(float a, float b)
Definition: glsl_math.hpp:1111
const double dDEG_TO_RAD
Constant to convert degree into radian using double precision.
Definition: std_types.hpp:66
Quaternion getNormalized(T_Scalar *len=NULL) const
Returns the normalized version of a quaternion.
Definition: Quaternion.hpp:395
Quaternion & normalize(T_Scalar *len=NULL)
Normalizes a quaternion.
Definition: Quaternion.hpp:391
The Matrix3 class is a template class that implements a generic 3x3 matrix, see also vl::dmat3...
Definition: Matrix3.hpp:48
static Quaternion getNoRotation()
Returns the no-rotation quaternion, i.e. Quaternion(0,0,0,1).
Definition: Quaternion.hpp:271
Matrix3< T_Scalar > get3x3() const
Definition: Matrix4.hpp:323
static Quaternion getFromVectors(const Vector3< T_Scalar > &from, const Vector3< T_Scalar > &to)
Sets the quaternion to represent the rotation transforming from into to.
Definition: Quaternion.hpp:286
static Quaternion & getFromVectors(Quaternion &q, const Vector3< T_Scalar > &from, const Vector3< T_Scalar > &to)
Sets the quaternion to represent the rotation transforming from into to.
Definition: Quaternion.hpp:292
Quaternion operator+(const Quaternion &q) const
Definition: Quaternion.hpp:194
The Vector3 class is a template class that implements a generic 3 components vector, see also vl::fvec3, vl::dvec3, vl::uvec3, vl::ivec3, vl::svec3, vl::usvec3, vl::bvec3, vl::ubvec3.
Definition: Vector3.hpp:44
Quaternion< real > quat
Definition: Quaternion.hpp:753
T_Scalar length() const
Returns the length of a quaternion.
Definition: Quaternion.hpp:387
Quaternion & getNormalized(Quaternion &q, T_Scalar *len=NULL) const
Returns the normalized version of a quaternion.
Definition: Quaternion.hpp:399
const T_Scalar & z() const
Definition: Quaternion.hpp:150
static Quaternion & getFromEulerZYX(Quaternion &q, T_Scalar degZ, T_Scalar degY, T_Scalar degX)
Definition: Quaternion.hpp:348
bool operator==(const Quaternion &q) const
Definition: Quaternion.hpp:109
Quaternion(T_Scalar degrees, const Vector3< T_Scalar > &axis)
Axis-angle constructor.
Definition: Quaternion.hpp:75
T_Scalar & z()
Definition: Quaternion.hpp:142
Matrix4< T_Scalar > toMatrix4() const
Converts a quaternion to a 4x4 rotation matrix.
Definition: Quaternion.hpp:653
Implements the OpenGL Shading Language convenience functions for scalar and vector operations...
static Quaternion & getNlerp(Quaternion &out, T_Scalar t, const Quaternion &a, const Quaternion &b)
Normalized spherical interpolation of two quaternions.
Definition: Quaternion.hpp:468
static Quaternion getFromAxisAngle(const Vector3< T_Scalar > &axis, T_Scalar degrees)
Definition: Quaternion.hpp:355
const T_Scalar & y() const
Definition: Vector3.hpp:90
static Quaternion & getFromMatrix(Quaternion &q, const Matrix4< T_Scalar > &m)
Converts the given rotation matrix into a quaternion.
Definition: Quaternion.hpp:308
const T_Scalar & y() const
Definition: Vector4.hpp:102
Quaternion & setFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX)
Definition: Quaternion.hpp:534
static Quaternion getSquad(T_Scalar t, const Quaternion &a, const Quaternion &p, const Quaternion &q, const Quaternion &b)
Spherical cubic interpolation of two quaternions.
Definition: Quaternion.hpp:444
Quaternion operator-(const Quaternion &q) const
Definition: Quaternion.hpp:213
Quaternion & setFromVectors(const Vector3< T_Scalar > &from, const Vector3< T_Scalar > &to)
Sets the quaternion to represent the rotation transforming from into to.
Definition: Quaternion.hpp:729
T_Scalar & x()
Definition: Quaternion.hpp:138
Quaternion getConjugate() const
Returns the conjugate of a quaternion.
Definition: Quaternion.hpp:408
Quaternion & operator-=(const Quaternion &q)
Definition: Quaternion.hpp:223
The Matrix4 class is a template class that implements a generic 4x4 matrix, see also vl::dmat4...
Definition: Matrix4.hpp:48
#define NULL
Definition: OpenGLDefs.hpp:81
const T_Scalar & y() const
Definition: Quaternion.hpp:148
Quaternion & getConjugate(Quaternion &q) const
Returns the conjugate of a quaternion.
Definition: Quaternion.hpp:414
Vector4< T_Scalar > mXYZW
Definition: Quaternion.hpp:477
Matrix3< T_Scalar > toMatrix3() const
Converts a quaternion to a 3x3 rotation matrix.
Definition: Quaternion.hpp:679
Quaternion & operator/=(T_Scalar val)
Definition: Quaternion.hpp:184
T cos(T a)
Definition: glsl_math.hpp:225
static Quaternion & getSquad(Quaternion &out, T_Scalar t, const Quaternion &a, const Quaternion &p, const Quaternion &q, const Quaternion &b)
Spherical cubic interpolation of two quaternions.
Definition: Quaternion.hpp:450
const T_Scalar & x() const
Definition: Vector3.hpp:89
Quaternion & operator=(const Quaternion &q)
Assignment operator.
Definition: Quaternion.hpp:90
static Quaternion getFromMatrix(const Matrix3< T_Scalar > &m)
Converts the given rotation matrix into a quaternion.
Definition: Quaternion.hpp:318
T acos(T a)
Definition: glsl_math.hpp:329
static Quaternion getNlerp(T_Scalar t, const Quaternion &a, const Quaternion &b)
Normalized spherical interpolation of two quaternions.
Definition: Quaternion.hpp:458
static Quaternion & getFromAxisAngle(Quaternion &q, const Vector3< T_Scalar > &axis, T_Scalar degrees)
Definition: Quaternion.hpp:360
T_Scalar dot(const Quaternion &q) const
Returns the dot product between a quaternion and the given quaternion.
Definition: Quaternion.hpp:381
T_Scalar & w()
Definition: Quaternion.hpp:144
static Quaternion & getFromEulerXYZ(Quaternion &q, T_Scalar degX, T_Scalar degY, T_Scalar degZ)
Definition: Quaternion.hpp:336
#define VL_CHECK(expr)
Definition: checks.hpp:73
Quaternion operator-() const
Returns the negated quaternion.
Definition: Quaternion.hpp:233
Quaternion & operator=(const Vector4< T_Scalar > &v)
Assignment operator for vec4.
Definition: Quaternion.hpp:100
const T_Scalar & w() const
Definition: Vector4.hpp:104
static Quaternion getSlerp(T_Scalar t, const Quaternion &a, const Quaternion &b)
Spherical linear interpolation of two quaternions.
Definition: Quaternion.hpp:686
Quaternion getInverse() const
Returns the inverse of a quaternion.
Definition: Quaternion.hpp:421