nlib
SimdMatrix.h
[詳解]
1 
2 #pragma once
3 #ifndef INCLUDE_NN_NLIB_SIMD_SIMDMATRIX_H_
4 #define INCLUDE_NN_NLIB_SIMD_SIMDMATRIX_H_
5 
10 // #include "nn/nlib/simd/SimdGeometry.h"
11 
12 NLIB_NAMESPACE_BEGIN
13 namespace simd {
14 
16  public:
17  static SimdMatrix __vectorcall LoadFloat4x4(const Float4x4* p) NLIB_NOEXCEPT;
18  static SimdMatrix __vectorcall LoadFloat3x4(const Float3x4* p) NLIB_NOEXCEPT;
19  static SimdMatrix __vectorcall LoadFloat4x3(const Float4x3* p) NLIB_NOEXCEPT;
20  static SimdMatrix __vectorcall LoadFloat3x3(const Float3x3* p) NLIB_NOEXCEPT;
21  static void __vectorcall StoreFloat4x4(Float4x4* p, SimdMatrixArg m) NLIB_NOEXCEPT;
22  static void __vectorcall StoreFloat3x4(Float3x4* p, SimdMatrixArg m) NLIB_NOEXCEPT;
23  static void __vectorcall StoreFloat4x3(Float4x3* p, SimdMatrixArg m) NLIB_NOEXCEPT;
24  static void __vectorcall StoreFloat3x3(Float3x3* p, SimdMatrixArg m) NLIB_NOEXCEPT;
25 
26  static SimdVector __vectorcall Determinant(SimdMatrixArg m) NLIB_NOEXCEPT;
27  static SimdMatrix __vectorcall Identity() NLIB_NOEXCEPT;
28  static SimdMatrix __vectorcall Inverse(SimdVector* det, SimdMatrixArg m) NLIB_NOEXCEPT;
29 
30  static bool __vectorcall IsIdentity(SimdMatrixArg m) NLIB_NOEXCEPT;
31  static bool __vectorcall IsInfinite(SimdMatrixArg m) NLIB_NOEXCEPT;
32  static bool __vectorcall IsNaN(SimdMatrixArg m) NLIB_NOEXCEPT;
33  static SimdMatrix __vectorcall Mult(SimdMatrixArg a, SimdMatrixArg b) NLIB_NOEXCEPT;
34  static SimdMatrix __vectorcall Transpose(SimdMatrixArg m) NLIB_NOEXCEPT;
35  static SimdMatrix __vectorcall MultTranspose(SimdMatrixArg a, SimdMatrixArg b) NLIB_NOEXCEPT;
36 
37  static SimdMatrix __vectorcall FromScaling(float scale_x, float scale_y,
38  float scale_z) NLIB_NOEXCEPT;
39  static SimdMatrix __vectorcall FromScaling(SimdVectorArg scale) NLIB_NOEXCEPT;
40  static SimdMatrix __vectorcall FromTranslation(float ofs_x, float ofs_y,
41  float ofs_z) NLIB_NOEXCEPT;
42  static SimdMatrix __vectorcall FromTranslation(SimdVectorArg ofs) NLIB_NOEXCEPT;
43 
44  static SimdMatrix __vectorcall FromRotationX(float sin_value, float cos_value) NLIB_NOEXCEPT;
45  static SimdMatrix __vectorcall FromRotationY(float sin_value, float cos_value) NLIB_NOEXCEPT;
46  static SimdMatrix __vectorcall FromRotationZ(float sin_value, float cos_value) NLIB_NOEXCEPT;
47  static SimdMatrix __vectorcall FromRotationAxisAndSinCos(SimdVectorArg axis_normalized,
48  float sin_value,
49  float cos_value) NLIB_NOEXCEPT;
50  static SimdMatrix __vectorcall FromRotationQuaternion(SimdQuaternionArg quat) NLIB_NOEXCEPT;
51  static SimdMatrix __vectorcall
52  FromRotationZXY(SimdVectorArg sin_xyz, SimdVectorArg cos_xyz) NLIB_NOEXCEPT;
53 
54  static SimdMatrix __vectorcall LookToLh(SimdVectorArg eye_pos, SimdVectorArg eye_dir_normalized,
55  SimdVectorArg up_dir_normalized) NLIB_NOEXCEPT;
56  static SimdMatrix __vectorcall LookAtLh(SimdVectorArg eye_pos, SimdVectorArg at_pos,
57  SimdVectorArg up_dir_normalized) NLIB_NOEXCEPT;
58  static SimdMatrix __vectorcall LookToRh(SimdVectorArg eye_pos, SimdVectorArg eye_dir_normalized,
59  SimdVectorArg up_dir_normalized) NLIB_NOEXCEPT;
60  static SimdMatrix __vectorcall LookAtRh(SimdVectorArg eye_pos, SimdVectorArg at_pos,
61  SimdVectorArg up_dir_normalized) NLIB_NOEXCEPT;
62 
63  static SimdMatrix __vectorcall
64  PerspectiveLh(float width, float height, float near_z, float far_z) NLIB_NOEXCEPT;
65  static SimdMatrix __vectorcall
66  PerspectiveRh(float width, float height, float near_z, float far_z) NLIB_NOEXCEPT;
67  static SimdMatrix __vectorcall PerspectiveFovLh(float half_fovy_sin, float half_fovy_cos,
68  float aspect, float near_z,
69  float far_z) NLIB_NOEXCEPT;
70  static SimdMatrix __vectorcall PerspectiveFovRh(float half_fovy_sin, float half_fovy_cos,
71  float aspect, float near_z,
72  float far_z) NLIB_NOEXCEPT;
73  static SimdMatrix __vectorcall PerspectiveOffCenterLh(float left, float right, float bottom,
74  float top, float near_z,
75  float far_z) NLIB_NOEXCEPT;
76  static SimdMatrix __vectorcall PerspectiveOffCenterRh(float left, float right, float bottom,
77  float top, float near_z,
78  float far_z) NLIB_NOEXCEPT;
79 
80  static SimdMatrix __vectorcall
81  OrthographicLh(float width, float height, float near_z, float far_z) NLIB_NOEXCEPT;
82  static SimdMatrix __vectorcall
83  OrthographicRh(float width, float height, float near_z, float far_z) NLIB_NOEXCEPT;
84  static SimdMatrix __vectorcall OrthographicOffCenterLh(float left, float right, float bottom,
85  float top, float near_z,
86  float far_z) NLIB_NOEXCEPT;
87  static SimdMatrix __vectorcall OrthographicOffCenterRh(float left, float right, float bottom,
88  float top, float near_z,
89  float far_z) NLIB_NOEXCEPT;
90 
91  static SimdMatrix __vectorcall
92  Shadow(SimdPlaneArg shadow_plane, SimdVector light_pos) NLIB_NOEXCEPT;
93  static SimdMatrix __vectorcall Reflect(SimdPlaneArg reflection_plane) NLIB_NOEXCEPT;
94  static void __vectorcall Decompose(SimdVector* scale, SimdMatrix* rot, SimdVector* trans,
96  // AffineTransform
97 
98  private:
99  Matrix(); // forbidden
100 };
101 
102 #ifndef NLIB_DOXYGEN
103 
104 #define NLIB_M(tp) inline tp __vectorcall
105 
106 NLIB_M(SimdMatrix) Matrix::LoadFloat4x4(const Float4x4* p) NLIB_NOEXCEPT {
107  SimdMatrix m;
108  m.r[0] = F128::LoadA16(&p->m[0][0]);
109  m.r[1] = F128::LoadA16(&p->m[1][0]);
110  m.r[2] = F128::LoadA16(&p->m[2][0]);
111  m.r[3] = F128::LoadA16(&p->m[3][0]);
112  return m;
113 }
114 
115 // Load and convert to column major order matrix
116 NLIB_M(SimdMatrix) Matrix::LoadFloat3x4(const Float3x4* p) NLIB_NOEXCEPT {
117  SimdMatrix m;
118  m.r[0] = F128::LoadA16(&p->m[0][0]);
119  m.r[1] = F128::LoadA16(&p->m[1][0]);
120  m.r[2] = F128::LoadA16(&p->m[2][0]);
121  m.r[3] = F128::Set0001();
122  NLIB_F128_TRANSPOSE(m.r[0], m.r[1], m.r[2], m.r[3]);
123  return m;
124 }
125 
126 NLIB_M(SimdMatrix) Matrix::LoadFloat4x3(const Float4x3* p) NLIB_NOEXCEPT {
127  f128 t0 = F128::LoadA16(&p->m[0][0]);
128  f128 t1 = F128::LoadA16(&p->m[1][1]);
129  f128 t2 = F128::LoadA16(&p->m[2][2]);
130  SimdMatrix m;
131  m.r[0] = F128::SetZeroToLane<3>(t0);
132  f128 tmp1 = F128::Permute<3, 4, 5, -1>(t0, t1);
133  m.r[1] = F128::SetZeroToLane<3>(tmp1);
134  f128 tmp2 = F128::Permute<2, 3, 4, -1>(t1, t2);
135  m.r[2] = F128::SetZeroToLane<3>(tmp2);
136  m.r[3] = F128::Permute<1, 2, 3, 7>(t2, F128::SetOne());
137  return m;
138 }
139 
140 NLIB_M(SimdMatrix) Matrix::LoadFloat3x3(const Float3x3* p) NLIB_NOEXCEPT {
141  f128 t0 = F128::LoadA4(&p->m[0][0]);
142  f128 t1 = F128::LoadA4(&p->m[1][0]);
143  f128 t2 = F128::LoadA4(&p->m[1][2]);
144  f128 zero = F128::SetZero();
145  SimdMatrix m;
146  m.r[0] = F128::SetZeroToLane<3>(t0);
147  m.r[1] = F128::SetZeroToLane<3>(t1);
148  m.r[2] = F128::Permute<1, 2, 3, 7>(t2, zero);
149  m.r[3] = F128::Set0001();
150  return m;
151 }
152 
153 inline void __vectorcall Matrix::StoreFloat4x4(Float4x4* p, SimdMatrixArg m) NLIB_NOEXCEPT {
154  F128::StoreA16(&p->m[0][0], m.r[0]);
155  F128::StoreA16(&p->m[1][0], m.r[1]);
156  F128::StoreA16(&p->m[2][0], m.r[2]);
157  F128::StoreA16(&p->m[3][0], m.r[3]);
158 }
159 
160 inline void __vectorcall Matrix::StoreFloat4x3(Float4x3* p, SimdMatrixArg m) NLIB_NOEXCEPT {
161  f128 t0 = F128::Permute<0, 1, 2, 4>(m.r[0], m.r[1]);
162  f128 t1 = F128::Permute<1, 2, 4, 5>(m.r[1], m.r[2]);
163  f128 t2 = F128::Permute<2, 4, 5, 6>(m.r[2], m.r[3]);
164  F128::StoreA16(&p->m[0][0], t0);
165  F128::StoreA16(&p->m[1][1], t1);
166  F128::StoreA16(&p->m[2][2], t2);
167 }
168 
169 inline void __vectorcall Matrix::StoreFloat3x3(Float3x3* p, SimdMatrixArg m) NLIB_NOEXCEPT {
170  f128 t0 = F128::Permute<0, 1, 2, 4>(m.r[0], m.r[1]);
171  f128 t1 = F128::Permute<1, 2, 4, 5>(m.r[1], m.r[2]);
172  F128::StoreA4(&p->m[0][0], t0);
173  F128::StoreA4(&p->m[1][1], t1);
174  p->m[2][2] = F128::GetFloatFromLane<2>(m.r[2]);
175 }
176 
177 // XMMatrixDeterminant
178 // r = determinant(m)
179 NLIB_M(f128) Matrix::Determinant(SimdMatrixArg m) NLIB_NOEXCEPT {
180  // a0 b0 c0 d0
181  // a1 b1 c1 d1
182  // a2 b2 c2 d2
183  // a3 b3 c3 d3
184  //
185  // a0|bcd123| - b0|acd123| + c0|abd123| + d0|abc123| -> SubAdd & Dot
186 
187  // |bcd123| = b1|cd23| - c1|bd23| + d1|bc23| -> lane0
188  // |acd123| = a1|cd23| - c1|ad23| + d1|ac23| -> lane1
189  // |abd123| = a1|bd23| - b1|ad23| + d1|ab23| -> lane2
190  // |abc123| = a1|bc23| - b1|ac23| + c1|ab23| -> lane3
191 
192  f128 c0det, c1det, c2det;
193  {
194  f128 ccbb_2 = F128::Swizzle<2, 2, 1, 1>(m.r[2]);
195  f128 ccbb_3 = F128::Swizzle<2, 2, 1, 1>(m.r[3]);
196 
197  f128 dddc_2 = F128::Swizzle<3, 3, 3, 2>(m.r[2]);
198  f128 dddc_3 = F128::Swizzle<3, 3, 3, 2>(m.r[3]);
199 
200  f128 baaa_2 = F128::Swizzle<1, 0, 0, 0>(m.r[2]);
201  f128 baaa_3 = F128::Swizzle<1, 0, 0, 0>(m.r[3]);
202 
203  f128 tmp0 = F128::Mult(ccbb_2, dddc_3);
204  f128 tmp1 = F128::Mult(baaa_2, dddc_3);
205  f128 tmp2 = F128::Mult(baaa_2, ccbb_3);
206 
207  c0det = F128::MultSub(dddc_2, ccbb_3, tmp0);
208  c1det = F128::MultSub(dddc_2, baaa_3, tmp1);
209  c2det = F128::MultSub(ccbb_2, baaa_3, tmp2);
210  }
211 
212  f128 ccbb_1 = F128::Swizzle<2, 2, 1, 1>(m.r[1]);
213  f128 baaa_1 = F128::Swizzle<1, 0, 0, 0>(m.r[1]);
214  f128 dddc_1 = F128::Swizzle<3, 3, 3, 2>(m.r[1]);
215  f128 r0x = F128::NegateEx<true, false, true, false>(m.r[0]);
216 
217  f128 det3_neg = F128::Mult(c1det, ccbb_1);
218  det3_neg = F128::MultSub(c0det, baaa_1, det3_neg);
219  det3_neg = F128::MultSub(c2det, dddc_1, det3_neg);
220  return Vector4::Dot(r0x, det3_neg);
221 }
222 
223 // XMMatrixIdentity
224 // r[0] = { 1, 0, 0, 0 }
225 // r[1] = { 0, 1, 0, 0 }
226 // r[2] = { 0, 0, 1, 0 }
227 // r[3] = { 0, 0, 0, 1 }
228 NLIB_M(SimdMatrix) Matrix::Identity() NLIB_NOEXCEPT {
229  SimdMatrix m;
230 #if defined(NLIB_NEON) && !defined(NLIB_F128_SIMD_NOUSE)
231  float32x2_t x10 = vcreate_f32(0x000000003F800000ULL);
232  float32x2_t x01 = vcreate_f32(0x3F80000000000000ULL);
233  float32x2_t x00 = vcreate_f32(0ULL);
234  m.r[0] = vcombine_f32(x10, x00);
235  m.r[1] = vcombine_f32(x01, x00);
236  m.r[2] = vcombine_f32(x00, x10);
237  m.r[3] = vcombine_f32(x00, x01);
238 #else
239  m.r[0] = F128::LoadA16(F128::v1000_);
240  m.r[1] = F128::LoadA16(F128::v0100_);
241  m.r[2] = F128::LoadA16(F128::v0010_);
242  m.r[3] = F128::LoadA16(F128::v0001_);
243 #endif
244  return m;
245 }
246 
247 // XMMatrixTranspose
248 // use NLIB_F128_TRANSPOSE(m.r[0], m.r[1], m.r[2], m.r[3]) if you can change 'm' itself
249 NLIB_M(SimdMatrix) Matrix::Transpose(SimdMatrixArg m) NLIB_NOEXCEPT {
250  SimdMatrix ret;
251  f128 r0 = m.r[0];
252  f128 r1 = m.r[1];
253  f128 r2 = m.r[2];
254  f128 r3 = m.r[3];
255  NLIB_F128_TRANSPOSE(r0, r1, r2, r3);
256  ret.r[0] = r0;
257  ret.r[1] = r1;
258  ret.r[2] = r2;
259  ret.r[3] = r3;
260  return ret;
261 }
262 
263 inline void __vectorcall Matrix::StoreFloat3x4(Float3x4* p, SimdMatrixArg m) NLIB_NOEXCEPT {
264  SimdMatrix M = Matrix::Transpose(m);
265  F128::StoreA16(&p->m[0][0], M.r[0]);
266  F128::StoreA16(&p->m[1][0], M.r[1]);
267  F128::StoreA16(&p->m[2][0], M.r[2]);
268 }
269 
270 #if 1
271 // XMMatrixInverse
272 NLIB_M(SimdMatrix) Matrix::Inverse(SimdVector* det, SimdMatrixArg m) NLIB_NOEXCEPT {
273  // faster on NEON
274  // SimdMatrix M = Transpose(m);
275  // M:
276  // a0 b0 c0 d0
277  // a1 b1 c1 d1
278  // a2 b2 c2 d2
279  // a3 b3 c3 d3
280 
281  // Inv:
282  // ( |bcd123|, -|acd123|, |abd123|, -|abc123|) / detm
283  // (-|bcd023|, |acd023|, -|abd023|, |abc023}) / detm
284  // ( |bcd012|, -|acd012|, |abd012|, -|abc012|) / detm
285  // (-|bcd013|, |acd013|, -|abd013|, |abc013|) / detm
286 
287  // row0:
288  // |bcd123| = b1|cd23| - c1|bd23| + d1|bc23| -> lane0
289  // |acd123| = a1|cd23| - c1|ad23| + d1|ac23| -> lane1
290  // |abd123| = a1|bd23| - b1|ad23| + d1|ab23| -> lane2
291  // |abc123| = a1|bc23| - b1|ac23| + c1|ab23| -> lane3
292 
293  // row1:
294  // |bcd023| = b0|cd23| - c0|bd23| + d0|bc23| -> lane0
295  // |acd023| = a0|cd23| - c0|ad23| + d0|ac23| -> lane1
296  // |abd023| = a0|bd23| - b0|ad23| + d0|ab23| -> lane2
297  // |abc023| = a0|bc23| - b0|ac23| + c0|ab23| -> lane3
298  f128 detValueReciprocal;
299  SimdMatrix ret;
300  f128 mydet;
301  {
302  f128 c0det, c1det, c2det;
303  {
304  f128 ccbb_2 = F128::Permute<2, 2, 6, 6>(m.r[2], m.r[1]);
305  f128 ccbb_3 = F128::Permute<3, 3, 7, 7>(m.r[2], m.r[1]);
306 
307  f128 dddc_2 = F128::Permute<2, 2, 2, 6>(m.r[3], m.r[2]);
308  f128 dddc_3 = F128::Permute<3, 3, 3, 7>(m.r[3], m.r[2]);
309 
310  f128 baaa_2 = F128::Permute<2, 6, 6, 6>(m.r[1], m.r[0]);
311  f128 baaa_3 = F128::Permute<3, 7, 7, 7>(m.r[1], m.r[0]);
312 
313  f128 tmp0 = F128::Mult(ccbb_2, dddc_3);
314  f128 tmp1 = F128::Mult(baaa_2, dddc_3);
315  f128 tmp2 = F128::Mult(baaa_2, ccbb_3);
316 
317  c0det = F128::MultSub(dddc_2, ccbb_3, tmp0);
318  c1det = F128::MultSub(dddc_2, baaa_3, tmp1);
319  c2det = F128::MultSub(ccbb_2, baaa_3, tmp2);
320  }
321  {
322  f128 baaa_1 = F128::Permute<1, 5, 5, 5>(m.r[1], m.r[0]);
323  f128 ccbb_1 = F128::Permute<1, 1, 5, 5>(m.r[2], m.r[1]);
324  f128 dddc_1 = F128::Permute<1, 1, 1, 5>(m.r[3], m.r[2]);
325 
326  f128 r0x_m = F128::Permute<0, -1, 4, -1>(m.r[0], m.r[2]);
327  f128 r0x_p = F128::Permute<-1, 0, -1, 4>(m.r[1], m.r[3]);
328  f128 r0x = F128::Permute<0, 5, 2, 7>(F128::Negate(r0x_m), r0x_p);
329 
330  f128 det3 = F128::Mult(c1det, ccbb_1);
331  det3 = F128::MultSub(c0det, baaa_1, det3);
332  det3 = F128::MultSub(c2det, dddc_1, det3);
333 
334  mydet = Vector4::Dot(r0x, det3);
335 
336  det3 = F128::NegateEx<true, false, true, false>(det3);
337  detValueReciprocal = F128::Recp(mydet);
338 
339  ret.r[0] = F128::Mult(detValueReciprocal, det3);
340  }
341  {
342  f128 baaa_0 = F128::Permute<0, 4, 4, 4>(m.r[1], m.r[0]);
343  f128 ccbb_0 = F128::Permute<0, 0, 4, 4>(m.r[2], m.r[1]);
344  f128 dddc_0 = F128::Permute<0, 0, 0, 4>(m.r[3], m.r[2]);
345 
346  f128 det3 = F128::Mult(c0det, baaa_0);
347  det3 = F128::MultAdd(c2det, dddc_0, det3);
348  det3 = F128::MultSub(c1det, ccbb_0, det3);
349  det3 = F128::NegateEx<true, false, true, false>(det3);
350  ret.r[1] = F128::Mult(detValueReciprocal, det3);
351  }
352  }
353 
354  // row3:
355  // |bcd012| = b2|cd01| - c2|bd01| + d2|bc01| -> lane0
356  // |acd012| = a2|cd01| - c2|ad01| + d2|ac01| -> lane1
357  // |abd012| = a2|bd01| - b2|ad01| + d2|ab01| -> lane2
358  // |abc012| = a2|bc01| - b2|ac01| + c2|ab01| -> lane3
359 
360  // row2:
361  // |bcd013| = b3|cd01| - c3|bd01| + d3|bc01| -> lane0
362  // |acd013| = a3|cd01| - c3|ad01| + d3|ac01| -> lane1
363  // |abd013| = a3|bd01| - b3|ad01| + d3|ab01| -> lane2
364  // |abc013| = a3|bc01| - b3|ac01| + c3|ab01| -> lane3
365  {
366  f128 c0det, c1det, c2det;
367  {
368  f128 ccbb_0 = F128::Permute<0, 0, 4, 4>(m.r[2], m.r[1]);
369  f128 ccbb_1 = F128::Permute<1, 1, 5, 5>(m.r[2], m.r[1]);
370 
371  f128 dddc_0 = F128::Permute<0, 0, 0, 4>(m.r[3], m.r[2]);
372  f128 dddc_1 = F128::Permute<1, 1, 1, 5>(m.r[3], m.r[2]);
373 
374  f128 baaa_0 = F128::Permute<0, 4, 4, 4>(m.r[1], m.r[0]);
375  f128 baaa_1 = F128::Permute<1, 5, 5, 5>(m.r[1], m.r[0]);
376 
377  f128 tmp0 = F128::Mult(ccbb_0, dddc_1);
378  f128 tmp1 = F128::Mult(baaa_0, dddc_1);
379  f128 tmp2 = F128::Mult(baaa_0, ccbb_1);
380 
381  c0det = F128::MultSub(dddc_0, ccbb_1, tmp0);
382  c1det = F128::MultSub(dddc_0, baaa_1, tmp1);
383  c2det = F128::MultSub(ccbb_0, baaa_1, tmp2);
384  }
385  {
386  f128 baaa_3 = F128::Permute<3, 7, 7, 7>(m.r[1], m.r[0]);
387  f128 ccbb_3 = F128::Permute<3, 3, 7, 7>(m.r[2], m.r[1]);
388  f128 dddc_3 = F128::Permute<3, 3, 3, 7>(m.r[3], m.r[2]);
389 
390  f128 det3 = F128::Mult(c1det, ccbb_3);
391  det3 = F128::MultSub(c0det, baaa_3, det3);
392  det3 = F128::MultSub(c2det, dddc_3, det3);
393  det3 = F128::NegateEx<true, false, true, false>(det3);
394 
395  ret.r[2] = F128::Mult(detValueReciprocal, det3);
396  }
397  {
398  f128 baaa_2 = F128::Permute<2, 6, 6, 6>(m.r[1], m.r[0]);
399  f128 ccbb_2 = F128::Permute<2, 2, 6, 6>(m.r[2], m.r[1]);
400  f128 dddc_2 = F128::Permute<2, 2, 2, 6>(m.r[3], m.r[2]);
401 
402  f128 det3 = F128::Mult(c0det, baaa_2);
403  det3 = F128::MultAdd(c2det, dddc_2, det3);
404  det3 = F128::MultSub(c1det, ccbb_2, det3);
405  det3 = F128::NegateEx<true, false, true, false>(det3);
406 
407  ret.r[3] = F128::Mult(detValueReciprocal, det3);
408  }
409  }
410  if (det) {
411  *det = mydet;
412  }
413  return ret;
414 }
415 #else
416 // XMMatrixInverse
417 NLIB_M(SimdMatrix) Matrix::Inverse(SimdVector* det, SimdMatrixArg m) NLIB_NOEXCEPT {
418  SimdMatrix M = Transpose(m);
419  // M:
420  // a0 b0 c0 d0
421  // a1 b1 c1 d1
422  // a2 b2 c2 d2
423  // a3 b3 c3 d3
424 
425  // Inv:
426  // ( |bcd123|, -|acd123|, |abd123|, -|abc123|) / detm
427  // (-|bcd023|, |acd023|, -|abd023|, |abc023}) / detm
428  // ( |bcd012|, -|acd012|, |abd012|, -|abc012|) / detm
429  // (-|bcd013|, |acd013|, -|abd013|, |abc013|) / detm
430 
431  // row0:
432  // |bcd123| = b1|cd23| - c1|bd23| + d1|bc23| -> lane0
433  // |acd123| = a1|cd23| - c1|ad23| + d1|ac23| -> lane1
434  // |abd123| = a1|bd23| - b1|ad23| + d1|ab23| -> lane2
435  // |abc123| = a1|bc23| - b1|ac23| + c1|ab23| -> lane3
436 
437  // row1:
438  // |bcd023| = b0|cd23| - c0|bd23| + d0|bc23| -> lane0
439  // |acd023| = a0|cd23| - c0|ad23| + d0|ac23| -> lane1
440  // |abd023| = a0|bd23| - b0|ad23| + d0|ab23| -> lane2
441  // |abc023| = a0|bc23| - b0|ac23| + c0|ab23| -> lane3
442  f128 detValueReciprocal;
443  SimdMatrix ret;
444  {
445  f128 c0det, c1det, c2det;
446  {
447  f128 ccbb_2 = F128::Swizzle<2, 2, 1, 1>(M.r[2]);
448  f128 ccbb_3 = F128::Swizzle<2, 2, 1, 1>(M.r[3]);
449 
450  f128 dddc_2 = F128::Swizzle<3, 3, 3, 2>(M.r[2]);
451  f128 dddc_3 = F128::Swizzle<3, 3, 3, 2>(M.r[3]);
452 
453  f128 baaa_2 = F128::Swizzle<1, 0, 0, 0>(M.r[2]);
454  f128 baaa_3 = F128::Swizzle<1, 0, 0, 0>(M.r[3]);
455 
456  f128 tmp0 = F128::Mult(ccbb_2, dddc_3);
457  f128 tmp1 = F128::Mult(baaa_2, dddc_3);
458  f128 tmp2 = F128::Mult(baaa_2, ccbb_3);
459 
460  c0det = F128::MultSub(dddc_2, ccbb_3, tmp0);
461  c1det = F128::MultSub(dddc_2, baaa_3, tmp1);
462  c2det = F128::MultSub(ccbb_2, baaa_3, tmp2);
463  }
464  {
465  f128 baaa_1 = F128::Swizzle<1, 0, 0, 0>(M.r[1]);
466  f128 ccbb_1 = F128::Swizzle<2, 2, 1, 1>(M.r[1]);
467  f128 dddc_1 = F128::Swizzle<3, 3, 3, 2>(M.r[1]);
468  f128 r0x = F128::NegateEx<true, false, true, false>(M.r[0]);
469 
470  f128 det3 = F128::Mult(c1det, ccbb_1);
471  det3 = F128::MultSub(c0det, baaa_1, det3);
472  det3 = F128::MultSub(c2det, dddc_1, det3);
473 
474  detValueReciprocal = Vector4::Dot(r0x, det3);
475  if (det) {
476  *det = detValueReciprocal;
477  }
478 
479  det3 = F128::NegateEx<true, false, true, false>(det3);
480  detValueReciprocal = F128::Recp(detValueReciprocal);
481 
482  ret.r[0] = F128::Mult(detValueReciprocal, det3);
483  }
484  {
485  f128 baaa_0 = F128::Swizzle<1, 0, 0, 0>(M.r[0]);
486  f128 ccbb_0 = F128::Swizzle<2, 2, 1, 1>(M.r[0]);
487  f128 dddc_0 = F128::Swizzle<3, 3, 3, 2>(M.r[0]);
488 
489  f128 det3 = F128::Mult(c0det, baaa_0);
490  det3 = F128::MultAdd(c2det, dddc_0, det3);
491  det3 = F128::MultSub(c1det, ccbb_0, det3);
492  det3 = F128::NegateEx<true, false, true, false>(det3);
493  ret.r[1] = F128::Mult(detValueReciprocal, det3);
494  }
495  }
496 
497  // row3:
498  // |bcd012| = b2|cd01| - c2|bd01| + d2|bc01| -> lane0
499  // |acd012| = a2|cd01| - c2|ad01| + d2|ac01| -> lane1
500  // |abd012| = a2|bd01| - b2|ad01| + d2|ab01| -> lane2
501  // |abc012| = a2|bc01| - b2|ac01| + c2|ab01| -> lane3
502 
503  // row2:
504  // |bcd013| = b3|cd01| - c3|bd01| + d3|bc01| -> lane0
505  // |acd013| = a3|cd01| - c3|ad01| + d3|ac01| -> lane1
506  // |abd013| = a3|bd01| - b3|ad01| + d3|ab01| -> lane2
507  // |abc013| = a3|bc01| - b3|ac01| + c3|ab01| -> lane3
508  {
509  f128 c0det, c1det, c2det;
510  {
511  f128 ccbb_0 = F128::Swizzle<2, 2, 1, 1>(M.r[0]);
512  f128 ccbb_1 = F128::Swizzle<2, 2, 1, 1>(M.r[1]);
513 
514  f128 dddc_0 = F128::Swizzle<3, 3, 3, 2>(M.r[0]);
515  f128 dddc_1 = F128::Swizzle<3, 3, 3, 2>(M.r[1]);
516 
517  f128 baaa_0 = F128::Swizzle<1, 0, 0, 0>(M.r[0]);
518  f128 baaa_1 = F128::Swizzle<1, 0, 0, 0>(M.r[1]);
519 
520  f128 tmp0 = F128::Mult(ccbb_0, dddc_1);
521  f128 tmp1 = F128::Mult(baaa_0, dddc_1);
522  f128 tmp2 = F128::Mult(baaa_0, ccbb_1);
523 
524  c0det = F128::MultSub(dddc_0, ccbb_1, tmp0);
525  c1det = F128::MultSub(dddc_0, baaa_1, tmp1);
526  c2det = F128::MultSub(ccbb_0, baaa_1, tmp2);
527  }
528  {
529  f128 baaa_3 = F128::Swizzle<1, 0, 0, 0>(M.r[3]);
530  f128 ccbb_3 = F128::Swizzle<2, 2, 1, 1>(M.r[3]);
531  f128 dddc_3 = F128::Swizzle<3, 3, 3, 2>(M.r[3]);
532 
533  f128 det3 = F128::Mult(c1det, ccbb_3);
534  det3 = F128::MultSub(c0det, baaa_3, det3);
535  det3 = F128::MultSub(c2det, dddc_3, det3);
536  det3 = F128::NegateEx<true, false, true, false>(det3);
537 
538  ret.r[2] = F128::Mult(detValueReciprocal, det3);
539  }
540  {
541  f128 baaa_2 = F128::Swizzle<1, 0, 0, 0>(M.r[2]);
542  f128 ccbb_2 = F128::Swizzle<2, 2, 1, 1>(M.r[2]);
543  f128 dddc_2 = F128::Swizzle<3, 3, 3, 2>(M.r[2]);
544 
545  f128 det3 = F128::Mult(c0det, baaa_2);
546  det3 = F128::MultAdd(c2det, dddc_2, det3);
547  det3 = F128::MultSub(c1det, ccbb_2, det3);
548  det3 = F128::NegateEx<true, false, true, false>(det3);
549 
550  ret.r[3] = F128::Mult(detValueReciprocal, det3);
551  }
552  }
553  return ret;
554 }
555 #endif
556 
557 // XMMatrixIsIdentity
558 // true if m is an indentity matrix
559 NLIB_M(bool) Matrix::IsIdentity(SimdMatrixArg m) NLIB_NOEXCEPT { // NOLINT
560  SimdMatrix x = Matrix::Identity();
561  f128 cmp0 = F128::CmpEq(x.r[0], m.r[0]);
562  f128 cmp1 = F128::CmpEq(x.r[1], m.r[1]);
563  f128 cmp2 = F128::CmpEq(x.r[2], m.r[2]);
564  f128 cmp3 = F128::CmpEq(x.r[3], m.r[3]);
565  cmp0 = F128::And(cmp0, cmp1);
566  cmp2 = F128::And(cmp2, cmp3);
567  cmp0 = F128::And(cmp0, cmp2);
568  return F128::IsAllMaskTrue(cmp0);
569 }
570 
571 // XMMatrixIsInfinite
572 // true if there is (i, j) which satisfies isinf(m[i][j])
573 NLIB_M(bool) Matrix::IsInfinite(SimdMatrixArg m) NLIB_NOEXCEPT { // NOLINT
574 #ifdef NLIB_F128_SIMD_NOUSE
575  f128 cmp0 = F128::IsInfinite(m.r[0]);
576  f128 cmp1 = F128::IsInfinite(m.r[1]);
577  f128 cmp2 = F128::IsInfinite(m.r[2]);
578  f128 cmp3 = F128::IsInfinite(m.r[3]);
579  cmp0 = F128::Or(cmp0, cmp1);
580  cmp2 = F128::Or(cmp2, cmp3);
581  cmp0 = F128::Or(cmp0, cmp2);
582  return !F128::IsAllMaskFalse(cmp0);
583 #else
584  f128 inf_value = F128::SetInfinity();
585  f128 cmp0 = F128::CmpEq(inf_value, F128::Abs(m.r[0]));
586  f128 cmp1 = F128::CmpEq(inf_value, F128::Abs(m.r[1]));
587  f128 cmp2 = F128::CmpEq(inf_value, F128::Abs(m.r[2]));
588  f128 cmp3 = F128::CmpEq(inf_value, F128::Abs(m.r[3]));
589  cmp0 = F128::Or(cmp0, cmp1);
590  cmp2 = F128::Or(cmp2, cmp3);
591  cmp0 = F128::Or(cmp0, cmp2);
592  return !F128::IsAllMaskFalse(cmp0);
593 #endif
594 }
595 
596 // XMMatrixIsNaN
597 // true if there is (i, j) which satisfies isnan(m[i][j])
598 NLIB_M(bool) Matrix::IsNaN(SimdMatrixArg m) NLIB_NOEXCEPT { // NOLINT
599  f128 cmp0 = F128::IsNaN(m.r[0]);
600  f128 cmp1 = F128::IsNaN(m.r[1]);
601  f128 cmp2 = F128::IsNaN(m.r[2]);
602  f128 cmp3 = F128::IsNaN(m.r[3]);
603  cmp0 = F128::Or(cmp0, cmp1);
604  cmp2 = F128::Or(cmp2, cmp3);
605  cmp0 = F128::Or(cmp0, cmp2);
606  return !F128::IsAllMaskFalse(cmp0);
607 }
608 
609 // XMMatrixMultiply
610 // r = a * b
611 NLIB_M(SimdMatrix) Matrix::Mult(SimdMatrixArg a, SimdMatrixArg b) NLIB_NOEXCEPT {
612  SimdMatrix m;
613  m.r[0] = Vector4::Transform(a.r[0], b);
614  m.r[1] = Vector4::Transform(a.r[1], b);
615  m.r[2] = Vector4::Transform(a.r[2], b);
616  m.r[3] = Vector4::Transform(a.r[3], b);
617  return m;
618 }
619 
620 // XMMatrixMultiplyTranspose
621 // r = transpose(a * b)
622 NLIB_M(SimdMatrix) Matrix::MultTranspose(SimdMatrixArg a, SimdMatrixArg b) NLIB_NOEXCEPT {
623  f128 r0 = Vector4::Transform(a.r[0], b);
624  f128 r1 = Vector4::Transform(a.r[1], b);
625  f128 r2 = Vector4::Transform(a.r[2], b);
626  f128 r3 = Vector4::Transform(a.r[3], b);
627  SimdMatrix ret;
628  NLIB_F128_TRANSPOSE(r0, r1, r2, r3);
629  ret.r[0] = r0;
630  ret.r[1] = r1;
631  ret.r[2] = r2;
632  ret.r[3] = r3;
633  return ret;
634 }
635 
636 // XMMatrixScaling
637 // r[0] = { x, 0, 0, 0 }
638 // r[1] = { 0, y, 0, 0 }
639 // r[2] = { 0, 0, z, 0 }
640 // r[3] = { 0, 0, 0, 1 }
641 NLIB_M(SimdMatrix) Matrix::FromScaling(float scale_x, float scale_y, float scale_z) NLIB_NOEXCEPT {
642  SimdMatrix m;
643  f128 zero = F128::SetZero();
644  m.r[0] = F128::SetFloatToLane<0>(zero, scale_x);
645  m.r[1] = F128::SetFloatToLane<1>(zero, scale_y);
646  m.r[2] = F128::SetFloatToLane<2>(zero, scale_z);
647  m.r[3] = F128::Set0001();
648  return m;
649 }
650 
651 // XMMatrixScalingFromVector
652 // r[0] = { x, 0, 0, 0 }
653 // r[1] = { 0, y, 0, 0 }
654 // r[2] = { 0, 0, z, 0 }
655 // r[3] = { 0, 0, 0, 1 }
656 NLIB_M(SimdMatrix) Matrix::FromScaling(SimdVectorArg scale) NLIB_NOEXCEPT {
657  SimdMatrix m;
658  f128 zero = F128::SetZero();
659  m.r[0] = F128::Splat<false, true, true, true>(scale, zero);
660  m.r[1] = F128::Splat<true, false, true, true>(scale, zero);
661  m.r[2] = F128::Splat<true, true, false, true>(scale, zero);
662  m.r[3] = F128::Set0001();
663  return m;
664 }
665 
666 // XMMatrixTranslation
667 // r[0] = { 1, 0, 0, 0 }
668 // r[1] = { 0, 1, 0, 0 }
669 // r[2] = { 0, 0, 1, 0 }
670 // r[3] = { x, y, z, 1 }
671 NLIB_M(SimdMatrix) Matrix::FromTranslation(float ofs_x, float ofs_y, float ofs_z) NLIB_NOEXCEPT {
672  SimdMatrix m;
673  m.r[0] = F128::Set1000();
674  m.r[1] = F128::Set0100();
675  m.r[2] = F128::Set0010();
676  m.r[3] = F128::SetValue(ofs_x, ofs_y, ofs_z, 1.f);
677  return m;
678 }
679 
680 // XMMatrixTranslationFromVector
681 // r[0] = { 1, 0, 0, 0 }
682 // r[1] = { 0, 1, 0, 0 }
683 // r[2] = { 0, 0, 1, 0 }
684 // r[3] = { x, y, z, 1 }
685 NLIB_M(SimdMatrix) Matrix::FromTranslation(SimdVectorArg ofs) NLIB_NOEXCEPT {
686  SimdMatrix m;
687  m.r[0] = F128::Set1000();
688  m.r[1] = F128::Set0100();
689  m.r[2] = F128::Set0010();
690  m.r[3] = F128::Permute<0, 1, 2, 4>(ofs, m.r[0]);
691  return m;
692 }
693 
694 // XMMatrixRotationX
695 // r[0] = { 1 0 0 0 }
696 // r[1] = { 0 c s 0 }
697 // r[2] = { 0 -s c 0 }
698 // r[3] = { 0 0 0 1 }
699 NLIB_M(SimdMatrix) Matrix::FromRotationX(float sin_value, float cos_value) NLIB_NOEXCEPT {
700  SimdMatrix m;
701  SimdVector zero = F128::SetZero();
702  f128 r1 = F128::SetFloatToLane<1>(zero, cos_value);
703  r1 = F128::SetFloatToLane<2>(r1, sin_value);
704  f128 r2 = F128::SetFloatToLane<1>(zero, -sin_value);
705  r2 = F128::SetFloatToLane<2>(r2, cos_value);
706 
707  m.r[0] = F128::Set1000();
708  m.r[1] = r1;
709  m.r[2] = r2;
710  m.r[3] = F128::Set0001();
711  return m;
712 }
713 
714 // XMMatrixRotationY
715 // r[0] = { c 0 -s 0 }
716 // r[1] = { 0 1 0 0 }
717 // r[2] = { s 0 c 0 }
718 // r[3] = { 0 0 0 1 }
719 NLIB_M(SimdMatrix) Matrix::FromRotationY(float sin_value, float cos_value) NLIB_NOEXCEPT {
720  SimdMatrix m;
721  SimdVector zero = F128::SetZero();
722  f128 r0 = F128::SetFloatToLane<0>(zero, cos_value);
723  r0 = F128::SetFloatToLane<2>(r0, -sin_value);
724  f128 r2 = F128::SetFloatToLane<0>(zero, sin_value);
725  r2 = F128::SetFloatToLane<2>(r2, cos_value);
726 
727  m.r[0] = r0;
728  m.r[1] = F128::Set0100();
729  m.r[2] = r2;
730  m.r[3] = F128::Set0001();
731  return m;
732 }
733 
734 // XMMatrixRotationZ
735 // r[0] = { c s 0 0 }
736 // r[1] = { -s c 0 0 }
737 // r[2] = { 0 0 1 0 }
738 // r[3] = { 0 0 0 1 }
739 NLIB_M(SimdMatrix) Matrix::FromRotationZ(float sin_value, float cos_value) NLIB_NOEXCEPT {
740  SimdMatrix m;
741  SimdVector zero = F128::SetZero();
742  f128 r0 = F128::SetFloatToLane<0>(zero, cos_value);
743  r0 = F128::SetFloatToLane<1>(r0, sin_value);
744  f128 r1 = F128::SetFloatToLane<0>(zero, -sin_value);
745  r1 = F128::SetFloatToLane<1>(r1, cos_value);
746 
747  m.r[0] = r0;
748  m.r[1] = r1;
749  m.r[2] = F128::Set0010();
750  m.r[3] = F128::Set0001();
751  return m;
752 }
753 
754 // XMMatrixRotationAxis
755 // The result may be different from DirectXMath's XMMatrixRotationAxis.
756 // It is because the calculation order is different.
757 NLIB_M(SimdMatrix) Matrix::FromRotationAxisAndSinCos(SimdVectorArg axis_normalized, float sin_value,
758  float cos_value) NLIB_NOEXCEPT {
759  // m00, m11, m22, *
760  f128 diagonal, c1;
761  {
762  f128 nn = F128::Mult(axis_normalized, axis_normalized);
763  f128 c = F128::SetValue(cos_value, each_float);
764  c1 = F128::SetValue(1.f - cos_value, each_float);
765  diagonal = F128::MultAdd(c1, nn, c);
766  diagonal = F128::SetZeroToLane<3>(diagonal);
767  }
768 
769  f128 zxy = F128::Swizzle<2, 0, 1, 2>(axis_normalized);
770  f128 s = F128::SetValue(sin_value, each_float);
771  f128 xy_yz_xz = F128::Mult(axis_normalized, F128::Swizzle<1, 2, 0, 3>(axis_normalized));
772  xy_yz_xz = F128::Mult(c1, xy_yz_xz);
773  f128 plus = F128::MultAdd(s, zxy, xy_yz_xz); // xy(1-c)+sz, yz(1-c)+sx, xz(1-c)+sy
774  f128 minus = F128::MultSub(s, zxy, xy_yz_xz); // xy(1-c)-sz, yz(1-c)-sx, xz(1-c)-sy
775 
776  f128 t0 = F128::Permute<1, 2, 4, 5>(plus, minus);
777  f128 t1 = F128::Permute<0, 6, 2, 3>(plus, minus);
778 
779  SimdMatrix m;
780  m.r[0] = F128::Permute<4, 0, 1, 7>(t1, diagonal);
781  m.r[1] = F128::Permute<2, 5, 0, 7>(t0, diagonal);
782  m.r[2] = F128::Permute<1, 3, 6, 7>(t0, diagonal);
783  m.r[3] = F128::Set0001();
784  return m;
785 }
786 
787 // XMMatrixRotationQuaternion
788 // the result may differ from directxmath's one because the computation sequence is different.
789 NLIB_M(SimdMatrix) Matrix::FromRotationQuaternion(SimdQuaternionArg quat) NLIB_NOEXCEPT {
790  // m00, m11, m22, *
791  f128 q2 = F128::Add(quat, quat);
792  f128 qq2 = F128::Mult(quat, q2);
793  f128 t0, t1;
794 
795  t0 = F128::Swizzle<1, 0, 0, -1>(qq2); // 2y^2, 2x^2, 2x^2, *
796  t1 = F128::Swizzle<2, 2, 1, -1>(qq2); // 2z^2, 2z^2, 2y^2, *
797  // 1-2y^2-2z^2, 1-2x^2-2z^2, 1-2x^2-2y^2, 0
798  f128 diagonal = F128::Sub(F128::Sub(F128::SetOne(), t0), t1);
799  diagonal = F128::SetFloatToLane<3>(diagonal, 0.f);
800 
801  t0 = F128::Swizzle<1, 0, 0, -1>(quat); // y, x, x, *
802  t1 = F128::Swizzle<2, 2, 1, -1>(q2); // 2z, 2z, 2y, *
803  f128 yz_xz_xy = F128::Mult(t0, t1); // 2yz, 2xz, 2xy, *
804 
805  t0 = F128::SetValue<3>(quat, each_select32); // w, w, w, *
806  f128 wx_wy_wz = F128::Mult(q2, t0); // 2wx, 2wy, 2wz
807 
808  f128 plus = F128::Add(yz_xz_xy, wx_wy_wz); // 2yz+2wx, 2xz+2wy, 2xy+2wz, *
809  f128 minus = F128::Sub(yz_xz_xy, wx_wy_wz); // 2yz-2wx, 2xz-2wy, 2xy-2wz, *
810 
811  t0 = F128::Permute<1, 2, 4, 5>(plus, minus);
812  t1 = F128::Permute<0, 6, 2, 3>(plus, minus);
813 
814  SimdMatrix m;
815  m.r[0] = F128::Permute<4, 1, 3, 7>(t0, diagonal);
816  m.r[1] = F128::Permute<1, 5, 0, 7>(t1, diagonal);
817  m.r[2] = F128::Permute<0, 2, 6, 7>(t0, diagonal);
818  m.r[3] = F128::Set0001();
819  return m;
820 }
821 
822 // XMMatrixRotationRollPitchYaw
823 NLIB_M(SimdMatrix) Matrix::FromRotationZXY(SimdVectorArg sin_xyz,
824  SimdVectorArg cos_xyz) NLIB_NOEXCEPT {
825  // CzCy+SzSxSy SzCx -CzSy+SzSxCy 0
826  // -SzCy+CzSxSy CzCx SzSy+CzSxCy 0
827  // CxSy -Sx CxCy 0
828  f128 m00_12_02_10;
829  {
830  f128 sz_cz_sz_cz = F128::Permute<2, 6, 2, 6>(sin_xyz, cos_xyz);
831  f128 sy_cy_cy_sy = F128::Permute<1, 5, 5, 1>(sin_xyz, cos_xyz);
832  f128 tmp = F128::Mult(sz_cz_sz_cz, sy_cy_cy_sy);
833  m00_12_02_10 = F128::Mult<0>(sin_xyz, tmp, each_select32);
834  tmp = F128::Swizzle<1, 0, 3, 2>(tmp);
835  tmp = F128::NegateEx<false, false, true, true>(tmp);
836  m00_12_02_10 = F128::Add(tmp, m00_12_02_10);
837  }
838  f128 m20_01_22_11;
839  {
840  f128 sy_sz_cy_cz = F128::Permute<1, 2, 5, 6>(sin_xyz, cos_xyz);
841  m20_01_22_11 = F128::Mult<0>(cos_xyz, sy_sz_cy_cz, each_select32);
842  }
843 
844  f128 r2 = F128::SetFloatToLane<3>(m20_01_22_11, 0.f);
845  f128 r1 = F128::Permute<3, 7, 1, 1>(m00_12_02_10, m20_01_22_11);
846 
847  SimdMatrix m;
848  m.r[0] = F128::Permute<0, 5, 2, 7>(m00_12_02_10, r2);
849  m.r[1] = F128::SetZeroToLane<3>(r1);
850  m.r[2] = F128::SetFloatToLane<1>(r2, -F128::GetFloatFromLane<0>(sin_xyz));
851  m.r[3] = F128::Set0001();
852  return m;
853 }
854 
855 // XMMatrixLookToLH
856 NLIB_M(SimdMatrix) Matrix::LookToLh(SimdVectorArg eye_pos, SimdVectorArg eye_dir_normalized,
857  SimdVectorArg up_dir_normalized) NLIB_NOEXCEPT {
858  SimdVector r0 = Vector3::Cross(up_dir_normalized, eye_dir_normalized);
859  SimdVector r1 = Vector3::Cross(eye_dir_normalized, r0);
860  SimdVector neg = F128::Negate(eye_pos);
861 #ifdef NLIB_NEON
862  neg = F128::SetZeroToLane<3>(neg);
863  f128 d012 = Vector4::Dot3(neg, r0, r1, eye_dir_normalized);
864  SimdMatrix m;
865  m.r[0] = r0;
866  m.r[1] = r1;
867  m.r[2] = eye_dir_normalized;
868  m.r[3] = F128::Set0001();
869  m = Transpose(m);
870  m.r[3] = F128::SetFloatToLane<3>(d012, 1.f);
871  return m;
872 #else
873  f128 d0 = Vector3::Dot(r0, neg);
874  f128 d1 = Vector3::Dot(r1, neg);
875  f128 d2 = Vector3::Dot(eye_dir_normalized, neg);
876  SimdMatrix m;
877  m.r[0] = F128::Splat<false, false, false, true>(r0, d0);
878  m.r[1] = F128::Splat<false, false, false, true>(r1, d1);
879  m.r[2] = F128::Splat<false, false, false, true>(eye_dir_normalized, d2);
880  m.r[3] = F128::Set0001();
881  return Transpose(m);
882 #endif
883 }
884 
885 // XMMatrixLookAtLH
886 NLIB_M(SimdMatrix) Matrix::LookAtLh(SimdVectorArg eye_pos, SimdVectorArg at_pos,
887  SimdVectorArg up_dir_normalized) NLIB_NOEXCEPT {
888  SimdVector eye_dir = F128::Sub(at_pos, eye_pos);
889  eye_dir = Vector3::Normalize(eye_dir);
890  return LookToLh(eye_pos, eye_dir, up_dir_normalized);
891 }
892 
893 // XMMatrixLookToRH
894 NLIB_M(SimdMatrix) Matrix::LookToRh(SimdVectorArg eye_pos, SimdVectorArg eye_dir_normalized,
895  SimdVectorArg up_dir_normalized) NLIB_NOEXCEPT {
896  return LookToLh(eye_pos, F128::Negate(eye_dir_normalized), up_dir_normalized);
897 }
898 
899 // XMMatrixLookAtRH
900 NLIB_M(SimdMatrix) Matrix::LookAtRh(SimdVectorArg eye_pos, SimdVectorArg at_pos,
901  SimdVectorArg up_dir_normalized) NLIB_NOEXCEPT {
902  SimdVector eye_dir = F128::Sub(eye_pos, at_pos);
903  eye_dir = Vector3::Normalize(eye_dir);
904  return LookToLh(eye_pos, eye_dir, up_dir_normalized);
905 }
906 
907 // XMMatrixPerspectiveLH
908 NLIB_M(SimdMatrix) Matrix::PerspectiveLh(float width, float height, float near_z,
909  float far_z) NLIB_NOEXCEPT {
910  float near2 = near_z + near_z;
911  float range = far_z / (far_z - near_z);
912  f128 zero = F128::SetZero();
913  f128 v = F128::SetValue(near2 / width, near2 / height, range, -range * near_z);
914  SimdMatrix m;
915  m.r[0] = F128::Splat<false, true, true, true>(v, zero);
916  m.r[1] = F128::Splat<true, false, true, true>(v, zero);
917  f128 tmp = F128::Splat<true, true, false, true>(v, zero);
918  m.r[2] = F128::SetFloatToLane<3>(tmp, 1.f);
919  m.r[3] = F128::Permute<0, 1, 7, 0>(zero, v);
920  return m;
921 }
922 
923 // XMMatrixPerspectiveRH
924 NLIB_M(SimdMatrix) Matrix::PerspectiveRh(float width, float height, float near_z,
925  float far_z) NLIB_NOEXCEPT {
926  float near2 = near_z + near_z;
927  float range = far_z / (near_z - far_z);
928  f128 zero = F128::SetZero();
929  f128 v = F128::SetValue(near2 / width, near2 / height, range, range * near_z);
930  SimdMatrix m;
931  m.r[0] = F128::Splat<false, true, true, true>(v, zero);
932  m.r[1] = F128::Splat<true, false, true, true>(v, zero);
933  f128 tmp = F128::Splat<true, true, false, true>(v, zero);
934  m.r[2] = F128::SetFloatToLane<3>(tmp, -1.f);
935  m.r[3] = F128::Permute<0, 1, 7, 0>(zero, v);
936  return m;
937 }
938 
939 // XMMatrixPerspectiveFovLH
940 NLIB_M(SimdMatrix) Matrix::PerspectiveFovLh(float half_fovy_sin, float half_fovy_cos, float aspect,
941  float near_z, float far_z) NLIB_NOEXCEPT {
942  float height = half_fovy_cos / half_fovy_sin;
943  float width = height / aspect;
944  float range = far_z / (far_z - near_z);
945 
946  f128 zero = F128::SetZero();
947  f128 v = F128::SetValue(width, height, range, -range * near_z);
948  SimdMatrix m;
949  m.r[0] = F128::Splat<false, true, true, true>(v, zero);
950  m.r[1] = F128::Splat<true, false, true, true>(v, zero);
951  f128 tmp = F128::Splat<true, true, false, true>(v, zero);
952  m.r[2] = F128::SetFloatToLane<3>(tmp, 1.f);
953  m.r[3] = F128::Permute<0, 1, 7, 0>(zero, v);
954  return m;
955 }
956 
957 // XMMatrixPerspectiveFovRH
958 NLIB_M(SimdMatrix) Matrix::PerspectiveFovRh(float half_fovy_sin, float half_fovy_cos, float aspect,
959  float near_z, float far_z) NLIB_NOEXCEPT {
960  float height = half_fovy_cos / half_fovy_sin;
961  float width = height / aspect;
962  float range = far_z / (near_z - far_z);
963 
964  f128 zero = F128::SetZero();
965  f128 v = F128::SetValue(width, height, range, range * near_z);
966  SimdMatrix m;
967  m.r[0] = F128::Splat<false, true, true, true>(v, zero);
968  m.r[1] = F128::Splat<true, false, true, true>(v, zero);
969  f128 tmp = F128::Splat<true, true, false, true>(v, zero);
970  m.r[2] = F128::SetFloatToLane<3>(tmp, -1.f);
971  m.r[3] = F128::Permute<0, 1, 7, 0>(zero, v);
972  return m;
973 }
974 
975 // XMMatrixPerspectiveOffCenterLH
976 NLIB_M(SimdMatrix) Matrix::PerspectiveOffCenterLh(float left, float right, float bottom, float top,
977  float near_z, float far_z) NLIB_NOEXCEPT {
978  float near2 = near_z + near_z;
979  f128 div;
980  {
981  f128 a = F128::SetValue(1.f, 1.f, far_z, 1.f);
982  f128 b = F128::SetValue(right - left, top - bottom, far_z - near_z, 1.f);
983  div = F128::Div(a, b);
984  // recpWidth, recpHeight, range, 1.f
985  }
986  f128 zero = F128::SetZero();
987  f128 v0 = F128::SetValue(near2, near2, -near_z, 1.f);
988  f128 r2 = F128::SetValue(-(left + right), -(top + bottom), 1.f, 1.f);
989  v0 = F128::Mult(v0, div);
990 
991  SimdMatrix m;
992  m.r[0] = F128::Splat<false, true, true, true>(v0, zero);
993  m.r[1] = F128::Splat<true, false, true, true>(v0, zero);
994  m.r[2] = F128::Mult(r2, div);
995  m.r[3] = F128::Splat<true, true, false, true>(v0, zero);
996  return m;
997 }
998 
999 // XMMatrixPerspectiveOffCenterRH
1000 NLIB_M(SimdMatrix) Matrix::PerspectiveOffCenterRh(float left, float right, float bottom, float top,
1001  float near_z, float far_z) NLIB_NOEXCEPT {
1002  float near2 = near_z + near_z;
1003  f128 div;
1004  {
1005  f128 a = F128::SetValue(1.f, 1.f, far_z, 1.f);
1006  f128 b = F128::SetValue(right - left, top - bottom, near_z - far_z, -1.f);
1007  div = F128::Div(a, b);
1008  // recpWidth, recpHeight, range, 1.f
1009  }
1010  f128 zero = F128::SetZero();
1011  f128 v0 = F128::SetValue(near2, near2, near_z, 1.f);
1012  f128 r2 = F128::SetValue((left + right), (top + bottom), 1.f, 1.f);
1013  v0 = F128::Mult(v0, div);
1014 
1015  SimdMatrix m;
1016  m.r[0] = F128::Splat<false, true, true, true>(v0, zero);
1017  m.r[1] = F128::Splat<true, false, true, true>(v0, zero);
1018  m.r[2] = F128::Mult(r2, div);
1019  m.r[3] = F128::Splat<true, true, false, true>(v0, zero);
1020  return m;
1021 }
1022 
1023 // XMMatrixOrthographicLH
1024 NLIB_M(SimdMatrix) Matrix::OrthographicLh(float width, float height, float near_z,
1025  float far_z) NLIB_NOEXCEPT {
1026  f128 div;
1027  {
1028  f128 a = F128::SetValue(2.f, 2.f, 1.f, -near_z);
1029  f128 b = F128::SetValue(width, height, far_z - near_z, far_z - near_z);
1030  div = F128::Div(a, b);
1031  }
1032  f128 zero = F128::SetZero();
1033 
1034  SimdMatrix m;
1035  m.r[0] = F128::Splat<false, true, true, true>(div, zero);
1036  m.r[1] = F128::Splat<true, false, true, true>(div, zero);
1037  m.r[2] = F128::Splat<true, true, false, true>(div, zero);
1038  f128 tmp = F128::Permute<0, 1, 7, 1>(zero, div);
1039  m.r[3] = F128::SetFloatToLane<3>(tmp, 1.f);
1040  return m;
1041 }
1042 
1043 // XMMatrixOrthographicRH
1044 NLIB_M(SimdMatrix) Matrix::OrthographicRh(float width, float height, float near_z,
1045  float far_z) NLIB_NOEXCEPT {
1046  f128 div;
1047  {
1048  f128 a = F128::SetValue(2.f, 2.f, 1.f, near_z);
1049  f128 b = F128::SetValue(width, height, near_z - far_z, near_z - far_z);
1050  div = F128::Div(a, b);
1051  }
1052  f128 zero = F128::SetZero();
1053 
1054  SimdMatrix m;
1055  m.r[0] = F128::Splat<false, true, true, true>(div, zero);
1056  m.r[1] = F128::Splat<true, false, true, true>(div, zero);
1057  m.r[2] = F128::Splat<true, true, false, true>(div, zero);
1058  f128 tmp = F128::Permute<0, 1, 7, 1>(zero, div);
1059  m.r[3] = F128::SetFloatToLane<3>(tmp, 1.f);
1060  return m;
1061 }
1062 
1063 // XMMatrixOrthographicOffCenterLH
1064 NLIB_M(SimdMatrix) Matrix::OrthographicOffCenterLh(float left, float right, float bottom, float top,
1065  float near_z, float far_z) NLIB_NOEXCEPT {
1066  f128 div;
1067  {
1068  f128 a = F128::SetOne();
1069  f128 b = F128::SetValue(right - left, top - bottom, far_z - near_z, 1.f);
1070  div = F128::Div(a, b);
1071  }
1072  f128 zero = F128::SetZero();
1073  f128 v0 = F128::SetValue(2.f, 2.f, 1.f, 1.f);
1074  f128 r3 = F128::SetValue(-(left + right), -(top + bottom), -near_z, 1.f);
1075  v0 = F128::Mult(v0, div);
1076 
1077  SimdMatrix m;
1078  m.r[0] = F128::Splat<false, true, true, true>(v0, zero);
1079  m.r[1] = F128::Splat<true, false, true, true>(v0, zero);
1080  m.r[2] = F128::Splat<true, true, false, true>(v0, zero);
1081  m.r[3] = F128::Mult(r3, div);
1082  return m;
1083 }
1084 
1085 // XMMatrixOrthographicOffCenterRH
1086 NLIB_M(SimdMatrix) Matrix::OrthographicOffCenterRh(float left, float right, float bottom, float top,
1087  float near_z, float far_z) NLIB_NOEXCEPT {
1088  f128 div;
1089  {
1090  f128 a = F128::SetOne();
1091  f128 b = F128::SetValue(right - left, top - bottom, near_z - far_z, 1.f);
1092  div = F128::Div(a, b);
1093  }
1094  f128 zero = F128::SetZero();
1095  f128 v0 = F128::SetValue(2.f, 2.f, 1.f, 1.f);
1096  f128 r3 = F128::SetValue(-(left + right), -(top + bottom), near_z, 1.f);
1097  v0 = F128::Mult(v0, div);
1098 
1099  SimdMatrix m;
1100  m.r[0] = F128::Splat<false, true, true, true>(v0, zero);
1101  m.r[1] = F128::Splat<true, false, true, true>(v0, zero);
1102  m.r[2] = F128::Splat<true, true, false, true>(v0, zero);
1103  m.r[3] = F128::Mult(r3, div);
1104  return m;
1105 }
1106 
1107 // XMMatrixShadow
1108 NLIB_M(SimdMatrix) Matrix::Shadow(SimdPlaneArg shadow_plane, SimdVector light_pos) NLIB_NOEXCEPT {
1109  // Plane::Normalize(shadow_plane);
1110  SimdPlane plane = F128::Mult(Vector3::RecpLength(shadow_plane), shadow_plane);
1111  // distance from plane to the light
1112  f128 r0 = Vector4::DotEx<true, false, false, false>(plane, light_pos);
1113  plane = F128::Negate(plane);
1114  f128 r1 = F128::RotateLeft<1>(r0);
1115  f128 r2 = F128::RotateLeft<2>(r0);
1116  f128 r3 = F128::RotateLeft<3>(r0);
1117 
1118  SimdMatrix m;
1119  m.r[0] = F128::MultAdd<0>(plane, light_pos, r0, each_select32);
1120  m.r[1] = F128::MultAdd<1>(plane, light_pos, r1, each_select32);
1121  m.r[2] = F128::MultAdd<2>(plane, light_pos, r2, each_select32);
1122  m.r[3] = F128::MultAdd<3>(plane, light_pos, r3, each_select32);
1123  return m;
1124 }
1125 
1126 // XMMatrixReflect
1127 NLIB_M(SimdMatrix) Matrix::Reflect(SimdPlaneArg reflection_plane) NLIB_NOEXCEPT {
1128  // SimdPlane plane = Plane::Normalize(reflection_plane);
1129  SimdPlane plane = F128::Mult(Vector3::RecpLength(reflection_plane), reflection_plane);
1130  f128 minus_2n = F128::Mult(-2.f, plane);
1131  minus_2n = F128::SetZeroToLane<3>(minus_2n);
1132 
1133  SimdMatrix m = Matrix::Identity();
1134  m.r[0] = F128::MultAdd<0>(plane, minus_2n, m.r[0], each_select32);
1135  m.r[1] = F128::MultAdd<1>(plane, minus_2n, m.r[1], each_select32);
1136  m.r[2] = F128::MultAdd<2>(plane, minus_2n, m.r[2], each_select32);
1137  m.r[3] = F128::MultAdd<3>(plane, minus_2n, m.r[3], each_select32);
1138  return m;
1139 }
1140 
1141 // different from XMMatrixDecompose
1142 // note that scale.xyz >= 0
1143 NLIB_M(void) Matrix::Decompose(SimdVector* scale, SimdMatrix* rot, SimdVector* trans, // NOLINT
1145  // translation
1146  *trans = m.r[3];
1147 
1148  // scaling
1149  f128 recp_scale;
1150  {
1151  f128 dot_x = Vector3::DotEx<true, false, false, true>(m.r[0], m.r[0]);
1152  f128 dot_y = Vector3::DotEx<false, true, false, true>(m.r[1], m.r[1]);
1153  f128 dot_z = Vector3::DotEx<false, false, true, true>(m.r[2], m.r[2]);
1154  f128 dot = F128::Or(dot_x, dot_y);
1155  dot = F128::Or(dot, dot_z);
1156  recp_scale = F128::RecpSqrt(dot);
1157  *scale = F128::Mult(dot, recp_scale);
1158  }
1159 
1160  // rotation
1161  rot->r[0] = F128::Mult<0>(recp_scale, m.r[0], each_select32);
1162  rot->r[1] = F128::Mult<1>(recp_scale, m.r[1], each_select32);
1163  rot->r[2] = F128::Mult<2>(recp_scale, m.r[2], each_select32);
1164  rot->r[3] = F128::Set0001();
1165 }
1166 
1167 #undef NLIB_M
1168 
1169 #endif // NLIB_DOXYGEN
1170 
1171 } // namespace simd
1172 NLIB_NAMESPACE_END
1173 
1174 #endif // INCLUDE_NN_NLIB_SIMD_SIMDMATRIX_H_
4x4行列を扱う関数が集められたクラスです。
Definition: SimdMatrix.h:15
クォータニオンが定義されています。
float m[3][4]
3x4の2次元配列です。
Definition: SimdFloat.h:4304
f128arg SimdVectorArg
f128argがtypedefされています。
Definition: SimdFloat.h:4137
Definition: Base64.h:8
float m[4][3]
4x3の2次元配列です。
Definition: SimdFloat.h:4312
float m[4][4]
4x4の2次元配列です。
Definition: SimdFloat.h:4320
#define NLIB_VIS_HIDDEN
関数やクラス等のシンボルをライブラリの外部に公開しません。
Definition: Platform_unix.h:60
#define NLIB_F128_TRANSPOSE(row0, row1, row2, row3)
インプレイスで行列を転置するためのマクロです。
Definition: SimdFloat.h:4211
constexpr const each_float_tag each_float
each_float_tag型の定数オブジェクトで、単精度浮動小数点数を示すためのタグです。
Definition: SimdFloat.h:55
f128arg SimdQuaternionArg
f128argがtypedefされています。
Definition: SimdFloat.h:4139
f128arg SimdPlaneArg
f128argがtypedefされています。
Definition: SimdFloat.h:4141
f128 r[4]
4x4行列の各行を保持します。
Definition: SimdFloat.h:4164
4x4行列を保持する構造体です。
Definition: SimdFloat.h:4148
#define NLIB_NOEXCEPT
環境に合わせてnoexcept 又は同等の定義がされます。
Definition: Config.h:86
単精度浮動小数点数のSIMD演算を行うためのクラスや関数が定義されています。
constexpr const each_select32_tag each_select32
each_select32_tag型の定数オブジェクトで、32bitのレーンを選択することを示すためのタグです。 ...
Definition: SimdInt.h:50
4x3行列をメモリから読み出したりメモリに書き出したりするための型です。データメンバmは4x3の配列で16バイ...
Definition: SimdFloat.h:4310
3x3行列をメモリから読み出したりメモリに書き出したりするための型です。データメンバmは3x3の配列です。 ...
Definition: SimdFloat.h:4295
3次元ベクトルが定義されています。
nlib_f128_t f128
nlib_f128_tがtypedefされています。
Definition: SimdFloat.h:58
4次元ベクトルが定義されています。
float m[3][3]
3x3の2次元配列です。
Definition: SimdFloat.h:4296
4x4行列をメモリから読み出したりメモリに書き出したりするための型です。データメンバmは4x4の配列で16バイ...
Definition: SimdFloat.h:4318
3x4行列をメモリから読み出したりメモリに書き出したりするための型です。データメンバmは3x4の配列で16バイ...
Definition: SimdFloat.h:4302
f128 SimdPlane
f128がtypedefされています。平面を扱う場合に利用されます。
Definition: SimdFloat.h:4140
f128 SimdVector
f128がtypedefされています。3次元ベクトル又は4次元ベクトルを扱う場合に利用されます。 ...
Definition: SimdFloat.h:4136