mathlib.c

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 "../common/common.h"

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

#define RT2 0.70710678118654752440084436210485

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

const byte dvright[CORE_DIRECTIONS] = { 7, 6, 4, 5, 0, 1, 2, 3 };
const byte dvleft[CORE_DIRECTIONS] = { 4, 5, 6, 7, 2, 3, 1, 0 };


int AngleToDir (int angle)
{
    static const int anglesToDV[8] = {0, 4, 2, 6, 1, 5, 3, 7};

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


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

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

    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 (max == 0.0) {
        VectorClear(out);
        return 0;
    }

    VectorScale(in, 1.0 / max, out);

    return max;
}

qboolean 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 (length) {
        const float ilength = 1.0 / 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])
{
    int i, j, k;

    for (j = 0; j < 4; j++) {
        k = j * 4;
        for (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)
{
    int i;

    for (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 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(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}

void VectorMix (const vec3_t v1, const vec3_t v2, float mix, vec3_t out)
{
    int i;

    for (i = 0; i < 3; i++)
        out[i] = v1[i] * (1.0 - mix) + v2[i] * 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 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;
    }
}

qboolean 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;
    VectorNormalize(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 qfalse;
    else
        return qtrue;
}

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

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

    return length;
}

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++) {
        if (fabs(src[i]) < minelem) {
            pos = i;
            minelem = fabs(src[i]);
        }
    }
    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 */
    VectorNormalize(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];
    int i;
    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];

    memset(zrot, 0, sizeof(zrot));
    zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;

    zrot[0][0] = cos(degrees * torad);
    zrot[0][1] = sin(degrees * torad);
    zrot[1][0] = -sin(degrees * torad);
    zrot[1][1] = cos(degrees * torad);

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

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

void Print3Vector (const vec3_t v)
{
    Com_Printf("(%f, %f, %f)\n", v[0], v[1], v[2]);
}

void Print2Vector (const vec2_t v)
{
    Com_Printf("(%f, %f)\n", v[0], v[1]);
}

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;

    if (value1[1] == 0 && value1[0] == 0) {
        yaw = 0;
        if (value1[2] > 0)
            pitch = 90;
        else
            pitch = 270;
    } else {
        const float forward = sqrt(value1[0] * value1[0] + value1[1] * value1[1]);
        if (value1[0])
            yaw = (int) (atan2(value1[1], value1[0]) * todeg);
        else if (value1[1] > 0)
            yaw = 90;
        else
            yaw = -90;
        if (yaw < 0)
            yaw += 360;

        pitch = (int) (atan2(value1[2], forward) * todeg);
        if (pitch < 0)
            pitch += 360;
    }

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


qboolean 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 VectorCenterFromMinsMaxs (const vec3_t mins, const vec3_t maxs, vec3_t center)
{
    VectorAdd(mins, maxs, center);
    VectorScale(center, 0.5, center);
}

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)
{
    int i;
    vec_t val;

    for (i = 0; i < 3; i++) {
        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);
    VectorNormalize(s);

    VectorCopy(tdir, t);
    VectorNormalize(t);

    /* project the directional vector onto the plane */
    VectorMA(s, -DotProduct(s, normal), normal, tangent);
    VectorNormalize(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 v1, const vec3_t v2)
{
    vec3_t tmp;
    VectorMul(DotProduct(v1, v2), v2, tmp);
    VectorSubtract(v1, tmp, v1);
    VectorNormalize(v1);
}

void MatrixTranspose (const vec3_t m[3], vec3_t t[3])
{
    int i, j;

    for (i = 0; i < 3; i++) {
        for(j = 0; j < 3; j++) {
            t[i][j] = m[j][i];
        }
    }
}