//========= Copyright Valve Corporation, All rights reserved. ============//
//
// Purpose:
//
//===========================================================================//

#ifndef MATH_LIB_H
#define MATH_LIB_H

#include <math.h>
#include "../tier0/basetypes.h"
#include "../tier0/commonmacros.h"
#include "../tier0/dbg.h"
#include "vector.h"
#include "vector2d.h"

#include "math_pfns.h"

#if defined(__i386__) || defined(_M_IX86)
// For MMX intrinsics
#include <xmmintrin.h>
#endif

// XXX remove me
#undef clamp

// Uncomment this to enable FP exceptions in parts of the code.
// This can help track down FP bugs. However the code is not
// FP exception clean so this not a turnkey operation.
//#define FP_EXCEPTIONS_ENABLED

#ifdef FP_EXCEPTIONS_ENABLED
#include <float.h>  // For _clearfp and _controlfp_s
#endif

// FPExceptionDisabler and FPExceptionEnabler taken from my blog post
// at http://www.altdevblogaday.com/2012/04/20/exceptional-floating-point/

// Declare an object of this type in a scope in order to suppress
// all floating-point exceptions temporarily. The old exception
// state will be reset at the end.
class FPExceptionDisabler {
   public:
#ifdef FP_EXCEPTIONS_ENABLED
    FPExceptionDisabler();
    ~FPExceptionDisabler();

   private:
    unsigned int mOldValues;
#else
    FPExceptionDisabler() {}
    ~FPExceptionDisabler() {}
#endif

   private:
    // Make the copy constructor and assignment operator private
    // and unimplemented to prohibit copying.
    FPExceptionDisabler(const FPExceptionDisabler &);
    FPExceptionDisabler &operator=(const FPExceptionDisabler &);
};

// Declare an object of this type in a scope in order to enable a
// specified set of floating-point exceptions temporarily. The old
// exception state will be reset at the end.
// This class can be nested.
class FPExceptionEnabler {
   public:
    // Overflow, divide-by-zero, and invalid-operation are the FP
    // exceptions most frequently associated with bugs.
#ifdef FP_EXCEPTIONS_ENABLED
    FPExceptionEnabler(unsigned int enableBits = _EM_OVERFLOW | _EM_ZERODIVIDE |
                                                 _EM_INVALID);
    ~FPExceptionEnabler();

   private:
    unsigned int mOldValues;
#else
    FPExceptionEnabler(unsigned int enableBits = 0) {}
    ~FPExceptionEnabler() {}
#endif

   private:
    // Make the copy constructor and assignment operator private
    // and unimplemented to prohibit copying.
    FPExceptionEnabler(const FPExceptionEnabler &);
    FPExceptionEnabler &operator=(const FPExceptionEnabler &);
};

#ifdef DEBUG  // stop crashing edit-and-continue
FORCEINLINE float clamp(float val, float minVal, float maxVal) {
    if (maxVal < minVal)
        return maxVal;
    else if (val < minVal)
        return minVal;
    else if (val > maxVal)
        return maxVal;
    else
        return val;
}
#else  // DEBUG
FORCEINLINE float clamp(float val, float minVal, float maxVal) {
#if defined(__i386__) || defined(_M_IX86)
    _mm_store_ss(&val,
                 _mm_min_ss(_mm_max_ss(_mm_load_ss(&val), _mm_load_ss(&minVal)),
                            _mm_load_ss(&maxVal)));
#else
    val = fpmax(minVal, val);
    val = fpmin(maxVal, val);
#endif
    return val;
}
#endif  // DEBUG

//
// Returns a clamped value in the range [min, max].
//
template <class T>
inline T clamp(T const &val, T const &minVal, T const &maxVal) {
    if (maxVal < minVal)
        return maxVal;
    else if (val < minVal)
        return minVal;
    else if (val > maxVal)
        return maxVal;
    else
        return val;
}

// plane_t structure
// !!! if this is changed, it must be changed in asm code too !!!
// FIXME: does the asm code even exist anymore?
// FIXME: this should move to a different file
struct cplane_t {
    Vector normal;
    float dist;
    byte type;      // for fast side tests
    byte signbits;  // signx + (signy<<1) + (signz<<1)
    byte pad[2];

#ifdef VECTOR_NO_SLOW_OPERATIONS
    cplane_t() {}

   private:
    // No copy constructors allowed if we're in optimal mode
    cplane_t(const cplane_t &vOther);
#endif
};

// structure offset for asm code
#define CPLANE_NORMAL_X 0
#define CPLANE_NORMAL_Y 4
#define CPLANE_NORMAL_Z 8
#define CPLANE_DIST 12
#define CPLANE_TYPE 16
#define CPLANE_SIGNBITS 17
#define CPLANE_PAD0 18
#define CPLANE_PAD1 19

// 0-2 are axial planes
#define PLANE_X 0
#define PLANE_Y 1
#define PLANE_Z 2

// 3-5 are non-axial planes snapped to the nearest
#define PLANE_ANYX 3
#define PLANE_ANYY 4
#define PLANE_ANYZ 5

//-----------------------------------------------------------------------------
// Frustum plane indices.
// WARNING: there is code that depends on these values
//-----------------------------------------------------------------------------

enum {
    FRUSTUM_RIGHT = 0,
    FRUSTUM_LEFT = 1,
    FRUSTUM_TOP = 2,
    FRUSTUM_BOTTOM = 3,
    FRUSTUM_NEARZ = 4,
    FRUSTUM_FARZ = 5,
    FRUSTUM_NUMPLANES = 6
};

extern int SignbitsForPlane(cplane_t *out);

class Frustum_t {
   public:
    void SetPlane(int i, int nType, const Vector &vecNormal, float dist) {
        m_Plane[i].normal = vecNormal;
        m_Plane[i].dist = dist;
        m_Plane[i].type = nType;
        m_Plane[i].signbits = SignbitsForPlane(&m_Plane[i]);
        m_AbsNormal[i].Init(fabs(vecNormal.x), fabs(vecNormal.y),
                            fabs(vecNormal.z));
    }

    inline const cplane_t *GetPlane(int i) const { return &m_Plane[i]; }
    inline const Vector &GetAbsNormal(int i) const { return m_AbsNormal[i]; }

   private:
    cplane_t m_Plane[FRUSTUM_NUMPLANES];
    Vector m_AbsNormal[FRUSTUM_NUMPLANES];
};

// Computes Y fov from an X fov and a screen aspect ratio + X from Y
float CalcFovY(float flFovX, float flScreenAspect);
float CalcFovX(float flFovY, float flScreenAspect);

// Generate a frustum based on perspective view parameters
// NOTE: FOV is specified in degrees, as the *full* view angle (not half-angle)
void GeneratePerspectiveFrustum(const Vector &origin, const QAngle &angles,
                                float flZNear, float flZFar, float flFovX,
                                float flAspectRatio, Frustum_t &frustum);
void GeneratePerspectiveFrustum(const Vector &origin, const Vector &forward,
                                const Vector &right, const Vector &up,
                                float flZNear, float flZFar, float flFovX,
                                float flFovY, Frustum_t &frustum);

// Cull the world-space bounding box to the specified frustum.
bool R_CullBox(const Vector &mins, const Vector &maxs,
               const Frustum_t &frustum);
bool R_CullBoxSkipNear(const Vector &mins, const Vector &maxs,
                       const Frustum_t &frustum);

struct matrix3x4_t {
    matrix3x4_t() {}
    matrix3x4_t(float m00, float m01, float m02, float m03, float m10,
                float m11, float m12, float m13, float m20, float m21,
                float m22, float m23) {
        m_flMatVal[0][0] = m00;
        m_flMatVal[0][1] = m01;
        m_flMatVal[0][2] = m02;
        m_flMatVal[0][3] = m03;
        m_flMatVal[1][0] = m10;
        m_flMatVal[1][1] = m11;
        m_flMatVal[1][2] = m12;
        m_flMatVal[1][3] = m13;
        m_flMatVal[2][0] = m20;
        m_flMatVal[2][1] = m21;
        m_flMatVal[2][2] = m22;
        m_flMatVal[2][3] = m23;
    }

    //-----------------------------------------------------------------------------
    // Creates a matrix where the X axis = forward
    // the Y axis = left, and the Z axis = up
    //-----------------------------------------------------------------------------
    void Init(const Vector &xAxis, const Vector &yAxis, const Vector &zAxis,
              const Vector &vecOrigin) {
        m_flMatVal[0][0] = xAxis.x;
        m_flMatVal[0][1] = yAxis.x;
        m_flMatVal[0][2] = zAxis.x;
        m_flMatVal[0][3] = vecOrigin.x;
        m_flMatVal[1][0] = xAxis.y;
        m_flMatVal[1][1] = yAxis.y;
        m_flMatVal[1][2] = zAxis.y;
        m_flMatVal[1][3] = vecOrigin.y;
        m_flMatVal[2][0] = xAxis.z;
        m_flMatVal[2][1] = yAxis.z;
        m_flMatVal[2][2] = zAxis.z;
        m_flMatVal[2][3] = vecOrigin.z;
    }

    //-----------------------------------------------------------------------------
    // Creates a matrix where the X axis = forward
    // the Y axis = left, and the Z axis = up
    //-----------------------------------------------------------------------------
    matrix3x4_t(const Vector &xAxis, const Vector &yAxis, const Vector &zAxis,
                const Vector &vecOrigin) {
        Init(xAxis, yAxis, zAxis, vecOrigin);
    }

    inline void Invalidate(void) {
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 4; j++) {
                m_flMatVal[i][j] = VEC_T_NAN;
            }
        }
    }

    float *operator[](int i) {
        Assert((i >= 0) && (i < 3));
        return m_flMatVal[i];
    }
    const float *operator[](int i) const {
        Assert((i >= 0) && (i < 3));
        return m_flMatVal[i];
    }
    float *Base() { return &m_flMatVal[0][0]; }
    const float *Base() const { return &m_flMatVal[0][0]; }

    float m_flMatVal[3][4];
};

