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); }