2
0

DMat44.inl 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310
  1. // Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
  2. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  3. // SPDX-License-Identifier: MIT
  4. #pragma once
  5. #include <Jolt/Math/DVec3.h>
  6. JPH_NAMESPACE_BEGIN
  7. DMat44::DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4) :
  8. mCol { inC1, inC2, inC3 },
  9. mCol3(inC4)
  10. {
  11. }
  12. DMat44::DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4) :
  13. mCol { inC1, inC2, inC3 },
  14. mCol3(inC4)
  15. {
  16. }
  17. DMat44::DMat44(Mat44Arg inM) :
  18. mCol { inM.GetColumn4(0), inM.GetColumn4(1), inM.GetColumn4(2) },
  19. mCol3(inM.GetTranslation())
  20. {
  21. }
  22. DMat44::DMat44(Mat44Arg inRot, DVec3Arg inT) :
  23. mCol { inRot.GetColumn4(0), inRot.GetColumn4(1), inRot.GetColumn4(2) },
  24. mCol3(inT)
  25. {
  26. }
  27. DMat44 DMat44::sZero()
  28. {
  29. return DMat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), DVec3::sZero());
  30. }
  31. DMat44 DMat44::sIdentity()
  32. {
  33. return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3::sZero());
  34. }
  35. DMat44 DMat44::sInverseRotationTranslation(QuatArg inR, DVec3Arg inT)
  36. {
  37. Mat44 m = Mat44::sRotation(inR.Conjugated());
  38. DMat44 dm(m, DVec3::sZero());
  39. dm.SetTranslation(-dm.Multiply3x3(inT));
  40. return dm;
  41. }
  42. bool DMat44::operator == (DMat44Arg inM2) const
  43. {
  44. return mCol[0] == inM2.mCol[0]
  45. && mCol[1] == inM2.mCol[1]
  46. && mCol[2] == inM2.mCol[2]
  47. && mCol3 == inM2.mCol3;
  48. }
  49. bool DMat44::IsClose(DMat44Arg inM2, float inMaxDistSq) const
  50. {
  51. for (int i = 0; i < 3; ++i)
  52. if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq))
  53. return false;
  54. return mCol3.IsClose(inM2.mCol3, double(inMaxDistSq));
  55. }
  56. DVec3 DMat44::operator * (Vec3Arg inV) const
  57. {
  58. #if defined(JPH_USE_AVX)
  59. __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
  60. t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
  61. t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
  62. return DVec3::sFixW(_mm256_add_pd(mCol3.mValue, _mm256_cvtps_pd(t)));
  63. #elif defined(JPH_USE_SSE)
  64. __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0)));
  65. t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1))));
  66. t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
  67. __m128d low = _mm_add_pd(mCol3.mValue.mLow, _mm_cvtps_pd(t));
  68. __m128d high = _mm_add_pd(mCol3.mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(t, t, _MM_SHUFFLE(2, 2, 2, 2))));
  69. return DVec3({ low, high });
  70. #elif defined(JPH_USE_NEON)
  71. float32x4_t t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0));
  72. t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1));
  73. t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2));
  74. float64x2_t low = vaddq_f64(mCol3.mValue.val[0], vcvt_f64_f32(vget_low_f32(t)));
  75. float64x2_t high = vaddq_f64(mCol3.mValue.val[1], vcvt_high_f64_f32(t));
  76. return DVec3::sFixW({ low, high });
  77. #else
  78. return DVec3(
  79. mCol3.mF64[0] + double(mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2]),
  80. mCol3.mF64[1] + double(mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2]),
  81. mCol3.mF64[2] + double(mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]));
  82. #endif
  83. }
  84. DVec3 DMat44::operator * (DVec3Arg inV) const
  85. {
  86. #if defined(JPH_USE_AVX)
  87. __m256d t = _mm256_add_pd(mCol3.mValue, _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0])));
  88. t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1])));
  89. t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2])));
  90. return DVec3::sFixW(t);
  91. #elif defined(JPH_USE_SSE)
  92. __m128d xxxx = _mm_set1_pd(inV.mF64[0]);
  93. __m128d yyyy = _mm_set1_pd(inV.mF64[1]);
  94. __m128d zzzz = _mm_set1_pd(inV.mF64[2]);
  95. __m128 col0 = mCol[0].mValue;
  96. __m128 col1 = mCol[1].mValue;
  97. __m128 col2 = mCol[2].mValue;
  98. __m128d t_low = _mm_add_pd(mCol3.mValue.mLow, _mm_mul_pd(_mm_cvtps_pd(col0), xxxx));
  99. t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy));
  100. t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz));
  101. __m128d t_high = _mm_add_pd(mCol3.mValue.mHigh, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx));
  102. t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy));
  103. t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz));
  104. return DVec3({ t_low, t_high });
  105. #elif defined(JPH_USE_NEON)
  106. float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0);
  107. float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1);
  108. float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0);
  109. float32x4_t col0 = mCol[0].mValue;
  110. float32x4_t col1 = mCol[1].mValue;
  111. float32x4_t col2 = mCol[2].mValue;
  112. float64x2_t t_low = vaddq_f64(mCol3.mValue.val[0], vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx));
  113. t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy));
  114. t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz));
  115. float64x2_t t_high = vaddq_f64(mCol3.mValue.val[1], vmulq_f64(vcvt_high_f64_f32(col0), xxxx));
  116. t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy));
  117. t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz));
  118. return DVec3::sFixW({ t_low, t_high });
  119. #else
  120. return DVec3(
  121. mCol3.mF64[0] + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2],
  122. mCol3.mF64[1] + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2],
  123. mCol3.mF64[2] + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]);
  124. #endif
  125. }
  126. DVec3 DMat44::Multiply3x3(DVec3Arg inV) const
  127. {
  128. #if defined(JPH_USE_AVX)
  129. __m256d t = _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0]));
  130. t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1])));
  131. t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2])));
  132. return DVec3::sFixW(t);
  133. #elif defined(JPH_USE_SSE)
  134. __m128d xxxx = _mm_set1_pd(inV.mF64[0]);
  135. __m128d yyyy = _mm_set1_pd(inV.mF64[1]);
  136. __m128d zzzz = _mm_set1_pd(inV.mF64[2]);
  137. __m128 col0 = mCol[0].mValue;
  138. __m128 col1 = mCol[1].mValue;
  139. __m128 col2 = mCol[2].mValue;
  140. __m128d t_low = _mm_mul_pd(_mm_cvtps_pd(col0), xxxx);
  141. t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy));
  142. t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz));
  143. __m128d t_high = _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx);
  144. t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy));
  145. t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz));
  146. return DVec3({ t_low, t_high });
  147. #elif defined(JPH_USE_NEON)
  148. float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0);
  149. float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1);
  150. float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0);
  151. float32x4_t col0 = mCol[0].mValue;
  152. float32x4_t col1 = mCol[1].mValue;
  153. float32x4_t col2 = mCol[2].mValue;
  154. float64x2_t t_low = vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx);
  155. t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy));
  156. t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz));
  157. float64x2_t t_high = vmulq_f64(vcvt_high_f64_f32(col0), xxxx);
  158. t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy));
  159. t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz));
  160. return DVec3::sFixW({ t_low, t_high });
  161. #else
  162. return DVec3(
  163. double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2],
  164. double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2],
  165. double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]);
  166. #endif
  167. }
  168. DMat44 DMat44::operator * (Mat44Arg inM) const
  169. {
  170. DMat44 result;
  171. // Rotation part
  172. #if defined(JPH_USE_SSE)
  173. for (int i = 0; i < 3; ++i)
  174. {
  175. __m128 c = inM.GetColumn4(i).mValue;
  176. __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
  177. t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
  178. t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
  179. result.mCol[i].mValue = t;
  180. }
  181. #elif defined(JPH_USE_NEON)
  182. for (int i = 0; i < 3; ++i)
  183. {
  184. Type c = inM.GetColumn4(i).mValue;
  185. Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
  186. t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
  187. t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
  188. result.mCol[i].mValue = t;
  189. }
  190. #else
  191. for (int i = 0; i < 3; ++i)
  192. {
  193. Vec4 coli = inM.GetColumn4(i);
  194. result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2];
  195. }
  196. #endif
  197. // Translation part
  198. result.mCol3 = *this * inM.GetTranslation();
  199. return result;
  200. }
  201. DMat44 DMat44::operator * (DMat44Arg inM) const
  202. {
  203. DMat44 result;
  204. // Rotation part
  205. #if defined(JPH_USE_SSE)
  206. for (int i = 0; i < 3; ++i)
  207. {
  208. __m128 c = inM.mCol[i].mValue;
  209. __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0)));
  210. t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1))));
  211. t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2))));
  212. result.mCol[i].mValue = t;
  213. }
  214. #elif defined(JPH_USE_NEON)
  215. for (int i = 0; i < 3; ++i)
  216. {
  217. Type c = inM.GetColumn4(i).mValue;
  218. Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0));
  219. t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1));
  220. t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2));
  221. result.mCol[i].mValue = t;
  222. }
  223. #else
  224. for (int i = 0; i < 3; ++i)
  225. {
  226. Vec4 coli = inM.mCol[i];
  227. result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2];
  228. }
  229. #endif
  230. // Translation part
  231. result.mCol3 = *this * inM.GetTranslation();
  232. return result;
  233. }
  234. void DMat44::SetRotation(Mat44Arg inRotation)
  235. {
  236. mCol[0] = inRotation.GetColumn4(0);
  237. mCol[1] = inRotation.GetColumn4(1);
  238. mCol[2] = inRotation.GetColumn4(2);
  239. }
  240. DMat44 DMat44::PreScaled(Vec3Arg inScale) const
  241. {
  242. return DMat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol3);
  243. }
  244. DMat44 DMat44::PostScaled(Vec3Arg inScale) const
  245. {
  246. Vec4 scale(inScale, 1);
  247. return DMat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], DVec3(scale) * mCol3);
  248. }
  249. DMat44 DMat44::PreTranslated(Vec3Arg inTranslation) const
  250. {
  251. return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation));
  252. }
  253. DMat44 DMat44::PreTranslated(DVec3Arg inTranslation) const
  254. {
  255. return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation));
  256. }
  257. DMat44 DMat44::PostTranslated(Vec3Arg inTranslation) const
  258. {
  259. return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation);
  260. }
  261. DMat44 DMat44::PostTranslated(DVec3Arg inTranslation) const
  262. {
  263. return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation);
  264. }
  265. DMat44 DMat44::Inversed() const
  266. {
  267. DMat44 m(GetRotation().Inversed3x3());
  268. m.mCol3 = -m.Multiply3x3(mCol3);
  269. return m;
  270. }
  271. DMat44 DMat44::InversedRotationTranslation() const
  272. {
  273. DMat44 m(GetRotation().Transposed3x3());
  274. m.mCol3 = -m.Multiply3x3(mCol3);
  275. return m;
  276. }
  277. JPH_NAMESPACE_END