#ifndef M_PI
#define M_PI 3.14159265358979323846  // matches value in gcc v2 math.h
#endif

#define M_PI_F ((float)(M_PI))  // Shouldn't collide with anything.

// NJS: Inlined to prevent floats from being autopromoted to doubles, as with
// the old system.
#ifndef RAD2DEG
#define RAD2DEG(x) ((float)(x) * (float)(180.f / M_PI_F))
#endif

#ifndef DEG2RAD
#define DEG2RAD(x) ((float)(x) * (float)(M_PI_F / 180.f))
#endif

// Used to represent sides of things like planes.
#define SIDE_FRONT 0
#define SIDE_BACK 1
#define SIDE_ON 2
#define SIDE_CROSS -2  // necessary for polylib.c

#define ON_VIS_EPSILON \
    0.01  // necessary for vvis (flow.c) -- again look into moving later!
#define EQUAL_EPSILON \
    0.001  // necessary for vbsp (faces.c) -- should look into moving it there?

extern bool s_bMathlibInitialized;

extern const Vector vec3_origin;
extern const QAngle vec3_angle;
extern const Quaternion quat_identity;
extern const Vector vec3_invalid;
extern const int nanmask;

#define IS_NAN(x) (((*(int *)&x) & nanmask) == nanmask)

FORCEINLINE vec_t DotProduct(const vec_t *v1, const vec_t *v2) {
    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}
FORCEINLINE void VectorSubtract(const vec_t *a, const vec_t *b, vec_t *c) {
    c[0] = a[0] - b[0];
    c[1] = a[1] - b[1];
    c[2] = a[2] - b[2];
}
FORCEINLINE void VectorAdd(const vec_t *a, const vec_t *b, vec_t *c) {
    c[0] = a[0] + b[0];
    c[1] = a[1] + b[1];
    c[2] = a[2] + b[2];
}
FORCEINLINE void VectorCopy(const vec_t *a, vec_t *b) {
    b[0] = a[0];
    b[1] = a[1];
    b[2] = a[2];
}
FORCEINLINE void VectorClear(vec_t *a) { a[0] = a[1] = a[2] = 0; }

FORCEINLINE float VectorMaximum(const vec_t *v) {
    return max(v[0], max(v[1], v[2]));
}

FORCEINLINE float VectorMaximum(const Vector &v) {
    return max(v.x, max(v.y, v.z));
}

FORCEINLINE void VectorScale(const float *in, vec_t scale, float *out) {
    out[0] = in[0] * scale;
    out[1] = in[1] * scale;
    out[2] = in[2] * scale;
}

// Cannot be forceinline as they have overloads:
inline void VectorFill(vec_t *a, float b) { a[0] = a[1] = a[2] = b; }

inline void VectorNegate(vec_t *a) {
    a[0] = -a[0];
    a[1] = -a[1];
    a[2] = -a[2];
}

//#define VectorMaximum(a)		( max( (a)[0], max( (a)[1], (a)[2] ) ) )
#define Vector2Clear(x) \
    { (x)[0] = (x)[1] = 0; }
#define Vector2Negate(x)    \
    {                       \
        (x)[0] = -((x)[0]); \
        (x)[1] = -((x)[1]); \
    }
#define Vector2Copy(a, b) \
    {                     \
        (b)[0] = (a)[0];  \
        (b)[1] = (a)[1];  \
    }
#define Vector2Subtract(a, b, c)  \
    {                             \
        (c)[0] = (a)[0] - (b)[0]; \
        (c)[1] = (a)[1] - (b)[1]; \
    }
#define Vector2Add(a, b, c)       \
    {                             \
        (c)[0] = (a)[0] + (b)[0]; \
        (c)[1] = (a)[1] + (b)[1]; \
    }
#define Vector2Scale(a, b, c)  \
    {                          \
        (c)[0] = (b) * (a)[0]; \
        (c)[1] = (b) * (a)[1]; \
    }

// NJS: Some functions in VBSP still need to use these for dealing with mixing
// vec4's and shorts with vec_t's. remove when no longer needed.
#define VECTOR_COPY(A, B) \
    do {                  \
        (B)[0] = (A)[0];  \
        (B)[1] = (A)[1];  \
        (B)[2] = (A)[2];  \
    } while (0)
#define DOT_PRODUCT(A, B) ((A)[0] * (B)[0] + (A)[1] * (B)[1] + (A)[2] * (B)[2])

FORCEINLINE void VectorMAInline(const float *start, float scale,
                                const float *direction, float *dest) {
    dest[0] = start[0] + direction[0] * scale;
    dest[1] = start[1] + direction[1] * scale;
    dest[2] = start[2] + direction[2] * scale;
}

FORCEINLINE void VectorMAInline(const Vector &start, float scale,
                                const Vector &direction, Vector &dest) {
    dest.x = start.x + direction.x * scale;
    dest.y = start.y + direction.y * scale;
    dest.z = start.z + direction.z * scale;
}

FORCEINLINE void VectorMA(const Vector &start, float scale,
                          const Vector &direction, Vector &dest) {
    VectorMAInline(start, scale, direction, dest);
}

FORCEINLINE void VectorMA(const float *start, float scale,
                          const float *direction, float *dest) {
    VectorMAInline(start, scale, direction, dest);
}

int VectorCompare(const float *v1, const float *v2);

inline float VectorLength(const float *v) {
    return FastSqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + FLT_EPSILON);
}

void CrossProduct(const float *v1, const float *v2, float *cross);

qboolean VectorsEqual(const float *v1, const float *v2);

inline vec_t RoundInt(vec_t in) { return floor(in + 0.5f); }

int Q_log2(int val);

// Math routines done in optimized assembly math package routines
void inline SinCos(float radians, float *sine, float *cosine) {
#if defined(_X360)
    XMScalarSinCos(sine, cosine, radians);
#elif defined(PLATFORM_WINDOWS_PC32) || defined(PLATFORM_WINDOWS_PC64)
    *sine = sin(radians);
    *cosine = cos(radians);
#elif defined(POSIX)
    double __cosr, __sinr;
    __asm("fsincos" : "=t"(__cosr), "=u"(__sinr) : "0"(radians));

    *sine = __sinr;
    *cosine = __cosr;
#endif
}

#define SIN_TABLE_SIZE 256
#define FTOIBIAS 12582912.f
extern float SinCosTable[SIN_TABLE_SIZE];

inline float TableCos(float theta) {
    union {
        int i;
        float f;
    } ftmp;

    // ideally, the following should compile down to: theta * constant +
    // constant, changing any of these constants from defines sometimes fubars
    // this.
    ftmp.f = theta * (float)(SIN_TABLE_SIZE / (2.0f * M_PI)) +
             (FTOIBIAS + (SIN_TABLE_SIZE / 4));
    return SinCosTable[ftmp.i & (SIN_TABLE_SIZE - 1)];
}

inline float TableSin(float theta) {
    union {
        int i;
        float f;
    } ftmp;

    // ideally, the following should compile down to: theta * constant +
    // constant
    ftmp.f = theta * (float)(SIN_TABLE_SIZE / (2.0f * M_PI)) + FTOIBIAS;
    return SinCosTable[ftmp.i & (SIN_TABLE_SIZE - 1)];
}

template <class T>
FORCEINLINE T Square(T const &a) {
    return a * a;
}

// return the smallest power of two >= x.
// returns 0 if x == 0 or x > 0x80000000 (ie numbers that would be negative if x
// was signed) NOTE: the old code took an int, and if you pass in an int of
// 0x80000000 casted to a uint,
//       you'll get 0x80000000, which is correct for uints, instead of 0, which
//       was correct for ints
FORCEINLINE uint SmallestPowerOfTwoGreaterOrEqual(uint x) {
    x -= 1;
    x |= x >> 1;
    x |= x >> 2;
    x |= x >> 4;
    x |= x >> 8;
    x |= x >> 16;
    return x + 1;
}

// return the largest power of two <= x. Will return 0 if passed 0
FORCEINLINE uint LargestPowerOfTwoLessThanOrEqual(uint x) {
    if (x >= 0x80000000) return 0x80000000;

    return SmallestPowerOfTwoGreaterOrEqual(x + 1) >> 1;
}

// Math routines for optimizing division
void FloorDivMod(double numer, double denom, int *quotient, int *rem);
int GreatestCommonDivisor(int i1, int i2);

// Test for FPU denormal mode
bool IsDenormal(const float &val);

// MOVEMENT INFO
enum {
    PITCH = 0,  // up / down
    YAW,        // left / right
    ROLL        // fall over
};

void MatrixAngles(const matrix3x4_t &matrix, float *angles);  // !!!!
void MatrixVectors(const matrix3x4_t &matrix, Vector *pForward, Vector *pRight,
                   Vector *pUp);
void VectorTransform(const float *in1, const matrix3x4_t &in2, float *out);
void VectorITransform(const float *in1, const matrix3x4_t &in2, float *out);
void VectorRotate(const float *in1, const matrix3x4_t &in2, float *out);
void VectorRotate(const Vector &in1, const QAngle &in2, Vector &out);
void VectorRotate(const Vector &in1, const Quaternion &in2, Vector &out);
void VectorIRotate(const float *in1, const matrix3x4_t &in2, float *out);

#ifndef VECTOR_NO_SLOW_OPERATIONS

QAngle TransformAnglesToLocalSpace(const QAngle &angles,
                                   const matrix3x4_t &parentMatrix);
QAngle TransformAnglesToWorldSpace(const QAngle &angles,
                                   const matrix3x4_t &parentMatrix);

#endif

void MatrixInitialize(matrix3x4_t &mat, const Vector &vecOrigin,
                      const Vector &vecXAxis, const Vector &vecYAxis,
                      const Vector &vecZAxis);
void MatrixCopy(const matrix3x4_t &in, matrix3x4_t &out);
void MatrixInvert(const matrix3x4_t &in, matrix3x4_t &out);

