diff --git a/Gems/EMotionFX/Code/MCore/Source/Quaternion.cpp b/Gems/EMotionFX/Code/MCore/Source/Quaternion.cpp deleted file mode 100644 index 2f7db53d2c..0000000000 --- a/Gems/EMotionFX/Code/MCore/Source/Quaternion.cpp +++ /dev/null @@ -1,604 +0,0 @@ -/* - * Copyright (c) Contributors to the Open 3D Engine Project. - * For complete copyright and license terms please see the LICENSE at the root of this distribution. - * - * SPDX-License-Identifier: Apache-2.0 OR MIT - * - */ - -// include required headers -#include "Quaternion.h" -#include - -namespace MCore -{ - // spherical quadratic interpolation - Quaternion Quaternion::Squad(const Quaternion& p, const Quaternion& a, const Quaternion& b, const Quaternion& q, float t) - { - Quaternion q0(p.Slerp(q, t)); - Quaternion q1(a.Slerp(b, t)); - return q0.Slerp(q1, 2.0f * t * (1.0f - t)); - } - - - // returns the approximately normalized linear interpolated result [t must be between 0..1] - Quaternion Quaternion::NLerp(const Quaternion& to, float t) const - { - AZ_Assert(t > -MCore::Math::epsilon && t < (1 + MCore::Math::epsilon), "Expected t to be between 0..1"); - static const float weightCloseToOne = 1.0f - MCore::Math::epsilon; - - // Early out for boundaries (common cases) - if (t < MCore::Math::epsilon) - { - return *this; - } - else if (t > weightCloseToOne) - { - return to; - } - - #if AZ_TRAIT_USE_PLATFORM_SIMD_SSE - __m128 num1, num2, num3, num4, fromVec, toVec; - const float omt = 1.0f - t; - float dot; - - // perform dot product between this quat and the 'to' quat - num4 = _mm_setzero_ps(); // sets sum to zero - fromVec = _mm_loadu_ps(&x); // - toVec = _mm_loadu_ps(&to.x); // - num3 = _mm_mul_ps(fromVec, toVec); // performs multiplication num3 = a[3]*b[3] a[2]*b[2] a[1]*b[1] a[0]*b[0] - num3 = _mm_hadd_ps(num3, num3); // performs horizontal addition - num3= a[3]*b[3]+ a[2]*b[2] a[1]*b[1]+a[0]*b[0] a[3]*b[3]+ a[2]*b[2] a[1]*b[1]+a[0]*b[0] - num4 = _mm_add_ps(num4, num3); // performs vertical addition - num4 = _mm_hadd_ps(num4, num4); - _mm_store_ss(&dot, num4); // store the dot result - - if (dot < 0.0f) - { - t = -t; - } - - // calculate interpolated value - num2 = _mm_load_ps1(&omt); - num3 = _mm_load_ps1(&t); - num4 = _mm_mul_ps(fromVec, num2); // omt * xyzw - num1 = _mm_mul_ps(toVec, num3); // t * to.xyzw - num2 = _mm_add_ps(num1, num4); // interpolated value - - // calculate the square length - num4 = _mm_setzero_ps(); - num3 = _mm_mul_ps(num2, num2); // square length - num1 = _mm_hadd_ps(num3, num3); - num4 = _mm_add_ps(num4, num1); - num3 = _mm_hadd_ps(num4, num4); - //num4 = _mm_rsqrt_ps( num3 ); // length (argh, too inaccurate on some models) - - AZStd::aligned_storage::type numFloatStorage; - float* numFloat = reinterpret_cast(&numFloatStorage); - - _mm_store_ps(numFloat, num3); - const float invLen = Math::InvSqrt(numFloat[0]); - num4 = _mm_load_ps1(&invLen); - - // calc inverse length, which normalizes everything - num1 = _mm_mul_ps(num2, num4); - - _mm_store_ps(numFloat, num1); - return Quaternion(numFloat[0], numFloat[1], numFloat[2], numFloat[3]); - #else - const float omt = 1.0f - t; - const float dot = x * to.x + y * to.y + z * to.z + w * to.w; - if (dot < 0.0f) - { - t = -t; - } - - // calculate the interpolated values - const float newX = (omt * x + t * to.x); - const float newY = (omt * y + t * to.y); - const float newZ = (omt * z + t * to.z); - const float newW = (omt * w + t * to.w); - - // calculate the inverse length - // const float invLen = 1.0f / Math::FastSqrt( newX*newX + newY*newY + newZ*newZ + newW*newW ); - // const float invLen = Math::FastInvSqrt( newX*newX + newY*newY + newZ*newZ + newW*newW ); - const float invLen = Math::InvSqrt(newX * newX + newY * newY + newZ * newZ + newW * newW); - - // return the normalized linear interpolation - return Quaternion(newX * invLen, - newY * invLen, - newZ * invLen, - newW * invLen); - #endif - } - - - - // returns the linear interpolated result [t must be between 0..1] - Quaternion Quaternion::Lerp(const Quaternion& to, float t) const - { - const float omt = 1.0f - t; - const float cosom = x * to.x + y * to.y + z * to.z + w * to.w; - if (cosom < 0.0f) - { - t = -t; - } - - // return the linear interpolation - return Quaternion(omt * x + t * to.x, - omt * y + t * to.y, - omt * z + t * to.z, - omt * w + t * to.w); - } - - - - // quaternion from an axis and angle - Quaternion::Quaternion(const AZ::Vector3& axis, float angle) - { - const float squaredLength = axis.GetLengthSq(); - if (squaredLength > 0.0f) - { - const float halfAngle = angle * 0.5f; - const float sinScale = Math::Sin(halfAngle) / Math::Sqrt(squaredLength); - x = axis.GetX() * sinScale; - y = axis.GetY() * sinScale; - z = axis.GetZ() * sinScale; - w = Math::Cos(halfAngle); - } - else - { - x = y = z = 0.0f; - w = 1.0f; - } - } - - - - // quaternion from a spherical rotation - Quaternion::Quaternion(const AZ::Vector2& spherical, float angle) - { - const float latitude = spherical.GetX(); - const float longitude = spherical.GetY(); - - const float s = Math::Sin(angle / 2.0f); - const float c = Math::Cos(angle / 2.0f); - - const float sin_lat = Math::Sin(latitude); - const float cos_lat = Math::Cos(latitude); - - const float sin_lon = Math::Sin(longitude); - const float cos_lon = Math::Cos(longitude); - - x = s * cos_lat * sin_lon; - y = s * sin_lat; - z = s * sin_lat * cos_lon; - w = c; - } - - - // convert to an axis and angle - void Quaternion::ToAxisAngle(AZ::Vector3* axis, float* angle) const - { - *angle = 2.0f * Math::ACos(w); - - const float sinHalfAngle = Math::Sin(*angle * 0.5f); - if (sinHalfAngle > 0.0f) - { - const float invS = 1.0f / sinHalfAngle; - axis->Set(x * invS, y * invS, z * invS); - } - else - { - axis->Set(0.0f, 1.0f, 0.0f); - *angle = 0.0f; - } - } - - - // converts from unit quaternion to spherical rotation angles - void Quaternion::ToSpherical(AZ::Vector2* spherical, float* angle) const - { - AZ::Vector3 axis; - ToAxisAngle(&axis, angle); - - float longitude; - if (axis.GetX() * axis.GetX() + axis.GetZ() * axis.GetZ() < 0.0001f) - { - longitude = 0.0f; - } - else - { - longitude = Math::ATan2(axis.GetX(), axis.GetZ()); - if (longitude < 0.0f) - { - longitude += Math::twoPi; - } - } - - spherical->SetX(-Math::ASin(axis.GetY())); - spherical->SetY(longitude); - } - - - - // setup the quaternion from a roll, pitch and yaw - Quaternion& Quaternion::SetEuler(float pitch, float yaw, float roll) - { - // METHOD #1: - const float halfYaw = yaw * 0.5f; - const float halfPitch = pitch * 0.5f; - const float halfRoll = roll * 0.5f; - - const float cY = Math::Cos(halfYaw); - const float sY = Math::Sin(halfYaw); - const float cP = Math::Cos(halfPitch); - const float sP = Math::Sin(halfPitch); - const float cR = Math::Cos(halfRoll); - const float sR = Math::Sin(halfRoll); - - x = cY * sP * cR - sY * cP * sR; - y = cY * sP * sR + sY * cP * cR; - z = cY * cP * sR - sY * sP * cR; - w = cY * cP * cR + sY * sP * sR; - - // Normalize(); // we might be able to leave the normalize away, but better safe than not, this is more robust :) - - return *this; - - /* - - // METHOD #2: - Quaternion Qx(Vector3(sP, 0, 0), cP); - Quaternion Qy(Vector3(0, sY, 0), cY); - Quaternion Qz(Vector3(0, 0, sR), cR); - - Quaternion result = Qx * Qy * Qz; - - x = result.x; - y = result.y; - z = result.z; - w = result.w; - - return *this; - */ - } - - - - // convert the quaternion to a matrix - Matrix Quaternion::ToMatrix() const - { - Matrix m; - - const float xx = x * x; - const float xy = x * y, yy = y * y; - const float xz = x * z, yz = y * z, zz = z * z; - const float xw = x * w, yw = y * w, zw = z * w, ww = w * w; - - MMAT(m, 0, 0) = +xx - yy - zz + ww; - MMAT(m, 0, 1) = +xy + zw + xy + zw; - MMAT(m, 0, 2) = +xz - yw + xz - yw; - MMAT(m, 0, 3) = 0.0f; - MMAT(m, 1, 0) = +xy - zw + xy - zw; - MMAT(m, 1, 1) = -xx + yy - zz + ww; - MMAT(m, 1, 2) = +yz + xw + yz + xw; - MMAT(m, 1, 3) = 0.0f; - MMAT(m, 2, 0) = +xz + yw + xz + yw; - MMAT(m, 2, 1) = +yz - xw + yz - xw; - MMAT(m, 2, 2) = -xx - yy + zz + ww; - MMAT(m, 2, 3) = 0.0f; - MMAT(m, 3, 0) = 0.0f; - MMAT(m, 3, 1) = 0.0f; - MMAT(m, 3, 2) = 0.0f; - MMAT(m, 3, 3) = 1.0f; - - return m; - } - - - - // construct the quaternion from a given rotation matrix - Quaternion Quaternion::ConvertFromMatrix(const Matrix& m) - { - Quaternion result; - - const float trace = MMAT(m, 0, 0) + MMAT(m, 1, 1) + MMAT(m, 2, 2); - if (trace > 0.0f /*Math::epsilon*/) - { - const float s = 0.5f / Math::Sqrt(trace + 1.0f); - result.w = 0.25f / s; - result.x = (MMAT(m, 1, 2) - MMAT(m, 2, 1)) * s; - result.y = (MMAT(m, 2, 0) - MMAT(m, 0, 2)) * s; - result.z = (MMAT(m, 0, 1) - MMAT(m, 1, 0)) * s; - } - else - { - if (MMAT(m, 0, 0) > MMAT(m, 1, 1) && MMAT(m, 0, 0) > MMAT(m, 2, 2)) - { - const float s = 2.0f * Math::Sqrt(1.0f + MMAT(m, 0, 0) - MMAT(m, 1, 1) - MMAT(m, 2, 2)); - const float oneOverS = 1.0f / s; - result.x = 0.25f * s; - result.y = (MMAT(m, 1, 0) + MMAT(m, 0, 1)) * oneOverS; - result.z = (MMAT(m, 2, 0) + MMAT(m, 0, 2)) * oneOverS; - result.w = (MMAT(m, 1, 2) - MMAT(m, 2, 1)) * oneOverS; - } - else - if (MMAT(m, 1, 1) > MMAT(m, 2, 2)) - { - const float s = 2.0f * Math::Sqrt(1.0f + MMAT(m, 1, 1) - MMAT(m, 0, 0) - MMAT(m, 2, 2)); - const float oneOverS = 1.0f / s; - result.x = (MMAT(m, 1, 0) + MMAT(m, 0, 1)) * oneOverS; - result.y = 0.25f * s; - result.z = (MMAT(m, 2, 1) + MMAT(m, 1, 2)) * oneOverS; - result.w = (MMAT(m, 2, 0) - MMAT(m, 0, 2)) * oneOverS; - } - else - { - const float s = 2.0f * Math::Sqrt(1.0f + MMAT(m, 2, 2) - MMAT(m, 0, 0) - MMAT(m, 1, 1)); - const float oneOverS = 1.0f / s; - result.x = (MMAT(m, 2, 0) + MMAT(m, 0, 2)) * oneOverS; - result.y = (MMAT(m, 2, 1) + MMAT(m, 1, 2)) * oneOverS; - result.z = 0.25f * s; - result.w = (MMAT(m, 0, 1) - MMAT(m, 1, 0)) * oneOverS; - } - } - - /* - const float trace = MMAT(m,0,0) + MMAT(m,1,1) + MMAT(m,2,2) + 1.0f; - if (trace > Math::epsilon) - { - const float s = 0.5f / Math::Sqrt(trace); - result.w = 0.25f / s; - result.x = ( MMAT(m,1,2) - MMAT(m,2,1) ) * s; - result.y = ( MMAT(m,2,0) - MMAT(m,0,2) ) * s; - result.z = ( MMAT(m,0,1) - MMAT(m,1,0) ) * s; - } - else - { - if (MMAT(m,0,0) > MMAT(m,1,1) && MMAT(m,0,0) > MMAT(m,2,2)) - { - const float s = 2.0f * Math::Sqrt( 1.0f + MMAT(m,0,0) - MMAT(m,1,1) - MMAT(m,2,2)); - const float oneOverS = 1.0f / s; - result.x = 0.25f * s; - result.y = (MMAT(m,1,0) + MMAT(m,0,1) ) * oneOverS; - result.z = (MMAT(m,2,0) + MMAT(m,0,2) ) * oneOverS; - result.w = (MMAT(m,2,1) - MMAT(m,1,2) ) * oneOverS; - } - else - if (MMAT(m,1,1) > MMAT(m,2,2)) - { - const float s = 2.0f * Math::Sqrt( 1.0f + MMAT(m,1,1) - MMAT(m,0,0) - MMAT(m,2,2)); - const float oneOverS = 1.0f / s; - result.x = (MMAT(m,1,0) + MMAT(m,0,1) ) * oneOverS; - result.y = 0.25f * s; - result.z = (MMAT(m,2,1) + MMAT(m,1,2) ) * oneOverS; - result.w = (MMAT(m,2,0) - MMAT(m,0,2) ) * oneOverS; - } - else - { - const float s = 2.0f * Math::Sqrt( 1.0f + MMAT(m,2,2) - MMAT(m,0,0) - MMAT(m,1,1) ); - const float oneOverS = 1.0f / s; - result.x = (MMAT(m,2,0) + MMAT(m,0,2) ) * oneOverS; - result.y = (MMAT(m,2,1) + MMAT(m,1,2) ) * oneOverS; - result.z = 0.25f * s; - result.w = (MMAT(m,1,0) - MMAT(m,0,1) ) * oneOverS; - } - } - */ - return result; - } - - - // convert a quaternion to euler angles (in degrees) - AZ::Vector3 Quaternion::ToEuler() const - { - /* - // METHOD #1: - - Vector3 euler; - - float matrix[3][3]; - float cx,sx; - float cy,sy,yr; - float cz,sz; - - matrix[0][0] = 1.0 - (2.0 * y * y) - (2.0 * z * z); - matrix[1][0] = (2.0 * x * y) + (2.0 * w * z); - matrix[2][0] = (2.0 * x * z) - (2.0 * w * y); - matrix[2][1] = (2.0 * y * z) + (2.0 * w * x); - matrix[2][2] = 1.0 - (2.0 * x * x) - (2.0 * y * y); - - sy = -matrix[2][0]; - cy = Math::Sqrt(1 - (sy * sy)); - yr = Math::ATan2(sy,cy); - euler.y = yr; - - // avoid divide by zero only where y ~90 or ~270 - if (sy != 1.0 && sy != -1.0) - { - cx = matrix[2][2] / cy; - sx = matrix[2][1] / cy; - euler.x = Math::ATan2(sx,cx); - - cz = matrix[0][0] / cy; - sz = matrix[1][0] / cy; - euler.z = Math::ATan2(sz,cz); - } - else - { - matrix[1][1] = 1.0 - (2.0 * x * x) - (2.0 * z * z); - matrix[1][2] = (2.0 * y * z) - (2.0 * w * x); - cx = matrix[1][1]; - sx = -matrix[1][2]; - euler.x = Math::ATan2(sx,cx); - - cz = 1.0; - sz = 0.0; - euler.z = Math::ATan2(sz,cz); - } - - return euler; - */ - - /* - // METHOD #2: - Matrix mat = ToMatrix(); - - // - float cy = Math::Sqrt(mat.m44[0][0]*mat.m44[0][0] + mat.m44[0][1]*mat.m44[0][1]); - if (cy > 16.0*Math::epsilon) - { - result.x = -atan2(mat.m44[1][2], mat.m44[2][2]); - result.y = -atan2(-mat.m44[0][2], cy); - result.z = -atan2(mat.m44[0][1], mat.m44[0][0]); - } - else - { - result.x = -atan2(-mat.m44[2][1], mat.m44[1][1]); - result.y = -atan2(-mat.m44[0][2], cy); - result.z = 0.0; - } - - return result; - */ - - // METHOD #3 (without conversion to matrix first): - // TODO: safety checks? - float m00 = 1.0f - (2.0f * ((y * y) + z * z)); - float m01 = 2.0f * (x * y + w * z); - - AZ::Vector3 result( - Math::ATan2(2.0f * (y * z + w * x), 1.0f - (2.0f * ((x * x) + (y * y)))), - Math::ATan2(-2.0f * (x * z - w * y), Math::Sqrt((m00 * m00) + (m01 * m01))), - Math::ATan2(m01, m00) - ); - - return result; - } - - float Quaternion::GetEulerZ() const - { - float m00 = 1.0f - (2.0f * ((y * y) + z * z)); - float m01 = 2.0f * (x * y + w * z); - return Math::ATan2(m01, m00); - } - - // returns the spherical interpolated result [t must be between 0..1] - Quaternion Quaternion::Slerp(const Quaternion& to, float t) const - { - float cosom = (x * to.x) + (y * to.y) + (z * to.z) + (w * to.w); - float scale0, scale1, scale1sign = 1.0f; - - if (cosom < 0.0f) - { - scale1sign = -1.0f; - cosom *= -1.0f; - } - - if ((1.0 - cosom) > Math::epsilon) - { - const float omega = Math::ACos(cosom); - const float sinOmega = Math::Sin(omega); - const float oosinom = 1.0f / sinOmega; - scale0 = Math::Sin((1.0f - t) * omega) * oosinom; - scale1 = Math::Sin(t * omega) * oosinom; - } - else - { - scale0 = 1.0f - t; - scale1 = t; - } - - scale1 *= scale1sign; - - return Quaternion(scale0 * x + scale1 * to.x, - scale0 * y + scale1 * to.y, - scale0 * z + scale1 * to.z, - scale0 * w + scale1 * to.w); - } - - - // set as delta rotation - Quaternion Quaternion::CreateDeltaRotation(const AZ::Vector3& fromVector, const AZ::Vector3& toVector) - { - Quaternion q; - q.SetAsDeltaRotation(fromVector, toVector); - return q; - } - - - // set as delta rotation but limited - Quaternion Quaternion::CreateDeltaRotation(const AZ::Vector3& fromVector, const AZ::Vector3& toVector, float maxAngleRadians) - { - Quaternion q; - q.SetAsDeltaRotation(fromVector, toVector, maxAngleRadians); - return q; - } - - - // set as delta rotation - void Quaternion::SetAsDeltaRotation(const AZ::Vector3& fromVector, const AZ::Vector3& toVector) - { - // check if we are in parallel or not - const float dot = fromVector.Dot(toVector); - if (dot < 0.99999f) // we have rotated compared to the forward direction - { - const float angleRadians = Math::ACos(dot); - const AZ::Vector3 rotAxis = fromVector.Cross(toVector); - *this = Quaternion(rotAxis, angleRadians); - } - else - { - Identity(); - } - } - - - // set as delta rotation, but limited - void Quaternion::SetAsDeltaRotation(const AZ::Vector3& fromVector, const AZ::Vector3& toVector, float maxAngleRadians) - { - // check if we are in parallel or not - const float dot = fromVector.Dot(toVector); - if (dot < 0.99999f) // we have rotated compared to the forward direction - { - const float angleRadians = Math::ACos(dot); - const float rotAngle = Min(angleRadians, maxAngleRadians); - const AZ::Vector3 rotAxis = fromVector.Cross(toVector); - *this = Quaternion(rotAxis, rotAngle); - } - else - { - Identity(); - } - } - - - /* - Decompose the rotation on to 2 parts. - 1. Twist - rotation around the "direction" vector - 2. Swing - rotation around axis that is perpendicular to "direction" vector - The rotation can be composed back by - rotation = swing * twist - - has singularity in case of swing_rotation close to 180 degrees rotation. - if the input quaternion is of non-unit length, the outputs are non-unit as well - otherwise, outputs are both unit - */ - void Quaternion::DecomposeSwingTwist(const AZ::Vector3& direction, Quaternion* outSwing, Quaternion* outTwist) const - { - AZ::Vector3 rotAxis(x, y, z); - AZ::Vector3 p = Projected(rotAxis, direction); // return projection v1 on to v2 (parallel component) - outTwist->Set(p.GetX(), p.GetY(), p.GetZ(), w); - outTwist->Normalize(); - *outSwing = *this * outTwist->Conjugated(); - } - - - // rotate the current quaternion and renormalize it - void Quaternion::RotateFromTo(const AZ::Vector3& fromVector, const AZ::Vector3& toVector) - { - *this = CreateDeltaRotation(fromVector, toVector) * *this; - Normalize(); - } -} // namespace MCore - diff --git a/Gems/EMotionFX/Code/MCore/Source/Quaternion.h b/Gems/EMotionFX/Code/MCore/Source/Quaternion.h deleted file mode 100644 index bbee1d8265..0000000000 --- a/Gems/EMotionFX/Code/MCore/Source/Quaternion.h +++ /dev/null @@ -1,386 +0,0 @@ -/* - * Copyright (c) Contributors to the Open 3D Engine Project. - * For complete copyright and license terms please see the LICENSE at the root of this distribution. - * - * SPDX-License-Identifier: Apache-2.0 OR MIT - * - */ - -#pragma once - -// include required headers -#include -#include -#include "StandardHeaders.h" -#include "FastMath.h" -#include "Vector.h" -#include "Matrix4.h" -#include "Algorithms.h" - - -namespace MCore -{ - /** - * Depracated. Please use AZ::Quaternion instead. - * The quaternion class in MCore. - * Quaternions are mostly used to represent rotations in 3D applications. - * The advantages of quaternions over matrices are that they take up less space and that interpolation between - * two quaternions is easier to perform. Instead of a 3x3 rotation matrix, which is 9 floats or doubles, a quaternion - * only uses 4 floats or doubles. This template/class provides you with methods to perform all kind of operations on - * these quaternions, from interpolation to conversion to matrices and other rotation representations. - */ - class MCORE_API Quaternion - { - public: - AZ_TYPE_INFO(MCore::Quaternion, "{1807CD22-EBB5-45E8-8113-3B1DABB53F12}") - - /** - * Default constructor. Sets x, y and z to 0 and w to 1. - */ - MCORE_INLINE Quaternion() - : x(0.0f) - , y(0.0f) - , z(0.0f) - , w(1.0f) {} - - /** - * Constructor which sets the x, y, z and w. - * @param xVal The value of x. - * @param yVal The value of y. - * @param zVal The value of z. - * @param wVal The value of w. - */ - MCORE_INLINE Quaternion(float xVal, float yVal, float zVal, float wVal) - : x(xVal) - , y(yVal) - , z(zVal) - , w(wVal) {} - - /** - * Copy constructor. Copies the x, y, z, w values from the other quaternion. - * @param other The quaternion to copy the attributes from. - */ - MCORE_INLINE Quaternion(const Quaternion& other) - : x(other.x) - , y(other.y) - , z(other.z) - , w(other.w) {} - - /** - * Constructor which creates a quaternion from a pitch, yaw and roll. - * @param pitch Rotation around the x-axis, in radians. - * @param yaw Rotation around the y-axis, in radians. - * @param roll Rotation around the z-axis, in radians. - */ - MCORE_INLINE Quaternion(float pitch, float yaw, float roll) { SetEuler(pitch, yaw, roll); } - - /** - * Constructor which takes a matrix as input parameter. - * This converts the rotation of the specified matrix into a quaternion. Please keep in mind that the matrix may NOT contain - * any scaling, so if it does, please normalize your matrix first! - * @param matrix The matrix to initialize the quaternion from. - */ - MCORE_INLINE Quaternion(const Matrix& matrix) { FromMatrix(matrix); } - - /** - * Constructor which creates a quaternion from a spherical rotation. - * @param spherical The spherical coordinates in radians, which creates an axis to rotate around. - * @param angle The angle to rotate around this axis. - */ - Quaternion(const AZ::Vector2& spherical, float angle); - - /** - * Constructor which creates a quaternion from an axis and angle. - * @param axis The axis to rotate around. - * @param angle The angle in radians to rotate around the given axis. - */ - Quaternion(const AZ::Vector3& axis, float angle); - - /** - * Set the quaternion x/y/z/w component values. - * @param vx The value of x. - * @param vy The value of y. - * @param vz The value of z. - * @param vw The value of w. - */ - MCORE_INLINE void Set(float vx, float vy, float vz, float vw) { x = vx; y = vy; z = vz; w = vw; } - - /** - * Calculates the square length of the quaternion. - * @result The square length (length*length). - */ - MCORE_INLINE float SquareLength() const { return (x * x + y * y + z * z + w * w); } - - /** - * Calculates the length of the quaternion. - * It's safe, since it prevents a division by 0. - * @result The length of the quaternion. - */ - MCORE_INLINE float Length() const; - - /** - * Performs a dot product on the quaternions. - * @param q The quaternion to multiply (dot product) this quaternion with. - * @result The quaternion which is the result of the dot product. - */ - MCORE_INLINE float Dot(const Quaternion& q) const { return (x * q.x + y * q.y + z * q.z + w * q.w); } - - /** - * Normalize the quaternion. - * @result The normalized quaternion. It modifies itself, so no new quaternion is returned. - */ - MCORE_INLINE Quaternion& Normalize(); - - /** - * Sets the quaternion to identity. Where x, y and z are set to 0 and w is set to 1. - * @result The quaternion, now set to identity. - */ - MCORE_INLINE Quaternion& Identity() { x = 0.0f; y = 0.0f; z = 0.0f; w = 1.0f; return *this; } - - /** - * Calculate the inversed version of this quaternion. - * @result The inversed version of this quaternion. - */ - MCORE_INLINE Quaternion& Inverse() { const float len = 1.0f / SquareLength(); x = -x * len; y = -y * len; z = -z * len; w = w * len; return *this; } - - /** - * Conjugate this quaternion. - * @result Returns itself Conjugated. - */ - MCORE_INLINE Quaternion& Conjugate() { x = -x; y = -y; z = -z; return *this; } - - /** - * Calculate the inversed version of this quaternion. - * @result The inversed version of this quaternion. - */ - MCORE_INLINE Quaternion Inversed() const { const float len = 1.0f / SquareLength(); return Quaternion(-x * len, -y * len, -z * len, w * len); } - - /** - * Returns the normalized version of this quaternion. - * @result The normalized version of this quaternion. - */ - MCORE_INLINE Quaternion Normalized() const { Quaternion result(*this); result.Normalize(); return result; } - - /** - * Return the conjugated version of this quaternion. - * @result The conjugated version of this quaternion. - */ - MCORE_INLINE Quaternion Conjugated() const { return Quaternion(-x, -y, -z, w); } - - /** - * Calculate the exponent of this quaternion. - * @result The resulting quaternion of the exp. - */ - MCORE_INLINE Quaternion Exp() const { const float r = Math::Sqrt(x * x + y * y + z * z); const float expW = Math::Exp(w); const float s = (r >= 0.00001f) ? expW* Math::Sin(r) / r : 0.0f; return Quaternion(s * x, s * y, s * z, expW * Math::Cos(r)); } - - /** - * Calculate the log of the quaternion. - * @result The resulting quaternion of the log. - */ - MCORE_INLINE Quaternion LogN() const { const float r = Math::Sqrt(x * x + y * y + z * z); float t = (r > 0.00001f) ? Math::ATan2(r, w) / r : 0.0f; return Quaternion(t * x, t * y, t * z, 0.5f * Math::Log(SquareLength())); } - - /** - * Calculate and get the right basis vector. - * @result The basis vector pointing to the right. This assumes x+ points to the right. - */ - MCORE_INLINE AZ::Vector3 CalcRightAxis() const; - - /** - * Calculate and get the up basis vector. - * @result The basis vector pointing upwards. This assumes z+ points up. - */ - MCORE_INLINE AZ::Vector3 CalcUpAxis() const; - - /** - * Calculate and get the forward basis vector. - * @result The basis vector pointing forward. This assumes y+ points forward, into the depth. - */ - MCORE_INLINE AZ::Vector3 CalcForwardAxis() const; - - /** - * Initialize the current quaternion from a specified matrix. - * Please note that the matrix may not contain any scaling! - * So make sure the matrix has been normalized before, if it contains any scale. - * @param m The matrix to initialize the quaternion from. - */ - MCORE_INLINE void FromMatrix(const Matrix& m) { *this = Quaternion::ConvertFromMatrix(m); } - - /** - * Setup the quaternion from a pitch, yaw and roll. - * @param pitch The rotation around the x-axis, in radians. - * @param yaw The rotation around the y-axis, in radians. - * @param roll The rotation around the z-axis in radians. - * @result The quaternion, now initialized with the given pitch, yaw, roll rotation. - */ - Quaternion& SetEuler(float pitch, float yaw, float roll); - - /** - * Convert the quaternion to an axis and angle. Which represents a rotation of the resulting angle around the resulting axis. - * @param axis Pointer to the vector to store the axis in. - * @param angle Pointer to the variable to store the angle in (will be in radians). - */ - void ToAxisAngle(AZ::Vector3* axis, float* angle) const; - - /** - * Convert the quaternion to a spherical rotation. - * @param spherical A pointer to the 2D vector to store the spherical coordinates in radians, which build the axis. - * @param angle The pointer to the variable to store the angle around this axis in radians. - */ - void ToSpherical(AZ::Vector2* spherical, float* angle) const; - - /** - * Extract the euler angles in radians. - * The x component of the resulting vector represents the rotation around the x-axis (pitch). - * The y component results the rotation around the y-axis (yaw) and the z component represents - * the rotation around the z-axis (roll). - * @result The 3D vector containing the euler angles in radians, around each axis. - */ - AZ::Vector3 ToEuler() const; - - /** - * Returns the angle of rotation about the z axis. This is same as - * the z component of the vector returned by the ToEuler method. It - * is just more efficient to call this when one is interested only in rotation about the z axis. - * @result The angle of rotation about z axis in radians. - */ - float GetEulerZ() const; - - /** - * Convert this quaternion into a matrix. - * @result The matrix representing the rotation of this quaternion. - */ - Matrix ToMatrix() const; - - /** - * Convert a matrix into a quaternion. - * Please keep in mind that the specified matrix may NOT contain any scaling! - * So make sure the matrix has been normalized before, if it contains any scale. - * @param m The matrix to extract the rotation from. - * @result The quaternion, now containing the rotation of the given matrix, in quaternion form. - */ - static Quaternion ConvertFromMatrix(const Matrix& m); - - /** - * Create a delta rotation that rotates one vector onto another vector. - * @param fromVector The normalized vector to start from. This must be normalized! - * @param toVector The normalized vector to rotate towards. This must be normalized as well! - * @result The delta rotation quaternion. - */ - static Quaternion CreateDeltaRotation(const AZ::Vector3& fromVector, const AZ::Vector3& toVector); - - /** - * Create a delta rotation that rotates one vector onto another vector. - * If the angle is bigger than the max allowed angle that is specified it will rotate with an angle of the maximum specified angle. - * So if the angle between the vectors is 40 degrees and you maxAngleRadians equals 10 degrees (in radians) it will only rotate 10 degrees. - * @param fromVector The normalized vector to start from. This must be normalized! - * @param toVector The normalized vector to rotate towards. This must be normalized as well! - * @param maxAngleRadians The maximum rotation angle on the plane defined by the two vectors. This cannot be more than Math::pi (180 degrees). - * @result The delta rotation quaternion. - */ - static Quaternion CreateDeltaRotation(const AZ::Vector3& fromVector, const AZ::Vector3& toVector, float maxAngleRadians); - - /** - * Init this quaternion as a delta rotation that rotates one vector onto another vector. - * @param fromVector The normalized vector to start from. This must be normalized! - * @param toVector The normalized vector to rotate towards. This must be normalized as well! - */ - void SetAsDeltaRotation(const AZ::Vector3& fromVector, const AZ::Vector3& toVector); - - /** - * Init this quaternion as a delta rotation that rotates one vector onto another vector. - * If the angle is bigger than the max allowed angle that is specified it will rotate with an angle of the maximum specified angle. - * So if the angle between the vectors is 40 degrees and you maxAngleRadians equals 10 degrees (in radians) it will only rotate 10 degrees. - * @param fromVector The normalized vector to start from. This must be normalized! - * @param toVector The normalized vector to rotate towards. This must be normalized as well! - * @param maxAngleRadians The maximum rotation angle on the plane defined by the two vectors. This cannot be more than Math::pi (180 degrees). - */ - void SetAsDeltaRotation(const AZ::Vector3& fromVector, const AZ::Vector3& toVector, float maxAngleRadians); - - /** - * Rotate this current quaternion using a given delta that is calculated from two vectors. - * The rotation axis used is the cross product between the from and to vector. The rotation angle is the angle between these two vectors. - * @param fromVector The current direction vector, must be normalized. - * @param toVector The desired new direction vector, must be normalized. - */ - void RotateFromTo(const AZ::Vector3& fromVector, const AZ::Vector3& toVector); - - /** - * Decompose into swing and twist. - * The original rotation quat can be reassembled by doing swing * twist. - * @param direction The direction vector to get the twist from. - * @param outSwing This will contain the swing quaternion. - * @param outTwist This will contain the twist quaternion. - */ - void DecomposeSwingTwist(const AZ::Vector3& direction, Quaternion* outSwing, Quaternion* outTwist) const; - - /** - * Linear interpolate between this and another quaternion. - * @param to The quaternion to interpolate towards. - * @param t The time value, between 0 and 1. - * @result The quaternion at the given time in the interpolation process. - */ - Quaternion Lerp(const Quaternion& to, float t) const; - - /** - * Linear interpolate between this and another quaternion, and normalize afterwards. - * @param to The quaternion to interpolate towards. - * @param t The time value, between 0 and 1. - * @result The normalized quaternion at the given time in the interpolation process. - */ - Quaternion NLerp(const Quaternion& to, float t) const; - - /** - * Spherical Linear interpolate between this and another quaternion. - * @param to The quaternion to interpolate towards. - * @param t The time value, between 0 and 1. - * @result The quaternion at the given time in the interpolation process. - */ - Quaternion Slerp(const Quaternion& to, float t) const; - - /** - * Spherical cubic interpolate. - * @param p The first quaternion. - * @param a The second quaternion. - * @param b The third quaternion. - * @param q The fourth quaternion. - * @param t The time value, between 0 and 1. - * @result The quaternion at the given time in the interpolation process. - */ - static Quaternion Squad(const Quaternion& p, const Quaternion& a, const Quaternion& b, const Quaternion& q, float t); - - // operators - MCORE_INLINE const Quaternion& operator=(const Matrix& m) { FromMatrix(m); return *this; } - MCORE_INLINE const Quaternion& operator=(const Quaternion& other) { x = other.x; y = other.y; z = other.z; w = other.w; return *this; } - MCORE_INLINE Quaternion operator-() const { return Quaternion(-x, -y, -z, -w); } - MCORE_INLINE const Quaternion& operator+=(const Quaternion& q) { x += q.x; y += q.y; z += q.z; w += q.w; return *this; } - MCORE_INLINE const Quaternion& operator-=(const Quaternion& q) { x -= q.x; y -= q.y; z -= q.z; w -= q.w; return *this; } - MCORE_INLINE const Quaternion& operator*=(const Quaternion& q); - MCORE_INLINE const Quaternion& operator*=(float f) { x *= f; y *= f; z *= f; w *= f; return *this; } - //MCORE_INLINE const Quaternion& operator*=(double f) { x*=f; y*=f; z*=f; w*=f; return *this; } - MCORE_INLINE bool operator==(const Quaternion& q) const { return ((q.x == x) && (q.y == y) && (q.z == z) && (q.w == w)); } - MCORE_INLINE bool operator!=(const Quaternion& q) const { return ((q.x != x) || (q.y != y) || (q.z != z) || (q.w != w)); } - - //MCORE_INLINE float& operator[](int32 row) { return ((float*)&x)[row]; } - MCORE_INLINE operator float*() { return (float*)&x; } - MCORE_INLINE operator const float*() const { return (const float*)&x; } - - MCORE_INLINE AZ::Vector3 operator*(const AZ::Vector3& p) const; // multiply a vector by a quaternion - MCORE_INLINE Quaternion operator/(const Quaternion& q) const; // returns the ratio of two quaternions - - // attributes - float x, y, z, w; - }; - - - // operators - MCORE_INLINE Quaternion operator*(const Quaternion& a, float f) { return Quaternion(a.x * f, a.y * f, a.z * f, a.w * f); } - MCORE_INLINE Quaternion operator*(float f, const Quaternion& b) { return Quaternion(f * b.x, f * b.y, f * b.z, f * b.w); } - //MCORE_INLINE Quaternion operator*(const Quaternion& a, double f) { return Quaternion(a.x*f, a.y*f, a.z*f, a.w*f); } - //MCORE_INLINE Quaternion operator*(double f, const Quaternion& b) { return Quaternion(f*b.x, f*b.y, f*b.z, f*b.w); } - MCORE_INLINE Quaternion operator+(const Quaternion& a, const Quaternion& b) { return Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); } - MCORE_INLINE Quaternion operator-(const Quaternion& a, const Quaternion& b) { return Quaternion(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); } - MCORE_INLINE Quaternion operator*(const Quaternion& a, const Quaternion& b) { return Quaternion(a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z, a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x, a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z); } - - // include the inline code -#include "Quaternion.inl" -} // namespace MCore diff --git a/Gems/EMotionFX/Code/MCore/Source/Quaternion.inl b/Gems/EMotionFX/Code/MCore/Source/Quaternion.inl deleted file mode 100644 index c89f497a1e..0000000000 --- a/Gems/EMotionFX/Code/MCore/Source/Quaternion.inl +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Copyright (c) Contributors to the Open 3D Engine Project. - * For complete copyright and license terms please see the LICENSE at the root of this distribution. - * - * SPDX-License-Identifier: Apache-2.0 OR MIT - * - */ - -// multiply a vector by a quaternion -MCORE_INLINE AZ::Vector3 Quaternion::operator * (const AZ::Vector3& p) const -{ - Quaternion v(p.GetX(), p.GetY(), p.GetZ(), 0.0f); - v = *this* v* this->Conjugated(); - return AZ::Vector3(v.x, v.y, v.z); -} - - - -// returns the ratio of two quaternions -MCORE_INLINE Quaternion Quaternion::operator / (const Quaternion& q) const -{ - Quaternion t((*this) * -q); - Quaternion s((-q) * (-q)); - t *= (1.0f / s.w); - return t; -} - - - -// calculates the length of the quaternion -MCORE_INLINE float Quaternion::Length() const -{ - const float sqLen = SquareLength(); - return Math::SafeSqrt(sqLen); -} - - -// normalizes the quaternion using approximation -MCORE_INLINE Quaternion& Quaternion::Normalize() -{ - // calculate 1.0 / length - // const float ooLen = 1.0f / Math::FastSqrt(x*x + y*y + z*z + w*w); - // const float ooLen = Math::FastInvSqrt(x*x + y*y + z*z + w*w); - const float squareValue = x * x + y * y + z * z + w * w; - const float ooLen = Math::InvSqrt(squareValue); - - x *= ooLen; - y *= ooLen; - z *= ooLen; - w *= ooLen; - - return *this; -} - - -// get the right axis -MCORE_INLINE AZ::Vector3 Quaternion::CalcRightAxis() const -{ - return AZ::Vector3(1.0f - 2.0f * y * y - 2.0f * z * z, - 2.0f * x * y + 2.0f * z * w, - 2.0f * x * z - 2.0f * y * w); -} - - -// get the forward axis -MCORE_INLINE AZ::Vector3 Quaternion::CalcForwardAxis() const -{ - return AZ::Vector3(2.0f * x * y - 2.0f * z * w, - 1.0f - 2.0f * x * x - 2.0f * z * z, - 2.0f * y * z + 2.0f * x * w); -} - - -// get the up axis -MCORE_INLINE AZ::Vector3 Quaternion::CalcUpAxis() const -{ - return AZ::Vector3(2.0f * x * z + 2.0f * y * w, - 2.0f * y * z - 2.0f * x * w, - 1.0f - 2.0f * x * x - 2.0f * y * y); -} - - -// multiply by a quaternion -MCORE_INLINE const Quaternion& Quaternion::operator*=(const Quaternion& q) -{ - const float vx = w * q.x + x * q.w + y * q.z - z * q.y; - const float vy = w * q.y + y * q.w + z * q.x - x * q.z; - const float vz = w * q.z + z * q.w + x * q.y - y * q.x; - const float vw = w * q.w - x * q.x - y * q.y - z * q.z; - x = vx; - y = vy; - z = vz; - w = vw; - return *this; -} - diff --git a/Gems/EMotionFX/Code/MCore/mcore_files.cmake b/Gems/EMotionFX/Code/MCore/mcore_files.cmake index b63c9e1bae..b0d8a67ccd 100644 --- a/Gems/EMotionFX/Code/MCore/mcore_files.cmake +++ b/Gems/EMotionFX/Code/MCore/mcore_files.cmake @@ -107,9 +107,6 @@ set(FILES Source/PlaneEq.cpp Source/PlaneEq.h Source/PlaneEq.inl - Source/Quaternion.cpp - Source/Quaternion.h - Source/Quaternion.inl Source/Random.cpp Source/Random.h Source/Ray.cpp