1 #pragma once
2
3 #include "../MathBuildConfig.h"
4
5 #ifdef MATH_SIMD
6
7 #include "SSEMath.h"
8 #include "float4_neon.h"
9
10 MATH_BEGIN_NAMESPACE
11
12 #ifdef MATH_SSE
13
14 /// Converts a quaternion to a row-major matrix.
15 /// From http://renderfeather.googlecode.com/hg-history/034a1900d6e8b6c92440382658d2b01fc732c5de/Doc/optimized%20Matrix%20quaternion%20conversion.pdf
16 inline void quat_to_mat3x4(__m128 q, __m128 t, __m128 *m)
17 {
18         // Constants:
19         const u32 sign = 0x80000000UL;
20         const __m128 sseX0 = set_ps_hex(sign, sign, sign, 0);
21         const __m128 sseX1 = set_ps_hex(sign, sign, 0, sign);
22         __m128 one = _mm_set_ps(0, 0, 0, 1);
23
24 #if 0 // The original code converted a quaternion into an hybrid of rotation/translation (bug?)
25         __m128 q2 = _mm_add_ps(q, q);                                 // [2w 2z 2y 2x]
26         __m128 yxxy = shuffle1_ps(q, _MM_SHUFFLE(1, 0, 0, 1));        // [ y  x  x  y]
27         __m128 yyzz2 = shuffle1_ps(q2, _MM_SHUFFLE(2, 2, 1, 1));      // [2z 2z 2y 2y]
28         __m128 yy_xy_xz_yz_2 = _mm_mul_ps(yxxy, yyzz2);               // [2yz 2xz 2xy 2yy]
29         
30         __m128 zwww = shuffle1_ps(q, _MM_SHUFFLE(3, 3, 3, 2));        // [w w w z]
31         __m128 zzyx2 = shuffle1_ps(q2, _MM_SHUFFLE(0, 1, 2, 2));      // [2x 2y 2z 2z]
32         __m128 zz_wz_wy_wx_2 = _mm_mul_ps(zwww, zzyx2);               // [2xw 2yw 2zw 2zz]
33
34         __m128 xx2 = _mm_mul_ss(q, q2);                               // [2xx]
35
36         // Calculate last two elements of the third row.
37         __m128 one_m_xx2 = _mm_sub_ss(one, xx2);                      // [0 0 0 1-2xx]
38         __m128 one_m_xx_yy_2 = _mm_sub_ss(one_m_xx2, yy_xy_xz_yz_2);  // [0 0 0 1-2xx-2yy]
39         __m128 one_m_xx_yy_2_0_tz_tw = _mm_shuffle_ps(one_m_xx_yy_2, t, _MM_SHUFFLE(3, 2, 1, 0)); // [tw tz 0 1-2xx-2yy]
40
41         // Calculate first row
42         __m128 m_yy_xy_xz_yz_2 = _mm_xor_ps(yy_xy_xz_yz_2, sseX0);     // [-2yz -2xz -2xy   2yy]
43         __m128 m_zz_wz_wy_wx_2 = _mm_xor_ps(zz_wz_wy_wx_2, sseX1);     // [-2xw -2yw  2zw  -2zz]
44         __m128 m_zz_one_wz_wy_wx_2 = _mm_add_ss(m_zz_wz_wy_wx_2, one); // [-2xw -2yw  2zw 1-2zz]
45         __m128 first_row = _mm_sub_ps(m_zz_one_wz_wy_wx_2, m_yy_xy_xz_yz_2); // [2yz-2xw 2xz-2yw 2xy+2zw 1-2zz-2yy]
46         m[0] = first_row;
47         _mm_store_ss((float*)m+3, t);
48
49         // Calculate second row
50         __m128 s1 = _mm_move_ss(m_yy_xy_xz_yz_2, xx2);                // [-2yz -2xz -2xy 2xx]
51         __m128 s2 = _mm_xor_ps(m_zz_one_wz_wy_wx_2, sseX0);           // [2xw 2yw -2zw 1-2zz]
52         __m128 s3 = _mm_sub_ps(s2, s1);                               // [2xw+2yz 2yw+2xz 2xy-2zw 1-2zz-2xx]
53         __m128 t_yzwx = shuffle1_ps(t, _MM_SHUFFLE(0, 3, 2, 1));      // [tx tw tz ty]
54         __m128 second_row = shuffle1_ps(s3, _MM_SHUFFLE(2, 3, 0, 1)); // [2yw+2xz 2xw+2yz 1-2zz-2xx 2xy-2zw]
55         m[1] = second_row;
56         _mm_store_ss((float*)m+7, t_yzwx);
57
58         // Calculate third row
59         __m128 t1 = _mm_movehl_ps(first_row, second_row);             // [2yz-2xw 2xz-2yw 2yw+2xz 2xw+2yz]
60         __m128 t2 = _mm_shuffle_ps(t1, one_m_xx_yy_2_0_tz_tw, _MM_SHUFFLE(2, 0, 3, 1)); // [tz 1-2xx-2yy 2yz-2xw 2yw+2xz]
61         m[2] = t2;
62 #else
63         __m128 q2 = _mm_add_ps(q, q);                                 // [2w 2z 2y 2x]
64         __m128 yxxy = shuffle1_ps(q, _MM_SHUFFLE(1, 0, 0, 1));        // [ y  x  x  y]
65         __m128 yyzz2 = shuffle1_ps(q2, _MM_SHUFFLE(2, 2, 1, 1));      // [2z 2z 2y 2y]
66         __m128 yy_xy_xz_yz_2 = _mm_mul_ps(yxxy, yyzz2);               // [2yz 2xz 2xy 2yy]
67         
68         __m128 zwww = shuffle1_ps(q, _MM_SHUFFLE(3, 3, 3, 2));        // [w w w z]
69         __m128 zzyx2 = shuffle1_ps(q2, _MM_SHUFFLE(0, 1, 2, 2));      // [2x 2y 2z 2z]
70         __m128 zz_wz_wy_wx_2 = _mm_mul_ps(zwww, zzyx2);               // [2xw 2yw 2zw 2zz]
71
72         __m128 xx2 = _mm_mul_ss(q, q2);                               // [2xx]
73
74         // Calculate last two elements of the third row.
75         __m128 one_m_xx2 = _mm_sub_ss(one, xx2);                      // [0 0 0 1-2xx]
76         __m128 one_m_xx_yy_2 = _mm_sub_ss(one_m_xx2, yy_xy_xz_yz_2);  // [0 0 0 1-2xx-2yy]
77         __m128 one_m_xx_yy_2_0_tz_tw = one_m_xx_yy_2;//_mm_shuffle_ps(one_m_xx_yy_2, t, _MM_SHUFFLE(3, 2, 1, 0)); // [tw tz 0 1-2xx-2yy]
78
79         // Calculate first row
80         __m128 m_yy_xy_xz_yz_2 = _mm_xor_ps(yy_xy_xz_yz_2, sseX0);     // [-2yz -2xz -2xy   2yy]
81         __m128 m_zz_wz_wy_wx_2 = _mm_xor_ps(zz_wz_wy_wx_2, sseX1);     // [-2xw -2yw  2zw  -2zz]
82         __m128 m_zz_one_wz_wy_wx_2 = _mm_add_ss(m_zz_wz_wy_wx_2, one); // [-2xw -2yw  2zw 1-2zz]
83         __m128 first_row = _mm_sub_ps(m_zz_one_wz_wy_wx_2, m_yy_xy_xz_yz_2); // [2yz-2xw 2xz-2yw 2xy+2zw 1-2zz-2yy]
84
85         // Calculate second row
86         __m128 s1 = _mm_move_ss(m_yy_xy_xz_yz_2, xx2);                // [-2yz -2xz -2xy 2xx]
87         __m128 s2 = _mm_xor_ps(m_zz_one_wz_wy_wx_2, sseX0);           // [2xw 2yw -2zw 1-2zz]
88         __m128 s3 = _mm_sub_ps(s2, s1);                               // [2xw+2yz 2yw+2xz 2xy-2zw 1-2zz-2xx]
89         __m128 second_row = shuffle1_ps(s3, _MM_SHUFFLE(2, 3, 0, 1)); // [2yw+2xz 2xw+2yz 1-2zz-2xx 2xy-2zw]
90
91         // Calculate third row
92         __m128 t1 = _mm_movehl_ps(first_row, second_row);             // [2yz-2xw 2xz-2yw 2yw+2xz 2xw+2yz]
93         __m128 third_row = _mm_shuffle_ps(t1, one_m_xx_yy_2_0_tz_tw, _MM_SHUFFLE(2, 0, 3, 1)); // [0 1-2xx-2yy 2yz-2xw 2yw+2xz]
94
95         __m128 tmp0 = _mm_unpacklo_ps(first_row, second_row);
96         __m128 tmp2 = _mm_unpacklo_ps(third_row, t);
97         __m128 tmp1 = _mm_unpackhi_ps(first_row, second_row);
98         __m128 tmp3 = _mm_unpackhi_ps(third_row, t);
99         m[0] = _mm_movelh_ps(tmp0, tmp2);
100         m[1] = _mm_movehl_ps(tmp2, tmp0);
101         m[2] = _mm_movelh_ps(tmp1, tmp3);
102 #endif
103 }
104
105 FORCE_INLINE void quat_to_mat4x4(__m128 q, __m128 t, __m128 *m)
106 {
107         quat_to_mat3x4(q, t, m);
108         m[3] = set_ps(1.f, 0.f, 0.f, 0.f);
109 }
110
111 FORCE_INLINE simd4f quat_transform_vec4(simd4f quat, simd4f vec)
112 {
113         __m128 W = wwww_ps(quat);
114
115 //      __m128 qxv = cross_ps(q, vec.v);
116         __m128 a_xzy = shuffle1_ps(quat, _MM_SHUFFLE(3, 0, 2, 1)); // a_xzy = [a.w, a.x, a.z, a.y]
117         __m128 b_yxz = shuffle1_ps(vec, _MM_SHUFFLE(3, 1, 0, 2)); // b_yxz = [b.w, b.y, b.x, b.z]
118         __m128 a_yxz = shuffle1_ps(quat, _MM_SHUFFLE(3, 1, 0, 2)); // a_yxz = [a.w, a.y, a.x, a.z]
119         __m128 b_xzy = shuffle1_ps(vec, _MM_SHUFFLE(3, 0, 2, 1)); // b_xzy = [b.w, b.x, b.z, b.y]
120         __m128 x = _mm_mul_ps(a_xzy, b_yxz); // [a.w*b.w, a.x*b.y, a.z*b.x, a.y*b.z]
121         __m128 y = _mm_mul_ps(a_yxz, b_xzy); // [a.w*b.w, a.y*b.x, a.x*b.z, a.z*b.y]
122         __m128 qxv = _mm_sub_ps(x, y); // [0, a.x*b.y - a.y*b.x, a.z*b.x - a.x*b.z, a.y*b.z - a.z*b.y]
123
124         __m128 Wv = _mm_mul_ps(W, vec);
125         __m128 s = _mm_add_ps(qxv, Wv);
126
127 //      s = cross_ps(q, s);
128         __m128 s_yxz = shuffle1_ps(s, _MM_SHUFFLE(3, 1, 0, 2)); // b_yxz = [b.w, b.y, b.x, b.z]
129         __m128 s_xzy = shuffle1_ps(s, _MM_SHUFFLE(3, 0, 2, 1)); // b_xzy = [b.w, b.x, b.z, b.y]
130         x = _mm_mul_ps(a_xzy, s_yxz); // [a.w*b.w, a.x*b.y, a.z*b.x, a.y*b.z]
131         y = _mm_mul_ps(a_yxz, s_xzy); // [a.w*b.w, a.y*b.x, a.x*b.z, a.z*b.y]
132         s = _mm_sub_ps(x, y); // [0, a.x*b.y - a.y*b.x, a.z*b.x - a.x*b.z, a.y*b.z - a.z*b.y]
133
134         s = _mm_add_ps(s, s);
135         s = _mm_add_ps(s, vec);
136         return s;
137 }
138
139 #endif // ~MATH_SSE
140
141 #ifdef ANDROID
142 inline void quat_mul_quat_asm(const void *q1, const void *q2, void *out)
143 {
144 /*      return Quat(x*r.w + y*r.z - z*r.y + w*r.x,
145                    -x*r.z + y*r.w + z*r.x + w*r.y,
146                     x*r.y - y*r.x + z*r.w + w*r.z,
147                    -x*r.x - y*r.y - z*r.z + w*r.w); */
148 #ifdef _DEBUG
149         assert(IS16ALIGNED(out));
150         assert(IS16ALIGNED(q1));
151         assert(IS16ALIGNED(q2));
152         assert(IS16ALIGNED(sx));
153         assert(IS16ALIGNED(sy));
154         assert(IS16ALIGNED(sz));
155 #endif
156         ///\todo 128-bit aligned loads: [%1,:128]
157         asm(
158                 "\t vld1.32 {d0, d1}, [%1]\n"    // q0 = quat1.xyzw
159                 "\t vmov.i32 d12, #0\n"          // q6.lo = 0
160                 "\t vmov.i32 d13, #0x80000000\n" // q6.hi = [- - + +] = 'signy'
161                 "\t vld1.32 {d8, d9}, [%2]\n"    // q4 = quat2.xyzw [%2]
162                 "\t vdup.32 q1, d0[1]\n"         // q1 = q0[1] = quat1.yyyy = 'Y'
163                 "\t vdup.32 q2, d1[0]\n"         // q2 = q0[2] = quat1.zzzz = 'Z'
164                 "\t vshl.i64 d10, d13, #32\n"    // q5.lo = q6.hi = [- +]
165                 "\t vdup.32 q3, d1[1]\n"         // q3 = q0[3] = quat1.wwww = 'W'
166                 "\t vdup.32 q0, d0[0]\n"         // q0 = q0[0] = quat1.xxxx = 'X'
167                 "\t vmov d11, d10\n"             // q5.hi = q5.lo = [- + - +] = 'signx'
168                 "\t vmov d15, d10\n"             // q7.hi = q5.lo = [- +]
169                 "\t vshr.u64 d14, d10, #32\n"    // q7.lo = q5.lo = [- + + -] = 'signz'
170
171                 "\t vmov d18, d9\n"              // q9.lo = q4.hi
172                 "\t vmov d19, d8\n"              // q9.hi = q4.lo, q9 = quat2.zwxy = 'q2 for Y'
173
174                 "\t veor q0, q0, q5\n"           // q0 = X*signx = [-x x -x x]
175                 "\t veor q1, q1, q6\n"           // q1 = Y*signy = [-y -y y y]
176                 "\t veor q2, q2, q7\n"           // q2 = Z*signz = [-z z z -z]
177
178                 "\t vrev64.32 q10, q9\n"         // q10 = quat2.wzyx = 'q2 for X'
179
180                 "\t vmul.f32 q0, q0, q10\n"      // q0 = X*signx * quat2
181                 "\t vmul.f32 q11, q1, q9\n"       // q0 += Y*signy * quat2
182                 "\t vrev64.32 q8, q4\n"          // q8 = quat2.yxwz  = 'q2 for Z'
183                 "\t vmla.f32 q0, q2, q8\n"       // q0 += Z*signz * quat2
184                 "\t vmla.f32 q11, q3, q4\n"       // q0 += W       * quat2
185
186                 "\t vadd.f32 q0, q0, q11\n"
187                 "\t vst1.32     {d0, d1}, [%0]\n"    // store output
188         : /* no outputs by value */
189         : [out]"r"(out), [quat1]"r"(q1), [quat2]"r"(q2)
190         : "memory", "q11", "q10", "q9", "q8", "q7", "q6", "q5", "q4", "q3", "q2", "q1", "q0");
191 }
192 #endif
193
194 FORCE_INLINE simd4f quat_mul_quat(simd4f q1, simd4f q2)
195 {
196 /*      return Quat(x*r.w + y*r.z - z*r.y + w*r.x,
197                    -x*r.z + y*r.w + z*r.x + w*r.y,
198                     x*r.y - y*r.x + z*r.w + w*r.z,
199                    -x*r.x - y*r.y - z*r.z + w*r.w); */
200 #ifdef MATH_SSE
201         const __m128 signx = set_ps_hex(0x80000000u, 0, 0x80000000u, 0); // [- + - +]
202         const __m128 signy = shuffle1_ps(signx, _MM_SHUFFLE(3,3,0,0));   // [- - + +]
203         const __m128 signz = shuffle1_ps(signx, _MM_SHUFFLE(3,0,0,3));   // [- + + -]
204
205         __m128 X = _mm_xor_ps(signx, xxxx_ps(q1));
206         __m128 Y = _mm_xor_ps(signy, yyyy_ps(q1));
207         __m128 Z = _mm_xor_ps(signz, zzzz_ps(q1));
208         __m128 W = wwww_ps(q1);
209
210         __m128 r1 = shuffle1_ps(q2, _MM_SHUFFLE(0, 1, 2, 3)); // [x,y,z,w]
211         __m128 r2 = shuffle1_ps(q2, _MM_SHUFFLE(1, 0, 3, 2)); // [y,x,w,z]
212         __m128 r3 = shuffle1_ps(q2, _MM_SHUFFLE(2, 3, 0, 1)); // [z,w,x,y]
213         // __m128 r4 = q2;
214
215         return _mm_add_ps(_mm_add_ps(_mm_mul_ps(X, r1), _mm_mul_ps(Y, r2)), 
216                           _mm_add_ps(_mm_mul_ps(Z, r3), _mm_mul_ps(W, q2)));
217 #elif defined(ANDROID)
218         simd4f ret;
219         quat_mul_quat_asm(&q1, &q2, &ret);
220         return ret;
221 #else // NEON
222         static const float32x4_t signx = set_ps_hex_const(0x80000000u, 0, 0x80000000u, 0);
223         static const float32x4_t signy = set_ps_hex_const(0x80000000u, 0x80000000u, 0, 0);
224         static const float32x4_t signz = set_ps_hex_const(0x80000000u, 0, 0, 0x80000000u);
225
226         const float32_t *q1f = (const float32_t *)&q1;
227         float32x4_t X = xor_ps(signx, vdupq_n_f32(q1f[0]));
228         float32x4_t Y = xor_ps(signy, vdupq_n_f32(q1f[1]));
229         float32x4_t Z = xor_ps(signz, vdupq_n_f32(q1f[2]));
230         float32x4_t W = vdupq_n_f32(q1f[3]);
231
232         float32x4_t r3 = vrev64q_f32(q2); // [z,w,x,y]
233         float32x4_t r1 = vcombine_f32(vget_high_f32(r3), vget_low_f32(r3)); // [x,y,z,w]
234         float32x4_t r2 = vrev64q_f32(r1); // [y,x,w,z]
235
236         float32x4_t ret = mul_ps(X, r1);
237         ret = vmlaq_f32(ret, Y, r2);
238         ret = vmlaq_f32(ret, Z, r3);
239         ret = vmlaq_f32(ret, W, q2);
240         return ret;
241 #endif
242 }
243
244 #ifdef MATH_SSE
245 FORCE_INLINE simd4f quat_div_quat(simd4f q1, simd4f q2)
246 {
247 /*      return Quat(x*r.w - y*r.z + z*r.y - w*r.x,
248                     x*r.z + y*r.w - z*r.x - w*r.y,
249                    -x*r.y + y*r.x + z*r.w - w*r.z,
250                     x*r.x + y*r.y + z*r.z + w*r.w); */
251
252         const __m128 signx = set_ps_hex(0x80000000u, 0, 0x80000000u, 0); // [- + - +]
253         const __m128 signy = shuffle1_ps(signx, _MM_SHUFFLE(3,3,0,0));   // [- - + +]
254         const __m128 signz = shuffle1_ps(signx, _MM_SHUFFLE(3,0,0,3));   // [- + + -]
255
256         __m128 X = _mm_xor_ps(signx, xxxx_ps(q1));
257         __m128 Y = _mm_xor_ps(signy, yyyy_ps(q1));
258         __m128 Z = _mm_xor_ps(signz, zzzz_ps(q1));
259         __m128 W = wwww_ps(q1);
260
261         q2 = negate3_ps(q2);
262         __m128 r1 = shuffle1_ps(q2, _MM_SHUFFLE(0, 1, 2, 3)); // [x,y,z,w]
263         __m128 r2 = shuffle1_ps(q2, _MM_SHUFFLE(1, 0, 3, 2)); // [y,x,w,z]
264         __m128 r3 = shuffle1_ps(q2, _MM_SHUFFLE(2, 3, 0, 1)); // [z,w,x,y]
265         // __m128 r4 = q2;
266
267         return _mm_add_ps(_mm_add_ps(_mm_mul_ps(X, r1), _mm_mul_ps(Y, r2)), 
268                           _mm_add_ps(_mm_mul_ps(Z, r3), _mm_mul_ps(W, q2)));
269 }
270 #endif
271
272 MATH_END_NAMESPACE
273
274 #endif // ~MATH_SIMD