// Matrix equality test
bool MatricesAreEqual(const matrix3x4_t &src1, const matrix3x4_t &src2,
                      float flTolerance = 1e-5);

void MatrixGetColumn(const matrix3x4_t &in, int column, Vector &out);
void MatrixSetColumn(const Vector &in, int column, matrix3x4_t &out);

inline void MatrixGetTranslation(const matrix3x4_t &in, Vector &out) {
    MatrixGetColumn(in, 3, out);
}

inline void MatrixSetTranslation(const Vector &in, matrix3x4_t &out) {
    MatrixSetColumn(in, 3, out);
}

void MatrixScaleBy(const float flScale, matrix3x4_t &out);
void MatrixScaleByZero(matrix3x4_t &out);

// void DecomposeRotation( const matrix3x4_t &mat, float *out );
void ConcatRotations(const matrix3x4_t &in1, const matrix3x4_t &in2,
                     matrix3x4_t &out);
void ConcatTransforms(const matrix3x4_t &in1, const matrix3x4_t &in2,
                      matrix3x4_t &out);

// For identical interface w/ VMatrix
inline void MatrixMultiply(const matrix3x4_t &in1, const matrix3x4_t &in2,
                           matrix3x4_t &out) {
    ConcatTransforms(in1, in2, out);
}

void QuaternionSlerp(const Quaternion &p, const Quaternion &q, float t,
                     Quaternion &qt);
void QuaternionSlerpNoAlign(const Quaternion &p, const Quaternion &q, float t,
                            Quaternion &qt);
void QuaternionBlend(const Quaternion &p, const Quaternion &q, float t,
                     Quaternion &qt);
void QuaternionBlendNoAlign(const Quaternion &p, const Quaternion &q, float t,
                            Quaternion &qt);
void QuaternionIdentityBlend(const Quaternion &p, float t, Quaternion &qt);
float QuaternionAngleDiff(const Quaternion &p, const Quaternion &q);
void QuaternionScale(const Quaternion &p, float t, Quaternion &q);
void QuaternionAlign(const Quaternion &p, const Quaternion &q, Quaternion &qt);
float QuaternionDotProduct(const Quaternion &p, const Quaternion &q);
void QuaternionConjugate(const Quaternion &p, Quaternion &q);
void QuaternionInvert(const Quaternion &p, Quaternion &q);
float QuaternionNormalize(Quaternion &q);
void QuaternionAdd(const Quaternion &p, const Quaternion &q, Quaternion &qt);
void QuaternionMult(const Quaternion &p, const Quaternion &q, Quaternion &qt);
void QuaternionMatrix(const Quaternion &q, matrix3x4_t &matrix);
void QuaternionMatrix(const Quaternion &q, const Vector &pos,
                      matrix3x4_t &matrix);
void QuaternionAngles(const Quaternion &q, QAngle &angles);
void AngleQuaternion(const QAngle &angles, Quaternion &qt);
void QuaternionAngles(const Quaternion &q, RadianEuler &angles);
void AngleQuaternion(RadianEuler const &angles, Quaternion &qt);
void QuaternionAxisAngle(const Quaternion &q, Vector &axis, float &angle);
void AxisAngleQuaternion(const Vector &axis, float angle, Quaternion &q);
void BasisToQuaternion(const Vector &vecForward, const Vector &vecRight,
                       const Vector &vecUp, Quaternion &q);
void MatrixQuaternion(const matrix3x4_t &mat, Quaternion &q);

// A couple methods to find the dot product of a vector with a matrix row or
// column...
inline float MatrixRowDotProduct(const matrix3x4_t &in1, int row,
                                 const Vector &in2) {
    Assert((row >= 0) && (row < 3));
    return DotProduct(in1[row], in2.Base());
}

inline float MatrixColumnDotProduct(const matrix3x4_t &in1, int col,
                                    const Vector &in2) {
    Assert((col >= 0) && (col < 4));
    return in1[0][col] * in2[0] + in1[1][col] * in2[1] + in1[2][col] * in2[2];
}

int __cdecl BoxOnPlaneSide(const float *emins, const float *emaxs,
                           const cplane_t *plane);

inline float anglemod(float a) {
    a = (360.f / 65536) * ((int)(a * (65536.f / 360.0f)) & 65535);
    return a;
}

// Remap a value in the range [A,B] to [C,D].
inline float RemapVal(float val, float A, float B, float C, float D) {
    if (A == B) return val >= B ? D : C;
    return C + (D - C) * (val - A) / (B - A);
}

inline float RemapValClamped(float val, float A, float B, float C, float D) {
    if (A == B) return val >= B ? D : C;
    float cVal = (val - A) / (B - A);
    cVal = clamp(cVal, 0.0f, 1.0f);

    return C + (D - C) * cVal;
}

// Returns A + (B-A)*flPercent.
// float Lerp( float flPercent, float A, float B );
template <class T>
FORCEINLINE T Lerp(float flPercent, T const &A, T const &B) {
    return A + (B - A) * flPercent;
}

FORCEINLINE float Sqr(float f) { return f * f; }

// 5-argument floating point linear interpolation.
// FLerp(f1,f2,i1,i2,x)=
//    f1 at x=i1
//    f2 at x=i2
//   smooth lerp between f1 and f2 at x>i1 and x<i2
//   extrapolation for x<i1 or x>i2
//
//   If you know a function f(x)'s value (f1) at position i1, and its value (f2)
//   at position i2, the function can be linearly interpolated with
//   FLerp(f1,f2,i1,i2,x)
//    i2=i1 will cause a divide by zero.
static inline float FLerp(float f1, float f2, float i1, float i2, float x) {
    return f1 + (f2 - f1) * (x - i1) / (i2 - i1);
}

#ifndef VECTOR_NO_SLOW_OPERATIONS

// YWB:  Specialization for interpolating euler angles via quaternions...
template <>
FORCEINLINE QAngle Lerp<QAngle>(float flPercent, const QAngle &q1,
                                const QAngle &q2) {
    // Avoid precision errors
    if (q1 == q2) return q1;

    Quaternion src, dest;

    // Convert to quaternions
    AngleQuaternion(q1, src);
    AngleQuaternion(q2, dest);

    Quaternion result;

    // Slerp
    QuaternionSlerp(src, dest, flPercent, result);

    // Convert to euler
    QAngle output;
    QuaternionAngles(result, output);
    return output;
}

#else

#pragma error

// NOTE NOTE: I haven't tested this!! It may not work! Check out
// interpolatedvar.cpp in the client dll to try it
template <>
FORCEINLINE QAngleByValue Lerp<QAngleByValue>(float flPercent,
                                              const QAngleByValue &q1,
                                              const QAngleByValue &q2) {
    // Avoid precision errors
    if (q1 == q2) return q1;

    Quaternion src, dest;

    // Convert to quaternions
    AngleQuaternion(q1, src);
    AngleQuaternion(q2, dest);

    Quaternion result;

    // Slerp
    QuaternionSlerp(src, dest, flPercent, result);

    // Convert to euler
    QAngleByValue output;
    QuaternionAngles(result, output);
    return output;
}

#endif  // VECTOR_NO_SLOW_OPERATIONS

/// Same as swap(), but won't cause problems with std::swap
template <class T>
FORCEINLINE void V_swap(T &x, T &y) {
    T temp = x;
    x = y;
    y = temp;
}

template <class T>
FORCEINLINE T AVG(T a, T b) {
    return (a + b) / 2;
}

// number of elements in an array of static size
#define NELEMS(x) ARRAYSIZE(x)

// XYZ macro, for printf type functions - ex printf("%f %f %f",XYZ(myvector));
#define XYZ(v) (v).x, (v).y, (v).z

inline float Sign(float x) { return (x < 0.0f) ? -1.0f : 1.0f; }

//
// Clamps the input integer to the given array bounds.
// Equivalent to the following, but without using any branches:
//
// if( n < 0 ) return 0;
// else if ( n > maxindex ) return maxindex;
// else return n;
//
// This is not always a clear performance win, but when you have situations
// where a clamped value is thrashing against a boundary this is a big win. (ie,
// valid, invalid, valid, invalid, ...)
//
// Note: This code has been run against all possible integers.
//
inline int ClampArrayBounds(int n, unsigned maxindex) {
    // mask is 0 if less than 4096, 0xFFFFFFFF if greater than
    unsigned int inrangemask = 0xFFFFFFFF + (((unsigned)n) > maxindex);
    unsigned int lessthan0mask = 0xFFFFFFFF + (n >= 0);

    // If the result was valid, set the result, (otherwise sets zero)
    int result = (inrangemask & n);

    // if the result was out of range or zero.
    result |= ((~inrangemask) & (~lessthan0mask)) & maxindex;

    return result;
}

#define BOX_ON_PLANE_SIDE(emins, emaxs, p)                                 \
    (((p)->type < 3) ? (((p)->dist <= (emins)[(p)->type])                  \
                            ? 1                                            \
                            : (((p)->dist >= (emaxs)[(p)->type]) ? 2 : 3)) \
                     : BoxOnPlaneSide((emins), (emaxs), (p)))

//-----------------------------------------------------------------------------
// FIXME: Vector versions.... the float versions will go away hopefully soon!
//-----------------------------------------------------------------------------

void AngleVectors(const QAngle &angles, Vector *forward);
void AngleVectors(const QAngle &angles, Vector *forward, Vector *right,
                  Vector *up);
void AngleVectorsTranspose(const QAngle &angles, Vector *forward, Vector *right,
                           Vector *up);
void AngleMatrix(const QAngle &angles, matrix3x4_t &mat);
void AngleMatrix(const QAngle &angles, const Vector &position,
                 matrix3x4_t &mat);
void AngleMatrix(const RadianEuler &angles, matrix3x4_t &mat);
void AngleMatrix(RadianEuler const &angles, const Vector &position,
                 matrix3x4_t &mat);
