mathlib.cpp

Go to the documentation of this file.

/*
Copyright (C) 1997-2001 Id Software, Inc.

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

*/

#include "mathlib.h"
#include "../common/common.h"
#include <algorithm>

#ifndef logf
#define logf(x) ((float)log((double)(x)))
#endif

const vec2_t vec2_origin = { 0, 0 };
const vec3_t vec3_origin = { 0, 0, 0 };
const vec4_t vec4_origin = { 0, 0, 0, 0 };
const pos3_t pos3_origin = { 0, 0, 0 };

#define RT2 0.70710678118654752440084436210485

const GridBox GridBox::EMPTY(pos3_origin, pos3_origin);

const vec4_t dvecs[PATHFINDING_DIRECTIONS] = {
    { 1,  0,  0,  0},   /* E */
    {-1,  0,  0,  0},   /* W */
    { 0,  1,  0,  0},   /* N */
    { 0, -1,  0,  0},   /* S */
    { 1,  1,  0,  0},   /* NE */
    {-1, -1,  0,  0},   /* SW */
    {-1,  1,  0,  0},   /* NW */
    { 1, -1,  0,  0},   /* SE */
    { 0,  0,  1,  0},   /* CLIMB UP */
    { 0,  0, -1,  0},   /* CLIMB DOWN */
    { 0,  0,  0, -1},   /* STAND UP */
    { 0,  0,  0,  1},   /* STAND DOWN */
    { 0,  0,  0,  0},   /* UNDEFINED OPPOSITE OF FALL DOWN */
    { 0,  0, -1,  0},   /* FALL DOWN */
    { 0,  0,  0,  0},   /* UNDEFINED */
    { 0,  0,  0,  0},   /* UNDEFINED */
    { 1,  0,  1,  0},   /* UP E (Fliers only)*/
    {-1,  0,  1,  0},   /* UP W (Fliers only) */
    { 0,  1,  1,  0},   /* UP N (Fliers only) */
    { 0, -1,  1,  0},   /* UP S (Fliers only) */
    { 1,  1,  1,  0},   /* UP NE (Fliers only) */
    {-1, -1,  1,  0},   /* UP SW (Fliers only) */
    {-1,  1,  1,  0},   /* UP NW (Fliers only) */
    { 1, -1,  1,  0},   /* UP SE (Fliers only) */
    { 1,  0,  0,  0},   /* E (Fliers only)*/
    {-1,  0,  0,  0},   /* W (Fliers only) */
    { 0,  1,  0,  0},   /* N (Fliers only) */
    { 0, -1,  0,  0},   /* S (Fliers only) */
    { 1,  1,  0,  0},   /* NE (Fliers only) */
    {-1, -1,  0,  0},   /* SW (Fliers only) */
    {-1,  1,  0,  0},   /* NW (Fliers only) */
    { 1, -1,  0,  0},   /* SE (Fliers only) */
    { 1,  0, -1,  0},   /* DOWN E (Fliers only) */
    {-1,  0, -1,  0},   /* DOWN W (Fliers only) */
    { 0,  1, -1,  0},   /* DOWN N (Fliers only) */
    { 0, -1, -1,  0},   /* DOWN S (Fliers only) */
    { 1,  1, -1,  0},   /* DOWN NE (Fliers only) */
    {-1, -1, -1,  0},   /* DOWN SW (Fliers only) */
    {-1,  1, -1,  0},   /* DOWN NW (Fliers only) */
    { 1, -1, -1,  0},   /* DOWN SE (Fliers only) */
    };

/*                                           0:E     1:W      2:N     3:S      4:NE        5:SW          6:NW         7:SE  */
const float dvecsn[CORE_DIRECTIONS][2] = { {1, 0}, {-1, 0}, {0, 1}, {0, -1}, {RT2, RT2}, {-RT2, -RT2}, {-RT2, RT2}, {RT2, -RT2} };
/*                                     0:E 1: W    2:N    3:S     4:NE   5:SW    6:NW    7:SE  */
const float directionAngles[CORE_DIRECTIONS] = { 0, 180.0f, 90.0f, 270.0f, 45.0f, 225.0f, 135.0f, 315.0f };

