Myo SDK – 0.9.0
Quaternion.hpp
1 // Copyright (C) 2013-2014 Thalmic Labs Inc.
2 // Distributed under the Myo SDK license agreement. See LICENSE.txt for details.
3 #pragma once
4 
5 #include <cmath>
6 
7 #include "Vector3.hpp"
8 
9 namespace myo {
10 
14 template<typename T>
15 class Quaternion {
16  public:
19  : _x(0)
20  , _y(0)
21  , _z(0)
22  , _w(1)
23  {
24  }
25 
27  Quaternion(T x, T y, T z, T w)
28  : _x(x)
29  , _y(y)
30  , _z(z)
31  , _w(w)
32  {
33  }
34 
37  {
38  _x = other._x;
39  _y = other._y;
40  _z = other._z;
41  _w = other._w;
42 
43  return *this;
44  }
45 
47  T x() const { return _x; }
48 
50  T y() const { return _y; }
51 
53  T z() const { return _z; }
54 
56  T w() const { return _w; }
57 
60  Quaternion operator*(const Quaternion& rhs) const
61  {
62  return Quaternion(
63  _w * rhs._x + _x * rhs._w + _y * rhs._z - _z * rhs._y,
64  _w * rhs._y - _x * rhs._z + _y * rhs._w + _z * rhs._x,
65  _w * rhs._z + _x * rhs._y - _y * rhs._x + _z * rhs._w,
66  _w * rhs._w - _x * rhs._x - _y * rhs._y - _z * rhs._z
67  );
68  }
69 
73  {
74  *this = *this * rhs;
75  return *this;
76  }
77 
80  {
81  T magnitude = std::sqrt(_x * _x + _y * _y + _z * _z + _w * _w);
82 
83  return Quaternion(_x / magnitude, _y / magnitude, _z / magnitude, _w / magnitude);
84  }
85 
88  {
89  return Quaternion(-_x, -_y, -_z, _w);
90  }
91 
95  static Quaternion fromAxisAngle(const myo::Vector3<T>& axis, T angle)
96  {
97  return Quaternion(axis.x() * std::sin(angle / 2),
98  axis.y() * std::sin(angle / 2),
99  axis.z() * std::sin(angle / 2),
100  std::cos(angle / 2));
101  }
102 
103  private:
104  T _x, _y, _z, _w;
105 };
106 
109 template<typename T>
110 Vector3<T> rotate(const Quaternion<T>& quat, const Vector3<T>& vec)
111 {
112  myo::Quaternion<T> qvec(vec.x(), vec.y(), vec.z(), 0);
113  myo::Quaternion<T> result = quat * qvec * quat.conjugate();
114  return Vector3<T>(result.x(), result.y(), result.z());
115 }
116 
121 template<typename T>
122 Quaternion<T> rotate(const Vector3<T>& from, const Vector3<T>& to)
123 {
124  Vector3<T> cross = from.cross(to);
125 
126  // The product of the square of magnitudes and the cosine of the angle between from and to.
127  T cosTheta = from.dot(to);
128 
129  // Return identity if the vectors are the same direction.
130  if (cosTheta >= 1) {
131  return Quaternion<T>();
132  }
133 
134  // The product of the square of the magnitudes
135  T k = std::sqrt(from.dot(from) * to.dot(to));
136 
137  // Return identity in the degenerate case.
138  if (k <= 0) {
139  return Quaternion<T>();
140  }
141 
142  // Special handling for vectors facing opposite directions.
143  if (cosTheta / k <= -1) {
144  Vector3<T> xAxis(1, 0, 0);
145  Vector3<T> yAxis(0, 1, 0);
146 
147  cross = from.cross(std::abs(from.dot(xAxis)) < 1 ? xAxis : yAxis);
148  k = cosTheta = 0;
149  }
150 
151  return Quaternion<T>(
152  cross.x(),
153  cross.y(),
154  cross.z(),
155  k + cosTheta);
156 }
157 
158 } // namespace myo
Quaternion normalized() const
Return the unit quaternion corresponding to the same rotation as this one.
Definition: Quaternion.hpp:79
static Quaternion fromAxisAngle(const myo::Vector3< T > &axis, T angle)
Return a quaternion that represents a right-handed rotation of angle radians about the given axis...
Definition: Quaternion.hpp:95
The namespace in which all of the Myo C++ bindings are contained.
Definition: DeviceListener.hpp:9
T z() const
Return the z-component of this quaternion's vector.
Definition: Quaternion.hpp:53
Quaternion< T > rotate(const Vector3< T > &from, const Vector3< T > &to)
Return a quaternion that represents a rotation from vector from to to.
Definition: Quaternion.hpp:122
A quaternion that can be used to represent a rotation.
Definition: DeviceListener.hpp:13
T w() const
Return the w-component (scalar) of this quaternion.
Definition: Quaternion.hpp:56
T x() const
Return the x-component of this vector.
Definition: Vector3.hpp:56
Vector3 cross(const Vector3 &rhs) const
Return the cross product of this vector and rhs.
Definition: Vector3.hpp:84
T y() const
Return the y-component of this vector.
Definition: Vector3.hpp:59
Quaternion(T x, T y, T z, T w)
Construct a quaternion with the provided components.
Definition: Quaternion.hpp:27
T z() const
Return the z-component of this vector.
Definition: Vector3.hpp:62
T x() const
Return the x-component of this quaternion's vector.
Definition: Quaternion.hpp:47
A vector of three components.
Definition: DeviceListener.hpp:15
Quaternion()
Construct a quaternion that represents zero rotation (i.e. the multiplicative identity).
Definition: Quaternion.hpp:18
T dot(const Vector3 &rhs) const
Return the dot product of this vector and rhs.
Definition: Vector3.hpp:78
Quaternion conjugate() const
Return this quaternion's conjugate.
Definition: Quaternion.hpp:87
T y() const
Return the y-component of this quaternion's vector.
Definition: Quaternion.hpp:50
Vector3< T > rotate(const Quaternion< T > &quat, const Vector3< T > &vec)
Return a copy of this vec rotated by quat.
Definition: Quaternion.hpp:110
Quaternion & operator*=(const Quaternion &rhs)
Multiply this quaternion by rhs.
Definition: Quaternion.hpp:72
Quaternion operator*(const Quaternion &rhs) const
Return the quaternion multiplied by rhs.
Definition: Quaternion.hpp:60
Quaternion & operator=(const Quaternion other)
Set the components of this quaternion to be those of the other.
Definition: Quaternion.hpp:36