| /* |
| * Copyright 2014 Google Inc. All rights reserved. |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| #ifndef MATHFU_QUATERNION_H_ |
| #define MATHFU_QUATERNION_H_ |
| |
| #ifdef _WIN32 |
| #if !defined(_USE_MATH_DEFINES) |
| #define _USE_MATH_DEFINES // For M_PI. |
| #endif // !defined(_USE_MATH_DEFINES) |
| #endif // _WIN32 |
| |
| #include "mathfu/matrix.h" |
| #include "mathfu/vector.h" |
| |
| #include <math.h> |
| |
| /// @file mathfu/quaternion.h |
| /// @brief Quaternion class and functions. |
| /// @addtogroup mathfu_quaternion |
| /// |
| /// MathFu provides a Quaternion class that utilizes SIMD optimized |
| /// Matrix and Vector classes. |
| |
| namespace mathfu { |
| |
| /// @addtogroup mathfu_quaternion |
| /// @{ |
| /// @class Quaternion |
| /// |
| /// @brief Stores a Quaternion of type T and provides a set of utility |
| /// operations on each Quaternion. |
| /// @tparam T Type of each element in the Quaternion. |
| template <class T> |
| class Quaternion { |
| public: |
| /// @brief Construct an uninitialized Quaternion. |
| inline Quaternion() {} |
| |
| /// @brief Construct a Quaternion from a copy. |
| /// @param q Quaternion to copy. |
| inline Quaternion(const Quaternion<T>& q) { |
| s_ = q.s_; |
| v_ = q.v_; |
| } |
| |
| /// @brief Construct a Quaternion using scalar values to initialize each |
| /// element. |
| /// |
| /// @param s1 Scalar component. |
| /// @param s2 First element of the Vector component. |
| /// @param s3 Second element of the Vector component. |
| /// @param s4 Third element of the Vector component. |
| inline Quaternion(const T& s1, const T& s2, const T& s3, const T& s4) { |
| s_ = s1; |
| v_ = Vector<T, 3>(s2, s3, s4); |
| } |
| |
| /// @brief Construct a quaternion from a scalar and 3-dimensional Vector. |
| /// |
| /// @param s1 Scalar component. |
| /// @param v1 Vector component. |
| inline Quaternion(const T& s1, const Vector<T, 3>& v1) { |
| s_ = s1; |
| v_ = v1; |
| } |
| |
| /// @brief Return the scalar component of the quaternion. |
| /// |
| /// @return The scalar component |
| inline T& scalar() { return s_; } |
| |
| /// @brief Return the scalar component of the quaternion. |
| /// |
| /// @return The scalar component |
| inline const T& scalar() const { return s_; } |
| |
| /// @brief Set the scalar component of the quaternion. |
| /// |
| /// @param s Scalar component. |
| inline void set_scalar(const T& s) { s_ = s; } |
| |
| /// @brief Return the vector component of the quaternion. |
| /// |
| /// @return The scalar component |
| inline Vector<T, 3>& vector() { return v_; } |
| |
| /// @brief Return the vector component of the quaternion. |
| /// |
| /// @return The scalar component |
| inline const Vector<T, 3>& vector() const { return v_; } |
| |
| /// @brief Set the vector component of the quaternion. |
| /// |
| /// @param v Vector component. |
| inline void set_vector(const Vector<T, 3>& v) { v_ = v; } |
| |
| /// @brief Calculate the inverse Quaternion. |
| /// |
| /// This calculates the inverse such that <code>(q * q).Inverse()</code> |
| /// is the identity. |
| /// |
| /// @return Quaternion containing the result. |
| inline Quaternion<T> Inverse() const { return Quaternion<T>(s_, -v_); } |
| |
| /// @brief Multiply this Quaternion with another Quaternion. |
| /// |
| /// @note This is equivalent to |
| /// <code>FromMatrix(ToMatrix() * q.ToMatrix()).</code> |
| /// @param q Quaternion to multiply with. |
| /// @return Quaternion containing the result. |
| inline Quaternion<T> operator*(const Quaternion<T>& q) const { |
| return Quaternion<T>( |
| s_ * q.s_ - Vector<T, 3>::DotProduct(v_, q.v_), |
| s_ * q.v_ + q.s_ * v_ + Vector<T, 3>::CrossProduct(v_, q.v_)); |
| } |
| |
| /// @brief Multiply this Quaternion by a scalar. |
| /// |
| /// This multiplies the angle of the rotation by a scalar factor. |
| /// @param s1 Scalar to multiply with. |
| /// @return Quaternion containing the result. |
| inline Quaternion<T> operator*(const T& s1) const { |
| T angle; |
| Vector<T, 3> axis; |
| ToAngleAxis(&angle, &axis); |
| angle *= s1; |
| return Quaternion<T>(cos(0.5f * angle), |
| axis.Normalized() * static_cast<T>(sin(0.5f * angle))); |
| } |
| |
| /// @brief Multiply a Vector by this Quaternion. |
| /// |
| /// This will rotate the specified vector by the rotation specified by this |
| /// Quaternion. |
| /// |
| /// @param v1 Vector to multiply by this Quaternion. |
| /// @return Rotated Vector. |
| inline Vector<T, 3> operator*(const Vector<T, 3>& v1) const { |
| T ss = s_ + s_; |
| return ss * Vector<T, 3>::CrossProduct(v_, v1) + (ss * s_ - 1) * v1 + |
| 2 * Vector<T, 3>::DotProduct(v_, v1) * v_; |
| } |
| |
| /// @brief Normalize this quaterion (in-place). |
| /// |
| /// @return Length of the quaternion. |
| inline T Normalize() { |
| T length = sqrt(s_ * s_ + Vector<T, 3>::DotProduct(v_, v_)); |
| T scale = (1 / length); |
| s_ *= scale; |
| v_ *= scale; |
| return length; |
| } |
| |
| /// @brief Calculate the normalized version of this quaternion. |
| /// |
| /// @return The normalized quaternion. |
| inline Quaternion<T> Normalized() const { |
| Quaternion<T> q(*this); |
| q.Normalize(); |
| return q; |
| } |
| |
| /// @brief Convert this Quaternion to an Angle and axis. |
| /// |
| /// The returned angle is the size of the rotation in radians about the |
| /// axis represented by this Quaternion. |
| /// |
| /// @param angle Receives the angle. |
| /// @param axis Receives the normalized axis. |
| inline void ToAngleAxis(T* angle, Vector<T, 3>* axis) const { |
| *axis = s_ > 0 ? v_ : -v_; |
| *angle = 2 * atan2(axis->Normalize(), s_ > 0 ? s_ : -s_); |
| } |
| |
| /// @brief Convert this Quaternion to 3 Euler Angles. |
| /// |
| /// @return 3-dimensional Vector where each element is a angle of rotation |
| /// (in radians) around the x, y, and z axes. |
| inline Vector<T, 3> ToEulerAngles() const { |
| Matrix<T, 3> m(ToMatrix()); |
| T cos2 = m[0] * m[0] + m[1] * m[1]; |
| if (cos2 < 1e-6f) { |
| return Vector<T, 3>( |
| 0, |
| m[2] < 0 ? static_cast<T>(0.5 * M_PI) : static_cast<T>(-0.5 * M_PI), |
| -std::atan2(m[3], m[4])); |
| } else { |
| return Vector<T, 3>(std::atan2(m[5], m[8]), |
| std::atan2(-m[2], std::sqrt(cos2)), |
| std::atan2(m[1], m[0])); |
| } |
| } |
| |
| /// @brief Convert to a 3x3 Matrix. |
| /// |
| /// @return 3x3 rotation Matrix. |
| inline Matrix<T, 3> ToMatrix() const { |
| const T x2 = v_[0] * v_[0], y2 = v_[1] * v_[1], z2 = v_[2] * v_[2]; |
| const T sx = s_ * v_[0], sy = s_ * v_[1], sz = s_ * v_[2]; |
| const T xz = v_[0] * v_[2], yz = v_[1] * v_[2], xy = v_[0] * v_[1]; |
| return Matrix<T, 3>(1 - 2 * (y2 + z2), 2 * (xy + sz), 2 * (xz - sy), |
| 2 * (xy - sz), 1 - 2 * (x2 + z2), 2 * (sx + yz), |
| 2 * (sy + xz), 2 * (yz - sx), 1 - 2 * (x2 + y2)); |
| } |
| |
| /// @brief Convert to a 4x4 Matrix. |
| /// |
| /// @return 4x4 transform Matrix. |
| inline Matrix<T, 4> ToMatrix4() const { |
| const T x2 = v_[0] * v_[0], y2 = v_[1] * v_[1], z2 = v_[2] * v_[2]; |
| const T sx = s_ * v_[0], sy = s_ * v_[1], sz = s_ * v_[2]; |
| const T xz = v_[0] * v_[2], yz = v_[1] * v_[2], xy = v_[0] * v_[1]; |
| return Matrix<T, 4>(1 - 2 * (y2 + z2), 2 * (xy + sz), 2 * (xz - sy), 0.0f, |
| 2 * (xy - sz), 1 - 2 * (x2 + z2), 2 * (sx + yz), 0.0f, |
| 2 * (sy + xz), 2 * (yz - sx), 1 - 2 * (x2 + y2), 0.0f, |
| 0.0f, 0.0f, 0.0f, 1.0f); |
| } |
| |
| /// @brief Create a Quaternion from an angle and axis. |
| /// |
| /// @param angle Angle in radians to rotate by. |
| /// @param axis Axis in 3D space to rotate around. |
| /// @return Quaternion containing the result. |
| static Quaternion<T> FromAngleAxis(const T& angle, const Vector<T, 3>& axis) { |
| const T halfAngle = static_cast<T>(0.5) * angle; |
| Vector<T, 3> localAxis(axis); |
| return Quaternion<T>( |
| cos(halfAngle), |
| localAxis.Normalized() * static_cast<T>(sin(halfAngle))); |
| } |
| |
| /// @brief Create a quaternion from 3 euler angles. |
| /// |
| /// @param angles 3-dimensional Vector where each element contains an |
| /// angle in radius to rotate by about the x, y and z axes. |
| /// @return Quaternion containing the result. |
| static Quaternion<T> FromEulerAngles(const Vector<T, 3>& angles) { |
| const Vector<T, 3> halfAngles(static_cast<T>(0.5) * angles[0], |
| static_cast<T>(0.5) * angles[1], |
| static_cast<T>(0.5) * angles[2]); |
| const T sinx = std::sin(halfAngles[0]); |
| const T cosx = std::cos(halfAngles[0]); |
| const T siny = std::sin(halfAngles[1]); |
| const T cosy = std::cos(halfAngles[1]); |
| const T sinz = std::sin(halfAngles[2]); |
| const T cosz = std::cos(halfAngles[2]); |
| return Quaternion<T>(cosx * cosy * cosz + sinx * siny * sinz, |
| sinx * cosy * cosz - cosx * siny * sinz, |
| cosx * siny * cosz + sinx * cosy * sinz, |
| cosx * cosy * sinz - sinx * siny * cosz); |
| } |
| |
| /// @brief Create a quaternion from a rotation Matrix. |
| /// |
| /// @param m 3x3 rotation Matrix. |
| /// @return Quaternion containing the result. |
| static Quaternion<T> FromMatrix(const Matrix<T, 3>& m) { |
| const T trace = m(0, 0) + m(1, 1) + m(2, 2); |
| if (trace > 0) { |
| const T s = sqrt(trace + 1) * 2; |
| const T oneOverS = 1 / s; |
| return Quaternion<T>(static_cast<T>(0.25) * s, (m[5] - m[7]) * oneOverS, |
| (m[6] - m[2]) * oneOverS, (m[1] - m[3]) * oneOverS); |
| } else if (m[0] > m[4] && m[0] > m[8]) { |
| const T s = sqrt(m[0] - m[4] - m[8] + 1) * 2; |
| const T oneOverS = 1 / s; |
| return Quaternion<T>((m[5] - m[7]) * oneOverS, static_cast<T>(0.25) * s, |
| (m[3] + m[1]) * oneOverS, (m[6] + m[2]) * oneOverS); |
| } else if (m[4] > m[8]) { |
| const T s = sqrt(m[4] - m[0] - m[8] + 1) * 2; |
| const T oneOverS = 1 / s; |
| return Quaternion<T>((m[6] - m[2]) * oneOverS, (m[3] + m[1]) * oneOverS, |
| static_cast<T>(0.25) * s, (m[5] + m[7]) * oneOverS); |
| } else { |
| const T s = sqrt(m[8] - m[0] - m[4] + 1) * 2; |
| const T oneOverS = 1 / s; |
| return Quaternion<T>((m[1] - m[3]) * oneOverS, (m[6] + m[2]) * oneOverS, |
| (m[5] + m[7]) * oneOverS, static_cast<T>(0.25) * s); |
| } |
| } |
| |
| /// @brief Calculate the dot product of two Quaternions. |
| /// |
| /// @param q1 First quaternion. |
| /// @param q2 Second quaternion |
| /// @return The scalar dot product of both Quaternions. |
| static inline T DotProduct(const Quaternion<T>& q1, const Quaternion<T>& q2) { |
| return q1.s_ * q2.s_ + Vector<T, 3>::DotProduct(q1.v_, q2.v_); |
| } |
| |
| /// @brief Calculate the spherical linear interpolation between two |
| /// Quaternions. |
| /// |
| /// @param q1 Start Quaternion. |
| /// @param q2 End Quaternion. |
| /// @param s1 The scalar value determining how far from q1 and q2 the |
| /// resulting quaternion should be. A value of 0 corresponds to q1 and a |
| /// value of 1 corresponds to q2. |
| /// @result Quaternion containing the result. |
| static inline Quaternion<T> Slerp(const Quaternion<T>& q1, |
| const Quaternion<T>& q2, const T& s1) { |
| if (q1.s_ * q2.s_ + Vector<T, 3>::DotProduct(q1.v_, q2.v_) > 0.999999f) |
| return Quaternion<T>(q1.s_ * (1 - s1) + q2.s_ * s1, |
| q1.v_ * (1 - s1) + q2.v_ * s1); |
| return q1 * ((q1.Inverse() * q2) * s1); |
| } |
| |
| /// @brief Access an element of the quaternion. |
| /// |
| /// @param i Index of the element to access. |
| /// @return A reference to the accessed data that can be modified by the |
| /// caller. |
| inline T& operator[](const int i) { |
| if (i == 0) return s_; |
| return v_[i - 1]; |
| } |
| |
| /// @brief Access an element of the quaternion. |
| /// |
| /// @param i Index of the element to access. |
| /// @return A const reference to the accessed. |
| inline const T& operator[](const int i) const { |
| return const_cast<Quaternion<T>*>(this)->operator[](i); |
| } |
| |
| /// @brief Returns a vector that is perpendicular to the supplied vector. |
| /// |
| /// @param v1 An arbitrary vector |
| /// @return A vector perpendicular to v1. Normally this will just be |
| /// the cross product of v1, v2. If they are parallel or opposite though, |
| /// the routine will attempt to pick a vector. |
| static inline Vector<T, 3> PerpendicularVector(const Vector<T, 3>& v) { |
| // We start out by taking the cross product of the vector and the x-axis to |
| // find something parallel to the input vectors. If that cross product |
| // turns out to be length 0 (i. e. the vectors already lie along the x axis) |
| // then we use the y-axis instead. |
| Vector<T, 3> axis = Vector<T, 3>::CrossProduct( |
| Vector<T, 3>(static_cast<T>(1), static_cast<T>(0), static_cast<T>(0)), |
| v); |
| // We use a fairly high epsilon here because we know that if this number |
| // is too small, the axis we'll get from a cross product with the y axis |
| // will be much better and more numerically stable. |
| if (axis.LengthSquared() < static_cast<T>(0.05)) { |
| axis = Vector<T, 3>::CrossProduct( |
| Vector<T, 3>(static_cast<T>(0), static_cast<T>(1), static_cast<T>(0)), |
| v); |
| } |
| return axis; |
| } |
| |
| /// @brief Returns the a Quaternion that rotates from start to end. |
| /// |
| /// @param v1 The starting vector |
| /// @param v2 The vector to rotate to |
| /// @param preferred_axis the axis to use, if v1 and v2 are parallel. |
| /// @return A Quaternion describing the rotation from v1 to v2 |
| /// See the comment on RotateFromToWithAxis for an explanation of the math. |
| static inline Quaternion<T> RotateFromToWithAxis( |
| const Vector<T, 3>& v1, const Vector<T, 3>& v2, |
| const Vector<T, 3>& preferred_axis) { |
| Vector<T, 3> start = v1.Normalized(); |
| Vector<T, 3> end = v2.Normalized(); |
| |
| T dot_product = Vector<T, 3>::DotProduct(start, end); |
| // Any rotation < 0.1 degrees is treated as no rotation |
| // in order to avoid division by zero errors. |
| // So we early-out in cases where it's less then 0.1 degrees. |
| // cos( 0.1 degrees) = 0.99999847691 |
| if (dot_product >= static_cast<T>(0.99999847691)) { |
| return Quaternion<T>::identity; |
| } |
| // If the vectors point in opposite directions, return a 180 degree |
| // rotation, on the axis that they asked for. |
| if (dot_product <= static_cast<T>(-0.99999847691)) { |
| return Quaternion<T>(static_cast<T>(0), preferred_axis); |
| } |
| // Degenerate cases have been handled, so if we're here, we have to |
| // actually compute the angle we want: |
| Vector<T, 3> cross_product = Vector<T, 3>::CrossProduct(start, end); |
| |
| return Quaternion<T>(static_cast<T>(1.0) + dot_product, cross_product) |
| .Normalized(); |
| } |
| |
| /// @brief Returns the a Quaternion that rotates from start to end. |
| /// |
| /// @param v1 The starting vector |
| /// @param v2 The vector to rotate to |
| /// @return A Quaternion describing the rotation from v1 to v2. In the case |
| /// where the vectors are parallel, it returns the identity. In the case |
| /// where |
| /// they point in opposite directions, it picks an arbitrary axis. (Since |
| /// there |
| /// are technically infinite possible quaternions to represent a 180 degree |
| /// rotation.) |
| /// |
| /// The final equation used here is fairly elegant, but its derivation is |
| /// not obvious: We want to find the quaternion that represents the angle |
| /// between Start and End. |
| /// |
| /// The angle can be expressed as a quaternion with the values: |
| /// angle: ArcCos(dotproduct(start, end) / (|start|*|end|) |
| /// axis: crossproduct(start, end).normalized * sin(angle/2) |
| /// |
| /// or written as: |
| /// quaternion(cos(angle/2), axis * sin(angle/2)) |
| /// |
| /// Using the trig identity: |
| /// sin(angle * 2) = 2 * sin(angle) * cos*angle) |
| /// Via substitution, we can turn this into: |
| /// sin(angle/2) = 0.5 * sin(angle)/cos(angle/2) |
| /// |
| /// Using this substitution, we get: |
| /// quaternion( cos(angle/2), |
| /// 0.5 * crossproduct(start, end).normalized |
| /// * sin(angle) / cos(angle/2)) |
| /// |
| /// If we scale the whole thing up by 2 * cos(angle/2) then we get: |
| /// quaternion(2 * cos(angle/2) * cos(angle/2), |
| /// crossproduct(start, end).normalized * sin(angle)) |
| /// |
| /// (Note that the quaternion is no longer normalized after this scaling) |
| /// |
| /// Another trig identity: |
| /// cos(angle/2) = sqrt((1 + cos(angle) / 2) |
| /// |
| /// Substituting this in, we can simplify the quaternion scalar: |
| /// |
| /// quaternion(1 + cos(angle), |
| /// crossproduct(start, end).normalized * sin(angle)) |
| /// |
| /// Because cross(start, end) has a magnitude of |start|*|end|*sin(angle), |
| /// crossproduct(start,end).normalized |
| /// is equivalent to |
| /// crossproduct(start,end) / |start| * |end| * sin(angle) |
| /// So after that substitution: |
| /// |
| /// quaternion(1 + cos(angle), |
| /// crossproduct(start, end) / (|start| * |end|)) |
| /// |
| /// dotproduct(start, end) has the value of |start| * |end| * cos(angle), |
| /// so by algebra, |
| /// cos(angle) = dotproduct(start, end) / (|start| * |end|) |
| /// we can replace our quaternion scalar here also: |
| /// |
| /// quaternion(1 + dotproduct(start, end) / (|start| * |end|), |
| /// crossproduct(start, end) / (|start| * |end|)) |
| /// |
| /// If start and end are normalized, then |start| * |end| = 1, giving us a |
| /// final quaternion of: |
| /// |
| /// quaternion(1 + dotproduct(start, end), crossproduct(start, end)) |
| static inline Quaternion<T> RotateFromTo(const Vector<T, 3>& v1, |
| const Vector<T, 3>& v2) { |
| Vector<T, 3> start = v1.Normalized(); |
| Vector<T, 3> end = v2.Normalized(); |
| |
| T dot_product = Vector<T, 3>::DotProduct(start, end); |
| // Any rotation < 0.1 degrees is treated as no rotation |
| // in order to avoid division by zero errors. |
| // So we early-out in cases where it's less then 0.1 degrees. |
| // cos( 0.1 degrees) = 0.99999847691 |
| if (dot_product >= static_cast<T>(0.99999847691)) { |
| return Quaternion<T>::identity; |
| } |
| // If the vectors point in opposite directions, return a 180 degree |
| // rotation, on an arbitrary axis. |
| if (dot_product <= static_cast<T>(-0.99999847691)) { |
| return Quaternion<T>(0, PerpendicularVector(start)); |
| } |
| // Degenerate cases have been handled, so if we're here, we have to |
| // actually compute the angle we want: |
| Vector<T, 3> cross_product = Vector<T, 3>::CrossProduct(start, end); |
| |
| return Quaternion<T>(static_cast<T>(1.0) + dot_product, cross_product) |
| .Normalized(); |
| } |
| |
| /// @brief Contains a quaternion doing the identity transform. |
| static Quaternion<T> identity; |
| |
| MATHFU_DEFINE_CLASS_SIMD_AWARE_NEW_DELETE |
| |
| private: |
| T s_; |
| Vector<T, 3> v_; |
| }; |
| |
| template <typename T> |
| Quaternion<T> Quaternion<T>::identity = Quaternion<T>(1, 0, 0, 0); |
| /// @} |
| |
| /// @addtogroup mathfu_quaternion |
| /// @{ |
| |
| /// @brief Multiply a Quaternion by a scalar. |
| /// |
| /// This multiplies the angle of the rotation of the specified Quaternion |
| /// by a scalar factor. |
| /// @param s Scalar to multiply with. |
| /// @param q Quaternion to scale. |
| /// @return Quaternion containing the result. |
| /// |
| /// @related Quaternion |
| template <class T> |
| inline Quaternion<T> operator*(const T& s, const Quaternion<T>& q) { |
| return q * s; |
| } |
| /// @} |
| |
| } // namespace mathfu |
| #endif // MATHFU_QUATERNION_H_ |