#define DIRECTION_EAST 0
#define DIRECTION_WEST 1
#define DIRECTION_NORTH 2
#define DIRECTION_SOUTH 3
#define DIRECTION_NORTHEAST 4
#define DIRECTION_SOUTHWEST 5
#define DIRECTION_NORTHWEST 6
#define DIRECTION_SOUTHEAST 7

const byte dvright[CORE_DIRECTIONS] = { DIRECTION_SOUTHEAST,
        DIRECTION_NORTHWEST, DIRECTION_NORTHEAST, DIRECTION_SOUTHWEST,
        DIRECTION_EAST, DIRECTION_WEST, DIRECTION_NORTH, DIRECTION_SOUTH };
const byte dvleft[CORE_DIRECTIONS] = { DIRECTION_NORTHEAST, DIRECTION_SOUTHWEST,
        DIRECTION_NORTHWEST, DIRECTION_SOUTHEAST, DIRECTION_NORTH,
        DIRECTION_SOUTH, DIRECTION_WEST, DIRECTION_EAST };


int AngleToDir (int angle)
{
    angle += 22;
    /* set angle between 0 <= angle < 360 */
    angle %= 360;
    /* next step is because the result of angle %= 360 when angle is negative depends of the compiler
     * (it can be between -360 < angle <= 0 or 0 <= angle < 360) */
    if (angle < 0)
        angle += 360;

    /* get an integer quotient */
    angle /= 45;

    if (angle >= 0 && angle < CORE_DIRECTIONS) {
        static const int anglesToDV[8] = {0, 4, 2, 6, 1, 5, 3, 7};
        return anglesToDV[angle];
    }

    /* This is the default for unknown values. */
    Com_Printf("Error in AngleToDV: shouldn't have reached this line\n");
    return 0;
}

vec_t Q_rint (const vec_t in)
{
    /* round x down to the nearest integer */
    return floor(in + 0.5);
}

double GetDistanceOnGlobe (const vec2_t pos1, const vec2_t pos2)
{
    /* convert into rad */
    const double latitude1 = pos1[1] * torad;
    const double latitude2 = pos2[1] * torad;
    const double deltaLongitude = (pos1[0] - pos2[0]) * torad;
    double distance;

    distance = cos(latitude1) * cos(latitude2) * cos(deltaLongitude) + sin(latitude1) * sin(latitude2);
    distance = std::min(std::max(-1.0, distance), 1.0);
    distance = acos(distance) * todeg;

    assert(distance >= 0.0);
    return distance;
}

vec_t ColorNormalize (const vec3_t in, vec3_t out)
{
    float max;

    /* find the brightest component */
    max = in[0];
    if (in[1] > max)
        max = in[1];
    if (in[2] > max)
        max = in[2];

    /* avoid FPE */
    if (EQUAL(max, 0.0f)) {
        VectorClear(out);
        return 0;
    }

    VectorScale(in, 1.0f / max, out);

    return max;
}

bool VectorNearer (const vec3_t v1, const vec3_t v2, const vec3_t comp)
{
    vec3_t d1, d2;

    VectorSubtract(comp, v1, d1);
    VectorSubtract(comp, v2, d2);

    return VectorLength(d1) < VectorLength(d2);
}

vec_t VectorNormalize2 (const vec3_t v, vec3_t out)
{
    float length;

    length = DotProduct(v, v);
    length = sqrt(length);      
    if (!EQUAL(length, 0.0f)) {
        const float ilength = 1.0f / length;
        out[0] = v[0] * ilength;
        out[1] = v[1] * ilength;
        out[2] = v[2] * ilength;
    }

    return length;
}

void VectorMA (const vec3_t veca, const float scale, const vec3_t vecb, vec3_t outVector)
{
    outVector[0] = veca[0] + scale * vecb[0];
    outVector[1] = veca[1] + scale * vecb[1];
    outVector[2] = veca[2] + scale * vecb[2];
}