void AngleIMatrix(const QAngle &angles, matrix3x4_t &mat);
void AngleIMatrix(const QAngle &angles, const Vector &position,
                  matrix3x4_t &mat);
void AngleIMatrix(const RadianEuler &angles, matrix3x4_t &mat);
void VectorAngles(const Vector &forward, QAngle &angles);
void VectorAngles(const Vector &forward, const Vector &pseudoup,
                  QAngle &angles);
void VectorMatrix(const Vector &forward, matrix3x4_t &mat);
void VectorVectors(const Vector &forward, Vector &right, Vector &up);
void SetIdentityMatrix(matrix3x4_t &mat);
void SetScaleMatrix(float x, float y, float z, matrix3x4_t &dst);
void MatrixBuildRotationAboutAxis(const Vector &vAxisOfRot, float angleDegrees,
                                  matrix3x4_t &dst);

inline void SetScaleMatrix(float flScale, matrix3x4_t &dst) {
    SetScaleMatrix(flScale, flScale, flScale, dst);
}

inline void SetScaleMatrix(const Vector &scale, matrix3x4_t &dst) {
    SetScaleMatrix(scale.x, scale.y, scale.z, dst);
}

// Computes the inverse transpose
void MatrixTranspose(matrix3x4_t &mat);
void MatrixTranspose(const matrix3x4_t &src, matrix3x4_t &dst);
void MatrixInverseTranspose(const matrix3x4_t &src, matrix3x4_t &dst);

inline void PositionMatrix(const Vector &position, matrix3x4_t &mat) {
    MatrixSetColumn(position, 3, mat);
}

inline void MatrixPosition(const matrix3x4_t &matrix, Vector &position) {
    MatrixGetColumn(matrix, 3, position);
}

inline void VectorRotate(const Vector &in1, const matrix3x4_t &in2,
                         Vector &out) {
    VectorRotate(&in1.x, in2, &out.x);
}

inline void VectorIRotate(const Vector &in1, const matrix3x4_t &in2,
                          Vector &out) {
    VectorIRotate(&in1.x, in2, &out.x);
}

inline void MatrixAngles(const matrix3x4_t &matrix, QAngle &angles) {
    MatrixAngles(matrix, &angles.x);
}

inline void MatrixAngles(const matrix3x4_t &matrix, QAngle &angles,
                         Vector &position) {
    MatrixAngles(matrix, angles);
    MatrixPosition(matrix, position);
}

inline void MatrixAngles(const matrix3x4_t &matrix, RadianEuler &angles) {
    MatrixAngles(matrix, &angles.x);

    angles.Init(DEG2RAD(angles.z), DEG2RAD(angles.x), DEG2RAD(angles.y));
}

void MatrixAngles(const matrix3x4_t &mat, RadianEuler &angles,
                  Vector &position);

void MatrixAngles(const matrix3x4_t &mat, Quaternion &q, Vector &position);

inline int VectorCompare(const Vector &v1, const Vector &v2) {
    return v1 == v2;
}

inline void VectorTransform(const Vector &in1, const matrix3x4_t &in2,
                            Vector &out) {
    VectorTransform(&in1.x, in2, &out.x);
}

inline void VectorITransform(const Vector &in1, const matrix3x4_t &in2,
                             Vector &out) {
    VectorITransform(&in1.x, in2, &out.x);
}

/*
inline void DecomposeRotation( const matrix3x4_t &mat, Vector &out )
{
        DecomposeRotation( mat, &out.x );
}
*/

inline int BoxOnPlaneSide(const Vector &emins, const Vector &emaxs,
                          const cplane_t *plane) {
    return BoxOnPlaneSide(&emins.x, &emaxs.x, plane);
}

inline void VectorFill(Vector &a, float b) { a[0] = a[1] = a[2] = b; }

inline void VectorNegate(Vector &a) {
    a[0] = -a[0];
    a[1] = -a[1];
    a[2] = -a[2];
}

inline vec_t VectorAvg(Vector &a) { return (a[0] + a[1] + a[2]) / 3; }

//-----------------------------------------------------------------------------
// Box/plane test (slow version)
//-----------------------------------------------------------------------------
inline int FASTCALL BoxOnPlaneSide2(const Vector &emins, const Vector &emaxs,
                                    const cplane_t *p, float tolerance = 0.f) {
    Vector corners[2];

    if (p->normal[0] < 0) {
        corners[0][0] = emins[0];
        corners[1][0] = emaxs[0];
    } else {
        corners[1][0] = emins[0];
        corners[0][0] = emaxs[0];
    }

    if (p->normal[1] < 0) {
        corners[0][1] = emins[1];
        corners[1][1] = emaxs[1];
    } else {
        corners[1][1] = emins[1];
        corners[0][1] = emaxs[1];
    }

    if (p->normal[2] < 0) {
        corners[0][2] = emins[2];
        corners[1][2] = emaxs[2];
    } else {
        corners[1][2] = emins[2];
        corners[0][2] = emaxs[2];
    }

    int sides = 0;

    float dist1 = DotProduct(p->normal, corners[0]) - p->dist;
    if (dist1 >= tolerance) sides = 1;

    float dist2 = DotProduct(p->normal, corners[1]) - p->dist;
    if (dist2 < -tolerance) sides |= 2;

    return sides;
}

//-----------------------------------------------------------------------------
// Helpers for bounding box construction
//-----------------------------------------------------------------------------

void ClearBounds(Vector &mins, Vector &maxs);
void AddPointToBounds(const Vector &v, Vector &mins, Vector &maxs);

//
// COLORSPACE/GAMMA CONVERSION STUFF
//
void BuildGammaTable(float gamma, float texGamma, float brightness,
                     int overbright);

// convert texture to linear 0..1 value
inline float TexLightToLinear(int c, int exponent) {
    extern float power2_n[256];
    Assert(exponent >= -128 && exponent <= 127);
    return (float)c * power2_n[exponent + 128];
}

// convert texture to linear 0..1 value
int LinearToTexture(float f);
// converts 0..1 linear value to screen gamma (0..255)
int LinearToScreenGamma(float f);
float TextureToLinear(int c);

// compressed color format
struct ColorRGBExp32 {
    byte r, g, b;
    signed char exponent;
};

void ColorRGBExp32ToVector(const ColorRGBExp32 &in, Vector &out);
void VectorToColorRGBExp32(const Vector &v, ColorRGBExp32 &c);

// solve for "x" where "a x^2 + b x + c = 0", return true if solution exists
bool SolveQuadratic(float a, float b, float c, float &root1, float &root2);

// solves for "a, b, c" where "a x^2 + b x + c = y", return true if solution
// exists
bool SolveInverseQuadratic(float x1, float y1, float x2, float y2, float x3,
                           float y3, float &a, float &b, float &c);

// solves for a,b,c specified as above, except that it always creates a
// monotonically increasing or decreasing curve if the data is monotonically
// increasing or decreasing. In order to enforce the monoticity condition, it is
// possible that the resulting quadratic will only approximate the data instead
// of interpolating it. This code is not especially fast.
bool SolveInverseQuadraticMonotonic(float x1, float y1, float x2, float y2,
                                    float x3, float y3, float &a, float &b,
                                    float &c);

// solves for "a, b, c" where "1/(a x^2 + b x + c ) = y", return true if
// solution exists
bool SolveInverseReciprocalQuadratic(float x1, float y1, float x2, float y2,
                                     float x3, float y3, float &a, float &b,
                                     float &c);

// rotate a vector around the Z axis (YAW)
void VectorYawRotate(const Vector &in, float flYaw, Vector &out);

// Bias takes an X value between 0 and 1 and returns another value between 0 and
// 1 The curve is biased towards 0 or 1 based on biasAmt, which is between 0
// and 1. Lower values of biasAmt bias the curve towards 0 and higher values
// bias it towards 1.
//
// For example, with biasAmt = 0.2, the curve looks like this:
//
// 1
// |				  *
// |				  *
// |			     *
// |			   **
// |			 **
// |	  	 ****
// |*********
// |___________________
// 0                   1
//
//
// With biasAmt = 0.8, the curve looks like this:
//
// 1
// | 	**************
// |  **
// | *
// | *
// |*
// |*
// |*
// |___________________
// 0                   1
//
// With a biasAmt of 0.5, Bias returns X.
float Bias(float x, float biasAmt);

// Gain is similar to Bias, but biasAmt biases towards or away from 0.5.
// Lower bias values bias towards 0.5 and higher bias values bias away from it.
//
// For example, with biasAmt = 0.2, the curve looks like this:
//
// 1
// | 				  *
// | 				 *
// | 				**
// |  ***************
// | **
// | *
// |*
// |___________________
// 0                   1
//
//
// With biasAmt = 0.8, the curve looks like this:
//
// 1
// |  		    *****
// |  		 ***
// |  		*
// | 		*
// | 		*
// |   	 ***
// |*****
// |___________________
// 0                   1
float Gain(float x, float biasAmt);

// SmoothCurve maps a 0-1 value into another 0-1 value based on a cosine wave
// where the derivatives of the function at 0 and 1 (and 0.5) are 0. This is
// useful for any fadein/fadeout effect where it should start and end smoothly.
//
// The curve looks like this:
//
// 1
// |  		**
// | 	   *  *
// | 	  *	   *
// | 	  *	   *
// | 	 *		*
// |   **		 **
// |***			   ***
// |___________________
// 0                   1
//
float SmoothCurve(float x);

// This works like SmoothCurve, with two changes:
//
// 1. Instead of the curve peaking at 0.5, it will peak at flPeakPos.
//    (So if you specify flPeakPos=0.2, then the peak will slide to the left).
//
// 2. flPeakSharpness is a 0-1 value controlling the sharpness of the peak.
//    Low values blunt the peak and high values sharpen the peak.
float SmoothCurve_Tweak(float x, float flPeakPos = 0.5,
                        float flPeakSharpness = 0.5);

