diff --git a/include/zeus/CQuaternion.hpp b/include/zeus/CQuaternion.hpp index dc893cf..be7c3c5 100644 --- a/include/zeus/CQuaternion.hpp +++ b/include/zeus/CQuaternion.hpp @@ -19,9 +19,9 @@ namespace zeus static inline float normalize_angle(float angle) { if (angle > M_PIF) - angle = -((2.f * angle) - M_PIF); + angle -= 2.f * M_PIF; else if (angle < -M_PIF) - angle = 2.f * angle + M_PIF; + angle += 2.f * M_PIF; return angle; } @@ -90,74 +90,7 @@ public: #endif - CQuaternion(const CMatrix3f& mat) - { - float trace = mat[0][0] + mat[1][1] + mat[2][2]; - if (trace >= 0.f) - { - float st = std::sqrt(trace + 1.0f); - float s = 0.5f / st; - w = 0.5f * st; - x = (mat[1][2] - mat[2][1]) * s; - y = (mat[2][0] - mat[0][2]) * s; - z = (mat[0][1] - mat[1][0]) * s; - } - else - { - int idx = 0; - if (mat[1][1] > mat[0][0]) - { - idx = 1; - if (mat[2][2] > mat[1][1]) - idx = 2; - } - else if (mat[2][2] > mat[0][0]) - { - idx = 2; - } - - switch (idx) - { - case 0: - { - float st = std::sqrt(mat[0][0] - (mat[1][1] + mat[2][2]) + 1.f); - float s = 0.5f / st; - w = (mat[1][2] - mat[2][1]) * s; - x = 0.5f * st; - y = (mat[1][0] + mat[0][1]) * s; - z = (mat[2][0] + mat[0][2]) * s; - break; - } - case 1: - { - float st = std::sqrt(mat[1][1] - (mat[2][2] + mat[0][0]) + 1.f); - float s = 0.5f / st; - w = (mat[2][0] - mat[0][2]) * s; - x = (mat[1][0] + mat[0][1]) * s; - y = 0.5f * st; - z = (mat[2][1] + mat[1][2]) * s; - break; - } - case 2: - { - float st = std::sqrt(mat[2][2] - (mat[0][0] + mat[1][1]) + 1.f); - float s = 0.5f / st; - w = (mat[0][1] - mat[1][0]) * s; - x = (mat[2][0] + mat[0][2]) * s; - y = (mat[2][1] + mat[1][2]) * s; - z = 0.5f * st; - break; - } - default: - w = 0.f; - x = 0.f; - y = 0.f; - z = 0.f; - break; - } - } - } - + CQuaternion(const CMatrix3f& mat); CQuaternion(const CVector3f& vec) { fromVector3f(vec); } CQuaternion(const CVector4f& vec) { @@ -224,40 +157,7 @@ public: return {q.x, q.y, q.z}; } - static inline CQuaternion lookAt(const CUnitVector3f& target, const CUnitVector3f& up, const CRelAngle& c) - { - CQuaternion q = skNoRotation; - zeus::CVector3f upCpy = up; - zeus::CVector3f targetCpy = target; - upCpy.z = 0.f; - targetCpy.z = 0.f; - zeus::CVector3f tmp; - if (upCpy.magnitude() > 0.0009f && upCpy.magnitude() > 0.0009f) - { - targetCpy.normalize(); - upCpy.normalize(); - - CRelAngle angleBetween = - normalize_angle(std::atan2(targetCpy.x, targetCpy.y) - std::atan2(upCpy.x, upCpy.y)); - CRelAngle realAngle = zeus::clamp(-c, angleBetween, c); - CQuaternion tmpQ; - tmpQ.rotateZ(realAngle); - q = tmpQ; - CQuaternion q2 = (q * CQuaternion{0.f, targetCpy}) * -tmpQ; - tmp.x = q2.x; - tmp.y = q2.y; - tmp.z = q2.z; - } - else if (upCpy.magnitude() > 0.0009f) - tmp = targetCpy.normalized(); - else if (upCpy.magnitude() > 0.0009f) - tmp = upCpy.normalized(); - else - return skNoRotation; - - CRelAngle realAngle = zeus::clamp(-c, normalize_angle(std::acos(up.z) - std::acos(target.z)), c); - return CQuaternion::fromAxisAngle(tmp.cross(CVector3f::skUp), realAngle) * q; - } + static CQuaternion lookAt(const CUnitVector3f& source, const CUnitVector3f& dest, const CRelAngle& maxAng); CVector3f transform(const CVector3f& v) const { diff --git a/include/zeus/CRelAngle.hpp b/include/zeus/CRelAngle.hpp index fc0f68e..76f3843 100644 --- a/include/zeus/CRelAngle.hpp +++ b/include/zeus/CRelAngle.hpp @@ -17,6 +17,8 @@ struct CRelAngle static float MakeRelativeAngle(float angle) { float absAngle = std::fabs(angle); + if (absAngle == 2.f * M_PIF) + return std::copysign(absAngle, angle); float ret = absAngle - std::floor(absAngle / (2.f * M_PIF)) * (2.f * M_PIF); if (ret < 0.f) ret += 2.f * M_PIF; diff --git a/src/CQuaternion.cpp b/src/CQuaternion.cpp index b8bf16a..19747f2 100644 --- a/src/CQuaternion.cpp +++ b/src/CQuaternion.cpp @@ -5,6 +5,74 @@ namespace zeus { const CQuaternion CQuaternion::skNoRotation; +CQuaternion::CQuaternion(const CMatrix3f& mat) +{ + float trace = mat[0][0] + mat[1][1] + mat[2][2]; + if (trace >= 0.f) + { + float st = std::sqrt(trace + 1.0f); + float s = 0.5f / st; + w = 0.5f * st; + x = (mat[1][2] - mat[2][1]) * s; + y = (mat[2][0] - mat[0][2]) * s; + z = (mat[0][1] - mat[1][0]) * s; + } + else + { + int idx = 0; + if (mat[1][1] > mat[0][0]) + { + idx = 1; + if (mat[2][2] > mat[1][1]) + idx = 2; + } + else if (mat[2][2] > mat[0][0]) + { + idx = 2; + } + + switch (idx) + { + case 0: + { + float st = std::sqrt(mat[0][0] - (mat[1][1] + mat[2][2]) + 1.f); + float s = 0.5f / st; + w = (mat[1][2] - mat[2][1]) * s; + x = 0.5f * st; + y = (mat[1][0] + mat[0][1]) * s; + z = (mat[2][0] + mat[0][2]) * s; + break; + } + case 1: + { + float st = std::sqrt(mat[1][1] - (mat[2][2] + mat[0][0]) + 1.f); + float s = 0.5f / st; + w = (mat[2][0] - mat[0][2]) * s; + x = (mat[1][0] + mat[0][1]) * s; + y = 0.5f * st; + z = (mat[2][1] + mat[1][2]) * s; + break; + } + case 2: + { + float st = std::sqrt(mat[2][2] - (mat[0][0] + mat[1][1]) + 1.f); + float s = 0.5f / st; + w = (mat[0][1] - mat[1][0]) * s; + x = (mat[2][0] + mat[0][2]) * s; + y = (mat[2][1] + mat[1][2]) * s; + z = 0.5f * st; + break; + } + default: + w = 0.f; + x = 0.f; + y = 0.f; + z = 0.f; + break; + } + } +} + void CQuaternion::fromVector3f(const CVector3f& vec) { float cosX = std::cos(0.5f * vec.x); @@ -295,4 +363,37 @@ CRelAngle CQuaternion::angleFrom(const zeus::CQuaternion& other) return std::acos(zeus::clamp(-1.f, dot(other), 1.f)); } +CQuaternion CQuaternion::lookAt(const CUnitVector3f& source, const CUnitVector3f& dest, const CRelAngle& maxAng) +{ + CQuaternion q = skNoRotation; + zeus::CVector3f destNoZ = dest; + zeus::CVector3f sourceNoZ = source; + destNoZ.z = 0.f; + sourceNoZ.z = 0.f; + zeus::CVector3f tmp; + if (sourceNoZ.magSquared() > 0.0001f && destNoZ.magSquared() > 0.0001f) + { + sourceNoZ.normalize(); + destNoZ.normalize(); + + float angleBetween = + normalize_angle(std::atan2(destNoZ.x, destNoZ.y) - std::atan2(sourceNoZ.x, sourceNoZ.y)); + float realAngle = zeus::clamp(-maxAng.asRadians(), angleBetween, maxAng.asRadians()); + CQuaternion tmpQ; + tmpQ.rotateZ(-realAngle); + q = tmpQ; + tmp = q.transform(sourceNoZ); + } + else if (sourceNoZ.magSquared() > 0.0001f) + tmp = sourceNoZ.normalized(); + else if (destNoZ.magSquared() > 0.0001f) + tmp = destNoZ.normalized(); + else + return skNoRotation; + + float realAngle = + zeus::clamp(-maxAng.asRadians(), normalize_angle(std::acos(dest.z) - std::acos(source.z)), maxAng.asRadians()); + return CQuaternion::fromAxisAngle(tmp.cross(CVector3f::skUp), -realAngle) * q; +} + }