diff --git a/include/zeus/CQuaternion.hpp b/include/zeus/CQuaternion.hpp index 0d21d00..8629e43 100644 --- a/include/zeus/CQuaternion.hpp +++ b/include/zeus/CQuaternion.hpp @@ -259,6 +259,9 @@ public: static CQuaternion slerp(const CQuaternion& a, const CQuaternion& b, double t); static CQuaternion slerpShort(const CQuaternion& a, const CQuaternion& b, double t); static CQuaternion nlerp(const CQuaternion& a, const CQuaternion& b, double t); + static CQuaternion shortestRotationArc(const zeus::CVector3f& v0, const zeus::CVector3f& v1); + static CQuaternion clampedRotateTo(const zeus::CVector3f& v0, const zeus::CVector3f& v1, + const zeus::CRelAngle& angle); inline float roll() const { return std::atan2(2.f * (x * y + w * z), w * w + x * x - y * y - z * z); } diff --git a/src/CQuaternion.cpp b/src/CQuaternion.cpp index 7d7a339..d6ccb9b 100644 --- a/src/CQuaternion.cpp +++ b/src/CQuaternion.cpp @@ -196,16 +196,53 @@ CQuaternion CQuaternion::slerp(const CQuaternion& a, const CQuaternion& b, doubl const double d = 1.0 / std::sin(theta); const double s0 = std::sin((1.0 - t) * theta); - ret.x = (float)(a.x * s0 + b.x * s1) * d; - ret.y = (float)(a.y * s0 + b.y * s1) * d; - ret.z = (float)(a.z * s0 + b.z * s1) * d; - ret.w = (float)(a.w * s0 + b.w * s1) * d; + ret.x = float((a.x * s0 + b.x * s1) * d); + ret.y = float((a.y * s0 + b.y * s1) * d); + ret.z = float((a.z * s0 + b.z * s1) * d); + ret.w = float((a.w * s0 + b.w * s1) * d); return ret; } return a; } +CQuaternion CQuaternion::shortestRotationArc(const zeus::CVector3f& v0, const zeus::CVector3f& v1) +{ + CVector3f v0N = v0; + CVector3f v1N = v1; + + if (!v0N.isZero()) + v0N.normalize(); + if (!v1N.isZero()) + v1N.normalize(); + + CVector3f cross = v0N.cross(v1N); + + if (cross.magSquared() < 0.001f) + { + if (v0N.dot(v1N) > 0.f) + return CQuaternion::skNoRotation; + if (cross.canBeNormalized()) + return CQuaternion(0.0f, cross.normalized()); + return CQuaternion::skNoRotation; + } + else + { + float w = (1.f + zeus::clamp(-1.f, v0N.dot(v1N), 1.f)) * 2.f; + return CQuaternion(0.5f * w, cross * (1.f / std::sqrt(w))); + } +} + +CQuaternion CQuaternion::clampedRotateTo(const zeus::CVector3f& v0, const zeus::CVector3f& v1, + const zeus::CRelAngle& angle) +{ + CQuaternion arc = shortestRotationArc(v0, v1); + if (angle >= 2.f * std::acos(arc.w)) + return arc; + + return fromAxisAngle(arc.getImaginary(), angle); +} + CQuaternion CQuaternion::slerpShort(const CQuaternion& a, const CQuaternion& b, double t) { return zeus::CQuaternion::slerp((b.dot(a) >= 0.f) ? a : a.buildEquivalent(), b, t);