void VectorClampMA (vec3_t veca, float scale, const vec3_t vecb, vec3_t vecc)
{
    int i;

    /* clamp veca to bounds */
    for (i = 0; i < 3; i++)
        if (veca[i] > 4094.0)
            veca[i] = 4094.0;
        else if (veca[i] < -4094.0)
            veca[i] = -4094.0;

    /* rescale to fit */
    for (i = 0; i < 3; i++) {
        const float test = veca[i] + scale * vecb[i];
        if (test < -4095.0f) {
            const float newScale = (-4094.0 - veca[i]) / vecb[i];
            if (fabs(newScale) < fabs(scale))
                scale = newScale;
        } else if (test > 4095.0f) {
            const float newScale = (4094.0 - veca[i]) / vecb[i];
            if (fabs(newScale) < fabs(scale))
                scale = newScale;
        }
    }

    /* use rescaled scale */
    VectorMA(veca, scale, vecb, vecc);
}

void MatrixMultiply (const vec3_t a[3], const vec3_t b[3], vec3_t c[3])
{
    c[0][0] = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2];
    c[0][1] = a[0][1] * b[0][0] + a[1][1] * b[0][1] + a[2][1] * b[0][2];
    c[0][2] = a[0][2] * b[0][0] + a[1][2] * b[0][1] + a[2][2] * b[0][2];

    c[1][0] = a[0][0] * b[1][0] + a[1][0] * b[1][1] + a[2][0] * b[1][2];
    c[1][1] = a[0][1] * b[1][0] + a[1][1] * b[1][1] + a[2][1] * b[1][2];
    c[1][2] = a[0][2] * b[1][0] + a[1][2] * b[1][1] + a[2][2] * b[1][2];

    c[2][0] = a[0][0] * b[2][0] + a[1][0] * b[2][1] + a[2][0] * b[2][2];
    c[2][1] = a[0][1] * b[2][0] + a[1][1] * b[2][1] + a[2][1] * b[2][2];
    c[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
}

void GLMatrixAssemble (const vec3_t origin, const vec3_t angles, float* matrix)
{
    /* fill in edge values */
    matrix[3] = matrix[7] = matrix[11] = 0.0;
    matrix[15] = 1.0;

    /* add rotation */
    AngleVectors(angles, &matrix[0], &matrix[4], &matrix[8]);
    /* flip an axis */
    VectorInverse(&matrix[4]);

    /* add translation */
    matrix[12] = origin[0];
    matrix[13] = origin[1];
    matrix[14] = origin[2];
}

void GLMatrixMultiply (const float a[16], const float b[16], float c[16])
{
    for (int j = 0; j < 4; j++) {
        int k = j * 4;
        for (int i = 0; i < 4; i++)
            c[i + k] = a[i] * b[k] + a[i + 4] * b[k + 1] + a[i + 8] * b[k + 2] + a[i + 12] * b[k + 3];
    }
}

void GLVectorTransform (const float m[16], const vec4_t in, vec4_t out)
{
    for (int i = 0; i < 4; i++)
        out[i] = m[i] * in[0] + m[i + 4] * in[1] + m[i + 8] * in[2] + m[i + 12] * in[3];
}

void GLPositionTransform (const float m[16], const vec3_t in, vec3_t out)
{
    for (int i = 0; i < 3; i++)
        out[i] = m[i] * in[0] + m[i + 4] * in[1] + m[i + 8] * in[2] + m[i + 12];
}

void VectorRotate (vec3_t m[3], const vec3_t va, vec3_t vb)
{
    vb[0] = m[0][0] * va[0] + m[1][0] * va[1] + m[2][0] * va[2];
    vb[1] = m[0][1] * va[0] + m[1][1] * va[1] + m[2][1] * va[2];
    vb[2] = m[0][2] * va[0] + m[1][2] * va[1] + m[2][2] * va[2];
}

