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#elif defined(JPH_USE_RVV)
25 const vfloat32m1_t src = __riscv_vle32_v_f32m1(inRHS.mF32, 3);
26 const vfloat64m2_t widened = __riscv_vfwcvt_f_f_v_f64m2(src, 3);
27 __riscv_vse64_v_f64m2(mF64, widened, 3);
28#else
29 mF64[0] = (double)inRHS.GetX();
30 mF64[1] = (double)inRHS.GetY();
31 mF64[2] = (double)inRHS.GetZ();
32 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
33 mF64[3] = mF64[2];
34 #endif
35#endif
36}
37
38DVec3::DVec3(Vec4Arg inRHS) :
39 DVec3(Vec3(inRHS))
40{
41}
42
43DVec3::DVec3(double inX, double inY, double inZ)
44{
45#if defined(JPH_USE_AVX)
46 mValue = _mm256_set_pd(inZ, inZ, inY, inX); // Assure Z and W are the same
47#elif defined(JPH_USE_SSE)
48 mValue.mLow = _mm_set_pd(inY, inX);
49 mValue.mHigh = _mm_set1_pd(inZ);
50#elif defined(JPH_USE_NEON)
51 mValue.val[0] = vcombine_f64(vcreate_f64(BitCast<uint64>(inX)), vcreate_f64(BitCast<uint64>(inY)));
52 mValue.val[1] = vdupq_n_f64(inZ);
53#elif defined(JPH_USE_RVV)
54 vfloat64m2_t v = __riscv_vfmv_v_f_f64m2(inZ, 4);
55 v = __riscv_vfslide1up_vf_f64m2(v, inY, 4);
56 v = __riscv_vfslide1up_vf_f64m2(v, inX, 4);
57 __riscv_vse64_v_f64m2(mF64, v, 4);
58#else
59 mF64[0] = inX;
60 mF64[1] = inY;
61 mF64[2] = inZ;
62 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
63 mF64[3] = mF64[2];
64 #endif
65#endif
66}
67
68DVec3::DVec3(const Double3 &inV)
69{
70#if defined(JPH_USE_AVX)
71 Type x = _mm256_castpd128_pd256(_mm_load_sd(&inV.x));
72 Type y = _mm256_castpd128_pd256(_mm_load_sd(&inV.y));
73 Type z = _mm256_broadcast_sd(&inV.z);
74 Type xy = _mm256_unpacklo_pd(x, y);
75 mValue = _mm256_blend_pd(xy, z, 0b1100); // Assure Z and W are the same
76#elif defined(JPH_USE_SSE)
77 mValue.mLow = _mm_loadu_pd(&inV.x);
78 mValue.mHigh = _mm_set1_pd(inV.z);
79#elif defined(JPH_USE_NEON)
80 mValue.val[0] = vld1q_f64(&inV.x);
81 mValue.val[1] = vdupq_n_f64(inV.z);
82#elif defined(JPH_USE_RVV)
83 vfloat64m2_t v = __riscv_vle64_v_f64m2(&inV.x, 3);
84 __riscv_vse64_v_f64m2(mF64, v, 3);
85#else
86 mF64[0] = inV.x;
87 mF64[1] = inV.y;
88 mF64[2] = inV.z;
89 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
90 mF64[3] = mF64[2];
91 #endif
92#endif
93}
94
95void DVec3::CheckW() const
96{
97#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
98 // Avoid asserts when both components are NaN
99 JPH_ASSERT(reinterpret_cast<const uint64 *>(mF64)[2] == reinterpret_cast<const uint64 *>(mF64)[3]);
100#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
101}
102
104DVec3::Type DVec3::sFixW(TypeArg inValue)
105{
106#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
107 #if defined(JPH_USE_AVX)
108 return _mm256_shuffle_pd(inValue, inValue, 2);
109 #elif defined(JPH_USE_SSE)
110 Type value;
111 value.mLow = inValue.mLow;
112 value.mHigh = _mm_shuffle_pd(inValue.mHigh, inValue.mHigh, 0);
113 return value;
114 #elif defined(JPH_USE_NEON)
115 Type value;
116 value.val[0] = inValue.val[0];
117 value.val[1] = vdupq_laneq_f64(inValue.val[1], 0);
118 return value;
119 #elif defined(JPH_USE_RVV)
120 Type value;
121 const vfloat64m2_t buffer = __riscv_vle64_v_f64m2(inValue.mData, 3);
122 __riscv_vse64_v_f64m2(value.mData, buffer, 3);
123 value.mData[3] = value.mData[2];
124 return value;
125 #else
126 Type value;
127 value.mData[0] = inValue.mData[0];
128 value.mData[1] = inValue.mData[1];
129 value.mData[2] = inValue.mData[2];
130 value.mData[3] = inValue.mData[2];
131 return value;
132 #endif
133#else
134 return inValue;
135#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
136}
137
138DVec3 DVec3::sZero()
139{
140#if defined(JPH_USE_AVX)
141 return _mm256_setzero_pd();
142#elif defined(JPH_USE_SSE)
143 __m128d zero = _mm_setzero_pd();
144 return DVec3({ zero, zero });
145#elif defined(JPH_USE_NEON)
146 float64x2_t zero = vdupq_n_f64(0.0);
147 return DVec3({ zero, zero });
148#elif defined(JPH_USE_RVV)
149 DVec3 vec;
150 const vfloat64m2_t v = __riscv_vfmv_v_f_f64m2(0.0, 3);
151 __riscv_vse64_v_f64m2(vec.mF64, v, 3);
152 return vec;
153#else
154 return DVec3(0, 0, 0);
155#endif
156}
157
158DVec3 DVec3::sReplicate(double inV)
159{
160#if defined(JPH_USE_AVX)
161 return _mm256_set1_pd(inV);
162#elif defined(JPH_USE_SSE)
163 __m128d value = _mm_set1_pd(inV);
164 return DVec3({ value, value });
165#elif defined(JPH_USE_NEON)
166 float64x2_t value = vdupq_n_f64(inV);
167 return DVec3({ value, value });
168#elif defined(JPH_USE_RVV)
169 DVec3 vec;
170 const vfloat64m2_t v = __riscv_vfmv_v_f_f64m2(inV, 3);
171 __riscv_vse64_v_f64m2(vec.mF64, v, 3);
172 return vec;
173#else
174 return DVec3(inV, inV, inV);
175#endif
176}
177
178DVec3 DVec3::sOne()
179{
180 return sReplicate(1.0);
181}
182
183DVec3 DVec3::sNaN()
184{
185 return sReplicate(numeric_limits<double>::quiet_NaN());
186}
187
188DVec3 DVec3::sLoadDouble3Unsafe(const Double3 &inV)
189{
190#if defined(JPH_USE_AVX)
191 Type v = _mm256_loadu_pd(&inV.x);
192#elif defined(JPH_USE_SSE)
193 Type v;
194 v.mLow = _mm_loadu_pd(&inV.x);
195 v.mHigh = _mm_set1_pd(inV.z);
196#elif defined(JPH_USE_NEON)
197 Type v = vld1q_f64_x2(&inV.x);
198#elif defined(JPH_USE_RVV)
199 Type v;
200 const vfloat64m2_t vec = __riscv_vle64_v_f64m2(&inV.x, 3);
201 __riscv_vse64_v_f64m2(v.mData, vec, 3);
202#else
203 Type v = { inV.x, inV.y, inV.z };
204#endif
205 return sFixW(v);
206}
207
208void DVec3::StoreDouble3(Double3 *outV) const
209{
210 outV->x = mF64[0];
211 outV->y = mF64[1];
212 outV->z = mF64[2];
213}
214
215DVec3::operator Vec3() const
216{
217#if defined(JPH_USE_AVX)
218 return _mm256_cvtpd_ps(mValue);
219#elif defined(JPH_USE_SSE)
220 __m128 low = _mm_cvtpd_ps(mValue.mLow);
221 __m128 high = _mm_cvtpd_ps(mValue.mHigh);
222 return _mm_shuffle_ps(low, high, _MM_SHUFFLE(1, 0, 1, 0));
223#elif defined(JPH_USE_NEON)
224 return vcvt_high_f32_f64(vcvtx_f32_f64(mValue.val[0]), mValue.val[1]);
225#elif defined(JPH_USE_RVV)
226 Vec3 v;
227 const vfloat64m2_t src = __riscv_vle64_v_f64m2(mF64, 3);
228 const vfloat32m1_t narrowed = __riscv_vfncvt_f_f_w_f32m1(src, 3);
229 __riscv_vse32_v_f32m1(v.mF32, narrowed, 3);
230 return v;
231#else
232 return Vec3((float)GetX(), (float)GetY(), (float)GetZ());
233#endif
234}
235
236DVec3 DVec3::sMin(DVec3Arg inV1, DVec3Arg inV2)
237{
238#if defined(JPH_USE_AVX)
239 return _mm256_min_pd(inV1.mValue, inV2.mValue);
240#elif defined(JPH_USE_SSE)
241 return DVec3({ _mm_min_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_min_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
242#elif defined(JPH_USE_NEON)
243 return DVec3({ vminq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vminq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) });
244#elif defined(JPH_USE_RVV)
245 DVec3 res;
246 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inV1.mF64, 3);
247 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
248 const vfloat64m2_t min = __riscv_vfmin_vv_f64m2(v1, v2, 3);
249 __riscv_vse64_v_f64m2(res.mF64, min, 3);
250 return res;
251#else
252 return DVec3(min(inV1.mF64[0], inV2.mF64[0]),
253 min(inV1.mF64[1], inV2.mF64[1]),
254 min(inV1.mF64[2], inV2.mF64[2]));
255#endif
256}
257
258DVec3 DVec3::sMax(DVec3Arg inV1, DVec3Arg inV2)
259{
260#if defined(JPH_USE_AVX)
261 return _mm256_max_pd(inV1.mValue, inV2.mValue);
262#elif defined(JPH_USE_SSE)
263 return DVec3({ _mm_max_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_max_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
264#elif defined(JPH_USE_NEON)
265 return DVec3({ vmaxq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vmaxq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) });
266#elif defined(JPH_USE_RVV)
267 DVec3 res;
268 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inV1.mF64, 3);
269 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
270 const vfloat64m2_t max = __riscv_vfmax_vv_f64m2(v1, v2, 3);
271 __riscv_vse64_v_f64m2(res.mF64, max, 3);
272 return res;
273#else
274 return DVec3(max(inV1.mF64[0], inV2.mF64[0]),
275 max(inV1.mF64[1], inV2.mF64[1]),
276 max(inV1.mF64[2], inV2.mF64[2]));
277#endif
278}
279
280DVec3 DVec3::sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax)
281{
282 return sMax(sMin(inV, inMax), inMin);
283}
284
285DVec3 DVec3::sEquals(DVec3Arg inV1, DVec3Arg inV2)
286{
287#if defined(JPH_USE_AVX)
288 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_EQ_OQ);
289#elif defined(JPH_USE_SSE)
290 return DVec3({ _mm_cmpeq_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpeq_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
291#elif defined(JPH_USE_NEON)
292 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])) });
293#elif defined(JPH_USE_RVV)
294 DVec3 res;
295 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inV1.mF64, 3);
296 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
297 const vbool32_t mask = __riscv_vmfeq_vv_f64m2_b32(v1, v2, 3);
298 const vfloat64m2_t zeros = __riscv_vfmv_v_f_f64m2(cFalse, 3);
299 const vfloat64m2_t merged = __riscv_vfmerge_vfm_f64m2(zeros, cTrue, mask, 3);
300 __riscv_vse64_v_f64m2(res.mF64, merged, 3);
301 return res;
302#else
303 return DVec3(inV1.mF64[0] == inV2.mF64[0]? cTrue : cFalse,
304 inV1.mF64[1] == inV2.mF64[1]? cTrue : cFalse,
305 inV1.mF64[2] == inV2.mF64[2]? cTrue : cFalse);
306#endif
307}
308
309DVec3 DVec3::sLess(DVec3Arg inV1, DVec3Arg inV2)
310{
311#if defined(JPH_USE_AVX)
312 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LT_OQ);
313#elif defined(JPH_USE_SSE)
314 return DVec3({ _mm_cmplt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmplt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
315#elif defined(JPH_USE_NEON)
316 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])) });
317#elif defined(JPH_USE_RVV)
318 DVec3 res;
319 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inV1.mF64, 3);
320 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
321 const vbool32_t mask = __riscv_vmflt_vv_f64m2_b32(v1, v2, 3);
322 const vfloat64m2_t zeros = __riscv_vfmv_v_f_f64m2(cFalse, 3);
323 const vfloat64m2_t merged = __riscv_vfmerge_vfm_f64m2(zeros, cTrue, mask, 3);
324 __riscv_vse64_v_f64m2(res.mF64, merged, 3);
325 return res;
326#else
327 return DVec3(inV1.mF64[0] < inV2.mF64[0]? cTrue : cFalse,
328 inV1.mF64[1] < inV2.mF64[1]? cTrue : cFalse,
329 inV1.mF64[2] < inV2.mF64[2]? cTrue : cFalse);
330#endif
331}
332
333DVec3 DVec3::sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2)
334{
335#if defined(JPH_USE_AVX)
336 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LE_OQ);
337#elif defined(JPH_USE_SSE)
338 return DVec3({ _mm_cmple_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmple_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
339#elif defined(JPH_USE_NEON)
340 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])) });
341#elif defined(JPH_USE_RVV)
342 DVec3 res;
343 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inV1.mF64, 3);
344 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
345 const vbool32_t mask = __riscv_vmfle_vv_f64m2_b32(v1, v2, 3);
346 const vfloat64m2_t zeros = __riscv_vfmv_v_f_f64m2(cFalse, 3);
347 const vfloat64m2_t merged = __riscv_vfmerge_vfm_f64m2(zeros, cTrue, mask, 3);
348 __riscv_vse64_v_f64m2(res.mF64, merged, 3);
349 return res;
350#else
351 return DVec3(inV1.mF64[0] <= inV2.mF64[0]? cTrue : cFalse,
352 inV1.mF64[1] <= inV2.mF64[1]? cTrue : cFalse,
353 inV1.mF64[2] <= inV2.mF64[2]? cTrue : cFalse);
354#endif
355}
356
357DVec3 DVec3::sGreater(DVec3Arg inV1, DVec3Arg inV2)
358{
359#if defined(JPH_USE_AVX)
360 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GT_OQ);
361#elif defined(JPH_USE_SSE)
362 return DVec3({ _mm_cmpgt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpgt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
363#elif defined(JPH_USE_NEON)
364 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])) });
365#elif defined(JPH_USE_RVV)
366 DVec3 res;
367 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inV1.mF64, 3);
368 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
369 const vbool32_t mask = __riscv_vmfgt_vv_f64m2_b32(v1, v2, 3);
370 const vfloat64m2_t zeros = __riscv_vfmv_v_f_f64m2(cFalse, 3);
371 const vfloat64m2_t merged = __riscv_vfmerge_vfm_f64m2(zeros, cTrue, mask, 3);
372 __riscv_vse64_v_f64m2(res.mF64, merged, 3);
373 return res;
374#else
375 return DVec3(inV1.mF64[0] > inV2.mF64[0]? cTrue : cFalse,
376 inV1.mF64[1] > inV2.mF64[1]? cTrue : cFalse,
377 inV1.mF64[2] > inV2.mF64[2]? cTrue : cFalse);
378#endif
379}
380
381DVec3 DVec3::sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2)
382{
383#if defined(JPH_USE_AVX)
384 return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GE_OQ);
385#elif defined(JPH_USE_SSE)
386 return DVec3({ _mm_cmpge_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpge_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
387#elif defined(JPH_USE_NEON)
388 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])) });
389#elif defined(JPH_USE_RVV)
390 DVec3 res;
391 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inV1.mF64, 3);
392 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
393 const vbool32_t mask = __riscv_vmfge_vv_f64m2_b32(v1, v2, 3);
394 const vfloat64m2_t zeros = __riscv_vfmv_v_f_f64m2(cFalse, 3);
395 const vfloat64m2_t merged = __riscv_vfmerge_vfm_f64m2(zeros, cTrue, mask, 3);
396 __riscv_vse64_v_f64m2(res.mF64, merged, 3);
397 return res;
398#else
399 return DVec3(inV1.mF64[0] >= inV2.mF64[0]? cTrue : cFalse,
400 inV1.mF64[1] >= inV2.mF64[1]? cTrue : cFalse,
401 inV1.mF64[2] >= inV2.mF64[2]? cTrue : cFalse);
402#endif
403}
404
405DVec3 DVec3::sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd)
406{
407#if defined(JPH_USE_AVX)
408 #ifdef JPH_USE_FMADD
409 return _mm256_fmadd_pd(inMul1.mValue, inMul2.mValue, inAdd.mValue);
410 #else
411 return _mm256_add_pd(_mm256_mul_pd(inMul1.mValue, inMul2.mValue), inAdd.mValue);
412 #endif
413#elif defined(JPH_USE_NEON)
414 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]) });
415#elif defined(JPH_USE_RVV)
416 DVec3 res;
417 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inMul1.mF64, 3);
418 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inMul2.mF64, 3);
419 const vfloat64m2_t rvv_add = __riscv_vle64_v_f64m2(inAdd.mF64, 3);
420 const vfloat64m2_t fmadd = __riscv_vfmacc_vv_f64m2(rvv_add, v1, v2, 3);
421 __riscv_vse64_v_f64m2(res.mF64, fmadd, 3);
422 return res;
423#else
424 return inMul1 * inMul2 + inAdd;
425#endif
426}
427
428DVec3 DVec3::sSelect(DVec3Arg inNotSet, DVec3Arg inSet, DVec3Arg inControl)
429{
430#if defined(JPH_USE_AVX)
431 return _mm256_blendv_pd(inNotSet.mValue, inSet.mValue, inControl.mValue);
432#elif defined(JPH_USE_SSE4_1)
433 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) };
434 return sFixW(v);
435#elif defined(JPH_USE_NEON)
436 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]),
437 vbslq_f64(vreinterpretq_u64_s64(vshrq_n_s64(vreinterpretq_s64_f64(inControl.mValue.val[1]), 63)), inSet.mValue.val[1], inNotSet.mValue.val[1]) };
438 return sFixW(v);
439#elif defined(JPH_USE_RVV)
440 DVec3 masked;
441 const vfloat64m2_t control_double = __riscv_vle64_v_f64m2(inControl.mF64, 3);
442 const vfloat64m2_t not_set = __riscv_vle64_v_f64m2(inNotSet.mF64, 3);
443 const vfloat64m2_t set = __riscv_vle64_v_f64m2(inSet.mF64, 3);
444 const vuint64m2_t control = __riscv_vreinterpret_v_f64m2_u64m2(control_double);
445
446 // Generate RVV bool mask from UVec4
447 const uint64 sign_bit_mask = 0x8000000000000000u;
448 const vuint64m2_t r = __riscv_vand_vx_u64m2(control, sign_bit_mask, 3);
449 const vbool32_t rvv_mask = __riscv_vmsne_vx_u64m2_b32(r, 0x0, 3);
450 const vfloat64m2_t merged = __riscv_vmerge_vvm_f64m2(not_set, set, rvv_mask, 3);
451 __riscv_vse64_v_f64m2(masked.mF64, merged, 3);
452 return masked;
453#else
454 DVec3 result;
455 for (int i = 0; i < 3; i++)
456 result.mF64[i] = (BitCast<uint64>(inControl.mF64[i]) & (uint64(1) << 63))? inSet.mF64[i] : inNotSet.mF64[i];
457#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
458 result.mF64[3] = result.mF64[2];
459#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
460 return result;
461#endif
462}
463
464DVec3 DVec3::sOr(DVec3Arg inV1, DVec3Arg inV2)
465{
466#if defined(JPH_USE_AVX)
467 return _mm256_or_pd(inV1.mValue, inV2.mValue);
468#elif defined(JPH_USE_SSE)
469 return DVec3({ _mm_or_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_or_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
470#elif defined(JPH_USE_NEON)
471 return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))),
472 vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) });
473#elif defined(JPH_USE_RVV)
474 DVec3 or_result;
475 const vuint64m2_t v1 = __riscv_vle64_v_u64m2(reinterpret_cast<const uint64 *>(inV1.mF64), 3);
476 const vuint64m2_t v2 = __riscv_vle64_v_u64m2(reinterpret_cast<const uint64 *>(inV2.mF64), 3);
477 const vuint64m2_t res = __riscv_vor_vv_u64m2(v1, v2, 3);
478 __riscv_vse64_v_u64m2(reinterpret_cast<uint64 *>(or_result.mF64), res, 3);
479 return or_result;
480#else
481 return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) | BitCast<uint64>(inV2.mF64[0])),
482 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) | BitCast<uint64>(inV2.mF64[1])),
483 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) | BitCast<uint64>(inV2.mF64[2])));
484#endif
485}
486
487DVec3 DVec3::sXor(DVec3Arg inV1, DVec3Arg inV2)
488{
489#if defined(JPH_USE_AVX)
490 return _mm256_xor_pd(inV1.mValue, inV2.mValue);
491#elif defined(JPH_USE_SSE)
492 return DVec3({ _mm_xor_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_xor_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
493#elif defined(JPH_USE_NEON)
494 return DVec3({ vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))),
495 vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) });
496#elif defined(JPH_USE_RVV)
497 DVec3 xor_result;
498 const vuint64m2_t v1 = __riscv_vle64_v_u64m2(reinterpret_cast<const uint64 *>(inV1.mF64), 3);
499 const vuint64m2_t v2 = __riscv_vle64_v_u64m2(reinterpret_cast<const uint64 *>(inV2.mF64), 3);
500 const vuint64m2_t res = __riscv_vxor_vv_u64m2(v1, v2, 3);
501 __riscv_vse64_v_u64m2(reinterpret_cast<uint64 *>(xor_result.mF64), res, 3);
502 return xor_result;
503#else
504 return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) ^ BitCast<uint64>(inV2.mF64[0])),
505 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) ^ BitCast<uint64>(inV2.mF64[1])),
506 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) ^ BitCast<uint64>(inV2.mF64[2])));
507#endif
508}
509
510DVec3 DVec3::sAnd(DVec3Arg inV1, DVec3Arg inV2)
511{
512#if defined(JPH_USE_AVX)
513 return _mm256_and_pd(inV1.mValue, inV2.mValue);
514#elif defined(JPH_USE_SSE)
515 return DVec3({ _mm_and_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_and_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) });
516#elif defined(JPH_USE_NEON)
517 return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[0]), vreinterpretq_u64_f64(inV2.mValue.val[0]))),
518 vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(inV1.mValue.val[1]), vreinterpretq_u64_f64(inV2.mValue.val[1]))) });
519#elif defined(JPH_USE_RVV)
520 DVec3 and_result;
521 const vuint64m2_t v1 = __riscv_vle64_v_u64m2(reinterpret_cast<const uint64 *>(inV1.mF64), 3);
522 const vuint64m2_t v2 = __riscv_vle64_v_u64m2(reinterpret_cast<const uint64 *>(inV2.mF64), 3);
523 const vuint64m2_t res = __riscv_vand_vv_u64m2(v1, v2, 3);
524 __riscv_vse64_v_u64m2(reinterpret_cast<uint64 *>(and_result.mF64), res, 3);
525 return and_result;
526#else
527 return DVec3(BitCast<double>(BitCast<uint64>(inV1.mF64[0]) & BitCast<uint64>(inV2.mF64[0])),
528 BitCast<double>(BitCast<uint64>(inV1.mF64[1]) & BitCast<uint64>(inV2.mF64[1])),
529 BitCast<double>(BitCast<uint64>(inV1.mF64[2]) & BitCast<uint64>(inV2.mF64[2])));
530#endif
531}
532
533int DVec3::GetTrues() const
534{
535#if defined(JPH_USE_AVX)
536 return _mm256_movemask_pd(mValue) & 0x7;
537#elif defined(JPH_USE_SSE)
538 return (_mm_movemask_pd(mValue.mLow) + (_mm_movemask_pd(mValue.mHigh) << 2)) & 0x7;
539#else
540 return int((BitCast<uint64>(mF64[0]) >> 63) | ((BitCast<uint64>(mF64[1]) >> 63) << 1) | ((BitCast<uint64>(mF64[2]) >> 63) << 2));
541#endif
542}
543
544bool DVec3::TestAnyTrue() const
545{
546 return GetTrues() != 0;
547}
548
549bool DVec3::TestAllTrue() const
550{
551 return GetTrues() == 0x7;
552}
553
554bool DVec3::operator == (DVec3Arg inV2) const
555{
556 return sEquals(*this, inV2).TestAllTrue();
557}
558
559bool DVec3::IsClose(DVec3Arg inV2, double inMaxDistSq) const
560{
561 return (inV2 - *this).LengthSq() <= inMaxDistSq;
562}
563
564bool DVec3::IsNearZero(double inMaxDistSq) const
565{
566 return LengthSq() <= inMaxDistSq;
567}
568
569DVec3 DVec3::operator * (DVec3Arg inV2) const
570{
571#if defined(JPH_USE_AVX)
572 return _mm256_mul_pd(mValue, inV2.mValue);
573#elif defined(JPH_USE_SSE)
574 return DVec3({ _mm_mul_pd(mValue.mLow, inV2.mValue.mLow), _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh) });
575#elif defined(JPH_USE_NEON)
576 return DVec3({ vmulq_f64(mValue.val[0], inV2.mValue.val[0]), vmulq_f64(mValue.val[1], inV2.mValue.val[1]) });
577#elif defined(JPH_USE_RVV)
578 DVec3 res;
579 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
580 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
581 const vfloat64m2_t mul = __riscv_vfmul_vv_f64m2(v1, v2, 3);
582 __riscv_vse64_v_f64m2(res.mF64, mul, 3);
583 return res;
584#else
585 return DVec3(mF64[0] * inV2.mF64[0], mF64[1] * inV2.mF64[1], mF64[2] * inV2.mF64[2]);
586#endif
587}
588
589DVec3 DVec3::operator * (double inV2) const
590{
591#if defined(JPH_USE_AVX)
592 return _mm256_mul_pd(mValue, _mm256_set1_pd(inV2));
593#elif defined(JPH_USE_SSE)
594 __m128d v = _mm_set1_pd(inV2);
595 return DVec3({ _mm_mul_pd(mValue.mLow, v), _mm_mul_pd(mValue.mHigh, v) });
596#elif defined(JPH_USE_NEON)
597 return DVec3({ vmulq_n_f64(mValue.val[0], inV2), vmulq_n_f64(mValue.val[1], inV2) });
598#elif defined(JPH_USE_RVV)
599 DVec3 res;
600 const vfloat64m2_t src = __riscv_vle64_v_f64m2(mF64, 3);
601 const vfloat64m2_t mul = __riscv_vfmul_vf_f64m2(src, inV2, 3);
602 __riscv_vse64_v_f64m2(res.mF64, mul, 3);
603 return res;
604#else
605 return DVec3(mF64[0] * inV2, mF64[1] * inV2, mF64[2] * inV2);
606#endif
607}
608
609DVec3 operator * (double inV1, DVec3Arg inV2)
610{
611#if defined(JPH_USE_AVX)
612 return _mm256_mul_pd(_mm256_set1_pd(inV1), inV2.mValue);
613#elif defined(JPH_USE_SSE)
614 __m128d v = _mm_set1_pd(inV1);
615 return DVec3({ _mm_mul_pd(v, inV2.mValue.mLow), _mm_mul_pd(v, inV2.mValue.mHigh) });
616#elif defined(JPH_USE_NEON)
617 return DVec3({ vmulq_n_f64(inV2.mValue.val[0], inV1), vmulq_n_f64(inV2.mValue.val[1], inV1) });
618#elif defined(JPH_USE_RVV)
619 DVec3 res;
620 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
621 const vfloat64m2_t mul = __riscv_vfmul_vf_f64m2(v1, inV1, 3);
622 __riscv_vse64_v_f64m2(res.mF64, mul, 3);
623 return res;
624#else
625 return DVec3(inV1 * inV2.mF64[0], inV1 * inV2.mF64[1], inV1 * inV2.mF64[2]);
626#endif
627}
628
629DVec3 DVec3::operator / (double inV2) const
630{
631#if defined(JPH_USE_AVX)
632 return _mm256_div_pd(mValue, _mm256_set1_pd(inV2));
633#elif defined(JPH_USE_SSE)
634 __m128d v = _mm_set1_pd(inV2);
635 return DVec3({ _mm_div_pd(mValue.mLow, v), _mm_div_pd(mValue.mHigh, v) });
636#elif defined(JPH_USE_NEON)
637 float64x2_t v = vdupq_n_f64(inV2);
638 return DVec3({ vdivq_f64(mValue.val[0], v), vdivq_f64(mValue.val[1], v) });
639#elif defined(JPH_USE_RVV)
640 DVec3 res;
641 const vfloat64m2_t src = __riscv_vle64_v_f64m2(mF64, 3);
642 const vfloat64m2_t div = __riscv_vfdiv_vf_f64m2(src, inV2, 3);
643 __riscv_vse64_v_f64m2(res.mF64, div, 3);
644 return res;
645#else
646 return DVec3(mF64[0] / inV2, mF64[1] / inV2, mF64[2] / inV2);
647#endif
648}
649
650DVec3 &DVec3::operator *= (double inV2)
651{
652#if defined(JPH_USE_AVX)
653 mValue = _mm256_mul_pd(mValue, _mm256_set1_pd(inV2));
654#elif defined(JPH_USE_SSE)
655 __m128d v = _mm_set1_pd(inV2);
656 mValue.mLow = _mm_mul_pd(mValue.mLow, v);
657 mValue.mHigh = _mm_mul_pd(mValue.mHigh, v);
658#elif defined(JPH_USE_NEON)
659 mValue.val[0] = vmulq_n_f64(mValue.val[0], inV2);
660 mValue.val[1] = vmulq_n_f64(mValue.val[1], inV2);
661#elif defined(JPH_USE_RVV)
662 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
663 const vfloat64m2_t res = __riscv_vfmul_vf_f64m2(v1, inV2, 3);
664 __riscv_vse64_v_f64m2(mF64, res, 3);
665#else
666 for (int i = 0; i < 3; ++i)
667 mF64[i] *= inV2;
668 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
669 mF64[3] = mF64[2];
670 #endif
671#endif
672 return *this;
673}
674
675DVec3 &DVec3::operator *= (DVec3Arg inV2)
676{
677#if defined(JPH_USE_AVX)
678 mValue = _mm256_mul_pd(mValue, inV2.mValue);
679#elif defined(JPH_USE_SSE)
680 mValue.mLow = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow);
681 mValue.mHigh = _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh);
682#elif defined(JPH_USE_NEON)
683 mValue.val[0] = vmulq_f64(mValue.val[0], inV2.mValue.val[0]);
684 mValue.val[1] = vmulq_f64(mValue.val[1], inV2.mValue.val[1]);
685#elif defined(JPH_USE_RVV)
686 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
687 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
688 const vfloat64m2_t rvv_res = __riscv_vfmul_vv_f64m2(v1, v2, 3);
689 __riscv_vse64_v_f64m2(mF64, rvv_res, 3);
690#else
691 for (int i = 0; i < 3; ++i)
692 mF64[i] *= inV2.mF64[i];
693 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
694 mF64[3] = mF64[2];
695 #endif
696#endif
697 return *this;
698}
699
700DVec3 &DVec3::operator /= (double inV2)
701{
702#if defined(JPH_USE_AVX)
703 mValue = _mm256_div_pd(mValue, _mm256_set1_pd(inV2));
704#elif defined(JPH_USE_SSE)
705 __m128d v = _mm_set1_pd(inV2);
706 mValue.mLow = _mm_div_pd(mValue.mLow, v);
707 mValue.mHigh = _mm_div_pd(mValue.mHigh, v);
708#elif defined(JPH_USE_NEON)
709 float64x2_t v = vdupq_n_f64(inV2);
710 mValue.val[0] = vdivq_f64(mValue.val[0], v);
711 mValue.val[1] = vdivq_f64(mValue.val[1], v);
712#elif defined(JPH_USE_RVV)
713 const vfloat64m2_t v = __riscv_vle64_v_f64m2(mF64, 3);
714 const vfloat64m2_t res = __riscv_vfdiv_vf_f64m2(v, inV2, 3);
715 __riscv_vse64_v_f64m2(mF64, res, 3);
716#else
717 for (int i = 0; i < 3; ++i)
718 mF64[i] /= inV2;
719 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
720 mF64[3] = mF64[2];
721 #endif
722#endif
723 return *this;
724}
725
726DVec3 DVec3::operator + (Vec3Arg inV2) const
727{
728#if defined(JPH_USE_AVX)
729 return _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
730#elif defined(JPH_USE_SSE)
731 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)))) });
732#elif defined(JPH_USE_NEON)
733 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)) });
734#elif defined(JPH_USE_RVV)
735 DVec3 res;
736 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
737 const vfloat32m1_t v2_f32 = __riscv_vle32_v_f32m1(inV2.mF32, 3);
738 const vfloat64m2_t v2 = __riscv_vfwcvt_f_f_v_f64m2(v2_f32, 3);
739 const vfloat64m2_t rvv_add = __riscv_vfadd_vv_f64m2(v1, v2, 3);
740 __riscv_vse64_v_f64m2(res.mF64, rvv_add, 3);
741 return res;
742#else
743 return DVec3(mF64[0] + inV2.mF32[0], mF64[1] + inV2.mF32[1], mF64[2] + inV2.mF32[2]);
744#endif
745}
746
747DVec3 DVec3::operator + (DVec3Arg inV2) const
748{
749#if defined(JPH_USE_AVX)
750 return _mm256_add_pd(mValue, inV2.mValue);
751#elif defined(JPH_USE_SSE)
752 return DVec3({ _mm_add_pd(mValue.mLow, inV2.mValue.mLow), _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh) });
753#elif defined(JPH_USE_NEON)
754 return DVec3({ vaddq_f64(mValue.val[0], inV2.mValue.val[0]), vaddq_f64(mValue.val[1], inV2.mValue.val[1]) });
755#elif defined(JPH_USE_RVV)
756 DVec3 res;
757 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
758 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
759 const vfloat64m2_t rvv_add = __riscv_vfadd_vv_f64m2(v1, v2, 3);
760 __riscv_vse64_v_f64m2(res.mF64, rvv_add, 3);
761 return res;
762#else
763 return DVec3(mF64[0] + inV2.mF64[0], mF64[1] + inV2.mF64[1], mF64[2] + inV2.mF64[2]);
764#endif
765}
766
767DVec3 &DVec3::operator += (Vec3Arg inV2)
768{
769#if defined(JPH_USE_AVX)
770 mValue = _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
771#elif defined(JPH_USE_SSE)
772 mValue.mLow = _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue));
773 mValue.mHigh = _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
774#elif defined(JPH_USE_NEON)
775 mValue.val[0] = vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue)));
776 mValue.val[1] = vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue));
777#elif defined(JPH_USE_RVV)
778 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
779 const vfloat32m1_t v2_f32 = __riscv_vle32_v_f32m1(inV2.mF32, 3);
780 const vfloat64m2_t v2 = __riscv_vfwcvt_f_f_v_f64m2(v2_f32, 3);
781 const vfloat64m2_t rvv_add = __riscv_vfadd_vv_f64m2(v1, v2, 3);
782 __riscv_vse64_v_f64m2(mF64, rvv_add, 3);
783#else
784 for (int i = 0; i < 3; ++i)
785 mF64[i] += inV2.mF32[i];
786 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
787 mF64[3] = mF64[2];
788 #endif
789#endif
790 return *this;
791}
792
793DVec3 &DVec3::operator += (DVec3Arg inV2)
794{
795#if defined(JPH_USE_AVX)
796 mValue = _mm256_add_pd(mValue, inV2.mValue);
797#elif defined(JPH_USE_SSE)
798 mValue.mLow = _mm_add_pd(mValue.mLow, inV2.mValue.mLow);
799 mValue.mHigh = _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh);
800#elif defined(JPH_USE_NEON)
801 mValue.val[0] = vaddq_f64(mValue.val[0], inV2.mValue.val[0]);
802 mValue.val[1] = vaddq_f64(mValue.val[1], inV2.mValue.val[1]);
803#elif defined(JPH_USE_RVV)
804 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
805 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
806 const vfloat64m2_t rvv_add = __riscv_vfadd_vv_f64m2(v1, v2, 3);
807 __riscv_vse64_v_f64m2(mF64, rvv_add, 3);
808#else
809 for (int i = 0; i < 3; ++i)
810 mF64[i] += inV2.mF64[i];
811 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
812 mF64[3] = mF64[2];
813 #endif
814#endif
815 return *this;
816}
817
818DVec3 DVec3::operator - () const
819{
820#if defined(JPH_USE_AVX)
821 return _mm256_sub_pd(_mm256_setzero_pd(), mValue);
822#elif defined(JPH_USE_SSE)
823 __m128d zero = _mm_setzero_pd();
824 return DVec3({ _mm_sub_pd(zero, mValue.mLow), _mm_sub_pd(zero, mValue.mHigh) });
825#elif defined(JPH_USE_NEON)
826 #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
827 float64x2_t zero = vdupq_n_f64(0);
828 return DVec3({ vsubq_f64(zero, mValue.val[0]), vsubq_f64(zero, mValue.val[1]) });
829 #else
830 return DVec3({ vnegq_f64(mValue.val[0]), vnegq_f64(mValue.val[1]) });
831 #endif
832#elif defined(JPH_USE_RVV)
833 #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
834 DVec3 res;
835 const vfloat64m2_t rvv_zero = __riscv_vfmv_v_f_f64m2(0.0, 3);
836 const vfloat64m2_t v = __riscv_vle64_v_f64m2(mF64, 3);
837 const vfloat64m2_t rvv_neg = __riscv_vfsub_vv_f64m2(rvv_zero, v, 3);
838 __riscv_vse64_v_f64m2(res.mF64, rvv_neg, 3);
839 return res;
840 #else
841 DVec3 res;
842 const vfloat64m2_t v = __riscv_vle64_v_f64m2(mF64, 3);
843 const vfloat64m2_t rvv_neg = __riscv_vfsgnjn_vv_f64m2(v, v, 3);
844 __riscv_vse64_v_f64m2(res.mF64, rvv_neg, 3);
845 return res;
846 #endif
847#else
848 #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC
849 return DVec3(0.0 - mF64[0], 0.0 - mF64[1], 0.0 - mF64[2]);
850 #else
851 return DVec3(-mF64[0], -mF64[1], -mF64[2]);
852 #endif
853#endif
854}
855
856DVec3 DVec3::operator - (Vec3Arg inV2) const
857{
858#if defined(JPH_USE_AVX)
859 return _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
860#elif defined(JPH_USE_SSE)
861 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)))) });
862#elif defined(JPH_USE_NEON)
863 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)) });
864#elif defined(JPH_USE_RVV)
865 DVec3 res;
866 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
867 const vfloat32m1_t v2_f32 = __riscv_vle32_v_f32m1(inV2.mF32, 3);
868 const vfloat64m2_t v2 = __riscv_vfwcvt_f_f_v_f64m2(v2_f32, 3);
869 const vfloat64m2_t rvv_sub = __riscv_vfsub_vv_f64m2(v1, v2, 3);
870 __riscv_vse64_v_f64m2(res.mF64, rvv_sub, 3);
871 return res;
872#else
873 return DVec3(mF64[0] - inV2.mF32[0], mF64[1] - inV2.mF32[1], mF64[2] - inV2.mF32[2]);
874#endif
875}
876
877DVec3 DVec3::operator - (DVec3Arg inV2) const
878{
879#if defined(JPH_USE_AVX)
880 return _mm256_sub_pd(mValue, inV2.mValue);
881#elif defined(JPH_USE_SSE)
882 return DVec3({ _mm_sub_pd(mValue.mLow, inV2.mValue.mLow), _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh) });
883#elif defined(JPH_USE_NEON)
884 return DVec3({ vsubq_f64(mValue.val[0], inV2.mValue.val[0]), vsubq_f64(mValue.val[1], inV2.mValue.val[1]) });
885#elif defined(JPH_USE_RVV)
886 DVec3 res;
887 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
888 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
889 const vfloat64m2_t rvv_sub = __riscv_vfsub_vv_f64m2(v1, v2, 3);
890 __riscv_vse64_v_f64m2(res.mF64, rvv_sub, 3);
891 return res;
892#else
893 return DVec3(mF64[0] - inV2.mF64[0], mF64[1] - inV2.mF64[1], mF64[2] - inV2.mF64[2]);
894#endif
895}
896
897DVec3 &DVec3::operator -= (Vec3Arg inV2)
898{
899#if defined(JPH_USE_AVX)
900 mValue = _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue));
901#elif defined(JPH_USE_SSE)
902 mValue.mLow = _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue));
903 mValue.mHigh = _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2))));
904#elif defined(JPH_USE_NEON)
905 mValue.val[0] = vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue)));
906 mValue.val[1] = vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue));
907#elif defined(JPH_USE_RVV)
908 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
909 const vfloat32m1_t v2_f32 = __riscv_vle32_v_f32m1(inV2.mF32, 3);
910 const vfloat64m2_t v2 = __riscv_vfwcvt_f_f_v_f64m2(v2_f32, 3);
911 const vfloat64m2_t rvv_sub = __riscv_vfsub_vv_f64m2(v1, v2, 3);
912 __riscv_vse64_v_f64m2(mF64, rvv_sub, 3);
913#else
914 for (int i = 0; i < 3; ++i)
915 mF64[i] -= inV2.mF32[i];
916 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
917 mF64[3] = mF64[2];
918 #endif
919#endif
920 return *this;
921}
922
923DVec3 &DVec3::operator -= (DVec3Arg inV2)
924{
925#if defined(JPH_USE_AVX)
926 mValue = _mm256_sub_pd(mValue, inV2.mValue);
927#elif defined(JPH_USE_SSE)
928 mValue.mLow = _mm_sub_pd(mValue.mLow, inV2.mValue.mLow);
929 mValue.mHigh = _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh);
930#elif defined(JPH_USE_NEON)
931 mValue.val[0] = vsubq_f64(mValue.val[0], inV2.mValue.val[0]);
932 mValue.val[1] = vsubq_f64(mValue.val[1], inV2.mValue.val[1]);
933#elif defined(JPH_USE_RVV)
934 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
935 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
936 const vfloat64m2_t rvv_sub = __riscv_vfsub_vv_f64m2(v1, v2, 3);
937 __riscv_vse64_v_f64m2(mF64, rvv_sub, 3);
938#else
939 for (int i = 0; i < 3; ++i)
940 mF64[i] -= inV2.mF64[i];
941 #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED
942 mF64[3] = mF64[2];
943 #endif
944#endif
945 return *this;
946}
947
948DVec3 DVec3::operator / (DVec3Arg inV2) const
949{
950 inV2.CheckW();
951#if defined(JPH_USE_AVX)
952 return _mm256_div_pd(mValue, inV2.mValue);
953#elif defined(JPH_USE_SSE)
954 return DVec3({ _mm_div_pd(mValue.mLow, inV2.mValue.mLow), _mm_div_pd(mValue.mHigh, inV2.mValue.mHigh) });
955#elif defined(JPH_USE_NEON)
956 return DVec3({ vdivq_f64(mValue.val[0], inV2.mValue.val[0]), vdivq_f64(mValue.val[1], inV2.mValue.val[1]) });
957#elif defined(JPH_USE_RVV)
958 DVec3 res;
959 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
960 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
961 const vfloat64m2_t rvv_div = __riscv_vfdiv_vv_f64m2(v1, v2, 3);
962 __riscv_vse64_v_f64m2(res.mF64, rvv_div, 3);
963 return res;
964#else
965 return DVec3(mF64[0] / inV2.mF64[0], mF64[1] / inV2.mF64[1], mF64[2] / inV2.mF64[2]);
966#endif
967}
968
969DVec3 DVec3::Abs() const
970{
971#if defined(JPH_USE_AVX512)
972 return _mm256_range_pd(mValue, mValue, 0b1000);
973#elif defined(JPH_USE_AVX)
974 return _mm256_max_pd(_mm256_sub_pd(_mm256_setzero_pd(), mValue), mValue);
975#elif defined(JPH_USE_SSE)
976 __m128d zero = _mm_setzero_pd();
977 return DVec3({ _mm_max_pd(_mm_sub_pd(zero, mValue.mLow), mValue.mLow), _mm_max_pd(_mm_sub_pd(zero, mValue.mHigh), mValue.mHigh) });
978#elif defined(JPH_USE_NEON)
979 return DVec3({ vabsq_f64(mValue.val[0]), vabsq_f64(mValue.val[1]) });
980#elif defined(JPH_USE_RVV)
981 DVec3 res;
982 const vfloat64m2_t v = __riscv_vle64_v_f64m2(mF64, 3);
983 const vfloat64m2_t rvv_abs = __riscv_vfsgnj_vf_f64m2(v, 1.0, 3);
984 __riscv_vse64_v_f64m2(res.mF64, rvv_abs, 3);
985 return res;
986#else
987 return DVec3(abs(mF64[0]), abs(mF64[1]), abs(mF64[2]));
988#endif
989}
990
991DVec3 DVec3::Reciprocal() const
992{
993 return sOne() / mValue;
994}
995
996DVec3 DVec3::Cross(DVec3Arg inV2) const
997{
998#if defined(JPH_USE_AVX2)
999 __m256d t1 = _mm256_permute4x64_pd(inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
1000 t1 = _mm256_mul_pd(t1, mValue);
1001 __m256d t2 = _mm256_permute4x64_pd(mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
1002 t2 = _mm256_mul_pd(t2, inV2.mValue);
1003 __m256d t3 = _mm256_sub_pd(t1, t2);
1004 return _mm256_permute4x64_pd(t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same
1005#elif defined(JPH_USE_RVV)
1006 const uint64 indices[3] = { 1, 2, 0 };
1007 const vuint64m2_t gather_indices = __riscv_vle64_v_u64m2(indices, 3);
1008 const vfloat64m2_t v0 = __riscv_vle64_v_f64m2(mF64, 3);
1009 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
1010 vfloat64m2_t t0 = __riscv_vrgather_vv_f64m2(v1, gather_indices, 3);
1011 t0 = __riscv_vfmul_vv_f64m2(t0, v0, 3);
1012 vfloat64m2_t t1 = __riscv_vrgather_vv_f64m2(v0, gather_indices, 3);
1013 t1 = __riscv_vfmul_vv_f64m2(t1, v1, 3);
1014 const vfloat64m2_t sub = __riscv_vfsub_vv_f64m2(t0, t1, 3);
1015 const vfloat64m2_t cross = __riscv_vrgather_vv_f64m2(sub, gather_indices, 3);
1016
1017 DVec3 cross_result;
1018 __riscv_vse64_v_f64m2(cross_result.mF64, cross, 3);
1019 return cross_result;
1020#else
1021 return DVec3(mF64[1] * inV2.mF64[2] - mF64[2] * inV2.mF64[1],
1022 mF64[2] * inV2.mF64[0] - mF64[0] * inV2.mF64[2],
1023 mF64[0] * inV2.mF64[1] - mF64[1] * inV2.mF64[0]);
1024#endif
1025}
1026
1027double DVec3::Dot(DVec3Arg inV2) const
1028{
1029#if defined(JPH_USE_AVX)
1030 __m256d mul = _mm256_mul_pd(mValue, inV2.mValue);
1031 __m128d xy = _mm256_castpd256_pd128(mul);
1032 __m128d yx = _mm_shuffle_pd(xy, xy, 1);
1033 __m128d sum = _mm_add_pd(xy, yx);
1034 __m128d zw = _mm256_extractf128_pd(mul, 1);
1035 sum = _mm_add_pd(sum, zw);
1036 return _mm_cvtsd_f64(sum);
1037#elif defined(JPH_USE_SSE)
1038 __m128d xy = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow);
1039 __m128d yx = _mm_shuffle_pd(xy, xy, 1);
1040 __m128d sum = _mm_add_pd(xy, yx);
1041 __m128d z = _mm_mul_sd(mValue.mHigh, inV2.mValue.mHigh);
1042 sum = _mm_add_pd(sum, z);
1043 return _mm_cvtsd_f64(sum);
1044#elif defined(JPH_USE_NEON)
1045 float64x2_t mul_low = vmulq_f64(mValue.val[0], inV2.mValue.val[0]);
1046 float64x2_t mul_high = vmulq_f64(mValue.val[1], inV2.mValue.val[1]);
1047 return vaddvq_f64(mul_low) + vgetq_lane_f64(mul_high, 0);
1048#elif defined(JPH_USE_RVV)
1049 const vfloat64m1_t zeros = __riscv_vfmv_v_f_f64m1(0.0, 3);
1050 const vfloat64m2_t v1 = __riscv_vle64_v_f64m2(mF64, 3);
1051 const vfloat64m2_t v2 = __riscv_vle64_v_f64m2(inV2.mF64, 3);
1052 const vfloat64m2_t mul = __riscv_vfmul_vv_f64m2(v1, v2, 3);
1053 const vfloat64m1_t sum = __riscv_vfredosum_vs_f64m2_f64m1(mul, zeros, 3);
1054 return __riscv_vfmv_f_s_f64m1_f64(sum);
1055#else
1056 double dot = 0.0;
1057 for (int i = 0; i < 3; i++)
1058 dot += mF64[i] * inV2.mF64[i];
1059 return dot;
1060#endif
1061}
1062
1063double DVec3::LengthSq() const
1064{
1065 return Dot(*this);
1066}
1067
1068DVec3 DVec3::Sqrt() const
1069{
1070#if defined(JPH_USE_AVX)
1071 return _mm256_sqrt_pd(mValue);
1072#elif defined(JPH_USE_SSE)
1073 return DVec3({ _mm_sqrt_pd(mValue.mLow), _mm_sqrt_pd(mValue.mHigh) });
1074#elif defined(JPH_USE_NEON)
1075 return DVec3({ vsqrtq_f64(mValue.val[0]), vsqrtq_f64(mValue.val[1]) });
1076#elif defined(JPH_USE_RVV)
1077 DVec3 res;
1078 const vfloat64m2_t v = __riscv_vle64_v_f64m2(mF64, 3);
1079 const vfloat64m2_t rvv_sqrt = __riscv_vfsqrt_v_f64m2(v, 3);
1080 __riscv_vse64_v_f64m2(res.mF64, rvv_sqrt, 3);
1081 return res;
1082#else
1083 return DVec3(sqrt(mF64[0]), sqrt(mF64[1]), sqrt(mF64[2]));
1084#endif
1085}
1086
1087double DVec3::Length() const
1088{
1089 return sqrt(Dot(*this));
1090}
1091
1092DVec3 DVec3::Normalized() const
1093{
1094 return *this / Length();
1095}
1096
1097bool DVec3::IsNormalized(double inTolerance) const
1098{
1099 return abs(LengthSq() - 1.0) <= inTolerance;
1100}
1101
1102bool DVec3::IsNaN() const
1103{
1104#if defined(JPH_USE_AVX512)
1105 return (_mm256_fpclass_pd_mask(mValue, 0b10000001) & 0x7) != 0;
1106#elif defined(JPH_USE_AVX)
1107 return (_mm256_movemask_pd(_mm256_cmp_pd(mValue, mValue, _CMP_UNORD_Q)) & 0x7) != 0;
1108#elif defined(JPH_USE_SSE)
1109 return ((_mm_movemask_pd(_mm_cmpunord_pd(mValue.mLow, mValue.mLow)) + (_mm_movemask_pd(_mm_cmpunord_pd(mValue.mHigh, mValue.mHigh)) << 2)) & 0x7) != 0;
1110#elif defined(JPH_USE_RVV)
1111 const vfloat64m2_t v = __riscv_vle64_v_f64m2(mF64, 3);
1112 const vbool32_t mask = __riscv_vmfeq_vv_f64m2_b32(v, v, 3);
1113 const uint32 eq = __riscv_vcpop_m_b32(mask, 3);
1114 return eq != 3;
1115#else
1116 return isnan(mF64[0]) || isnan(mF64[1]) || isnan(mF64[2]);
1117#endif
1118}
1119
1120DVec3 DVec3::GetSign() const
1121{
1122#if defined(JPH_USE_AVX512)
1123 return _mm256_fixupimm_pd(mValue, mValue, _mm256_set1_epi32(0xA9A90A00), 0);
1124#elif defined(JPH_USE_AVX)
1125 __m256d minus_one = _mm256_set1_pd(-1.0);
1126 __m256d one = _mm256_set1_pd(1.0);
1127 return _mm256_or_pd(_mm256_and_pd(mValue, minus_one), one);
1128#elif defined(JPH_USE_SSE)
1129 __m128d minus_one = _mm_set1_pd(-1.0);
1130 __m128d one = _mm_set1_pd(1.0);
1131 return DVec3({ _mm_or_pd(_mm_and_pd(mValue.mLow, minus_one), one), _mm_or_pd(_mm_and_pd(mValue.mHigh, minus_one), one) });
1132#elif defined(JPH_USE_NEON)
1133 uint64x2_t minus_one = vreinterpretq_u64_f64(vdupq_n_f64(-1.0f));
1134 uint64x2_t one = vreinterpretq_u64_f64(vdupq_n_f64(1.0f));
1135 return DVec3({ vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), minus_one), one)),
1136 vreinterpretq_f64_u64(vorrq_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), minus_one), one)) });
1137#elif defined(JPH_USE_RVV)
1138 DVec3 res;
1139 const vfloat64m2_t rvv_in = __riscv_vle64_v_f64m2(mF64, 3);
1140 const vfloat64m2_t rvv_one = __riscv_vfmv_v_f_f64m2(1.0, 3);
1141 const vfloat64m2_t rvv_signs = __riscv_vfsgnj_vv_f64m2(rvv_one, rvv_in, 3);
1142 __riscv_vse64_v_f64m2(res.mF64, rvv_signs, 3);
1143 return res;
1144#else
1145 return DVec3(std::signbit(mF64[0])? -1.0 : 1.0,
1146 std::signbit(mF64[1])? -1.0 : 1.0,
1147 std::signbit(mF64[2])? -1.0 : 1.0);
1148#endif
1149}
1150
1151DVec3 DVec3::PrepareRoundToZero() const
1152{
1153 // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float
1154 constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1;
1155
1156#if defined(JPH_USE_AVX)
1157 return _mm256_and_pd(mValue, _mm256_castsi256_pd(_mm256_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss))));
1158#elif defined(JPH_USE_SSE)
1159 __m128d mask = _mm_castsi128_pd(_mm_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss)));
1160 return DVec3({ _mm_and_pd(mValue.mLow, mask), _mm_and_pd(mValue.mHigh, mask) });
1161#elif defined(JPH_USE_NEON)
1162 uint64x2_t mask = vdupq_n_u64(~cDoubleToFloatMantissaLoss);
1163 return DVec3({ vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mask)),
1164 vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mask)) });
1165#elif defined(JPH_USE_RVV)
1166 const vfloat64m2_t dvec = __riscv_vle64_v_f64m2(mF64, 3);
1167 const vuint64m2_t dvec_u64 = __riscv_vreinterpret_v_f64m2_u64m2(dvec);
1168 const vuint64m2_t chopped = __riscv_vand_vx_u64m2(dvec_u64, ~cDoubleToFloatMantissaLoss, 3);
1169 const vfloat64m2_t chopped_f64 = __riscv_vreinterpret_v_u64m2_f64m2(chopped);
1170
1171 DVec3 res;
1172 __riscv_vse64_v_f64m2(res.mF64, chopped_f64, 3);
1173 return res;
1174#else
1175 double x = BitCast<double>(BitCast<uint64>(mF64[0]) & ~cDoubleToFloatMantissaLoss);
1176 double y = BitCast<double>(BitCast<uint64>(mF64[1]) & ~cDoubleToFloatMantissaLoss);
1177 double z = BitCast<double>(BitCast<uint64>(mF64[2]) & ~cDoubleToFloatMantissaLoss);
1178
1179 return DVec3(x, y, z);
1180#endif
1181}
1182
1183DVec3 DVec3::PrepareRoundToInf() const
1184{
1185 // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float
1186 constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1;
1187
1188#if defined(JPH_USE_AVX512)
1189 __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss);
1190 __mmask8 is_zero = _mm256_testn_epi64_mask(_mm256_castpd_si256(mValue), mantissa_loss);
1191 __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
1192 return _mm256_mask_blend_pd(is_zero, value_or_mantissa_loss, mValue);
1193#elif defined(JPH_USE_AVX)
1194 __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss);
1195 __m256d value_and_mantissa_loss = _mm256_and_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
1196 __m256d is_zero = _mm256_cmp_pd(value_and_mantissa_loss, _mm256_setzero_pd(), _CMP_EQ_OQ);
1197 __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss));
1198 return _mm256_blendv_pd(value_or_mantissa_loss, mValue, is_zero);
1199#elif defined(JPH_USE_SSE4_1)
1200 __m128i mantissa_loss = _mm_set1_epi64x(cDoubleToFloatMantissaLoss);
1201 __m128d zero = _mm_setzero_pd();
1202 __m128d value_and_mantissa_loss_low = _mm_and_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss));
1203 __m128d is_zero_low = _mm_cmpeq_pd(value_and_mantissa_loss_low, zero);
1204 __m128d value_or_mantissa_loss_low = _mm_or_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss));
1205 __m128d value_and_mantissa_loss_high = _mm_and_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss));
1206 __m128d is_zero_high = _mm_cmpeq_pd(value_and_mantissa_loss_high, zero);
1207 __m128d value_or_mantissa_loss_high = _mm_or_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss));
1208 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) });
1209#elif defined(JPH_USE_NEON)
1210 uint64x2_t mantissa_loss = vdupq_n_u64(cDoubleToFloatMantissaLoss);
1211 float64x2_t zero = vdupq_n_f64(0.0);
1212 float64x2_t value_and_mantissa_loss_low = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss));
1213 uint64x2_t is_zero_low = vceqq_f64(value_and_mantissa_loss_low, zero);
1214 float64x2_t value_or_mantissa_loss_low = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[0]), mantissa_loss));
1215 float64x2_t value_and_mantissa_loss_high = vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss));
1216 float64x2_t value_low = vbslq_f64(is_zero_low, mValue.val[0], value_or_mantissa_loss_low);
1217 uint64x2_t is_zero_high = vceqq_f64(value_and_mantissa_loss_high, zero);
1218 float64x2_t value_or_mantissa_loss_high = vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(mValue.val[1]), mantissa_loss));
1219 float64x2_t value_high = vbslq_f64(is_zero_high, mValue.val[1], value_or_mantissa_loss_high);
1220 return DVec3({ value_low, value_high });
1221#elif defined(JPH_USE_RVV)
1222 const vfloat64m2_t dvec = __riscv_vle64_v_f64m2(mF64, 3);
1223 const vuint64m2_t dvec_u64 = __riscv_vreinterpret_v_f64m2_u64m2(dvec);
1224 const vuint64m2_t and_loss = __riscv_vand_vx_u64m2(dvec_u64, cDoubleToFloatMantissaLoss, 3);
1225 const vuint64m2_t or_loss = __riscv_vor_vx_u64m2(dvec_u64, cDoubleToFloatMantissaLoss, 3);
1226 const vbool32_t is_zero = __riscv_vmseq_vx_u64m2_b32(and_loss, 0x0, 3);
1227 const vuint64m2_t select = __riscv_vmerge_vvm_u64m2(or_loss, dvec_u64, is_zero, 3);
1228 const vfloat64m2_t select_f64 = __riscv_vreinterpret_v_u64m2_f64m2(select);
1229
1230 DVec3 res;
1231 __riscv_vse64_v_f64m2(res.mF64, select_f64, 3);
1232 return res;
1233#else
1234 uint64 ux = BitCast<uint64>(mF64[0]);
1235 uint64 uy = BitCast<uint64>(mF64[1]);
1236 uint64 uz = BitCast<uint64>(mF64[2]);
1237
1238 double x = BitCast<double>((ux & cDoubleToFloatMantissaLoss) == 0? ux : (ux | cDoubleToFloatMantissaLoss));
1239 double y = BitCast<double>((uy & cDoubleToFloatMantissaLoss) == 0? uy : (uy | cDoubleToFloatMantissaLoss));
1240 double z = BitCast<double>((uz & cDoubleToFloatMantissaLoss) == 0? uz : (uz | cDoubleToFloatMantissaLoss));
1241
1242 return DVec3(x, y, z);
1243#endif
1244}
1245
1246Vec3 DVec3::ToVec3RoundDown() const
1247{
1248 DVec3 to_zero = PrepareRoundToZero();
1249 DVec3 to_inf = PrepareRoundToInf();
1250 return Vec3(DVec3::sSelect(to_zero, to_inf, DVec3::sLess(*this, DVec3::sZero())));
1251}
1252
1253Vec3 DVec3::ToVec3RoundUp() const
1254{
1255 DVec3 to_zero = PrepareRoundToZero();
1256 DVec3 to_inf = PrepareRoundToInf();
1257 return Vec3(DVec3::sSelect(to_inf, to_zero, DVec3::sLess(*this, DVec3::sZero())));
1258}
1259
1260JPH_NAMESPACE_END
uint64
std::uint64_t uint64
Definition Core.h:510
JPH_NAMESPACE_END
#define JPH_NAMESPACE_END
Definition Core.h:428
uint32
std::uint32_t uint32
Definition Core.h:508
JPH_NAMESPACE_BEGIN
#define JPH_NAMESPACE_BEGIN
Definition Core.h:422
operator*
DVec3 operator*(double inV1, DVec3Arg inV2)
Definition DVec3.inl:609
xy
#define xy
Definition HLSLToCPP.h:511
yx
#define yx
Definition HLSLToCPP.h:512
JPH_MAKE_HASHABLE
#define JPH_MAKE_HASHABLE(type,...)
Definition HashCombine.h:223
JPH_ASSERT
#define JPH_ASSERT(...)
Definition IssueReporting.h:33
BitCast
JPH_INLINE To BitCast(const From &inValue)
Definition Math.h:192
DVec3
Definition DVec3.h:14
DVec3::sLess
static JPH_INLINE DVec3 sLess(DVec3Arg inV1, DVec3Arg inV2)
Less than (component wise)
Definition DVec3.inl:309
DVec3::mF64
double mF64[4]
Definition DVec3.h:283
DVec3::sMax
static JPH_INLINE DVec3 sMax(DVec3Arg inV1, DVec3Arg inV2)
Return the maximum of each of the components.
Definition DVec3.inl:258
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:544
DVec3::ToVec3RoundDown
JPH_INLINE Vec3 ToVec3RoundDown() const
Convert to float vector 3 rounding down.
Definition DVec3.inl:1246
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:280
DVec3::sMin
static JPH_INLINE DVec3 sMin(DVec3Arg inV1, DVec3Arg inV2)
Return the minimum value of each of the components.
Definition DVec3.inl:236
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:533
DVec3::sAnd
static JPH_INLINE DVec3 sAnd(DVec3Arg inV1, DVec3Arg inV2)
Logical and (component wise)
Definition DVec3.inl:510
DVec3::operator*=
JPH_INLINE DVec3 & operator*=(double inV2)
Multiply vector with double.
Definition DVec3.inl:650
DVec3::Abs
JPH_INLINE DVec3 Abs() const
Return the absolute value of each of the components.
Definition DVec3.inl:969
DVec3::sFusedMultiplyAdd
static JPH_INLINE DVec3 sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd)
Calculates inMul1 * inMul2 + inAdd.
Definition DVec3.inl:405
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:104
DVec3::Sqrt
JPH_INLINE DVec3 Sqrt() const
Component wise square root.
Definition DVec3.inl:1068
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:1120
DVec3::mValue
Type mValue
Definition DVec3.h:282
DVec3::sXor
static JPH_INLINE DVec3 sXor(DVec3Arg inV1, DVec3Arg inV2)
Logical xor (component wise)
Definition DVec3.inl:487
DVec3::sOne
static JPH_INLINE DVec3 sOne()
Vector with all ones.
Definition DVec3.inl:178
DVec3::sGreaterOrEqual
static JPH_INLINE DVec3 sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2)
Greater than or equal (component wise)
Definition DVec3.inl:381
DVec3::operator+
JPH_INLINE DVec3 operator+(Vec3Arg inV2) const
Add two vectors (component wise)
Definition DVec3.inl:726
DVec3::IsClose
JPH_INLINE bool IsClose(DVec3Arg inV2, double inMaxDistSq=1.0e-24) const
Test if two vectors are close.
Definition DVec3.inl:559
DVec3::IsNormalized
JPH_INLINE bool IsNormalized(double inTolerance=1.0e-12) const
Test if vector is normalized.
Definition DVec3.inl:1097
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:428
DVec3::TypeArg
const Type & TypeArg
Definition DVec3.h:30
DVec3::sNaN
static JPH_INLINE DVec3 sNaN()
Vector with all NaN's.
Definition DVec3.inl:183
DVec3::operator*
friend JPH_INLINE DVec3 operator*(double inV1, DVec3Arg inV2)
Multiply vector with double.
Definition DVec3.inl:609
DVec3::sGreater
static JPH_INLINE DVec3 sGreater(DVec3Arg inV1, DVec3Arg inV2)
Greater than (component wise)
Definition DVec3.inl:357
DVec3::StoreDouble3
JPH_INLINE void StoreDouble3(Double3 *outV) const
Store 3 doubles to memory.
Definition DVec3.inl:208
DVec3::sOr
static JPH_INLINE DVec3 sOr(DVec3Arg inV1, DVec3Arg inV2)
Logical or (component wise)
Definition DVec3.inl:464
DVec3::sZero
static JPH_INLINE DVec3 sZero()
Vector with all zeros.
Definition DVec3.inl:138
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:549
DVec3::Length
JPH_INLINE double Length() const
Length of vector.
Definition DVec3.inl:1087
DVec3::operator-
JPH_INLINE DVec3 operator-() const
Negate.
Definition DVec3.inl:818
DVec3::IsNaN
JPH_INLINE bool IsNaN() const
Test if vector contains NaN elements.
Definition DVec3.inl:1102
DVec3::ToVec3RoundUp
JPH_INLINE Vec3 ToVec3RoundUp() const
Convert to float vector 3 rounding up.
Definition DVec3.inl:1253
DVec3::cTrue
static const double cTrue
Representations of true and false for boolean operations.
Definition DVec3.h:277
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:95
DVec3::LengthSq
JPH_INLINE double LengthSq() const
Squared length of vector.
Definition DVec3.inl:1063
DVec3::Normalized
JPH_INLINE DVec3 Normalized() const
Normalize vector.
Definition DVec3.inl:1092
DVec3::operator/
JPH_INLINE DVec3 operator/(double inV2) const
Divide vector by double.
Definition DVec3.inl:629
DVec3::Dot
JPH_INLINE double Dot(DVec3Arg inV2) const
Dot product.
Definition DVec3.inl:1027
DVec3::sReplicate
static JPH_INLINE DVec3 sReplicate(double inV)
Replicate inV across all components.
Definition DVec3.inl:158
DVec3::sLessOrEqual
static JPH_INLINE DVec3 sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2)
Less than or equal (component wise)
Definition DVec3.inl:333
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:1183
DVec3::operator+=
JPH_INLINE DVec3 & operator+=(Vec3Arg inV2)
Add two vectors (component wise)
Definition DVec3.inl:767
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:188
DVec3::operator/=
JPH_INLINE DVec3 & operator/=(double inV2)
Divide vector by double.
Definition DVec3.inl:700
DVec3::Cross
JPH_INLINE DVec3 Cross(DVec3Arg inV2) const
Cross product.
Definition DVec3.inl:996
DVec3::operator-=
JPH_INLINE DVec3 & operator-=(Vec3Arg inV2)
Subtract two vectors (component wise)
Definition DVec3.inl:897
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:1151
DVec3::Reciprocal
JPH_INLINE DVec3 Reciprocal() const
Reciprocal vector (1 / value) for each of the components.
Definition DVec3.inl:991
DVec3::sEquals
static JPH_INLINE DVec3 sEquals(DVec3Arg inV1, DVec3Arg inV2)
Equals (component wise)
Definition DVec3.inl:285
DVec3::Type
struct { double mData[4];} Type
Definition DVec3.h:29
DVec3::IsNearZero
JPH_INLINE bool IsNearZero(double inMaxDistSq=1.0e-24) const
Test if vector is near zero.
Definition DVec3.inl:564
DVec3::operator==
JPH_INLINE bool operator==(DVec3Arg inV2) const
Comparison.
Definition DVec3.inl:554
DVec3::cFalse
static const double cFalse
Definition DVec3.h:278
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:299
Vec3::mF32
float mF32[4]
Definition Vec3.h:300
Vec4
Definition Vec4.h:14