diff --git a/src/Math/Quaternion.h b/src/Math/Quaternion.h index 5b3bb6aa4..0280732e8 100644 --- a/src/Math/Quaternion.h +++ b/src/Math/Quaternion.h @@ -366,7 +366,7 @@ template class Quaternion { * * Equivalent to conjugated(). Expects that the quaternion is * normalized. @f[ - * q^{-1} = q^* = [-\boldsymbol q_V, q_S] ~~~~~ |q| = 1 + * q^{-1} = q^* = [-\boldsymbol q_V, q_S] * @f] */ inline Quaternion invertedNormalized() const { @@ -376,6 +376,33 @@ template class Quaternion { return conjugated(); } + /** + * @brief Rotate vector with quaternion + * + * @f[ + * v' = qvq^{-1} = q [\boldsymbol v, 0] q^{-1} + * @f] + * @see rotateVectorNormalized() + */ + inline Vector3 rotateVector(const Vector3& vector) const { + return ((*this)*Quaternion(vector)*inverted()).vector(); + } + + /** + * @brief Rotate vector with normalized quaternion + * + * Faster alternative to rotateVector(), expects that the quaternion is + * normalized. @f[ + * v' = qvq^{-1} = qvq^* = q [\boldsymbol v, 0] q^* + * @f] + */ + inline Vector3 rotateVectorNormalized(const Vector3& vector) const { + CORRADE_ASSERT(MathTypeTraits::equals(dot(), T(1)), + "Math::Quaternion::rotateVectorNormalized(): quaternion must be normalized", + Vector3(std::numeric_limits::quiet_NaN())); + return ((*this)*Quaternion(vector)*conjugated()).vector(); + } + private: /* Used in angle() and slerp() (no assertions) */ inline static T angleInternal(const Quaternion& normalizedA, const Quaternion& normalizedB) { diff --git a/src/Math/Test/QuaternionTest.cpp b/src/Math/Test/QuaternionTest.cpp index 1503152dc..2954c0c58 100644 --- a/src/Math/Test/QuaternionTest.cpp +++ b/src/Math/Test/QuaternionTest.cpp @@ -45,6 +45,8 @@ class QuaternionTest: public Corrade::TestSuite::Tester { void matrix(); void lerp(); void slerp(); + void rotateVector(); + void rotateVectorNormalized(); void debug(); }; @@ -72,6 +74,8 @@ QuaternionTest::QuaternionTest() { &QuaternionTest::matrix, &QuaternionTest::lerp, &QuaternionTest::slerp, + &QuaternionTest::rotateVector, + &QuaternionTest::rotateVectorNormalized, &QuaternionTest::debug); } @@ -257,6 +261,32 @@ void QuaternionTest::slerp() { CORRADE_COMPARE(slerp, Quaternion({0.119165f, 0.0491109f, 0.0491109f}, 0.990442f)); } +void QuaternionTest::rotateVector() { + Quaternion a = Quaternion::fromRotation(deg(23.0f), Vector3::xAxis()); + Vector3 v(0.0f, -3.6f, 0.7f); + + Vector3 rotated = a.rotateVector(v); + CORRADE_COMPARE(Vector3::angle(v.normalized(), rotated.normalized()), deg(23.0f)); + CORRADE_COMPARE(Vector3::cross(v, rotated).normalized(), Vector3::xAxis()); + CORRADE_COMPARE(rotated.length(), v.length()); +} + +void QuaternionTest::rotateVectorNormalized() { + Quaternion a = Quaternion::fromRotation(deg(23.0f), Vector3::xAxis()); + Vector3 v(0.0f, -3.6f, 0.7f); + + std::ostringstream o; + Corrade::Utility::Error::setOutput(&o); + Vector3 notRotated = (a*2).rotateVectorNormalized(v); + CORRADE_VERIFY(notRotated != notRotated); + CORRADE_COMPARE(o.str(), "Math::Quaternion::rotateVectorNormalized(): quaternion must be normalized\n"); + + Vector3 rotated = a.rotateVector(v); + CORRADE_COMPARE(Vector3::angle(v.normalized(), rotated.normalized()), deg(23.0f)); + CORRADE_COMPARE(Vector3::cross(v, rotated).normalized(), Vector3::xAxis()); + CORRADE_COMPARE(rotated.length(), v.length()); +} + void QuaternionTest::debug() { std::ostringstream o;