int VectorCompareEps (const vec3_t v1, const vec3_t v2, float epsilon)
{
    vec3_t d;

    VectorSubtract(v1, v2, d);
    d[0] = fabs(d[0]);
    d[1] = fabs(d[1]);
    d[2] = fabs(d[2]);

    if (d[0] > epsilon || d[1] > epsilon || d[2] > epsilon)
        return 0;

    return 1;
}

vec_t VectorLength (const vec3_t v)
{
    return sqrtf(DotProduct(v, v));
}

void VectorMix (const vec3_t v1, const vec3_t v2, float mix, vec3_t out)
{
    const float number = 1.0 - mix;

    out[0] = v1[0] * number + v2[0] * mix;
    out[1] = v1[1] * number + v2[1] * mix;
    out[2] = v1[2] * number + v2[2] * mix;
}

void VectorInverse (vec3_t v)
{
    v[0] = -v[0];
    v[1] = -v[1];
    v[2] = -v[2];
}

void VectorMidpoint (const vec3_t point1, const vec3_t point2, vec3_t midpoint)
{
    VectorAdd(point1, point2, midpoint);
    VectorScale(midpoint, 0.5f, midpoint);
}

float VectorAngleBetween (const vec3_t vec1, const vec3_t vec2)
{
    const float dot = DotProduct(vec1, vec2);
    const float angle = acos(dot);
    return angle;
}


int Q_log2 (int val)
{
    int answer = 0;

    while (val >>= 1)
        answer++;
    return answer;
}

float frand (void)
{
    return (rand() & 32767) * (1.0 / 32767);
}


float crand (void)
{
    return (rand() & 32767) * (2.0 / 32767) - 1;
}

void gaussrand (float* gauss1, float* gauss2)
{
    float x1, x2, w, tmp;

    do {
        x1 = crand();
        x2 = crand();
        w = x1 * x1 + x2 * x2;
    } while (w >= 1.0);

    tmp = -2 * logf(w);
    w = sqrt(tmp / w);
    *gauss1 = x1 * w;
    *gauss2 = x2 * w;
}
void CalculateMinsMaxs (const vec3_t angles, const AABB& relBox, const vec3_t origin, AABB& absBox)
{
    /* expand for rotation */
    if (VectorNotEmpty(angles)) {
        vec3_t minVec, maxVec, tmpMinVec, tmpMaxVec;
        vec3_t centerVec, halfVec, newCenterVec, newHalfVec;
        vec3_t m[3];

        /* Find the center of the extents. */
        relBox.getCenter(centerVec);

        /* Find the half height and half width of the extents. */
        VectorSubtract(relBox.maxs, centerVec, halfVec);

        /* Rotate the center about the origin. */
        VectorCreateRotationMatrix(angles, m);
        VectorRotate(m, centerVec, newCenterVec);
        VectorRotate(m, halfVec, newHalfVec);

        /* Set minVec and maxVec to bound around newCenterVec at halfVec size. */
        VectorSubtract(newCenterVec, newHalfVec, tmpMinVec);
        VectorAdd(newCenterVec, newHalfVec, tmpMaxVec);

        /* rotation may have changed min and max of the box, so adjust it */
        minVec[0] = std::min(tmpMinVec[0], tmpMaxVec[0]);
        minVec[1] = std::min(tmpMinVec[1], tmpMaxVec[1]);
        minVec[2] = std::min(tmpMinVec[2], tmpMaxVec[2]);
        maxVec[0] = std::max(tmpMinVec[0], tmpMaxVec[0]);
        maxVec[1] = std::max(tmpMinVec[1], tmpMaxVec[1]);
        maxVec[2] = std::max(tmpMinVec[2], tmpMaxVec[2]);

        /* Adjust the absolute mins/maxs */
        absBox.set(minVec, maxVec);
    } else {  /* normal */
        absBox.set(relBox);
    }
    absBox.shift(origin);
}

void VectorCreateRotationMatrix (const vec3_t angles, vec3_t matrix[3])
{
    AngleVectors(angles, matrix[0], matrix[1], matrix[2]);
    VectorInverse(matrix[1]);
}

