float3 Quat::Axis() const
{
float3 axis;
float angle;
ToAxisAngle(axis, angle);
return axis;
}
Vec3 Quat::ToExponentialMap() const
{
Vec3 v;
Real a;
ToAxisAngle( v, a );
return a * v;
}
std::string MUST_USE_RESULT Quat::ToString2() const
{
float3 axis;
float angle;
ToAxisAngle(axis, angle);
char str[256];
sprintf(str, "Quat(axis:(%.2f,%.2f,%.2f) angle:%2.f)", axis.x, axis.y, axis.z, RadToDeg(angle));
return str;
}
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
}
