bool MUST_USE_RESULT float4::AreOrthonormal(const float4 &a, const float4 &b, const float4 &c, float epsilon)
{
return a.IsPerpendicular(b, epsilon) &&
a.IsPerpendicular(c, epsilon) &&
b.IsPerpendicular(c, epsilon) &&
a.IsNormalized(epsilon*epsilon) &&
b.IsNormalized(epsilon*epsilon) &&
c.IsNormalized(epsilon*epsilon);
}
float4 float4::Perpendicular(const float4 &hint, const float4 &hint2) const
{
assume(!this->IsZero3());
assume(EqualAbs(w, 0));
assume(hint.IsNormalized());
assume(hint2.IsNormalized());
float4 v = this->Cross(hint);
float len = v.Normalize();
if (len == 0)
return hint2;
else
return v;
}
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
}
float4 float4::Reflect(const float4 &normal) const
{
assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length());
assume(EqualAbs(w, 0));
return 2.f * this->ProjectToNorm(normal) - *this;
}
float4 float4::ProjectToNorm(const float4 &target) const
{
assume(target.IsNormalized());
assume(this->IsWZeroOrOne());
return target * this->Dot(target);
}