void VectorRotatePoint (vec3_t point, vec3_t matrix[3])
{
    vec3_t tvec;

    VectorCopy(point, tvec);

    point[0] = DotProduct(matrix[0], tvec);
    point[1] = DotProduct(matrix[1], tvec);
    point[2] = DotProduct(matrix[2], tvec);
}

void AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
{
    const float anglePitch = angles[PITCH] * torad;
    const float sp = sin(anglePitch);
    const float cp = cos(anglePitch);
    const float angleYaw = angles[YAW] * torad;
    const float sy = sin(angleYaw);
    const float cy = cos(angleYaw);
    const float angleRoll = angles[ROLL] * torad;
    const float sr = sin(angleRoll);
    const float cr = cos(angleRoll);

    if (forward) {
        forward[0] = cp * cy;
        forward[1] = cp * sy;
        forward[2] = -sp;
    }
    if (right) {
        right[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
        right[1] = (-1 * sr * sp * sy + -1 * cr * cy);
        right[2] = -1 * sr * cp;
    }
    if (up) {
        up[0] = (cr * sp * cy + -sr * -sy);
        up[1] = (cr * sp * sy + -sr * cy);
        up[2] = cr * cp;
    }
}

bool FrustumVis (const vec3_t origin, int dir, const vec3_t point)
{
    /* view frustum check */
    vec3_t delta;
    byte dv;

    delta[0] = point[0] - origin[0];
    delta[1] = point[1] - origin[1];
    delta[2] = 0;
    VectorNormalizeFast(delta);
    dv = dir & (DIRECTIONS - 1);

    /* test 120 frustum (cos 60 = 0.5) */
    if ((delta[0] * dvecsn[dv][0] + delta[1] * dvecsn[dv][1]) < 0.5)
        return false;

    return true;
}

static inline void ProjectPointOnPlane (vec3_t dst, const vec3_t point, const vec3_t normal)
{
    float distance; 
#if 0
    vec3_t n;
    float inv_denom;
    /* I added a sqrt there, otherwise this function does not work for unnormalized vector (13052007 Kracken) */
    /* old line was inv_denom = 1.0F / DotProduct(normal, normal); */
    inv_denom = 1.0F / sqrt(DotProduct(normal, normal));
#endif

    distance = DotProduct(normal, point);
#if 0
    n[0] = normal[0] * inv_denom;
    n[1] = normal[1] * inv_denom;
    n[2] = normal[2] * inv_denom;
#endif

    dst[0] = point[0] - distance * normal[0];
    dst[1] = point[1] - distance * normal[1];
    dst[2] = point[2] - distance * normal[2];
}

static inline float Q_rsqrtApprox (const float number)
{
    union
    {
        float f;
        int i;
    } t;
    float y;
    float x2;
    const float threehalfs = 1.5F;

    x2 = number * 0.5F;
    t.f = number;
    /* what the fuck? */
    t.i = 0x5f3759df - (t.i >> 1);
    y = t.f;
    /* 1st iteration */
    y = y * (threehalfs - (x2 * y * y));
    /* 2nd iteration */
    y = y * (threehalfs - (x2 * y * y));
    return y;
}

vec_t VectorNormalize (vec3_t v)
{
    const float length = sqrt(DotProduct(v, v));
    if (length) {
        const float ilength = 1.0 / length;
        v[0] *= ilength;
        v[1] *= ilength;
        v[2] *= ilength;
    }

    return length;
}

void VectorNormalizeFast (vec3_t v)
{
    const float ilength = Q_rsqrtApprox(DotProduct(v, v));
    v[0] *= ilength;
    v[1] *= ilength;
    v[2] *= ilength;
}

void PerpendicularVector (vec3_t dst, const vec3_t src)
{
    int pos;
    int i;
    float minelem = 1.0F;
    vec3_t tempvec;

    /* find the smallest magnitude axially aligned vector */
    for (pos = 0, i = 0; i < 3; i++) {
        const float a = fabs(src[i]);
        if (a < minelem) {
            pos = i;
            minelem = a;
        }
    }
    tempvec[0] = tempvec[1] = tempvec[2] = 0.0F;
    tempvec[pos] = 1.0F;

    /* project the point onto the plane defined by src */
    ProjectPointOnPlane(dst, tempvec, src);

    /* normalize the result */
    VectorNormalizeFast(dst);
}

void CrossProduct (const vec3_t v1, const vec3_t v2, vec3_t cross)
{
    cross[0] = v1[1] * v2[2] - v1[2] * v2[1];
    cross[1] = v1[2] * v2[0] - v1[0] * v2[2];
    cross[2] = v1[0] * v2[1] - v1[1] * v2[0];
}

static inline void R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
{
    out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] + in1[0][2] * in2[2][0];
    out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] + in1[0][2] * in2[2][1];
    out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] + in1[0][2] * in2[2][2];
    out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] + in1[1][2] * in2[2][0];
    out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] + in1[1][2] * in2[2][1];
    out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] + in1[1][2] * in2[2][2];
    out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] + in1[2][2] * in2[2][0];
    out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] + in1[2][2] * in2[2][1];
    out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] + in1[2][2] * in2[2][2];
}

