1/****************************************************************************
2**
3** Copyright (C) 2016 Paul Lemire <paul.lemire350@gmail.com>
4** Contact: https://www.qt.io/licensing/
5**
6** This file is part of the Qt3D module of the Qt Toolkit.
7**
8** $QT_BEGIN_LICENSE:LGPL$
9** Commercial License Usage
10** Licensees holding valid commercial Qt licenses may use this file in
11** accordance with the commercial license agreement provided with the
12** Software or, alternatively, in accordance with the terms contained in
13** a written agreement between you and The Qt Company. For licensing terms
14** and conditions see https://www.qt.io/terms-conditions. For further
15** information use the contact form at https://www.qt.io/contact-us.
16**
17** GNU Lesser General Public License Usage
18** Alternatively, this file may be used under the terms of the GNU Lesser
19** General Public License version 3 as published by the Free Software
20** Foundation and appearing in the file LICENSE.LGPL3 included in the
21** packaging of this file. Please review the following information to
22** ensure the GNU Lesser General Public License version 3 requirements
23** will be met: https://www.gnu.org/licenses/lgpl-3.0.html.
24**
25** GNU General Public License Usage
26** Alternatively, this file may be used under the terms of the GNU
27** General Public License version 2.0 or (at your option) the GNU General
28** Public license version 3 or any later version approved by the KDE Free
29** Qt Foundation. The licenses are as published by the Free Software
30** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3
31** included in the packaging of this file. Please review the following
32** information to ensure the GNU General Public License requirements will
33** be met: https://www.gnu.org/licenses/gpl-2.0.html and
34** https://www.gnu.org/licenses/gpl-3.0.html.
35**
36** $QT_END_LICENSE$
37**
38****************************************************************************/
39
40#ifndef QT3DCORE_VECTOR3D_SSE_P_H
41#define QT3DCORE_VECTOR3D_SSE_P_H
42
43//
44// W A R N I N G
45// -------------
46//
47// This file is not part of the Qt3D API. It exists purely as an
48// implementation detail. This header file may change from version to
49// version without notice, or even be removed.
50//
51// We mean it.
52//
53
54#include <Qt3DCore/private/qt3dcore_global_p.h>
55#include <QtCore/private/qsimd_p.h>
56#include <QtCore/QtGlobal>
57#include <QtGui/qvector3d.h>
58#include <QDebug>
59#include <math.h>
60
61#ifdef QT_COMPILER_SUPPORTS_SSE2
62
63QT_BEGIN_NAMESPACE
64
65namespace Qt3DCore {
66
67class Matrix4x4_SSE;
68class Matrix4x4_AVX2;
69class Vector4D_SSE;
70
71class Vector3D_SSE
72{
73public:
74
75 Q_ALWAYS_INLINE Vector3D_SSE()
76 : m_xyzw(_mm_setzero_ps())
77 {
78 }
79
80 explicit Q_ALWAYS_INLINE Vector3D_SSE(Qt::Initialization) {}
81
82 explicit Q_ALWAYS_INLINE Vector3D_SSE(float x, float y, float z)
83 : m_xyzw(_mm_set_ps(z: 0.0f, y: z, x: y, w: x))
84 {
85 }
86
87 explicit Q_ALWAYS_INLINE Vector3D_SSE(QVector3D v)
88 : m_xyzw(_mm_set_ps(z: 0.0f, y: v.z(), x: v.y(), w: v.x()))
89 {
90 }
91
92 explicit Q_3DCORE_PRIVATE_EXPORT Vector3D_SSE(const Vector4D_SSE &v);
93
94 Q_ALWAYS_INLINE Vector3D_SSE &operator+=(Vector3D_SSE vector)
95 {
96 m_xyzw = _mm_add_ps(a: m_xyzw, b: vector.m_xyzw);
97 return *this;
98 }
99
100 Q_ALWAYS_INLINE Vector3D_SSE &operator-=(Vector3D_SSE vector)
101 {
102 m_xyzw = _mm_sub_ps(a: m_xyzw, b: vector.m_xyzw);
103 return *this;
104 }
105
106 Q_ALWAYS_INLINE Vector3D_SSE &operator*=(Vector3D_SSE vector)
107 {
108 m_xyzw = _mm_mul_ps(a: m_xyzw, b: vector.m_xyzw);
109 return *this;
110 }
111
112 Q_ALWAYS_INLINE Vector3D_SSE &operator/=(Vector3D_SSE vector)
113 {
114 m_xyzw = _mm_div_ps(a: m_xyzw, b: vector.m_xyzw);
115 return *this;
116 }
117
118 Q_ALWAYS_INLINE Vector3D_SSE &operator*=(float factor)
119 {
120 m_xyzw = _mm_mul_ps(a: m_xyzw, b: _mm_set1_ps(w: factor));
121 return *this;
122 }
123
124 Q_ALWAYS_INLINE Vector3D_SSE &operator/=(float factor)
125 {
126 m_xyzw = _mm_div_ps(a: m_xyzw, b: _mm_set1_ps(w: factor));
127 return *this;
128 }
129
130 Q_ALWAYS_INLINE bool operator==(Vector3D_SSE other) const
131 {
132 // 0b111 == 0x7
133 return ((_mm_movemask_ps(a: _mm_cmpeq_ps(a: m_xyzw, b: other.m_xyzw)) & 0x7) == 0x7);
134 }
135
136 Q_ALWAYS_INLINE bool operator!=(Vector3D_SSE other) const
137 {
138 return !(*this == other);
139 }
140
141 Q_ALWAYS_INLINE QVector3D toQVector3D() const
142 {
143 return QVector3D(x(), y(), z());
144 }
145
146 Q_ALWAYS_INLINE float lengthSquared() const
147 {
148 return Qt3DCore::Vector3D_SSE::dotProduct(a: *this, b: *this);
149 }
150
151 Q_ALWAYS_INLINE float length() const
152 {
153 return sqrt(x: Qt3DCore::Vector3D_SSE::dotProduct(a: *this, b: *this));
154 }
155
156 Q_ALWAYS_INLINE float distanceToPoint(const Vector3D_SSE &point) const
157 {
158 return (*this - point).length();
159 }
160
161 Q_ALWAYS_INLINE void normalize()
162 {
163 const float len = length();
164 m_xyzw = _mm_div_ps(a: m_xyzw, b: _mm_set_ps1(w: len));
165 }
166
167 Q_ALWAYS_INLINE Vector3D_SSE normalized() const
168 {
169 Vector3D_SSE v = *this;
170 v.normalize();
171 return v;
172 }
173
174 Q_ALWAYS_INLINE bool isNull() const
175 {
176 // Ignore last bit
177 // 0b111 = 0x7
178 return ((_mm_movemask_ps(a: _mm_cmpeq_ps(a: m_xyzw, b: _mm_set_ps1(w: 0.0f))) & 0x7) == 0x7);
179 }
180
181#if defined(__AVX2__) && defined(QT_COMPILER_SUPPORTS_AVX2)
182 Q_3DCORE_PRIVATE_EXPORT Vector3D_SSE unproject(const Matrix4x4_AVX2 &modelView, const Matrix4x4_AVX2 &projection, const QRect &viewport) const;
183 Q_3DCORE_PRIVATE_EXPORT Vector3D_SSE project(const Matrix4x4_AVX2 &modelView, const Matrix4x4_AVX2 &projection, const QRect &viewport) const;
184#else
185 Q_3DCORE_PRIVATE_EXPORT Vector3D_SSE unproject(const Matrix4x4_SSE &modelView, const Matrix4x4_SSE &projection, const QRect &viewport) const;
186 Q_3DCORE_PRIVATE_EXPORT Vector3D_SSE project(const Matrix4x4_SSE &modelView, const Matrix4x4_SSE &projection, const QRect &viewport) const;
187#endif
188
189 Q_ALWAYS_INLINE float x() const { return _mm_cvtss_f32(a: m_xyzw); }
190
191 Q_ALWAYS_INLINE float y() const
192 {
193 // 0b01010101 = 0x55
194 return _mm_cvtss_f32(_mm_shuffle_ps(m_xyzw, m_xyzw, 0x55));
195 }
196
197 Q_ALWAYS_INLINE float z() const
198 {
199 // 0b10101010 = 0xaa
200 return _mm_cvtss_f32(a: _mm_unpackhi_ps(a: m_xyzw, b: m_xyzw));
201 }
202
203 Q_ALWAYS_INLINE void setX(float x)
204 {
205 m_xyzw = _mm_move_ss(a: m_xyzw, b: _mm_set_ss(w: x));
206 }
207
208 Q_ALWAYS_INLINE void setY(float y)
209 {
210 // m_xyzw = a, b, c, d
211
212 // y, y, y, y
213 const __m128 yVec = _mm_set_ps1(w: y);
214
215 // y, y, a, a
216 // 0b00000000 == 0x0
217 const __m128 yaVec = _mm_shuffle_ps(yVec, m_xyzw, 0x0);
218
219 // a, y, c, d
220 // 0b11100010 == 0xe2
221 m_xyzw = _mm_shuffle_ps(yaVec, m_xyzw, 0xe2);
222 }
223
224 Q_ALWAYS_INLINE void setZ(float z)
225 {
226 // m_xyzw = a, b, c, d
227
228 // z, z, z, z
229 const __m128 zVec = _mm_set_ps1(w: z);
230
231 // z, z, d, d
232 // 0b11110000 == 0xf0
233 const __m128 zdVec = _mm_shuffle_ps(zVec, m_xyzw, 0xf0);
234
235 // a, b, z, d
236 // 0b10000100 == 0x84
237 m_xyzw = _mm_shuffle_ps(m_xyzw, zdVec, 0x84);
238 }
239
240 Q_ALWAYS_INLINE float operator[](int idx) const
241 {
242 switch (idx) {
243 case 0:
244 return x();
245 case 1:
246 return y();
247 case 2:
248 return z();
249 default:
250 Q_UNREACHABLE();
251 return 0.0f;
252 }
253 }
254
255 struct DigitWrapper
256 {
257 explicit DigitWrapper(int idx, Vector3D_SSE *vec)
258 : m_vec(vec)
259 , m_idx(idx)
260 {}
261
262 operator float() const
263 {
264 switch (m_idx) {
265 case 0:
266 return m_vec->x();
267 case 1:
268 return m_vec->y();
269 case 2:
270 return m_vec->z();
271 default:
272 Q_UNREACHABLE();
273 return 0.0f;
274 }
275 }
276
277 void operator =(float value)
278 {
279 switch (m_idx) {
280 case 0:
281 m_vec->setX(value);
282 break;
283 case 1:
284 m_vec->setY(value);
285 break;
286 case 2:
287 m_vec->setZ(value);
288 break;
289 default:
290 Q_UNREACHABLE();
291 }
292 }
293
294 private:
295 Vector3D_SSE *m_vec;
296 const int m_idx;
297 };
298
299 Q_ALWAYS_INLINE DigitWrapper operator[](int idx)
300 {
301 return DigitWrapper(idx, this);
302 }
303
304 static Q_ALWAYS_INLINE float dotProduct(Vector3D_SSE a, Vector3D_SSE b)
305 {
306#if defined(__SSE4_1__)
307 // 0b01111111 = 0x7f
308 return _mm_cvtss_f32(_mm_dp_ps(a.m_xyzw, b.m_xyzw, 0x7f));
309#elif defined(__SSE3__)
310 const __m128 mult = _mm_mul_ps(a.m_xyzw, b.m_xyzw);
311 // a + b, c + d, a + d, c + d
312 const __m128 partialSum = _mm_hadd_ps(mult, mult);
313 // c + d, ......
314 // 0x00000001 =
315 const __m128 partialSumShuffle = _mm_shuffle_ps(partialSum, partialSum, 0x1);
316 return _mm_cvtss_f32(_mm_hadd_ps(partialSum, partialSumShuffle));
317#else
318 const __m128 mult = _mm_mul_ps(a: a.m_xyzw, b: b.m_xyzw);
319
320 // (multX, 0, 0, 0) + (multY, 0, 0, 0) -> (multX + multY, 0, 0, 0)
321 // 0b11111101 == 0xfd
322 const __m128 shuffled = _mm_shuffle_ps(mult, mult, 0xfd);
323 // (multX + multY, 0, 0, 0) + (multZ, 0, 0, 0);
324 // 0b11111110 == 0xfe
325 const __m128 shuffled2 = _mm_shuffle_ps(mult, mult, 0xfe);
326 const __m128 result = _mm_add_ps(a: _mm_add_ps(a: shuffled, b: mult), b: shuffled2);
327 return _mm_cvtss_f32(a: result);
328#endif
329 }
330
331 static Q_ALWAYS_INLINE Vector3D_SSE crossProduct(Vector3D_SSE a, Vector3D_SSE b)
332 {
333 // a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x
334 // (a.y, a.z, a.z, a.x, a.x, a.y) (b.z, b.y, b.x, b.z, b.y, b.x)
335 // (a.y, a.z, a.x) * (b.z, b.x, b.y) - (a.z, a.x, a.y) (b.y, b.z, b.x)
336
337 // 0b11001001 == 0xc9
338 const __m128 a1 = _mm_shuffle_ps(a.m_xyzw, a.m_xyzw, 0xc9);
339 const __m128 b2 = _mm_shuffle_ps(b.m_xyzw, b.m_xyzw, 0xc9);
340 // 0b11010010 == 0xd2
341 const __m128 a2 = _mm_shuffle_ps(a.m_xyzw, a.m_xyzw, 0xd2);
342 const __m128 b1 = _mm_shuffle_ps(b.m_xyzw, b.m_xyzw, 0xd2);
343
344 Vector3D_SSE v(Qt::Uninitialized);
345 v.m_xyzw = _mm_sub_ps(a: _mm_mul_ps(a: a1, b: b1), b: _mm_mul_ps(a: a2, b: b2));
346 return v;
347 }
348
349 friend class Vector4D_SSE;
350
351#if defined(__AVX2__) && defined(QT_COMPILER_SUPPORTS_AVX2)
352 friend class Matrix4x4_AVX2;
353 friend Vector3D_SSE operator*(const Vector3D_SSE &vector, const Matrix4x4_AVX2 &matrix);
354 friend Vector3D_SSE operator*(const Matrix4x4_AVX2 &matrix, const Vector3D_SSE &vector);
355#endif
356
357 friend class Matrix4x4_SSE;
358 friend Vector3D_SSE operator*(const Vector3D_SSE &vector, const Matrix4x4_SSE &matrix);
359 friend Vector3D_SSE operator*(const Matrix4x4_SSE &matrix, const Vector3D_SSE &vector);
360
361 friend Q_ALWAYS_INLINE const Vector3D_SSE operator+(Vector3D_SSE v1, Vector3D_SSE v2) { return v1 += v2; }
362 friend Q_ALWAYS_INLINE const Vector3D_SSE operator-(Vector3D_SSE v1, Vector3D_SSE v2) { return v1 -= v2; }
363 friend Q_ALWAYS_INLINE const Vector3D_SSE operator*(float factor, Vector3D_SSE vector) { return vector *= factor; }
364 friend Q_ALWAYS_INLINE const Vector3D_SSE operator*(Vector3D_SSE vector, float factor) { return vector *= factor; }
365 friend Q_ALWAYS_INLINE const Vector3D_SSE operator*(Vector3D_SSE v1, Vector3D_SSE v2) { return v1 *= v2; }
366 friend Q_ALWAYS_INLINE const Vector3D_SSE operator-(Vector3D_SSE vector)
367 {
368 Vector3D_SSE c(Qt::Uninitialized);
369
370 c.m_xyzw = _mm_xor_ps(a: vector.m_xyzw, b: _mm_set1_ps(w: -0.0f));
371
372 return c;
373 }
374
375 friend Q_ALWAYS_INLINE const Vector3D_SSE operator/(Vector3D_SSE vector, float divisor) { return vector /= divisor; }
376 friend Q_ALWAYS_INLINE const Vector3D_SSE operator/(Vector3D_SSE vector, Vector3D_SSE divisor) { return vector /= divisor; }
377
378 friend Q_3DCORE_PRIVATE_EXPORT QDebug operator<<(QDebug dbg, const Vector3D_SSE &v);
379 friend Q_ALWAYS_INLINE bool qFuzzyCompare(const Vector3D_SSE& v1, const Vector3D_SSE& v2)
380 {
381 return ::qFuzzyCompare(p1: v1.x(), p2: v2.x()) &&
382 ::qFuzzyCompare(p1: v1.y(), p2: v2.y()) &&
383 ::qFuzzyCompare(p1: v1.z(), p2: v2.z());
384 }
385
386private:
387 // Q_DECL_ALIGN(16) float m[4];// for SSE support
388 __m128 m_xyzw;
389};
390
391} // Qt3DCore
392
393Q_DECLARE_TYPEINFO(Qt3DCore::Vector3D_SSE, Q_PRIMITIVE_TYPE);
394
395QT_END_NAMESPACE
396
397Q_DECLARE_METATYPE(Qt3DCore::Vector3D_SSE)
398
399#endif // QT_COMPILER_SUPPORTS_SSE2
400
401#endif // QT3DCORE_VECTOR3D_SSE_P_H
402

source code of qt3d/src/core/transforms/vector3d_sse_p.h