Archive/openmohaa
0
0
Fork 0
mirror of https://github.com/openmoh/openmohaa.git synced 2025-04-28 21:57:57 +03:00
Code Issues Projects Releases Packages Wiki Activity Actions 3
openmohaa/code/qcommon/q_math.c

4129 lines
102 KiB
C
Raw Normal View History

Hard reset
2016-03-27 11:49:47 +02:00
/*
===========================================================================
Copyright (C) 1999-2005 Id Software, Inc.
This file is part of Quake III Arena source code.
Quake III Arena source code 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.
Quake III Arena source code 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 Quake III Arena source code; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
===========================================================================
*/
//
// q_math.c -- stateless support routines that are included in each code module
// Some of the vector functions are static inline in q_shared.h. q3asm
// doesn't understand static functions though, so we only want them in
// one file. That's what this is about.
#ifdef Q3_VM
#define __Q3_VM_MATH
#endif
#include "q_shared.h"
#define X 0
#define Y 1
#define Z 2
#define W 3
vec3_t vec3_origin = {0,0,0};
vec3_t axisDefault[3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
matrix_t matrixIdentity = { 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1};
vec4_t colorBlack = {0, 0, 0, 1};
vec4_t colorRed = {1, 0, 0, 1};
vec4_t colorGreen = {0, 1, 0, 1};
vec4_t colorBlue = {0, 0, 1, 1};
vec4_t colorYellow = {1, 1, 0, 1};
vec4_t colorMagenta= {1, 0, 1, 1};
vec4_t colorCyan = {0, 1, 1, 1};
vec4_t colorWhite = {1, 1, 1, 1};
vec4_t colorLtGrey = {0.75, 0.75, 0.75, 1};
vec4_t colorMdGrey = {0.5, 0.5, 0.5, 1};
vec4_t colorDkGrey = {0.25, 0.25, 0.25, 1};
vec4_t g_color_table[8] =
{
{0.0, 0.0, 0.0, 1.0},
{1.0, 0.0, 0.0, 1.0},
{0.0, 1.0, 0.0, 1.0},
{1.0, 1.0, 0.0, 1.0},
{0.0, 0.0, 1.0, 1.0},
{0.0, 1.0, 1.0, 1.0},
{1.0, 0.0, 1.0, 1.0},
{1.0, 1.0, 1.0, 1.0},
};
vec3_t bytedirs[NUMVERTEXNORMALS] =
{
{-0.525731f, 0.000000f, 0.850651f}, {-0.442863f, 0.238856f, 0.864188f},
{-0.295242f, 0.000000f, 0.955423f}, {-0.309017f, 0.500000f, 0.809017f},
{-0.162460f, 0.262866f, 0.951056f}, {0.000000f, 0.000000f, 1.000000f},
{0.000000f, 0.850651f, 0.525731f}, {-0.147621f, 0.716567f, 0.681718f},
{0.147621f, 0.716567f, 0.681718f}, {0.000000f, 0.525731f, 0.850651f},
{0.309017f, 0.500000f, 0.809017f}, {0.525731f, 0.000000f, 0.850651f},
{0.295242f, 0.000000f, 0.955423f}, {0.442863f, 0.238856f, 0.864188f},
{0.162460f, 0.262866f, 0.951056f}, {-0.681718f, 0.147621f, 0.716567f},
{-0.809017f, 0.309017f, 0.500000f},{-0.587785f, 0.425325f, 0.688191f},
{-0.850651f, 0.525731f, 0.000000f},{-0.864188f, 0.442863f, 0.238856f},
{-0.716567f, 0.681718f, 0.147621f},{-0.688191f, 0.587785f, 0.425325f},
{-0.500000f, 0.809017f, 0.309017f}, {-0.238856f, 0.864188f, 0.442863f},
{-0.425325f, 0.688191f, 0.587785f}, {-0.716567f, 0.681718f, -0.147621f},
{-0.500000f, 0.809017f, -0.309017f}, {-0.525731f, 0.850651f, 0.000000f},
{0.000000f, 0.850651f, -0.525731f}, {-0.238856f, 0.864188f, -0.442863f},
{0.000000f, 0.955423f, -0.295242f}, {-0.262866f, 0.951056f, -0.162460f},
{0.000000f, 1.000000f, 0.000000f}, {0.000000f, 0.955423f, 0.295242f},
{-0.262866f, 0.951056f, 0.162460f}, {0.238856f, 0.864188f, 0.442863f},
{0.262866f, 0.951056f, 0.162460f}, {0.500000f, 0.809017f, 0.309017f},
{0.238856f, 0.864188f, -0.442863f},{0.262866f, 0.951056f, -0.162460f},
{0.500000f, 0.809017f, -0.309017f},{0.850651f, 0.525731f, 0.000000f},
{0.716567f, 0.681718f, 0.147621f}, {0.716567f, 0.681718f, -0.147621f},
{0.525731f, 0.850651f, 0.000000f}, {0.425325f, 0.688191f, 0.587785f},
{0.864188f, 0.442863f, 0.238856f}, {0.688191f, 0.587785f, 0.425325f},
{0.809017f, 0.309017f, 0.500000f}, {0.681718f, 0.147621f, 0.716567f},
{0.587785f, 0.425325f, 0.688191f}, {0.955423f, 0.295242f, 0.000000f},
{1.000000f, 0.000000f, 0.000000f}, {0.951056f, 0.162460f, 0.262866f},
{0.850651f, -0.525731f, 0.000000f},{0.955423f, -0.295242f, 0.000000f},
{0.864188f, -0.442863f, 0.238856f}, {0.951056f, -0.162460f, 0.262866f},
{0.809017f, -0.309017f, 0.500000f}, {0.681718f, -0.147621f, 0.716567f},
{0.850651f, 0.000000f, 0.525731f}, {0.864188f, 0.442863f, -0.238856f},
{0.809017f, 0.309017f, -0.500000f}, {0.951056f, 0.162460f, -0.262866f},
{0.525731f, 0.000000f, -0.850651f}, {0.681718f, 0.147621f, -0.716567f},
{0.681718f, -0.147621f, -0.716567f},{0.850651f, 0.000000f, -0.525731f},
{0.809017f, -0.309017f, -0.500000f}, {0.864188f, -0.442863f, -0.238856f},
{0.951056f, -0.162460f, -0.262866f}, {0.147621f, 0.716567f, -0.681718f},
{0.309017f, 0.500000f, -0.809017f}, {0.425325f, 0.688191f, -0.587785f},
{0.442863f, 0.238856f, -0.864188f}, {0.587785f, 0.425325f, -0.688191f},
{0.688191f, 0.587785f, -0.425325f}, {-0.147621f, 0.716567f, -0.681718f},
{-0.309017f, 0.500000f, -0.809017f}, {0.000000f, 0.525731f, -0.850651f},
{-0.525731f, 0.000000f, -0.850651f}, {-0.442863f, 0.238856f, -0.864188f},
{-0.295242f, 0.000000f, -0.955423f}, {-0.162460f, 0.262866f, -0.951056f},
{0.000000f, 0.000000f, -1.000000f}, {0.295242f, 0.000000f, -0.955423f},
{0.162460f, 0.262866f, -0.951056f}, {-0.442863f, -0.238856f, -0.864188f},
{-0.309017f, -0.500000f, -0.809017f}, {-0.162460f, -0.262866f, -0.951056f},
{0.000000f, -0.850651f, -0.525731f}, {-0.147621f, -0.716567f, -0.681718f},
{0.147621f, -0.716567f, -0.681718f}, {0.000000f, -0.525731f, -0.850651f},
{0.309017f, -0.500000f, -0.809017f}, {0.442863f, -0.238856f, -0.864188f},
{0.162460f, -0.262866f, -0.951056f}, {0.238856f, -0.864188f, -0.442863f},
{0.500000f, -0.809017f, -0.309017f}, {0.425325f, -0.688191f, -0.587785f},
{0.716567f, -0.681718f, -0.147621f}, {0.688191f, -0.587785f, -0.425325f},
{0.587785f, -0.425325f, -0.688191f}, {0.000000f, -0.955423f, -0.295242f},
{0.000000f, -1.000000f, 0.000000f}, {0.262866f, -0.951056f, -0.162460f},
{0.000000f, -0.850651f, 0.525731f}, {0.000000f, -0.955423f, 0.295242f},
{0.238856f, -0.864188f, 0.442863f}, {0.262866f, -0.951056f, 0.162460f},
{0.500000f, -0.809017f, 0.309017f}, {0.716567f, -0.681718f, 0.147621f},
{0.525731f, -0.850651f, 0.000000f}, {-0.238856f, -0.864188f, -0.442863f},
{-0.500000f, -0.809017f, -0.309017f}, {-0.262866f, -0.951056f, -0.162460f},
{-0.850651f, -0.525731f, 0.000000f}, {-0.716567f, -0.681718f, -0.147621f},
{-0.716567f, -0.681718f, 0.147621f}, {-0.525731f, -0.850651f, 0.000000f},
{-0.500000f, -0.809017f, 0.309017f}, {-0.238856f, -0.864188f, 0.442863f},
{-0.262866f, -0.951056f, 0.162460f}, {-0.864188f, -0.442863f, 0.238856f},
{-0.809017f, -0.309017f, 0.500000f}, {-0.688191f, -0.587785f, 0.425325f},
{-0.681718f, -0.147621f, 0.716567f}, {-0.442863f, -0.238856f, 0.864188f},
{-0.587785f, -0.425325f, 0.688191f}, {-0.309017f, -0.500000f, 0.809017f},
{-0.147621f, -0.716567f, 0.681718f}, {-0.425325f, -0.688191f, 0.587785f},
{-0.162460f, -0.262866f, 0.951056f}, {0.442863f, -0.238856f, 0.864188f},
{0.162460f, -0.262866f, 0.951056f}, {0.309017f, -0.500000f, 0.809017f},
{0.147621f, -0.716567f, 0.681718f}, {0.000000f, -0.525731f, 0.850651f},
{0.425325f, -0.688191f, 0.587785f}, {0.587785f, -0.425325f, 0.688191f},
{0.688191f, -0.587785f, 0.425325f}, {-0.955423f, 0.295242f, 0.000000f},
{-0.951056f, 0.162460f, 0.262866f}, {-1.000000f, 0.000000f, 0.000000f},
{-0.850651f, 0.000000f, 0.525731f}, {-0.955423f, -0.295242f, 0.000000f},
{-0.951056f, -0.162460f, 0.262866f}, {-0.864188f, 0.442863f, -0.238856f},
{-0.951056f, 0.162460f, -0.262866f}, {-0.809017f, 0.309017f, -0.500000f},
{-0.864188f, -0.442863f, -0.238856f}, {-0.951056f, -0.162460f, -0.262866f},
{-0.809017f, -0.309017f, -0.500000f}, {-0.681718f, 0.147621f, -0.716567f},
{-0.681718f, -0.147621f, -0.716567f}, {-0.850651f, 0.000000f, -0.525731f},
{-0.688191f, 0.587785f, -0.425325f}, {-0.587785f, 0.425325f, -0.688191f},
{-0.425325f, 0.688191f, -0.587785f}, {-0.425325f, -0.688191f, -0.587785f},
{-0.587785f, -0.425325f, -0.688191f}, {-0.688191f, -0.587785f, -0.425325f}
};
//==============================================================
int Q_rand( int *seed ) {
*seed = (69069 * *seed + 1);
return *seed;
}
float Q_random( int *seed ) {
return ( Q_rand( seed ) & 0xffff ) / (float)0x10000;
}
float Q_crandom( int *seed ) {
return 2.0 * ( Q_random( seed ) - 0.5 );
}
/*
grandom
This function produces a random number with a gaussian
distribution. This also also known as a normal or bell
curve distribution; it has a mean value of zero and a
standard deviation of one.
*/
float grandom( void ) {
double v1;
double v2;
double s;
float x1;
static float x2 = 0;
static int toggle = 0;
if( toggle ) {
toggle = 0;
return x2;
}
do {
v1 = -1.0 + 2.0 * random();
v2 = -1.0 + 2.0 * random();
s = v1 * v1 + v2 * v2;
} while( s >= 1.0 || s == 0 );
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
s = sqrtf( -2.0 * log( s ) / s );
Hard reset
2016-03-27 11:49:47 +02:00
x1 = v1 * s;
x2 = v2 * s;
toggle = 1;
return x1;
}
/*
erandom
This function produces a random number with a exponential
distribution and the specified mean value.
*/
float erandom( float mean ) {
float r;
do {
r = random();
} while( r == 0.0 );
return -mean * log( r );
}
//=======================================================
byte ClampByte(int i) {
if(i < 0) {
return 0;
}
if(i > 255) {
return 255;
}
return i;
}
signed char ClampChar( int i ) {
if ( i < -128 ) {
return -128;
}
if ( i > 127 ) {
return 127;
}
return i;
}
signed short ClampShort( int i ) {
if ( i < -32768 ) {
return -32768;
}
if ( i > 0x7fff ) {
return 0x7fff;
}
return i;
}
//===========================================================================
//
// Global functions base on type double
//
//===========================================================================
#define SCALAR_EPSILON (0.000001f)
#define SCALAR_IDENTITY (0.0f)
double dEpsilon( void )
{
return ( double )SCALAR_EPSILON;
}
double dIdentity( void )
{
return ( double )SCALAR_IDENTITY;
}
double dSign( const double number )
{
if( number >= 0.0 )
{
return 1;
} else
{
return -1;
}
}
double dClamp( const double value, const double min, const double max )
{
assert( min <= max );
if( value < min )
{
return min;
}
if( value > max )
{
return max;
}
return value;
}
double dDistance( const double value1, const double value2 )
{
return fabs( value1 - value2 );
}
qboolean dCloseEnough( const double value1, const double value2, const double epsilon )
{
return dDistance( value1, value2 ) < epsilon;
}
qboolean dSmallEnough( const double value, const double epsilon )
{
return dDistance( dIdentity(), value ) < epsilon;
}
//===========================================================================
//
// Global functions base on type float
//
//===========================================================================
float fEpsilon( void )
{
return SCALAR_EPSILON;
}
float fIdentity( void )
{
return SCALAR_IDENTITY;
}
float fSign( const float number )
{
Use a correct implementation for fSign()
2023-10-07 23:49:35 +02:00
if (number > 0) {
Hard reset
2016-03-27 11:49:47 +02:00
return 1;
Use a correct implementation for fSign()
2023-10-07 23:49:35 +02:00
} else if (number == 0) {
return 0;
} else {
Hard reset
2016-03-27 11:49:47 +02:00
return -1;
}
}
float fClamp( const float value, const float min, const float max )
{
assert( min <= max );
if( value < min )
{
return min;
}
if( value > max )
{
return max;
}
return value;
}
float fDistance( const float value1, const float value2 )
{
return fabs( value1 - value2 );
}
qboolean fCloseEnough( const float value1, const float value2, const float epsilon )
{
return fDistance( value1, value2 ) < epsilon;
}
qboolean fSmallEnough( const float value, const float epsilon )
{
return fDistance( fIdentity(), value ) < epsilon;
}
//===========================================================================
//
// Global functions base on type int
//
//===========================================================================
int iSign( const int number )
{
if( number >= 0 )
{
return 1;
} else
{
return -1;
}
}
int iClamp( const int value, const int min, const int max )
{
assert( min <= max );
if( value < min )
{
return min;
}
if( value > max )
{
return max;
}
return value;
}
// this isn't a real cheap function to call!
int DirToByte( vec3_t dir ) {
int i, best;
float d, bestd;
if ( !dir ) {
return 0;
}
bestd = 0;
best = 0;
for (i=0 ; i<NUMVERTEXNORMALS ; i++)
{
d = DotProduct (dir, bytedirs[i]);
if (d > bestd)
{
bestd = d;
best = i;
}
}
return best;
}
void ByteToDir( int b, vec3_t dir ) {
if ( b < 0 || b >= NUMVERTEXNORMALS ) {
VectorCopy( vec3_origin, dir );
return;
}
VectorCopy (bytedirs[b], dir);
}
unsigned ColorBytes3 (float r, float g, float b) {
unsigned i;
( (byte *)&i )[0] = r * 255;
( (byte *)&i )[1] = g * 255;
( (byte *)&i )[2] = b * 255;
return i;
}
unsigned ColorBytes4 (float r, float g, float b, float a) {
unsigned i;
( (byte *)&i )[0] = r * 255;
( (byte *)&i )[1] = g * 255;
( (byte *)&i )[2] = b * 255;
( (byte *)&i )[3] = a * 255;
return i;
}
float NormalizeColor( const vec3_t in, vec3_t out ) {
float max;
max = in[0];
if ( in[1] > max ) {
max = in[1];
}
if ( in[2] > max ) {
max = in[2];
}
if ( !max ) {
VectorClear( out );
} else {
out[0] = in[0] / max;
out[1] = in[1] / max;
out[2] = in[2] / max;
}
return max;
}
Added RotatePointAroundAxis
2023-07-30 15:42:59 +02:00
void RotatePointAroundAxis(vec3_t dst, int axis, const vec3_t point, float degrees) {
if (degrees != 0.0) {
float fCos, fSin;
int i1, i2;
fCos = cos(DEG2RAD(degrees));
fSin = sin(DEG2RAD(degrees));
i2 = (axis + 1) % 3;
i1 = (axis + 2) % 3;
dst[axis] = point[axis];
dst[i2] = point[i2] * fCos - point[i1] * fSin;
dst[i1] = point[i2] * fSin - point[i1] * fCos;
} else {
VectorCopy(point, dst);
}
}
Hard reset
2016-03-27 11:49:47 +02:00
void ClampColor(vec4_t color)
{
int i;
for(i = 0; i < 4; i++)
{
if(color[i] < 0)
color[i] = 0;
if(color[i] > 1)
color[i] = 1;
}
}
/*
=====================
PlaneFromPoints
Returns false if the triangle is degenrate.
The normal will point out of the clock for clockwise ordered points
=====================
*/
qboolean PlaneFromPoints( vec4_t plane, const vec3_t a, const vec3_t b, const vec3_t c ) {
vec3_t d1, d2;
VectorSubtract( b, a, d1 );
VectorSubtract( c, a, d2 );
CrossProduct( d2, d1, plane );
if ( VectorNormalize( plane ) == 0 ) {
return qfalse;
}
plane[3] = DotProduct( a, plane );
return qtrue;
}
qboolean PlanesGetIntersectionPoint(const vec4_t plane1, const vec4_t plane2, const vec4_t plane3, vec3_t out)
{
// http://www.cgafaq.info/wiki/Intersection_of_three_planes
vec3_t n1, n2, n3;
vec3_t n1n2, n2n3, n3n1;
vec_t denom;
VectorNormalize2(plane1, n1);
VectorNormalize2(plane2, n2);
VectorNormalize2(plane3, n3);
CrossProduct(n1, n2, n1n2);
CrossProduct(n2, n3, n2n3);
CrossProduct(n3, n1, n3n1);
denom = DotProduct(n1, n2n3);
// check if the denominator is zero (which would mean that no intersection is to be found
if(denom == 0)
{
// no intersection could be found, return <0,0,0>
VectorClear(out);
return qfalse;
}
VectorClear(out);
VectorMA(out, plane1[3], n2n3, out);
VectorMA(out, plane2[3], n3n1, out);
VectorMA(out, plane3[3], n1n2, out);
VectorScale(out, 1.0f / denom, out);
return qtrue;
}
void PlaneIntersectRay(const vec3_t rayPos, const vec3_t rayDir, const vec4_t plane, vec3_t res)
{
vec3_t dir;
float sect;
float distToPlane;
float planeDotRay;
VectorNormalize2(rayDir, dir);
distToPlane = DotProduct(plane, rayPos) - plane[3];
planeDotRay = DotProduct(plane, dir);
sect = -(distToPlane) / planeDotRay;
VectorMA(rayPos, sect, dir, res);
}
vec_t PlaneNormalize(vec4_t plane)
{
vec_t length, ilength;
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
length = sqrtf(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);
Hard reset
2016-03-27 11:49:47 +02:00
if(length == 0)
{
VectorClear(plane);
return 0;
}
ilength = 1.0 / length;
plane[0] = plane[0] * ilength;
plane[1] = plane[1] * ilength;
plane[2] = plane[2] * ilength;
plane[3] = plane[3] * ilength;
return length;
}
/*
===============
RotatePointAroundVector
This is not implemented very well...
===============
*/
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;
float rad;
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;
rad = DEG2RAD( degrees );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
zrot[0][0] = cos( rad );
zrot[0][1] = sin( rad );
zrot[1][0] = -sin( rad );
zrot[1][1] = cos( rad );
Hard reset
2016-03-27 11:49:47 +02:00
Matrix3x3Multiply( m, zrot, tmpmat );
Matrix3x3Multiply( 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];
}
}
/*
===============
RotateAroundDirection
===============
*/
void RotateAroundDirection( vec3_t axis[3], float yaw ) {
// create an arbitrary axis[1]
PerpendicularVector( axis[1], axis[0] );
// rotate it around axis[0] by yaw
if ( yaw ) {
vec3_t temp;
VectorCopy( axis[1], temp );
RotatePointAroundVector( axis[1], axis[0], temp, yaw );
}
// cross to get axis[2]
CrossProduct( axis[0], axis[1], axis[2] );
}
void vectoangles( const vec3_t value1, vec3_t angles ) {
float forward;
float yaw, pitch;
if ( value1[1] == 0 && value1[0] == 0 ) {
yaw = 0;
if ( value1[2] > 0 ) {
pitch = 90;
}
else {
pitch = 270;
}
}
else {
Actor Improvements p3_3
2019-06-30 23:03:24 +02:00
//if ( value1[0] ) {
Hard reset
2016-03-27 11:49:47 +02:00
yaw = ( atan2 ( value1[1], value1[0] ) * 180 / M_PI );
Actor Improvements p3_3
2019-06-30 23:03:24 +02:00
//}
//else if ( value1[1] > 0 ) {
// yaw = 90;
//}
//else {
// yaw = 270;
//}
Hard reset
2016-03-27 11:49:47 +02:00
if ( yaw < 0 ) {
yaw += 360;
}
forward = sqrt ( value1[0]*value1[0] + value1[1]*value1[1] );
pitch = ( atan2(value1[2], forward) * 180 / M_PI );
if ( pitch < 0 ) {
pitch += 360;
}
}
angles[PITCH] = -pitch;
angles[YAW] = yaw;
angles[ROLL] = 0;
}
void VectorToAngles( const vec3_t vec, vec3_t angles )
{
float forward;
float yaw, pitch;
if( vec[ 1 ] == 0 && vec[ 0 ] == 0 )
{
yaw = 0;
if( vec[ 2 ] > 0 ) {
pitch = 90;
} else {
pitch = 270;
}
}
else
{
yaw = ( atan2( vec[ 1 ], vec[ 0 ] ) * ( 180.0f / M_PI ) );
if( yaw < 0 ) {
yaw += 360;
}
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
forward = sqrtf( 1.0f - vec[ 2 ] * vec[ 2 ] );
Hard reset
2016-03-27 11:49:47 +02:00
pitch = ( atan2( vec[ 2 ], forward ) * -( 180.0f / M_PI ) );
if( pitch < 0 ) {
pitch += 360;
}
}
angles[ PITCH ] = pitch;
angles[ YAW ] = yaw;
angles[ ROLL ] = 0;
}
/*
=================
AnglesToAxis
=================
*/
void AnglesToAxis( const vec3_t angles, vec3_t axis[3] ) {
float angle;
static float sr, sp, sy, cr, cp, cy;
// static to help MS compiler fp bugs
angle = angles[ YAW ] * ( M_PI * 2 / 360 );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sy = sin( angle );
cy = cos( angle );
Hard reset
2016-03-27 11:49:47 +02:00
angle = angles[ PITCH ] * ( M_PI * 2 / 360 );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sp = sin( angle );
cp = cos( angle );
Hard reset
2016-03-27 11:49:47 +02:00
angle = angles[ ROLL ] * ( M_PI * 2 / 360 );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sr = sin( angle );
cr = cos( angle );
Hard reset
2016-03-27 11:49:47 +02:00
axis[ 0 ][ 0 ] = cp*cy;
axis[ 0 ][ 1 ] = cp*sy;
axis[ 0 ][ 2 ] = -sp;
axis[ 1 ][ 0 ] = ( sr*sp*cy + cr*-sy );
axis[ 1 ][ 1 ] = ( sr*sp*sy + cr*cy );
axis[ 1 ][ 2 ] = sr*cp;
axis[ 2 ][ 0 ] = ( cr*sp*cy + -sr*-sy );
axis[ 2 ][ 1 ] = ( cr*sp*sy + -sr*cy );
axis[ 2 ][ 2 ] = cr*cp;
}
Added YawToAxis
2023-08-15 22:20:33 +02:00
void YawToAxis(float yaw, float axis[2]) {
axis[0] = cos(DEG2RAD(yaw));
axis[1] = sin(DEG2RAD(yaw));
}
Hard reset
2016-03-27 11:49:47 +02:00
void AxisClear( vec3_t axis[3] ) {
axis[0][0] = 1;
axis[0][1] = 0;
axis[0][2] = 0;
axis[1][0] = 0;
axis[1][1] = 1;
axis[1][2] = 0;
axis[2][0] = 0;
axis[2][1] = 0;
axis[2][2] = 1;
}
void AxisCopy( const vec3_t in[3], vec3_t out[3] ) {
VectorCopy( in[0], out[0] );
VectorCopy( in[1], out[1] );
VectorCopy( in[2], out[2] );
}
void ProjectPointOnPlane( vec3_t dst, const vec3_t p, const vec3_t normal )
{
float d;
vec3_t n;
float inv_denom;
Removed zero vectors assert in ProjectPointOnPlane(), it was removed since F.A.K.K 2
2023-08-27 17:13:12 +02:00
inv_denom = 1.0f / DotProduct( normal, normal );
Hard reset
2016-03-27 11:49:47 +02:00
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];
}
/*
================
MakeNormalVectors
Given a normalized forward vector, create two
other perpendicular vectors
================
*/
void MakeNormalVectors( const vec3_t forward, vec3_t right, vec3_t up) {
float d;
// this rotate and negate guarantees a vector
// not colinear with the original
right[1] = -forward[0];
right[2] = forward[1];
right[0] = forward[2];
d = DotProduct (right, forward);
VectorMA (right, -d, forward, right);
VectorNormalize (right);
CrossProduct (right, forward, up);
}
void VectorRotate( vec3_t in, vec3_t matrix[3], vec3_t out )
{
out[0] = DotProduct( in, matrix[0] );
out[1] = DotProduct( in, matrix[1] );
out[2] = DotProduct( in, matrix[2] );
}
//============================================================================
#if !idppc
/*
** float q_rsqrt( float number )
*/
float Q_rsqrt( float number )
{
union {
float f;
int i;
} t;
float x2, y;
const float threehalfs = 1.5F;
x2 = number * 0.5F;
t.f = number;
t.i = 0x5f3759df - ( t.i >> 1 ); // what the fuck?
y = t.f;
y = y * ( threehalfs - ( x2 * y * y ) ); // 1st iteration
// y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed
return y;
}
float Q_fabs( float f ) {
int tmp = * ( int * ) &f;
tmp &= 0x7FFFFFFF;
return * ( float * ) &tmp;
}
#endif
/*
=====================
Q_acos
the msvc acos doesn't always return a value between -PI and PI:
int i;
i = 1065353246;
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
acos(*(float*) &i) == -1.#IND0
Hard reset
2016-03-27 11:49:47 +02:00
This should go in q_math but it is too late to add new traps
to game and ui
=====================
*/
float Q_acos(float c) {
float angle;
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
angle = acos(c);
Hard reset
2016-03-27 11:49:47 +02:00
if (angle > M_PI) {
return (float)M_PI;
}
if (angle < -M_PI) {
return (float)M_PI;
}
return angle;
}
//============================================================
/*
===============
LerpAngle
===============
*/
float LerpAngle (float from, float to, float frac) {
float a;
if ( to - from > 180 ) {
to -= 360;
}
if ( to - from < -180 ) {
to += 360;
}
a = from + frac * (to - from);
return a;
}
Fixed compilation errors on Unix
2023-02-01 00:28:40 +01:00
static int p[514];
static float g3[514][3];
static float g2[514][2];
static float g1[514];
Reversing Actor Final Part
2018-09-05 16:55:10 +02:00
static void init(void)
{
//This is an ugly func I wont bother variable naming.
int i;
for (i = 0; i < 256; i++)
{
Fixed compilation errors on Unix
2023-02-01 00:28:40 +01:00
p[i] = i;
g1[i] = (rand() % 512 - 256) * 0.00390625;
Reversing Actor Final Part
2018-09-05 16:55:10 +02:00
}
int v21, v22;
Fixed compilation errors on Unix
2023-02-01 00:28:40 +01:00
for (int j = i - 1; j; p[v22 % 256] = v21)
Reversing Actor Final Part
2018-09-05 16:55:10 +02:00
{
Fixed compilation errors on Unix
2023-02-01 00:28:40 +01:00
v21 = p[j];
Reversing Actor Final Part
2018-09-05 16:55:10 +02:00
v22 = rand();
Fixed compilation errors on Unix
2023-02-01 00:28:40 +01:00
p[j--] = p[v22 % 256];
Reversing Actor Final Part
2018-09-05 16:55:10 +02:00
}
for (size_t k = 0; k < 258; k++)
{
Fixed compilation errors on Unix
2023-02-01 00:28:40 +01:00
g1[k + 256] = g1[k];
Reversing Actor Final Part
2018-09-05 16:55:10 +02:00
}
}
static int start = 1;
float noise1(float arg)
{
Fixed compilation errors on Unix
2023-02-01 00:28:40 +01:00
float rx0;
float rx1;
float u;
float sx;
float t;
int bx0;
Reversing Actor Final Part
2018-09-05 16:55:10 +02:00
if (start)
{
start = 0;
Fixed compilation errors on Unix
2023-02-01 00:28:40 +01:00
init();
Reversing Actor Final Part
2018-09-05 16:55:10 +02:00
}
Fixed compilation errors on Unix
2023-02-01 00:28:40 +01:00
rx0 = arg + 4096.0;
bx0 = (int)rx0;
rx1 = rx0 - bx0;
sx = rx1 - 1.0;
t = rx1 * rx1 * (3.0 - (rx1 + rx1));
u = rx1 * g1[p[bx0]];
return u + t * (sx * g1[p[bx0 + 1]] - u);
Reversing Actor Final Part
2018-09-05 16:55:10 +02:00
}
Hard reset
2016-03-27 11:49:47 +02:00
/*
================
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 ];
}
/*
===============
LerpAngleFromCurrent
===============
*/
float LerpAngleFromCurrent( float from, float to, float current, float frac ) {
float a;
if( to - current > 180 ) {
to -= 360;
}
if( to - current < -180 ) {
to += 360;
}
a = from + frac * ( to - from );
return a;
}
/*
=================
AngleSubtract
Always returns a value from -180 to 180
=================
*/
float AngleSubtract( float a1, float a2 ) {
float a;
a = a1 - a2;
while ( a > 180 ) {
a -= 360;
}
while ( a < -180 ) {
a += 360;
}
return a;
}
void AnglesSubtract( vec3_t v1, vec3_t v2, vec3_t v3 ) {
v3[0] = AngleSubtract( v1[0], v2[0] );
v3[1] = AngleSubtract( v1[1], v2[1] );
v3[2] = AngleSubtract( v1[2], v2[2] );
}
float AngleMod(float a) {
a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
return a;
}
/*
=================
AngleNormalize360
returns angle normalized to the range [0 <= angle < 360]
=================
*/
float AngleNormalize360 ( float angle ) {
return (360.0 / 65536) * ((int)(angle * (65536 / 360.0)) & 65535);
}
/*
=================
AngleNormalize180
returns angle normalized to the range [-180 < angle <= 180]
=================
*/
float AngleNormalize180 ( float angle ) {
angle = AngleNormalize360( angle );
if ( angle > 180.0 ) {
angle -= 360.0;
}
return angle;
}
/*
=================
AngleDelta
returns the normalized delta from angle1 to angle2
=================
*/
float AngleDelta ( float angle1, float angle2 ) {
return AngleNormalize180( angle1 - angle2 );
}
//============================================================
/*
=================
SetPlaneSignbits
=================
*/
void SetPlaneSignbits (cplane_t *out) {
int bits, j;
// for fast box on planeside test
bits = 0;
for (j=0 ; j<3 ; j++) {
if (out->normal[j] < 0) {
bits |= 1<<j;
}
}
out->signbits = bits;
}
//============================================================================
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;
}
float angledist( float ang )
{
float a;
a = anglemod( ang );
if( a > 180 )
{
a -= 360;
}
return a;
}
int i;
vec3_t corners[ 2 ];
/*
==================
BoxOnPlaneSide
Returns 1, 2, or 1 + 2
// this is the slow, general version
int BoxOnPlaneSide2 (vec3_t emins, vec3_t emaxs, struct cplane_s *p)
{
int i;
float dist1, dist2;
int sides;
vec3_t corners[2];
for (i=0 ; i<3 ; i++)
{
if (p->normal[i] < 0)
{
corners[0][i] = emins[i];
corners[1][i] = emaxs[i];
}
else
{
corners[1][i] = emins[i];
corners[0][i] = emaxs[i];
}
}
dist1 = DotProduct (p->normal, corners[0]) - p->dist;
dist2 = DotProduct (p->normal, corners[1]) - p->dist;
sides = 0;
if (dist1 >= 0)
sides = 1;
if (dist2 < 0)
sides |= 2;
return sides;
}
==================
*/
Using const parameters for BoxOnPlaneSide
2023-07-17 20:08:26 +02:00
int BoxOnPlaneSide (const vec3_t emins, const vec3_t emaxs, struct cplane_s *p)
Hard reset
2016-03-27 11:49:47 +02:00
{
float dist1, dist2;
int sides;
// 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;
}
// 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
break;
}
sides = 0;
if (dist1 >= p->dist)
sides = 1;
if (dist2 < p->dist)
sides |= 2;
return sides;
}
/*
=================
CalculateRotatedBounds
=================
*/
void CalculateRotatedBounds( vec3_t angles, vec3_t mins, vec3_t maxs )
{
int i;
vec3_t rotmins, rotmaxs;
float trans[ 3 ][ 3 ];
AnglesToAxis( angles, trans );
ClearBounds( rotmins, rotmaxs );
for( i = 0; i < 8; i++ )
{
vec3_t tmp, rottemp;
if( i & 1 )
tmp[ 0 ] = mins[ 0 ];
else
tmp[ 0 ] = maxs[ 0 ];
if( i & 2 )
tmp[ 1 ] = mins[ 1 ];
else
tmp[ 1 ] = maxs[ 1 ];
if( i & 4 )
tmp[ 2 ] = mins[ 2 ];
else
tmp[ 2 ] = maxs[ 2 ];
MatrixTransformVector( tmp, trans, rottemp );
AddPointToBounds( rottemp, rotmins, rotmaxs );
}
VectorCopy( rotmins, mins );
VectorCopy( rotmaxs, maxs );
}
/*
=================
CalculateRotatedBounds2
=================
*/
void CalculateRotatedBounds2( float trans[ 3 ][ 3 ], vec3_t mins, vec3_t maxs )
{
int i;
vec3_t rotmins, rotmaxs;
ClearBounds( rotmins, rotmaxs );
for( i = 0; i < 8; i++ )
{
vec3_t tmp, rottemp;
if( i & 1 )
tmp[ 0 ] = mins[ 0 ];
else
tmp[ 0 ] = maxs[ 0 ];
if( i & 2 )
tmp[ 1 ] = mins[ 1 ];
else
tmp[ 1 ] = maxs[ 1 ];
if( i & 4 )
tmp[ 2 ] = mins[ 2 ];
else
tmp[ 2 ] = maxs[ 2 ];
MatrixTransformVector( tmp, trans, rottemp );
AddPointToBounds( rottemp, rotmins, rotmaxs );
}
VectorCopy( rotmins, mins );
VectorCopy( rotmaxs, maxs );
}
#define BBOX_XBITS 9
#define BBOX_YBITS 8
#define BBOX_ZBOTTOMBITS 5
#define BBOX_ZTOPBITS 9
#define BBOX_MAX_X ( 1 << BBOX_XBITS )
#define BBOX_MAX_Y ( 1 << BBOX_YBITS )
#define BBOX_MAX_BOTTOM_Z ( 1 << ( BBOX_ZBOTTOMBITS - 1 ) )
#define BBOX_REALMAX_BOTTOM_Z ( 1 << BBOX_ZBOTTOMBITS )
#define BBOX_MAX_TOP_Z ( 1 << BBOX_ZTOPBITS )
/*
=================
BoundingBoxToInteger
=================
*/
int BoundingBoxToInteger( vec3_t mins, vec3_t maxs )
{
int x, y, zd, zu, result;
x = maxs[ 0 ];
if( x < 0 )
x = 0;
if( x >= BBOX_MAX_X )
x = BBOX_MAX_X - 1;
y = maxs[ 1 ];
if( y < 0 )
y = 0;
if( y >= BBOX_MAX_Y )
y = BBOX_MAX_Y - 1;
zd = mins[ 2 ] + BBOX_MAX_BOTTOM_Z;
if( zd < 0 )
{
zd = 0;
}
if( zd >= BBOX_REALMAX_BOTTOM_Z )
{
zd = BBOX_REALMAX_BOTTOM_Z - 1;
}
zu = maxs[ 2 ];
if( zu < 0 )
zu = 0;
if( zu >= BBOX_MAX_TOP_Z )
zu = BBOX_MAX_TOP_Z - 1;
result = x |
( y << BBOX_XBITS ) |
( zd << ( BBOX_XBITS + BBOX_YBITS ) ) |
( zu << ( BBOX_XBITS + BBOX_YBITS + BBOX_ZBOTTOMBITS ) );
return result;
}
/*
=================
IntegerToBoundingBox
=================
*/
void IntegerToBoundingBox( int num, vec3_t mins, vec3_t maxs )
{
int x, y, zd, zu;
x = num & ( BBOX_MAX_X - 1 );
y = ( num >> ( BBOX_XBITS ) ) & ( BBOX_MAX_Y - 1 );
zd = ( num >> ( BBOX_XBITS + BBOX_YBITS ) ) & ( BBOX_REALMAX_BOTTOM_Z - 1 );
zd -= BBOX_MAX_BOTTOM_Z;
zu = ( num >> ( BBOX_XBITS + BBOX_YBITS + BBOX_ZBOTTOMBITS ) ) & ( BBOX_MAX_TOP_Z - 1 );
mins[ 0 ] = -x;
mins[ 1 ] = -y;
mins[ 2 ] = zd;
maxs[ 0 ] = x;
maxs[ 1 ] = y;
maxs[ 2 ] = zu;
}
/*
=================
RadiusFromBounds
=================
*/
float RadiusFromBounds( const vec3_t mins, const vec3_t maxs ) {
int i;
vec3_t corner;
float a, b;
for (i=0 ; i<3 ; i++) {
a = fabs( mins[i] );
b = fabs( maxs[i] );
corner[i] = a > b ? a : b;
}
return VectorLength (corner);
}
#define BOUNDS_CLEAR_VALUE 99999
void ClearBounds( vec3_t mins, vec3_t maxs ) {
mins[ 0 ] = mins[ 1 ] = mins[ 2 ] = BOUNDS_CLEAR_VALUE;
maxs[ 0 ] = maxs[ 1 ] = maxs[ 2 ] = -BOUNDS_CLEAR_VALUE;
}
qboolean BoundsClear( vec3_t mins, vec3_t maxs )
{
if(
( mins[ 0 ] == BOUNDS_CLEAR_VALUE ) &&
( mins[ 1 ] == BOUNDS_CLEAR_VALUE ) &&
( mins[ 2 ] == BOUNDS_CLEAR_VALUE ) &&
( maxs[ 0 ] == -BOUNDS_CLEAR_VALUE ) &&
( maxs[ 1 ] == -BOUNDS_CLEAR_VALUE ) &&
( maxs[ 2 ] == -BOUNDS_CLEAR_VALUE )
)
{
return qtrue;
}
else
{
return qfalse;
}
}
void AddPointToBounds( const vec3_t v, vec3_t mins, vec3_t maxs ) {
if( v[ 0 ] < mins[ 0 ] ) {
mins[ 0 ] = v[ 0 ];
}
if( v[ 0 ] > maxs[ 0 ] ) {
maxs[ 0 ] = v[ 0 ];
}
if( v[ 1 ] < mins[ 1 ] ) {
mins[ 1 ] = v[ 1 ];
}
if( v[ 1 ] > maxs[ 1 ] ) {
maxs[ 1 ] = v[ 1 ];
}
if( v[ 2 ] < mins[ 2 ] ) {
mins[ 2 ] = v[ 2 ];
}
if( v[ 2 ] > maxs[ 2 ] ) {
maxs[ 2 ] = v[ 2 ];
}
}
qboolean BoundsIntersect(const vec3_t mins, const vec3_t maxs,
const vec3_t mins2, const vec3_t maxs2)
{
if ( maxs[0] < mins2[0] ||
maxs[1] < mins2[1] ||
maxs[2] < mins2[2] ||
mins[0] > maxs2[0] ||
mins[1] > maxs2[1] ||
mins[2] > maxs2[2])
{
return qfalse;
}
return qtrue;
}
qboolean BoundsIntersectSphere(const vec3_t mins, const vec3_t maxs,
const vec3_t origin, vec_t radius)
{
if ( origin[0] - radius > maxs[0] ||
origin[0] + radius < mins[0] ||
origin[1] - radius > maxs[1] ||
origin[1] + radius < mins[1] ||
origin[2] - radius > maxs[2] ||
origin[2] + radius < mins[2])
{
return qfalse;
}
return qtrue;
}
qboolean BoundsIntersectPoint(const vec3_t mins, const vec3_t maxs,
const vec3_t origin)
{
if ( origin[0] > maxs[0] ||
origin[0] < mins[0] ||
origin[1] > maxs[1] ||
origin[1] < mins[1] ||
origin[2] > maxs[2] ||
origin[2] < mins[2])
{
return qfalse;
}
return qtrue;
}
void QuatToMat
(
const float q[ 4 ],
float m[ 3 ][ 3 ]
)
{
float wx, wy, wz;
float xx, yy, yz;
float xy, xz, zz;
float x2, y2, z2;
x2 = q[ X ] + q[ X ];
y2 = q[ Y ] + q[ Y ];
z2 = q[ Z ] + q[ Z ];
xx = q[ X ] * x2;
xy = q[ X ] * y2;
xz = q[ X ] * z2;
yy = q[ Y ] * y2;
yz = q[ Y ] * z2;
zz = q[ Z ] * z2;
wx = q[ W ] * x2;
wy = q[ W ] * y2;
wz = q[ W ] * z2;
m[ 0 ][ 0 ] = 1.0 - ( yy + zz );
m[ 0 ][ 1 ] = xy - wz;
m[ 0 ][ 2 ] = xz + wy;
m[ 1 ][ 0 ] = xy + wz;
m[ 1 ][ 1 ] = 1.0 - ( xx + zz );
m[ 1 ][ 2 ] = yz - wx;
m[ 2 ][ 0 ] = xz - wy;
m[ 2 ][ 1 ] = yz + wx;
m[ 2 ][ 2 ] = 1.0 - ( xx + yy );
}
void MatToQuat
(
float srcMatrix[ 3 ][ 3 ],
float destQuat[ 4 ]
)
{
double trace, s;
int i, j, k;
static int next[ 3 ] = { Y, Z, X };
trace = srcMatrix[ X ][ X ] + srcMatrix[ Y ][ Y ] + srcMatrix[ Z ][ Z ];
if( trace > 0.0 )
{
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
s = sqrtf( trace + 1.0 );
Hard reset
2016-03-27 11:49:47 +02:00
destQuat[ W ] = s * 0.5;
s = 0.5 / s;
destQuat[ X ] = ( srcMatrix[ Z ][ Y ] - srcMatrix[ Y ][ Z ] ) * s;
destQuat[ Y ] = ( srcMatrix[ X ][ Z ] - srcMatrix[ Z ][ X ] ) * s;
destQuat[ Z ] = ( srcMatrix[ Y ][ X ] - srcMatrix[ X ][ Y ] ) * s;
}
else
{
i = X;
if( srcMatrix[ Y ][ Y ] > srcMatrix[ X ][ X ] )
i = Y;
if( srcMatrix[ Z ][ Z ] > srcMatrix[ i ][ i ] )
i = Z;
j = next[ i ];
k = next[ j ];
s = sqrt( ( srcMatrix[ i ][ i ] - ( srcMatrix[ j ][ j ] + srcMatrix[ k ][ k ] ) ) + 1.0 );
destQuat[ i ] = s * 0.5;
s = 0.5 / s;
destQuat[ W ] = ( srcMatrix[ k ][ j ] - srcMatrix[ j ][ k ] ) * s;
destQuat[ j ] = ( srcMatrix[ j ][ i ] + srcMatrix[ i ][ j ] ) * s;
destQuat[ k ] = ( srcMatrix[ k ][ i ] + srcMatrix[ i ][ k ] ) * s;
}
}
Added SlerpQuaternion
2023-04-30 01:09:45 +02:00
#define DELTA 1e-6
void SlerpQuaternion
(
float from[4],
float to[4],
float t,
float res[4]
)
{
float to1[4];
double omega, cosom, sinom, scale0, scale1;
cosom = from[X] * to[X] + from[Y] * to[Y] + from[Z] * to[Z] + from[W] * to[W];
if (cosom < 0.0)
{
cosom = -cosom;
to1[X] = -to[X];
to1[Y] = -to[Y];
to1[Z] = -to[Z];
to1[W] = -to[W];
}
else if
(
(from[X] == to[X]) &&
(from[Y] == to[Y]) &&
(from[Z] == to[Z]) &&
(from[W] == to[W])
)
{
// equal case, early exit
res[X] = to[X];
res[Y] = to[Y];
res[Z] = to[Z];
res[W] = to[W];
return;
}
else
{
to1[X] = to[X];
to1[Y] = to[Y];
to1[Z] = to[Z];
to1[W] = to[W];
}
if ((1.0 - cosom) > DELTA)
{
omega = acos(cosom);
sinom = sin(omega);
scale0 = sin((1.0 - t) * omega) / sinom;
scale1 = sin(t * omega) / sinom;
}
else
{
scale0 = 1.0 - t;
scale1 = t;
}
res[X] = scale0 * from[X] + scale1 * to1[X];
res[Y] = scale0 * from[Y] + scale1 * to1[Y];
res[Z] = scale0 * from[Z] + scale1 * to1[Z];
res[W] = scale0 * from[W] + scale1 * to1[W];
}
Hard reset
2016-03-27 11:49:47 +02:00
void EulerToQuat
(
float ang[ 3 ],
float q[ 4 ]
)
{
float mat[ 3 ][ 3 ];
int *i;
i = ( int * )ang;
if( !i[ 0 ] && !i[ 1 ] && !i[ 2 ] )
{
q[ 0 ] = 0;
q[ 1 ] = 0;
q[ 2 ] = 0;
q[ 3 ] = 1.0f;
}
else
{
AnglesToAxis( ang, mat );
MatToQuat( mat, q );
}
}
float ProjectPointOnLine
(
Replaced vec_t with vec3_t
2023-07-10 21:10:23 +02:00
const vec3_t i_vStart,
const vec3_t i_vEnd,
const vec3_t i_vPoint,
vec3_t o_vProj
Hard reset
2016-03-27 11:49:47 +02:00
)
{
float fDot;
vec3_t vDir;
vec3_t vDelta;
VectorSubtract( i_vEnd, i_vStart, vDir );
VectorNormalizeFast( vDir );
VectorSubtract( i_vPoint, i_vStart, vDelta );
fDot = DotProduct( vDelta, vDir );
VectorScale( vDir, fDot, o_vProj );
VectorAdd( o_vProj, i_vStart, o_vProj );
return fDot;
}
float ProjectLineOnPlane
(
Replaced vec_t with vec3_t
2023-07-10 21:10:23 +02:00
const vec3_t vPlaneNorm,
Hard reset
2016-03-27 11:49:47 +02:00
float fPlaneDist,
Replaced vec_t with vec3_t
2023-07-10 21:10:23 +02:00
const vec3_t vStart,
const vec3_t vEnd,
vec3_t vProj
Hard reset
2016-03-27 11:49:47 +02:00
)
{
float d1;
float d2;
float f;
d1 = DotProduct( vStart, vPlaneNorm ) - fPlaneDist;
d2 = DotProduct( vEnd, vPlaneNorm ) - fPlaneDist;
if( d1 == d2 )
{
if( vProj )
{
VectorCopy( vStart, vProj );
}
return 0.0f;
}
else
{
f = d1 / ( d1 - d2 );
if( vProj )
{
VectorSubtract( vEnd, vStart, vProj );
VectorScale( vProj, f, vProj );
VectorAdd( vProj, vStart, vProj );
}
return f;
}
}
vec_t VectorNormalize( vec3_t v ) {
// NOTE: TTimo - Apple G4 altivec source uses double?
float length, ilength;
length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
length = sqrt (length);
if ( length ) {
ilength = 1/length;
v[0] *= ilength;
v[1] *= ilength;
v[2] *= ilength;
}
return length;
}
vec_t VectorNormalize2( const vec3_t v, vec3_t out ) {
float length, ilength;
length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
length = sqrt (length);
if (length)
{
ilength = 1/length;
out[0] = v[0]*ilength;
out[1] = v[1]*ilength;
out[2] = v[2]*ilength;
} else {
VectorClear( out );
}
return length;
}
vec_t VectorNormalize2D( vec2_t v ) {
float length, ilength;
Reversing Actor p3
2018-08-19 08:26:59 +02:00
length = VectorLength2D(v);
Hard reset
2016-03-27 11:49:47 +02:00
if( length ) {
ilength = 1 / length;
v[ 0 ] *= ilength;
v[ 1 ] *= ilength;
}
return length;
}
vec_t VectorNormalize2D2( const vec2_t v, vec2_t out ) {
float length, ilength;
length = v[ 0 ] * v[ 0 ] + v[ 1 ] * v[ 1 ];
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
length = sqrtf( length );
Hard reset
2016-03-27 11:49:47 +02:00
if( length )
{
ilength = 1 / length;
out[ 0 ] = v[ 0 ] * ilength;
out[ 1 ] = v[ 1 ] * ilength;
}
else {
VectorClear2D( out );
}
return length;
}
void VectorPackTo01( vec3_t v ) {
VectorNormalize( v );
VectorScale( v, 0.5f, v );
v[ 0 ] += 0.5f;
v[ 1 ] += 0.5f;
v[ 2 ] += 0.5f;
}
Lot of changes
2016-08-13 18:32:13 +02:00
vec_t Q_rint( vec_t in )
{
return floor( in + 0.5 );
}
Hard reset
2016-03-27 11:49:47 +02:00
void _VectorMA( const vec3_t veca, float scale, const 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( const vec3_t v1, const vec3_t v2 ) {
return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
}
void _VectorSubtract( const vec3_t veca, const 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( const vec3_t veca, const 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( const vec3_t in, vec3_t out ) {
out[0] = in[0];
out[1] = in[1];
out[2] = in[2];
}
void _VectorScale( const vec3_t in, vec_t scale, vec3_t out ) {
out[0] = in[0]*scale;
out[1] = in[1]*scale;
out[2] = in[2]*scale;
}
void Vector4Scale( const vec4_t in, vec_t scale, vec4_t out ) {
out[0] = in[0]*scale;
out[1] = in[1]*scale;
out[2] = in[2]*scale;
out[3] = in[3]*scale;
}
int NearestPowerOfTwo(int val)
{
int answer;
for(answer = 1; answer < val; answer <<= 1)
;
return answer;
}
int Q_log2( int val ) {
int answer;
answer = 0;
while ( ( val>>=1 ) != 0 ) {
answer++;
}
return answer;
}
/*
=================
PlaneTypeForNormal
=================
*/
/*
int PlaneTypeForNormal (vec3_t normal) {
if ( normal[0] == 1.0 )
return PLANE_X;
if ( normal[1] == 1.0 )
return PLANE_Y;
if ( normal[2] == 1.0 )
return PLANE_Z;
return PLANE_NON_AXIAL;
}
*/
/*
================
MatrixTransformVector
================
*/
void MatrixTransformVector
(
const vec3_t in,
const float mat[ 3 ][ 3 ],
vec3_t out
)
{
out[ 0 ] = ( in[ 0 ] * mat[ 0 ][ 0 ] ) + ( in[ 1 ] * mat[ 1 ][ 0 ] ) + ( in[ 2 ] * mat[ 2 ][ 0 ] );
out[ 1 ] = ( in[ 0 ] * mat[ 0 ][ 1 ] ) + ( in[ 1 ] * mat[ 1 ][ 1 ] ) + ( in[ 2 ] * mat[ 2 ][ 1 ] );
out[ 2 ] = ( in[ 0 ] * mat[ 0 ][ 2 ] ) + ( in[ 1 ] * mat[ 1 ][ 2 ] ) + ( in[ 2 ] * mat[ 2 ][ 2 ] );
}
/*
================
MatrixTransformVectorRight
================
*/
void MatrixTransformVectorRight
(
const float mat[ 3 ][ 3 ],
const vec3_t in,
vec3_t out
)
{
out[ 0 ] = ( in[ 0 ] * mat[ 0 ][ 0 ] ) + ( in[ 1 ] * mat[ 0 ][ 1 ] ) + ( in[ 2 ] * mat[ 0 ][ 2 ] );
out[ 1 ] = ( in[ 0 ] * mat[ 1 ][ 0 ] ) + ( in[ 1 ] * mat[ 1 ][ 1 ] ) + ( in[ 2 ] * mat[ 1 ][ 2 ] );
out[ 2 ] = ( in[ 0 ] * mat[ 2 ][ 0 ] ) + ( in[ 1 ] * mat[ 2 ][ 1 ] ) + ( in[ 2 ] * mat[ 2 ][ 2 ] );
}
/*
================
Matrix3x3Multiply
================
*/
void Matrix3x3Multiply( const float in1[ 3 ][ 3 ], const 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 AngleVectors( const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up ) {
float angle;
static float sr, sp, sy, cr, cp, cy;
// static to help MS compiler fp bugs
angle = angles[ YAW ] * ( M_PI * 2 / 360 );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sy = sin( angle );
cy = cos( angle );
Hard reset
2016-03-27 11:49:47 +02:00
angle = angles[ PITCH ] * ( M_PI * 2 / 360 );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sp = sin( angle );
cp = cos( angle );
Hard reset
2016-03-27 11:49:47 +02:00
angle = angles[ ROLL ] * ( M_PI * 2 / 360 );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sr = sin( angle );
cr = cos( angle );
Hard reset
2016-03-27 11:49:47 +02:00
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;
}
}
void AngleVectorsLeft( const vec3_t angles, vec3_t forward, vec3_t left, vec3_t up )
{
float angle;
static float sr, sp, sy, cr, cp, cy;
// static to help MS compiler fp bugs
angle = angles[ YAW ] * ( M_PI * 2 / 360 );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sy = sin( angle );
cy = cos( angle );
Hard reset
2016-03-27 11:49:47 +02:00
angle = angles[ PITCH ] * ( M_PI * 2 / 360 );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sp = sin( angle );
cp = cos( angle );
Hard reset
2016-03-27 11:49:47 +02:00
if( forward )
{
forward[ 0 ] = cp*cy;
forward[ 1 ] = cp*sy;
forward[ 2 ] = -sp;
}
if( left || up )
{
angle = angles[ ROLL ] * ( M_PI * 2 / 360 );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sr = sin( angle );
cr = cos( angle );
Hard reset
2016-03-27 11:49:47 +02:00
if( left )
{
left[ 0 ] = ( sr*sp*cy + cr*-sy );
left[ 1 ] = ( sr*sp*sy + cr*cy );
left[ 2 ] = sr*cp;
}
if( up )
{
up[ 0 ] = ( cr*sp*cy + -sr*-sy );
up[ 1 ] = ( cr*sp*sy + -sr*cy );
up[ 2 ] = cr*cp;
}
}
}
/*
** 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 );
}
/*
================
Q_isnan
Don't pass doubles to this
================
*/
int Q_isnan( float x )
{
union
{
float f;
unsigned int i;
} t;
t.f = x;
t.i &= 0x7FFFFFFF;
t.i = 0x7F800000 - t.i;
return (int)( (unsigned int)t.i >> 31 );
}
/*
=================
GetPerpendicularViewVector
Used to find an "up" vector for drawing a sprite so that it always faces the view as best as possible
=================
*/
void GetPerpendicularViewVector(const vec3_t point, const vec3_t p1, const vec3_t p2, vec3_t up)
{
vec3_t v1, v2;
VectorSubtract(point, p1, v1);
VectorNormalize(v1);
VectorSubtract(point, p2, v2);
VectorNormalize(v2);
CrossProduct(v1, v2, up);
VectorNormalize(up);
}
/*
================
ProjectPointOntoVector
================
*/
void ProjectPointOntoVector(vec3_t point, vec3_t vStart, vec3_t vEnd, vec3_t vProj)
{
vec3_t pVec, vec;
VectorSubtract(point, vStart, pVec);
VectorSubtract(vEnd, vStart, vec);
VectorNormalize(vec);
// project onto the directional vector for this segment
VectorMA(vStart, DotProduct(pVec, vec), vec, vProj);
}
/*
================
VectorMaxComponent
Return the biggest component of some vector
================
*/
float VectorMaxComponent(vec3_t v)
{
float biggest = v[0];
if(v[1] > biggest)
biggest = v[1];
if(v[2] > biggest)
biggest = v[2];
return biggest;
}
/*
================
VectorMinComponent
Return the smallest component of some vector
================
*/
float VectorMinComponent(vec3_t v)
{
float smallest = v[0];
if(v[1] < smallest)
smallest = v[1];
if(v[2] < smallest)
smallest = v[2];
return smallest;
}
#define LINE_DISTANCE_EPSILON 1e-05f
/*
================
DistanceBetweenLineSegmentsSquared
Return the smallest distance between two line segments, squared
================
*/
vec_t DistanceBetweenLineSegmentsSquared(const vec3_t sP0, const vec3_t sP1,
const vec3_t tP0, const vec3_t tP1, float *s, float *t)
{
vec3_t sMag, tMag, diff;
float a, b, c, d, e;
float D;
float sN, sD;
float tN, tD;
vec3_t separation;
VectorSubtract(sP1, sP0, sMag);
VectorSubtract(tP1, tP0, tMag);
VectorSubtract(sP0, tP0, diff);
a = DotProduct(sMag, sMag);
b = DotProduct(sMag, tMag);
c = DotProduct(tMag, tMag);
d = DotProduct(sMag, diff);
e = DotProduct(tMag, diff);
sD = tD = D = a * c - b * b;
if(D < LINE_DISTANCE_EPSILON)
{
// the lines are almost parallel
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else
{
// get the closest points on the infinite lines
sN = (b * e - c * d);
tN = (a * e - b * d);
if(sN < 0.0)
{
// sN < 0 => the s=0 edge is visible
sN = 0.0;
tN = e;
tD = c;
}
else if(sN > sD)
{
// sN > sD => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if(tN < 0.0)
{
// tN < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sN for this edge
if(-d < 0.0)
sN = 0.0;
else if(-d > a)
sN = sD;
else
{
sN = -d;
sD = a;
}
}
else if(tN > tD)
{
// tN > tD => the t=1 edge is visible
tN = tD;
// recompute sN for this edge
if((-d + b) < 0.0)
sN = 0;
else if((-d + b) > a)
sN = sD;
else
{
sN = (-d + b);
sD = a;
}
}
// finally do the division to get *s and *t
*s = (fabs(sN) < LINE_DISTANCE_EPSILON ? 0.0 : sN / sD);
*t = (fabs(tN) < LINE_DISTANCE_EPSILON ? 0.0 : tN / tD);
// get the difference of the two closest points
VectorScale(sMag, *s, sMag);
VectorScale(tMag, *t, tMag);
VectorAdd(diff, sMag, separation);
VectorSubtract(separation, tMag, separation);
return VectorLengthSquared(separation);
}
/*
================
DistanceBetweenLineSegments
Return the smallest distance between two line segments
================
*/
vec_t DistanceBetweenLineSegments(const vec3_t sP0, const vec3_t sP1, const vec3_t tP0, const vec3_t tP1, float *s, float *t)
{
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
return (vec_t) sqrtf(DistanceBetweenLineSegmentsSquared(sP0, sP1, tP0, tP1, s, t));
Hard reset
2016-03-27 11:49:47 +02:00
}
void VectorMatrixInverse( void* DstMatrix, const void* SrcMatrix )
{
typedef float Float4x4[ 4 ][ 4 ];
const Float4x4 M;
Float4x4 Result;
float Det[ 4 ];
Float4x4 Tmp;
memcpy( ( void * )M, SrcMatrix, sizeof( Float4x4 ) );
Tmp[ 0 ][ 0 ] = M[ 2 ][ 2 ] * M[ 3 ][ 3 ] - M[ 2 ][ 3 ] * M[ 3 ][ 2 ];
Tmp[ 0 ][ 1 ] = M[ 1 ][ 2 ] * M[ 3 ][ 3 ] - M[ 1 ][ 3 ] * M[ 3 ][ 2 ];
Tmp[ 0 ][ 2 ] = M[ 1 ][ 2 ] * M[ 2 ][ 3 ] - M[ 1 ][ 3 ] * M[ 2 ][ 2 ];
Tmp[ 1 ][ 0 ] = M[ 2 ][ 2 ] * M[ 3 ][ 3 ] - M[ 2 ][ 3 ] * M[ 3 ][ 2 ];
Tmp[ 1 ][ 1 ] = M[ 0 ][ 2 ] * M[ 3 ][ 3 ] - M[ 0 ][ 3 ] * M[ 3 ][ 2 ];
Tmp[ 1 ][ 2 ] = M[ 0 ][ 2 ] * M[ 2 ][ 3 ] - M[ 0 ][ 3 ] * M[ 2 ][ 2 ];
Tmp[ 2 ][ 0 ] = M[ 1 ][ 2 ] * M[ 3 ][ 3 ] - M[ 1 ][ 3 ] * M[ 3 ][ 2 ];
Tmp[ 2 ][ 1 ] = M[ 0 ][ 2 ] * M[ 3 ][ 3 ] - M[ 0 ][ 3 ] * M[ 3 ][ 2 ];
Tmp[ 2 ][ 2 ] = M[ 0 ][ 2 ] * M[ 1 ][ 3 ] - M[ 0 ][ 3 ] * M[ 1 ][ 2 ];
Tmp[ 3 ][ 0 ] = M[ 1 ][ 2 ] * M[ 2 ][ 3 ] - M[ 1 ][ 3 ] * M[ 2 ][ 2 ];
Tmp[ 3 ][ 1 ] = M[ 0 ][ 2 ] * M[ 2 ][ 3 ] - M[ 0 ][ 3 ] * M[ 2 ][ 2 ];
Tmp[ 3 ][ 2 ] = M[ 0 ][ 2 ] * M[ 1 ][ 3 ] - M[ 0 ][ 3 ] * M[ 1 ][ 2 ];
Det[ 0 ] = M[ 1 ][ 1 ] * Tmp[ 0 ][ 0 ] - M[ 2 ][ 1 ] * Tmp[ 0 ][ 1 ] + M[ 3 ][ 1 ] * Tmp[ 0 ][ 2 ];
Det[ 1 ] = M[ 0 ][ 1 ] * Tmp[ 1 ][ 0 ] - M[ 2 ][ 1 ] * Tmp[ 1 ][ 1 ] + M[ 3 ][ 1 ] * Tmp[ 1 ][ 2 ];
Det[ 2 ] = M[ 0 ][ 1 ] * Tmp[ 2 ][ 0 ] - M[ 1 ][ 1 ] * Tmp[ 2 ][ 1 ] + M[ 3 ][ 1 ] * Tmp[ 2 ][ 2 ];
Det[ 3 ] = M[ 0 ][ 1 ] * Tmp[ 3 ][ 0 ] - M[ 1 ][ 1 ] * Tmp[ 3 ][ 1 ] + M[ 2 ][ 1 ] * Tmp[ 3 ][ 2 ];
float Determinant = M[ 0 ][ 0 ] * Det[ 0 ] - M[ 1 ][ 0 ] * Det[ 1 ] + M[ 2 ][ 0 ] * Det[ 2 ] - M[ 3 ][ 0 ] * Det[ 3 ];
const float RDet = 1.0f / Determinant;
Result[ 0 ][ 0 ] = RDet * Det[ 0 ];
Result[ 0 ][ 1 ] = -RDet * Det[ 1 ];
Result[ 0 ][ 2 ] = RDet * Det[ 2 ];
Result[ 0 ][ 3 ] = -RDet * Det[ 3 ];
Result[ 1 ][ 0 ] = -RDet * ( M[ 1 ][ 0 ] * Tmp[ 0 ][ 0 ] - M[ 2 ][ 0 ] * Tmp[ 0 ][ 1 ] + M[ 3 ][ 0 ] * Tmp[ 0 ][ 2 ] );
Result[ 1 ][ 1 ] = RDet * ( M[ 0 ][ 0 ] * Tmp[ 1 ][ 0 ] - M[ 2 ][ 0 ] * Tmp[ 1 ][ 1 ] + M[ 3 ][ 0 ] * Tmp[ 1 ][ 2 ] );
Result[ 1 ][ 2 ] = -RDet * ( M[ 0 ][ 0 ] * Tmp[ 2 ][ 0 ] - M[ 1 ][ 0 ] * Tmp[ 2 ][ 1 ] + M[ 3 ][ 0 ] * Tmp[ 2 ][ 2 ] );
Result[ 1 ][ 3 ] = RDet * ( M[ 0 ][ 0 ] * Tmp[ 3 ][ 0 ] - M[ 1 ][ 0 ] * Tmp[ 3 ][ 1 ] + M[ 2 ][ 0 ] * Tmp[ 3 ][ 2 ] );
Result[ 2 ][ 0 ] = RDet * (
M[ 1 ][ 0 ] * ( M[ 2 ][ 1 ] * M[ 3 ][ 3 ] - M[ 2 ][ 3 ] * M[ 3 ][ 1 ] ) -
M[ 2 ][ 0 ] * ( M[ 1 ][ 1 ] * M[ 3 ][ 3 ] - M[ 1 ][ 3 ] * M[ 3 ][ 1 ] ) +
M[ 3 ][ 0 ] * ( M[ 1 ][ 1 ] * M[ 2 ][ 3 ] - M[ 1 ][ 3 ] * M[ 2 ][ 1 ] )
);
Result[ 2 ][ 1 ] = -RDet * (
M[ 0 ][ 0 ] * ( M[ 2 ][ 1 ] * M[ 3 ][ 3 ] - M[ 2 ][ 3 ] * M[ 3 ][ 1 ] ) -
M[ 2 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 3 ][ 3 ] - M[ 0 ][ 3 ] * M[ 3 ][ 1 ] ) +
M[ 3 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 2 ][ 3 ] - M[ 0 ][ 3 ] * M[ 2 ][ 1 ] )
);
Result[ 2 ][ 2 ] = RDet * (
M[ 0 ][ 0 ] * ( M[ 1 ][ 1 ] * M[ 3 ][ 3 ] - M[ 1 ][ 3 ] * M[ 3 ][ 1 ] ) -
M[ 1 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 3 ][ 3 ] - M[ 0 ][ 3 ] * M[ 3 ][ 1 ] ) +
M[ 3 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 1 ][ 3 ] - M[ 0 ][ 3 ] * M[ 1 ][ 1 ] )
);
Result[ 2 ][ 3 ] = -RDet * (
M[ 0 ][ 0 ] * ( M[ 1 ][ 1 ] * M[ 2 ][ 3 ] - M[ 1 ][ 3 ] * M[ 2 ][ 1 ] ) -
M[ 1 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 2 ][ 3 ] - M[ 0 ][ 3 ] * M[ 2 ][ 1 ] ) +
M[ 2 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 1 ][ 3 ] - M[ 0 ][ 3 ] * M[ 1 ][ 1 ] )
);
Result[ 3 ][ 0 ] = -RDet * (
M[ 1 ][ 0 ] * ( M[ 2 ][ 1 ] * M[ 3 ][ 2 ] - M[ 2 ][ 2 ] * M[ 3 ][ 1 ] ) -
M[ 2 ][ 0 ] * ( M[ 1 ][ 1 ] * M[ 3 ][ 2 ] - M[ 1 ][ 2 ] * M[ 3 ][ 1 ] ) +
M[ 3 ][ 0 ] * ( M[ 1 ][ 1 ] * M[ 2 ][ 2 ] - M[ 1 ][ 2 ] * M[ 2 ][ 1 ] )
);
Result[ 3 ][ 1 ] = RDet * (
M[ 0 ][ 0 ] * ( M[ 2 ][ 1 ] * M[ 3 ][ 2 ] - M[ 2 ][ 2 ] * M[ 3 ][ 1 ] ) -
M[ 2 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 3 ][ 2 ] - M[ 0 ][ 2 ] * M[ 3 ][ 1 ] ) +
M[ 3 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 2 ][ 2 ] - M[ 0 ][ 2 ] * M[ 2 ][ 1 ] )
);
Result[ 3 ][ 2 ] = -RDet * (
M[ 0 ][ 0 ] * ( M[ 1 ][ 1 ] * M[ 3 ][ 2 ] - M[ 1 ][ 2 ] * M[ 3 ][ 1 ] ) -
M[ 1 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 3 ][ 2 ] - M[ 0 ][ 2 ] * M[ 3 ][ 1 ] ) +
M[ 3 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 1 ][ 2 ] - M[ 0 ][ 2 ] * M[ 1 ][ 1 ] )
);
Result[ 3 ][ 3 ] = RDet * (
M[ 0 ][ 0 ] * ( M[ 1 ][ 1 ] * M[ 2 ][ 2 ] - M[ 1 ][ 2 ] * M[ 2 ][ 1 ] ) -
M[ 1 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 2 ][ 2 ] - M[ 0 ][ 2 ] * M[ 2 ][ 1 ] ) +
M[ 2 ][ 0 ] * ( M[ 0 ][ 1 ] * M[ 1 ][ 2 ] - M[ 0 ][ 2 ] * M[ 1 ][ 1 ] )
);
memcpy( DstMatrix, &Result, 16 * sizeof( float ) );
}
// *INDENT-OFF*
void MatrixIdentity(matrix_t m)
{
m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = 0;
m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixClear(matrix_t m)
{
m[ 0] = 0; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
m[ 1] = 0; m[ 5] = 0; m[ 9] = 0; m[13] = 0;
m[ 2] = 0; m[ 6] = 0; m[10] = 0; m[14] = 0;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 0;
}
void MatrixCopy(const matrix_t in, matrix_t out)
{
#if id386_sse && defined __GNUC__ && 0
asm volatile
(
"movups (%%edx), %%xmm0\n"
"movups 0x10(%%edx), %%xmm1\n"
"movups 0x20(%%edx), %%xmm2\n"
"movups 0x30(%%edx), %%xmm3\n"
"movups %%xmm0, (%%eax)\n"
"movups %%xmm1, 0x10(%%eax)\n"
"movups %%xmm2, 0x20(%%eax)\n"
"movups %%xmm3, 0x30(%%eax)\n"
:
: "a"( out ), "d"( in )
: "memory"
);
#elif id386_3dnow && defined __GNUC__
asm volatile
(
"femms\n"
"movq (%%edx), %%mm0\n"
"movq 8(%%edx), %%mm1\n"
"movq 16(%%edx), %%mm2\n"
"movq 24(%%edx), %%mm3\n"
"movq 32(%%edx), %%mm4\n"
"movq 40(%%edx), %%mm5\n"
"movq 48(%%edx), %%mm6\n"
"movq 56(%%edx), %%mm7\n"
"movq %%mm0, (%%eax)\n"
"movq %%mm1, 8(%%eax)\n"
"movq %%mm2, 16(%%eax)\n"
"movq %%mm3, 24(%%eax)\n"
"movq %%mm4, 32(%%eax)\n"
"movq %%mm5, 40(%%eax)\n"
"movq %%mm6, 48(%%eax)\n"
"movq %%mm7, 56(%%eax)\n"
"femms\n"
:
: "a"(out), "d"(in)
: "memory"
);
#else
out[ 0] = in[ 0]; out[ 4] = in[ 4]; out[ 8] = in[ 8]; out[12] = in[12];
out[ 1] = in[ 1]; out[ 5] = in[ 5]; out[ 9] = in[ 9]; out[13] = in[13];
out[ 2] = in[ 2]; out[ 6] = in[ 6]; out[10] = in[10]; out[14] = in[14];
out[ 3] = in[ 3]; out[ 7] = in[ 7]; out[11] = in[11]; out[15] = in[15];
#endif
}
qboolean MatrixCompare(const matrix_t a, const matrix_t b)
{
return (a[ 0] == b[ 0] && a[ 4] == b[ 4] && a[ 8] == b[ 8] && a[12] == b[12] &&
a[ 1] == b[ 1] && a[ 5] == b[ 5] && a[ 9] == b[ 9] && a[13] == b[13] &&
a[ 2] == b[ 2] && a[ 6] == b[ 6] && a[10] == b[10] && a[14] == b[14] &&
a[ 3] == b[ 3] && a[ 7] == b[ 7] && a[11] == b[11] && a[15] == b[15]);
}
void MatrixTransposeIntoXMM(const matrix_t m)
{
#if id386_sse && defined __GNUC__ && 0
asm volatile
( // reg[0] | reg[1] | reg[2] | reg[3]
// load transpose into XMM registers
"movlps (%%eax), %%xmm4\n" // m[0][0] | m[0][1] | - | -
"movhps 16(%%eax), %%xmm4\n" // m[0][0] | m[0][1] | m[1][0] | m[1][1]
"movlps 32(%%eax), %%xmm3\n" // m[2][0] | m[2][1] | - | -
"movhps 48(%%eax), %%xmm3\n" // m[2][0] | m[2][1] | m[3][0] | m[3][1]
"movups %%xmm4, %%xmm5\n" // m[0][0] | m[0][1] | m[1][0] | m[1][1]
// 0x88 = 10 00 | 10 00 <-> 00 10 | 00 10 xmm4[00] xmm4[10] xmm3[00] xmm3[10]
"shufps $0x88, %%xmm3, %%xmm4\n" // m[0][0] | m[1][0] | m[2][0] | m[3][0]
// 0xDD = 11 01 | 11 01 <-> 01 11 | 01 11 xmm5[01] xmm5[11] xmm3[01] xmm3[11]
"shufps $0xDD, %%xmm3, %%xmm5\n" // m[0][1] | m[1][1] | m[2][1] | m[3][1]
"movlps 8(%%eax), %%xmm6\n" // m[0][2] | m[0][3] | - | -
"movhps 24(%%eax), %%xmm6\n" // m[0][2] | m[0][3] | m[1][2] | m[1][3]
"movlps 40(%%eax), %%xmm3\n" // m[2][2] | m[2][3] | - | -
"movhps 56(%%eax), %%xmm3\n" // m[2][2] | m[2][3] | m[3][2] | m[3][3]
"movups %%xmm6, %%xmm7\n" // m[0][2] | m[0][3] | m[1][2] | m[1][3]
// 0x88 = 10 00 | 10 00 <-> 00 10 | 00 10 xmm6[00] xmm6[10] xmm3[00] xmm3[10]
"shufps $0x88, %%xmm3, %%xmm6\n" // m[0][2] | m[1][2] | m[2][2] | m[3][2]
// 0xDD = 11 01 | 11 01 <-> 01 11 | 01 11 xmm7[01] xmm7[11] xmm3[01] xmm3[11]
"shufps $0xDD, %%xmm3, %%xmm7\n" // m[0][3] | m[1][3] | m[2][3] | m[3][3]
:
: "a"( m )
: "memory"
);
#endif
}
void MatrixTranspose(const matrix_t in, matrix_t out)
{
#if id386_sse && defined __GNUC__ && 0
// transpose the matrix into the xmm4-7
MatrixTransposeIntoXMM(in);
asm volatile
(
"movups %%xmm4, (%%eax)\n"
"movups %%xmm5, 0x10(%%eax)\n"
"movups %%xmm6, 0x20(%%eax)\n"
"movups %%xmm7, 0x30(%%eax)\n"
:
: "a"( out )
: "memory"
);
#else
out[ 0] = in[ 0]; out[ 1] = in[ 4]; out[ 2] = in[ 8]; out[ 3] = in[12];
out[ 4] = in[ 1]; out[ 5] = in[ 5]; out[ 6] = in[ 9]; out[ 7] = in[13];
out[ 8] = in[ 2]; out[ 9] = in[ 6]; out[10] = in[10]; out[11] = in[14];
out[12] = in[ 3]; out[13] = in[ 7]; out[14] = in[11]; out[15] = in[15];
#endif
}
// helper functions for MatrixInverse from GtkRadiant C mathlib
static float m3_det( matrix3x3_t mat )
{
float det;
det = mat[0] * ( mat[4]*mat[8] - mat[7]*mat[5] )
- mat[1] * ( mat[3]*mat[8] - mat[6]*mat[5] )
+ mat[2] * ( mat[3]*mat[7] - mat[6]*mat[4] );
return( det );
}
/*static int m3_inverse( matrix3x3_t mr, matrix3x3_t ma )
{
float det = m3_det( ma );
if (det == 0 )
{
return 1;
}
mr[0] = ma[4]*ma[8] - ma[5]*ma[7] / det;
mr[1] = -( ma[1]*ma[8] - ma[7]*ma[2] ) / det;
mr[2] = ma[1]*ma[5] - ma[4]*ma[2] / det;
mr[3] = -( ma[3]*ma[8] - ma[5]*ma[6] ) / det;
mr[4] = ma[0]*ma[8] - ma[6]*ma[2] / det;
mr[5] = -( ma[0]*ma[5] - ma[3]*ma[2] ) / det;
mr[6] = ma[3]*ma[7] - ma[6]*ma[4] / det;
mr[7] = -( ma[0]*ma[7] - ma[6]*ma[1] ) / det;
mr[8] = ma[0]*ma[4] - ma[1]*ma[3] / det;
return 0;
}*/
static void m4_submat( matrix_t mr, matrix3x3_t mb, int i, int j )
{
int ti, tj, idst = 0, jdst = 0;
for ( ti = 0; ti < 4; ti++ )
{
if ( ti < i )
idst = ti;
else
if ( ti > i )
idst = ti-1;
for ( tj = 0; tj < 4; tj++ )
{
if ( tj < j )
jdst = tj;
else
if ( tj > j )
jdst = tj-1;
if ( ti != i && tj != j )
mb[idst*3 + jdst] = mr[ti*4 + tj ];
}
}
}
static float m4_det( matrix_t mr )
{
float det, result = 0, i = 1;
matrix3x3_t msub3;
int n;
for ( n = 0; n < 4; n++, i *= -1 )
{
m4_submat( mr, msub3, 0, n );
det = m3_det( msub3 );
result += mr[n] * det * i;
}
return result;
}
qboolean MatrixInverse(matrix_t matrix)
{
float mdet = m4_det(matrix);
matrix3x3_t mtemp;
int i, j, sign;
matrix_t m4x4_temp;
#if 0
if ( fabs( mdet ) < 0.0000000001 )
return qtrue;
#endif
MatrixCopy(matrix, m4x4_temp);
for ( i = 0; i < 4; i++ )
for ( j = 0; j < 4; j++ )
{
sign = 1 - ( (i +j) % 2 ) * 2;
m4_submat( m4x4_temp, mtemp, i, j );
// FIXME: try using * inverse det and see if speed/accuracy are good enough
matrix[i+j*4] = ( m3_det( mtemp ) * sign ) / mdet;
}
return qfalse;
}
void MatrixSetupXRotation(matrix_t m, vec_t degrees)
{
vec_t a = DEG2RAD(degrees);
m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
m[ 1] = 0; m[ 5] = cos(a); m[ 9] =-sin(a); m[13] = 0;
m[ 2] = 0; m[ 6] = sin(a); m[10] = cos(a); m[14] = 0;
Hard reset
2016-03-27 11:49:47 +02:00
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixSetupYRotation(matrix_t m, vec_t degrees)
{
vec_t a = DEG2RAD(degrees);
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
m[ 0] = cos(a); m[ 4] = 0; m[ 8] = sin(a); m[12] = 0;
Hard reset
2016-03-27 11:49:47 +02:00
m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = 0;
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
m[ 2] =-sin(a); m[ 6] = 0; m[10] = cos(a); m[14] = 0;
Hard reset
2016-03-27 11:49:47 +02:00
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixSetupZRotation(matrix_t m, vec_t degrees)
{
vec_t a = DEG2RAD(degrees);
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
m[ 0] = cos(a); m[ 4] =-sin(a); m[ 8] = 0; m[12] = 0;
m[ 1] = sin(a); m[ 5] = cos(a); m[ 9] = 0; m[13] = 0;
Hard reset
2016-03-27 11:49:47 +02:00
m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixSetupTranslation(matrix_t m, vec_t x, vec_t y, vec_t z)
{
m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = x;
m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = y;
m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = z;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixSetupScale(matrix_t m, vec_t x, vec_t y, vec_t z)
{
m[ 0] = x; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
m[ 1] = 0; m[ 5] = y; m[ 9] = 0; m[13] = 0;
m[ 2] = 0; m[ 6] = 0; m[10] = z; m[14] = 0;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixSetupShear(matrix_t m, vec_t x, vec_t y)
{
m[ 0] = 1; m[ 4] = x; m[ 8] = 0; m[12] = 0;
m[ 1] = y; m[ 5] = 1; m[ 9] = 0; m[13] = 0;
m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void Matrix4x4Multiply(const matrix_t a, const matrix_t b, matrix_t out)
{
#if id386_sse
//#error Matrix3x3Multiply
int i;
__m128 _t0, _t1, _t2, _t3, _t4, _t5, _t6, _t7;
_t4 = _mm_loadu_ps(&a[0]);
_t5 = _mm_loadu_ps(&a[4]);
_t6 = _mm_loadu_ps(&a[8]);
_t7 = _mm_loadu_ps(&a[12]);
for(i = 0; i < 4; i++)
{
_t0 = _mm_load1_ps(&b[i * 4 + 0]);
_t0 = _mm_mul_ps(_t4, _t0);
_t1 = _mm_load1_ps(&b[i * 4 + 1]);
_t1 = _mm_mul_ps(_t5, _t1);
_t2 = _mm_load1_ps(&b[i * 4 + 2]);
_t2 = _mm_mul_ps(_t6, _t2);
_t3 = _mm_load1_ps(&b[i * 4 + 3]);
_t3 = _mm_mul_ps(_t7, _t3);
_t1 = _mm_add_ps(_t0, _t1);
_t2 = _mm_add_ps(_t1, _t2);
_t3 = _mm_add_ps(_t2, _t3);
_mm_storeu_ps(&out[i * 4], _t3);
}
#else
out[ 0] = b[ 0]*a[ 0] + b[ 1]*a[ 4] + b[ 2]*a[ 8] + b[ 3]*a[12];
out[ 1] = b[ 0]*a[ 1] + b[ 1]*a[ 5] + b[ 2]*a[ 9] + b[ 3]*a[13];
out[ 2] = b[ 0]*a[ 2] + b[ 1]*a[ 6] + b[ 2]*a[10] + b[ 3]*a[14];
out[ 3] = b[ 0]*a[ 3] + b[ 1]*a[ 7] + b[ 2]*a[11] + b[ 3]*a[15];
out[ 4] = b[ 4]*a[ 0] + b[ 5]*a[ 4] + b[ 6]*a[ 8] + b[ 7]*a[12];
out[ 5] = b[ 4]*a[ 1] + b[ 5]*a[ 5] + b[ 6]*a[ 9] + b[ 7]*a[13];
out[ 6] = b[ 4]*a[ 2] + b[ 5]*a[ 6] + b[ 6]*a[10] + b[ 7]*a[14];
out[ 7] = b[ 4]*a[ 3] + b[ 5]*a[ 7] + b[ 6]*a[11] + b[ 7]*a[15];
out[ 8] = b[ 8]*a[ 0] + b[ 9]*a[ 4] + b[10]*a[ 8] + b[11]*a[12];
out[ 9] = b[ 8]*a[ 1] + b[ 9]*a[ 5] + b[10]*a[ 9] + b[11]*a[13];
out[10] = b[ 8]*a[ 2] + b[ 9]*a[ 6] + b[10]*a[10] + b[11]*a[14];
out[11] = b[ 8]*a[ 3] + b[ 9]*a[ 7] + b[10]*a[11] + b[11]*a[15];
out[12] = b[12]*a[ 0] + b[13]*a[ 4] + b[14]*a[ 8] + b[15]*a[12];
out[13] = b[12]*a[ 1] + b[13]*a[ 5] + b[14]*a[ 9] + b[15]*a[13];
out[14] = b[12]*a[ 2] + b[13]*a[ 6] + b[14]*a[10] + b[15]*a[14];
out[15] = b[12]*a[ 3] + b[13]*a[ 7] + b[14]*a[11] + b[15]*a[15];
#endif
}
/*
================
MatrixMultiply
================
*/
void MatrixMultiply( const float in1[ 3 ][ 3 ], const 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 MatrixMultiply2(matrix_t m, const matrix_t m2)
{
matrix_t tmp;
MatrixCopy(m, tmp);
Matrix4x4Multiply(tmp, m2, m);
}
void MatrixMultiplyRotation(matrix_t m, vec_t pitch, vec_t yaw, vec_t roll)
{
matrix_t tmp, rot;
MatrixCopy(m, tmp);
MatrixFromAngles(rot, pitch, yaw, roll);
Matrix4x4Multiply(tmp, rot, m);
}
void MatrixMultiplyZRotation(matrix_t m, vec_t degrees)
{
matrix_t tmp, rot;
MatrixCopy(m, tmp);
MatrixSetupZRotation(rot, degrees);
Matrix4x4Multiply(tmp, rot, m);
}
void MatrixMultiplyTranslation(matrix_t m, vec_t x, vec_t y, vec_t z)
{
#if 1
matrix_t tmp, trans;
MatrixCopy(m, tmp);
MatrixSetupTranslation(trans, x, y, z);
Matrix4x4Multiply(tmp, trans, m);
#else
m[12] += m[ 0] * x + m[ 4] * y + m[ 8] * z;
m[13] += m[ 1] * x + m[ 5] * y + m[ 9] * z;
m[14] += m[ 2] * x + m[ 6] * y + m[10] * z;
m[15] += m[ 3] * x + m[ 7] * y + m[11] * z;
#endif
}
void MatrixMultiplyScale(matrix_t m, vec_t x, vec_t y, vec_t z)
{
#if 0
matrix_t tmp, scale;
MatrixCopy(m, tmp);
MatrixSetupScale(scale, x, y, z);
Matrix4x4Multiply(tmp, scale, m);
#else
m[ 0] *= x; m[ 4] *= y; m[ 8] *= z;
m[ 1] *= x; m[ 5] *= y; m[ 9] *= z;
m[ 2] *= x; m[ 6] *= y; m[10] *= z;
m[ 3] *= x; m[ 7] *= y; m[11] *= z;
#endif
}
void MatrixMultiplyShear(matrix_t m, vec_t x, vec_t y)
{
matrix_t tmp, shear;
MatrixCopy(m, tmp);
MatrixSetupShear(shear, x, y);
Matrix4x4Multiply(tmp, shear, m);
}
#ifndef FLT_EPSILON
#define FLT_EPSILON 1.19209290E-07F
#endif
void MatrixToAngles(const matrix_t m, vec3_t angles)
{
#if 1
double theta;
double cp;
double sp;
sp = m[2];
// cap off our sin value so that we don't get any NANs
if(sp > 1.0)
{
sp = 1.0;
}
else if(sp < -1.0)
{
sp = -1.0;
}
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
theta = -asin(sp);
cp = cos(theta);
Hard reset
2016-03-27 11:49:47 +02:00
if(cp > 8192 * FLT_EPSILON)
{
angles[PITCH] = RAD2DEG(theta);
angles[YAW] = RAD2DEG(atan2(m[1], m[0]));
angles[ROLL] = RAD2DEG(atan2(m[6], m[10]));
}
else
{
angles[PITCH] = RAD2DEG(theta);
angles[YAW] = RAD2DEG(-atan2(m[4], m[5]));
angles[ROLL] = 0;
}
#else
double a;
double ca;
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
a = asin(-m[2]);
ca = cos(a);
Hard reset
2016-03-27 11:49:47 +02:00
if(fabs(ca) > 0.005) // Gimbal lock?
{
angles[PITCH] = RAD2DEG(atan2(m[6] / ca, m[10] / ca));
angles[YAW] = RAD2DEG(a);
angles[ROLL] = RAD2DEG(atan2(m[1] / ca, m[0] / ca));
}
else
{
// Gimbal lock has occurred
angles[PITCH] = RAD2DEG(atan2(-m[9], m[5]));
angles[YAW] = RAD2DEG(a);
angles[ROLL] = 0;
}
#endif
}
void MatrixFromAngles(matrix_t m, vec_t pitch, vec_t yaw, vec_t roll)
{
static float sr, sp, sy, cr, cp, cy;
// static to help MS compiler fp bugs
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sp = sin(DEG2RAD(pitch));
cp = cos(DEG2RAD(pitch));
Hard reset
2016-03-27 11:49:47 +02:00
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sy = sin(DEG2RAD(yaw));
cy = cos(DEG2RAD(yaw));
Hard reset
2016-03-27 11:49:47 +02:00
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sr = sin(DEG2RAD(roll));
cr = cos(DEG2RAD(roll));
Hard reset
2016-03-27 11:49:47 +02:00
m[ 0] = cp*cy; m[ 4] = (sr*sp*cy+cr*-sy); m[ 8] = (cr*sp*cy+-sr*-sy); m[12] = 0;
m[ 1] = cp*sy; m[ 5] = (sr*sp*sy+cr*cy); m[ 9] = (cr*sp*sy+-sr*cy); m[13] = 0;
m[ 2] = -sp; m[ 6] = sr*cp; m[10] = cr*cp; m[14] = 0;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixFromVectorsFLU(matrix_t m, const vec3_t forward, const vec3_t left, const vec3_t up)
{
m[ 0] = forward[0]; m[ 4] = left[0]; m[ 8] = up[0]; m[12] = 0;
m[ 1] = forward[1]; m[ 5] = left[1]; m[ 9] = up[1]; m[13] = 0;
m[ 2] = forward[2]; m[ 6] = left[2]; m[10] = up[2]; m[14] = 0;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixFromVectorsFRU(matrix_t m, const vec3_t forward, const vec3_t right, const vec3_t up)
{
m[ 0] = forward[0]; m[ 4] =-right[0]; m[ 8] = up[0]; m[12] = 0;
m[ 1] = forward[1]; m[ 5] =-right[1]; m[ 9] = up[1]; m[13] = 0;
m[ 2] = forward[2]; m[ 6] =-right[2]; m[10] = up[2]; m[14] = 0;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixFromQuat(matrix_t m, const quat_t q)
{
#if 1
/*
From Quaternion to Matrix and Back
February 27th 2005
J.M.P. van Waveren
http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
*/
float x2, y2, z2, w2;
float yy2, xy2;
float xz2, yz2, zz2;
float wz2, wy2, wx2, xx2;
x2 = q[0] + q[0];
y2 = q[1] + q[1];
z2 = q[2] + q[2];
w2 = q[3] + q[3];
yy2 = q[1] * y2;
xy2 = q[0] * y2;
xz2 = q[0] * z2;
yz2 = q[1] * z2;
zz2 = q[2] * z2;
wz2 = q[3] * z2;
wy2 = q[3] * y2;
wx2 = q[3] * x2;
xx2 = q[0] * x2;
m[ 0] = - yy2 - zz2 + 1.0f;
m[ 1] = xy2 + wz2;
m[ 2] = xz2 - wy2;
m[ 4] = xy2 - wz2;
m[ 5] = - xx2 - zz2 + 1.0f;
m[ 6] = yz2 + wx2;
m[ 8] = xz2 + wy2;
m[ 9] = yz2 - wx2;
m[10] = - xx2 - yy2 + 1.0f;
m[ 3] = m[ 7] = m[11] = m[12] = m[13] = m[14] = 0;
m[15] = 1;
#else
/*
http://www.gamedev.net/reference/articles/article1691.asp#Q54
Q54. How do I convert a quaternion to a rotation matrix?
Assuming that a quaternion has been created in the form:
Q = |X Y Z W|
Then the quaternion can then be converted into a 4x4 rotation
matrix using the following expression (Warning: you might have to
transpose this matrix if you (do not) follow the OpenGL order!):
? 2 2 ?
? 1 - (2Y + 2Z ) 2XY - 2ZW 2XZ + 2YW ?
? ?
? 2 2 ?
M = ? 2XY + 2ZW 1 - (2X + 2Z ) 2YZ - 2XW ?
? ?
? 2 2 ?
? 2XZ - 2YW 2YZ + 2XW 1 - (2X + 2Y ) ?
? ?
*/
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
float xx, xy, xz, xw, yy, yz, yw, zz, zw;
xx = q[0] * q[0];
xy = q[0] * q[1];
xz = q[0] * q[2];
xw = q[0] * q[3];
yy = q[1] * q[1];
yz = q[1] * q[2];
yw = q[1] * q[3];
zz = q[2] * q[2];
zw = q[2] * q[3];
m[ 0] = 1 - 2 * ( yy + zz );
m[ 1] = 2 * ( xy + zw );
m[ 2] = 2 * ( xz - yw );
m[ 4] = 2 * ( xy - zw );
m[ 5] = 1 - 2 * ( xx + zz );
m[ 6] = 2 * ( yz + xw );
m[ 8] = 2 * ( xz + yw );
m[ 9] = 2 * ( yz - xw );
m[10] = 1 - 2 * ( xx + yy );
m[ 3] = m[ 7] = m[11] = m[12] = m[13] = m[14] = 0;
m[15] = 1;
#endif
}
void MatrixFromPlanes(matrix_t m, const vec4_t left, const vec4_t right, const vec4_t bottom, const vec4_t top, const vec4_t near, const vec4_t far)
{
m[ 0] = (right[0] - left[0]) / 2;
m[ 1] = (top[0] - bottom[0]) / 2;
m[ 2] = (far[0] - near[0]) / 2;
m[ 3] = right[0] - (right[0] - left[0]) / 2;
m[ 4] = (right[1] - left[1]) / 2;
m[ 5] = (top[1] - bottom[1]) / 2;
m[ 6] = (far[1] - near[1]) / 2;
m[ 7] = right[1] - (right[1] - left[1]) / 2;
m[ 8] = (right[2] - left[2]) / 2;
m[ 9] = (top[2] - bottom[2]) / 2;
m[10] = (far[2] - near[2]) / 2;
m[11] = right[2] - (right[2] - left[2]) / 2;
#if 0
m[12] = (right[3] - left[3]) / 2;
m[13] = (top[3] - bottom[3]) / 2;
m[14] = (far[3] - near[3]) / 2;
m[15] = right[3] - (right[3] - left[3]) / 2;
#else
m[12] = (-right[3] - -left[3]) / 2;
m[13] = (-top[3] - -bottom[3]) / 2;
m[14] = (-far[3] - -near[3]) / 2;
m[15] = -right[3] - (-right[3] - -left[3]) / 2;
#endif
}
void MatrixToVectorsFLU(const matrix_t m, vec3_t forward, vec3_t left, vec3_t up)
{
if(forward)
{
forward[0] = m[ 0]; // cp*cy;
forward[1] = m[ 1]; // cp*sy;
forward[2] = m[ 2]; //-sp;
}
if(left)
{
left[0] = m[ 4]; // sr*sp*cy+cr*-sy;
left[1] = m[ 5]; // sr*sp*sy+cr*cy;
left[2] = m[ 6]; // sr*cp;
}
if(up)
{
up[0] = m[ 8]; // cr*sp*cy+-sr*-sy;
up[1] = m[ 9]; // cr*sp*sy+-sr*cy;
up[2] = m[10]; // cr*cp;
}
}
void MatrixToVectorsFRU(const matrix_t m, vec3_t forward, vec3_t right, vec3_t up)
{
if(forward)
{
forward[0] = m[ 0];
forward[1] = m[ 1];
forward[2] = m[ 2];
}
if(right)
{
right[0] =-m[ 4];
right[1] =-m[ 5];
right[2] =-m[ 6];
}
if(up)
{
up[0] = m[ 8];
up[1] = m[ 9];
up[2] = m[10];
}
}
void MatrixSetupTransformFromVectorsFLU(matrix_t m, const vec3_t forward, const vec3_t left, const vec3_t up, const vec3_t origin)
{
m[ 0] = forward[0]; m[ 4] = left[0]; m[ 8] = up[0]; m[12] = origin[0];
m[ 1] = forward[1]; m[ 5] = left[1]; m[ 9] = up[1]; m[13] = origin[1];
m[ 2] = forward[2]; m[ 6] = left[2]; m[10] = up[2]; m[14] = origin[2];
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixSetupTransformFromVectorsFRU(matrix_t m, const vec3_t forward, const vec3_t right, const vec3_t up, const vec3_t origin)
{
m[ 0] = forward[0]; m[ 4] = -right[0]; m[ 8] = up[0]; m[12] = origin[0];
m[ 1] = forward[1]; m[ 5] = -right[1]; m[ 9] = up[1]; m[13] = origin[1];
m[ 2] = forward[2]; m[ 6] = -right[2]; m[10] = up[2]; m[14] = origin[2];
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixSetupTransformFromRotation(matrix_t m, const matrix_t rot, const vec3_t origin)
{
m[ 0] = rot[ 0]; m[ 4] = rot[ 4]; m[ 8] = rot[ 8]; m[12] = origin[0];
m[ 1] = rot[ 1]; m[ 5] = rot[ 5]; m[ 9] = rot[ 9]; m[13] = origin[1];
m[ 2] = rot[ 2]; m[ 6] = rot[ 6]; m[10] = rot[10]; m[14] = origin[2];
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixSetupTransformFromQuat(matrix_t m, const quat_t quat, const vec3_t origin)
{
matrix_t rot;
MatrixFromQuat(rot, quat);
m[ 0] = rot[ 0]; m[ 4] = rot[ 4]; m[ 8] = rot[ 8]; m[12] = origin[0];
m[ 1] = rot[ 1]; m[ 5] = rot[ 5]; m[ 9] = rot[ 9]; m[13] = origin[1];
m[ 2] = rot[ 2]; m[ 6] = rot[ 6]; m[10] = rot[10]; m[14] = origin[2];
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixAffineInverse(const matrix_t in, matrix_t out)
{
#if 0
MatrixCopy(in, out);
MatrixInverse(out);
#else
// Tr3B - cleaned up
out[ 0] = in[ 0]; out[ 4] = in[ 1]; out[ 8] = in[ 2];
out[ 1] = in[ 4]; out[ 5] = in[ 5]; out[ 9] = in[ 6];
out[ 2] = in[ 8]; out[ 6] = in[ 9]; out[10] = in[10];
out[ 3] = 0; out[ 7] = 0; out[11] = 0; out[15] = 1;
out[12] = -( in[12] * out[ 0] + in[13] * out[ 4] + in[14] * out[ 8] );
out[13] = -( in[12] * out[ 1] + in[13] * out[ 5] + in[14] * out[ 9] );
out[14] = -( in[12] * out[ 2] + in[13] * out[ 6] + in[14] * out[10] );
#endif
}
void MatrixTransformNormal(const matrix_t m, const vec3_t in, vec3_t out)
{
out[ 0] = m[ 0] * in[ 0] + m[ 4] * in[ 1] + m[ 8] * in[ 2];
out[ 1] = m[ 1] * in[ 0] + m[ 5] * in[ 1] + m[ 9] * in[ 2];
out[ 2] = m[ 2] * in[ 0] + m[ 6] * in[ 1] + m[10] * in[ 2];
}
void MatrixTransformNormal2(const matrix_t m, vec3_t inout)
{
vec3_t tmp;
tmp[ 0] = m[ 0] * inout[ 0] + m[ 4] * inout[ 1] + m[ 8] * inout[ 2];
tmp[ 1] = m[ 1] * inout[ 0] + m[ 5] * inout[ 1] + m[ 9] * inout[ 2];
tmp[ 2] = m[ 2] * inout[ 0] + m[ 6] * inout[ 1] + m[10] * inout[ 2];
VectorCopy(tmp, inout);
}
void MatrixTransformPoint(const matrix_t m, const vec3_t in, vec3_t out)
{
out[ 0] = m[ 0] * in[ 0] + m[ 4] * in[ 1] + m[ 8] * in[ 2] + m[12];
out[ 1] = m[ 1] * in[ 0] + m[ 5] * in[ 1] + m[ 9] * in[ 2] + m[13];
out[ 2] = m[ 2] * in[ 0] + m[ 6] * in[ 1] + m[10] * in[ 2] + m[14];
}
void MatrixTransformPoint2(const matrix_t m, vec3_t inout)
{
vec3_t tmp;
tmp[ 0] = m[ 0] * inout[ 0] + m[ 4] * inout[ 1] + m[ 8] * inout[ 2] + m[12];
tmp[ 1] = m[ 1] * inout[ 0] + m[ 5] * inout[ 1] + m[ 9] * inout[ 2] + m[13];
tmp[ 2] = m[ 2] * inout[ 0] + m[ 6] * inout[ 1] + m[10] * inout[ 2] + m[14];
VectorCopy(tmp, inout);
}
void MatrixTransform4(const matrix_t m, const vec4_t in, vec4_t out)
{
#if id386_sse
//#error MatrixTransform4
__m128 _t0, _t1, _t2, _x, _y, _z, _w, _m0, _m1, _m2, _m3;
_m0 = _mm_loadu_ps(&m[0]);
_m1 = _mm_loadu_ps(&m[4]);
_m2 = _mm_loadu_ps(&m[8]);
_m3 = _mm_loadu_ps(&m[12]);
_t0 = _mm_loadu_ps(in);
_x = _mm_shuffle_ps(_t0, _t0, _MM_SHUFFLE(0, 0, 0, 0));
_y = _mm_shuffle_ps(_t0, _t0, _MM_SHUFFLE(1, 1, 1, 1));
_z = _mm_shuffle_ps(_t0, _t0, _MM_SHUFFLE(2, 2, 2, 2));
_w = _mm_shuffle_ps(_t0, _t0, _MM_SHUFFLE(3, 3, 3, 3));
_t0 = _mm_mul_ps(_m3, _w);
_t1 = _mm_mul_ps(_m2, _z);
_t0 = _mm_add_ps(_t0, _t1);
_t1 = _mm_mul_ps(_m1, _y);
_t2 = _mm_mul_ps(_m0, _x);
_t1 = _mm_add_ps(_t1, _t2);
_t0 = _mm_add_ps(_t0, _t1);
_mm_storeu_ps(out, _t0);
#else
out[ 0] = m[ 0] * in[ 0] + m[ 4] * in[ 1] + m[ 8] * in[ 2] + m[12] * in[ 3];
out[ 1] = m[ 1] * in[ 0] + m[ 5] * in[ 1] + m[ 9] * in[ 2] + m[13] * in[ 3];
out[ 2] = m[ 2] * in[ 0] + m[ 6] * in[ 1] + m[10] * in[ 2] + m[14] * in[ 3];
out[ 3] = m[ 3] * in[ 0] + m[ 7] * in[ 1] + m[11] * in[ 2] + m[15] * in[ 3];
#endif
}
void MatrixTransformPlane(const matrix_t m, const vec4_t in, vec4_t out)
{
vec3_t translation;
vec3_t planePos;
// rotate the plane normal
MatrixTransformNormal(m, in, out);
// add new position to current plane position
VectorSet(translation, m[12], m[13], m[14]);
VectorMA(translation, in[3], out, planePos);
out[3] = DotProduct(out, planePos);
}
void MatrixTransformPlane2(const matrix_t m, vec4_t inout)
{
vec4_t tmp;
MatrixTransformPlane(m, inout, tmp);
VectorCopy4(tmp, inout);
}
/*
replacement for glFrustum
see glspec30.pdf chapter 2.12 Coordinate Transformations
*/
void MatrixPerspectiveProjection(matrix_t m, vec_t left, vec_t right, vec_t bottom, vec_t top, vec_t near, vec_t far)
{
m[0] = (2 * near) / (right - left); m[4] = 0; m[8] = (right + left) / (right - left); m[12] = 0;
m[1] = 0; m[5] = (2 * near) / (top - bottom); m[9] = (top + bottom) / (top - bottom); m[13] = 0;
m[2] = 0; m[6] = 0; m[10] = -(far + near) / (far - near); m[14] = -(2 * far * near) / (far - near);
m[3] = 0; m[7] = 0; m[11] = -1; m[15] = 0;
}
/*
same as D3DXMatrixPerspectiveOffCenterLH
http://msdn.microsoft.com/en-us/library/bb205353(VS.85).aspx
*/
void MatrixPerspectiveProjectionLH(matrix_t m, vec_t left, vec_t right, vec_t bottom, vec_t top, vec_t near, vec_t far)
{
m[0] = (2 * near) / (right - left); m[4] = 0; m[8] = (left + right) / (left - right); m[12] = 0;
m[1] = 0; m[5] = (2 * near) / (top - bottom); m[9] = (top + bottom) / (bottom - top); m[13] = 0;
m[2] = 0; m[6] = 0; m[10] = far / (far - near); m[14] = (near * far) / (near - far);
m[3] = 0; m[7] = 0; m[11] = 1; m[15] = 0;
}
/*
same as D3DXMatrixPerspectiveOffCenterRH
http://msdn.microsoft.com/en-us/library/bb205354(VS.85).aspx
*/
void MatrixPerspectiveProjectionRH(matrix_t m, vec_t left, vec_t right, vec_t bottom, vec_t top, vec_t near, vec_t far)
{
m[0] = (2 * near) / (right - left); m[4] = 0; m[8] = (left + right) / (right - left); m[12] = 0;
m[1] = 0; m[5] = (2 * near) / (top - bottom); m[9] = (top + bottom) / (top - bottom); m[13] = 0;
m[2] = 0; m[6] = 0; m[10] = far / (near - far); m[14] = (near * far) / (near - far);
m[3] = 0; m[7] = 0; m[11] = -1; m[15] = 0;
}
/*
same as D3DXMatrixPerspectiveFovLH
http://msdn.microsoft.com/en-us/library/bb205350(VS.85).aspx
*/
void MatrixPerspectiveProjectionFovYAspectLH(matrix_t m, vec_t fov, vec_t aspect, vec_t near, vec_t far)
{
vec_t width, height;
width = tanf(DEG2RAD(fov * 0.5f));
height = width / aspect;
m[0] = 1 / width; m[4] = 0; m[8] = 0; m[12] = 0;
m[1] = 0; m[5] = 1 / height; m[9] = 0; m[13] = 0;
m[2] = 0; m[6] = 0; m[10] = far / (far - near); m[14] = -(near * far) / (far - near);
m[3] = 0; m[7] = 0; m[11] = 1; m[15] = 0;
}
void MatrixPerspectiveProjectionFovXYLH(matrix_t m, vec_t fovX, vec_t fovY, vec_t near, vec_t far)
{
vec_t width, height;
width = tanf(DEG2RAD(fovX * 0.5f));
height = tanf(DEG2RAD(fovY * 0.5f));
m[0] = 1 / width; m[4] = 0; m[8] = 0; m[12] = 0;
m[1] = 0; m[5] = 1 / height; m[9] = 0; m[13] = 0;
m[2] = 0; m[6] = 0; m[10] = far / (far - near); m[14] = -(near * far) / (far - near);
m[3] = 0; m[7] = 0; m[11] = 1; m[15] = 0;
}
void MatrixPerspectiveProjectionFovXYRH(matrix_t m, vec_t fovX, vec_t fovY, vec_t near, vec_t far)
{
vec_t width, height;
width = tanf(DEG2RAD(fovX * 0.5f));
height = tanf(DEG2RAD(fovY * 0.5f));
m[0] = 1 / width; m[4] = 0; m[8] = 0; m[12] = 0;
m[1] = 0; m[5] = 1 / height; m[9] = 0; m[13] = 0;
m[2] = 0; m[6] = 0; m[10] = far / (near - far); m[14] = (near * far) / (near - far);
m[3] = 0; m[7] = 0; m[11] = -1; m[15] = 0;
}
// Tr3B: far plane at infinity, see RobustShadowVolumes.pdf by Nvidia
void MatrixPerspectiveProjectionFovXYInfiniteRH(matrix_t m, vec_t fovX, vec_t fovY, vec_t near)
{
vec_t width, height;
width = tanf(DEG2RAD(fovX * 0.5f));
height = tanf(DEG2RAD(fovY * 0.5f));
m[0] = 1 / width; m[4] = 0; m[8] = 0; m[12] = 0;
m[1] = 0; m[5] = 1 / height; m[9] = 0; m[13] = 0;
m[2] = 0; m[6] = 0; m[10] = -1; m[14] = -2 * near;
m[3] = 0; m[7] = 0; m[11] = -1; m[15] = 0;
}
/*
replacement for glOrtho
see glspec30.pdf chapter 2.12 Coordinate Transformations
*/
void MatrixOrthogonalProjection(matrix_t m, vec_t left, vec_t right, vec_t bottom, vec_t top, vec_t near, vec_t far)
{
m[0] = 2 / (right - left); m[4] = 0; m[8] = 0; m[12] = -(right + left) / (right - left);
m[1] = 0; m[5] = 2 / (top - bottom); m[9] = 0; m[13] = -(top + bottom) / (top - bottom);
m[2] = 0; m[6] = 0; m[10] = -2 / (far - near); m[14] = -(far + near) / (far - near);
m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1;
}
/*
same as D3DXMatrixOrthoOffCenterLH
http://msdn.microsoft.com/en-us/library/bb205347(VS.85).aspx
*/
void MatrixOrthogonalProjectionLH(matrix_t m, vec_t left, vec_t right, vec_t bottom, vec_t top, vec_t near, vec_t far)
{
m[0] = 2 / (right - left); m[4] = 0; m[8] = 0; m[12] = (left + right) / (left - right);
m[1] = 0; m[5] = 2 / (top - bottom); m[9] = 0; m[13] = (top + bottom) / (bottom - top);
m[2] = 0; m[6] = 0; m[10] = 1 / (far - near); m[14] = near / (near - far);
m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1;
}
/*
same as D3DXMatrixOrthoOffCenterRH
http://msdn.microsoft.com/en-us/library/bb205348(VS.85).aspx
*/
void MatrixOrthogonalProjectionRH(matrix_t m, vec_t left, vec_t right, vec_t bottom, vec_t top, vec_t near, vec_t far)
{
m[0] = 2 / (right - left); m[4] = 0; m[8] = 0; m[12] = (left + right) / (left - right);
m[1] = 0; m[5] = 2 / (top - bottom); m[9] = 0; m[13] = (top + bottom) / (bottom - top);
m[2] = 0; m[6] = 0; m[10] = 1 / (near - far); m[14] = near / (near - far);
m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1;
}
/*
same as D3DXMatrixReflect
http://msdn.microsoft.com/en-us/library/bb205356%28v=VS.85%29.aspx
*/
void MatrixPlaneReflection(matrix_t m, const vec4_t plane)
{
vec4_t P;
VectorCopy4(plane, P);
PlaneNormalize(P);
/*
-2 * P.a * P.a + 1 -2 * P.b * P.a -2 * P.c * P.a 0
-2 * P.a * P.b -2 * P.b * P.b + 1 -2 * P.c * P.b 0
-2 * P.a * P.c -2 * P.b * P.c -2 * P.c * P.c + 1 0
-2 * P.a * P.d -2 * P.b * P.d -2 * P.c * P.d 1
*/
// Quake uses a different plane equation
m[0] = -2 * P[0] * P[0] + 1; m[4] = -2 * P[0] * P[1]; m[8] = -2 * P[0] * P[2]; m[12] = 2 * P[0] * P[3];
m[1] = -2 * P[1] * P[0]; m[5] = -2 * P[1] * P[1] + 1; m[9] = -2 * P[1] * P[2]; m[13] = 2 * P[1] * P[3];
m[2] = -2 * P[2] * P[0]; m[6] = -2 * P[2] * P[1]; m[10] = -2 * P[2] * P[2] + 1; m[14] = 2 * P[2] * P[3];
m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1;
#if 0
matrix_t m2;
MatrixCopy(m, m2);
MatrixTranspose(m2, m);
#endif
}
void MatrixLookAtLH(matrix_t m, const vec3_t eye, const vec3_t dir, const vec3_t up)
{
vec3_t dirN;
vec3_t upN;
vec3_t sideN;
#if 1
CrossProduct(up, dir, sideN);
VectorNormalize(sideN);
CrossProduct(dir, sideN, upN);
VectorNormalize(upN);
#else
CrossProduct(dir, up, sideN);
VectorNormalize(sideN);
CrossProduct(sideN, dir, upN);
VectorNormalize(upN);
#endif
VectorNormalize2(dir, dirN);
m[ 0] = sideN[0]; m[ 4] = sideN[1]; m[ 8] = sideN[2]; m[12] = -DotProduct(sideN, eye);
m[ 1] = upN[0]; m[ 5] = upN[1]; m[ 9] = upN[2]; m[13] = -DotProduct(upN, eye);
m[ 2] = dirN[0]; m[ 6] = dirN[1]; m[10] = dirN[2]; m[14] = -DotProduct(dirN, eye);
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixLookAtRH(matrix_t m, const vec3_t eye, const vec3_t dir, const vec3_t up)
{
vec3_t dirN;
vec3_t upN;
vec3_t sideN;
CrossProduct(dir, up, sideN);
VectorNormalize(sideN);
CrossProduct(sideN, dir, upN);
VectorNormalize(upN);
VectorNormalize2(dir, dirN);
m[ 0] = sideN[0]; m[ 4] = sideN[1]; m[ 8] = sideN[2]; m[12] = -DotProduct(sideN, eye);
m[ 1] = upN[0]; m[ 5] = upN[1]; m[ 9] = upN[2]; m[13] = -DotProduct(upN, eye);
m[ 2] = -dirN[0]; m[ 6] = -dirN[1]; m[10] = -dirN[2]; m[14] = DotProduct(dirN, eye);
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
void MatrixScaleTranslateToUnitCube(matrix_t m, const vec3_t mins, const vec3_t maxs)
{
m[ 0] = 2/(maxs[0]-mins[0]);
m[ 4] = 0;
m[ 8] = 0;
m[12] = -(maxs[0]+mins[0])/(maxs[0]-mins[0]);
m[ 1] = 0;
m[ 5] = 2/(maxs[1]-mins[1]);
m[ 9] = 0;
m[13] = -(maxs[1]+mins[1])/(maxs[1]-mins[1]);
m[ 2] = 0;
m[ 6] = 0;
m[10] = 2/(maxs[2]-mins[2]);
m[14] = -(maxs[2]+mins[2])/(maxs[2]-mins[2]);
m[ 3] = 0;
m[ 7] = 0;
m[11] = 0;
m[15] = 1;
}
void MatrixCrop(matrix_t m, const vec3_t mins, const vec3_t maxs)
{
float scaleX, scaleY, scaleZ;
float offsetX, offsetY, offsetZ;
scaleX = 2.0f / (maxs[0] - mins[0]);
scaleY = 2.0f / (maxs[1] - mins[1]);
offsetX = -0.5f * (maxs[0] + mins[0]) * scaleX;
offsetY = -0.5f * (maxs[1] + mins[1]) * scaleY;
scaleZ = 1.0f / (maxs[2] - mins[2]);
offsetZ = -mins[2] * scaleZ;
m[ 0] = scaleX; m[ 4] = 0; m[ 8] = 0; m[12] = offsetX;
m[ 1] = 0; m[ 5] = scaleY; m[ 9] = 0; m[13] = offsetY;
m[ 2] = 0; m[ 6] = 0; m[10] = scaleZ; m[14] = offsetZ;
m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
}
// *INDENT-ON*
float ClampAxis( float ang )
{
// returns Angle in the range (-360,360)
ang = fmod( ang, 360.f );
if( ang < 0.f )
{
// shift to [0,360) range
ang += 360.f;
}
return ang;
}
float NormalizeAxis( float ang )
{
// returns Angle in the range [0,360)
ang = ClampAxis( ang );
if( ang > 180.f )
{
// shift to (-180,180]
ang -= 360.f;
}
return ang;
}
vec_t QuatNormalize(quat_t q)
{
float length, ilength;
length = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
length = sqrtf(length);
Hard reset
2016-03-27 11:49:47 +02:00
if(length)
{
ilength = 1 / length;
q[0] *= ilength;
q[1] *= ilength;
q[2] *= ilength;
q[3] *= ilength;
}
return length;
}
void QuatFromAngles(quat_t q, vec_t pitch, vec_t yaw, vec_t roll)
{
#if 1
matrix_t tmp;
MatrixFromAngles(tmp, pitch, yaw, roll);
QuatFromMatrix(q, tmp);
#else
static float sr, sp, sy, cr, cp, cy;
// static to help MS compiler fp bugs
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sp = sin(DEG2RAD(pitch));
cp = cos(DEG2RAD(pitch));
Hard reset
2016-03-27 11:49:47 +02:00
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sy = sin(DEG2RAD(yaw));
cy = cos(DEG2RAD(yaw));
Hard reset
2016-03-27 11:49:47 +02:00
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
sr = sin(DEG2RAD(roll));
cr = cos(DEG2RAD(roll));
Hard reset
2016-03-27 11:49:47 +02:00
q[0] = sr * cp * cy - cr * sp * sy; // x
q[1] = cr * sp * cy + sr * cp * sy; // y
q[2] = cr * cp * sy - sr * sp * cy; // z
q[3] = cr * cp * cy + sr * sp * sy; // w
#endif
}
void QuatFromMatrix(quat_t q, const matrix_t m)
{
#if 1
/*
From Quaternion to Matrix and Back
February 27th 2005
J.M.P. van Waveren
http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
*/
float t, s;
if(m[0] + m[5] + m[10] > 0.0f)
{
t = m[0] + m[5] + m[10] + 1.0f;
s = (1.0f / sqrtf(t)) * 0.5f;
q[3] = s * t;
q[2] = (m[1] - m[4]) * s;
q[1] = (m[8] - m[2]) * s;
q[0] = (m[6] - m[9]) * s;
}
else if(m[0] > m[5] && m[0] > m[10])
{
t = m[0] - m[5] - m[10] + 1.0f;
s = (1.0f / sqrtf(t)) * 0.5f;
q[0] = s * t;
q[1] = (m[1] + m[4]) * s;
q[2] = (m[8] + m[2]) * s;
q[3] = (m[6] - m[9]) * s;
}
else if(m[5] > m[10])
{
t = -m[0] + m[5] - m[10] + 1.0f;
s = (1.0f / sqrtf(t)) * 0.5f;
q[1] = s * t;
q[0] = (m[1] + m[4]) * s;
q[3] = (m[8] - m[2]) * s;
q[2] = (m[6] + m[9]) * s;
}
else
{
t = -m[0] - m[5] + m[10] + 1.0f;
s = (1.0f / sqrtf(t)) * 0.5f;
q[2] = s * t;
q[3] = (m[1] - m[4]) * s;
q[0] = (m[8] + m[2]) * s;
q[1] = (m[6] + m[9]) * s;
}
#else
float trace;
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
trace = 1.0f + m[0] + m[5] + m[10];
if(trace > 0.0f)
{
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
vec_t s = 0.5f / sqrtf(trace);
Hard reset
2016-03-27 11:49:47 +02:00
q[0] = (m[6] - m[9]) * s;
q[1] = (m[8] - m[2]) * s;
q[2] = (m[1] - m[4]) * s;
q[3] = 0.25f / s;
}
else
{
if(m[0] > m[5] && m[0] > m[10])
{
// column 0
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
float s = sqrtf(1.0f + m[0] - m[5] - m[10]) * 2.0f;
Hard reset
2016-03-27 11:49:47 +02:00
q[0] = 0.25f * s;
q[1] = (m[4] + m[1]) / s;
q[2] = (m[8] + m[2]) / s;
q[3] = (m[9] - m[6]) / s;
}
else if(m[5] > m[10])
{
// column 1
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
float s = sqrtf(1.0f + m[5] - m[0] - m[10]) * 2.0f;
Hard reset
2016-03-27 11:49:47 +02:00
q[0] = (m[4] + m[1]) / s;
q[1] = 0.25f * s;
q[2] = (m[9] + m[6]) / s;
q[3] = (m[8] - m[2]) / s;
}
else
{
// column 2
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
float s = sqrtf(1.0f + m[10] - m[0] - m[5]) * 2.0f;
Hard reset
2016-03-27 11:49:47 +02:00
q[0] = (m[8] + m[2]) / s;
q[1] = (m[9] + m[6]) / s;
q[2] = 0.25f * s;
q[3] = (m[4] - m[1]) / s;
}
}
QuatNormalize(q);
#endif
}
void QuatToVectorsFLU(const quat_t q, vec3_t forward, vec3_t left, vec3_t up)
{
matrix_t tmp;
MatrixFromQuat(tmp, q);
MatrixToVectorsFRU(tmp, forward, left, up);
}
void QuatToVectorsFRU(const quat_t q, vec3_t forward, vec3_t right, vec3_t up)
{
matrix_t tmp;
MatrixFromQuat(tmp, q);
MatrixToVectorsFRU(tmp, forward, right, up);
}
void QuatToAxis(const quat_t q, vec3_t axis[3])
{
matrix_t tmp;
MatrixFromQuat(tmp, q);
MatrixToVectorsFLU(tmp, axis[0], axis[1], axis[2]);
}
// wombat: pretty straightforward
void QuatToRotAngle( const quat_t q, vec_t *angle ) {
*angle = 360.0f / M_PI * atan( VectorLength(q)/q[3] );
}
void QuatToRotAngleAxis( const quat_t q, vec_t *angle, vec3_t axis ) {
*angle = atan( VectorLength(q)/q[3] );
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
axis[0] = q[0] / sin(*angle);
axis[1] = q[1] / sin(*angle);
axis[2] = q[2] / sin(*angle);
Hard reset
2016-03-27 11:49:47 +02:00
*angle *= 360.0f / M_PI;
}
void QuatFromRotAngleAxis( quat_t q, vec_t angle, const vec3_t axis ) {
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
q[0] = axis[0] * sin( angle*M_PI/360 );
q[1] = axis[1] * sin( angle*M_PI/360 );
q[2] = axis[2] * sin( angle*M_PI/360 );
q[3] = cos( angle*M_PI/360 );
Hard reset
2016-03-27 11:49:47 +02:00
}
void QuatToAngles( const quat_t q, vec3_t angles )
{
quat_t q2;
q2[ 0 ] = q[ 0 ] * q[ 0 ];
q2[ 1 ] = q[ 1 ] * q[ 1 ];
q2[ 2 ] = q[ 2 ] * q[ 2 ];
q2[ 3 ] = q[ 3 ] * q[ 3 ];
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
angles[ PITCH ] = RAD2DEG( asin( -2 * ( q[ 2 ] * q[ 0 ] - q[ 3 ] * q[ 1 ] ) ) );
Hard reset
2016-03-27 11:49:47 +02:00
angles[ YAW ] = RAD2DEG( atan2( 2 * ( q[ 2 ] * q[ 3 ] + q[ 0 ] * q[ 1 ] ), ( q2[ 2 ] - q2[ 3 ] - q2[ 0 ] + q2[ 1 ] ) ) );
angles[ ROLL ] = RAD2DEG( atan2( 2 * ( q[ 3 ] * q[ 0 ] + q[ 2 ] * q[ 1 ] ), ( -q2[ 2 ] - q2[ 3 ] + q2[ 0 ] + q2[ 1 ] ) ) );
}
/*void QuatToAngles(const quat_t q, vec3_t angles)
{
const float SingularityTest = q[Z]*q[X] - q[W]*q[Y];
const float YawY = 2.f*( q[W]*q[Z] + q[X]*q[Y] );
const float YawX = ( 1.f - 2.f*( Square( q[Y] ) + Square( q[Z] ) ) );
const float SINGULARITY_THRESHOLD = 0.4999995f;
const float RAD_TO_DEG = ( 180.f ) / M_PI;
if( SingularityTest < -SINGULARITY_THRESHOLD )
{
angles[ PITCH ] = -90.0f;
angles[ YAW ] = atan2( YawY, YawX ) * RAD_TO_DEG;
angles[ ROLL ] = NormalizeAxis( -angles[ YAW ] - ( 2.f * atan2( q[ X ], q[ W ] ) * RAD_TO_DEG ) );
}
else if( SingularityTest > SINGULARITY_THRESHOLD )
{
angles[ PITCH ] = 90.0f;
angles[ YAW ] = atan2( YawY, YawX ) * RAD_TO_DEG;
angles[ ROLL ] = NormalizeAxis( angles[ YAW ] - ( 2.f * atan2( q[X], q[W] ) * RAD_TO_DEG ) );
}
else
{
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
angles[ PITCH ] = asin( 2.f*( SingularityTest ) ) * RAD_TO_DEG;
Hard reset
2016-03-27 11:49:47 +02:00
angles[ YAW ] = atan2( YawY, YawX ) * RAD_TO_DEG;
angles[ ROLL ] = atan2( -2.f*( q[W]*q[X] + q[Y]*q[Z] ), ( 1.f - 2.f*( Square( q[X] ) + Square( q[Y] ) ) ) ) * RAD_TO_DEG;
}
}*/
void QuaternionMultiply( quat_t output, quat_t first, quat_t second )
{
output[3] = (second[3] * first[3]) - (second[0] * first[0]) - (second[1] * first[1]) - (second[2] * first[2]);
output[0] = (second[3] * first[0]) + (second[0] * first[3]) + (second[1] * first[2]) - (second[2] * first[1]);
output[1] = (second[3] * first[1]) + (second[1] * first[3]) + (second[2] * first[0]) - (second[0] * first[2]);
output[2] = (second[3] * first[2]) + (second[2] * first[3]) + (second[0] * first[1]) - (second[1] * first[0]);
}
void QuatMultiply0(quat_t qa, const quat_t qb)
{
quat_t tmp;
QuatCopy(qa, tmp);
QuatMultiply1(tmp, qb, qa);
}
void QuatMultiply1(const quat_t qa, const quat_t qb, quat_t qc)
{
/*
from matrix and quaternion faq
x = w1x2 + x1w2 + y1z2 - z1y2
y = w1y2 + y1w2 + z1x2 - x1z2
z = w1z2 + z1w2 + x1y2 - y1x2
w = w1w2 - x1x2 - y1y2 - z1z2
*/
qc[0] = qa[3] * qb[0] + qa[0] * qb[3] + qa[1] * qb[2] - qa[2] * qb[1];
qc[1] = qa[3] * qb[1] + qa[1] * qb[3] + qa[2] * qb[0] - qa[0] * qb[2];
qc[2] = qa[3] * qb[2] + qa[2] * qb[3] + qa[0] * qb[1] - qa[1] * qb[0];
qc[3] = qa[3] * qb[3] - qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2];
}
void QuatMultiply2(const quat_t qa, const quat_t qb, quat_t qc)
{
qc[0] = qa[3] * qb[0] + qa[0] * qb[3] + qa[1] * qb[2] + qa[2] * qb[1];
qc[1] = qa[3] * qb[1] - qa[1] * qb[3] - qa[2] * qb[0] + qa[0] * qb[2];
qc[2] = qa[3] * qb[2] - qa[2] * qb[3] - qa[0] * qb[1] + qa[1] * qb[0];
qc[3] = qa[3] * qb[3] - qa[0] * qb[0] - qa[1] * qb[1] + qa[2] * qb[2];
}
void QuatMultiply3(const quat_t qa, const quat_t qb, quat_t qc)
{
qc[0] = qa[3] * qb[0] + qa[0] * qb[3] + qa[1] * qb[2] + qa[2] * qb[1];
qc[1] = -qa[3] * qb[1] + qa[1] * qb[3] - qa[2] * qb[0] + qa[0] * qb[2];
qc[2] = -qa[3] * qb[2] + qa[2] * qb[3] - qa[0] * qb[1] + qa[1] * qb[0];
qc[3] = -qa[3] * qb[3] + qa[0] * qb[0] - qa[1] * qb[1] + qa[2] * qb[2];
}
void QuatMultiply4(const quat_t qa, const quat_t qb, quat_t qc)
{
qc[0] = qa[3] * qb[0] - qa[0] * qb[3] - qa[1] * qb[2] - qa[2] * qb[1];
qc[1] = -qa[3] * qb[1] - qa[1] * qb[3] + qa[2] * qb[0] - qa[0] * qb[2];
qc[2] = -qa[3] * qb[2] - qa[2] * qb[3] + qa[0] * qb[1] - qa[1] * qb[0];
qc[3] = -qa[3] * qb[3] - qa[0] * qb[0] + qa[1] * qb[1] - qa[2] * qb[2];
}
void QuatSlerp(const quat_t from, const quat_t to, float frac, quat_t out)
{
#if 0
quat_t to1;
double omega, cosom, sinom, scale0, scale1;
cosom = from[0] * to[0] + from[1] * to[1] + from[2] * to[2] + from[3] * to[3];
if(cosom < 0.0)
{
cosom = -cosom;
QuatCopy(to, to1);
QuatAntipodal(to1);
}
else
{
QuatCopy(to, to1);
}
if((1.0 - cosom) > 0)
{
omega = acos(cosom);
sinom = sin(omega);
scale0 = sin((1.0 - frac) * omega) / sinom;
scale1 = sin(frac * omega) / sinom;
}
else
{
scale0 = 1.0 - frac;
scale1 = frac;
}
out[0] = scale0 * from[0] + scale1 * to1[0];
out[1] = scale0 * from[1] + scale1 * to1[1];
out[2] = scale0 * from[2] + scale1 * to1[2];
out[3] = scale0 * from[3] + scale1 * to1[3];
#else
/*
Slerping Clock Cycles
February 27th 2005
J.M.P. van Waveren
http://www.intel.com/cd/ids/developer/asmo-na/eng/293747.htm
*/
float cosom, absCosom, sinom, sinSqr, omega, scale0, scale1;
if(frac <= 0.0f)
{
QuatCopy(from, out);
return;
}
if(frac >= 1.0f)
{
QuatCopy(to, out);
return;
}
if(QuatCompare(from, to))
{
QuatCopy(from, out);
return;
}
cosom = from[0] * to[0] + from[1] * to[1] + from[2] * to[2] + from[3] * to[3];
absCosom = fabs(cosom);
if((1.0f - absCosom) > 1e-6f)
{
sinSqr = 1.0f - absCosom * absCosom;
Tiny fixes related to bad printf formatting and sizes
2023-06-21 21:01:37 +02:00
sinom = 1.0f / sqrtf(sinSqr);
omega = atan2f(sinSqr * sinom, absCosom);
Hard reset
2016-03-27 11:49:47 +02:00
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
scale0 = sin((1.0f - frac) * omega) * sinom;
scale1 = sin(frac * omega) * sinom;
Hard reset
2016-03-27 11:49:47 +02:00
}
else
{
scale0 = 1.0f - frac;
scale1 = frac;
}
scale1 = (cosom >= 0.0f) ? scale1 : -scale1;
out[0] = scale0 * from[0] + scale1 * to[0];
out[1] = scale0 * from[1] + scale1 * to[1];
out[2] = scale0 * from[2] + scale1 * to[2];
out[3] = scale0 * from[3] + scale1 * to[3];
#endif
}
void QuatTransformVector(const quat_t q, const vec3_t in, vec3_t out)
{
matrix_t m;
MatrixFromQuat(m, q);
MatrixTransformNormal(m, in, out);
}
void MatrixToEulerAngles
(
const float mat[ 3 ][ 3 ],
vec3_t ang
)
{
double theta;
double cp;
double sp;
sp = mat[ 0 ][ 2 ];
// cap off our sin value so that we don't get any NANs
if( sp > 1.0 )
{
sp = 1.0;
}
if( sp < -1.0 )
{
sp = -1.0;
}
Use sin/cos instead of sinf/cosf (accuracy)
2024-02-12 15:05:33 +01:00
theta = -asin( sp );
cp = cos( theta );
Hard reset
2016-03-27 11:49:47 +02:00
if( cp > 8192 * FLT_EPSILON )
{
ang[ 0 ] = theta * 180 / M_PI;
ang[ 1 ] = atan2( mat[ 0 ][ 1 ], mat[ 0 ][ 0 ] ) * 180 / M_PI;
ang[ 2 ] = atan2( mat[ 1 ][ 2 ], mat[ 2 ][ 2 ] ) * 180 / M_PI;
}
else
{
ang[ 0 ] = theta * 180 / M_PI;
ang[ 1 ] = -atan2( mat[ 1 ][ 0 ], mat[ 1 ][ 1 ] ) * 180 / M_PI;
ang[ 2 ] = 0;
}
}
void TransposeMatrix( float in[ 3 ][ 3 ], float out[ 3 ][ 3 ] )
{
out[ 0 ][ 0 ] = in[ 0 ][ 0 ];
out[ 0 ][ 1 ] = in[ 1 ][ 0 ];
out[ 0 ][ 2 ] = in[ 2 ][ 0 ];
out[ 1 ][ 0 ] = in[ 0 ][ 1 ];
out[ 1 ][ 1 ] = in[ 1 ][ 1 ];
out[ 1 ][ 2 ] = in[ 2 ][ 1 ];
out[ 2 ][ 0 ] = in[ 0 ][ 2 ];
out[ 2 ][ 1 ] = in[ 1 ][ 2 ];
out[ 2 ][ 2 ] = in[ 2 ][ 2 ];
}
Added Q_clamp_int and Q_clamp_float
2023-07-21 23:35:44 +02:00
int Q_clamp_int(int value, int min, int max) {
if (value < min) {
return min;
} else if (value > max) {
return max;
} else {
return value;
}
}
float Q_clamp_float(float value, float min, float max) {
if (value < min) {
return min;
} else if (value > max) {
return max;
} else {
return value;
}
Added PointToSegmentDistanceSquared
2023-09-12 23:35:22 +02:00
}
float PointToSegmentDistanceSquared(const vec3_t origin, const vec3_t a, const vec3_t b)
{
vec3_t delta;
vec3_t final;
float distSqr;
VectorSubtract(b, a, delta);
VectorSubtract(origin, a, final);
distSqr = DotProduct(delta, final);
if (distSqr > 0) {
float deltaDistSqr = VectorLengthSquared(delta);
vec3_t final;
if (distSqr < deltaDistSqr) {
vec3_t tmp;
VectorScale(delta, distSqr / deltaDistSqr, tmp);
VectorAdd(final, tmp, final);
} else {
VectorSubtract(b, origin, final);
}
}
return VectorLengthSquared(final);
Added Q_clamp_int and Q_clamp_float
2023-07-21 23:35:44 +02:00
}
Reference in a new issue Copy permalink