text: // Copyright © 1996-2018, Valve Corporation, All rights reserved.
  //
  // Purpose: Defines SIMD "structure of arrays" classes and functions.
  
  #ifndef SOURCE_MATHLIB_SSEQUATMATH_H_
  #define SOURCE_MATHLIB_SSEQUATMATH_H_
  
  #ifdef _WIN32
  #pragma once
  #endif
  
  #include "mathlib/ssemath.h"
  
  #include "base/include/base_types.h"
  #include "tier0/include/floattypes.h"
  
  // Use this #define to allow SSE versions of Quaternion math
  // to exist on PC.
  // On PC, certain horizontal vector operations are not supported.
  // This causes the SSE implementation of quaternion math to mix the
  // vector and scalar floating point units, which is extremely
  // performance negative if you don't compile to native SSE2 (which
  // we don't as of Sept 1, 2007). So, it's best not to allow these
  // functions to exist at all. It's not good enough to simply replace
  // the contents of the functions with scalar math, because each call
  // to LoadAligned and StoreAligned will result in an unnecssary copy
  // of the quaternion, and several moves to and from the XMM registers.
  //
  // Basically, the problem you run into is that for efficient SIMD code,
  // you need to load the quaternions and vectors into SIMD registers and
  // keep them there as long as possible while doing only SIMD math,
  // whereas for efficient scalar code, each time you copy onto or ever
  // use a fltx4, it hoses your pipeline. So the difference has to be
  // in the management of temporary variables in the calling function,
  // not inside the math functions.
  //
  // If you compile assuming the presence of SSE2, the MSVC will abandon
  // the traditional x87 FPU operations altogether and make everything use
  // the SSE2 registers, which lessens this problem a little.
  
  // Load/store quaternions
  #if ALLOW_SIMD_QUATERNION_MATH
  // Using STDC or SSE
  SOURCE_FORCEINLINE fltx4 LoadAlignedSIMD(const QuaternionAligned &pSIMD) {
    fltx4 retval = LoadAlignedSIMD(pSIMD.Base());
    return retval;
  }
  
  SOURCE_FORCEINLINE fltx4
  LoadAlignedSIMD(const QuaternionAligned *SOURCE_RESTRICT pSIMD) {
    fltx4 retval = LoadAlignedSIMD(pSIMD);
    return retval;
  }
  
  SOURCE_FORCEINLINE void StoreAlignedSIMD(
      QuaternionAligned *SOURCE_RESTRICT pSIMD, const fltx4 &a) {
    StoreAlignedSIMD(pSIMD->Base(), a);
  }
  #endif
  
  #if ALLOW_SIMD_QUATERNION_MATH
  // Make sure quaternions are within 180 degrees of one another, if not, reverse
  // q
  SOURCE_FORCEINLINE fltx4 QuaternionAlignSIMD(const fltx4 &p, const fltx4 &q) {
    // decide if one of the quaternions is backwards
    fltx4 a = SubSIMD(p, q);
    fltx4 b = AddSIMD(p, q);
    a = Dot4SIMD(a, a);
    b = Dot4SIMD(b, b);
    fltx4 cmp = CmpGtSIMD(a, b);
    fltx4 result = MaskedAssign(cmp, NegSIMD(q), q);
    return result;
  }
  
  // Normalize Quaternion
  #if USE_STDC_FOR_SIMD
  
  SOURCE_FORCEINLINE fltx4 QuaternionNormalizeSIMD(const fltx4 &q) {
    fltx4 radius, result;
    radius = Dot4SIMD(q, q);
  
    if (SubFloat(radius, 0))  // > FLT_EPSILON && ((radius < 1.0f - 4*FLT_EPSILON)
                              // || (radius > 1.0f + 4*FLT_EPSILON))
    {
      f32 iradius = 1.0f / sqrt(SubFloat(radius, 0));
      result = ReplicateX4(iradius);
      result = MulSIMD(result, q);
      return result;
    }
    return q;
  }
  
  #else
  
  // SSE + X360 implementation
  SOURCE_FORCEINLINE fltx4 QuaternionNormalizeSIMD(const fltx4 &q) {
    fltx4 radius, result, mask;
    radius = Dot4SIMD(q, q);
    mask = CmpEqSIMD(radius, Four_Zeros);  // all ones iff radius = 0
    result = ReciprocalSqrtSIMD(radius);
    result = MulSIMD(result, q);
    return MaskedAssign(mask, q, result);  // if radius was 0, just return q
  }
  
  #endif
  
  // 0.0 returns p, 1.0 return q.
  SOURCE_FORCEINLINE fltx4 QuaternionBlendNoAlignSIMD(const fltx4 &p,
                                                      const fltx4 &q, f32 t) {
    fltx4 sclp, sclq, result;
    sclq = ReplicateX4(t);
    sclp = SubSIMD(Four_Ones, sclq);
    result = MulSIMD(sclp, p);
    result = MaddSIMD(sclq, q, result);
    return QuaternionNormalizeSIMD(result);
  }
  
  // Blend Quaternions
  SOURCE_FORCEINLINE fltx4 QuaternionBlendSIMD(const fltx4 &p, const fltx4 &q,
                                               f32 t) {
    // decide if one of the quaternions is backwards
    fltx4 q2, result;
    q2 = QuaternionAlignSIMD(p, q);
    result = QuaternionBlendNoAlignSIMD(p, q2, t);
    return result;
  }
  
  // Multiply Quaternions
  
  // SSE and STDC
  SOURCE_FORCEINLINE fltx4 QuaternionMultSIMD(const fltx4 &p, const fltx4 &q) {
    // decide if one of the quaternions is backwards
    fltx4 q2, result;
    q2 = QuaternionAlignSIMD(p, q);
    SubFloat(result, 0) =
        SubFloat(p, 0) * SubFloat(q2, 3) + SubFloat(p, 1) * SubFloat(q2, 2) -
        SubFloat(p, 2) * SubFloat(q2, 1) + SubFloat(p, 3) * SubFloat(q2, 0);
    SubFloat(result, 1) =
        -SubFloat(p, 0) * SubFloat(q2, 2) + SubFloat(p, 1) * SubFloat(q2, 3) +
        SubFloat(p, 2) * SubFloat(q2, 0) + SubFloat(p, 3) * SubFloat(q2, 1);
    SubFloat(result, 2) =
        SubFloat(p, 0) * SubFloat(q2, 1) - SubFloat(p, 1) * SubFloat(q2, 0) +
        SubFloat(p, 2) * SubFloat(q2, 3) + SubFloat(p, 3) * SubFloat(q2, 2);
    SubFloat(result, 3) =
        -SubFloat(p, 0) * SubFloat(q2, 0) - SubFloat(p, 1) * SubFloat(q2, 1) -
        SubFloat(p, 2) * SubFloat(q2, 2) + SubFloat(p, 3) * SubFloat(q2, 3);
    return result;
  }
  
  // Quaternion scale
  
  // SSE and STDC
  SOURCE_FORCEINLINE fltx4 QuaternionScaleSIMD(const fltx4 &p, f32 t) {
    f32 r;
    fltx4 q;
  
    // TODO(d.rattman): nick, this isn't overly sensitive to accuracy, and it may
    // be faster to use the cos part (w) of the quaternion
    // (sin(omega)*N,cos(omega)) to figure the new scale.
    f32 sinom =
        sqrt(SubFloat(p, 0) * SubFloat(p, 0) + SubFloat(p, 1) * SubFloat(p, 1) +
             SubFloat(p, 2) * SubFloat(p, 2));
    sinom = std::min(sinom, 1.f);
  
    f32 sinsom = sin(asin(sinom) * t);
  
    t = sinsom / (sinom + FLT_EPSILON);
    SubFloat(q, 0) = t * SubFloat(p, 0);
    SubFloat(q, 1) = t * SubFloat(p, 1);
    SubFloat(q, 2) = t * SubFloat(p, 2);
  
    // rescale rotation
    r = 1.0f - sinsom * sinsom;
  
    // Assert( r >= 0 );
    if (r < 0.0f) r = 0.0f;
    r = sqrt(r);
  
    // keep sign of rotation
    SubFloat(q, 3) = fsel(SubFloat(p, 3), r, -r);
    return q;
  }
  
  // Quaternion sphereical linear interpolation
  
  // SSE and STDC
  SOURCE_FORCEINLINE fltx4 QuaternionSlerpNoAlignSIMD(const fltx4 &p,
                                                      const fltx4 &q, f32 t) {
    f32 omega, cosom, sinom, sclp, sclq;
  
    fltx4 result;
  
    // 0.0 returns p, 1.0 return q.
    cosom = SubFloat(p, 0) * SubFloat(q, 0) + SubFloat(p, 1) * SubFloat(q, 1) +
            SubFloat(p, 2) * SubFloat(q, 2) + SubFloat(p, 3) * SubFloat(q, 3);
  
    if ((1.0f + cosom) > 0.000001f) {
      if ((1.0f - cosom) > 0.000001f) {
        omega = acos(cosom);
        sinom = sin(omega);
        sclp = sin((1.0f - t) * omega) / sinom;
        sclq = sin(t * omega) / sinom;
      } else {
        // TODO: add i16 circuit for cosom == 1.0f?
        sclp = 1.0f - t;
        sclq = t;
      }
      SubFloat(result, 0) = sclp * SubFloat(p, 0) + sclq * SubFloat(q, 0);
      SubFloat(result, 1) = sclp * SubFloat(p, 1) + sclq * SubFloat(q, 1);
      SubFloat(result, 2) = sclp * SubFloat(p, 2) + sclq * SubFloat(q, 2);
      SubFloat(result, 3) = sclp * SubFloat(p, 3) + sclq * SubFloat(q, 3);
    } else {
      SubFloat(result, 0) = -SubFloat(q, 1);
      SubFloat(result, 1) = SubFloat(q, 0);
      SubFloat(result, 2) = -SubFloat(q, 3);
      SubFloat(result, 3) = SubFloat(q, 2);
      sclp = sin((1.0f - t) * (0.5f * M_PI));
      sclq = sin(t * (0.5f * M_PI));
      SubFloat(result, 0) = sclp * SubFloat(p, 0) + sclq * SubFloat(result, 0);
      SubFloat(result, 1) = sclp * SubFloat(p, 1) + sclq * SubFloat(result, 1);
      SubFloat(result, 2) = sclp * SubFloat(p, 2) + sclq * SubFloat(result, 2);
    }
  
    return result;
  }
  
  SOURCE_FORCEINLINE fltx4 QuaternionSlerpSIMD(const fltx4 &p, const fltx4 &q,
                                               f32 t) {
    fltx4 q2, result;
    q2 = QuaternionAlignSIMD(p, q);
    result = QuaternionSlerpNoAlignSIMD(p, q2, t);
    return result;
  }
  
  #endif  // ALLOW_SIMD_QUATERNION_MATH
  
  #endif  // SOURCE_MATHLIB_SSEQUATMATH_H_
  