// float ExponentialDecay( float halflife, float dt );
// float ExponentialDecay( float decayTo, float decayTime, float dt );

// halflife is time for value to reach 50%
inline float ExponentialDecay(float halflife, float dt) {
    // log(0.5) == -0.69314718055994530941723212145818
    return expf(-0.69314718f / halflife * dt);
}

// decayTo is factor the value should decay to in decayTime
inline float ExponentialDecay(float decayTo, float decayTime, float dt) {
    return expf(logf(decayTo) / decayTime * dt);
}

// Get the integrated distanced traveled
// decayTo is factor the value should decay to in decayTime
// dt is the time relative to the last velocity update
inline float ExponentialDecayIntegral(float decayTo, float decayTime,
                                      float dt) {
    return (powf(decayTo, dt / decayTime) * decayTime - decayTime) /
           logf(decayTo);
}

// hermite basis function for smooth interpolation
// Similar to Gain() above, but very cheap to call
// value should be between 0 & 1 inclusive
inline float SimpleSpline(float value) {
    float valueSquared = value * value;

    // Nice little ease-in, ease-out spline-like curve
    return (3 * valueSquared - 2 * valueSquared * value);
}

// remaps a value in [startInterval, startInterval+rangeInterval] from linear to
// spline using SimpleSpline
inline float SimpleSplineRemapVal(float val, float A, float B, float C,
                                  float D) {
    if (A == B) return val >= B ? D : C;
    float cVal = (val - A) / (B - A);
    return C + (D - C) * SimpleSpline(cVal);
}

// remaps a value in [startInterval, startInterval+rangeInterval] from linear to
// spline using SimpleSpline
inline float SimpleSplineRemapValClamped(float val, float A, float B, float C,
                                         float D) {
    if (A == B) return val >= B ? D : C;
    float cVal = (val - A) / (B - A);
    cVal = clamp(cVal, 0.0f, 1.0f);
    return C + (D - C) * SimpleSpline(cVal);
}

FORCEINLINE int RoundFloatToInt(float f) {
#if defined(__i386__) || defined(_M_IX86) || defined(PLATFORM_WINDOWS_PC64) || \
    defined(__x86_64__)
    return _mm_cvtss_si32(_mm_load_ss(&f));
#elif defined(_X360)
#ifdef Assert
    Assert(IsFPUControlWordSet());
#endif
    union {
        double flResult;
        int pResult[2];
    };
    flResult = __fctiw(f);
    return pResult[1];
#else
#error Unknown architecture
#endif
}

FORCEINLINE unsigned char RoundFloatToByte(float f) {
    int nResult = RoundFloatToInt(f);
#ifdef Assert
    Assert((nResult & ~0xFF) == 0);
#endif
    return (unsigned char)nResult;
}

FORCEINLINE unsigned long RoundFloatToUnsignedLong(float f) {
#if defined(_X360)
#ifdef Assert
    Assert(IsFPUControlWordSet());
#endif
    union {
        double flResult;
        int pIntResult[2];
        unsigned long pResult[2];
    };
    flResult = __fctiw(f);
    Assert(pIntResult[1] >= 0);
    return pResult[1];
#else  // !X360

#if defined(PLATFORM_WINDOWS_PC64)
    uint nRet = (uint)f;
    if (nRet & 1) {
        if ((f - floor(f) >= 0.5)) {
            nRet++;
        }
    } else {
        if ((f - floor(f) > 0.5)) {
            nRet++;
        }
    }
    return nRet;
#else  // PLATFORM_WINDOWS_PC64
    unsigned char nResult[8];

#if defined(_WIN32)
#if defined(__GNUG__)
    __asm __volatile__("fistpl %0;" : "=m"(nResult) : "t"(f) : "st");
#else
    __asm
    {
				fld f
				fistp       qword ptr nResult
    }
#endif
#elif POSIX
    __asm __volatile__("fistpl %0;" : "=m"(nResult) : "t"(f) : "st");
#endif

    return *((unsigned long *)nResult);
#endif  // PLATFORM_WINDOWS_PC64
#endif  // !X360
}

FORCEINLINE bool IsIntegralValue(float flValue, float flTolerance = 0.001f) {
    return fabs(RoundFloatToInt(flValue) - flValue) < flTolerance;
}

// Fast, accurate ftol:
FORCEINLINE int Float2Int(float a) {
#if defined(_X360)
    union {
        double flResult;
        int pResult[2];
    };
    flResult = __fctiwz(a);
    return pResult[1];
#else  // !X360
       // Rely on compiler to generate CVTTSS2SI on x86
    return (int)a;
#endif
}

// Over 15x faster than: (int)floor(value)
inline int Floor2Int(float a) {
    int RetVal;
#if defined(__i386__)
    // Convert to int and back, compare, subtract one if too big
    __m128 a128 = _mm_set_ss(a);
    RetVal = _mm_cvtss_si32(a128);
    __m128 rounded128 = _mm_cvt_si2ss(_mm_setzero_ps(), RetVal);
    RetVal -= _mm_comigt_ss(rounded128, a128);
#else
    RetVal = static_cast<int>(floor(a));
#endif
    return RetVal;
}

//-----------------------------------------------------------------------------
// Fast color conversion from float to unsigned char
//-----------------------------------------------------------------------------
FORCEINLINE unsigned int FastFToC(float c) {
#if defined(__i386__)
    // IEEE float bit manipulation works for values between [0, 1<<23)
    union {
        float f;
        int i;
    } convert = {c * 255.0f + (float)(1 << 23)};
    return convert.i & 255;
#else
    // consoles CPUs suffer from load-hit-store penalty
    return Float2Int(c * 255.0f);
#endif
}

//-----------------------------------------------------------------------------
// Fast conversion from float to integer with magnitude less than 2**22
//-----------------------------------------------------------------------------
FORCEINLINE int FastFloatToSmallInt(float c) {
#if defined(__i386__)
    // IEEE float bit manipulation works for values between [-1<<22, 1<<22)
    union {
        float f;
        int i;
    } convert = {c + (float)(3 << 22)};
    return (convert.i & ((1 << 23) - 1)) - (1 << 22);
#else
    // consoles CPUs suffer from load-hit-store penalty
    return Float2Int(c);
#endif
}

//-----------------------------------------------------------------------------
// Purpose: Bound input float to .001 (millisecond) boundary
// Input  : in -
// Output : inline float
//-----------------------------------------------------------------------------
inline float ClampToMsec(float in) {
    int msec = Floor2Int(in * 1000.0f + 0.5f);
    return 0.001f * msec;
}

// Over 15x faster than: (int)ceil(value)
inline int Ceil2Int(float a) {
    int RetVal;
#if defined(__i386__)
    // Convert to int and back, compare, add one if too small
    __m128 a128 = _mm_load_ss(&a);
    RetVal = _mm_cvtss_si32(a128);
    __m128 rounded128 = _mm_cvt_si2ss(_mm_setzero_ps(), RetVal);
    RetVal += _mm_comilt_ss(rounded128, a128);
#else
    RetVal = static_cast<int>(ceil(a));
#endif
    return RetVal;
}

// Regular signed area of triangle
#define TriArea2D(A, B, C) \
    (0.5f * ((B.x - A.x) * (C.y - A.y) - (B.y - A.y) * (C.x - A.x)))

// This version doesn't premultiply by 0.5f, so it's the area of the rectangle
// instead
#define TriArea2DTimesTwo(A, B, C) \
    (((B.x - A.x) * (C.y - A.y) - (B.y - A.y) * (C.x - A.x)))

// Get the barycentric coordinates of "pt" in triangle [A,B,C].
inline void GetBarycentricCoords2D(Vector2D const &A, Vector2D const &B,
                                   Vector2D const &C, Vector2D const &pt,
                                   float bcCoords[3]) {
    // Note, because to top and bottom are both x2, the issue washes out in the
    // composite
    float invTriArea = 1.0f / TriArea2DTimesTwo(A, B, C);

    // NOTE: We assume here that the lightmap coordinate vertices go
    // counterclockwise. If not, TriArea2D() is negated so this works out right.
    bcCoords[0] = TriArea2DTimesTwo(B, C, pt) * invTriArea;
    bcCoords[1] = TriArea2DTimesTwo(C, A, pt) * invTriArea;
    bcCoords[2] = TriArea2DTimesTwo(A, B, pt) * invTriArea;
}

// Return true of the sphere might touch the box (the sphere is actually treated
// like a box itself, so this may return true if the sphere's bounding box
// touches a corner of the box but the sphere itself doesn't).
inline bool QuickBoxSphereTest(const Vector &vOrigin, float flRadius,
                               const Vector &bbMin, const Vector &bbMax) {
    return vOrigin.x - flRadius < bbMax.x && vOrigin.x + flRadius > bbMin.x &&
           vOrigin.y - flRadius < bbMax.y && vOrigin.y + flRadius > bbMin.y &&
           vOrigin.z - flRadius < bbMax.z && vOrigin.z + flRadius > bbMin.z;
}

// Return true of the boxes intersect (but not if they just touch).
inline bool QuickBoxIntersectTest(const Vector &vBox1Min,
                                  const Vector &vBox1Max,
                                  const Vector &vBox2Min,
                                  const Vector &vBox2Max) {
    return vBox1Min.x < vBox2Max.x && vBox1Max.x > vBox2Min.x &&
           vBox1Min.y < vBox2Max.y && vBox1Max.y > vBox2Min.y &&
           vBox1Min.z < vBox2Max.z && vBox1Max.z > vBox2Min.z;
}

extern float GammaToLinearFullRange(float gamma);
extern float LinearToGammaFullRange(float linear);
extern float GammaToLinear(float gamma);
extern float LinearToGamma(float linear);

extern float SrgbGammaToLinear(float flSrgbGammaValue);
extern float SrgbLinearToGamma(float flLinearValue);
extern float X360GammaToLinear(float fl360GammaValue);
extern float X360LinearToGamma(float flLinearValue);
extern float SrgbGammaTo360Gamma(float flSrgbGammaValue);