void RotatePointAroundVector (vec3_t dst, const vec3_t dir, const vec3_t point, float degrees)
{
    float m[3][3];
    float im[3][3];
    float zrot[3][3];
    float tmpmat[3][3];
    float rot[3][3];
    vec3_t vr, vup, vf;

    vf[0] = dir[0];
    vf[1] = dir[1];
    vf[2] = dir[2];

    PerpendicularVector(vr, dir);
    CrossProduct(vr, vf, vup);

    m[0][0] = vr[0];
    m[1][0] = vr[1];
    m[2][0] = vr[2];

    m[0][1] = vup[0];
    m[1][1] = vup[1];
    m[2][1] = vup[2];

    m[0][2] = vf[0];
    m[1][2] = vf[1];
    m[2][2] = vf[2];

    memcpy(im, m, sizeof(im));

    im[0][1] = m[1][0];
    im[0][2] = m[2][0];
    im[1][0] = m[0][1];
    im[1][2] = m[2][1];
    im[2][0] = m[0][2];
    im[2][1] = m[1][2];

    OBJZERO(zrot);

    /* now prepare the rotation matrix */
    zrot[0][0] = cos(degrees * torad);
    zrot[0][1] = sin(degrees * torad);
    zrot[1][0] = -sin(degrees * torad);
    zrot[1][1] = cos(degrees * torad);
    zrot[2][2] = 1.0F;

    R_ConcatRotations(m, zrot, tmpmat);
    R_ConcatRotations(tmpmat, im, rot);

    for (int i = 0; i < 3; i++) {
        dst[i] = DotProduct(rot[i], point);
    }
}

void PolarToVec (const vec2_t a, vec3_t v)
{
    const float p = a[0] * torad;   /* long */
    const float t = a[1] * torad;   /* lat */
    /* v[0] = z, v[1] = x, v[2] = y - wtf? */
    VectorSet(v, cos(p) * cos(t), sin(p) * cos(t), sin(t));
}

void VecToPolar (const vec3_t v, vec2_t a)
{
    a[0] = todeg * atan2(v[1], v[0]);   /* long */
    a[1] = 90 - todeg * acos(v[2]); /* lat */
}

void VecToAngles (const vec3_t value1, vec3_t angles)
{
    float yaw, pitch;

    /* only check the first two values for being zero */
    if (Vector2Empty(value1)) {
        yaw = 0.0f;
        if (value1[2] > 0.0f)
            pitch = 90.0f;
        else
            pitch = 270.0f;
    } else {
        const float forward = sqrt(value1[0] * value1[0] + value1[1] * value1[1]);
        if (!EQUAL(value1[0], 0.0f))
            yaw = atan2(value1[1], value1[0]) * todeg;
        else if (value1[1] > 0.0f)
            yaw = 90.0f;
        else
            yaw = -90.0f;
        if (yaw < 0.0f)
            yaw += 360.0f;

        pitch = atan2(value1[2], forward) * todeg;
        if (pitch < 0.0f)
            pitch += 360.0f;
    }

    /* up and down */
    angles[PITCH] = -pitch;
    /* left and right */
    angles[YAW] = yaw;
    /* tilt left and right */
    angles[ROLL] = 0.0f;
}

