2
0

Quat.inl 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401
  1. // Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
  2. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  3. // SPDX-License-Identifier: MIT
  4. JPH_NAMESPACE_BEGIN
  5. Quat Quat::operator * (QuatArg inRHS) const
  6. {
  7. #if defined(JPH_USE_SSE4_1)
  8. // Taken from: http://momchil-velikov.blogspot.nl/2013/10/fast-sse-quternion-multiplication.html
  9. __m128 abcd = mValue.mValue;
  10. __m128 xyzw = inRHS.mValue.mValue;
  11. __m128 t0 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(3, 3, 3, 3));
  12. __m128 t1 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 3, 0, 1));
  13. __m128 t3 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(0, 0, 0, 0));
  14. __m128 t4 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(1, 0, 3, 2));
  15. __m128 t5 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(1, 1, 1, 1));
  16. __m128 t6 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 0, 3, 1));
  17. // [d,d,d,d] * [z,w,x,y] = [dz,dw,dx,dy]
  18. __m128 m0 = _mm_mul_ps(t0, t1);
  19. // [a,a,a,a] * [y,x,w,z] = [ay,ax,aw,az]
  20. __m128 m1 = _mm_mul_ps(t3, t4);
  21. // [b,b,b,b] * [z,x,w,y] = [bz,bx,bw,by]
  22. __m128 m2 = _mm_mul_ps(t5, t6);
  23. // [c,c,c,c] * [w,z,x,y] = [cw,cz,cx,cy]
  24. __m128 t7 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(2, 2, 2, 2));
  25. __m128 t8 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 2, 0, 1));
  26. __m128 m3 = _mm_mul_ps(t7, t8);
  27. // [dz,dw,dx,dy] + -[ay,ax,aw,az] = [dz+ay,dw-ax,dx+aw,dy-az]
  28. __m128 e = _mm_addsub_ps(m0, m1);
  29. // [dx+aw,dz+ay,dy-az,dw-ax]
  30. e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(1, 3, 0, 2));
  31. // [dx+aw,dz+ay,dy-az,dw-ax] + -[bz,bx,bw,by] = [dx+aw+bz,dz+ay-bx,dy-az+bw,dw-ax-by]
  32. e = _mm_addsub_ps(e, m2);
  33. // [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz]
  34. e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 0, 1, 3));
  35. // [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz] + -[cw,cz,cx,cy] = [dz+ay-bx+cw,dw-ax-by-cz,dy-az+bw+cx,dx+aw+bz-cy]
  36. e = _mm_addsub_ps(e, m3);
  37. // [dw-ax-by-cz,dz+ay-bx+cw,dy-az+bw+cx,dx+aw+bz-cy]
  38. return Quat(Vec4(_mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 3, 1, 0))));
  39. #else
  40. float lx = mValue.GetX();
  41. float ly = mValue.GetY();
  42. float lz = mValue.GetZ();
  43. float lw = mValue.GetW();
  44. float rx = inRHS.mValue.GetX();
  45. float ry = inRHS.mValue.GetY();
  46. float rz = inRHS.mValue.GetZ();
  47. float rw = inRHS.mValue.GetW();
  48. float x = lw * rx + lx * rw + ly * rz - lz * ry;
  49. float y = lw * ry - lx * rz + ly * rw + lz * rx;
  50. float z = lw * rz + lx * ry - ly * rx + lz * rw;
  51. float w = lw * rw - lx * rx - ly * ry - lz * rz;
  52. return Quat(x, y, z, w);
  53. #endif
  54. }
  55. Quat Quat::sMultiplyImaginary(Vec3Arg inLHS, QuatArg inRHS)
  56. {
  57. #if defined(JPH_USE_SSE4_1)
  58. __m128 abc0 = inLHS.mValue;
  59. __m128 xyzw = inRHS.mValue.mValue;
  60. // [a,a,a,a] * [w,y,z,x] = [aw,ay,az,ax]
  61. __m128 aaaa = _mm_shuffle_ps(abc0, abc0, _MM_SHUFFLE(0, 0, 0, 0));
  62. __m128 xzyw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 1, 2, 0));
  63. __m128 axazayaw = _mm_mul_ps(aaaa, xzyw);
  64. // [b,b,b,b] * [z,x,w,y] = [bz,bx,bw,by]
  65. __m128 bbbb = _mm_shuffle_ps(abc0, abc0, _MM_SHUFFLE(1, 1, 1, 1));
  66. __m128 ywxz = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 0, 3, 1));
  67. __m128 bybwbxbz = _mm_mul_ps(bbbb, ywxz);
  68. // [c,c,c,c] * [w,z,x,y] = [cw,cz,cx,cy]
  69. __m128 cccc = _mm_shuffle_ps(abc0, abc0, _MM_SHUFFLE(2, 2, 2, 2));
  70. __m128 yxzw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 2, 0, 1));
  71. __m128 cycxczcw = _mm_mul_ps(cccc, yxzw);
  72. // [+aw,+ay,-az,-ax]
  73. __m128 e = _mm_xor_ps(axazayaw, _mm_set_ps(0.0f, 0.0f, -0.0f, -0.0f));
  74. // [+aw,+ay,-az,-ax] + -[bz,bx,bw,by] = [+aw+bz,+ay-bx,-az+bw,-ax-by]
  75. e = _mm_addsub_ps(e, bybwbxbz);
  76. // [+ay-bx,-ax-by,-az+bw,+aw+bz]
  77. e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 0, 1, 3));
  78. // [+ay-bx,-ax-by,-az+bw,+aw+bz] + -[cw,cz,cx,cy] = [+ay-bx+cw,-ax-by-cz,-az+bw+cx,+aw+bz-cy]
  79. e = _mm_addsub_ps(e, cycxczcw);
  80. // [-ax-by-cz,+ay-bx+cw,-az+bw+cx,+aw+bz-cy]
  81. return Quat(Vec4(_mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 3, 1, 0))));
  82. #else
  83. float lx = inLHS.GetX();
  84. float ly = inLHS.GetY();
  85. float lz = inLHS.GetZ();
  86. float rx = inRHS.mValue.GetX();
  87. float ry = inRHS.mValue.GetY();
  88. float rz = inRHS.mValue.GetZ();
  89. float rw = inRHS.mValue.GetW();
  90. float x = (lx * rw) + ly * rz - lz * ry;
  91. float y = -(lx * rz) + ly * rw + lz * rx;
  92. float z = (lx * ry) - ly * rx + lz * rw;
  93. float w = -(lx * rx) - ly * ry - lz * rz;
  94. return Quat(x, y, z, w);
  95. #endif
  96. }
  97. Quat Quat::sRotation(Vec3Arg inAxis, float inAngle)
  98. {
  99. // returns [inAxis * sin(0.5f * inAngle), cos(0.5f * inAngle)]
  100. JPH_ASSERT(inAxis.IsNormalized());
  101. Vec4 s, c;
  102. Vec4::sReplicate(0.5f * inAngle).SinCos(s, c);
  103. return Quat(Vec4::sSelect(Vec4(inAxis) * s, c, UVec4(0, 0, 0, 0xffffffffU)));
  104. }
  105. void Quat::GetAxisAngle(Vec3 &outAxis, float &outAngle) const
  106. {
  107. JPH_ASSERT(IsNormalized());
  108. Quat w_pos = EnsureWPositive();
  109. float abs_w = w_pos.GetW();
  110. if (abs_w >= 1.0f)
  111. {
  112. outAxis = Vec3::sZero();
  113. outAngle = 0.0f;
  114. }
  115. else
  116. {
  117. outAngle = 2.0f * ACos(abs_w);
  118. outAxis = w_pos.GetXYZ().NormalizedOr(Vec3::sZero());
  119. }
  120. }
  121. Quat Quat::sFromTo(Vec3Arg inFrom, Vec3Arg inTo)
  122. {
  123. /*
  124. Uses (inFrom = v1, inTo = v2):
  125. angle = arcos(v1 . v2 / |v1||v2|)
  126. axis = normalize(v1 x v2)
  127. Quaternion is then:
  128. s = sin(angle / 2)
  129. x = axis.x * s
  130. y = axis.y * s
  131. z = axis.z * s
  132. w = cos(angle / 2)
  133. Using identities:
  134. sin(2 * a) = 2 * sin(a) * cos(a)
  135. cos(2 * a) = cos(a)^2 - sin(a)^2
  136. sin(a)^2 + cos(a)^2 = 1
  137. This reduces to:
  138. x = (v1 x v2).x
  139. y = (v1 x v2).y
  140. z = (v1 x v2).z
  141. w = |v1||v2| + v1 . v2
  142. which then needs to be normalized because the whole equation was multiplied by 2 cos(angle / 2)
  143. */
  144. float len_v1_v2 = sqrt(inFrom.LengthSq() * inTo.LengthSq());
  145. float w = len_v1_v2 + inFrom.Dot(inTo);
  146. if (w == 0.0f)
  147. {
  148. if (len_v1_v2 == 0.0f)
  149. {
  150. // If either of the vectors has zero length, there is no rotation and we return identity
  151. return Quat::sIdentity();
  152. }
  153. else
  154. {
  155. // If vectors are perpendicular, take one of the many 180 degree rotations that exist
  156. return Quat(Vec4(inFrom.GetNormalizedPerpendicular(), 0));
  157. }
  158. }
  159. Vec3 v = inFrom.Cross(inTo);
  160. return Quat(Vec4(v, w)).Normalized();
  161. }
  162. template <class Random>
  163. Quat Quat::sRandom(Random &inRandom)
  164. {
  165. std::uniform_real_distribution<float> zero_to_one(0.0f, 1.0f);
  166. float x0 = zero_to_one(inRandom);
  167. float r1 = sqrt(1.0f - x0), r2 = sqrt(x0);
  168. std::uniform_real_distribution<float> zero_to_two_pi(0.0f, 2.0f * JPH_PI);
  169. Vec4 s, c;
  170. Vec4(zero_to_two_pi(inRandom), zero_to_two_pi(inRandom), 0, 0).SinCos(s, c);
  171. return Quat(s.GetX() * r1, c.GetX() * r1, s.GetY() * r2, c.GetY() * r2);
  172. }
  173. Quat Quat::sEulerAngles(Vec3Arg inAngles)
  174. {
  175. Vec4 half(0.5f * inAngles);
  176. Vec4 s, c;
  177. half.SinCos(s, c);
  178. float cx = c.GetX();
  179. float sx = s.GetX();
  180. float cy = c.GetY();
  181. float sy = s.GetY();
  182. float cz = c.GetZ();
  183. float sz = s.GetZ();
  184. return Quat(
  185. cz * sx * cy - sz * cx * sy,
  186. cz * cx * sy + sz * sx * cy,
  187. sz * cx * cy - cz * sx * sy,
  188. cz * cx * cy + sz * sx * sy);
  189. }
  190. Vec3 Quat::GetEulerAngles() const
  191. {
  192. float y_sq = GetY() * GetY();
  193. // X
  194. float t0 = 2.0f * (GetW() * GetX() + GetY() * GetZ());
  195. float t1 = 1.0f - 2.0f * (GetX() * GetX() + y_sq);
  196. // Y
  197. float t2 = 2.0f * (GetW() * GetY() - GetZ() * GetX());
  198. t2 = t2 > 1.0f? 1.0f : t2;
  199. t2 = t2 < -1.0f? -1.0f : t2;
  200. // Z
  201. float t3 = 2.0f * (GetW() * GetZ() + GetX() * GetY());
  202. float t4 = 1.0f - 2.0f * (y_sq + GetZ() * GetZ());
  203. return Vec3(ATan2(t0, t1), ASin(t2), ATan2(t3, t4));
  204. }
  205. Quat Quat::GetTwist(Vec3Arg inAxis) const
  206. {
  207. Quat twist(Vec4(GetXYZ().Dot(inAxis) * inAxis, GetW()));
  208. float twist_len = twist.LengthSq();
  209. if (twist_len != 0.0f)
  210. return twist / sqrt(twist_len);
  211. else
  212. return Quat::sIdentity();
  213. }
  214. void Quat::GetSwingTwist(Quat &outSwing, Quat &outTwist) const
  215. {
  216. float x = GetX(), y = GetY(), z = GetZ(), w = GetW();
  217. float s = sqrt(Square(w) + Square(x));
  218. if (s != 0.0f)
  219. {
  220. outTwist = Quat(x / s, 0, 0, w / s);
  221. outSwing = Quat(0, (w * y - x * z) / s, (w * z + x * y) / s, s);
  222. }
  223. else
  224. {
  225. // If both x and w are zero, this must be a 180 degree rotation around either y or z
  226. outTwist = Quat::sIdentity();
  227. outSwing = *this;
  228. }
  229. }
  230. Quat Quat::LERP(QuatArg inDestination, float inFraction) const
  231. {
  232. float scale0 = 1.0f - inFraction;
  233. return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(inFraction) * inDestination.mValue);
  234. }
  235. Quat Quat::SLERP(QuatArg inDestination, float inFraction) const
  236. {
  237. // Difference at which to LERP instead of SLERP
  238. const float delta = 0.0001f;
  239. // Calc cosine
  240. float sign_scale1 = 1.0f;
  241. float cos_omega = Dot(inDestination);
  242. // Adjust signs (if necessary)
  243. if (cos_omega < 0.0f)
  244. {
  245. cos_omega = -cos_omega;
  246. sign_scale1 = -1.0f;
  247. }
  248. // Calculate coefficients
  249. float scale0, scale1;
  250. if (1.0f - cos_omega > delta)
  251. {
  252. // Standard case (slerp)
  253. float omega = ACos(cos_omega);
  254. float sin_omega = Sin(omega);
  255. scale0 = Sin((1.0f - inFraction) * omega) / sin_omega;
  256. scale1 = sign_scale1 * Sin(inFraction * omega) / sin_omega;
  257. }
  258. else
  259. {
  260. // Quaternions are very close so we can do a linear interpolation
  261. scale0 = 1.0f - inFraction;
  262. scale1 = sign_scale1 * inFraction;
  263. }
  264. // Interpolate between the two quaternions
  265. return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(scale1) * inDestination.mValue).Normalized();
  266. }
  267. Vec3 Quat::operator * (Vec3Arg inValue) const
  268. {
  269. // Rotating a vector by a quaternion is done by: p' = q * (p, 0) * q^-1 (q^-1 = conjugated(q) for a unit quaternion)
  270. // Using Rodrigues formula: https://en.m.wikipedia.org/wiki/Euler%E2%80%93Rodrigues_formula
  271. // This is equivalent to: p' = p + 2 * (q.w * q.xyz x p + q.xyz x (q.xyz x p))
  272. //
  273. // This is:
  274. //
  275. // Vec3 xyz = GetXYZ();
  276. // Vec3 q_cross_p = xyz.Cross(inValue);
  277. // Vec3 q_cross_q_cross_p = xyz.Cross(q_cross_p);
  278. // Vec3 v = mValue.SplatW3() * q_cross_p + q_cross_q_cross_p;
  279. // return inValue + (v + v);
  280. //
  281. // But we can write out the cross products in a more efficient way:
  282. JPH_ASSERT(IsNormalized());
  283. Vec3 xyz = GetXYZ();
  284. Vec3 yzx = xyz.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
  285. Vec3 q_cross_p = (inValue.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * xyz - yzx * inValue).Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
  286. Vec3 q_cross_q_cross_p = (q_cross_p.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * xyz - yzx * q_cross_p).Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
  287. Vec3 v = mValue.SplatW3() * q_cross_p + q_cross_q_cross_p;
  288. return inValue + (v + v);
  289. }
  290. Vec3 Quat::InverseRotate(Vec3Arg inValue) const
  291. {
  292. JPH_ASSERT(IsNormalized());
  293. Vec3 xyz = GetXYZ(); // Needs to be negated, but we do this in the equations below
  294. Vec3 yzx = xyz.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
  295. Vec3 q_cross_p = (yzx * inValue - inValue.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * xyz).Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
  296. Vec3 q_cross_q_cross_p = (yzx * q_cross_p - q_cross_p.Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>() * xyz).Swizzle<SWIZZLE_Y, SWIZZLE_Z, SWIZZLE_X>();
  297. Vec3 v = mValue.SplatW3() * q_cross_p + q_cross_q_cross_p;
  298. return inValue + (v + v);
  299. }
  300. Vec3 Quat::RotateAxisX() const
  301. {
  302. // This is *this * Vec3::sAxisX() written out:
  303. JPH_ASSERT(IsNormalized());
  304. Vec4 t = mValue + mValue;
  305. return Vec3(t.SplatX() * mValue + (t.SplatW() * mValue.Swizzle<SWIZZLE_W, SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X>()).FlipSign<1, 1, -1, 1>() - Vec4(1, 0, 0, 0));
  306. }
  307. Vec3 Quat::RotateAxisY() const
  308. {
  309. // This is *this * Vec3::sAxisY() written out:
  310. JPH_ASSERT(IsNormalized());
  311. Vec4 t = mValue + mValue;
  312. return Vec3(t.SplatY() * mValue + (t.SplatW() * mValue.Swizzle<SWIZZLE_Z, SWIZZLE_W, SWIZZLE_X, SWIZZLE_Y>()).FlipSign<-1, 1, 1, 1>() - Vec4(0, 1, 0, 0));
  313. }
  314. Vec3 Quat::RotateAxisZ() const
  315. {
  316. // This is *this * Vec3::sAxisZ() written out:
  317. JPH_ASSERT(IsNormalized());
  318. Vec4 t = mValue + mValue;
  319. return Vec3(t.SplatZ() * mValue + (t.SplatW() * mValue.Swizzle<SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W, SWIZZLE_Z>()).FlipSign<1, -1, 1, 1>() - Vec4(0, 0, 1, 0));
  320. }
  321. void Quat::StoreFloat3(Float3 *outV) const
  322. {
  323. JPH_ASSERT(IsNormalized());
  324. EnsureWPositive().GetXYZ().StoreFloat3(outV);
  325. }
  326. Quat Quat::sLoadFloat3Unsafe(const Float3 &inV)
  327. {
  328. Vec3 v = Vec3::sLoadFloat3Unsafe(inV);
  329. float w = sqrt(max(1.0f - v.LengthSq(), 0.0f)); // It is possible that the length of v is a fraction above 1, and we don't want to introduce NaN's in that case so we clamp to 0
  330. return Quat(Vec4(v, w));
  331. }
  332. JPH_NAMESPACE_END