123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158 |
- // Copyright (C) 2013-2014 Thalmic Labs Inc.
- // Distributed under the Myo SDK license agreement. See LICENSE.txt for details.
- #pragma once
-
- #include <cmath>
-
- #include "Vector3.hpp"
-
- namespace myo {
-
- /// A quaternion that can be used to represent a rotation.
- /// This type provides only very basic functionality to store quaternions that's sufficient to retrieve the data to
- /// be placed in a full featured quaternion type.
- template<typename T>
- class Quaternion {
- public:
- /// Construct a quaternion that represents zero rotation (i.e. the multiplicative identity).
- Quaternion()
- : _x(0)
- , _y(0)
- , _z(0)
- , _w(1)
- {
- }
-
- /// Construct a quaternion with the provided components.
- Quaternion(T x, T y, T z, T w)
- : _x(x)
- , _y(y)
- , _z(z)
- , _w(w)
- {
- }
-
- /// Set the components of this quaternion to be those of the other.
- Quaternion& operator=(const Quaternion other)
- {
- _x = other._x;
- _y = other._y;
- _z = other._z;
- _w = other._w;
-
- return *this;
- }
-
- /// Return the x-component of this quaternion's vector.
- T x() const { return _x; }
-
- /// Return the y-component of this quaternion's vector.
- T y() const { return _y; }
-
- /// Return the z-component of this quaternion's vector.
- T z() const { return _z; }
-
- /// Return the w-component (scalar) of this quaternion.
- T w() const { return _w; }
-
- /// Return the quaternion multiplied by \a rhs.
- /// Note that quaternion multiplication is not commutative.
- Quaternion operator*(const Quaternion& rhs) const
- {
- return Quaternion(
- _w * rhs._x + _x * rhs._w + _y * rhs._z - _z * rhs._y,
- _w * rhs._y - _x * rhs._z + _y * rhs._w + _z * rhs._x,
- _w * rhs._z + _x * rhs._y - _y * rhs._x + _z * rhs._w,
- _w * rhs._w - _x * rhs._x - _y * rhs._y - _z * rhs._z
- );
- }
-
- /// Multiply this quaternion by \a rhs.
- /// Return this quaternion updated with the result.
- Quaternion& operator*=(const Quaternion& rhs)
- {
- *this = *this * rhs;
- return *this;
- }
-
- /// Return the unit quaternion corresponding to the same rotation as this one.
- Quaternion normalized() const
- {
- T magnitude = std::sqrt(_x * _x + _y * _y + _z * _z + _w * _w);
-
- return Quaternion(_x / magnitude, _y / magnitude, _z / magnitude, _w / magnitude);
- }
-
- /// Return this quaternion's conjugate.
- Quaternion conjugate() const
- {
- return Quaternion(-_x, -_y, -_z, _w);
- }
-
- /// Return a quaternion that represents a right-handed rotation of \a angle radians about the given \a axis.
- /// \a axis The unit vector representing the axis of rotation.
- /// \a angle The angle of rotation, in radians.
- static Quaternion fromAxisAngle(const myo::Vector3<T>& axis, T angle)
- {
- return Quaternion(axis.x() * std::sin(angle / 2),
- axis.y() * std::sin(angle / 2),
- axis.z() * std::sin(angle / 2),
- std::cos(angle / 2));
- }
-
- private:
- T _x, _y, _z, _w;
- };
-
- /// Return a copy of this \a vec rotated by \a quat.
- /// \relates myo::Quaternion
- template<typename T>
- Vector3<T> rotate(const Quaternion<T>& quat, const Vector3<T>& vec)
- {
- myo::Quaternion<T> qvec(vec.x(), vec.y(), vec.z(), 0);
- myo::Quaternion<T> result = quat * qvec * quat.conjugate();
- return Vector3<T>(result.x(), result.y(), result.z());
- }
-
- /// Return a quaternion that represents a rotation from vector \a from to \a to.
- /// \relates myo::Quaternion
- /// See http://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
- /// for some explanation.
- template<typename T>
- Quaternion<T> rotate(const Vector3<T>& from, const Vector3<T>& to)
- {
- Vector3<T> cross = from.cross(to);
-
- // The product of the square of magnitudes and the cosine of the angle between from and to.
- T cosTheta = from.dot(to);
-
- // Return identity if the vectors are the same direction.
- if (cosTheta >= 1) {
- return Quaternion<T>();
- }
-
- // The product of the square of the magnitudes
- T k = std::sqrt(from.dot(from) * to.dot(to));
-
- // Return identity in the degenerate case.
- if (k <= 0) {
- return Quaternion<T>();
- }
-
- // Special handling for vectors facing opposite directions.
- if (cosTheta / k <= -1) {
- Vector3<T> xAxis(1, 0, 0);
- Vector3<T> yAxis(0, 1, 0);
-
- cross = from.cross(std::abs(from.dot(xAxis)) < 1 ? xAxis : yAxis);
- k = cosTheta = 0;
- }
-
- return Quaternion<T>(
- cross.x(),
- cross.y(),
- cross.z(),
- k + cosTheta);
- }
-
- } // namespace myo
|