diff --git a/src/Math/Quaternion.h b/src/Math/Quaternion.h index 83f62f6f4..5498ca159 100644 --- a/src/Math/Quaternion.h +++ b/src/Math/Quaternion.h @@ -33,6 +33,22 @@ namespace Magnum { namespace Math { /** @brief %Quaternion */ template class Quaternion { public: + /** + * @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] + */ + inline static Quaternion lerp(const Quaternion& normalizedA, const Quaternion& normalizedB, T t) { + CORRADE_ASSERT(MathTypeTraits::equals(normalizedA.lengthSquared(), T(1)) && MathTypeTraits::equals(normalizedB.lengthSquared(), T(1)), + "Math::Quaternion::lerp(): quaternions must be normalized", Quaternion({}, std::numeric_limits::quiet_NaN())); + return ((T(1) - t)*normalizedA + t*normalizedB).normalized(); + } + /** * @brief Create quaternion from rotation * @param angle Rotation angle (counterclockwise, in radians) diff --git a/src/Math/Test/MathQuaternionTest.cpp b/src/Math/Test/MathQuaternionTest.cpp index 99512883e..20414b0d6 100644 --- a/src/Math/Test/MathQuaternionTest.cpp +++ b/src/Math/Test/MathQuaternionTest.cpp @@ -38,6 +38,7 @@ class QuaternionTest: public Corrade::TestSuite::Tester { void invertedNormalized(); void rotation(); void matrix(); + void lerp(); void debug(); }; @@ -58,6 +59,7 @@ QuaternionTest::QuaternionTest() { &QuaternionTest::invertedNormalized, &QuaternionTest::rotation, &QuaternionTest::matrix, + &QuaternionTest::lerp, &QuaternionTest::debug); } @@ -166,6 +168,28 @@ void QuaternionTest::matrix() { CORRADE_COMPARE((-q).matrix(), expected); } +void QuaternionTest::lerp() { + Quaternion a = Quaternion::fromRotation(deg(15.0f), Vector3(1.0f/Constants::sqrt3())); + Quaternion b = Quaternion::fromRotation(deg(23.0f), Vector3::xAxis()); + + std::ostringstream o; + Corrade::Utility::Error::setOutput(&o); + + Quaternion notLerpA = Quaternion::lerp(a*3.0f, b, 0.35f); + CORRADE_COMPARE(notLerpA.vector(), Vector3()); + CORRADE_COMPARE(notLerpA.scalar(), std::numeric_limits::quiet_NaN()); + CORRADE_COMPARE(o.str(), "Math::Quaternion::lerp(): quaternions must be normalized\n"); + + o.str(""); + Quaternion notLerpB = Quaternion::lerp(a, b*-3.0f, 0.35f); + CORRADE_COMPARE(notLerpB.vector(), Vector3()); + CORRADE_COMPARE(notLerpB.scalar(), std::numeric_limits::quiet_NaN()); + CORRADE_COMPARE(o.str(), "Math::Quaternion::lerp(): quaternions must be normalized\n"); + + Quaternion lerp = Quaternion::lerp(a, b, 0.35f); + CORRADE_COMPARE(lerp, Quaternion({0.119127f, 0.049134f, 0.049134f}, 0.990445f)); +} + void QuaternionTest::debug() { std::ostringstream o;