void cQuaternion::SetFromVectors(const cVec3& source, const cVec3& destination)
{
const cVec3 axis(source.CrossProduct(destination));
const float fAngleRadians = acos(source.GetNormalised().DotProduct(destination.GetNormalised()));
SetFromAxisAngle(axis, fAngleRadians);
}
void Quat::SetFromAxisAngle(const float3 &axis, float angle)
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
SetFromAxisAngle(load_vec3(axis.ptr(), 0.f), angle);
#else
assume1(axis.IsNormalized(), axis);
assume1(MATH_NS::IsFinite(angle), angle);
float sinz, cosz;
SinCos(angle*0.5f, sinz, cosz);
x = axis.x * sinz;
y = axis.y * sinz;
z = axis.z * sinz;
w = cosz;
#endif
}
void Quat::SetFromAxisAngle(const float4 &axis, float angle)
{
assume1(EqualAbs(axis.w, 0.f), axis);
assume2(axis.IsNormalized(1e-4f), axis, axis.Length4());
assume1(MATH_NS::IsFinite(angle), angle);
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE2)
// Best: 26.499 nsecs / 71.024 ticks, Avg: 26.856 nsecs, Worst: 27.651 nsecs
simd4f halfAngle = set1_ps(0.5f*angle);
simd4f sinAngle, cosAngle;
sincos_ps(halfAngle, &sinAngle, &cosAngle);
simd4f quat = mul_ps(axis, sinAngle);
// Set the w component to cosAngle.
simd4f highPart = _mm_unpackhi_ps(quat, cosAngle); // [_ _ 1 z]
q = _mm_movelh_ps(quat, highPart); // [1 z y x]
#else
// Best: 36.868 nsecs / 98.312 ticks, Avg: 36.980 nsecs, Worst: 41.477 nsecs
SetFromAxisAngle(axis.xyz(), angle);
#endif
}
Quat::Quat(const float3 &rotationAxis, float rotationAngle)
{
SetFromAxisAngle(rotationAxis, rotationAngle);
}
//轴角对构造
HawkQuaternion::HawkQuaternion(const HawkVector3D& vAxis,Float fRadian)
{
SetFromAxisAngle(vAxis,fRadian);
}
void cQuaternion::SetFromAxisAngleDegrees(const cVec3& axis, float fAngleDegrees)
{
SetFromAxisAngle(axis, DegreesToRadians(fAngleDegrees));
}