bool Q_IsPowerOfTwo (int i)
{
    return (i > 0 && !(i & (i - 1)));
}

float LerpAngle (float a2, float a1, float frac)
{
    if (a1 - a2 > 180)
        a1 -= 360;
    if (a1 - a2 < -180)
        a1 += 360;
    return a2 + frac * (a1 - a2);
}

float AngleNormalize360 (float angle)
{
    return (360.0 / 65536) * ((int)(angle * (65536 / 360.0)) & 65535);
}

float AngleNormalize180 (float angle)
{
    angle = AngleNormalize360(angle);
    if (angle > 180.0)
        angle -= 360.0;
    return angle;
}

void VectorCalcMinsMaxs (const vec3_t center, const vec3_t size, vec3_t mins, vec3_t maxs)
{
    for (int i = 0; i < 3; i++) {
        const vec_t length = fabsf(size[i]) / 2;
        mins[i] = center[i] - length;
        maxs[i] = center[i] + length;
    }
}

void ClearBounds (vec3_t mins, vec3_t maxs)
{
    mins[0] = mins[1] = mins[2] = 99999;
    maxs[0] = maxs[1] = maxs[2] = -99999;
}

void AddPointToBounds (const vec3_t v, vec3_t mins, vec3_t maxs)
{
    for (int i = 0; i < 3; i++) {
        vec_t val = v[i];
        if (val < mins[i])
            mins[i] = val;
        if (val > maxs[i])
            maxs[i] = val;
    }
}

void TangentVectors (const vec3_t normal, const vec3_t sdir, const vec3_t tdir, vec4_t tangent, vec3_t binormal)
{
    vec3_t s, t;

    /* normalize the directional vectors */
    VectorCopy(sdir, s);
    VectorNormalizeFast(s);

    VectorCopy(tdir, t);
    VectorNormalizeFast(t);

    /* project the directional vector onto the plane */
    VectorMA(s, -DotProduct(s, normal), normal, tangent);
    VectorNormalizeFast(tangent);

    /* resolve sidedness, encode as fourth tangent component */
    CrossProduct(normal, tangent, binormal);

    if (DotProduct(t, binormal) < 0.0)
        tangent[3] = -1.0;
    else
        tangent[3] = 1.0;

    VectorScale(binormal, tangent[3], binormal);
}

void Orthogonalize (vec3_t out, const vec3_t in)
{
    vec3_t tmp;
    VectorMul(DotProduct(out, in), in, tmp);
    VectorSubtract(out, tmp, out);
    VectorNormalizeFast(out);
}

void MatrixTranspose (const vec3_t m[3], vec3_t t[3])
{
    for (int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++) {
            t[i][j] = m[j][i];
        }
    }
}

bool RayIntersectAABB (const vec3_t start, const vec3_t end, const AABB& aabb)
{
    float t0 = 0.0f;
    float t1 = 1.0f;
    vec3_t delta;

    VectorSubtract(end, start, delta);

    for (int i = 0; i < 3; i++) {
        const float threshold = 1.0e-6f;
        float u0, u1;

        if (fabs(delta[i]) < threshold) {
            if (delta[i] > 0.0f) {
                return !(end[i] < aabb.mins[i] || start[i] > aabb.maxs[i]);
            } else {
                return !(start[i] < aabb.mins[i] || end[i] > aabb.maxs[i]);
            }
        }

        u0 = (aabb.mins[i] - start[i]) / delta[i];
        u1 = (aabb.maxs[i] - start[i]) / delta[i];

        if (u0 > u1) {
            const float temp = u0;
            u0 = u1;
            u1 = temp;
        }

        if (u1 < t0 || u0 > t1) {
            return false;
        }

        t0 = std::max(u0, t0);
        t1 = std::min(u1, t1);

        if (t1 < t0) {
            return false;
        }
    }

    return true;
}