void SinCos(float angleRadians, float &outSin, float &outCos)
{
#ifdef MATH_USE_SINCOS_LOOKUPTABLE
return sincos_lookuptable(angleRadians, outSin, outCos);
#elif defined(MATH_SSE2)
__m128 angle = modf_ps(setx_ps(angleRadians), pi2);
__m128 sin, cos;
sincos_ps(angle, &sin, &cos);
outSin = s4f_x(sin);
outCos = s4f_x(cos);
#else
outSin = Sin(angleRadians);
outCos = Cos(angleRadians);
#endif
}
float Cos(float angleRadians)
{
#ifdef MATH_USE_SINCOS_LOOKUPTABLE
return cos_lookuptable(angleRadians);
#elif defined(MATH_SSE2)
// Do range reduction by 2pi before calling cos - this enchances precision of cos_ps a lot
return s4f_x(cos_ps(modf_ps(setx_ps(angleRadians), pi2)));
#else
return cosf(angleRadians);
#endif
}
void Quat::ToAxisAngle(float4 &axis, float &angle) const
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
// Best: 35.332 nsecs / 94.328 ticks, Avg: 35.870 nsecs, Worst: 57.607 nsecs
assume2(this->IsNormalized(), *this, this->Length());
simd4f cosAngle = _mm_shuffle_ps(q, q, _MM_SHUFFLE(3, 3, 3, 3));
simd4f rcpSinAngle = rsqrt_ps(sub_ps(set1_ps(1.f), mul_ps(cosAngle, cosAngle)));
angle = Acos(s4f_x(cosAngle)) * 2.f;
simd4f a = mul_ps(q, rcpSinAngle);
// Set the w component to zero.
simd4f highPart = _mm_unpackhi_ps(a, zero_ps()); // [_ _ 0 z]
axis.v = _mm_movelh_ps(a, highPart); // [0 z y x]
#else
// Best: 85.258 nsecs / 227.656 ticks, Avg: 85.492 nsecs, Worst: 86.410 nsecs
ToAxisAngle(reinterpret_cast<float3&>(axis), angle);
axis.w = 0.f;
#endif
}
float Quat::Normalize()
{
#ifdef MATH_AUTOMATIC_SSE
simd4f lenSq = vec4_length_sq_ps(q);
simd4f len = vec4_rsqrt(lenSq);
simd4f isZero = cmplt_ps(lenSq, simd4fEpsilon); // Was the length zero?
simd4f normalized = mul_ps(q, len); // Normalize.
q = cmov_ps(normalized, float4::unitX.v, isZero); // If length == 0, output the vector (1,0,0,0).
return s4f_x(len);
#else
float length = Length();
if (length < 1e-4f)
return 0.f;
float rcpLength = 1.f / length;
x *= rcpLength;
y *= rcpLength;
z *= rcpLength;
w *= rcpLength;
return length;
#endif
}