Quat MUST_USE_RESULT Quat::RotateFromTo(const float4 &sourceDirection, const float4 &targetDirection) { #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE) // Best: 12.289 nsecs / 33.144 ticks, Avg: 12.489 nsecs, Worst: 14.210 nsecs simd4f cosAngle = dot4_ps(sourceDirection.v, targetDirection.v); cosAngle = negate3_ps(cosAngle); // [+ - - -] // XYZ channels use the trigonometric formula sin(x/2) = +/-sqrt(0.5-0.5*cosx)) // The W channel uses the trigonometric formula cos(x/2) = +/-sqrt(0.5+0.5*cosx)) simd4f half = set1_ps(0.5f); simd4f cosSinHalfAngle = sqrt_ps(add_ps(half, mul_ps(half, cosAngle))); // [cos(x/2), sin(x/2), sin(x/2), sin(x/2)] simd4f axis = cross_ps(sourceDirection.v, targetDirection.v); simd4f recipLen = rsqrt_ps(dot4_ps(axis, axis)); axis = mul_ps(axis, recipLen); // [0 z y x] // Set the w component to one. simd4f one = add_ps(half, half); // [1 1 1 1] simd4f highPart = _mm_unpackhi_ps(axis, one); // [_ _ 1 z] axis = _mm_movelh_ps(axis, highPart); // [1 z y x] Quat q; q.q = mul_ps(axis, cosSinHalfAngle); return q; #else // Best: 19.970 nsecs / 53.632 ticks, Avg: 20.197 nsecs, Worst: 21.122 nsecs assume(EqualAbs(sourceDirection.w, 0.f)); assume(EqualAbs(targetDirection.w, 0.f)); return Quat::RotateFromTo(sourceDirection.xyz(), targetDirection.xyz()); #endif }
Quat MUST_USE_RESULT Quat::Conjugated() const { #ifdef MATH_AUTOMATIC_SSE return negate3_ps(q); #else return Quat(-x, -y, -z, w); #endif }
void Quat::Conjugate() { #ifdef MATH_AUTOMATIC_SSE q = negate3_ps(q); #else x = -x; y = -y; z = -z; #endif }