// linear (0..4) to screen corrected vertex space (0..1?)
FORCEINLINE float LinearToVertexLight(float f) {
    extern float lineartovertex[4096];

    // Gotta clamp before the multiply; could overflow...
    // assume 0..4 range
    int i = RoundFloatToInt(f * 1024.f);

    // Presumably the comman case will be not to clamp, so check that first:
    if ((unsigned)i > 4095) {
        if (i < 0)
            i = 0;  // Compare to zero instead of 4095 to save 4 bytes in the
                    // instruction stream
        else
            i = 4095;
    }

    return lineartovertex[i];
}

FORCEINLINE unsigned char LinearToLightmap(float f) {
    extern unsigned char lineartolightmap[4096];

    // Gotta clamp before the multiply; could overflow...
    int i = RoundFloatToInt(f * 1024.f);  // assume 0..4 range

    // Presumably the comman case will be not to clamp, so check that first:
    if ((unsigned)i > 4095) {
        if (i < 0)
            i = 0;  // Compare to zero instead of 4095 to save 4 bytes in the
                    // instruction stream
        else
            i = 4095;
    }

    return lineartolightmap[i];
}

FORCEINLINE void ColorClamp(Vector &color) {
    float maxc = max(color.x, max(color.y, color.z));
    if (maxc > 1.0f) {
        float ooMax = 1.0f / maxc;
        color.x *= ooMax;
        color.y *= ooMax;
        color.z *= ooMax;
    }

    if (color[0] < 0.f) color[0] = 0.f;
    if (color[1] < 0.f) color[1] = 0.f;
    if (color[2] < 0.f) color[2] = 0.f;
}

inline void ColorClampTruncate(Vector &color) {
    if (color[0] > 1.0f)
        color[0] = 1.0f;
    else if (color[0] < 0.0f)
        color[0] = 0.0f;
    if (color[1] > 1.0f)
        color[1] = 1.0f;
    else if (color[1] < 0.0f)
        color[1] = 0.0f;
    if (color[2] > 1.0f)
        color[2] = 1.0f;
    else if (color[2] < 0.0f)
        color[2] = 0.0f;
}

// Interpolate a Catmull-Rom spline.
// t is a [0,1] value and interpolates a curve between p2 and p3.
void Catmull_Rom_Spline(const Vector &p1, const Vector &p2, const Vector &p3,
                        const Vector &p4, float t, Vector &output);

// Interpolate a Catmull-Rom spline.
// Returns the tangent of the point at t of the spline
void Catmull_Rom_Spline_Tangent(const Vector &p1, const Vector &p2,
                                const Vector &p3, const Vector &p4, float t,
                                Vector &output);

// area under the curve [0..t]
void Catmull_Rom_Spline_Integral(const Vector &p1, const Vector &p2,
                                 const Vector &p3, const Vector &p4, float t,
                                 Vector &output);

// area under the curve [0..1]
void Catmull_Rom_Spline_Integral(const Vector &p1, const Vector &p2,
                                 const Vector &p3, const Vector &p4,
                                 Vector &output);

// Interpolate a Catmull-Rom spline.
// Normalize p2->p1 and p3->p4 to be the same length as p2->p3
void Catmull_Rom_Spline_Normalize(const Vector &p1, const Vector &p2,
                                  const Vector &p3, const Vector &p4, float t,
                                  Vector &output);

// area under the curve [0..t]
// Normalize p2->p1 and p3->p4 to be the same length as p2->p3
void Catmull_Rom_Spline_Integral_Normalize(const Vector &p1, const Vector &p2,
                                           const Vector &p3, const Vector &p4,
                                           float t, Vector &output);

// Interpolate a Catmull-Rom spline.
// Normalize p2.x->p1.x and p3.x->p4.x to be the same length as p2.x->p3.x
void Catmull_Rom_Spline_NormalizeX(const Vector &p1, const Vector &p2,
                                   const Vector &p3, const Vector &p4, float t,
                                   Vector &output);

// area under the curve [0..t]
void Catmull_Rom_Spline_NormalizeX(const Vector &p1, const Vector &p2,
                                   const Vector &p3, const Vector &p4, float t,
                                   Vector &output);

// Interpolate a Hermite spline.
// t is a [0,1] value and interpolates a curve between p1 and p2 with the deltas
// d1 and d2.
void Hermite_Spline(const Vector &p1, const Vector &p2, const Vector &d1,
                    const Vector &d2, float t, Vector &output);

float Hermite_Spline(float p1, float p2, float d1, float d2, float t);

// t is a [0,1] value and interpolates a curve between p1 and p2 with the slopes
// p0->p1 and p1->p2
void Hermite_Spline(const Vector &p0, const Vector &p1, const Vector &p2,
                    float t, Vector &output);

float Hermite_Spline(float p0, float p1, float p2, float t);

void Hermite_SplineBasis(float t, float basis[]);

void Hermite_Spline(const Quaternion &q0, const Quaternion &q1,
                    const Quaternion &q2, float t, Quaternion &output);

// See http://en.wikipedia.org/wiki/Kochanek-Bartels_curves
//
// Tension:  -1 = Round -> 1 = Tight
// Bias:     -1 = Pre-shoot (bias left) -> 1 = Post-shoot (bias right)
// Continuity: -1 = Box corners -> 1 = Inverted corners
//
// If T=B=C=0 it's the same matrix as Catmull-Rom.
// If T=1 & B=C=0 it's the same as Cubic.
// If T=B=0 & C=-1 it's just linear interpolation
//
// See
// http://news.povray.org/povray.binaries.tutorials/attachment/%3CXns91B880592482seed7@povray.org%3E/Splines.bas.txt
// for example code and descriptions of various spline types...
//
void Kochanek_Bartels_Spline(float tension, float bias, float continuity,
                             const Vector &p1, const Vector &p2,
                             const Vector &p3, const Vector &p4, float t,
                             Vector &output);

void Kochanek_Bartels_Spline_NormalizeX(float tension, float bias,
                                        float continuity, const Vector &p1,
                                        const Vector &p2, const Vector &p3,
                                        const Vector &p4, float t,
                                        Vector &output);

// See link at Kochanek_Bartels_Spline for info on the basis matrix used
void Cubic_Spline(const Vector &p1, const Vector &p2, const Vector &p3,
                  const Vector &p4, float t, Vector &output);

void Cubic_Spline_NormalizeX(const Vector &p1, const Vector &p2,
                             const Vector &p3, const Vector &p4, float t,
                             Vector &output);

// See link at Kochanek_Bartels_Spline for info on the basis matrix used
void BSpline(const Vector &p1, const Vector &p2, const Vector &p3,
             const Vector &p4, float t, Vector &output);

void BSpline_NormalizeX(const Vector &p1, const Vector &p2, const Vector &p3,
                        const Vector &p4, float t, Vector &output);

// See link at Kochanek_Bartels_Spline for info on the basis matrix used
void Parabolic_Spline(const Vector &p1, const Vector &p2, const Vector &p3,
                      const Vector &p4, float t, Vector &output);

void Parabolic_Spline_NormalizeX(const Vector &p1, const Vector &p2,
                                 const Vector &p3, const Vector &p4, float t,
                                 Vector &output);

// quintic interpolating polynomial from Perlin.
// 0->0, 1->1, smooth-in between with smooth tangents
FORCEINLINE float QuinticInterpolatingPolynomial(float t) {
    // 6t^5-15t^4+10t^3
    return t * t * t * (t * (t * 6.0 - 15.0) + 10.0);
}

// given a table of sorted tabulated positions, return the two indices and
// blendfactor to linear interpolate. Does a search. Can be used to find the
// blend value to interpolate between keyframes.
void GetInterpolationData(float const *pKnotPositions, float const *pKnotValues,
                          int nNumValuesinList, int nInterpolationRange,
                          float flPositionToInterpolateAt, bool bWrap,
                          float *pValueA, float *pValueB,
                          float *pInterpolationValue);

float RangeCompressor(float flValue, float flMin, float flMax, float flBase);

// Get the minimum distance from vOrigin to the bounding box defined by
// [mins,maxs] using voronoi regions. 0 is returned if the origin is inside the
// box.
float CalcSqrDistanceToAABB(const Vector &mins, const Vector &maxs,
                            const Vector &point);
void CalcClosestPointOnAABB(const Vector &mins, const Vector &maxs,
                            const Vector &point, Vector &closestOut);
void CalcSqrDistAndClosestPointOnAABB(const Vector &mins, const Vector &maxs,
                                      const Vector &point, Vector &closestOut,
                                      float &distSqrOut);

inline float CalcDistanceToAABB(const Vector &mins, const Vector &maxs,
                                const Vector &point) {
    float flDistSqr = CalcSqrDistanceToAABB(mins, maxs, point);
    return sqrt(flDistSqr);
}

// Get the closest point from P to the (infinite) line through vLineA and vLineB
// and calculate the shortest distance from P to the line. If you pass in a
// value for t, it will tell you the t for (A + (B-A)t) to get the closest
// point. If the closest point lies on the segment between A and B, then 0 <= t
// <= 1.
void CalcClosestPointOnLine(const Vector &P, const Vector &vLineA,
                            const Vector &vLineB, Vector &vClosest,
                            float *t = 0);
float CalcDistanceToLine(const Vector &P, const Vector &vLineA,
                         const Vector &vLineB, float *t = 0);
float CalcDistanceSqrToLine(const Vector &P, const Vector &vLineA,
                            const Vector &vLineB, float *t = 0);

// The same three functions as above, except now the line is closed between A
// and B.
void CalcClosestPointOnLineSegment(const Vector &P, const Vector &vLineA,
                                   const Vector &vLineB, Vector &vClosest,
                                   float *t = 0);
float CalcDistanceToLineSegment(const Vector &P, const Vector &vLineA,
                                const Vector &vLineB, float *t = 0);
