mirror of
https://github.com/Q3Rally-Team/q3rally.git
synced 2024-11-23 04:12:02 +00:00
95fbb18fa1
Fix going to previous browser source in q3_ui Limit ui_smallFont/ui_bigFont/cg_noTaunt cvars to missionpack Fix team chat box for spectators Don't draw crosshair 0 in Team Arena setup menu Make client for Windows x86_64 use OpenAL64.dll by default Fix loading renderer DLLs on Windows x86 Add Windows application manifest Disable DPI scaling on Windows ignore window resize event on fullscreen Don't reload arenas.txt/*.arena files in Team Arena UI Fix crash when out of memory in Team Arena's String_Alloc Fix in_nograb not releasing the mouse cursor Update UI player animation handling to match CGame Fix specifying minimum mac os version in make-macosx.sh Fix listen server sending snapshots each client frame Statically link libgcc on Windows Fix hit accuracy stats for lightning gun and shotgun kills Don't link to libGL at compile time Add common OpenGL version parsing + OpenGL 3 fixes Support parsing OpenGL ES version strings Fix setting cflags/libs from sdl2-config Load OpenGL ES 1.1 function procs [qcommon] Use unsigned types where wrapping arithmetic is intended OpenGL2: Fix brightness when r_autoExposure is disabled OpenGL2: Fix MD3 surface with zero shaders dividing by zero [botlib/be_aas_def.h] Change array size from MAX_PATH to MAX_QPATH Don't redefine MAX_PATH in bot code Fix memory leak in (unused) AAS_FloodAreas() Fix compiling GLSL shaders under Windows. Only draw cm_patch/bot debug polygons in world scenes Fix reading crash log when log wraps around buffer Don't send team overlay info to bots Fix a race condition in the makedirs target Fix shader corruption on OpenBSD
2103 lines
50 KiB
C
2103 lines
50 KiB
C
/*
|
|
===========================================================================
|
|
Copyright (C) 1999-2005 Id Software, Inc.
|
|
Copyright (C) 2002-2015 Q3Rally Team (Per Thormann - q3rally@gmail.com)
|
|
|
|
This file is part of q3rally source code.
|
|
|
|
q3rally 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.
|
|
|
|
q3rally 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 q3rally; 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"
|
|
|
|
vec3_t vec3_origin = {0,0,0};
|
|
vec3_t axisDefault[3] = { { 1, 0, 0 }, { 0, 1, 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 = (69069U * *seed + 1U);
|
|
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 );
|
|
}
|
|
|
|
//=======================================================
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
// 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;
|
|
}
|
|
|
|
|
|
/*
|
|
=====================
|
|
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;
|
|
}
|
|
|
|
/*
|
|
===============
|
|
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 );
|
|
zrot[0][0] = cos( rad );
|
|
zrot[0][1] = sin( rad );
|
|
zrot[1][0] = -sin( rad );
|
|
zrot[1][1] = cos( rad );
|
|
|
|
MatrixMultiply( m, zrot, tmpmat );
|
|
MatrixMultiply( 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 {
|
|
if ( value1[0] ) {
|
|
// STONELANCE
|
|
// yaw = ( atan2 ( value1[1], value1[0] ) * 180 / M_PI );
|
|
yaw = ( atan2 ( value1[1], value1[0] ) * M_180_PI );
|
|
// END
|
|
}
|
|
else if ( value1[1] > 0 ) {
|
|
yaw = 90;
|
|
}
|
|
else {
|
|
yaw = 270;
|
|
}
|
|
if ( yaw < 0 ) {
|
|
yaw += 360;
|
|
}
|
|
|
|
forward = sqrt ( value1[0]*value1[0] + value1[1]*value1[1] );
|
|
// STONELANCE
|
|
// pitch = ( atan2(value1[2], forward) * 180 / M_PI );
|
|
pitch = ( atan2(value1[2], forward) * M_180_PI );
|
|
// END
|
|
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] ) {
|
|
vec3_t right;
|
|
|
|
// angle vectors returns "right" instead of "y axis"
|
|
AngleVectors( angles, axis[0], right, axis[2] );
|
|
VectorSubtract( vec3_origin, right, axis[1] );
|
|
}
|
|
|
|
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( 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;
|
|
|
|
inv_denom = DotProduct( normal, normal );
|
|
#ifndef Q3_VM
|
|
assert( Q_fabs(inv_denom) != 0.0f ); // zero vectors get here
|
|
#endif
|
|
inv_denom = 1.0f / inv_denom;
|
|
|
|
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 )
|
|
{
|
|
floatint_t 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 ) {
|
|
floatint_t fi;
|
|
fi.f = f;
|
|
fi.i &= 0x7FFFFFFF;
|
|
return fi.f;
|
|
}
|
|
#endif
|
|
|
|
//============================================================
|
|
|
|
/*
|
|
===============
|
|
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;
|
|
}
|
|
|
|
|
|
/*
|
|
=================
|
|
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;
|
|
}
|
|
|
|
|
|
/*
|
|
==================
|
|
BoxOnPlaneSide
|
|
|
|
Returns 1, 2, or 1 + 2
|
|
==================
|
|
*/
|
|
int BoxOnPlaneSide(vec3_t emins, vec3_t emaxs, struct cplane_s *p)
|
|
{
|
|
float dist[2];
|
|
int sides, b, i;
|
|
|
|
// 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
|
|
dist[0] = dist[1] = 0;
|
|
if (p->signbits < 8) // >= 8: default case is original code (dist[0]=dist[1]=0)
|
|
{
|
|
for (i=0 ; i<3 ; i++)
|
|
{
|
|
b = (p->signbits >> i) & 1;
|
|
dist[ b] += p->normal[i]*emaxs[i];
|
|
dist[!b] += p->normal[i]*emins[i];
|
|
}
|
|
}
|
|
|
|
sides = 0;
|
|
if (dist[0] >= p->dist)
|
|
sides = 1;
|
|
if (dist[1] < p->dist)
|
|
sides |= 2;
|
|
|
|
return sides;
|
|
}
|
|
|
|
|
|
/*
|
|
=================
|
|
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);
|
|
}
|
|
|
|
|
|
void ClearBounds( vec3_t mins, vec3_t maxs ) {
|
|
mins[0] = mins[1] = mins[2] = 99999;
|
|
maxs[0] = maxs[1] = maxs[2] = -99999;
|
|
}
|
|
|
|
void AddPointToBounds( const vec3_t v, vec3_t mins, vec3_t maxs ) {
|
|
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;
|
|
}
|
|
|
|
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];
|
|
|
|
if ( length ) {
|
|
/* writing it this way allows gcc to recognize that rsqrt can be used */
|
|
ilength = 1/(float)sqrt (length);
|
|
/* sqrt(length) = length * (1 / sqrt(length)) */
|
|
length *= ilength;
|
|
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];
|
|
|
|
if (length)
|
|
{
|
|
/* writing it this way allows gcc to recognize that rsqrt can be used */
|
|
ilength = 1/(float)sqrt (length);
|
|
/* sqrt(length) = length * (1 / sqrt(length)) */
|
|
length *= ilength;
|
|
out[0] = v[0]*ilength;
|
|
out[1] = v[1]*ilength;
|
|
out[2] = v[2]*ilength;
|
|
} else {
|
|
VectorClear( out );
|
|
}
|
|
|
|
return length;
|
|
|
|
}
|
|
|
|
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 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;
|
|
}
|
|
*/
|
|
|
|
|
|
// STONELANCE
|
|
/*
|
|
================================================================================
|
|
VectorNAN
|
|
================================================================================
|
|
*/
|
|
qboolean VectorNAN( const vec3_t vec ){
|
|
if (IS_NAN(vec[0]) || IS_NAN(vec[1]) || IS_NAN(vec[2])){
|
|
return qtrue;
|
|
}
|
|
|
|
return qfalse;
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
MatrixMultiply
|
|
================
|
|
*/
|
|
void MatrixMultiply( float in1[3][3], float in2[3][3], float out[3][3]) {
|
|
if( in1 == out || in2 == out )
|
|
{
|
|
float temp[3][3];
|
|
|
|
temp[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
|
|
in1[0][2] * in2[2][0];
|
|
temp[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
|
|
in1[0][2] * in2[2][1];
|
|
temp[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
|
|
in1[0][2] * in2[2][2];
|
|
temp[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
|
|
in1[1][2] * in2[2][0];
|
|
temp[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
|
|
in1[1][2] * in2[2][1];
|
|
temp[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
|
|
in1[1][2] * in2[2][2];
|
|
temp[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
|
|
in1[2][2] * in2[2][0];
|
|
temp[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
|
|
in1[2][2] * in2[2][1];
|
|
temp[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
|
|
in1[2][2] * in2[2][2];
|
|
|
|
out[0][0] = temp[0][0];
|
|
out[0][1] = temp[0][1];
|
|
out[0][2] = temp[0][2];
|
|
out[1][0] = temp[1][0];
|
|
out[1][1] = temp[1][1];
|
|
out[1][2] = temp[1][2];
|
|
out[2][0] = temp[2][0];
|
|
out[2][1] = temp[2][1];
|
|
out[2][2] = temp[2][2];
|
|
}
|
|
else
|
|
{
|
|
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];
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
MatrixTranspose
|
|
|
|
Cant do MatrixTranspose(m, m);
|
|
================
|
|
*/
|
|
void MatrixTranspose( 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];
|
|
}
|
|
|
|
/*
|
|
================
|
|
MatrixAdd
|
|
================
|
|
*/
|
|
void MatrixAdd( float in1[3][3], float in2[3][3], float out[3][3] ) {
|
|
out[0][0] = in1[0][0] + in2[0][0];
|
|
out[0][1] = in1[0][1] + in2[0][1];
|
|
out[0][2] = in1[0][2] + in2[0][2];
|
|
out[1][0] = in1[1][0] + in2[1][0];
|
|
out[1][1] = in1[1][1] + in2[1][1];
|
|
out[1][2] = in1[1][2] + in2[1][2];
|
|
out[2][0] = in1[2][0] + in2[2][0];
|
|
out[2][1] = in1[2][1] + in2[2][1];
|
|
out[2][2] = in1[2][2] + in2[2][2];
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
MatrixScale
|
|
================
|
|
*/
|
|
void MatrixScale( float in[3][3], float s, float out[3][3] ) {
|
|
out[0][0] = in[0][0] * s;
|
|
out[0][1] = in[0][1] * s;
|
|
out[0][2] = in[0][2] * s;
|
|
out[1][0] = in[1][0] * s;
|
|
out[1][1] = in[1][1] * s;
|
|
out[1][2] = in[1][2] * s;
|
|
out[2][0] = in[2][0] * s;
|
|
out[2][1] = in[2][1] * s;
|
|
out[2][2] = in[2][2] * s;
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
AnglesToOrientation
|
|
|
|
Converts car angles to an orientation matrix
|
|
================
|
|
*/
|
|
void AnglesToOrientation( const vec3_t angles, float t[3][3] ) {
|
|
float angle;
|
|
static float sr, sp, sy, cr, cp, cy;
|
|
// static to help MS compiler fp bugs
|
|
|
|
angle = angles[YAW] * M_PI_180;
|
|
sy = sin(angle);
|
|
cy = cos(angle);
|
|
angle = angles[PITCH] * M_PI_180;
|
|
sp = sin(angle);
|
|
cp = cos(angle);
|
|
angle = angles[ROLL] * M_PI_180;
|
|
sr = sin(angle);
|
|
cr = cos(angle);
|
|
|
|
t[0][0] = (-1*sr*sp*cy+-1*cr*-sy); t[1][0] = (-1*sr*sp*sy+-1*cr*cy); t[2][0] = -1*sr*cp;
|
|
t[0][1] = cp*cy; t[1][1] = cp*sy; t[2][1] = -sp;
|
|
t[0][2] = (cr*sp*cy+-sr*-sy); t[1][2] = (cr*sp*sy+-sr*cy); t[2][2] = cr*cp;
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
OrientationToVectors
|
|
|
|
Converts orientation matrix to angle vectors
|
|
================
|
|
*/
|
|
void OrientationToVectors( float t[3][3], vec3_t forward, vec3_t right, vec3_t up ) {
|
|
right[0] = t[0][0]; right[1] = t[1][0]; right[2] = t[2][0];
|
|
forward[0] = t[0][1]; forward[1] = t[1][1]; forward[2] = t[2][1];
|
|
up[0] = t[0][2]; up[1] = t[1][2]; up[2] = t[2][2];
|
|
|
|
// VectorNormalize(right);
|
|
// VectorNormalize(forward);
|
|
// VectorNormalize(up);
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
AnglesToDeltaAngles
|
|
|
|
Converts orientation matrix to car angles
|
|
================
|
|
*/
|
|
void AnglesToDeltaAngles( vec3_t angles, const vec3_t w, vec3_t deltaAngles ) {
|
|
/*
|
|
float cr, cy;
|
|
float sr, sy;
|
|
float aaa;
|
|
|
|
sy = sin( angles[YAW] * M_180_PI );
|
|
cy = cos( angles[YAW] * M_180_PI );
|
|
|
|
sr = sin( angles[ROLL] * M_180_PI );
|
|
cr = cos( angles[ROLL] * M_180_PI );
|
|
|
|
|
|
// [ dpsi dtheta dphi ]T = M-1(theta,phi) * Omega
|
|
//
|
|
// M-1(theta,phi) = [ cos(phi)/cos(theta) sin(phi)/cos(theta) 0 ]
|
|
// [ -sin(phi) cos(phi) 0 ]
|
|
// [ cos(phi)*sin(theta)/cos(theta) sin(phi)*sin(theta)/cos(theta) 1 ]
|
|
|
|
aaa = ( cr + sr ) / cy;
|
|
delta_angles[0] = ( aaa ) * w[0];
|
|
delta_angles[1] = ( cr - sr ) * w[1];
|
|
delta_angles[2] = ( aaa * sy + 1 ) * w[2];
|
|
*/
|
|
|
|
float c1, c2, c3, s1, s2, s3;
|
|
float p, sp;
|
|
|
|
c1 = cos( angles[0] * M_PI_180 );
|
|
c2 = cos( angles[1] * M_PI_180 );
|
|
c3 = cos( angles[2] * M_PI_180 );
|
|
s1 = sin( angles[0] * M_PI_180 );
|
|
s2 = sin( angles[1] * M_PI_180 );
|
|
s3 = sin( angles[2] * M_PI_180 );
|
|
|
|
p = 2.0f * Q_acos( c1*c2*c3 - s1*s2*s3 );
|
|
sp = p / sin( p / 2.0f ) ;
|
|
|
|
deltaAngles[0] = (c1*s2*c3 + s1*c2*c3) * sp * M_180_PI;
|
|
deltaAngles[1] = (s1*s2*c3 + c1*s2*s3) * sp * M_180_PI;
|
|
deltaAngles[2] = (c1*c2*s3 + s1*s2*c3) * sp * M_180_PI;
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
OrientationToDeltaAngles
|
|
|
|
Converts orientation matrix to car angles
|
|
================
|
|
*/
|
|
void OrientationToDeltaAngles( float t[3][3], const vec3_t w, vec3_t delta_angles ) {
|
|
// vec3_t forward, right, up;
|
|
// float cp, as;
|
|
|
|
|
|
// OrientationToVectors(t, forward, right, up);
|
|
/*
|
|
forward[0] = -w[2] * t[1][1] + w[1] * t[1][2];
|
|
forward[1] = w[2] * t[1][0] + w[0] * t[1][2];
|
|
forward[2] = -w[1] * t[1][0] + w[0] * t[1][1];
|
|
up[2] = -w[1] * t[2][0] + w[0] * t[2][1];
|
|
right[2] = -w[1] * t[0][0] + w[0] * t[0][1];
|
|
|
|
delta_angles[PITCH] = Q_asin( -forward[2] ) * M_180_PI;
|
|
if (up[2] < 0.0f)
|
|
delta_angles[PITCH] = 180 - delta_angles[PITCH];
|
|
|
|
cp = cos( delta_angles[PITCH] * M_PI_180 );
|
|
if (cp){
|
|
// fix small floating point errors that would cause it to
|
|
// have asin() of a number > 1.00
|
|
as = forward[1] / cp > 1.00f ? 1.00f : forward[1] / cp;
|
|
as = as < -1.00f ? -1.00f : as;
|
|
|
|
delta_angles[YAW] = Q_asin(as) * M_180_PI;
|
|
if (forward[0] < 0.0f)
|
|
delta_angles[YAW] = 180 - delta_angles[YAW];
|
|
if (up[2] < 0.0f)
|
|
delta_angles[YAW] = 180 - delta_angles[YAW];
|
|
|
|
as = -right[2] / cp > 1.00f ? 1.00f : -right[2] / cp;
|
|
as = as < -1.00f ? -1.00f : as;
|
|
delta_angles[ROLL] = Q_asin(as) * M_180_PI;
|
|
}
|
|
else {
|
|
delta_angles[YAW]=0;
|
|
delta_angles[ROLL]=0;
|
|
}
|
|
|
|
// we still want yaw to be facing the front of the car so spin yaw
|
|
// 180 and adjust pitch and roll to keep car in the same position
|
|
if (fabs(delta_angles[PITCH]) > 90){
|
|
delta_angles[YAW] += 180;
|
|
delta_angles[PITCH] = 180 - delta_angles[PITCH];
|
|
delta_angles[ROLL] += 180;
|
|
}
|
|
*/
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
OrientationToAngles
|
|
|
|
Converts orientation matrix to car angles
|
|
================
|
|
*/
|
|
void OrientationToAngles( float t[3][3], vec3_t angles ) {
|
|
vec3_t forward, right, up;
|
|
float cp, as;
|
|
|
|
OrientationToVectors(t, forward, right, up);
|
|
|
|
angles[PITCH] = Q_asin(-forward[2]) * M_180_PI;
|
|
if (up[2] < 0.0f)
|
|
angles[PITCH] = 180 - angles[PITCH];
|
|
|
|
cp = cos(angles[PITCH] * M_PI_180 );
|
|
if (cp){
|
|
// fix small floating point errors that would cause it to
|
|
// have asin() of a number > 1.00
|
|
as = forward[1] / cp > 1.00f ? 1.00f : forward[1] / cp;
|
|
as = as < -1.00f ? -1.00f : as;
|
|
|
|
angles[YAW] = Q_asin(as) * M_180_PI;
|
|
if (forward[0] < 0.0f)
|
|
angles[YAW] = 180 - angles[YAW];
|
|
if (up[2] < 0.0f)
|
|
angles[YAW] = 180 - angles[YAW];
|
|
|
|
as = -right[2] / cp > 1.00f ? 1.00f : -right[2] / cp;
|
|
as = as < -1.00f ? -1.00f : as;
|
|
angles[ROLL] = Q_asin(as) * M_180_PI;
|
|
}
|
|
else {
|
|
angles[YAW]=0;
|
|
angles[ROLL]=0;
|
|
}
|
|
|
|
// we still want yaw to be facing the front of the car so spin yaw
|
|
// 180 and adjust pitch and roll to keep car in the same position
|
|
if (fabs(angles[PITCH]) > 90){
|
|
angles[YAW] += 180;
|
|
angles[PITCH] = 180 - angles[PITCH];
|
|
angles[ROLL] += 180;
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
OrthonormalizeOrientation
|
|
|
|
Normalizes orientation matrix
|
|
================
|
|
*/
|
|
void OrthonormalizeOrientation( float t[3][3] ){
|
|
vec3_t x, y, z;
|
|
|
|
VectorSet(x, t[0][0], t[1][0], t[2][0]);
|
|
VectorSet(y, t[0][1], t[1][1], t[2][1]);
|
|
|
|
// FIXME: check for 0 length?
|
|
VectorNormalize(x);
|
|
|
|
CrossProduct(x, y, z);
|
|
VectorNormalize(z);
|
|
|
|
CrossProduct(z, x, y);
|
|
VectorNormalize(y);
|
|
|
|
t[0][0] = x[0]; t[0][1] = y[0]; t[0][2] = z[0];
|
|
t[1][0] = x[1]; t[1][1] = y[1]; t[1][2] = z[1];
|
|
t[2][0] = x[2]; t[2][1] = y[2]; t[2][2] = z[2];
|
|
}
|
|
|
|
/*
|
|
================
|
|
QuaternionLengthSquared
|
|
|
|
Returns the DotProduct(q, q)
|
|
|
|
================
|
|
*/
|
|
float QuaternionLengthSquared( const vec4_t q ){
|
|
return q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
QuaternionLength
|
|
|
|
Returns the sqrt( DotProduct(q, q) )
|
|
|
|
================
|
|
*/
|
|
float QuaternionLength( const vec4_t q ){
|
|
return sqrt( q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] );
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
QuaternionFastNormalize
|
|
|
|
Normalizes quat avoiding a sqrt
|
|
returns squared length of quaternion;
|
|
|
|
Quaternion function code from racer
|
|
================
|
|
*/
|
|
float QuaternionFastNormalize( vec4_t quat )
|
|
{
|
|
// Check if quaternion needs normalizing
|
|
float len = QuaternionLengthSquared( quat );
|
|
if ( len == 0.0f )
|
|
{
|
|
// TODO: Make w 0 instead of this?
|
|
quat[3] = 1.0f;
|
|
quat[0] = quat[1] = quat[2] = 0.0f;
|
|
}
|
|
else if( len < 0.9999f || len > 1.0001f )
|
|
{
|
|
// Push quat back to 1.0 (avoid a sqrt())
|
|
float n = ( len + 1.0f ) / ( 2.0f * len );
|
|
quat[0] *= n;
|
|
quat[1] *= n;
|
|
quat[2] *= n;
|
|
quat[3] *= n;
|
|
}
|
|
|
|
return len;
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
QuaternionNormalize
|
|
|
|
Normalizes a quat using the normal sqrt method
|
|
returns length of quaternion;
|
|
|
|
Quaternion function code from racer
|
|
================
|
|
*/
|
|
float QuaternionNormalize( vec4_t quat )
|
|
{
|
|
float tmp, len;
|
|
|
|
len = QuaternionLength( quat );
|
|
if ( len == 0.0f )
|
|
{
|
|
// TODO: Make w 0 instead of this?
|
|
quat[3] = 1.0f;
|
|
quat[0] = quat[1] = quat[2] = 0.0f;
|
|
return len;
|
|
}
|
|
|
|
tmp = 1.0f / len;
|
|
quat[3] *= tmp;
|
|
quat[0] *= tmp;
|
|
quat[1] *= tmp;
|
|
quat[2] *= tmp;
|
|
|
|
return len;
|
|
}
|
|
|
|
/*
|
|
================
|
|
QuaternionMultiply
|
|
|
|
Multiplies two quaternions together
|
|
|
|
Quaternion function code from skwid
|
|
================
|
|
*/
|
|
void QuaternionMultiply(const vec4_t in1, const vec4_t in2, vec4_t out){
|
|
/*
|
|
// from racer
|
|
w = in1[3] * in2[3] - in1[0] * in2[0] - in1[1] * in2[1] - in1[2] * in2[2];
|
|
x = in1[3] * in2[0] + in1[0] * in2[3] + in1[1] * in2[2] - in1[2] * in2[1];
|
|
y = in1[3] * in2[1] - in1[0] * in2[2] + in1[1] * in2[3] + in1[2] * in2[0];
|
|
z = in1[3] * in2[2] + in1[0] * in2[1] - in1[1] * in2[0] + in1[2] * in2[3];
|
|
*/
|
|
/* this crap doesnt work
|
|
float A, B, C, D, E, F, G, H;
|
|
|
|
A = (q1[3] + q1[0]) * (q2[3] + q2[0]);
|
|
B = (q1[2] - q1[1]) * (q2[1] - q2[2]);
|
|
C = (q1[3] - q1[0]) * (q2[1] + q2[2]);
|
|
D = (q1[1] + q1[2]) * (q2[3] - q2[0]);
|
|
E = (q1[0] + q1[2]) * (q2[0] + q2[1]);
|
|
F = (q1[0] - q1[2]) * (q2[0] - q2[1]);
|
|
G = (q1[3] + q1[1]) * (q2[3] - q2[2]);
|
|
H = (q1[3] - q1[1]) * (q2[3] + q2[2]);
|
|
|
|
res[0] = A - (E + F + G + H)/2;
|
|
res[1] = C + (E - F + G - H)/2;
|
|
res[2] = D + (E - F - G + H)/2;
|
|
res[3] = B + (-E - F + G + H) /2;
|
|
*/
|
|
|
|
vec3_t temp; // in case of mulp(a, b, a)
|
|
temp[0] = (in1[1] * in2[2] - in1[2] * in2[1]) + in2[3] * in1[0] + in1[3] * in2[0];
|
|
temp[1] = (in1[2] * in2[0] - in1[0] * in2[2]) + in2[3] * in1[1] + in1[3] * in2[1];
|
|
temp[2] = (in1[0] * in2[1] - in1[1] * in2[0]) + in2[3] * in1[2] + in1[3] * in2[2];
|
|
|
|
out[3] = in1[3] * in2[3] - (in1[0] * in2[0] + in1[1] * in2[1] + in1[2] * in2[2]);
|
|
out[0] = temp[0];
|
|
out[1] = temp[1];
|
|
out[2] = temp[2];
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
QuaternionRotate
|
|
|
|
Rotates quat by the amount specified in w*time
|
|
|
|
w in body coords
|
|
|
|
Note: out cannot be the same as quat
|
|
|
|
================
|
|
*/
|
|
void QuaternionRotate( const vec4_t quat, const vec3_t w, const float time, vec4_t out){
|
|
vec3_t tempVec;
|
|
|
|
// divide by 2.0f is part of quaternion derivative calculation
|
|
VectorScale( w, time / 2.0f, tempVec );
|
|
|
|
out[0] = quat[0] + ( quat[3]*tempVec[0] - quat[2]*tempVec[1] + quat[1]*tempVec[2] );
|
|
out[1] = quat[1] + ( quat[2]*tempVec[0] + quat[3]*tempVec[1] - quat[0]*tempVec[2] );
|
|
out[2] = quat[2] + (-quat[1]*tempVec[0] + quat[0]*tempVec[1] + quat[3]*tempVec[2] );
|
|
out[3] = quat[3] + (-quat[0]*tempVec[0] - quat[1]*tempVec[1] - quat[2]*tempVec[2] );
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
QuaternionSLERP
|
|
|
|
4D Spherically lerps from one quarternion to another.
|
|
'from' and 'to' need to be normalized
|
|
|
|
t = fraction of move from start quarternion to end quarternion [0 .. 1]
|
|
================
|
|
*/
|
|
void QuaternionSLERP(const vec4_t from, const vec4_t to, float t, vec4_t res){
|
|
/*
|
|
// from racer
|
|
float theta, costheta, w1, w2, sintheta;
|
|
|
|
costheta = from[0] * to[0] + from[1] * to[1] + from[2] * to[2] + from[3] * to[3];
|
|
|
|
// OPTIMIZE: If costheta was squared then i could use the sin(t)^2 = 1-cos(t)^s
|
|
// Is sintheta ever negative? If not then sin(t)^2 > 0 == sin(t) > 0
|
|
theta = Q_acos( costheta );
|
|
sintheta = sin( theta );
|
|
if( sintheta > 0.0f )
|
|
{
|
|
w1 = sin( (1.0f - t) * theta ) / sintheta;
|
|
w2 = sin( t * theta ) / sintheta;
|
|
}
|
|
else
|
|
{
|
|
// They're the same quaternion, so who cares?
|
|
w1 = 1.0f;
|
|
w2 = 0.0f;
|
|
}
|
|
|
|
res[0] = w1 * from[0] + w2 * to[0];
|
|
res[1] = w1 * from[1] + w2 * to[1];
|
|
res[2] = w1 * from[2] + w2 * to[2];
|
|
res[3] = w1 * from[3] + w2 * to[3];
|
|
*/
|
|
|
|
/*
|
|
// Quaternion function from 'Rotating Objects Using Quaternions' by Nick Bobick
|
|
vec4_t to1;
|
|
double omega, cosom, sinom, scale0, scale1;
|
|
|
|
// calc cosine
|
|
cosom = from[0] * to[0] + from[1] * to[1] + from[2] * to[2] + from[3] * to[3];
|
|
|
|
// adjust signs (if necessary)
|
|
if ( cosom < 0.0 ){
|
|
cosom = -cosom;
|
|
to1[0] = -to[0];
|
|
to1[1] = -to[1];
|
|
to1[2] = -to[2];
|
|
to1[3] = -to[3];
|
|
} else {
|
|
to1[0] = to[0];
|
|
to1[1] = to[1];
|
|
to1[2] = to[2];
|
|
to1[3] = to[3];
|
|
}
|
|
|
|
// calculate coefficients
|
|
if ( (1.0 - cosom) > 0.1f ) {
|
|
// standard case (slerp)
|
|
omega = Q_acos(cosom);
|
|
sinom = sin(omega);
|
|
scale0 = sin((1.0 - t) * omega) / sinom;
|
|
scale1 = sin(t * omega) / sinom;
|
|
} else {
|
|
// "from" and "to" quaternions are very close
|
|
// ... so we can do a linear interpolation
|
|
scale0 = 1.0 - t;
|
|
scale1 = t;
|
|
}
|
|
|
|
// calculate final values
|
|
res[0] = scale0 * from[0] + scale1 * to1[0];
|
|
res[1] = scale0 * from[1] + scale1 * to1[1];
|
|
res[2] = scale0 * from[2] + scale1 * to1[2];
|
|
res[3] = scale0 * from[3] + scale1 * to1[3];
|
|
*/
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
================
|
|
AnglesToQuaternion
|
|
|
|
Converts euler angles to a quaternion
|
|
================
|
|
*/
|
|
void AnglesToQuaternion( const vec3_t angles, vec4_t quat){
|
|
float t[3][3];
|
|
|
|
// OPTIMIZE: See if this can be simplified
|
|
AnglesToOrientation( angles, t );
|
|
OrientationToQuaternion( t, quat );
|
|
|
|
// based on Quaternion function from 'Rotating Objects Using Quaternions' by Nick Bobick
|
|
/*
|
|
float cr, cp, cy, sr, sp, sy, spcy, spsy;
|
|
float tr, s;
|
|
int i, j, k;
|
|
vec3_t t[3];
|
|
|
|
// calculate trig identities
|
|
cr = cos(angles[ROLL] * M_PI / 180.0f);
|
|
cp = cos(angles[PITCH] * M_PI / 180.0f);
|
|
cy = cos(angles[YAW] * M_PI / 180.0f);
|
|
|
|
sr = sin(angles[ROLL] * M_PI / 180.0f);
|
|
sp = sin(angles[PITCH] * M_PI / 180.0f);
|
|
sy = sin(angles[YAW] * M_PI / 180.0f);
|
|
|
|
spcy = sp * cy;
|
|
spsy = sp * sy;
|
|
|
|
t[0][0] = (-sr*spcy + cr*sy); t[1][0] = (-sr*spsy - cr*cy); t[2][0] = -sr*cp;
|
|
t[0][1] = cp*cy; t[1][1] = cp*sy; t[2][1] = -sp;
|
|
t[0][2] = (cr*spcy + sr*sy); t[1][2] = (cr*spsy - sr*cy); t[2][2] = cr*cp;
|
|
*/
|
|
/*
|
|
tr = t[0][0] + t[1][1] + t[2][2];
|
|
|
|
// check the diagonal
|
|
if (tr > 0.0) {
|
|
s = sqrt (tr + 1.0);
|
|
quat[3] = s / 2.0;
|
|
s = 0.5 / s;
|
|
quat[0] = (t[1][2] - t[2][1]) * s;
|
|
quat[1] = (t[2][0] - t[0][2]) * s;
|
|
quat[2] = (t[0][1] - t[1][0]) * s;
|
|
} else {
|
|
// diagonal is negative
|
|
i = 0;
|
|
if (t[1][1] > t[0][0]) i = 1;
|
|
if (t[2][2] > t[i][i]) i = 2;
|
|
|
|
j = (i+1) % 3;
|
|
k = (j+1) % 3;
|
|
|
|
s = sqrt ((t[i][i] - (t[j][j] + t[k][k])) + 1.0);
|
|
|
|
quat[i] = s * 0.5;
|
|
|
|
if (s != 0.0) s = 0.5 / s;
|
|
|
|
quat[3] = (t[j][k] - t[k][j]) * s;
|
|
quat[j] = (t[i][j] + t[j][i]) * s;
|
|
quat[k] = (t[i][k] + t[k][i]) * s;
|
|
}
|
|
*/
|
|
/*
|
|
// Quaternion function code from skwid
|
|
vec4_t qy = { 0, sin(angles[YAW] * M_PI / 180.0f), 0, cos(angles[YAW] * M_PI / 180.0f) };
|
|
vec4_t qp = { sin(angles[PITCH] * M_PI / 180.0f), 0, 0, cos(angles[PITCH] * M_PI / 180.0f) };
|
|
vec4_t qr = { 0, 0, sin(angles[ROLL] * M_PI / 180.0f), cos(angles[ROLL] * M_PI / 180.0f) };
|
|
|
|
Com_Printf("qy %f %f %f %f\n", qy[0], qy[1], qy[2], qy[3]);
|
|
Com_Printf("qp %f %f %f %f\n", qp[0], qp[1], qp[2], qp[3]);
|
|
Com_Printf("qr %f %f %f %f\n", qr[0], qr[1], qr[2], qr[3]);
|
|
|
|
QuaternionMultiply( qy, qp, quat );
|
|
|
|
Com_Printf("quat %f %f %f %f\n", quat[0], quat[1], quat[2], quat[3]);
|
|
|
|
QuaternionMultiply( quat, qr, quat );
|
|
*/
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
QuaternionToAngles
|
|
|
|
Converts a quaternion to angles
|
|
|
|
================
|
|
*/
|
|
void QuaternionToAngles( const vec4_t quat, vec3_t angles ){
|
|
float t[3][3];
|
|
|
|
// OPTIMIZE: See if this can be simplified, some parts of t dont need to be calculated
|
|
QuaternionToOrientation( quat, t );
|
|
OrientationToAngles( t, angles );
|
|
|
|
/*
|
|
float wx, wy, wz, xx, yy, yz, xy, xz, zz;
|
|
float x2, y2, z2;
|
|
vec3_t f, r, u;
|
|
float cp, as;
|
|
|
|
// calculate coefficients
|
|
x2 = quat[0] + quat[0];
|
|
y2 = quat[1] + quat[1];
|
|
z2 = quat[2] + quat[2];
|
|
xx = quat[0] * x2; xy = quat[0] * y2; xz = quat[0] * z2;
|
|
yy = quat[1] * y2; yz = quat[1] * z2; zz = quat[2] * z2;
|
|
wx = quat[3] * x2; wy = quat[3] * y2; wz = quat[3] * z2;
|
|
|
|
right[0] = 1.0 - (yy + zz); right[1] = xy - wz; right[2] = xz + wy;
|
|
forward[0] = xy + wz; forward[1] = 1.0 - (xx + zz); forward[2] = yz - wx;
|
|
up[0] = xz - wy; up[1] = yz + wx; up[2] = 1.0 - (xx + yy);
|
|
*/
|
|
/*
|
|
// Quaternion function code based on skwids
|
|
float wx, wy, wz, xx, yy, yz, xy, xz, zz;
|
|
float x2, y2, z2;
|
|
vec3_t f, r, u;
|
|
float cp, as;
|
|
|
|
float s = 2 / QuaternionNormal( quat );
|
|
|
|
xx = quat[0] * quat[0]; xy = quat[0] * quat[1]; xz = quat[0] * quat[2];
|
|
yy = quat[1] * quat[1]; yz = quat[1] * quat[2]; zz = quat[2] * quat[2];
|
|
wx = quat[3] * quat[0]; wy = quat[3] * quat[1]; wz = quat[3] * quat[2];
|
|
|
|
// r[0] = 1 - s * (yy + zz); r[1] = s * (xy - wz);
|
|
r[2] = s * (xz + wy);
|
|
f[0] = s * (xy + wz); f[1] = 1 - s * (xx + zz); f[2] = s * (yz - wx);
|
|
// u[0] = s * (xz - wy); u[1] = s * (yz + wx);
|
|
u[2] = 1 - s * (xx + yy);
|
|
|
|
angles[PITCH] = Q_asin(-f[2]) * 180.0f / M_PI;
|
|
if (u[2] < 0.0f)
|
|
angles[PITCH] = 180 - angles[PITCH];
|
|
|
|
cp = cos(angles[PITCH] / 180.0f * M_PI);
|
|
if (cp){
|
|
// fix small floating point errors that would cause it to
|
|
// have asin() of a number > 1.00
|
|
as = f[1] / cp > 1.00f ? 1.00f : f[1] / cp;
|
|
as = as < -1.00f ? -1.00f : as;
|
|
|
|
angles[YAW] = Q_asin(as) * 180.0f / M_PI;
|
|
if (f[0] < 0.0f)
|
|
angles[YAW] = 180 - angles[YAW];
|
|
if (u[2] < 0.0f)
|
|
angles[YAW] = 180 - angles[YAW];
|
|
|
|
as = -r[2] / cp > 1.00f ? 1.00f : -r[2] / cp;
|
|
as = as < -1.00f ? -1.00f : as;
|
|
angles[ROLL] = Q_asin(as) * 180 / M_PI;
|
|
}
|
|
else {
|
|
angles[YAW]=0;
|
|
angles[ROLL]=0;
|
|
}
|
|
|
|
// we still want yaw to be facing the front of the car so spin yaw
|
|
// 180 and adjust pitch and roll to keep car in the same position
|
|
if (fabs(angles[PITCH]) > 90){
|
|
angles[YAW] += 180;
|
|
angles[PITCH] = 180 - angles[PITCH];
|
|
angles[ROLL] += 180;
|
|
}
|
|
*/
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
OrientationToQuaternion
|
|
|
|
Converts orientation matrix to a quaternion
|
|
================
|
|
*/
|
|
void OrientationToQuaternion( float t[3][3], vec4_t quat ){
|
|
|
|
// from racer
|
|
int i;
|
|
float qw2, qx2, qy2, qz2, tmp;
|
|
|
|
// Quaternion components squared
|
|
qw2 = 0.25 * (t[0][0] + t[1][1] + t[2][2] + 1.0);
|
|
qx2 = qw2 - 0.5 * (t[1][1] + t[2][2]);
|
|
qy2 = qw2 - 0.5 * (t[2][2] + t[0][0]);
|
|
qz2 = qw2 - 0.5 * (t[0][0] + t[1][1]);
|
|
|
|
// Decide maximum magnitude component
|
|
i = ( qw2 > qx2 ) ?
|
|
( ( qw2 > qy2 ) ? (( qw2 > qz2 ) ? 0 : 3) : (( qy2 > qz2 ) ? 2 : 3)) :
|
|
( ( qx2 > qy2 ) ? (( qx2 > qz2 ) ? 1 : 3) : (( qy2 > qz2 ) ? 2 : 3));
|
|
|
|
// Compute signed quat components using numerically stable method
|
|
switch( i )
|
|
{
|
|
case 0:
|
|
quat[3] = sqrt(qw2); tmp = 0.25f / quat[3];
|
|
quat[0] = (t[1][2] - t[2][1]) * tmp;
|
|
quat[1] = (t[2][0] - t[0][2]) * tmp;
|
|
quat[2] = (t[0][1] - t[1][0]) * tmp;
|
|
break;
|
|
|
|
case 1:
|
|
quat[0] = sqrt(qx2); tmp = 0.25f / quat[0];
|
|
quat[3] = (t[1][2] - t[2][1]) * tmp;
|
|
quat[1] = (t[1][0] + t[0][1]) * tmp;
|
|
quat[2] = (t[0][2] + t[2][0]) * tmp;
|
|
break;
|
|
|
|
case 2:
|
|
quat[1] = sqrt(qy2); tmp = 0.25f / quat[1];
|
|
quat[3] = (t[2][0] - t[0][2]) * tmp;
|
|
quat[0] = (t[1][0] + t[0][1]) * tmp;
|
|
quat[2] = (t[2][1] + t[1][2]) * tmp;
|
|
break;
|
|
|
|
case 3:
|
|
quat[2] = sqrt(qz2); tmp = 0.25f / quat[2];
|
|
quat[3] = (t[0][1] - t[1][0]) * tmp;
|
|
quat[0] = (t[2][0] + t[0][2]) * tmp;
|
|
quat[1] = (t[2][1] + t[1][2]) * tmp;
|
|
break;
|
|
}
|
|
|
|
// Always keep all components positive
|
|
// (note that scalar*quat is equivalent to quat, so q==-q)
|
|
if ( i && quat[3] < 0.0f )
|
|
{
|
|
quat[3] = -quat[3];
|
|
quat[0] = -quat[0];
|
|
quat[1] = -quat[1];
|
|
quat[2] = -quat[2];
|
|
}
|
|
|
|
// Normalize it to be safe
|
|
QuaternionFastNormalize( quat );
|
|
// tmp = 1.0f / sqrt( quat[3]*quat[3] + quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] );
|
|
// quat[3] *= tmp;
|
|
// quat[0] *= tmp;
|
|
// quat[1] *= tmp;
|
|
// quat[2] *= tmp;
|
|
|
|
|
|
// Quaternion function from 'Rotating Objects Using Quaternions' by Nick Bobick
|
|
/*
|
|
float tr, s;
|
|
int i, j, k;
|
|
|
|
int nxt[3] = {1, 2, 0};
|
|
|
|
tr = t[0][0] + t[1][1] + t[2][2];
|
|
|
|
// check the diagonal
|
|
if (tr > 0.0) {
|
|
s = sqrt (tr + 1.0);
|
|
quat[3] = s / 2.0;
|
|
s = 0.5 / s;
|
|
quat[0] = (t[1][2] - t[2][1]) * s;
|
|
quat[1] = (t[2][0] - t[0][2]) * s;
|
|
quat[2] = (t[0][1] - t[1][0]) * s;
|
|
} else {
|
|
// diagonal is negative
|
|
i = 0;
|
|
if (t[1][1] > t[0][0]) i = 1;
|
|
if (t[2][2] > t[i][i]) i = 2;
|
|
|
|
j = nxt[i];
|
|
k = nxt[j];
|
|
|
|
s = sqrt ((t[i][i] - (t[j][j] + t[k][k])) + 1.0);
|
|
|
|
quat[i] = s * 0.5;
|
|
|
|
if (s != 0.0) s = 0.5 / s;
|
|
|
|
quat[3] = (t[j][k] - t[k][j]) * s;
|
|
quat[j] = (t[i][j] + t[j][i]) * s;
|
|
quat[k] = (t[i][k] + t[k][i]) * s;
|
|
}
|
|
*/
|
|
|
|
/*
|
|
// thanks to skwid for the equations
|
|
quat[3] = 0.5f * sqrt( t[0][0] + t[1][1] + t[2][2] + 1.0f );
|
|
quat[0] = (t[1][2] - t[2][1]) / (4 * quat[3]);
|
|
quat[1] = (t[2][0] - t[0][2]) / (4 * quat[3]);
|
|
quat[2] = (t[0][1] - t[1][0]) / (4 * quat[3]);
|
|
*/
|
|
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
QuaternionToOrientation
|
|
|
|
Converts normalized quaternion to a orientation matrix
|
|
================
|
|
*/
|
|
void QuaternionToOrientation( const vec4_t quat, float t[3][3] ){
|
|
float xy, wz, xz, wy, yz, wx, x2, y2, z2;
|
|
|
|
// based on code from racer
|
|
x2 = quat[0] * quat[0];
|
|
y2 = quat[1] * quat[1];
|
|
z2 = quat[2] * quat[2];
|
|
|
|
xy = quat[0] * quat[1];
|
|
wz = quat[3] * quat[2];
|
|
xz = quat[0] * quat[2];
|
|
wy = quat[3] * quat[1];
|
|
yz = quat[1] * quat[2];
|
|
wx = quat[3] * quat[0];
|
|
t[0][0] = 1.0f - 2.0f * (y2 + z2);
|
|
t[1][0] = 2.0f * (xy - wz);
|
|
t[2][0] = 2.0f * (xz + wy);
|
|
|
|
t[0][1] = 2.0f * (xy + wz);
|
|
t[1][1] = 1.0f - 2.0f * (z2 + x2);
|
|
t[2][1] = 2.0f * (yz - wx);
|
|
|
|
t[0][2] = 2.0f * (xz - wy);
|
|
t[1][2] = 2.0f * (yz + wx);
|
|
t[2][2] = 1.0f - 2.0f * (x2 + y2);
|
|
|
|
/*
|
|
Quaternion function from 'Rotating Objects Using Quaternions' by Nick Bobick
|
|
|
|
float x2, y2, z2;
|
|
|
|
// calculate coefficients
|
|
x2 = quat[0] + quat[0];
|
|
y2 = quat[1] + quat[1];
|
|
z2 = quat[2] + quat[2];
|
|
xx = quat[0] * x2; xy = quat[0] * y2; xz = quat[0] * z2;
|
|
yy = quat[1] * y2; yz = quat[1] * z2; zz = quat[2] * z2;
|
|
wx = quat[3] * x2; wy = quat[3] * y2; wz = quat[3] * z2;
|
|
|
|
t[0][0] = 1.0 - (yy + zz); t[1][0] = xy - wz; t[2][0] = xz + wy;
|
|
t[0][1] = xy + wz; t[1][1] = 1.0 - (xx + zz); t[2][1] = yz - wx;
|
|
t[0][2] = xz - wy; t[1][2] = yz + wx; t[2][2] = 1.0 - (xx + yy);
|
|
*/
|
|
/*
|
|
// Quaternion function code based on skwids
|
|
float s = 2 / QuaternionNormal( quat );
|
|
|
|
xx = quat[0] * quat[0]; xy = quat[0] * quat[1]; xz = quat[0] * quat[2];
|
|
yy = quat[1] * quat[1]; yz = quat[1] * quat[2]; zz = quat[2] * quat[2];
|
|
wx = quat[3] * quat[0]; wy = quat[3] * quat[1]; wz = quat[3] * quat[2];
|
|
|
|
t[0][0] = 1 - s * (yy + zz); t[1][0] = s * (xy - wz); t[2][0] = s * (xz + wy);
|
|
t[0][1] = s * (xy + wz); t[1][1] = 1 - s * (xx + zz); t[2][1] = s * (yz - wx);
|
|
t[0][2] = s * (xz - wy); t[1][2] = s * (yz + wx); t[2][2] = 1 - s * (xx + yy);
|
|
*/
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
QuaternionL2ToOrientation
|
|
|
|
Converts non-normalize quaternion to a orientation matrix
|
|
'l2' is the squared length of the quaternion, used to get 't' orthogonal
|
|
================
|
|
*/
|
|
void QuaternionL2ToOrientation( const vec4_t quat, const float l2, float t[3][3] ){
|
|
float xy, wz, xz, wy, yz, wx, x2, y2, z2, s;
|
|
|
|
// based on code from racer
|
|
x2 = quat[0] * quat[0];
|
|
y2 = quat[1] * quat[1];
|
|
z2 = quat[2] * quat[2];
|
|
|
|
xy = quat[0] * quat[1];
|
|
wz = quat[3] * quat[2];
|
|
xz = quat[0] * quat[2];
|
|
wy = quat[3] * quat[1];
|
|
yz = quat[1] * quat[2];
|
|
wx = quat[3] * quat[0];
|
|
|
|
s = 2.0f / l2;
|
|
t[0][0] = 1.0f - s * (y2 + z2);
|
|
t[1][0] = s * (xy - wz);
|
|
t[2][0] = s * (xz + wy);
|
|
|
|
t[0][1] = s * (xy + wz);
|
|
t[1][1] = 1.0f - s * (z2 + x2);
|
|
t[2][1] = s * (yz - wx);
|
|
|
|
t[0][2] = s * (xz - wy);
|
|
t[1][2] = s * (yz + wx);
|
|
t[2][2] = 1.0f - s * (x2 + y2);
|
|
}
|
|
|
|
|
|
/*
|
|
================
|
|
QuaternionToVectors
|
|
|
|
Converts a quaternion to angle vectors
|
|
================
|
|
*/
|
|
void QuaternionToVectors( const vec4_t quat, vec3_t forward, vec3_t right, vec3_t up ) {
|
|
float t[3][3];
|
|
|
|
QuaternionToOrientation( quat, t );
|
|
OrientationToVectors( t, forward, right, up );
|
|
/*
|
|
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
|
|
|
// calculate coefficients
|
|
x2 = quat[0] + quat[0];
|
|
y2 = quat[1] + quat[1];
|
|
z2 = quat[2] + quat[2];
|
|
xx = quat[0] * x2; xy = quat[0] * y2; xz = quat[0] * z2;
|
|
yy = quat[1] * y2; yz = quat[1] * z2; zz = quat[2] * z2;
|
|
wx = quat[3] * x2; wy = quat[3] * y2; wz = quat[3] * z2;
|
|
|
|
right[0] = 1.0 - (yy + zz); right[1] = xy - wz; right[2] = xz + wy;
|
|
forward[0] = xy + wz; forward[1] = 1.0 - (xx + zz); forward[2] = yz - wx;
|
|
up[0] = xz - wy; up[1] = yz + wx; up[2] = 1.0 - (xx + yy);
|
|
|
|
VectorNormalize(right);
|
|
VectorNormalize(forward);
|
|
VectorNormalize(up);
|
|
*/
|
|
}
|
|
// END
|
|
|
|
|
|
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
|
|
|
|
// STONELANCE
|
|
// angle = angles[YAW] * (M_PI*2 / 360);
|
|
angle = angles[YAW] * M_PI_180;
|
|
// END
|
|
sy = sin(angle);
|
|
cy = cos(angle);
|
|
// STONELANCE
|
|
// angle = angles[PITCH] * (M_PI*2 / 360);
|
|
angle = angles[PITCH] * M_PI_180;
|
|
// END
|
|
sp = sin(angle);
|
|
cp = cos(angle);
|
|
// STONELANCE
|
|
// angle = angles[ROLL] * (M_PI*2 / 360);
|
|
angle = angles[ROLL] * M_PI_180;
|
|
// END
|
|
sr = sin(angle);
|
|
cr = cos(angle);
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
/*
|
|
** 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 )
|
|
{
|
|
floatint_t fi;
|
|
|
|
fi.f = x;
|
|
fi.ui &= 0x7FFFFFFF;
|
|
fi.ui = 0x7F800000 - fi.ui;
|
|
|
|
return (int)( (unsigned int)fi.ui >> 31 );
|
|
}
|
|
//------------------------------------------------------------------------
|
|
|
|
#ifndef Q3_VM
|
|
/*
|
|
=====================
|
|
Q_acos
|
|
|
|
the msvc acos doesn't always return a value between 0 and PI:
|
|
|
|
int i;
|
|
i = 1065353246;
|
|
acos(*(float*) &i) == -1.#IND0
|
|
|
|
=====================
|
|
*/
|
|
float Q_acos(float c) {
|
|
float angle;
|
|
|
|
angle = acos(c);
|
|
|
|
if (angle > M_PI) {
|
|
return M_PI;
|
|
}
|
|
if (angle < 0.0f) {
|
|
return 0.0f;
|
|
}
|
|
return angle;
|
|
}
|
|
|
|
/*
|
|
=====================
|
|
Q_asin
|
|
|
|
the msvc asin probably has same type of behavior as acos
|
|
|
|
=====================
|
|
*/
|
|
float Q_asin(float c) {
|
|
float angle;
|
|
|
|
angle = asin(c);
|
|
|
|
if (angle > M_PI_2) {
|
|
return M_PI_2;
|
|
}
|
|
if (angle < -M_PI_2) {
|
|
return -M_PI_2;
|
|
}
|
|
return angle;
|
|
}
|
|
#endif
|