Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
DVec3.inl
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
7#include <Jolt/Core/HashCombine.h>
8
9// Create a std::hash/JPH::Hash for DVec3
10JPH_MAKE_HASHABLE(JPH::DVec3, t.GetX(), t.GetY(), t.GetZ())
11
12JPH_NAMESPACE_BEGIN
13
14DVec3::DVec3(Vec3Arg inRHS)
15{
16#if defined(JPH_USE_AVX)
17 mValue = _mm256_cvtps_pd(inRHS.mValue);
18#elif defined(JPH_USE_SSE)
19 mValue.mLow = _mm_cvtps_pd(inRHS.mValue);
20 mValue.mHigh = _mm_cvtps_pd(_mm_shuffle_ps(inRHS.mValue, inRHS.mValue, _MM_SHUFFLE(2, 2, 2, 2)));
21#elif defined(JPH_USE_NEON)
22 mValue.val[0] = vcvt_f64_f32(vget_low_f32(inRHS.mValue));
23 mValue.val[1] = vcvt_high_f64_f32(inRHS.mValue);
24#else
25 mF64[0] = (double)inRHS.GetX();
26 mF64[1] = (double)inRHS.GetY();
27 mF64[2] = (double)inRHS.GetZ();
28 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
29 mF64[3] = mF64[2];
30 #endif
31#endif
32}
33
34DVec3::DVec3(Vec4Arg inRHS) :
35 DVec3(Vec3(inRHS))
36{
37}
38
39DVec3::DVec3(double inX, double inY, double inZ)
40{
41#if defined(JPH_USE_AVX)
42 mValue = _mm256_set_pd(inZ, inZ, inY, inX); // Assure Z and W are the same
43#elif defined(JPH_USE_SSE)
44 mValue.mLow = _mm_set_pd(inY, inX);
45 mValue.mHigh = _mm_set1_pd(inZ);
46#elif defined(JPH_USE_NEON)
47 mValue.val[0] = vcombine_f64(vcreate_f64(BitCast<uint64>(inX)), vcreate_f64(BitCast<uint64>(inY)));
48 mValue.val[1] = vdupq_n_f64(inZ);
49#else
50 mF64[0] = inX;
51 mF64[1] = inY;
52 mF64[2] = inZ;
53 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
54 mF64[3] = mF64[2];
55 #endif
56#endif
57}
58
59DVec3::DVec3(const Double3 &inV)
60{
61#if defined(JPH_USE_AVX)
62 Type x = _mm256_castpd128_pd256(_mm_load_sd(&inV.x));
63 Type y = _mm256_castpd128_pd256(_mm_load_sd(&inV.y));
64 Type z = _mm256_broadcast_sd(&inV.z);
65 Type xy = _mm256_unpacklo_pd(x, y);
66 mValue = _mm256_blend_pd(xy, z, 0b1100); // Assure Z and W are the same
67#elif defined(JPH_USE_SSE)
68 mValue.mLow = _mm_loadu_pd(&inV.x);
69 mValue.mHigh = _mm_set1_pd(inV.z);
70#elif defined(JPH_USE_NEON)
71 mValue.val[0] = vld1q_f64(&inV.x);
72 mValue.val[1] = vdupq_n_f64(inV.z);
73#else
74 mF64[0] = inV.x;
75 mF64[1] = inV.y;
76 mF64[2] = inV.z;
77 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
78 mF64[3] = mF64[2];
79 #endif
80#endif
81}
82
83void DVec3::CheckW() const
84{
85#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
86 // Avoid asserts when both components are NaN
87 JPH_ASSERT(reinterpret_cast<const uint64 *>(mF64)[2] == reinterpret_cast<const uint64 *>(mF64)[3]);
88#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
89}
90
92DVec3::Type DVec3::sFixW(TypeArg inValue)
93{
94#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
95 #if defined(JPH_USE_AVX)
96 return _mm256_shuffle_pd(inValue, inValue, 2);
97 #elif defined(JPH_USE_SSE)
98 Type value;
99 value.mLow = inValue.mLow;
100 value.mHigh = _mm_shuffle_pd(inValue.mHigh, inValue.mHigh, 0);
101 return value;
102 #elif defined(JPH_USE_NEON)
103 Type value;
104 value.val[0] = inValue.val[0];
105 value.val[1] = vdupq_laneq_f64(inValue.val[1], 0);
106 return value;
107 #else
108 Type value;
109 value.mData[0] = inValue.mData[0];
110 value.mData[1] = inValue.mData[1];
111 value.mData[2] = inValue.mData[2];
112 value.mData[3] = inValue.mData[2];
113 return value;
114 #endif
115#else
116 return inValue;
117#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
118}
119
120DVec3 DVec3::sZero()
121{
122#if defined(JPH_USE_AVX)
123 return _mm256_setzero_pd();
124#elif defined(JPH_USE_SSE)
125 __m128d zero = _mm_setzero_pd();
126 return DVec3({ zero, zero });
127#elif defined(JPH_USE_NEON)
128 float64x2_t zero = vdupq_n_f64(0.0);
129 return DVec3({ zero, zero });
130#else
131 return DVec3(0, 0, 0);
132#endif
133}
134
135DVec3 DVec3::sReplicate(double inV)
136{
137#if defined(JPH_USE_AVX)
138 return _mm256_set1_pd(inV);
139#elif defined(JPH_USE_SSE)
140 __m128d value = _mm_set1_pd(inV);
141 return DVec3({ value, value });
142#elif defined(JPH_USE_NEON)
143 float64x2_t value = vdupq_n_f64(inV);
144 return DVec3({ value, value });
145#else
146 return DVec3(inV, inV, inV);
147#endif
148}
149
150DVec3 DVec3::sNaN()
151{
152 return sReplicate(numeric_limits<double>::quiet_NaN());
153}
154
155DVec3 DVec3::sLoadDouble3Unsafe(const Double3 &inV)
156{
157#if defined(JPH_USE_AVX)
158 Type v = _mm256_loadu_pd(&inV.x);
159#elif defined(JPH_USE_SSE)
160 Type v;
161 v.mLow = _mm_loadu_pd(&inV.x);
162 v.mHigh = _mm_set1_pd(inV.z);
163#elif defined(JPH_USE_NEON)
164 Type v = vld1q_f64_x2(&inV.x);
165#else
166 Type v = { inV.x, inV.y, inV.z };
167#endif
168 return sFixW(v);
169}
170
171void DVec3::StoreDouble3(Double3 *outV) const
172{
173 outV->x = mF64[0];
174 outV->y = mF64[1];
175 outV->z = mF64[2];
176}
177
178DVec3::operator Vec3() const
179{
180#if defined(JPH_USE_AVX)
181 return _mm256_cvtpd_ps(mValue);
182#elif defined(JPH_USE_SSE)
183 __m128 low = _mm_cvtpd_ps(mValue.mLow);
184 __m128 high = _mm_cvtpd_ps(mValue.mHigh);
185 return _mm_shuffle_ps(low, high, _MM_SHUFFLE(1, 0, 1, 0));
186#elif defined(JPH_USE_NEON)
187 return vcvt_high_f32_f64(vcvtx_f32_f64(mValue.val[0]), mValue.val[1]);
188#else
189 return Vec3((float)GetX(), (float)GetY(), (float)GetZ());
190#endif
191}
192
193DVec3 DVec3::sMin(DVec3Arg inV1, DVec3Arg inV2)
194{
195#if defined(JPH_USE_AVX)
196 return _mm256_min_pd(inV1.mValue, inV2.mValue);
197#elif defined(JPH_USE_SSE)
198 return DVec3({ _mm_min_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_min_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
199#elif defined(JPH_USE_NEON)
200 return DVec3({ vminq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vminq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) });
201#else
202 return DVec3(min(inV1.mF64[0], inV2.mF64[0]),
203 min(inV1.mF64[1], inV2.mF64[1]),
204 min(inV1.mF64[2], inV2.mF64[2]));
205#endif
206}
207
208DVec3 DVec3::sMax(DVec3Arg inV1, DVec3Arg inV2)
209{
210#if defined(JPH_USE_AVX)
211 return _mm256_max_pd(inV1.mValue, inV2.mValue);
212#elif defined(JPH_USE_SSE)
213 return DVec3({ _mm_max_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_max_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
214#elif defined(JPH_USE_NEON)
215 return DVec3({ vmaxq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vmaxq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) });
216#else
217 return DVec3(max(inV1.mF64[0], inV2.mF64[0]),
218 max(inV1.mF64[1], inV2.mF64[1]),
219 max(inV1.mF64[2], inV2.mF64[2]));
220#endif
221}
222
223DVec3 DVec3::sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax)
224{
225 return sMax(sMin(inV, inMax), inMin);
226}
227
228DVec3 DVec3::sEquals(DVec3Arg inV1, DVec3Arg inV2)
229{
230#if defined(JPH_USE_AVX)
231 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_EQ_OQ);
232#elif defined(JPH_USE_SSE)
233 return DVec3({ _mm_cmpeq_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpeq_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
234#elif defined(JPH_USE_NEON)
235 return DVec3({ vreinterpretq_f64_u64(vceqq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vceqq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
236#else
237 return DVec3(inV1.mF64[0] == inV2.mF64[0]? cTrue : cFalse,
238 inV1.mF64[1] == inV2.mF64[1]? cTrue : cFalse,
239 inV1.mF64[2] == inV2.mF64[2]? cTrue : cFalse);
240#endif
241}
242
243DVec3 DVec3::sLess(DVec3Arg inV1, DVec3Arg inV2)
244{
245#if defined(JPH_USE_AVX)
246 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LT_OQ);
247#elif defined(JPH_USE_SSE)
248 return DVec3({ _mm_cmplt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmplt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
249#elif defined(JPH_USE_NEON)
250 return DVec3({ vreinterpretq_f64_u64(vcltq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcltq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
251#else
252 return DVec3(inV1.mF64[0] < inV2.mF64[0]? cTrue : cFalse,
253 inV1.mF64[1] < inV2.mF64[1]? cTrue : cFalse,
254 inV1.mF64[2] < inV2.mF64[2]? cTrue : cFalse);
255#endif
256}
257
258DVec3 DVec3::sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2)
259{
260#if defined(JPH_USE_AVX)
261 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LE_OQ);
262#elif defined(JPH_USE_SSE)
263 return DVec3({ _mm_cmple_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmple_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
264#elif defined(JPH_USE_NEON)
265 return DVec3({ vreinterpretq_f64_u64(vcleq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcleq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
266#else
267 return DVec3(inV1.mF64[0] <= inV2.mF64[0]? cTrue : cFalse,
268 inV1.mF64[1] <= inV2.mF64[1]? cTrue : cFalse,
269 inV1.mF64[2] <= inV2.mF64[2]? cTrue : cFalse);
270#endif
271}
272
273DVec3 DVec3::sGreater(DVec3Arg inV1, DVec3Arg inV2)
274{
275#if defined(JPH_USE_AVX)
276 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GT_OQ);
277#elif defined(JPH_USE_SSE)
278 return DVec3({ _mm_cmpgt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpgt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
279#elif defined(JPH_USE_NEON)
280 return DVec3({ vreinterpretq_f64_u64(vcgtq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcgtq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
281#else
282 return DVec3(inV1.mF64[0] > inV2.mF64[0]? cTrue : cFalse,
283 inV1.mF64[1] > inV2.mF64[1]? cTrue : cFalse,
284 inV1.mF64[2] > inV2.mF64[2]? cTrue : cFalse);
285#endif
286}
287
288DVec3 DVec3::sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2)
289{
290#if defined(JPH_USE_AVX)
291 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GE_OQ);
292#elif defined(JPH_USE_SSE)
293 return DVec3({ _mm_cmpge_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpge_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
294#elif defined(JPH_USE_NEON)
295 return DVec3({ vreinterpretq_f64_u64(vcgeq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_f64_u64(vcgeq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) });
296#else
297 return DVec3(inV1.mF64[0] >= inV2.mF64[0]? cTrue : cFalse,
298 inV1.mF64[1] >= inV2.mF64[1]? cTrue : cFalse,
299 inV1.mF64[2] >= inV2.mF64[2]? cTrue : cFalse);
300#endif
301}
302
303DVec3 DVec3::sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd)
304{
305#if defined(JPH_USE_AVX)
306 #ifdef JPH_USE_FMADD
307 return _mm256_fmadd_pd(inMul1.mValue, inMul2.mValue, inAdd.mValue);
308 #else
309 return _mm256_add_pd(_mm256_mul_pd(inMul1.mValue, inMul2.mValue), inAdd.mValue);
310 #endif
311#elif defined(JPH_USE_NEON)
312 return DVec3({ vmlaq_f64(inAdd.mValue.val[0], inMul1.mValue.val[0], inMul2.mValue.val[0]), vmlaq_f64(inAdd.mValue.val[1], inMul1.mValue.val[1], inMul2.mValue.val[1]) });
313#else
314 return inMul1 * inMul2 + inAdd;
315#endif
316}
317
318DVec3 DVec3::sSelect(DVec3Arg inNotSet, DVec3Arg inSet, DVec3Arg inControl)
319{
320#if defined(JPH_USE_AVX)
321 return _mm256_blendv_pd(inNotSet.mValue, inSet.mValue, inControl.mValue);
322#elif defined(JPH_USE_SSE4_1)
323 Type v = { _mm_blendv_pd(inNotSet.mValue.mLow, inSet.mValue.mLow, inControl.mValue.mLow), _mm_blendv_pd(inNotSet.mValue.mHigh, inSet.mValue.mHigh, inControl.mValue.mHigh) };
324 return sFixW(v);
325#elif defined(JPH_USE_NEON)
326 Type v = { vbslq_f64(vreinterpretq_u64_s64(vshrq_n_s64(vreinterpretq_s64_f64(inControl.mValue.val[0]), 63)), inSet.mValue.val[0], inNotSet.mValue.val[0]),
327 vbslq_f64(vreinterpretq_u64_s64(vshrq_n_s64(vreinterpretq_s64_f64(inControl.mValue.val[1]), 63)), inSet.mValue.val[1], inNotSet.mValue.val[1]) };
328 return sFixW(v);
329#else
330 DVec3 result;
331 for (int i = 0; i < 3; i++)
332 result.mF64[i] = (BitCast<uint64>(inControl.mF64[i]) & (uint64(1) << 63))? inSet.mF64[i] : inNotSet.mF64[i];
333#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
334 result.mF64[3] = result.mF64[2];
335#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
336 return result;
337#endif
338}
339
340DVec3 DVec3::sOr(DVec3Arg inV1, DVec3Arg inV2)
341{
342#if defined(JPH_USE_AVX)
343 return _mm256_or_pd(inV1.mValue, inV2.mValue);
344#elif defined(JPH_USE_SSE)
345 return DVec3({ _mm_or_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_or_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
346#elif defined(JPH_USE_NEON)
347 return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))),
348 vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) });
349#else
350 return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) | BitCast<uint64>(inV2.mF64[0])),
351 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) | BitCast<uint64>(inV2.mF64[1])),
352 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) | BitCast<uint64>(inV2.mF64[2])));
353#endif
354}
355
356DVec3 DVec3::sXor(DVec3Arg inV1, DVec3Arg inV2)
357{
358#if defined(JPH_USE_AVX)
359 return _mm256_xor_pd(inV1.mValue, inV2.mValue);
360#elif defined(JPH_USE_SSE)
361 return DVec3({ _mm_xor_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_xor_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
362#elif defined(JPH_USE_NEON)
363 return DVec3({ vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))),
364 vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) });
365#else
366 return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) ^ BitCast<uint64>(inV2.mF64[0])),
367 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) ^ BitCast<uint64>(inV2.mF64[1])),
368 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) ^ BitCast<uint64>(inV2.mF64[2])));
369#endif
370}
371
372DVec3 DVec3::sAnd(DVec3Arg inV1, DVec3Arg inV2)
373{
374#if defined(JPH_USE_AVX)
375 return _mm256_and_pd(inV1.mValue, inV2.mValue);
376#elif defined(JPH_USE_SSE)
377 return DVec3({ _mm_and_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_and_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
378#elif defined(JPH_USE_NEON)
379 return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))),
380 vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) });
381#else
382 return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) & BitCast<uint64>(inV2.mF64[0])),
383 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) & BitCast<uint64>(inV2.mF64[1])),
384 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) & BitCast<uint64>(inV2.mF64[2])));
385#endif
386}
387
388int DVec3::GetTrues() const
389{
390#if defined(JPH_USE_AVX)
391 return _mm256_movemask_pd(mValue) & 0x7;
392#elif defined(JPH_USE_SSE)
393 return (_mm_movemask_pd(mValue.mLow) + (_mm_movemask_pd(mValue.mHigh) << 2)) & 0x7;
394#else
395 return int((BitCast<uint64>(mF64[0]) >> 63) | ((BitCast<uint64>(mF64[1]) >> 63) << 1) | ((BitCast<uint64>(mF64[2]) >> 63) << 2));
396#endif
397}
398
399bool DVec3::TestAnyTrue() const
400{
401 return GetTrues() != 0;
402}
403
404bool DVec3::TestAllTrue() const
405{
406 return GetTrues() == 0x7;
407}
408
409bool DVec3::operator == (DVec3Arg inV2) const
410{
411 return sEquals(*this, inV2).TestAllTrue();
412}
413
414bool DVec3::IsClose(DVec3Arg inV2, double inMaxDistSq) const
415{
416 return (inV2 - *this).LengthSq() <= inMaxDistSq;
417}
418
419bool DVec3::IsNearZero(double inMaxDistSq) const
420{
421 return LengthSq() <= inMaxDistSq;
422}
423
424DVec3 DVec3::operator * (DVec3Arg inV2) const
425{
426#if defined(JPH_USE_AVX)
427 return _mm256_mul_pd(mValue, inV2.mValue);
428#elif defined(JPH_USE_SSE)
429 return DVec3({ _mm_mul_pd(mValue.mLow, inV2.mValue.mLow), _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh) });
430#elif defined(JPH_USE_NEON)
431 return DVec3({ vmulq_f64(mValue.val[0], inV2.mValue.val[0]), vmulq_f64(mValue.val[1], inV2.mValue.val[1]) });
432#else
433 return DVec3(mF64[0] * inV2.mF64[0], mF64[1] * inV2.mF64[1], mF64[2] * inV2.mF64[2]);
434#endif
435}
436
437DVec3 DVec3::operator * (double inV2) const
438{
439#if defined(JPH_USE_AVX)
440 return _mm256_mul_pd(mValue, _mm256_set1_pd(inV2));
441#elif defined(JPH_USE_SSE)
442 __m128d v = _mm_set1_pd(inV2);
443 return DVec3({ _mm_mul_pd(mValue.mLow, v), _mm_mul_pd(mValue.mHigh, v) });
444#elif defined(JPH_USE_NEON)
445 return DVec3({ vmulq_n_f64(mValue.val[0], inV2), vmulq_n_f64(mValue.val[1], inV2) });
446#else
447 return DVec3(mF64[0] * inV2, mF64[1] * inV2, mF64[2] * inV2);
448#endif
449}
450
451DVec3 operator * (double inV1, DVec3Arg inV2)
452{
453#if defined(JPH_USE_AVX)
454 return _mm256_mul_pd(_mm256_set1_pd(inV1), inV2.mValue);
455#elif defined(JPH_USE_SSE)
456 __m128d v = _mm_set1_pd(inV1);
457 return DVec3({ _mm_mul_pd(v, inV2.mValue.mLow), _mm_mul_pd(v, inV2.mValue.mHigh) });
458#elif defined(JPH_USE_NEON)
459 return DVec3({ vmulq_n_f64(inV2.mValue.val[0], inV1), vmulq_n_f64(inV2.mValue.val[1], inV1) });
460#else
461 return DVec3(inV1 * inV2.mF64[0], inV1 * inV2.mF64[1], inV1 * inV2.mF64[2]);
462#endif
463}
464
465DVec3 DVec3::operator / (double inV2) const
466{
467#if defined(JPH_USE_AVX)
468 return _mm256_div_pd(mValue, _mm256_set1_pd(inV2));
469#elif defined(JPH_USE_SSE)
470 __m128d v = _mm_set1_pd(inV2);
471 return DVec3({ _mm_div_pd(mValue.mLow, v), _mm_div_pd(mValue.mHigh, v) });
472#elif defined(JPH_USE_NEON)
473 float64x2_t v = vdupq_n_f64(inV2);
474 return DVec3({ vdivq_f64(mValue.val[0], v), vdivq_f64(mValue.val[1], v) });
475#else
476 return DVec3(mF64[0] / inV2, mF64[1] / inV2, mF64[2] / inV2);
477#endif
478}
479
480DVec3 &DVec3::operator *= (double inV2)
481{
482#if defined(JPH_USE_AVX)
483 mValue = _mm256_mul_pd(mValue, _mm256_set1_pd(inV2));
484#elif defined(JPH_USE_SSE)
485 __m128d v = _mm_set1_pd(inV2);
486 mValue.mLow = _mm_mul_pd(mValue.mLow, v);
487 mValue.mHigh = _mm_mul_pd(mValue.mHigh, v);
488#elif defined(JPH_USE_NEON)
489 mValue.val[0] = vmulq_n_f64(mValue.val[0], inV2);
490 mValue.val[1] = vmulq_n_f64(mValue.val[1], inV2);
491#else
492 for (int i = 0; i < 3; ++i)
493 mF64[i] *= inV2;
494 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
495 mF64[3] = mF64[2];
496 #endif
497#endif
498 return *this;
499}
500
501DVec3 &DVec3::operator *= (DVec3Arg inV2)
502{
503#if defined(JPH_USE_AVX)
504 mValue = _mm256_mul_pd(mValue, inV2.mValue);
505#elif defined(JPH_USE_SSE)
506 mValue.mLow = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow);
507 mValue.mHigh = _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh);
508#elif defined(JPH_USE_NEON)
509 mValue.val[0] = vmulq_f64(mValue.val[0], inV2.mValue.val[0]);
510 mValue.val[1] = vmulq_f64(mValue.val[1], inV2.mValue.val[1]);
511#else
512 for (int i = 0; i < 3; ++i)
513 mF64[i] *= inV2.mF64[i];
514 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
515 mF64[3] = mF64[2];
516 #endif
517#endif
518 return *this;
519}
520
521DVec3 &DVec3::operator /= (double inV2)
522{
523#if defined(JPH_USE_AVX)
524 mValue = _mm256_div_pd(mValue, _mm256_set1_pd(inV2));
525#elif defined(JPH_USE_SSE)
526 __m128d v = _mm_set1_pd(inV2);
527 mValue.mLow = _mm_div_pd(mValue.mLow, v);
528 mValue.mHigh = _mm_div_pd(mValue.mHigh, v);
529#elif defined(JPH_USE_NEON)
530 float64x2_t v = vdupq_n_f64(inV2);
531 mValue.val[0] = vdivq_f64(mValue.val[0], v);
532 mValue.val[1] = vdivq_f64(mValue.val[1], v);
533#else
534 for (int i = 0; i < 3; ++i)
535 mF64[i] /= inV2;
536 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
537 mF64[3] = mF64[2];
538 #endif
539#endif
540 return *this;
541}
542
543DVec3 DVec3::operator + (Vec3Arg inV2) const
544{
545#if defined(JPH_USE_AVX)
546 return _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
547#elif defined(JPH_USE_SSE)
548 return DVec3({ _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) });
549#elif defined(JPH_USE_NEON)
550 return DVec3({ vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) });
551#else
552 return DVec3(mF64[0] + inV2.mF32[0], mF64[1] + inV2.mF32[1], mF64[2] + inV2.mF32[2]);
553#endif
554}
555
556DVec3 DVec3::operator + (DVec3Arg inV2) const
557{
558#if defined(JPH_USE_AVX)
559 return _mm256_add_pd(mValue, inV2.mValue);
560#elif defined(JPH_USE_SSE)
561 return DVec3({ _mm_add_pd(mValue.mLow, inV2.mValue.mLow), _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh) });
562#elif defined(JPH_USE_NEON)
563 return DVec3({ vaddq_f64(mValue.val[0], inV2.mValue.val[0]), vaddq_f64(mValue.val[1], inV2.mValue.val[1]) });
564#else
565 return DVec3(mF64[0] + inV2.mF64[0], mF64[1] + inV2.mF64[1], mF64[2] + inV2.mF64[2]);
566#endif
567}
568
569DVec3 &DVec3::operator += (Vec3Arg inV2)
570{
571#if defined(JPH_USE_AVX)
572 mValue = _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
573#elif defined(JPH_USE_SSE)
574 mValue.mLow = _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue));
575 mValue.mHigh = _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
576#elif defined(JPH_USE_NEON)
577 mValue.val[0] = vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue)));
578 mValue.val[1] = vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue));
579#else
580 for (int i = 0; i < 3; ++i)
581 mF64[i] += inV2.mF32[i];
582 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
583 mF64[3] = mF64[2];
584 #endif
585#endif
586 return *this;
587}
588
589DVec3 &DVec3::operator += (DVec3Arg inV2)
590{
591#if defined(JPH_USE_AVX)
592 mValue = _mm256_add_pd(mValue, inV2.mValue);
593#elif defined(JPH_USE_SSE)
594 mValue.mLow = _mm_add_pd(mValue.mLow, inV2.mValue.mLow);
595 mValue.mHigh = _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh);
596#elif defined(JPH_USE_NEON)
597 mValue.val[0] = vaddq_f64(mValue.val[0], inV2.mValue.val[0]);
598 mValue.val[1] = vaddq_f64(mValue.val[1], inV2.mValue.val[1]);
599#else
600 for (int i = 0; i < 3; ++i)
601 mF64[i] += inV2.mF64[i];
602 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
603 mF64[3] = mF64[2];
604 #endif
605#endif
606 return *this;
607}
608
609DVec3 DVec3::operator - () const
610{
611#if defined(JPH_USE_AVX)
612 return _mm256_sub_pd(_mm256_setzero_pd(), mValue);
613#elif defined(JPH_USE_SSE)
614 __m128d zero = _mm_setzero_pd();
615 return DVec3({ _mm_sub_pd(zero, mValue.mLow), _mm_sub_pd(zero, mValue.mHigh) });
616#elif defined(JPH_USE_NEON)
617 #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
618 float64x2_t zero = vdupq_n_f64(0);
619 return DVec3({ vsubq_f64(zero, mValue.val[0]), vsubq_f64(zero, mValue.val[1]) });
620 #else
621 return DVec3({ vnegq_f64(mValue.val[0]), vnegq_f64(mValue.val[1]) });
622 #endif
623#else
624 #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
625 return DVec3(0.0 - mF64[0], 0.0 - mF64[1], 0.0 - mF64[2]);
626 #else
627 return DVec3(-mF64[0], -mF64[1], -mF64[2]);
628 #endif
629#endif
630}
631
632DVec3 DVec3::operator - (Vec3Arg inV2) const
633{
634#if defined(JPH_USE_AVX)
635 return _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
636#elif defined(JPH_USE_SSE)
637 return DVec3({ _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) });
638#elif defined(JPH_USE_NEON)
639 return DVec3({ vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) });
640#else
641 return DVec3(mF64[0] - inV2.mF32[0], mF64[1] - inV2.mF32[1], mF64[2] - inV2.mF32[2]);
642#endif
643}
644
645DVec3 DVec3::operator - (DVec3Arg inV2) const
646{
647#if defined(JPH_USE_AVX)
648 return _mm256_sub_pd(mValue, inV2.mValue);
649#elif defined(JPH_USE_SSE)
650 return DVec3({ _mm_sub_pd(mValue.mLow, inV2.mValue.mLow), _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh) });
651#elif defined(JPH_USE_NEON)
652 return DVec3({ vsubq_f64(mValue.val[0], inV2.mValue.val[0]), vsubq_f64(mValue.val[1], inV2.mValue.val[1]) });
653#else
654 return DVec3(mF64[0] - inV2.mF64[0], mF64[1] - inV2.mF64[1], mF64[2] - inV2.mF64[2]);
655#endif
656}
657
658DVec3 &DVec3::operator -= (Vec3Arg inV2)
659{
660#if defined(JPH_USE_AVX)
661 mValue = _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
662#elif defined(JPH_USE_SSE)
663 mValue.mLow = _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue));
664 mValue.mHigh = _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
665#elif defined(JPH_USE_NEON)
666 mValue.val[0] = vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue)));
667 mValue.val[1] = vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue));
668#else
669 for (int i = 0; i < 3; ++i)
670 mF64[i] -= inV2.mF32[i];
671 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
672 mF64[3] = mF64[2];
673 #endif
674#endif
675 return *this;
676}
677
678DVec3 &DVec3::operator -= (DVec3Arg inV2)
679{
680#if defined(JPH_USE_AVX)
681 mValue = _mm256_sub_pd(mValue, inV2.mValue);
682#elif defined(JPH_USE_SSE)
683 mValue.mLow = _mm_sub_pd(mValue.mLow, inV2.mValue.mLow);
684 mValue.mHigh = _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh);
685#elif defined(JPH_USE_NEON)
686 mValue.val[0] = vsubq_f64(mValue.val[0], inV2.mValue.val[0]);
687 mValue.val[1] = vsubq_f64(mValue.val[1], inV2.mValue.val[1]);
688#else
689 for (int i = 0; i < 3; ++i)
690 mF64[i] -= inV2.mF64[i];
691 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
692 mF64[3] = mF64[2];
693 #endif
694#endif
695 return *this;
696}
697
698DVec3 DVec3::operator / (DVec3Arg inV2) const
699{
700 inV2.CheckW();
701#if defined(JPH_USE_AVX)
702 return _mm256_div_pd(mValue, inV2.mValue);
703#elif defined(JPH_USE_SSE)
704 return DVec3({ _mm_div_pd(mValue.mLow, inV2.mValue.mLow), _mm_div_pd(mValue.mHigh, inV2.mValue.mHigh) });
705#elif defined(JPH_USE_NEON)
706 return DVec3({ vdivq_f64(mValue.val[0], inV2.mValue.val[0]), vdivq_f64(mValue.val[1], inV2.mValue.val[1]) });
707#else
708 return DVec3(mF64[0] / inV2.mF64[0], mF64[1] / inV2.mF64[1], mF64[2] / inV2.mF64[2]);
709#endif
710}
711
712DVec3 DVec3::Abs() const
713{
714#if defined(JPH_USE_AVX512)
715 return _mm256_range_pd(mValue, mValue, 0b1000);
716#elif defined(JPH_USE_AVX)
717 return _mm256_max_pd(_mm256_sub_pd(_mm256_setzero_pd(), mValue), mValue);
718#elif defined(JPH_USE_SSE)
719 __m128d zero = _mm_setzero_pd();
720 return DVec3({ _mm_max_pd(_mm_sub_pd(zero, mValue.mLow), mValue.mLow), _mm_max_pd(_mm_sub_pd(zero, mValue.mHigh), mValue.mHigh) });
721#elif defined(JPH_USE_NEON)
722 return DVec3({ vabsq_f64(mValue.val[0]), vabsq_f64(mValue.val[1]) });
723#else
724 return DVec3(abs(mF64[0]), abs(mF64[1]), abs(mF64[2]));
725#endif
726}
727
728DVec3 DVec3::Reciprocal() const
729{
730 return sReplicate(1.0) / mValue;
731}
732
733DVec3 DVec3::Cross(DVec3Arg inV2) const
734{
735#if defined(JPH_USE_AVX2)
736 __m256d t1 = _mm256_permute4x64_pd(inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
737 t1 = _mm256_mul_pd(t1, mValue);
738 __m256d t2 = _mm256_permute4x64_pd(mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
739 t2 = _mm256_mul_pd(t2, inV2.mValue);
740 __m256d t3 = _mm256_sub_pd(t1, t2);
741 return _mm256_permute4x64_pd(t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
742#else
743 return DVec3(mF64[1] * inV2.mF64[2] - mF64[2] * inV2.mF64[1],
744 mF64[2] * inV2.mF64[0] - mF64[0] * inV2.mF64[2],
745 mF64[0] * inV2.mF64[1] - mF64[1] * inV2.mF64[0]);
746#endif
747}
748
749double DVec3::Dot(DVec3Arg inV2) const
750{
751#if defined(JPH_USE_AVX)
752 __m256d mul = _mm256_mul_pd(mValue, inV2.mValue);
753 __m128d xy = _mm256_castpd256_pd128(mul);
754 __m128d yx = _mm_shuffle_pd(xy, xy, 1);
755 __m128d sum = _mm_add_pd(xy, yx);
756 __m128d zw = _mm256_extractf128_pd(mul, 1);
757 sum = _mm_add_pd(sum, zw);
758 return _mm_cvtsd_f64(sum);
759#elif defined(JPH_USE_SSE)
760 __m128d xy = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow);
761 __m128d yx = _mm_shuffle_pd(xy, xy, 1);
762 __m128d sum = _mm_add_pd(xy, yx);
763 __m128d z = _mm_mul_sd(mValue.mHigh, inV2.mValue.mHigh);
764 sum = _mm_add_pd(sum, z);
765 return _mm_cvtsd_f64(sum);
766#elif defined(JPH_USE_NEON)
767 float64x2_t mul_low = vmulq_f64(mValue.val[0], inV2.mValue.val[0]);
768 float64x2_t mul_high = vmulq_f64(mValue.val[1], inV2.mValue.val[1]);
769 return vaddvq_f64(mul_low) + vgetq_lane_f64(mul_high, 0);
770#else
771 double dot = 0.0;
772 for (int i = 0; i < 3; i++)
773 dot += mF64[i] * inV2.mF64[i];
774 return dot;
775#endif
776}
777
778double DVec3::LengthSq() const
779{
780 return Dot(*this);
781}
782
783DVec3 DVec3::Sqrt() const
784{
785#if defined(JPH_USE_AVX)
786 return _mm256_sqrt_pd(mValue);
787#elif defined(JPH_USE_SSE)
788 return DVec3({ _mm_sqrt_pd(mValue.mLow), _mm_sqrt_pd(mValue.mHigh) });
789#elif defined(JPH_USE_NEON)
790 return DVec3({ vsqrtq_f64(mValue.val[0]), vsqrtq_f64(mValue.val[1]) });
791#else
792 return DVec3(sqrt(mF64[0]), sqrt(mF64[1]), sqrt(mF64[2]));
793#endif
794}
795
796double DVec3::Length() const
797{
798 return sqrt(Dot(*this));
799}
800
801DVec3 DVec3::Normalized() const
802{
803 return *this / Length();
804}
805
806bool DVec3::IsNormalized(double inTolerance) const
807{
808 return abs(LengthSq() - 1.0) <= inTolerance;
809}
810
811bool DVec3::IsNaN() const
812{
813#if defined(JPH_USE_AVX512)
814 return (_mm256_fpclass_pd_mask(mValue, 0b10000001) & 0x7) != 0;
815#elif defined(JPH_USE_AVX)
816 return (_mm256_movemask_pd(_mm256_cmp_pd(mValue, mValue, _CMP_UNORD_Q)) & 0x7) != 0;
817#elif defined(JPH_USE_SSE)
818 return ((_mm_movemask_pd(_mm_cmpunord_pd(mValue.mLow, mValue.mLow)) + (_mm_movemask_pd(_mm_cmpunord_pd(mValue.mHigh, mValue.mHigh)) << 2)) & 0x7) != 0;
819#else
820 return isnan(mF64[0]) || isnan(mF64[1]) || isnan(mF64[2]);
821#endif
822}
823
824DVec3 DVec3::GetSign() const
825{
826#if defined(JPH_USE_AVX512)
827 return _mm256_fixupimm_pd(mValue, mValue, _mm256_set1_epi32(0xA9A90A00), 0);
828#elif defined(JPH_USE_AVX)
829 __m256d minus_one = _mm256_set1_pd(-1.0);
830 __m256d one = _mm256_set1_pd(1.0);
831 return _mm256_or_pd(_mm256_and_pd(mValue, minus_one), one);
832#elif defined(JPH_USE_SSE)
833 __m128d minus_one = _mm_set1_pd(-1.0);
834 __m128d one = _mm_set1_pd(1.0);
835 return DVec3({ _mm_or_pd(_mm_and_pd(mValue.mLow, minus_one), one), _mm_or_pd(_mm_and_pd(mValue.mHigh, minus_one), one) });
836#elif defined(JPH_USE_NEON)
837 uint64x2_t minus_one = vreinterpretq_u64_f64(vdupq_n_f64(-1.0f));
838 uint64x2_t one = vreinterpretq_u64_f64(vdupq_n_f64(1.0f));
839 return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), minus_one), one)),
840 vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), minus_one), one)) });
841#else
842 return DVec3(std::signbit(mF64[0])? -1.0 : 1.0,
843 std::signbit(mF64[1])? -1.0 : 1.0,
844 std::signbit(mF64[2])? -1.0 : 1.0);
845#endif
846}
847
848DVec3 DVec3::PrepareRoundToZero() const
849{
850 // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float
851 constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1;
852
853#if defined(JPH_USE_AVX)
854 return _mm256_and_pd(mValue, _mm256_castsi256_pd(_mm256_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss))));
855#elif defined(JPH_USE_SSE)
856 __m128d mask = _mm_castsi128_pd(_mm_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss)));
857 return DVec3({ _mm_and_pd(mValue.mLow, mask), _mm_and_pd(mValue.mHigh, mask) });
858#elif defined(JPH_USE_NEON)
859 uint64x2_t mask = vdupq_n_u64(~cDoubleToFloatMantissaLoss);
860 return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mask)),
861 vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mask)) });
862#else
863 double x = BitCast<double>(BitCast<uint64>(mF64[0]) & ~cDoubleToFloatMantissaLoss);
864 double y = BitCast<double>(BitCast<uint64>(mF64[1]) & ~cDoubleToFloatMantissaLoss);
865 double z = BitCast<double>(BitCast<uint64>(mF64[2]) & ~cDoubleToFloatMantissaLoss);
866
867 return DVec3(x, y, z);
868#endif
869}
870
871DVec3 DVec3::PrepareRoundToInf() const
872{
873 // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float
874 constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1;
875
876#if defined(JPH_USE_AVX512)
877 __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss);
878 __mmask8 is_zero = _mm256_testn_epi64_mask(_mm256_castpd_si256(mValue), mantissa_loss);
879 __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
880 return _mm256_mask_blend_pd(is_zero, value_or_mantissa_loss, mValue);
881#elif defined(JPH_USE_AVX)
882 __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss);
883 __m256d value_and_mantissa_loss = _mm256_and_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
884 __m256d is_zero = _mm256_cmp_pd(value_and_mantissa_loss, _mm256_setzero_pd(), _CMP_EQ_OQ);
885 __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
886 return _mm256_blendv_pd(value_or_mantissa_loss, mValue, is_zero);
887#elif defined(JPH_USE_SSE4_1)
888 __m128i mantissa_loss = _mm_set1_epi64x(cDoubleToFloatMantissaLoss);
889 __m128d zero = _mm_setzero_pd();
890 __m128d value_and_mantissa_loss_low = _mm_and_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss));
891 __m128d is_zero_low = _mm_cmpeq_pd(value_and_mantissa_loss_low, zero);
892 __m128d value_or_mantissa_loss_low = _mm_or_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss));
893 __m128d value_and_mantissa_loss_high = _mm_and_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss));
894 __m128d is_zero_high = _mm_cmpeq_pd(value_and_mantissa_loss_high, zero);
895 __m128d value_or_mantissa_loss_high = _mm_or_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss));
896 return DVec3({ _mm_blendv_pd(value_or_mantissa_loss_low, mValue.mLow, is_zero_low), _mm_blendv_pd(value_or_mantissa_loss_high, mValue.mHigh, is_zero_high) });
897#elif defined(JPH_USE_NEON)
898 uint64x2_t mantissa_loss = vdupq_n_u64(cDoubleToFloatMantissaLoss);
899 float64x2_t zero = vdupq_n_f64(0.0);
900 float64x2_t value_and_mantissa_loss_low = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss));
901 uint64x2_t is_zero_low = vceqq_f64(value_and_mantissa_loss_low, zero);
902 float64x2_t value_or_mantissa_loss_low = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss));
903 float64x2_t value_and_mantissa_loss_high = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss));
904 float64x2_t value_low = vbslq_f64(is_zero_low, mValue.val[0], value_or_mantissa_loss_low);
905 uint64x2_t is_zero_high = vceqq_f64(value_and_mantissa_loss_high, zero);
906 float64x2_t value_or_mantissa_loss_high = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss));
907 float64x2_t value_high = vbslq_f64(is_zero_high, mValue.val[1], value_or_mantissa_loss_high);
908 return DVec3({ value_low, value_high });
909#else
910 uint64 ux = BitCast<uint64>(mF64[0]);
911 uint64 uy = BitCast<uint64>(mF64[1]);
912 uint64 uz = BitCast<uint64>(mF64[2]);
913
914 double x = BitCast<double>((ux & cDoubleToFloatMantissaLoss) == 0? ux : (ux | cDoubleToFloatMantissaLoss));
915 double y = BitCast<double>((uy & cDoubleToFloatMantissaLoss) == 0? uy : (uy | cDoubleToFloatMantissaLoss));
916 double z = BitCast<double>((uz & cDoubleToFloatMantissaLoss) == 0? uz : (uz | cDoubleToFloatMantissaLoss));
917
918 return DVec3(x, y, z);
919#endif
920}
921
922Vec3 DVec3::ToVec3RoundDown() const
923{
924 DVec3 to_zero = PrepareRoundToZero();
925 DVec3 to_inf = PrepareRoundToInf();
926 return Vec3(DVec3::sSelect(to_zero, to_inf, DVec3::sLess(*this, DVec3::sZero())));
927}
928
929Vec3 DVec3::ToVec3RoundUp() const
930{
931 DVec3 to_zero = PrepareRoundToZero();
932 DVec3 to_inf = PrepareRoundToInf();
933 return Vec3(DVec3::sSelect(to_inf, to_zero, DVec3::sLess(*this, DVec3::sZero())));
934}
935
936JPH_NAMESPACE_END
uint64
std::uint64_t uint64
Definition Core.h:450
JPH_NAMESPACE_END
#define JPH_NAMESPACE_END
Definition Core.h:379
JPH_NAMESPACE_BEGIN
#define JPH_NAMESPACE_BEGIN
Definition Core.h:373
operator*
DVec3 operator*(double inV1, DVec3Arg inV2)
Definition DVec3.inl:451
JPH_MAKE_HASHABLE
#define JPH_MAKE_HASHABLE(type,...)
Definition HashCombine.h:191
JPH_ASSERT
#define JPH_ASSERT(...)
Definition IssueReporting.h:33
DVec3
Definition DVec3.h:14
DVec3::sLess
static JPH_INLINE DVec3 sLess(DVec3Arg inV1, DVec3Arg inV2)
Less than (component wise)
Definition DVec3.inl:243
DVec3::mF64
double mF64[4]
Definition DVec3.h:280
DVec3::sMax
static JPH_INLINE DVec3 sMax(DVec3Arg inV1, DVec3Arg inV2)
Return the maximum of each of the components.
Definition DVec3.inl:208
DVec3::Type
{ double mData[4] Type
Definition DVec3.h:29
DVec3::TestAnyTrue
JPH_INLINE bool TestAnyTrue() const
Test if any of the components are true (true is when highest bit of component is set)
Definition DVec3.inl:399
DVec3::ToVec3RoundDown
JPH_INLINE Vec3 ToVec3RoundDown() const
Convert to float vector 3 rounding down.
Definition DVec3.inl:922
DVec3::sClamp
static JPH_INLINE DVec3 sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax)
Clamp a vector between min and max (component wise)
Definition DVec3.inl:223
DVec3::sMin
static JPH_INLINE DVec3 sMin(DVec3Arg inV1, DVec3Arg inV2)
Return the minimum value of each of the components.
Definition DVec3.inl:193
DVec3::GetTrues
JPH_INLINE int GetTrues() const
Store if X is true in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 (true is when highest bit of compo...
Definition DVec3.inl:388
DVec3::sAnd
static JPH_INLINE DVec3 sAnd(DVec3Arg inV1, DVec3Arg inV2)
Logical and (component wise)
Definition DVec3.inl:372
DVec3::operator*=
JPH_INLINE DVec3 & operator*=(double inV2)
Multiply vector with double.
Definition DVec3.inl:480
DVec3::Abs
JPH_INLINE DVec3 Abs() const
Return the absolute value of each of the components.
Definition DVec3.inl:712
DVec3::sFusedMultiplyAdd
static JPH_INLINE DVec3 sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd)
Calculates inMul1 * inMul2 + inAdd.
Definition DVec3.inl:303
DVec3::sFixW
static JPH_INLINE Type sFixW(TypeArg inValue)
Internal helper function that ensures that the Z component is replicated to the W component to preven...
Definition DVec3.inl:92
DVec3::Sqrt
JPH_INLINE DVec3 Sqrt() const
Component wise square root.
Definition DVec3.inl:783
DVec3::GetSign
JPH_INLINE DVec3 GetSign() const
Get vector that contains the sign of each element (returns 1 if positive, -1 if negative)
Definition DVec3.inl:824
DVec3::mValue
Type mValue
Definition DVec3.h:279
DVec3::sXor
static JPH_INLINE DVec3 sXor(DVec3Arg inV1, DVec3Arg inV2)
Logical xor (component wise)
Definition DVec3.inl:356
DVec3::sGreaterOrEqual
static JPH_INLINE DVec3 sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2)
Greater than or equal (component wise)
Definition DVec3.inl:288
DVec3::operator+
JPH_INLINE DVec3 operator+(Vec3Arg inV2) const
Add two vectors (component wise)
Definition DVec3.inl:543
DVec3::IsClose
JPH_INLINE bool IsClose(DVec3Arg inV2, double inMaxDistSq=1.0e-24) const
Test if two vectors are close.
Definition DVec3.inl:414
DVec3::IsNormalized
JPH_INLINE bool IsNormalized(double inTolerance=1.0e-12) const
Test if vector is normalized.
Definition DVec3.inl:806
DVec3::sSelect
static JPH_INLINE DVec3 sSelect(DVec3Arg inNotSet, DVec3Arg inSet, DVec3Arg inControl)
Component wise select, returns inNotSet when highest bit of inControl = 0 and inSet when highest bit ...
Definition DVec3.inl:318
DVec3::TypeArg
const Type & TypeArg
Definition DVec3.h:30
DVec3::sNaN
static JPH_INLINE DVec3 sNaN()
Vector with all NaN's.
Definition DVec3.inl:150
DVec3::operator*
friend JPH_INLINE DVec3 operator*(double inV1, DVec3Arg inV2)
Multiply vector with double.
Definition DVec3.inl:451
DVec3::sGreater
static JPH_INLINE DVec3 sGreater(DVec3Arg inV1, DVec3Arg inV2)
Greater than (component wise)
Definition DVec3.inl:273
DVec3::StoreDouble3
JPH_INLINE void StoreDouble3(Double3 *outV) const
Store 3 doubles to memory.
Definition DVec3.inl:171
DVec3::sOr
static JPH_INLINE DVec3 sOr(DVec3Arg inV1, DVec3Arg inV2)
Logical or (component wise)
Definition DVec3.inl:340
DVec3::sZero
static JPH_INLINE DVec3 sZero()
Vector with all zeros.
Definition DVec3.inl:120
DVec3::TestAllTrue
JPH_INLINE bool TestAllTrue() const
Test if all components are true (true is when highest bit of component is set)
Definition DVec3.inl:404
DVec3::Length
JPH_INLINE double Length() const
Length of vector.
Definition DVec3.inl:796
DVec3::operator-
JPH_INLINE DVec3 operator-() const
Negate.
Definition DVec3.inl:609
DVec3::IsNaN
JPH_INLINE bool IsNaN() const
Test if vector contains NaN elements.
Definition DVec3.inl:811
DVec3::ToVec3RoundUp
JPH_INLINE Vec3 ToVec3RoundUp() const
Convert to float vector 3 rounding up.
Definition DVec3.inl:929
DVec3::cTrue
static const double cTrue
Representations of true and false for boolean operations.
Definition DVec3.h:274
DVec3::DVec3
DVec3()=default
Constructor.
DVec3::CheckW
JPH_INLINE void CheckW() const
Internal helper function that checks that W is equal to Z, so e.g. dividing by it should not generate...
Definition DVec3.inl:83
DVec3::LengthSq
JPH_INLINE double LengthSq() const
Squared length of vector.
Definition DVec3.inl:778
DVec3::Normalized
JPH_INLINE DVec3 Normalized() const
Normalize vector.
Definition DVec3.inl:801
DVec3::operator/
JPH_INLINE DVec3 operator/(double inV2) const
Divide vector by double.
Definition DVec3.inl:465
DVec3::Dot
JPH_INLINE double Dot(DVec3Arg inV2) const
Dot product.
Definition DVec3.inl:749
DVec3::sReplicate
static JPH_INLINE DVec3 sReplicate(double inV)
Replicate inV across all components.
Definition DVec3.inl:135
DVec3::sLessOrEqual
static JPH_INLINE DVec3 sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2)
Less than or equal (component wise)
Definition DVec3.inl:258
DVec3::PrepareRoundToInf
JPH_INLINE DVec3 PrepareRoundToInf() const
Prepare to convert to float vector 3 rounding towards positive/negative inf (returns DVec3 that can b...
Definition DVec3.inl:871
DVec3::operator+=
JPH_INLINE DVec3 & operator+=(Vec3Arg inV2)
Add two vectors (component wise)
Definition DVec3.inl:569
DVec3::sLoadDouble3Unsafe
static JPH_INLINE DVec3 sLoadDouble3Unsafe(const Double3 &inV)
Load 3 doubles from memory (reads 64 bits extra which it doesn't use)
Definition DVec3.inl:155
DVec3::operator/=
JPH_INLINE DVec3 & operator/=(double inV2)
Divide vector by double.
Definition DVec3.inl:521
DVec3::Cross
JPH_INLINE DVec3 Cross(DVec3Arg inV2) const
Cross product.
Definition DVec3.inl:733
DVec3::operator-=
JPH_INLINE DVec3 & operator-=(Vec3Arg inV2)
Subtract two vectors (component wise)
Definition DVec3.inl:658
DVec3::PrepareRoundToZero
JPH_INLINE DVec3 PrepareRoundToZero() const
Prepare to convert to float vector 3 rounding towards zero (returns DVec3 that can be converted to a ...
Definition DVec3.inl:848
DVec3::Reciprocal
JPH_INLINE DVec3 Reciprocal() const
Reciprocal vector (1 / value) for each of the components.
Definition DVec3.inl:728
DVec3::sEquals
static JPH_INLINE DVec3 sEquals(DVec3Arg inV1, DVec3Arg inV2)
Equals (component wise)
Definition DVec3.inl:228
DVec3::IsNearZero
JPH_INLINE bool IsNearZero(double inMaxDistSq=1.0e-24) const
Test if vector is near zero.
Definition DVec3.inl:419
DVec3::operator==
JPH_INLINE bool operator==(DVec3Arg inV2) const
Comparison.
Definition DVec3.inl:409
DVec3::cFalse
static const double cFalse
Definition DVec3.h:275
Double3
Class that holds 3 doubles. Used as a storage class. Convert to DVec3 for calculations.
Definition Double3.h:13
Double3::z
double z
Definition Double3.h:40
Double3::y
double y
Definition Double3.h:39
Double3::x
double x
Definition Double3.h:38
Vec3
Definition Vec3.h:17
Vec3::mValue
Type mValue
Definition Vec3.h:286
Vec3::mF32
float mF32[4]
Definition Vec3.h:287
Vec4
Definition Vec4.h:14