QuaternionTests.cpp 38 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743
  1. /*
  2. * Copyright (c) Contributors to the Open 3D Engine Project.
  3. * For complete copyright and license terms please see the LICENSE at the root of this distribution.
  4. *
  5. * SPDX-License-Identifier: Apache-2.0 OR MIT
  6. *
  7. */
  8. #include <AZTestShared/Math/MathTestHelpers.h>
  9. #include <AzCore/Math/Quaternion.h>
  10. #include <AzCore/Math/Vector3.h>
  11. #include <AzCore/Math/Matrix3x3.h>
  12. #include <AzCore/Math/Transform.h>
  13. #include <AzCore/UnitTest/TestTypes.h>
  14. #include <AZTestShared/Math/MathTestHelpers.h>
  15. using namespace AZ;
  16. namespace UnitTest
  17. {
  18. constexpr float normalizeEpsilon = 0.002f;
  19. constexpr float values[4] = { 10.0f, 20.0f, 30.0f, 40.0f };
  20. TEST(MATH_Quaternion, TestHandedness)
  21. {
  22. //test to make sure our rotations follow the right hand rule,
  23. // a positive rotation around z should transform the x axis to the y axis
  24. Matrix4x4 matrix = Matrix4x4::CreateRotationZ(DegToRad(90.0f));
  25. Vector3 v = matrix * Vector3(1.0f, 0.0f, 0.0f);
  26. EXPECT_THAT(v, IsClose(Vector3(0.0f, 1.0f, 0.0f)));
  27. AZ::Quaternion quat = AZ::Quaternion::CreateRotationZ(DegToRad(90.0f));
  28. v = quat.TransformVector(Vector3(1.0f, 0.0f, 0.0f));
  29. EXPECT_THAT(v, IsClose(Vector3(0.0f, 1.0f, 0.0f)));
  30. }
  31. TEST(MATH_Quaternion, TestConstruction)
  32. {
  33. AZ::Quaternion q1(0.0f, 0.0f, 0.0f, 1.0f);
  34. EXPECT_TRUE((q1.GetX() == 0.0f) && (q1.GetY() == 0.0f) && (q1.GetZ() == 0.0f) && (q1.GetW() == 1.0f));
  35. AZ::Quaternion q2(5.0f);
  36. EXPECT_TRUE((q2.GetX() == 5.0f) && (q2.GetY() == 5.0f) && (q2.GetZ() == 5.0f) && (q2.GetW() == 5.0f));
  37. AZ::Quaternion q3(1.0f, 2.0f, 3.0f, 4.0f);
  38. EXPECT_TRUE((q3.GetX() == 1.0f) && (q3.GetY() == 2.0f) && (q3.GetZ() == 3.0f) && (q3.GetW() == 4.0f));
  39. AZ::Quaternion q4 = AZ::Quaternion::CreateFromVector3AndValue(Vector3(1.0f, 2.0f, 3.0f), 4.0f);
  40. EXPECT_TRUE((q4.GetX() == 1.0f) && (q4.GetY() == 2.0f) && (q4.GetZ() == 3.0f) && (q4.GetW() == 4.0f));
  41. AZ::Quaternion q5 = AZ::Quaternion::CreateFromFloat4(values);
  42. EXPECT_TRUE((q5.GetX() == 10.0f) && (q5.GetY() == 20.0f) && (q5.GetZ() == 30.0f) && (q5.GetW() == 40.0f));
  43. AZ::Quaternion q6 = AZ::Quaternion::CreateFromVector3(Vector3(1.0f, 2.0f, 3.0f));
  44. EXPECT_TRUE((q6.GetX() == 1.0f) && (q6.GetY() == 2.0f) && (q6.GetZ() == 3.0f) && (q6.GetW() == 0.0f));
  45. AZ::Quaternion q7 = AZ::Quaternion::CreateFromAxisAngle(Vector3::CreateAxisZ(), DegToRad(45.0f));
  46. EXPECT_THAT(q7, IsClose(AZ::Quaternion::CreateRotationZ(DegToRad(45.0f))));
  47. AZ::Quaternion q8 = Transform::CreateRotationX(DegToRad(60.0f)).GetRotation();
  48. EXPECT_THAT(q8, IsClose(AZ::Quaternion(0.5f, 0.0f, 0.0f, 0.866f)));
  49. AZ::Quaternion q9 = AZ::Quaternion::CreateFromMatrix3x3(Matrix3x3::CreateRotationX(DegToRad(120.0f)));
  50. EXPECT_THAT(q9, IsClose(AZ::Quaternion(0.866f, 0.0f, 0.0f, 0.5f)));
  51. AZ::Quaternion q10 = AZ::Quaternion::CreateFromMatrix4x4(Matrix4x4::CreateRotationX(DegToRad(120.0f)));
  52. EXPECT_THAT(q10, IsClose(AZ::Quaternion(0.866f, 0.0f, 0.0f, 0.5f)));
  53. AZ::Quaternion q11 = AZ::Quaternion::CreateFromMatrix3x3(Matrix3x3::CreateRotationX(DegToRad(-60.0f)));
  54. EXPECT_THAT(q11, IsClose(AZ::Quaternion(-0.5f, 0.0f, 0.0f, 0.866f)));
  55. AZ::Quaternion q12 = AZ::Quaternion::CreateFromMatrix4x4(Matrix4x4::CreateRotationX(DegToRad(-60.0f)));
  56. EXPECT_THAT(q12, IsClose(AZ::Quaternion(-0.5f, 0.0f, 0.0f, 0.866f)));
  57. AZ::Quaternion q13 = AZ::Quaternion::CreateFromMatrix3x3(Matrix3x3::CreateRotationY(DegToRad(120.0f)));
  58. EXPECT_THAT(q13, IsClose(AZ::Quaternion(0.0f, 0.866f, 0.0f, 0.5f)));
  59. AZ::Quaternion q14 = AZ::Quaternion::CreateFromMatrix4x4(Matrix4x4::CreateRotationY(DegToRad(120.0f)));
  60. EXPECT_THAT(q14, IsClose(AZ::Quaternion(0.0f, 0.866f, 0.0f, 0.5f)));
  61. AZ::Quaternion q15 = AZ::Quaternion::CreateFromMatrix3x3(Matrix3x3::CreateRotationY(DegToRad(-60.0f)));
  62. EXPECT_THAT(q15, IsClose(AZ::Quaternion(0.0f, -0.5f, 0.0f, 0.866f)));
  63. AZ::Quaternion q16 = AZ::Quaternion::CreateFromMatrix4x4(Matrix4x4::CreateRotationY(DegToRad(-60.0f)));
  64. EXPECT_THAT(q16, IsClose(AZ::Quaternion(0.0f, -0.5f, 0.0f, 0.866f)));
  65. AZ::Quaternion q17 = AZ::Quaternion::CreateFromMatrix3x3(Matrix3x3::CreateRotationZ(DegToRad(120.0f)));
  66. EXPECT_THAT(q17, IsClose(AZ::Quaternion(0.0f, 0.0f, 0.866f, 0.5f)));
  67. AZ::Quaternion q18 = AZ::Quaternion::CreateFromMatrix4x4(Matrix4x4::CreateRotationZ(DegToRad(120.0f)));
  68. EXPECT_THAT(q18, IsClose(AZ::Quaternion(0.0f, 0.0f, 0.866f, 0.5f)));
  69. AZ::Quaternion q19 = AZ::Quaternion::CreateFromMatrix3x3(Matrix3x3::CreateRotationZ(DegToRad(-60.0f)));
  70. EXPECT_THAT(q19, IsClose(AZ::Quaternion(0.0f, 0.0f, -0.5f, 0.866f)));
  71. AZ::Quaternion q20 = AZ::Quaternion::CreateFromMatrix4x4(Matrix4x4::CreateRotationZ(DegToRad(-60.0f)));
  72. EXPECT_THAT(q20, IsClose(AZ::Quaternion(0.0f, 0.0f, -0.5f, 0.866f)));
  73. }
  74. TEST(MATH_Quaternion, TestCreate)
  75. {
  76. EXPECT_THAT(AZ::Quaternion::CreateIdentity(), IsClose(AZ::Quaternion(0.0f, 0.0f, 0.0f, 1.0f)));
  77. EXPECT_THAT(AZ::Quaternion::CreateZero(), IsClose(AZ::Quaternion(0.0f)));
  78. EXPECT_THAT(AZ::Quaternion::CreateRotationX(DegToRad(60.0f)), IsClose(AZ::Quaternion(0.5f, 0.0f, 0.0f, 0.866f)));
  79. EXPECT_THAT(AZ::Quaternion::CreateRotationY(DegToRad(60.0f)), IsClose(AZ::Quaternion(0.0f, 0.5f, 0.0f, 0.866f)));
  80. EXPECT_THAT(AZ::Quaternion::CreateRotationZ(DegToRad(60.0f)), IsClose(AZ::Quaternion(0.0f, 0.0f, 0.5f, 0.866f)));
  81. }
  82. TEST(MATH_Quaternion, TestConcatenate)
  83. {
  84. Quaternion q1(1.0f, 2.0f, 3.0f, 4.0f);
  85. Quaternion q2(-1.0f, -2.0f, -3.0f, -4.0f);
  86. Quaternion result = q1 * q2;
  87. EXPECT_THAT(result, IsClose(Quaternion(-8.0f, -16.0f, -24.0f, -2.0f)));
  88. }
  89. TEST(MATH_Quaternion, TestShortestArc)
  90. {
  91. Vector3 v1 = Vector3(1.0f, 2.0f, 3.0f).GetNormalized();
  92. Vector3 v2 = Vector3(-2.0f, 7.0f, -1.0f).GetNormalized();
  93. Quaternion q3 = AZ::Quaternion::CreateShortestArc(v1, v2); //q3 should transform v1 into v2
  94. EXPECT_THAT(v2, IsCloseTolerance(q3.TransformVector(v1), 1e-3f));
  95. Quaternion q4 = AZ::Quaternion::CreateShortestArc(Vector3(1.0f, 0.0f, 0.0f), Vector3(0.0f, 1.0f, 0.0f));
  96. EXPECT_THAT((q4.TransformVector(Vector3(0.0f, 0.0f, 1.0f))), IsCloseTolerance(Vector3(0.0f, 0.0f, 1.0f), 1e-3f)); //perpendicular vector should be unaffected
  97. EXPECT_THAT((q4.TransformVector(Vector3(0.0f, -1.0f, 0.0f))), IsCloseTolerance(Vector3(1.0f, 0.0f, 0.0f), 1e-3f)); //make sure we rotate the right direction, i.e. actually shortest arc
  98. v2 = (v1 + Vector3(1e-5f, 1e-5f, 1e-5f)).GetNormalized(); // test almost parallel vectors
  99. Quaternion q5 = AZ::Quaternion::CreateShortestArc(v1, v2);
  100. EXPECT_THAT(v2, IsCloseTolerance(q5.TransformVector(v1), 1e-3f));
  101. v2 = (-v1 + Vector3(1e-5f, 1e-5f, 1e-5f)).GetNormalized(); // test almost anti-parallel vectors
  102. Quaternion q6 = AZ::Quaternion::CreateShortestArc(v1, v2);
  103. EXPECT_THAT(v2, IsCloseTolerance(q6.TransformVector(v1), 1e-3f));
  104. }
  105. TEST(MATH_Quaternion, TestGetSet)
  106. {
  107. Quaternion q1;
  108. q1.SetX(10.0f);
  109. EXPECT_NEAR(q1.GetX(), 10.0f, 1e-6f);
  110. q1.SetY(11.0f);
  111. EXPECT_NEAR(q1.GetY(), 11.0f, 1e-6f);
  112. q1.SetZ(12.0f);
  113. EXPECT_NEAR(q1.GetZ(), 12.0f, 1e-6f);
  114. q1.SetW(13.0f);
  115. EXPECT_NEAR(q1.GetW(), 13.0f, 1e-6f);
  116. q1.Set(15.0f);
  117. EXPECT_THAT(q1, IsClose(AZ::Quaternion(15.0f)));
  118. q1.Set(2.0f, 3.0f, 4.0f, 5.0f);
  119. EXPECT_THAT(q1, IsClose(AZ::Quaternion(2.0f, 3.0f, 4.0f, 5.0f)));
  120. q1.Set(Vector3(5.0f, 6.0f, 7.0f), 8.0f);
  121. EXPECT_THAT(q1, IsClose(AZ::Quaternion(5.0f, 6.0f, 7.0f, 8.0f)));
  122. q1.Set(values);
  123. EXPECT_TRUE((q1.GetX() == 10.0f) && (q1.GetY() == 20.0f) && (q1.GetZ() == 30.0f) && (q1.GetW() == 40.0f));
  124. }
  125. TEST(MATH_Quaternion, TestGetElementSetElement)
  126. {
  127. Quaternion q1;
  128. q1.SetElement(0, 1.0f);
  129. q1.SetElement(1, 2.0f);
  130. q1.SetElement(2, 3.0f);
  131. q1.SetElement(3, 4.0f);
  132. EXPECT_NEAR(q1.GetElement(0), 1.0f, 1e-6f);
  133. EXPECT_NEAR(q1.GetElement(1), 2.0f, 1e-6f);
  134. EXPECT_NEAR(q1.GetElement(2), 3.0f, 1e-6f);
  135. EXPECT_NEAR(q1.GetElement(3), 4.0f, 1e-6f);
  136. }
  137. TEST(MATH_Quaternion, TestIndexOperators)
  138. {
  139. Quaternion q1(1.0f, 2.0f, 3.0f, 4.0f);
  140. EXPECT_NEAR(q1(0), 1.0f, 1e-6f);
  141. EXPECT_NEAR(q1(1), 2.0f, 1e-6f);
  142. EXPECT_NEAR(q1(2), 3.0f, 1e-6f);
  143. EXPECT_NEAR(q1(3), 4.0f, 1e-6f);
  144. }
  145. TEST(MATH_Quaternion, TestIsIdentity)
  146. {
  147. Quaternion q1(0.0f, 0.0f, 0.0f, 1.0f);
  148. EXPECT_TRUE(q1.IsIdentity());
  149. }
  150. TEST(MATH_Quaternion, TestConjugate)
  151. {
  152. Quaternion q1(1.0f, 2.0f, 3.0f, 4.0f);
  153. EXPECT_THAT(q1.GetConjugate(), IsClose(AZ::Quaternion(-1.0f, -2.0f, -3.0f, 4.0f)));
  154. }
  155. TEST(MATH_Quaternion, TestInverse)
  156. {
  157. Quaternion q1 = AZ::Quaternion::CreateRotationX(DegToRad(25.0f)) * AZ::Quaternion::CreateRotationY(DegToRad(70.0f));
  158. EXPECT_THAT((q1 * q1.GetInverseFast()), IsClose(AZ::Quaternion::CreateIdentity()));
  159. Quaternion q2 = q1;
  160. q2.InvertFast();
  161. EXPECT_NEAR(q1.GetX(), -q2.GetX(), 1e-6f);
  162. EXPECT_NEAR(q1.GetY(), -q2.GetY(), 1e-6f);
  163. EXPECT_NEAR(q1.GetZ(), -q2.GetZ(), 1e-6f);
  164. EXPECT_NEAR(q1.GetW(), q2.GetW(), 1e-6f);
  165. EXPECT_THAT((q1 * q2), IsClose(AZ::Quaternion::CreateIdentity()));
  166. }
  167. TEST(MATH_Quaternion, TestGetInverseFull)
  168. {
  169. Quaternion q1(1.0f, 2.0f, 3.0f, 4.0f);
  170. EXPECT_THAT((q1 * q1.GetInverseFull()), IsClose(AZ::Quaternion::CreateIdentity()));
  171. }
  172. TEST(MATH_Quaternion, TestDot)
  173. {
  174. EXPECT_NEAR(AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f).Dot(AZ::Quaternion(-1.0f, 5.0f, 3.0f, 2.0f)), 26.0f, normalizeEpsilon);
  175. }
  176. TEST(MATH_Quaternion, TestLength)
  177. {
  178. EXPECT_NEAR(AZ::Quaternion(-1.0f, 2.0f, 1.0f, 3.0f).GetLengthSq(), 15.0f, normalizeEpsilon);
  179. EXPECT_NEAR(AZ::Quaternion(-4.0f, 2.0f, 0.0f, 4.0f).GetLength(), 6.0f, normalizeEpsilon);
  180. }
  181. TEST(MATH_Quaternion, TestNormalize)
  182. {
  183. EXPECT_THAT(AZ::Quaternion(0.0f, -4.0f, 2.0f, 4.0f).GetNormalized(), IsClose(AZ::Quaternion(0.0f, -0.66666f, 0.33333f, 0.66666f)));
  184. Quaternion q1(2.0f, 0.0f, 4.0f, -4.0f);
  185. q1.Normalize();
  186. EXPECT_THAT(q1, IsClose(AZ::Quaternion(0.33333f, 0.0f, 0.66666f, -0.66666f)));
  187. q1.Set(2.0f, 0.0f, 4.0f, -4.0f);
  188. float length = q1.NormalizeWithLength();
  189. EXPECT_NEAR(length, 6.0f, normalizeEpsilon);
  190. EXPECT_THAT(q1, IsClose(AZ::Quaternion(0.33333f, 0.0f, 0.66666f, -0.66666f)));
  191. }
  192. TEST(MATH_Quaternion, TestInterpolation)
  193. {
  194. EXPECT_THAT(AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f).Lerp(AZ::Quaternion(2.0f, 3.0f, 4.0f, 5.0f), 0.5f), IsClose(AZ::Quaternion(1.5f, 2.5f, 3.5f, 4.5f)));
  195. EXPECT_THAT(AZ::Quaternion::CreateRotationX(DegToRad(10.0f)).Slerp(AZ::Quaternion::CreateRotationY(DegToRad(60.0f)), 0.5f), IsCloseTolerance(AZ::Quaternion(0.045f, 0.259f, 0.0f, 0.965f), 1e-3f));
  196. EXPECT_THAT(AZ::Quaternion::CreateRotationX(DegToRad(10.0f)).Squad(AZ::Quaternion::CreateRotationY(DegToRad(60.0f)), AZ::Quaternion::CreateRotationZ(DegToRad(35.0f)), AZ::Quaternion::CreateRotationX(DegToRad(80.0f)), 0.5f), IsCloseTolerance(AZ::Quaternion(0.2f, 0.132f, 0.083f, 0.967f), 1e-3f));
  197. }
  198. TEST(MATH_Quaternion, TestClose)
  199. {
  200. EXPECT_THAT(AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f), IsClose(AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f)));
  201. EXPECT_THAT(AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f), testing::Not(IsClose(AZ::Quaternion(1.0f, 2.0f, 3.0f, 5.0f))));
  202. EXPECT_THAT(AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f), IsCloseTolerance(AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.4f), 0.5f));
  203. }
  204. TEST(MATH_Quaternion, TestOperators)
  205. {
  206. EXPECT_THAT((-AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f)), IsClose(AZ::Quaternion(-1.0f, -2.0f, -3.0f, -4.0f)));
  207. EXPECT_THAT((AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f) + AZ::Quaternion(2.0f, 3.0f, 5.0f, -1.0f)), IsClose(AZ::Quaternion(3.0f, 5.0f, 8.0f, 3.0f)));
  208. EXPECT_THAT((AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f) - AZ::Quaternion(2.0f, 3.0f, 5.0f, -1.0f)), IsClose(AZ::Quaternion(-1.0f, -1.0f, -2.0f, 5.0f)));
  209. EXPECT_THAT((AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f) * AZ::Quaternion(2.0f, 3.0f, 5.0f, -1.0f)), IsClose(AZ::Quaternion(8.0f, 11.0f, 16.0f, -27.0f)));
  210. EXPECT_THAT((AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f) * 2.0f), IsClose(AZ::Quaternion(2.0f, 4.0f, 6.0f, 8.0f)));
  211. EXPECT_THAT((2.0f * AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f)), IsClose(AZ::Quaternion(2.0f, 4.0f, 6.0f, 8.0f)));
  212. EXPECT_THAT((AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f) / 2.0f), IsClose(AZ::Quaternion(0.5f, 1.0f, 1.5f, 2.0f)));
  213. Quaternion q1(1.0f, 2.0f, 3.0f, 4.0f);
  214. q1 += AZ::Quaternion(5.0f, 6.0f, 7.0f, 8.0f);
  215. EXPECT_THAT(q1, IsClose(AZ::Quaternion(6.0f, 8.0f, 10.0f, 12.0f)));
  216. q1 -= AZ::Quaternion(3.0f, -1.0f, 5.0f, 7.0f);
  217. EXPECT_THAT(q1, IsClose(AZ::Quaternion(3.0f, 9.0f, 5.0f, 5.0f)));
  218. q1.Set(1.0f, 2.0f, 3.0f, 4.0f);
  219. q1 *= AZ::Quaternion(2.0f, 3.0f, 5.0f, -1.0f);
  220. EXPECT_THAT(q1, IsClose(AZ::Quaternion(8.0f, 11.0f, 16.0f, -27.0f)));
  221. q1 *= 2.0f;
  222. EXPECT_THAT(q1, IsClose(AZ::Quaternion(16.0f, 22.0f, 32.0f, -54.0f)));
  223. q1 /= 4.0f;
  224. EXPECT_THAT(q1, IsClose(AZ::Quaternion(4.0f, 5.5f, 8.0f, -13.5f)));
  225. }
  226. TEST(MATH_Quaternion, TestEquality)
  227. {
  228. Quaternion q3(1.0f, 2.0f, 3.0f, 4.0f);
  229. EXPECT_TRUE(q3 == AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0));
  230. EXPECT_TRUE(!(q3 == AZ::Quaternion(1.0f, 2.0f, 3.0f, 5.0f)));
  231. EXPECT_TRUE(q3 != AZ::Quaternion(1.0f, 2.0f, 3.0f, 5.0f));
  232. EXPECT_TRUE(!(q3 != AZ::Quaternion(1.0f, 2.0f, 3.0f, 4.0f)));
  233. }
  234. TEST(MATH_Quaternion, TestVectorTransform)
  235. {
  236. EXPECT_THAT((AZ::Quaternion::CreateRotationX(DegToRad(45.0f)).TransformVector(Vector3(4.0f, 1.0f, 0.0f))), IsClose(Vector3(4.0f, 0.7071f, 0.7071f)));
  237. }
  238. TEST(MATH_Quaternion, TestGetImaginary)
  239. {
  240. Quaternion q1(21.0f, 22.0f, 23.0f, 24.0f);
  241. EXPECT_THAT(q1.GetImaginary(), IsClose(Vector3(21.0f, 22.0f, 23.0f)));
  242. }
  243. TEST(MATH_Quaternion, TestGetAngle)
  244. {
  245. Quaternion q1 = AZ::Quaternion::CreateRotationX(DegToRad(35.0f));
  246. EXPECT_NEAR(q1.GetAngle(), DegToRad(35.0f), AZ::Constants::Tolerance);
  247. }
  248. TEST(MATH_Quaternion, TestConcatenation)
  249. {
  250. Quaternion q1 = AZ::Quaternion::CreateRotationZ(DegToRad(90.0f));
  251. Quaternion q2 = AZ::Quaternion::CreateRotationX(DegToRad(90.0f));
  252. Vector3 v = (q2 * q1).TransformVector(Vector3(1.0f, 0.0f, 0.0f));
  253. EXPECT_THAT(v, IsClose(Vector3(0.0f, 0.0f, 1.0f)));
  254. }
  255. TEST(MATH_Quaternion, ToEulerDegrees)
  256. {
  257. float halfAngle = 0.5f * AZ::Constants::QuarterPi;
  258. float sin = sinf(halfAngle);
  259. float cos = cosf(halfAngle);
  260. AZ::Quaternion testQuat = AZ::Quaternion::CreateFromVector3AndValue(sin * AZ::Vector3::CreateAxisX(), cos);
  261. AZ::Vector3 resultVector = testQuat.GetEulerDegrees();
  262. EXPECT_THAT(resultVector, IsClose(AZ::Vector3(45.0f, 0.0f, 0.0f)));
  263. resultVector = ConvertQuaternionToEulerDegrees(testQuat);
  264. EXPECT_THAT(resultVector, IsClose(AZ::Vector3(45.0f, 0.0f, 0.0f)));
  265. }
  266. TEST(MATH_Quaternion, ToEulerRadians)
  267. {
  268. constexpr float getEulerRadiansEpsilon = 0.001f;
  269. float halfAngle = 0.5f * AZ::Constants::HalfPi;
  270. float sin = sinf(halfAngle);
  271. float cos = cosf(halfAngle);
  272. AZ::Quaternion testQuat = AZ::Quaternion::CreateFromVector3AndValue(sin * AZ::Vector3::CreateAxisY(), cos);
  273. AZ::Vector3 resultVector = testQuat.GetEulerRadians();
  274. EXPECT_NEAR(AZ::Constants::HalfPi, resultVector.GetY(), getEulerRadiansEpsilon);
  275. resultVector = ConvertQuaternionToEulerRadians(testQuat);
  276. EXPECT_NEAR(AZ::Constants::HalfPi, resultVector.GetY(), getEulerRadiansEpsilon);
  277. }
  278. using QuaternionEulerFixture = ::testing::TestWithParam<AZ::Quaternion>;
  279. static const AZ::Quaternion TestUnitQuaternions[] = {
  280. AZ::Quaternion(0.64f, 0.36f, 0.48f, 0.48f),
  281. AZ::Quaternion(0.70f, -0.34f, 0.10f, 0.62f),
  282. AZ::Quaternion(-0.38f, 0.34f, 0.70f, -0.50f),
  283. AZ::Quaternion(0.70f, -0.34f, -0.38f, 0.50f),
  284. AZ::Quaternion(0.00f, 0.00f, -0.28f, 0.96f),
  285. AZ::Quaternion(0.24f, -0.64f, 0.72f, 0.12f),
  286. AZ::Quaternion(-0.66f, 0.62f, 0.42f, 0.06f),
  287. AZ::Quaternion(0.5f, 0.5f, 0.5f, 0.5f),
  288. AZ::Quaternion(0.5f, -0.5f, 0.5f, -0.5f),
  289. AZ::Quaternion(0.34f, 0.62f, -0.34f, -0.62f),
  290. AZ::Quaternion(-0.1f, -0.7f, 0.1f, 0.7f)
  291. };
  292. TEST_P(QuaternionEulerFixture, EulerOrderCorrect)
  293. {
  294. // the quaternion should be equivalent to a series of rotations in the order z, then y, then x
  295. const AZ::Quaternion quaternion = GetParam();
  296. const AZ::Vector3 euler = quaternion.GetEulerRadians();
  297. const AZ::Quaternion productOfRotations =
  298. AZ::Quaternion::CreateRotationX(euler.GetX()) *
  299. AZ::Quaternion::CreateRotationY(euler.GetY()) *
  300. AZ::Quaternion::CreateRotationZ(euler.GetZ());
  301. EXPECT_TRUE(productOfRotations.IsClose(quaternion) || productOfRotations.IsClose(-quaternion));
  302. }
  303. TEST_P(QuaternionEulerFixture, QuaternionEulerQuaternionCycle)
  304. {
  305. // converting a quaternion to Euler angles and back again should recover the original quaternion
  306. // note that because the Euler angle representation is not unique, the same is not necessarily true
  307. // for Euler -> quaternion -> Euler
  308. const AZ::Quaternion originalQuaternion = GetParam();
  309. const AZ::Vector3 euler = originalQuaternion.GetEulerRadians();
  310. const AZ::Quaternion recoveredQuaternion = AZ::Quaternion::CreateFromEulerRadiansXYZ(euler);
  311. EXPECT_TRUE(recoveredQuaternion.IsClose(originalQuaternion) || recoveredQuaternion.IsClose(-originalQuaternion));
  312. }
  313. TEST_P(QuaternionEulerFixture, EulerViaTransformEquivalentToDirectEuler)
  314. {
  315. // quaternion -> transform -> Euler -> quaternion should give an equivalent result to
  316. // quaternion -> Euler -> quaternion
  317. const AZ::Quaternion originalQuaternion = GetParam();
  318. const AZ::Vector3 euler1 = originalQuaternion.GetEulerRadians();
  319. const AZ::Vector3 euler2 = AZ::Transform::CreateFromQuaternion(originalQuaternion).GetEulerRadians();
  320. const AZ::Quaternion recoveredQuaternion1 = AZ::ConvertEulerRadiansToQuaternion(euler1);
  321. const AZ::Quaternion recoveredQuaternion2 = AZ::ConvertEulerRadiansToQuaternion(euler2);
  322. EXPECT_TRUE(recoveredQuaternion1.IsClose(recoveredQuaternion2) || recoveredQuaternion1.IsClose(-recoveredQuaternion2));
  323. }
  324. INSTANTIATE_TEST_SUITE_P(MATH_Quaternion, QuaternionEulerFixture, ::testing::ValuesIn(TestUnitQuaternions));
  325. TEST(MATH_Quaternion, FromEulerDegrees)
  326. {
  327. const AZ::Vector3 testDegrees(45.0f, 45.0f, 45.0f);
  328. AZ::Quaternion testQuat;
  329. testQuat.SetFromEulerDegrees(testDegrees);
  330. EXPECT_THAT(testQuat, IsCloseTolerance(AZ::Quaternion(0.46193981170654296875f, 0.1913417130708694458f, 0.46193981170654296875f, 0.73253774642944335938f), 1e-6f));
  331. }
  332. TEST(MATH_Quaternion, FromEulerRadians)
  333. {
  334. const AZ::Vector3 testRadians(AZ::Constants::QuarterPi, AZ::Constants::QuarterPi, AZ::Constants::QuarterPi);
  335. AZ::Quaternion testQuat;
  336. testQuat.SetFromEulerRadians(testRadians);
  337. EXPECT_THAT(testQuat, IsCloseTolerance(AZ::Quaternion(0.46193981170654296875f, 0.1913417130708694458f, 0.46193981170654296875f, 0.73253774642944335938f), 1e-6f));
  338. }
  339. TEST(MATH_Quaternion, FromAxisAngle)
  340. {
  341. AZ::Quaternion q10 = AZ::Quaternion::CreateFromAxisAngle(AZ::Vector3::CreateAxisZ(), AZ::Constants::QuarterPi);
  342. EXPECT_THAT(q10, IsClose(AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi)));
  343. q10 = AZ::Quaternion::CreateFromAxisAngle(AZ::Vector3::CreateAxisY(), AZ::Constants::HalfPi);
  344. EXPECT_THAT(q10, IsClose(AZ::Quaternion::CreateRotationY(AZ::Constants::HalfPi)));
  345. q10 = AZ::Quaternion::CreateFromAxisAngle(AZ::Vector3::CreateAxisX(), AZ::Constants::TwoPi / 3.0f);
  346. EXPECT_THAT(q10, IsClose(AZ::Quaternion::CreateRotationX(AZ::Constants::TwoPi / 3.0f)));
  347. }
  348. TEST(MATH_Quaternion, ToAxisAngle)
  349. {
  350. constexpr float axisAngleEpsilon = 0.002f;
  351. AZ::Quaternion testQuat = AZ::Quaternion::CreateIdentity();
  352. Vector3 resultAxis = Vector3::CreateZero();
  353. float resultAngle{};
  354. testQuat.ConvertToAxisAngle(resultAxis, resultAngle);
  355. EXPECT_THAT(resultAxis, IsClose(Vector3::CreateAxisY()));
  356. EXPECT_NEAR(0.0f, resultAngle, axisAngleEpsilon);
  357. }
  358. TEST(MATH_Quaternion, MatrixConversionTest)
  359. {
  360. Matrix4x4 rotMatrix = Matrix4x4::CreateRotationZ(DegToRad(90.0f));
  361. AZ::Quaternion rotQuat = AZ::Quaternion::CreateRotationZ(DegToRad(90.0f));
  362. AZ::Quaternion q = AZ::Quaternion::CreateFromMatrix4x4(rotMatrix);
  363. EXPECT_THAT(q, IsClose(rotQuat));
  364. Matrix4x4 m = Matrix4x4::CreateFromQuaternion(rotQuat);
  365. EXPECT_THAT(m, IsClose(rotMatrix));
  366. }
  367. class QuaternionScaledAxisAngleConversionFixture
  368. : public ::testing::TestWithParam<AZ::Quaternion>
  369. {
  370. public:
  371. AZ::Quaternion GetAbs(const AZ::Quaternion& in)
  372. {
  373. // Take the shortest path for quaternions containing rotations bigger than 180.0°.
  374. if (in.GetW() < 0.0f)
  375. {
  376. return -in;
  377. }
  378. return in;
  379. }
  380. };
  381. static const AZ::Quaternion RotationRepresentationConversionTestQuats[] =
  382. {
  383. AZ::Quaternion::CreateIdentity(),
  384. -AZ::Quaternion::CreateIdentity(),
  385. AZ::Quaternion::CreateRotationX(AZ::Constants::TwoPi),
  386. AZ::Quaternion::CreateRotationY(AZ::Constants::Pi),
  387. AZ::Quaternion::CreateRotationZ(AZ::Constants::HalfPi),
  388. AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi),
  389. AZ::Quaternion(0.64f, 0.36f, 0.48f, 0.48f),
  390. AZ::Quaternion(0.70f, -0.34f, 0.10f, 0.62f),
  391. AZ::Quaternion(-0.38f, 0.34f, 0.70f, -0.50f),
  392. AZ::Quaternion(0.70f, -0.34f, -0.38f, 0.50f),
  393. AZ::Quaternion(0.00f, 0.00f, -0.28f, 0.96f),
  394. AZ::Quaternion(0.24f, -0.64f, 0.72f, 0.12f),
  395. AZ::Quaternion(-0.66f, 0.62f, 0.42f, 0.06f)
  396. };
  397. TEST_P(QuaternionScaledAxisAngleConversionFixture, ScaledAxisAngleQuatRoundtripTests)
  398. {
  399. const AZ::Quaternion testQuat = GetAbs(GetParam());
  400. // Convert test quaternion to scaled axis-angle representation.
  401. const AZ::Vector3 scaledAxisAngle = testQuat.ConvertToScaledAxisAngle();
  402. // Convert the scaled axis-angle back into a quaternion.
  403. AZ::Quaternion backFromScaledAxisAngle = AZ::Quaternion::CreateFromScaledAxisAngle(scaledAxisAngle);
  404. // Compare the original quaternion with the one after the conversion.
  405. EXPECT_THAT(testQuat, IsCloseTolerance(backFromScaledAxisAngle, 1e-6f));
  406. }
  407. TEST_P(QuaternionScaledAxisAngleConversionFixture, AxisAngleQuatRoundtripTests)
  408. {
  409. const AZ::Quaternion testQuat = GetAbs(GetParam());
  410. // Convert test quaternion to axis-angle representation.
  411. AZ::Vector3 axis;
  412. float angle;
  413. testQuat.ConvertToAxisAngle(axis, angle);
  414. // Convert the axis-angle back into a quaternion and compare the original quaternion with the one after the conversion.
  415. const AZ::Quaternion backFromAxisAngle = AZ::Quaternion::CreateFromAxisAngle(axis, angle);
  416. EXPECT_THAT(testQuat, IsCloseTolerance(backFromAxisAngle, 1e-6f));
  417. }
  418. TEST_P(QuaternionScaledAxisAngleConversionFixture, CompareAxisAngleConversionTests)
  419. {
  420. const AZ::Quaternion testQuat = GetAbs(GetParam());
  421. // Convert test quaternion to scaled axis-angle representation.
  422. const AZ::Vector3 scaledAxisAngle = testQuat.ConvertToScaledAxisAngle();
  423. // Convert test quaternion to axis-angle representation and scale it manually.
  424. AZ::Vector3 axis;
  425. float angle;
  426. testQuat.ConvertToAxisAngle(axis, angle);
  427. // Compare the scaled result to the version from the helper that directly converts it to scaled axis-angle.
  428. AZ::Vector3 scaledResult = axis*angle;
  429. EXPECT_THAT(scaledResult, IsCloseTolerance(scaledAxisAngle, 1e-5f));
  430. }
  431. TEST_P(QuaternionScaledAxisAngleConversionFixture, CompareScaledAxisAngleConversionTests)
  432. {
  433. const AZ::Quaternion testQuat = GetAbs(GetParam());
  434. // Convert test quaternion to axis-angle representation and scale it manually.
  435. AZ::Vector3 axis;
  436. float angle;
  437. testQuat.ConvertToAxisAngle(axis, angle);
  438. AZ::Vector3 scaledResult = axis*angle;
  439. // Special case handling for identity rotation.
  440. AZ::Vector3 axisFromScaledResult = scaledResult.GetNormalized();
  441. float angleFromScaledResult = scaledResult.GetLength();
  442. if (AZ::IsClose(angleFromScaledResult, 0.0f))
  443. {
  444. axisFromScaledResult = AZ::Vector3::CreateAxisY();
  445. }
  446. const AZ::Quaternion backFromAxisAngle = AZ::Quaternion::CreateFromAxisAngle(axisFromScaledResult, angleFromScaledResult);
  447. EXPECT_THAT(testQuat, IsCloseTolerance(backFromAxisAngle, 1e-6f));
  448. }
  449. INSTANTIATE_TEST_SUITE_P(MATH_Quaternion, QuaternionScaledAxisAngleConversionFixture, ::testing::ValuesIn(RotationRepresentationConversionTestQuats));
  450. TEST(MATH_Quaternion, ShortestEquivalent)
  451. {
  452. const AZ::Quaternion testQuat = AZ::Quaternion::CreateRotationX(AZ::Constants::HalfPi * 3.0f);
  453. AZ::Quaternion absQuat = testQuat;
  454. absQuat.ShortestEquivalent();
  455. EXPECT_THAT(testQuat.GetShortestEquivalent(), IsCloseTolerance(absQuat, 1e-6f));
  456. const float angle = absQuat.GetEulerRadians().GetX();
  457. EXPECT_THAT(angle, testing::FloatEq(-AZ::Constants::HalfPi));
  458. }
  459. struct EulerTestArgs
  460. {
  461. AZ::Vector3 euler;
  462. AZ::Quaternion result;
  463. };
  464. using AngleRadianTestFixtureXYZ = ::testing::TestWithParam<EulerTestArgs>;
  465. TEST_P(AngleRadianTestFixtureXYZ, EulerRadiansXYZ)
  466. {
  467. auto& param = GetParam();
  468. EXPECT_THAT(AZ::Quaternion::CreateFromEulerRadiansXYZ(param.euler), IsClose(param.result));
  469. // Test backwards computation Quaternion -> Tait-Brian angles
  470. const auto sourceDegrees = Vector3RadToDeg(param.euler);
  471. const bool isGimbleLock = fabs(fabs(sourceDegrees.GetY()) - 90.0f) < Constants::Tolerance;
  472. const auto anglesTaitBryanRadiansXYZ = param.result.GetEulerRadiansXYZ();
  473. const auto resultDegrees = Vector3RadToDeg(anglesTaitBryanRadiansXYZ);
  474. auto succeeded = resultDegrees.IsClose(sourceDegrees);
  475. EXPECT_TRUE((succeeded || isGimbleLock));
  476. // O3DE_DEPRECATION_NOTICE(GHI-10929)
  477. // Test backwards computation Quaternion -> Euler (actually Tait-Brian) angles
  478. // with the method GetEulerRadians(), which is subject to deprecation,
  479. // as methods to be deprecated are roughly equivalent in computations:
  480. // - SetFromEulerRadians(), CreateFromEulerAnglesRadians(), ConvertEulerRadiansToQuaternion() - with CreateFromEulerRadiansXYZ();
  481. // - SetFromEulerDegrees(), CreateFromEulerAnglesDegrees(), ConvertEulerDegreesToQuaternion() - with CreateFromEulerDegreesXYZ();
  482. // - GetEulerRadians() - with GetEulerRadiansXYZ(), which is somewhat optimized;
  483. // - GetEulerDegrees() - with GetEulerDegreesXYZ().
  484. const auto anglesEulerRadians = param.result.GetEulerRadians();
  485. const auto resultEulerDegrees = Vector3RadToDeg(anglesEulerRadians);
  486. succeeded = succeeded && resultDegrees.IsClose(resultEulerDegrees);
  487. EXPECT_TRUE((succeeded || isGimbleLock));
  488. }
  489. TEST_P(AngleRadianTestFixtureXYZ, EulerDegreesXYZ)
  490. {
  491. auto& param = GetParam();
  492. const auto sourceDegrees = Vector3RadToDeg(param.euler);
  493. EXPECT_THAT(AZ::Quaternion::CreateFromEulerDegreesXYZ(sourceDegrees), IsClose(param.result));
  494. // Test backwards computation Quaternion -> Tait-Brian angles
  495. const bool isGimbleLock = fabs(fabs(sourceDegrees.GetY()) - 90.0f) < Constants::Tolerance;
  496. const auto anglesTaitBryanDegreesXYZ = param.result.GetEulerDegreesXYZ();
  497. auto succeeded = anglesTaitBryanDegreesXYZ.IsClose(sourceDegrees);
  498. EXPECT_TRUE((succeeded || isGimbleLock));
  499. }
  500. INSTANTIATE_TEST_SUITE_P(
  501. MATH_Quaternion,
  502. AngleRadianTestFixtureXYZ,
  503. ::testing::Values(
  504. EulerTestArgs{ AZ::Vector3(AZ::Constants::QuarterPi, 0, 0), AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi) },
  505. EulerTestArgs{ AZ::Vector3(0, AZ::Constants::QuarterPi, 0), AZ::Quaternion::CreateRotationY(AZ::Constants::QuarterPi) },
  506. EulerTestArgs{ AZ::Vector3(0, 0, AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  507. EulerTestArgs{ AZ::Vector3(-AZ::Constants::QuarterPi, 0, 0), AZ::Quaternion::CreateRotationX(-AZ::Constants::QuarterPi) },
  508. EulerTestArgs{ AZ::Vector3(0, -AZ::Constants::QuarterPi, 0), AZ::Quaternion::CreateRotationY(-AZ::Constants::QuarterPi) },
  509. EulerTestArgs{ AZ::Vector3(0, 0, -AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationZ(-AZ::Constants::QuarterPi) },
  510. EulerTestArgs{ AZ::Vector3(AZ::Constants::QuarterPi, AZ::Constants::QuarterPi, 0), AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi) *
  511. AZ::Quaternion::CreateRotationY(AZ::Constants::QuarterPi) },
  512. EulerTestArgs{ AZ::Vector3(0, AZ::Constants::QuarterPi, AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationY(AZ::Constants::QuarterPi) *
  513. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  514. EulerTestArgs{ AZ::Vector3(AZ::Constants::QuarterPi, 0, AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi) *
  515. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  516. EulerTestArgs{ AZ::Vector3(AZ::Constants::HalfPi, 0, AZ::Constants::QuarterPi),
  517. AZ::Quaternion::CreateRotationX(AZ::Constants::HalfPi) *
  518. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  519. EulerTestArgs{ AZ::Vector3(-AZ::Constants::QuarterPi, -AZ::Constants::HalfPi, AZ::Constants::QuarterPi),
  520. AZ::Quaternion::CreateRotationX(-AZ::Constants::QuarterPi) *
  521. AZ::Quaternion::CreateRotationY(-AZ::Constants::HalfPi) *
  522. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  523. EulerTestArgs{ AZ::Vector3(-AZ::Constants::QuarterPi, AZ::Constants::HalfPi, AZ::Constants::TwoOverPi),
  524. AZ::Quaternion::CreateRotationX(-AZ::Constants::QuarterPi) *
  525. AZ::Quaternion::CreateRotationY(AZ::Constants::HalfPi) *
  526. AZ::Quaternion::CreateRotationZ(AZ::Constants::TwoOverPi) }
  527. ));
  528. using AngleRadianTestFixtureYXZ = ::testing::TestWithParam<EulerTestArgs>;
  529. TEST_P(AngleRadianTestFixtureYXZ, EulerRadiansYXZ)
  530. {
  531. auto& param = GetParam();
  532. EXPECT_THAT(AZ::Quaternion::CreateFromEulerRadiansYXZ(param.euler), IsClose(param.result));
  533. // Test backwards computation Quaternion -> Tait-Brian angles
  534. const auto sourceDegrees = Vector3RadToDeg(param.euler);
  535. const auto anglesTaitBryanRadiansYXZ = param.result.GetEulerRadiansYXZ();
  536. const auto resultDegrees = Vector3RadToDeg(anglesTaitBryanRadiansYXZ);
  537. EXPECT_TRUE(resultDegrees.IsClose(sourceDegrees));
  538. }
  539. TEST_P(AngleRadianTestFixtureYXZ, EulerDegreesYXZ)
  540. {
  541. auto& param = GetParam();
  542. const auto sourceDegrees = Vector3RadToDeg(param.euler);
  543. EXPECT_THAT(AZ::Quaternion::CreateFromEulerDegreesYXZ(sourceDegrees), IsClose(param.result));
  544. // Test backwards computation Quaternion -> Tait-Brian angles
  545. const auto anglesTaitBryanRadiansYXZ = param.result.GetEulerRadiansYXZ();
  546. const auto resultDegrees = Vector3RadToDeg(anglesTaitBryanRadiansYXZ);
  547. EXPECT_TRUE(resultDegrees.IsClose(sourceDegrees));
  548. }
  549. INSTANTIATE_TEST_SUITE_P(
  550. MATH_Quaternion,
  551. AngleRadianTestFixtureYXZ,
  552. ::testing::Values(
  553. EulerTestArgs{ AZ::Vector3(AZ::Constants::QuarterPi, 0, 0), AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi) },
  554. EulerTestArgs{ AZ::Vector3(0, AZ::Constants::QuarterPi, 0), AZ::Quaternion::CreateRotationY(AZ::Constants::QuarterPi) },
  555. EulerTestArgs{ AZ::Vector3(0, 0, AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  556. EulerTestArgs{ AZ::Vector3(-AZ::Constants::QuarterPi, 0, 0), AZ::Quaternion::CreateRotationX(-AZ::Constants::QuarterPi) },
  557. EulerTestArgs{ AZ::Vector3(0, -AZ::Constants::QuarterPi, 0), AZ::Quaternion::CreateRotationY(-AZ::Constants::QuarterPi) },
  558. EulerTestArgs{ AZ::Vector3(0, 0, -AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationZ(-AZ::Constants::QuarterPi) },
  559. EulerTestArgs{ AZ::Vector3(AZ::Constants::QuarterPi, AZ::Constants::QuarterPi, 0), AZ::Quaternion::CreateRotationY(AZ::Constants::QuarterPi) *
  560. AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi) },
  561. EulerTestArgs{ AZ::Vector3(0, AZ::Constants::QuarterPi, AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationY(AZ::Constants::QuarterPi) *
  562. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  563. EulerTestArgs{ AZ::Vector3(AZ::Constants::QuarterPi, 0, AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi) *
  564. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  565. EulerTestArgs{ AZ::Vector3(AZ::Constants::HalfPi, 0, AZ::Constants::QuarterPi),
  566. AZ::Quaternion::CreateRotationX(AZ::Constants::HalfPi) *
  567. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  568. EulerTestArgs{ AZ::Vector3(-AZ::Constants::QuarterPi, -AZ::Constants::HalfPi, AZ::Constants::QuarterPi),
  569. AZ::Quaternion::CreateRotationY(-AZ::Constants::HalfPi) *
  570. AZ::Quaternion::CreateRotationX(-AZ::Constants::QuarterPi) *
  571. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  572. EulerTestArgs{ AZ::Vector3(-AZ::Constants::QuarterPi, AZ::Constants::HalfPi, AZ::Constants::TwoOverPi),
  573. AZ::Quaternion::CreateRotationY(AZ::Constants::HalfPi) *
  574. AZ::Quaternion::CreateRotationX(-AZ::Constants::QuarterPi) *
  575. AZ::Quaternion::CreateRotationZ(AZ::Constants::TwoOverPi) }
  576. ));
  577. using AngleRadianTestFixtureZYX = ::testing::TestWithParam<EulerTestArgs>;
  578. TEST_P(AngleRadianTestFixtureZYX, EulerRadiansZYX)
  579. {
  580. auto& param = GetParam();
  581. EXPECT_THAT(AZ::Quaternion::CreateFromEulerRadiansZYX(param.euler), IsClose(param.result));
  582. // Test backwards computation Quaternion -> Tait-Brian angles
  583. const auto sourceDegrees = Vector3RadToDeg(param.euler);
  584. const bool isGimbleLock = fabs(fabs(sourceDegrees.GetY()) - 90.0f) < Constants::Tolerance;
  585. const auto anglesTaitBryanRadiansZYX = param.result.GetEulerRadiansZYX();
  586. const auto resultDegrees = Vector3RadToDeg(anglesTaitBryanRadiansZYX);
  587. const auto succeeded = resultDegrees.IsClose(sourceDegrees);
  588. EXPECT_TRUE((succeeded || isGimbleLock));
  589. }
  590. TEST_P(AngleRadianTestFixtureZYX, EulerDegreesZYX)
  591. {
  592. auto& param = GetParam();
  593. const auto sourceDegrees = Vector3RadToDeg(param.euler);
  594. EXPECT_THAT(AZ::Quaternion::CreateFromEulerDegreesZYX(sourceDegrees), IsClose(param.result));
  595. // Test backwards computation Quaternion -> Tait-Brian angles
  596. const bool isGimbleLock = fabs(fabs(sourceDegrees.GetY()) - 90.0f) < Constants::Tolerance;
  597. const auto anglesTaitBryanDegreesZYX = param.result.GetEulerDegreesZYX();
  598. const auto succeeded = anglesTaitBryanDegreesZYX.IsClose(sourceDegrees);
  599. EXPECT_TRUE((succeeded || isGimbleLock));
  600. }
  601. INSTANTIATE_TEST_SUITE_P(
  602. MATH_Quaternion,
  603. AngleRadianTestFixtureZYX,
  604. ::testing::Values(
  605. EulerTestArgs{ AZ::Vector3(AZ::Constants::QuarterPi, 0, 0), AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi) },
  606. EulerTestArgs{ AZ::Vector3(0, AZ::Constants::QuarterPi, 0), AZ::Quaternion::CreateRotationY(AZ::Constants::QuarterPi) },
  607. EulerTestArgs{ AZ::Vector3(0, 0, AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) },
  608. EulerTestArgs{ AZ::Vector3(-AZ::Constants::QuarterPi, 0, 0), AZ::Quaternion::CreateRotationX(-AZ::Constants::QuarterPi) },
  609. EulerTestArgs{ AZ::Vector3(0, -AZ::Constants::QuarterPi, 0), AZ::Quaternion::CreateRotationY(-AZ::Constants::QuarterPi) },
  610. EulerTestArgs{ AZ::Vector3(0, 0, -AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationZ(-AZ::Constants::QuarterPi) },
  611. EulerTestArgs{ AZ::Vector3(AZ::Constants::QuarterPi, AZ::Constants::QuarterPi, 0), AZ::Quaternion::CreateRotationY(AZ::Constants::QuarterPi) *
  612. AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi) },
  613. EulerTestArgs{ AZ::Vector3(0, AZ::Constants::QuarterPi, AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) *
  614. AZ::Quaternion::CreateRotationY(AZ::Constants::QuarterPi) },
  615. EulerTestArgs{ AZ::Vector3(AZ::Constants::QuarterPi, 0, AZ::Constants::QuarterPi), AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) *
  616. AZ::Quaternion::CreateRotationX(AZ::Constants::QuarterPi) },
  617. EulerTestArgs{ AZ::Vector3(AZ::Constants::HalfPi, 0, AZ::Constants::QuarterPi),
  618. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) *
  619. AZ::Quaternion::CreateRotationX(AZ::Constants::HalfPi) },
  620. EulerTestArgs{ AZ::Vector3(-AZ::Constants::QuarterPi, -AZ::Constants::HalfPi, AZ::Constants::QuarterPi),
  621. AZ::Quaternion::CreateRotationZ(AZ::Constants::QuarterPi) *
  622. AZ::Quaternion::CreateRotationY(-AZ::Constants::HalfPi) *
  623. AZ::Quaternion::CreateRotationX(-AZ::Constants::QuarterPi) },
  624. EulerTestArgs{ AZ::Vector3(-AZ::Constants::QuarterPi, AZ::Constants::HalfPi, AZ::Constants::TwoOverPi),
  625. AZ::Quaternion::CreateRotationZ(AZ::Constants::TwoOverPi) *
  626. AZ::Quaternion::CreateRotationY(AZ::Constants::HalfPi) *
  627. AZ::Quaternion::CreateRotationX(-AZ::Constants::QuarterPi) }
  628. ));
  629. } // namespace UnitTest