float CalcDistanceSqrToLineSegment(const Vector &P, const Vector &vLineA,
                                   const Vector &vLineB, float *t = 0);

// A function to compute the closes line segment connnection two lines (or false
// if the lines are parallel, etc.)
bool CalcLineToLineIntersectionSegment(const Vector &p1, const Vector &p2,
                                       const Vector &p3, const Vector &p4,
                                       Vector *s1, Vector *s2, float *t1,
                                       float *t2);

// The above functions in 2D
void CalcClosestPointOnLine2D(Vector2D const &P, Vector2D const &vLineA,
                              Vector2D const &vLineB, Vector2D &vClosest,
                              float *t = 0);
float CalcDistanceToLine2D(Vector2D const &P, Vector2D const &vLineA,
                           Vector2D const &vLineB, float *t = 0);
float CalcDistanceSqrToLine2D(Vector2D const &P, Vector2D const &vLineA,
                              Vector2D const &vLineB, float *t = 0);
void CalcClosestPointOnLineSegment2D(Vector2D const &P, Vector2D const &vLineA,
                                     Vector2D const &vLineB, Vector2D &vClosest,
                                     float *t = 0);
float CalcDistanceToLineSegment2D(Vector2D const &P, Vector2D const &vLineA,
                                  Vector2D const &vLineB, float *t = 0);
float CalcDistanceSqrToLineSegment2D(Vector2D const &P, Vector2D const &vLineA,
                                     Vector2D const &vLineB, float *t = 0);

// Init the mathlib
void MathLib_Init(float gamma = 2.2f, float texGamma = 2.2f,
                  float brightness = 0.0f, int overbright = 2.0f,
                  bool bAllow3DNow = true, bool bAllowSSE = true,
                  bool bAllowSSE2 = true, bool bAllowMMX = true);
bool MathLib_3DNowEnabled(void);
bool MathLib_MMXEnabled(void);
bool MathLib_SSEEnabled(void);
bool MathLib_SSE2Enabled(void);

float Approach(float target, float value, float speed);
float ApproachAngle(float target, float value, float speed);
float AngleDiff(float destAngle, float srcAngle);
float AngleDistance(float next, float cur);
float AngleNormalize(float angle);

// ensure that 0 <= angle <= 360
float AngleNormalizePositive(float angle);

bool AnglesAreEqual(float a, float b, float tolerance = 0.0f);

void RotationDeltaAxisAngle(const QAngle &srcAngles, const QAngle &destAngles,
                            Vector &deltaAxis, float &deltaAngle);
void RotationDelta(const QAngle &srcAngles, const QAngle &destAngles,
                   QAngle *out);

void ComputeTrianglePlane(const Vector &v1, const Vector &v2, const Vector &v3,
                          Vector &normal, float &intercept);
int PolyFromPlane(Vector *outVerts, const Vector &normal, float dist,
                  float fHalfScale = 9000.0f);
int ClipPolyToPlane(Vector *inVerts, int vertCount, Vector *outVerts,
                    const Vector &normal, float dist,
                    float fOnPlaneEpsilon = 0.1f);
int ClipPolyToPlane_Precise(double *inVerts, int vertCount, double *outVerts,
                            const double *normal, double dist,
                            double fOnPlaneEpsilon = 0.1);

//-----------------------------------------------------------------------------
// Computes a reasonable tangent space for a triangle
//-----------------------------------------------------------------------------
void CalcTriangleTangentSpace(const Vector &p0, const Vector &p1,
                              const Vector &p2, const Vector2D &t0,
                              const Vector2D &t1, const Vector2D &t2,
                              Vector &sVect, Vector &tVect);

//-----------------------------------------------------------------------------
// Transforms a AABB into another space; which will inherently grow the box.
//-----------------------------------------------------------------------------
void TransformAABB(const matrix3x4_t &in1, const Vector &vecMinsIn,
                   const Vector &vecMaxsIn, Vector &vecMinsOut,
                   Vector &vecMaxsOut);

//-----------------------------------------------------------------------------
// Uses the inverse transform of in1
//-----------------------------------------------------------------------------
void ITransformAABB(const matrix3x4_t &in1, const Vector &vecMinsIn,
                    const Vector &vecMaxsIn, Vector &vecMinsOut,
                    Vector &vecMaxsOut);

//-----------------------------------------------------------------------------
// Rotates a AABB into another space; which will inherently grow the box.
// (same as TransformAABB, but doesn't take the translation into account)
//-----------------------------------------------------------------------------
void RotateAABB(const matrix3x4_t &in1, const Vector &vecMinsIn,
                const Vector &vecMaxsIn, Vector &vecMinsOut,
                Vector &vecMaxsOut);

//-----------------------------------------------------------------------------
// Uses the inverse transform of in1
//-----------------------------------------------------------------------------
void IRotateAABB(const matrix3x4_t &in1, const Vector &vecMinsIn,
                 const Vector &vecMaxsIn, Vector &vecMinsOut,
                 Vector &vecMaxsOut);

//-----------------------------------------------------------------------------
// Transform a plane
//-----------------------------------------------------------------------------
inline void MatrixTransformPlane(const matrix3x4_t &src,
                                 const cplane_t &inPlane, cplane_t &outPlane) {
    // What we want to do is the following:
    // 1) transform the normal into the new space.
    // 2) Determine a point on the old plane given by plane dist * plane normal
    // 3) Transform that point into the new space
    // 4) Plane dist = DotProduct( new normal, new point )

    // An optimized version, which works if the plane is orthogonal.
    // 1) Transform the normal into the new space
    // 2) Realize that transforming the old plane point into the new space
    // is given by [ d * n'x + Tx, d * n'y + Ty, d * n'z + Tz ]
    // where d = old plane dist, n' = transformed normal, Tn = translational
    // component of transform 3) Compute the new plane dist using the dot
    // product of the normal result of #2

    // For a correct result, this should be an inverse-transpose matrix
    // but that only matters if there are nonuniform scale or skew factors in
    // this matrix.
    VectorRotate(inPlane.normal, src, outPlane.normal);
    outPlane.dist = inPlane.dist * DotProduct(outPlane.normal, outPlane.normal);
    outPlane.dist += outPlane.normal.x * src[0][3] +
                     outPlane.normal.y * src[1][3] +
                     outPlane.normal.z * src[2][3];
}

inline void MatrixITransformPlane(const matrix3x4_t &src,
                                  const cplane_t &inPlane, cplane_t &outPlane) {
    // The trick here is that Tn = translational component of transform,
    // but for an inverse transform, Tn = - R^-1 * T
    Vector vecTranslation;
    MatrixGetColumn(src, 3, vecTranslation);

    Vector vecInvTranslation;
    VectorIRotate(vecTranslation, src, vecInvTranslation);

    VectorIRotate(inPlane.normal, src, outPlane.normal);
    outPlane.dist = inPlane.dist * DotProduct(outPlane.normal, outPlane.normal);
    outPlane.dist -= outPlane.normal.x * vecInvTranslation[0] +
                     outPlane.normal.y * vecInvTranslation[1] +
                     outPlane.normal.z * vecInvTranslation[2];
}

int CeilPow2(int in);
int FloorPow2(int in);

FORCEINLINE float *UnpackNormal_HEND3N(const unsigned int *pPackedNormal,
                                       float *pNormal) {
    int temp[3];
    temp[0] = ((*pPackedNormal >> 0L) & 0x7ff);
    if (temp[0] & 0x400) {
        temp[0] = 2048 - temp[0];
    }
    temp[1] = ((*pPackedNormal >> 11L) & 0x7ff);
    if (temp[1] & 0x400) {
        temp[1] = 2048 - temp[1];
    }
    temp[2] = ((*pPackedNormal >> 22L) & 0x3ff);
    if (temp[2] & 0x200) {
        temp[2] = 1024 - temp[2];
    }
    pNormal[0] = (float)temp[0] * 1.0f / 1023.0f;
    pNormal[1] = (float)temp[1] * 1.0f / 1023.0f;
    pNormal[2] = (float)temp[2] * 1.0f / 511.0f;
    return pNormal;
}

FORCEINLINE unsigned int *PackNormal_HEND3N(const float *pNormal,
                                            unsigned int *pPackedNormal) {
    int temp[3];

    temp[0] = Float2Int(pNormal[0] * 1023.0f);
    temp[1] = Float2Int(pNormal[1] * 1023.0f);
    temp[2] = Float2Int(pNormal[2] * 511.0f);

    // the normal is out of bounds, determine the source and fix
    // clamping would be even more of a slowdown here
    Assert(temp[0] >= -1023 && temp[0] <= 1023);
    Assert(temp[1] >= -1023 && temp[1] <= 1023);
    Assert(temp[2] >= -511 && temp[2] <= 511);

    *pPackedNormal = ((temp[2] & 0x3ff) << 22L) | ((temp[1] & 0x7ff) << 11L) |
                     ((temp[0] & 0x7ff) << 0L);
    return pPackedNormal;
}

FORCEINLINE unsigned int *PackNormal_HEND3N(float nx, float ny, float nz,
                                            unsigned int *pPackedNormal) {
    int temp[3];

    temp[0] = Float2Int(nx * 1023.0f);
    temp[1] = Float2Int(ny * 1023.0f);
    temp[2] = Float2Int(nz * 511.0f);

    // the normal is out of bounds, determine the source and fix
    // clamping would be even more of a slowdown here
    Assert(temp[0] >= -1023 && temp[0] <= 1023);
    Assert(temp[1] >= -1023 && temp[1] <= 1023);
    Assert(temp[2] >= -511 && temp[2] <= 511);

    *pPackedNormal = ((temp[2] & 0x3ff) << 22L) | ((temp[1] & 0x7ff) << 11L) |
                     ((temp[0] & 0x7ff) << 0L);
    return pPackedNormal;
}

