ic
/
magnum
mirror of https://github.com/mosra/magnum.git
6
0
Fork 0
Code Issues Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1573 Commits
21 Branches
19 Tags
70 MiB
 
 
 
 
 
Tree: d1f222d8d9
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'd1f222d8d9'
${ noResults }
magnum/src/Math/Quaternion.h

486 lines
17 KiB
Raw Blame History

#ifndef Magnum_Math_Quaternion_h
#define Magnum_Math_Quaternion_h
/*
Copyright © 2010, 2011, 2012 Vladimír Vondruš <mosra@centrum.cz>
This file is part of Magnum.
Magnum is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License version 3
only, as published by the Free Software Foundation.
Magnum is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License version 3 for more details.
*/
/** @file
* @brief Class Magnum::Math::Quaternion
*/
#include <cmath>
#include <Utility/Assert.h>
#include <Utility/Debug.h>
#include "Math/MathTypeTraits.h"
#include "Math/Matrix.h"
#include "Math/Vector3.h"
namespace Magnum { namespace Math {
/**
@brief %Quaternion
@tparam T Underlying data type
Represents 3D rotation.
@see Magnum::Quaternion, DualQuaternion, Matrix4
*/
template<class T> class Quaternion {
public:
typedef T Type; /**< @brief Underlying data type */
/**
* @brief Dot product
*
* @f[
* p \cdot q = \boldsymbol p_V \cdot \boldsymbol q_V + p_S q_S
* @f]
* @see dot() const
*/
inline static T dot(const Quaternion<T>& a, const Quaternion<T>& b) {
/** @todo Use four-component SIMD implementation when available */
return Vector3<T>::dot(a.vector(), b.vector()) + a.scalar()*b.scalar();
}
/**
* @brief Angle between normalized quaternions
*
* Expects that both quaternions are normalized. @f[
* \theta = acos \left( \frac{p \cdot q}{|p| \cdot |q|} \right)
* @f]
*/
inline static Rad<T> angle(const Quaternion<T>& normalizedA, const Quaternion<T>& normalizedB) {
CORRADE_ASSERT(MathTypeTraits<T>::equals(normalizedA.dot(), T(1)) && MathTypeTraits<T>::equals(normalizedB.dot(), T(1)),
"Math::Quaternion::angle(): quaternions must be normalized", Rad<T>(std::numeric_limits<T>::quiet_NaN()));
return Rad<T>(angleInternal(normalizedA, normalizedB));
}
/**
* @brief Linear interpolation of two quaternions
* @param normalizedA First quaternion
* @param normalizedB Second quaternion
* @param t Interpolation phase (from range @f$ [0; 1] @f$)
*
* Expects that both quaternions are normalized. @f[
* q_{LERP} = \frac{(1 - t) q_A + t q_B}{|(1 - t) q_A + t q_B|}
* @f]
* @see slerp(), Math::lerp()
*/
inline static Quaternion<T> lerp(const Quaternion<T>& normalizedA, const Quaternion<T>& normalizedB, T t) {
CORRADE_ASSERT(MathTypeTraits<T>::equals(normalizedA.dot(), T(1)) && MathTypeTraits<T>::equals(normalizedB.dot(), T(1)),
"Math::Quaternion::lerp(): quaternions must be normalized",
Quaternion<T>({}, std::numeric_limits<T>::quiet_NaN()));
return ((T(1) - t)*normalizedA + t*normalizedB).normalized();
}
/**
* @brief Spherical linear interpolation of two quaternions
* @param normalizedA First quaternion
* @param normalizedB Second quaternion
* @param t Interpolation phase (from range @f$ [0; 1] @f$)
*
* Expects that both quaternions are normalized. @f[
* q_{SLERP} = \frac{sin((1 - t) \theta) q_A + sin(t \theta) q_B}{sin \theta}
* ~~~~~~~~~~
* \theta = acos \left( \frac{q_A \cdot q_B}{|q_A| \cdot |q_B|} \right)
* @f]
* @see lerp()
*/
inline static Quaternion<T> slerp(const Quaternion<T>& normalizedA, const Quaternion<T>& normalizedB, T t) {
CORRADE_ASSERT(MathTypeTraits<T>::equals(normalizedA.dot(), T(1)) && MathTypeTraits<T>::equals(normalizedB.dot(), T(1)),
"Math::Quaternion::slerp(): quaternions must be normalized",
Quaternion<T>({}, std::numeric_limits<T>::quiet_NaN()));
T a = angleInternal(normalizedA, normalizedB);
return (std::sin((T(1) - t)*a)*normalizedA + std::sin(t*a)*normalizedB)/std::sin(a);
}
/**
* @brief Rotation quaternion
* @param angle Rotation angle (counterclockwise, in radians)
* @param normalizedAxis Normalized rotation axis
*
* Expects that the rotation axis is normalized. @f[
* q = [\boldsymbol a \cdot sin \frac \theta 2, cos \frac \theta 2]
* @f]
* @see rotationAngle(), rotationAxis(), DualQuaternion::rotation(),
* Matrix4::rotation(), Vector3::xAxis(), Vector3::yAxis(),
* Vector3::zAxis()
*/
inline static Quaternion<T> rotation(Rad<T> angle, const Vector3<T>& normalizedAxis) {
CORRADE_ASSERT(MathTypeTraits<T>::equals(normalizedAxis.dot(), T(1)),
"Math::Quaternion::rotation(): axis must be normalized", {});
return {normalizedAxis*std::sin(T(angle)/2), std::cos(T(angle)/2)};
}
/**
* @brief Default constructor
*
* @f[
* q = [\boldsymbol 0, 1]
* @f]
*/
inline constexpr /*implicit*/ Quaternion(): _scalar(T(1)) {}
/**
* @brief Construct quaternion from vector and scalar
*
* @f[
* q = [\boldsymbol v, s]
* @f]
*/
inline constexpr /*implicit*/ Quaternion(const Vector3<T>& vector, T scalar): _vector(vector), _scalar(scalar) {}
/**
* @brief Construct quaternion from vector
*
* To be used in transformations later. @f[
* q = [\boldsymbol v, 0]
* @f]
* @see transformVector(), transformVectorNormalized()
*/
inline constexpr explicit Quaternion(const Vector3<T>& vector): _vector(vector), _scalar(T(0)) {}
/** @brief Equality comparison */
inline bool operator==(const Quaternion<T>& other) const {
return _vector == other._vector && MathTypeTraits<T>::equals(_scalar, other._scalar);
}
/** @brief Non-equality comparison */
inline bool operator!=(const Quaternion<T>& other) const {
return !operator==(other);
}
/** @brief %Vector part */
inline constexpr Vector3<T> vector() const { return _vector; }
/** @brief %Scalar part */
inline constexpr T scalar() const { return _scalar; }
/**
* @brief Rotation angle of unit quaternion
*
* Expects that the quaternion is normalized. @f[
* \theta = 2 \cdot acos q_S
* @f]
* @see rotationAxis(), rotation()
*/
inline Rad<T> rotationAngle() const {
CORRADE_ASSERT(MathTypeTraits<T>::equals(dot(), T(1)),
"Math::Quaternion::rotationAngle(): quaternion must be normalized",
Rad<T>(std::numeric_limits<T>::quiet_NaN()));
return Rad<T>(T(2)*std::acos(_scalar));
}
/**
* @brief Rotation axis of unit quaternion
*
* Expects that the quaternion is normalized. Returns either unit-length
* vector for valid rotation quaternion or NaN vector for
* default-constructed quaternion. @f[
* \boldsymbol a = \frac{\boldsymbol q_V}{\sqrt{1 - q_S^2}}
* @f]
* @see rotationAngle(), rotation()
*/
inline Vector3<T> rotationAxis() const {
CORRADE_ASSERT(MathTypeTraits<T>::equals(dot(), T(1)),
"Math::Quaternion::rotationAxis(): quaternion must be normalized",
{});
return _vector/std::sqrt(1-pow2(_scalar));
}
/**
* @brief Convert quaternion to rotation matrix
*
* @see DualQuaternion::matrix(), Matrix4::from(const Matrix<3, T>&, const Vector3<T>&)
*/
Matrix<3, T> matrix() const {
return {
Vector<3, T>(T(1) - 2*pow2(_vector.y()) - 2*pow2(_vector.z()),
2*_vector.x()*_vector.y() + 2*_vector.z()*_scalar,
2*_vector.x()*_vector.z() - 2*_vector.y()*_scalar),
Vector<3, T>(2*_vector.x()*_vector.y() - 2*_vector.z()*_scalar,
T(1) - 2*pow2(_vector.x()) - 2*pow2(_vector.z()),
2*_vector.y()*_vector.z() + 2*_vector.x()*_scalar),
Vector<3, T>(2*_vector.x()*_vector.z() + 2*_vector.y()*_scalar,
2*_vector.y()*_vector.z() - 2*_vector.x()*_scalar,
T(1) - 2*pow2(_vector.x()) - 2*pow2(_vector.y()))
};
}
/**
* @brief Add and assign quaternion
*
* The computation is done in-place. @f[
* p + q = [\boldsymbol p_V + \boldsymbol q_V, p_S + q_S]
* @f]
*/
inline Quaternion<T>& operator+=(const Quaternion<T>& other) {
_vector += other._vector;
_scalar += other._scalar;
return *this;
}
/**
* @brief Add quaternion
*
* @see operator+=()
*/
inline Quaternion<T> operator+(const Quaternion<T>& other) const {
return Quaternion<T>(*this) += other;
}
/**
* @brief Negated quaternion
*
* @f[
* -q = [-\boldsymbol q_V, -q_S]
* @f]
*/
inline Quaternion<T> operator-() const {
return {-_vector, -_scalar};
}
/**
* @brief Subtract and assign quaternion
*
* The computation is done in-place. @f[
* p - q = [\boldsymbol p_V - \boldsymbol q_V, p_S - q_S]
* @f]
*/
inline Quaternion<T>& operator-=(const Quaternion<T>& other) {
_vector -= other._vector;
_scalar -= other._scalar;
return *this;
}
/**
* @brief Subtract quaternion
*
* @see operator-=()
*/
inline Quaternion<T> operator-(const Quaternion<T>& other) const {
return Quaternion<T>(*this) -= other;
}
/**
* @brief Multiply with scalar and assign
*
* The computation is done in-place. @f[
* q \cdot a = [\boldsymbol q_V \cdot a, q_S \cdot a]
* @f]
*/
inline Quaternion<T>& operator*=(T scalar) {
_vector *= scalar;
_scalar *= scalar;
return *this;
}
/**
* @brief Multiply with scalar
*
* @see operator*=(T)
*/
inline Quaternion<T> operator*(T scalar) const {
return Quaternion<T>(*this) *= scalar;
}
/**
* @brief Divide with scalar and assign
*
* The computation is done in-place. @f[
* \frac q a = [\frac {\boldsymbol q_V} a, \frac {q_S} a]
* @f]
*/
inline Quaternion<T>& operator/=(T scalar) {
_vector /= scalar;
_scalar /= scalar;
return *this;
}
/**
* @brief Divide with scalar
*
* @see operator/=(T)
*/
inline Quaternion<T> operator/(T scalar) const {
return Quaternion<T>(*this) /= scalar;
}
/**
* @brief Multiply with quaternion
*
* @f[
* p q = [p_S \boldsymbol q_V + q_S \boldsymbol p_V + \boldsymbol p_V \times \boldsymbol q_V,
* p_S q_S - \boldsymbol p_V \cdot \boldsymbol q_V]
* @f]
*/
inline Quaternion<T> operator*(const Quaternion<T>& other) const {
return {_scalar*other._vector + other._scalar*_vector + Vector3<T>::cross(_vector, other._vector),
_scalar*other._scalar - Vector3<T>::dot(_vector, other._vector)};
}
/**
* @brief Dot product of the quaternion
*
* Should be used instead of length() for comparing quaternion length
* with other values, because it doesn't compute the square root. @f[
* q \cdot q = \boldsymbol q_V \cdot \boldsymbol q_V + q_S^2
* @f]
* @see dot(const Quaternion<T>&, const Quaternion<T>&)
*/
inline T dot() const {
return dot(*this, *this);
}
/**
* @brief %Quaternion length
*
* See also dot() const which is faster for comparing length with other
* values. @f[
* |q| = \sqrt{q \cdot q}
* @f]
*/
inline T length() const {
return std::sqrt(dot());
}
/** @brief Normalized quaternion (of length 1) */
inline Quaternion<T> normalized() const {
return (*this)/length();
}
/**
* @brief Conjugated quaternion
*
* @f[
* q^* = [-\boldsymbol q_V, q_S]
* @f]
*/
inline Quaternion<T> conjugated() const {
return {-_vector, _scalar};
}
/**
* @brief Inverted quaternion
*
* See invertedNormalized() which is faster for normalized
* quaternions. @f[
* q^{-1} = \frac{q^*}{|q|^2} = \frac{[-\boldsymbol q_V, q_S]}{q \cdot q}
* @f]
*/
inline Quaternion<T> inverted() const {
return conjugated()/dot();
}
/**
* @brief Inverted normalized quaternion
*
* Equivalent to conjugated(). Expects that the quaternion is
* normalized. @f[
* q^{-1} = q^* = [-\boldsymbol q_V, q_S]
* @f]
*/
inline Quaternion<T> invertedNormalized() const {
CORRADE_ASSERT(MathTypeTraits<T>::equals(dot(), T(1)),
"Math::Quaternion::invertedNormalized(): quaternion must be normalized",
Quaternion<T>({}, std::numeric_limits<T>::quiet_NaN()));
return conjugated();
}
/**
* @brief Rotate vector with quaternion
*
* See transformVectorNormalized(), which is faster for normalized
* quaternions. @f[
* v' = qvq^{-1} = q [\boldsymbol v, 0] q^{-1}
* @f]
* @see Quaternion(const Vector3&), Matrix4::transformVector(), DualQuaternion::transformPoint()
*/
inline Vector3<T> transformVector(const Vector3<T>& vector) const {
return ((*this)*Quaternion<T>(vector)*inverted()).vector();
}
/**
* @brief Rotate vector with normalized quaternion
*
* Faster alternative to transformVector(), expects that the quaternion is
* normalized. @f[
* v' = qvq^{-1} = qvq^* = q [\boldsymbol v, 0] q^*
* @f]
* @see Quaternion(const Vector3&), Matrix4::transformVector(), DualQuaternion::transformPointNormalized()
*/
inline Vector3<T> transformVectorNormalized(const Vector3<T>& vector) const {
CORRADE_ASSERT(MathTypeTraits<T>::equals(dot(), T(1)),
"Math::Quaternion::transformVectorNormalized(): quaternion must be normalized",
Vector3<T>(std::numeric_limits<T>::quiet_NaN()));
return ((*this)*Quaternion<T>(vector)*conjugated()).vector();
}
private:
/* Used to avoid including Functions.h */
inline constexpr static T pow2(T value) {
return value*value;
}
/* Used in angle() and slerp() (no assertions) */
inline static T angleInternal(const Quaternion<T>& normalizedA, const Quaternion<T>& normalizedB) {
return std::acos(dot(normalizedA, normalizedB));
}
Vector3<T> _vector;
T _scalar;
};
/** @relates Quaternion
@brief Multiply scalar with quaternion
Same as Quaternion::operator*(T) const.
*/
template<class T> inline Quaternion<T> operator*(T scalar, const Quaternion<T>& quaternion) {
return quaternion*scalar;
}
/** @relates Quaternion
@brief Divide quaternion with number and invert
@f[
\frac a q = [\frac a {\boldsymbol q_V}, \frac a {q_S}]
@f]
@see Quaternion::operator/()
*/
template<class T> inline Quaternion<T> operator/(T scalar, const Quaternion<T>& quaternion) {
return {scalar/quaternion.vector(), scalar/quaternion.scalar()};
}
/** @debugoperator{Magnum::Math::Quaternion} */
template<class T> Corrade::Utility::Debug operator<<(Corrade::Utility::Debug debug, const Quaternion<T>& value) {
debug << "Quaternion({";
debug.setFlag(Corrade::Utility::Debug::SpaceAfterEachValue, false);
debug << value.vector().x() << ", " << value.vector().y() << ", " << value.vector().z() << "}, " << value.scalar() << ")";
debug.setFlag(Corrade::Utility::Debug::SpaceAfterEachValue, true);
return debug;
}
/* Explicit instantiation for commonly used types */
#ifndef DOXYGEN_GENERATING_OUTPUT
extern template Corrade::Utility::Debug MAGNUM_EXPORT operator<<(Corrade::Utility::Debug, const Quaternion<float>&);
#ifndef MAGNUM_TARGET_GLES
extern template Corrade::Utility::Debug MAGNUM_EXPORT operator<<(Corrade::Utility::Debug, const Quaternion<double>&);
#endif
#endif
}}
#endif