16 #ifndef INCLUDE_NN_NLIB_SIMD_SIMDQUATERNION_H_ 17 #define INCLUDE_NN_NLIB_SIMD_SIMDQUATERNION_H_ 49 FromRotationAxisAndSinCos(
SimdVectorArg axis_normalized,
float sin_half_rad,
76 #define NLIB_M(tp) inline tp __vectorcall 83 return F128::NegateEx<true, true, true, false>(q);
91 return Vector4::LengthSq(q);
96 return Vector4::RecpLength(q);
101 return Vector4::Normalize(q);
106 return Vector4::NormalizeEst(q);
111 f128 len_sq = LengthSq(q);
114 f128 eps = F128::SetEpsilon();
115 f128 near_zero = F128::CmpLe(len_sq, eps);
118 return F128::AndNot(near_zero, inv);
123 static const float one_eps = 1.f - 0.00001f;
124 f128 q0 = F128::SetZeroToLane<3>(q_normalized);
126 f128 xyz_len = Vector4::Length(q0);
127 f128 w_not_near_one = F128::InBound(w, F128::SetValue(one_eps,
each_float));
129 f128 theta = F128::ArcTan2(xyz_len, w);
130 f128 result = F128::Div(theta, xyz_len);
131 result = F128::Mult(q0, result);
132 result = F128::Select(w_not_near_one, result, q0);
138 f128 xyz_len = Vector4::Length(F128::SetZeroToLane<3>(q));
139 f128x2 sc = F128::SinCos(xyz_len);
141 f128 result = F128::Mult(q, F128::Div(sc.val[0], xyz_len));
142 f128 near_zero = F128::CmpNearEqZero(xyz_len, F128::SetEpsilon());
143 result = F128::Select(near_zero, q, result);
144 result = F128::Splat<false, false, false, true>(result, sc.val[1]);
150 return Vector4::CmpEq(q, Identity());
155 return Vector4::IsInfinite(q);
160 return Vector4::IsNaN(q);
166 return Vector4::CmpEq(q0, q1);
172 return Vector4::CmpNe(q0, q1);
177 return Vector4::Dot(q0, q1);
182 SimdVector v1 = F128::Swizzle<3, 0, 1, 2>(q1);
183 f128 r1 = F128::Swizzle<3, 2, 1, 0>(q0);
184 f128 r2 = F128::Swizzle<2, 3, 0, 1>(q0);
185 f128 r3 = F128::Swizzle<1, 0, 3, 2>(q0);
188 m.r[1] = F128::NegateEx<false, true, false, true>(r1);
189 m.r[2] = F128::NegateEx<false, false, true, true>(r2);
190 m.r[3] = F128::NegateEx<true, false, false, true>(r3);
191 return Vector4::Transform(v1, m);
199 SimdVector axis = F128::SetFloatToLane<3>(axis_normalized, 1.f);
200 f128 scale = F128::SetValue(sin_half_rad, sin_half_rad, sin_half_rad, cos_half_rad);
201 return F128::Mult(axis, scale);
221 m00x = F128::NegateEx<false, true, true, false>(m00x);
222 m11x = F128::NegateEx<true, false, true, false>(m11x);
223 m22x = F128::NegateEx<true, true, false, false>(m22x);
224 f128 one = F128::SetOne();
225 elem = F128::Add(m00x, m11x);
226 elem = F128::Add(elem, m22x);
227 elem = F128::Add(elem, one);
240 xx_ge_yy = F128::CmpGe(t0, t1);
241 zz_ge_ww = F128::CmpGe(t2, t3);
243 t0 = F128::PairwiseMax(elem, elem);
246 elem_max = F128::PairwiseMax(t0, t0);
247 xxyy_ge_zzww = F128::CmpGe(t2, t3);
251 elem_max = F128::Mult(v0_25, elem_max);
252 f128 mult = F128::RecpSqrt(elem_max);
253 f128 v = F128::Mult(mult, elem_max);
254 mult = F128::Mult(v0_25, mult);
257 m01_20_12 = F128::Permute<1, 4, 2, -1>(r0, r2);
258 m01_20_12 = F128::Permute<0, 1, 6, -1>(m01_20_12, r1);
261 m10_02_21 = F128::Permute<0, 1, 5, -1>(r1, r2);
262 m10_02_21 = F128::Permute<0, 6, 2, -1>(m10_02_21, r0);
264 f128 ans_x_biggest, ans_y_biggest, ans_z_biggest, ans_w_biggest;
266 f128 tmp_x, tmp_y, tmp_z, tmp_w;
267 tmp_x = F128::NegateEx<false, false, true, true>(m10_02_21);
268 tmp_x = F128::Mult(mult, F128::Add(m01_20_12, tmp_x));
270 tmp_y = F128::NegateEx<false, true, false, true>(m10_02_21);
271 tmp_y = F128::Mult(mult, F128::Add(m01_20_12, tmp_y));
273 tmp_z = F128::NegateEx<true, false, false, true>(m10_02_21);
274 tmp_z = F128::Mult(mult, F128::Add(m01_20_12, tmp_z));
276 tmp_w = F128::Mult(mult, F128::Sub(m01_20_12, m10_02_21));
278 ans_x_biggest = F128::Permute<4, 0, 1, 2>(tmp_x, v);
279 ans_y_biggest = F128::Permute<0, 4, 2, 1>(tmp_y, v);
280 ans_z_biggest = F128::Permute<1, 2, 4, 0>(tmp_z, v);
281 ans_w_biggest = F128::Permute<2, 1, 0, 4>(tmp_w, v);
284 f128 ans_xy = F128::Select(xx_ge_yy, ans_x_biggest, ans_y_biggest);
285 f128 ans_zw = F128::Select(zz_ge_ww, ans_z_biggest, ans_w_biggest);
286 return F128::Select(xxyy_ge_zzww, ans_xy, ans_zw);
298 f128 x1 = F128::Permute<4, 0, 0, 0>(sin_half_xyz, cos_half_xyz);
299 f128 y1 = F128::Permute<1, 5, 1, 1>(sin_half_xyz, cos_half_xyz);
300 f128 z1 = F128::Permute<2, 2, 6, 2>(sin_half_xyz, cos_half_xyz);
301 x1 = F128::NegateEx<false, true, true, false>(x1);
302 f128 x0 = F128::Permute<0, 4, 4, 4>(sin_half_xyz, cos_half_xyz);
303 f128 y0 = F128::Permute<5, 1, 5, 5>(sin_half_xyz, cos_half_xyz);
304 f128 z0 = F128::Permute<6, 6, 2, 6>(sin_half_xyz, cos_half_xyz);
306 f128 z0x0y0 = F128::Mult(x0, y0);
307 f128 z1x1y1 = F128::Mult(x1, y1);
308 z0x0y0 = F128::Mult(z0x0y0, z0);
309 return F128::MultAdd(z1x1y1, z1, z0x0y0);
316 *rad = 2.f * F128::GetFloatFromLane<3>(F128::ArcCos(q));
324 f128 q0q1 = Dot(q0_normalized, q1_normalized);
328 f128 ss = F128::MultSub(q0q1, q0q1, F128::SetValue(1.f,
each_float));
329 f128 eps = F128::SetEpsilon();
330 too_near = F128::CmpLe(ss, eps);
331 sp = F128::RecpSqrt(ss);
333 f128 ph = F128::ArcCos(q0q1);
334 f128 k = F128::SetValue(1.f - t, t, 0.f, 0.f);
335 f128 t0t1 = F128::Mult(sp, F128::Sin(F128::Mult(ph, k)));
339 SimdVector ret = F128::Mult(q0_normalized, t0);
340 ret = F128::MultAdd(q1_normalized, t1, ret);
341 return F128::Select(too_near, q0_normalized, ret);
349 float t2 = (t - t * t) * 2.f;
352 return Slerp(q03, q12, t2);
359 if (fg <= 0.00001f && fg >= -0.00001f)
return q0;
362 return Slerp(q01, q02, g / fg);
373 f128 lensq_a01 = Quaternion::LengthSq(F128::Add(q0, q1));
374 f128 lensq_s01 = Quaternion::LengthSq(F128::Sub(q0, q1));
375 f128 cmp01 = F128::CmpLt(lensq_a01, lensq_s01);
376 f128 neg_q0 = F128::Negate(q0);
377 Q0 = F128::Select(cmp01, neg_q0, q0);
379 f128 lensq_a12 = Quaternion::LengthSq(F128::Add(q1, q2));
380 f128 lensq_s12 = Quaternion::LengthSq(F128::Sub(q1, q2));
381 f128 cmp12 = F128::CmpLt(lensq_a12, lensq_s12);
382 f128 neg_q2 = F128::Negate(q2);
383 Q2 = F128::Select(cmp12, neg_q2, q2);
385 f128 lensq_a23 = Quaternion::LengthSq(F128::Add(q2, q3));
386 f128 lensq_s23 = Quaternion::LengthSq(F128::Sub(q2, q3));
387 f128 cmp23 = F128::CmpLt(lensq_a23, lensq_s23);
388 f128 neg_q3 = F128::Negate(q3);
389 Q3 = F128::Select(cmp23, neg_q3, q3);
394 SimdQuaternion Ln_ExpQ1_Q2 = Quaternion::Ln(Quaternion::Mult(InvQ1, Q2));
395 SimdQuaternion Ln_ExpQ1_Q0 = Quaternion::Ln(Quaternion::Mult(InvQ1, Q0));
398 SimdQuaternion Ln_ExpQ2_Q3 = Quaternion::Ln(Quaternion::Mult(InvQ2, Q3));
399 SimdQuaternion Ln_ExpQ2_Q1 = Quaternion::Ln(Quaternion::Mult(InvQ2, Q1));
402 SimdQuaternion A = F128::Mult(v0_25, F128::Add(Ln_ExpQ1_Q2, Ln_ExpQ1_Q0));
403 SimdQuaternion B = F128::Mult(v0_25, F128::Add(Ln_ExpQ2_Q3, Ln_ExpQ2_Q1));
404 A = Quaternion::Exp(A);
405 B = Quaternion::Exp(B);
407 *a = Quaternion::Mult(Q1, A);
408 *b = Quaternion::Mult(Q2, B);
414 #endif // NLIB_DOXYGEN 419 #endif // INCLUDE_NN_NLIB_SIMD_SIMDQUATERNION_H_
f128arg SimdVectorArg
f128argがtypedefされています。
128bitの単精度浮動小数点数用SIMDレジスタを2つ持つ型です。
constexpr const each_float_tag each_float
each_float_tag型の定数オブジェクトで、単精度浮動小数点数を示すためのタグです。
f128arg SimdQuaternionArg
f128argがtypedefされています。
#define NLIB_NOEXCEPT
環境に合わせてnoexcept 又は同等の定義がされます。
単精度浮動小数点数のSIMD演算を行うためのクラスや関数が定義されています。
constexpr const each_select32_tag each_select32
each_select32_tag型の定数オブジェクトで、32bitのレーンを選択することを示すためのタグです。 ...
nlib_f128_t f128
nlib_f128_tがtypedefされています。
f128 SimdQuaternion
f128がtypedefされています。クォータニオンを扱う場合に利用されます。
f128 SimdVector
f128がtypedefされています。3次元ベクトル又は4次元ベクトルを扱う場合に利用されます。 ...