FORCEINLINE float *UnpackNormal_SHORT2(const unsigned int *pPackedNormal,
                                       float *pNormal,
                                       bool bIsTangent = FALSE) {
    // Unpacks from Jason's 2-short format (fills in a 4th binormal-sign (+1/-1)
    // value, if this is a tangent vector)

    // FIXME: short math is slow on 360 - use ints here instead (bit-twiddle to
    // deal w/ the sign bits)
    short iX = (*pPackedNormal & 0x0000FFFF);
    short iY = (*pPackedNormal & 0xFFFF0000) >> 16;

    float zSign = +1;
    if (iX < 0) {
        zSign = -1;
        iX = -iX;
    }
    float tSign = +1;
    if (iY < 0) {
        tSign = -1;
        iY = -iY;
    }

    pNormal[0] = (iX - 16384.0f) / 16384.0f;
    pNormal[1] = (iY - 16384.0f) / 16384.0f;
    pNormal[2] =
        zSign *
        sqrtf(1.0f - (pNormal[0] * pNormal[0] + pNormal[1] * pNormal[1]));
    if (bIsTangent) {
        pNormal[3] = tSign;
    }

    return pNormal;
}

FORCEINLINE unsigned int *PackNormal_SHORT2(float nx, float ny, float nz,
                                            unsigned int *pPackedNormal,
                                            float binormalSign = +1.0f) {
    // Pack a vector (ASSUMED TO BE NORMALIZED) into Jason's 4-byte (SHORT2)
    // format. This simply reconstructs Z from X & Y. It uses the sign bits of
    // the X & Y coords to reconstruct the sign of Z and, if this is a tangent
    // vector, the sign of the binormal (this is needed because tangent/binormal
    // vectors are supposed to follow UV gradients, but shaders reconstruct the
    // binormal from the tangent and normal assuming that they form a
    // right-handed basis).

    nx += 1;  // [-1,+1] -> [0,2]
    ny += 1;
    nx *= 16384.0f;  // [ 0, 2] -> [0,32768]
    ny *= 16384.0f;

    // '0' and '32768' values are invalid encodings
    nx = max(nx, 1.0f);  // Make sure there are no zero values
    ny = max(ny, 1.0f);
    nx = min(nx, 32767.0f);  // Make sure there are no 32768 values
    ny = min(ny, 32767.0f);

    if (nz < 0.0f) nx = -nx;  // Set the sign bit for z

    ny *= binormalSign;  // Set the sign bit for the binormal (use when encoding
                         // a tangent vector)

    // FIXME: short math is slow on 360 - use ints here instead (bit-twiddle to
    // deal w/ the sign bits), also use Float2Int()
    short sX = (short)nx;  // signed short [1,32767]
    short sY = (short)ny;

    *pPackedNormal =
        (sX & 0x0000FFFF) | (sY << 16);  // NOTE: The mask is necessary (if sX
                                         // is negative and cast to an int...)

    return pPackedNormal;
}

FORCEINLINE unsigned int *PackNormal_SHORT2(const float *pNormal,
                                            unsigned int *pPackedNormal,
                                            float binormalSign = +1.0f) {
    return PackNormal_SHORT2(pNormal[0], pNormal[1], pNormal[2], pPackedNormal,
                             binormalSign);
}

// Unpacks a UBYTE4 normal (for a tangent, the result's fourth component
// receives the binormal 'sign')
FORCEINLINE float *UnpackNormal_UBYTE4(const unsigned int *pPackedNormal,
                                       float *pNormal,
                                       bool bIsTangent = FALSE) {
    unsigned char cX, cY;
    if (bIsTangent) {
        cX = *pPackedNormal >> 16;  // Unpack Z
        cY = *pPackedNormal >> 24;  // Unpack W
    } else {
        cX = *pPackedNormal >> 0;  // Unpack X
        cY = *pPackedNormal >> 8;  // Unpack Y
    }

    float x = cX - 128.0f;
    float y = cY - 128.0f;
    float z;

    float zSignBit =
        x < 0 ? 1.0f
              : 0.0f;  // z and t negative bits (like slt asm instruction)
    float tSignBit = y < 0 ? 1.0f : 0.0f;
    float zSign = -(2 * zSignBit - 1);  // z and t signs
    float tSign = -(2 * tSignBit - 1);

    x = x * zSign - zSignBit;  // 0..127
    y = y * tSign - tSignBit;
    x = x - 64;  // -64..63
    y = y - 64;

    float xSignBit =
        x < 0 ? 1.0f
              : 0.0f;  // x and y negative bits (like slt asm instruction)
    float ySignBit = y < 0 ? 1.0f : 0.0f;
    float xSign = -(2 * xSignBit - 1);  // x and y signs
    float ySign = -(2 * ySignBit - 1);

    x = (x * xSign - xSignBit) / 63.0f;  // 0..1 range
    y = (y * ySign - ySignBit) / 63.0f;
    z = 1.0f - x - y;

    float oolen = 1.0f / sqrt(x * x + y * y + z * z);  // Normalize and
    x *= oolen * xSign;                                // Recover signs
    y *= oolen * ySign;
    z *= oolen * zSign;

    pNormal[0] = x;
    pNormal[1] = y;
    pNormal[2] = z;
    if (bIsTangent) {
        pNormal[3] = tSign;
    }

    return pNormal;
}

//////////////////////////////////////////////////////////////////////////////
// See:
// http://www.oroboro.com/rafael/docserv.php/index/programming/article/unitv2
//
// UBYTE4 encoding, using per-octant projection onto x+y+z=1
// Assume input vector is already unit length
//
// binormalSign specifies 'sign' of binormal, stored in t sign bit of tangent
// (lets the shader know whether norm/tan/bin form a right-handed basis)
//
// bIsTangent is used to specify which WORD of the output to store the data
// The expected usage is to call once with the normal and once with
// the tangent and binormal sign flag, bitwise OR'ing the returned DWORDs
FORCEINLINE unsigned int *PackNormal_UBYTE4(float nx, float ny, float nz,
                                            unsigned int *pPackedNormal,
                                            bool bIsTangent = false,
                                            float binormalSign = +1.0f) {
    float xSign = nx < 0.0f ? -1.0f : 1.0f;  // -1 or 1 sign
    float ySign = ny < 0.0f ? -1.0f : 1.0f;
    float zSign = nz < 0.0f ? -1.0f : 1.0f;
    float tSign = binormalSign;
    Assert((binormalSign == +1.0f) || (binormalSign == -1.0f));

    float xSignBit = 0.5f * (1 - xSign);  // [-1,+1] -> [1,0]
    float ySignBit =
        0.5f * (1 - ySign);  // 1 is negative bit (like slt instruction)
    float zSignBit = 0.5f * (1 - zSign);
    float tSignBit = 0.5f * (1 - binormalSign);

    float absX = xSign * nx;  // 0..1 range (abs)
    float absY = ySign * ny;
    float absZ = zSign * nz;

    float xbits = absX / (absX + absY + absZ);  // Project onto x+y+z=1 plane
    float ybits = absY / (absX + absY + absZ);

    xbits *= 63;  // 0..63
    ybits *= 63;

    xbits = xbits * xSign - xSignBit;  // -64..63 range
    ybits = ybits * ySign - ySignBit;
    xbits += 64.0f;  // 0..127 range
    ybits += 64.0f;

    xbits = xbits * zSign - zSignBit;  // Negate based on z and t
    ybits = ybits * tSign - tSignBit;  // -128..127 range

    xbits += 128.0f;  // 0..255 range
    ybits += 128.0f;

    unsigned char cX = (unsigned char)xbits;
    unsigned char cY = (unsigned char)ybits;

    if (!bIsTangent)
        *pPackedNormal = (cX << 0) | (cY << 8);  // xy for normal
    else
        *pPackedNormal = (cX << 16) | (cY << 24);  // zw for tangent

    return pPackedNormal;
}

FORCEINLINE unsigned int *PackNormal_UBYTE4(const float *pNormal,
                                            unsigned int *pPackedNormal,
                                            bool bIsTangent = false,
                                            float binormalSign = +1.0f) {
    return PackNormal_UBYTE4(pNormal[0], pNormal[1], pNormal[2], pPackedNormal,
                             bIsTangent, binormalSign);
}

//-----------------------------------------------------------------------------
// Convert RGB to HSV
//-----------------------------------------------------------------------------
void RGBtoHSV(const Vector &rgb, Vector &hsv);

//-----------------------------------------------------------------------------
// Convert HSV to RGB
//-----------------------------------------------------------------------------
void HSVtoRGB(const Vector &hsv, Vector &rgb);

//-----------------------------------------------------------------------------
// Fast version of pow and log
//-----------------------------------------------------------------------------

float FastLog2(float i);          // log2( i )
float FastPow2(float i);          // 2^i
float FastPow(float a, float b);  // a^b
float FastPow10(float i);         // 10^i

//-----------------------------------------------------------------------------
// For testing float equality
//-----------------------------------------------------------------------------

inline bool CloseEnough(float a, float b, float epsilon = EQUAL_EPSILON) {
    return fabs(a - b) <= epsilon;
}

inline bool CloseEnough(const Vector &a, const Vector &b,
                        float epsilon = EQUAL_EPSILON) {
    return fabs(a.x - b.x) <= epsilon && fabs(a.y - b.y) <= epsilon &&
           fabs(a.z - b.z) <= epsilon;
}

// Fast compare
// maxUlps is the maximum error in terms of Units in the Last Place. This
// specifies how big an error we are willing to accept in terms of the value
// of the least significant digit of the floating point number�s
// representation. maxUlps can also be interpreted in terms of how many
// representable floats we are willing to accept between A and B.
// This function will allow maxUlps-1 floats between A and B.
bool AlmostEqual(float a, float b, int maxUlps = 10);

inline bool AlmostEqual(const Vector &a, const Vector &b, int maxUlps = 10) {
    return AlmostEqual(a.x, b.x, maxUlps) && AlmostEqual(a.y, b.y, maxUlps) &&
           AlmostEqual(a.z, b.z, maxUlps);
}

#endif  // MATH_BASE_H