team-fte
/
fteqw
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
fteqw/engine/common/mathlib.c

1111 lines
26 KiB
C
Raw Blame History

/*
Copyright (C) 1996-1997 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.
*/
// mathlib.c -- math primitives
#include "quakedef.h"
#include <math.h>
vec3_t vec3_origin = {0,0,0};
int nanmask = 255<<23;
/*-----------------------------------------------------------------*/
#define DEG2RAD( a ) ( a * M_PI ) / 180.0F
void ProjectPointOnPlane( vec3_t dst, const vec3_t p, const vec3_t normal )
{
float d;
vec3_t n;
float inv_denom;
inv_denom = 1.0F / DotProduct( normal, normal );
d = DotProduct( normal, p ) * inv_denom;
n[0] = normal[0] * inv_denom;
n[1] = normal[1] * inv_denom;
n[2] = normal[2] * inv_denom;
dst[0] = p[0] - d * n[0];
dst[1] = p[1] - d * n[1];
dst[2] = p[2] - d * n[2];
}
/*
** assumes "src" is normalized
*/
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 );
}
#ifdef _MSC_VER
#pragma optimize( "", off )
#endif
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( DEG2RAD( degrees ) );
zrot[0][1] = sin( DEG2RAD( degrees ) );
zrot[1][0] = -sin( DEG2RAD( degrees ) );
zrot[1][1] = cos( DEG2RAD( degrees ) );
R_ConcatRotations( m, zrot, tmpmat );
R_ConcatRotations( tmpmat, im, rot );
for ( i = 0; i < 3; i++ )
{
dst[i] = rot[i][0] * point[0] + rot[i][1] * point[1] + rot[i][2] * point[2];
}
}
#ifdef _MSC_VER
#pragma optimize( "", on )
#endif
/*-----------------------------------------------------------------*/
float anglemod(float a)
{
#if 0
if (a >= 0)
a -= 360*(int)(a/360);
else
a += 360*( 1 + (int)(-a/360) );
#endif
a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
return a;
}
/*
==================
BOPS_Error
Split out like this for ASM to call.
==================
*/
void VARGS BOPS_Error (void)
{
Sys_Error ("BoxOnPlaneSide: Bad signbits");
}
#if !id386
/*
==================
BoxOnPlaneSide
Returns 1, 2, or 1 + 2
==================
*/
int BoxOnPlaneSide (vec3_t emins, vec3_t emaxs, mplane_t *p)
{
float dist1, dist2;
int sides;
#if 0 // this is done by the BOX_ON_PLANE_SIDE macro before calling this
// function
// fast axial cases
if (p->type < 3)
{
if (p->dist <= emins[p->type])
return 1;
if (p->dist >= emaxs[p->type])
return 2;
return 3;
}
#endif
// general case
switch (p->signbits)
{
case 0:
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
break;
case 1:
dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
break;
case 2:
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
break;
case 3:
dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
break;
case 4:
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
break;
case 5:
dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
break;
case 6:
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
break;
case 7:
dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
break;
default:
dist1 = dist2 = 0; // shut up compiler
BOPS_Error ();
break;
}
#if 0
int i;
vec3_t corners[2];
for (i=0 ; i<3 ; i++)
{
if (plane->normal[i] < 0)
{
corners[0][i] = emins[i];
corners[1][i] = emaxs[i];
}
else
{
corners[1][i] = emins[i];
corners[0][i] = emaxs[i];
}
}
dist = DotProduct (plane->normal, corners[0]) - plane->dist;
dist2 = DotProduct (plane->normal, corners[1]) - plane->dist;
sides = 0;
if (dist1 >= 0)
sides = 1;
if (dist2 < 0)
sides |= 2;
#endif
sides = 0;
if (dist1 >= p->dist)
sides = 1;
if (dist2 < p->dist)
sides |= 2;
#ifdef PARANOID
if (sides == 0)
Sys_Error ("BoxOnPlaneSide: sides==0");
#endif
return sides;
}
#endif
void VVPerpendicularVector(vec3_t dst, const vec3_t src)
{
if (!src[0])
{
dst[0] = 1;
dst[1] = dst[2] = 0;
}
else if (!src[1])
{
dst[1] = 1;
dst[0] = dst[2] = 0;
}
else if (!src[2])
{
dst[2] = 1;
dst[0] = dst[1] = 0;
}
else
{
dst[0] = -src[1];
dst[1] = src[0];
dst[2] = 0;
VectorNormalize(dst);
}
}
void VectorVectors(vec3_t forward, vec3_t right, vec3_t up)
{
VVPerpendicularVector(right, forward);
CrossProduct(right, forward, up);
}
void AngleVectors (vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
{
float angle;
float sr, sp, sy, cr, cp, cy;
angle = angles[YAW] * (M_PI*2 / 360);
sy = sin(angle);
cy = cos(angle);
angle = angles[PITCH] * (M_PI*2 / 360);
sp = sin(angle);
cp = cos(angle);
angle = angles[ROLL] * (M_PI*2 / 360);
sr = sin(angle);
cr = cos(angle);
forward[0] = cp*cy;
forward[1] = cp*sy;
forward[2] = -sp;
right[0] = (-1*sr*sp*cy+-1*cr*-sy);
right[1] = (-1*sr*sp*sy+-1*cr*cy);
right[2] = -1*sr*cp;
up[0] = (cr*sp*cy+-sr*-sy);
up[1] = (cr*sp*sy+-sr*cy);
up[2] = cr*cp;
}
int VectorCompare (vec3_t v1, vec3_t v2)
{
int i;
for (i=0 ; i<3 ; i++)
if (v1[i] != v2[i])
return 0;
return 1;
}
void VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
{
vecc[0] = veca[0] + scale*vecb[0];
vecc[1] = veca[1] + scale*vecb[1];
vecc[2] = veca[2] + scale*vecb[2];
}
vec_t _DotProduct (vec3_t v1, vec3_t v2)
{
return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
}
void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
{
out[0] = veca[0]-vecb[0];
out[1] = veca[1]-vecb[1];
out[2] = veca[2]-vecb[2];
}
void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
{
out[0] = veca[0]+vecb[0];
out[1] = veca[1]+vecb[1];
out[2] = veca[2]+vecb[2];
}
void _VectorCopy (vec3_t in, vec3_t out)
{
out[0] = in[0];
out[1] = in[1];
out[2] = in[2];
}
void CrossProduct (vec3_t v1, 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];
}
vec_t Length(vec3_t v)
{
int i;
float length;
length = 0;
for (i=0 ; i< 3 ; i++)
length += v[i]*v[i];
length = sqrt (length); // FIXME
return length;
}
float VectorNormalize (vec3_t v)
{
float length, ilength;
length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
length = sqrt (length); // FIXME
if (length)
{
ilength = 1/length;
v[0] *= ilength;
v[1] *= ilength;
v[2] *= ilength;
}
return length;
}
void VectorInverse (vec3_t v)
{
v[0] = -v[0];
v[1] = -v[1];
v[2] = -v[2];
}
void VectorScale (vec3_t in, vec_t scale, vec3_t out)
{
out[0] = in[0]*scale;
out[1] = in[1]*scale;
out[2] = in[2]*scale;
}
int Q_log2(int val)
{
int answer=0;
while ((val>>=1) != 0)
answer++;
return answer;
}
/*
================
R_ConcatRotations
================
*/
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];
}
/*
================
R_ConcatTransforms
================
*/
void R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
{
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[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
in1[0][2] * in2[2][3] + in1[0][3];
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[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
in1[1][2] * in2[2][3] + in1[1][3];
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];
out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
in1[2][2] * in2[2][3] + in1[2][3];
}
void R_ConcatRotationsPad (float in1[3][4], float in2[3][4], float out[3][4])
{
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];
}
/*
===================
FloorDivMod
Returns mathematically correct (floor-based) quotient and remainder for
numer and denom, both of which should contain no fractional part. The
quotient must fit in 32 bits.
====================
*/
void FloorDivMod (double numer, double denom, int *quotient,
int *rem)
{
int q, r;
double x;
#ifndef PARANOID
if (denom <= 0.0)
Sys_Error ("FloorDivMod: bad denominator %d\n", denom);
// if ((floor(numer) != numer) || (floor(denom) != denom))
// Sys_Error ("FloorDivMod: non-integer numer or denom %f %f\n",
// numer, denom);
#endif
if (numer >= 0.0)
{
x = floor(numer / denom);
q = (int)x;
r = (int)floor(numer - (x * denom));
}
else
{
//
// perform operations with positive values, and fix mod to make floor-based
//
x = floor(-numer / denom);
q = -(int)x;
r = (int)floor(-numer - (x * denom));
if (r != 0)
{
q--;
r = (int)denom - r;
}
}
*quotient = q;
*rem = r;
}
/*
===================
GreatestCommonDivisor
====================
*/
int GreatestCommonDivisor (int i1, int i2)
{
if (i1 > i2)
{
if (i2 == 0)
return (i1);
return GreatestCommonDivisor (i2, i1 % i2);
}
else
{
if (i1 == 0)
return (i2);
return GreatestCommonDivisor (i1, i2 % i1);
}
}
#if !id386
// TODO: move to nonintel.c
/*
===================
Invert24To16
Inverts an 8.24 value to a 16.16 value
====================
*/
fixed16_t Invert24To16(fixed16_t val)
{
if (val < 256)
return (0xFFFFFFFF);
return (fixed16_t)
(((double)0x10000 * (double)0x1000000 / (double)val) + 0.5);
}
#endif
void VectorTransform (const vec3_t in1, const float in2[3][4], vec3_t out)
{
out[0] = DotProduct(in1, in2[0]) + in2[0][3];
out[1] = DotProduct(in1, in2[1]) + in2[1][3];
out[2] = DotProduct(in1, in2[2]) + in2[2][3];
}
#ifdef HALFLIFEMODELS
void AngleQuaternion( const vec3_t angles, vec4_t quaternion )
{
float angle;
float sr, sp, sy, cr, cp, cy;
// FIXME: rescale the inputs to 1/2 angle
angle = angles[2] * 0.5;
sy = sin(angle);
cy = cos(angle);
angle = angles[1] * 0.5;
sp = sin(angle);
cp = cos(angle);
angle = angles[0] * 0.5;
sr = sin(angle);
cr = cos(angle);
quaternion[0] = sr*cp*cy-cr*sp*sy; // X
quaternion[1] = cr*sp*cy+sr*cp*sy; // Y
quaternion[2] = cr*cp*sy-sr*sp*cy; // Z
quaternion[3] = cr*cp*cy+sr*sp*sy; // W
}
void QuaternionMatrix( const vec4_t quaternion, float (*matrix)[4] )
{
matrix[0][0] = 1.0 - 2.0 * quaternion[1] * quaternion[1] - 2.0 * quaternion[2] * quaternion[2];
matrix[1][0] = 2.0 * quaternion[0] * quaternion[1] + 2.0 * quaternion[3] * quaternion[2];
matrix[2][0] = 2.0 * quaternion[0] * quaternion[2] - 2.0 * quaternion[3] * quaternion[1];
matrix[0][1] = 2.0 * quaternion[0] * quaternion[1] - 2.0 * quaternion[3] * quaternion[2];
matrix[1][1] = 1.0 - 2.0 * quaternion[0] * quaternion[0] - 2.0 * quaternion[2] * quaternion[2];
matrix[2][1] = 2.0 * quaternion[1] * quaternion[2] + 2.0 * quaternion[3] * quaternion[0];
matrix[0][2] = 2.0 * quaternion[0] * quaternion[2] + 2.0 * quaternion[3] * quaternion[1];
matrix[1][2] = 2.0 * quaternion[1] * quaternion[2] - 2.0 * quaternion[3] * quaternion[0];
matrix[2][2] = 1.0 - 2.0 * quaternion[0] * quaternion[0] - 2.0 * quaternion[1] * quaternion[1];
}
void QuaternionSlerp( const vec4_t p, vec4_t q, float t, vec4_t qt )
{
int i;
float omega, cosom, sinom, sclp, sclq;
// decide if one of the quaternions is backwards
float a = 0;
float b = 0;
for (i = 0; i < 4; i++) {
a += (p[i]-q[i])*(p[i]-q[i]);
b += (p[i]+q[i])*(p[i]+q[i]);
}
if (a > b) {
for (i = 0; i < 4; i++) {
q[i] = -q[i];
}
}
cosom = p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3];
if ((1.0 + cosom) > 0.00000001) {
if ((1.0 - cosom) > 0.00000001) {
omega = acos( cosom );
sinom = sin( omega );
sclp = sin( (1.0 - t)*omega) / sinom;
sclq = sin( t*omega ) / sinom;
}
else {
sclp = 1.0 - t;
sclq = t;
}
for (i = 0; i < 4; i++) {
qt[i] = sclp * p[i] + sclq * q[i];
}
}
else {
qt[0] = -p[1];
qt[1] = p[0];
qt[2] = -p[3];
qt[3] = p[2];
sclp = sin( (1.0 - t) * 0.5 * M_PI);
sclq = sin( t * 0.5 * M_PI);
for (i = 0; i < 3; i++) {
qt[i] = sclp * p[i] + sclq * qt[i];
}
}
}
#endif
//This function is GL stylie (use as 2nd arg to ML_MultMatrix4).
float *Matrix4_NewRotation(float a, float x, float y, float z)
{
static float ret[16];
float c = cos(a* M_PI / 180.0);
float s = sin(a* M_PI / 180.0);
ret[0] = x*x*(1-c)+c;
ret[4] = x*y*(1-c)-z*s;
ret[8] = x*z*(1-c)+y*s;
ret[12] = 0;
ret[1] = y*x*(1-c)+z*s;
ret[5] = y*y*(1-c)+c;
ret[9] = y*z*(1-c)-x*s;
ret[13] = 0;
ret[2] = x*z*(1-c)-y*s;
ret[6] = y*z*(1-c)+x*s;
ret[10] = z*z*(1-c)+c;
ret[14] = 0;
ret[3] = 0;
ret[7] = 0;
ret[11] = 0;
ret[15] = 1;
return ret;
}
//This function is GL stylie (use as 2nd arg to ML_MultMatrix4).
float *Matrix4_NewTranslation(float x, float y, float z)
{
static float ret[16];
ret[0] = 1;
ret[4] = 0;
ret[8] = 0;
ret[12] = x;
ret[1] = 0;
ret[5] = 1;
ret[9] = 0;
ret[13] = y;
ret[2] = 0;
ret[6] = 0;
ret[10] = 1;
ret[14] = z;
ret[3] = 0;
ret[7] = 0;
ret[11] = 0;
ret[15] = 1;
return ret;
}
//be aware that this generates two sorts of matricies depending on order of a+b
void Matrix4_Multiply(float *a, float *b, float *out)
{
out[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2] + a[12] * b[3];
out[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2] + a[13] * b[3];
out[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2] + a[14] * b[3];
out[3] = a[3] * b[0] + a[7] * b[1] + a[11] * b[2] + a[15] * b[3];
out[4] = a[0] * b[4] + a[4] * b[5] + a[8] * b[6] + a[12] * b[7];
out[5] = a[1] * b[4] + a[5] * b[5] + a[9] * b[6] + a[13] * b[7];
out[6] = a[2] * b[4] + a[6] * b[5] + a[10] * b[6] + a[14] * b[7];
out[7] = a[3] * b[4] + a[7] * b[5] + a[11] * b[6] + a[15] * b[7];
out[8] = a[0] * b[8] + a[4] * b[9] + a[8] * b[10] + a[12] * b[11];
out[9] = a[1] * b[8] + a[5] * b[9] + a[9] * b[10] + a[13] * b[11];
out[10] = a[2] * b[8] + a[6] * b[9] + a[10] * b[10] + a[14] * b[11];
out[11] = a[3] * b[8] + a[7] * b[9] + a[11] * b[10] + a[15] * b[11];
out[12] = a[0] * b[12] + a[4] * b[13] + a[8] * b[14] + a[12] * b[15];
out[13] = a[1] * b[12] + a[5] * b[13] + a[9] * b[14] + a[13] * b[15];
out[14] = a[2] * b[12] + a[6] * b[13] + a[10] * b[14] + a[14] * b[15];
out[15] = a[3] * b[12] + a[7] * b[13] + a[11] * b[14] + a[15] * b[15];
}
//transform 4d vector by a 4d matrix.
void Matrix4_Transform4(float *matrix, float *vector, float *product)
{
product[0] = matrix[0]*vector[0] + matrix[4]*vector[1] + matrix[8]*vector[2] + matrix[12]*vector[3];
product[1] = matrix[1]*vector[0] + matrix[5]*vector[1] + matrix[9]*vector[2] + matrix[13]*vector[3];
product[2] = matrix[2]*vector[0] + matrix[6]*vector[1] + matrix[10]*vector[2] + matrix[14]*vector[3];
product[3] = matrix[3]*vector[0] + matrix[7]*vector[1] + matrix[11]*vector[2] + matrix[15]*vector[3];
}
void Matrix4_Transform3(float *matrix, float *vector, float *product)
{
product[0] = matrix[0]*vector[0] + matrix[4]*vector[1] + matrix[8]*vector[2] + matrix[12];
product[1] = matrix[1]*vector[0] + matrix[5]*vector[1] + matrix[9]*vector[2] + matrix[13];
product[2] = matrix[2]*vector[0] + matrix[6]*vector[1] + matrix[10]*vector[2] + matrix[14];
}
void ML_ModelViewMatrix(float *modelview, vec3_t viewangles, vec3_t vieworg)
{
float tempmat[16];
//load identity.
memset(modelview, 0, sizeof(*modelview)*16);
#if FULLYGL
modelview[0] = 1;
modelview[5] = 1;
modelview[10] = 1;
modelview[15] = 1;
Matrix4_Multiply(modelview, Matrix4_NewRotation(-90, 1, 0, 0), tempmat); // put Z going up
Matrix4_Multiply(tempmat, Matrix4_NewRotation(90, 0, 0, 1), modelview); // put Z going up
#else
//use this lame wierd and crazy identity matrix..
modelview[2] = -1;
modelview[4] = -1;
modelview[9] = 1;
modelview[15] = 1;
#endif
//figure out the current modelview matrix
//I would if some of these, but then I'd still need a couple of copys
Matrix4_Multiply(modelview, Matrix4_NewRotation(-viewangles[2], 1, 0, 0), tempmat);
Matrix4_Multiply(tempmat, Matrix4_NewRotation(-viewangles[0], 0, 1, 0), modelview);
Matrix4_Multiply(modelview, Matrix4_NewRotation(-viewangles[1], 0, 0, 1), tempmat);
Matrix4_Multiply(tempmat, Matrix4_NewTranslation(-vieworg[0], -vieworg[1], -vieworg[2]), modelview); // put Z going up
}
void ML_ModelViewMatrixFromAxis(float *modelview, vec3_t pn, vec3_t right, vec3_t up, vec3_t vieworg)
{
float tempmat[16];
tempmat[ 0] = right[0];
tempmat[ 1] = up[0];
tempmat[ 2] = -pn[0];
tempmat[ 3] = 0;
tempmat[ 4] = right[1];
tempmat[ 5] = up[1];
tempmat[ 6] = -pn[1];
tempmat[ 7] = 0;
tempmat[ 8] = right[2];
tempmat[ 9] = up[2];
tempmat[10] = -pn[2];
tempmat[11] = 0;
tempmat[12] = 0;
tempmat[13] = 0;
tempmat[14] = 0;
tempmat[15] = 1;
Matrix4_Multiply(tempmat, Matrix4_NewTranslation(-vieworg[0], -vieworg[1], -vieworg[2]), modelview); // put Z going up
}
void ML_ProjectionMatrix(float *proj, float wdivh, float fovy)
{
float xmin, xmax, ymin, ymax;
float nudge = 1;
//proj
ymax = 4 * tan( fovy * M_PI / 360.0 );
ymin = -ymax;
xmin = ymin * wdivh;
xmax = ymax * wdivh;
proj[0] = (2*4) / (xmax - xmin);
proj[4] = 0;
proj[8] = (xmax + xmin) / (xmax - xmin);
proj[12] = 0;
proj[1] = 0;
proj[5] = (2*4) / (ymax - ymin);
proj[9] = (ymax + ymin) / (ymax - ymin);
proj[13] = 0;
proj[2] = 0;
proj[6] = 0;
proj[10] = -1 * nudge;
proj[14] = -2*4 * nudge;
proj[3] = 0;
proj[7] = 0;
proj[11] = -1;
proj[15] = 0;
}
void ML_ProjectionMatrix2(float *proj, float fovx, float fovy)
{
float xmin, xmax, ymin, ymax;
float nudge = 1;
//proj
ymax = 4 * tan( fovy * M_PI / 360.0 );
ymin = -ymax;
xmax = 4 * tan( fovx * M_PI / 360.0 );
xmin = -xmax;
proj[0] = (2*4) / (xmax - xmin);
proj[4] = 0;
proj[8] = (xmax + xmin) / (xmax - xmin);
proj[12] = 0;
proj[1] = 0;
proj[5] = (2*4) / (ymax - ymin);
proj[9] = (ymax + ymin) / (ymax - ymin);
proj[13] = 0;
proj[2] = 0;
proj[6] = 0;
proj[10] = -1 * nudge;
proj[14] = -2*4 * nudge;
proj[3] = 0;
proj[7] = 0;
proj[11] = -1;
proj[15] = 0;
}
typedef struct {
float m[4][4];
} matrix4x4_t;
void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
{
// we only support uniform scaling, so assume the first row is enough
// (note the lack of sqrt here, because we're trying to undo the scaling,
// this means multiplying by the inverse scale twice - squaring it, which
// makes the sqrt a waste of time)
#if 1
double scale = 1.0 / (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
#else
double scale = 3.0 / sqrt
(in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
+ in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
+ in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
scale *= scale;
#endif
// invert the rotation by transposing and multiplying by the squared
// recipricol of the input matrix scale as described above
out->m[0][0] = (float)(in1->m[0][0] * scale);
out->m[0][1] = (float)(in1->m[1][0] * scale);
out->m[0][2] = (float)(in1->m[2][0] * scale);
out->m[1][0] = (float)(in1->m[0][1] * scale);
out->m[1][1] = (float)(in1->m[1][1] * scale);
out->m[1][2] = (float)(in1->m[2][1] * scale);
out->m[2][0] = (float)(in1->m[0][2] * scale);
out->m[2][1] = (float)(in1->m[1][2] * scale);
out->m[2][2] = (float)(in1->m[2][2] * scale);
// invert the translate
out->m[0][3] = -(in1->m[0][3] * out->m[0][0] + in1->m[1][3] * out->m[0][1] + in1->m[2][3] * out->m[0][2]);
out->m[1][3] = -(in1->m[0][3] * out->m[1][0] + in1->m[1][3] * out->m[1][1] + in1->m[2][3] * out->m[1][2]);
out->m[2][3] = -(in1->m[0][3] * out->m[2][0] + in1->m[1][3] * out->m[2][1] + in1->m[2][3] * out->m[2][2]);
// don't know if there's anything worth doing here
out->m[3][0] = 0;
out->m[3][1] = 0;
out->m[3][2] = 0;
out->m[3][3] = 1;
}
//screen->3d
void ML_UnProject(vec3_t in, vec3_t out, vec3_t viewangles, vec3_t vieworg, float wdivh, float fovy)
{
float modelview[16];
float proj[16];
float tempm[16];
ML_ModelViewMatrix(modelview, viewangles, vieworg);
ML_ProjectionMatrix(proj, wdivh, fovy);
Matrix4_Multiply(proj, modelview, tempm);
Matrix4x4_Invert_Simple((void*)proj, (void*)tempm);
{
float v[4], tempv[4];
v[0] = in[0]*2-1;
v[1] = in[1]*2-1;
v[2] = in[2]*2-1;
v[3] = 1;
Matrix4_Transform4(proj, v, tempv);
out[0] = tempv[0];
out[1] = tempv[1];
out[2] = tempv[2];
}
}
//returns fractions of screen.
//uses GL style rotations and translations and stuff.
//3d -> screen (fixme: offscreen return values needed)
void ML_Project (vec3_t in, vec3_t out, vec3_t viewangles, vec3_t vieworg, float wdivh, float fovy)
{
float modelview[16];
float proj[16];
ML_ModelViewMatrix(modelview, viewangles, vieworg);
ML_ProjectionMatrix(proj, wdivh, fovy);
{
float v[4], tempv[4];
v[0] = in[0];
v[1] = in[1];
v[2] = in[2];
v[3] = 1;
Matrix4_Transform4(modelview, v, tempv);
Matrix4_Transform4(proj, tempv, v);
v[0] /= v[3];
v[1] /= v[3];
v[2] /= v[3];
out[0] = (1+v[0])/2;
out[1] = (1+v[1])/2;
out[2] = (1+v[2])/2;
}
}
//I much prefer it to take float*...
void Matrix3_Multiply (vec3_t *in1, vec3_t *in2, vec3_t *out)
{
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];
}
vec_t VectorNormalize2 (vec3_t v, vec3_t out)
{
float length, ilength;
length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
if (length)
{
length = sqrt (length); // FIXME
ilength = 1/length;
out[0] = v[0]*ilength;
out[1] = v[1]*ilength;
out[2] = v[2]*ilength;
}
else
{
VectorClear (out);
}
return length;
}
float ColorNormalize (vec3_t in, vec3_t out)
{
float f = max (max (in[0], in[1]), in[2]);
if ( f > 1.0 ) {
f = 1.0 / f;
out[0] = in[0] * f;
out[1] = in[1] * f;
out[2] = in[2] * f;
} else {
out[0] = in[0];
out[1] = in[1];
out[2] = in[2];
}
return f;
}
Reference in New Issue View Git Blame Copy Permalink