You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 
hlsdk-xash3d/pm_shared/pm_math.c

422 lines
8.0 KiB

/***
*
* Copyright (c) 1996-2002, Valve LLC. All rights reserved.
*
* This product contains software technology licensed from Id
* Software, Inc. ("Id Technology"). Id Technology (c) 1996 Id Software, Inc.
* All Rights Reserved.
*
* Use, distribution, and modification of this source code and/or resulting
* object code is restricted to non-commercial enhancements to products from
* Valve LLC. All other use, distribution, or modification is prohibited
* without written permission from Valve LLC.
*
****/
// pm_math.c -- math primitives
#include "mathlib.h"
#include "const.h"
#include <math.h>
// up / down
#define PITCH 0
// left / right
#define YAW 1
// fall over
#define ROLL 2
#pragma warning(disable : 4244)
vec3_t vec3_origin = {0,0,0};
int nanmask = 255<<23;
float anglemod(float a)
{
a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
return a;
}
void AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
{
float angle;
float sr, sp, sy, cr, cp, cy;
angle = angles[YAW] * (M_PI*2 / 360);
sy = sin(angle);
cy = cos(angle);
angle = angles[PITCH] * (M_PI*2 / 360);
sp = sin(angle);
cp = cos(angle);
angle = angles[ROLL] * (M_PI*2 / 360);
sr = sin(angle);
cr = cos(angle);
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 AngleVectorsTranspose (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
{
float angle;
float sr, sp, sy, cr, cp, cy;
angle = angles[YAW] * (M_PI*2 / 360);
sy = sin(angle);
cy = cos(angle);
angle = angles[PITCH] * (M_PI*2 / 360);
sp = sin(angle);
cp = cos(angle);
angle = angles[ROLL] * (M_PI*2 / 360);
sr = sin(angle);
cr = cos(angle);
if (forward)
{
forward[0] = cp*cy;
forward[1] = (sr*sp*cy+cr*-sy);
forward[2] = (cr*sp*cy+-sr*-sy);
}
if (right)
{
right[0] = cp*sy;
right[1] = (sr*sp*sy+cr*cy);
right[2] = (cr*sp*sy+-sr*cy);
}
if (up)
{
up[0] = -sp;
up[1] = sr*cp;
up[2] = cr*cp;
}
}
void AngleMatrix (const vec3_t angles, float (*matrix)[4] )
{
float angle;
float sr, sp, sy, cr, cp, cy;
angle = angles[YAW] * (M_PI*2 / 360);
sy = sin(angle);
cy = cos(angle);
angle = angles[PITCH] * (M_PI*2 / 360);
sp = sin(angle);
cp = cos(angle);
angle = angles[ROLL] * (M_PI*2 / 360);
sr = sin(angle);
cr = cos(angle);
// matrix = (YAW * PITCH) * ROLL
matrix[0][0] = cp*cy;
matrix[1][0] = cp*sy;
matrix[2][0] = -sp;
matrix[0][1] = sr*sp*cy+cr*-sy;
matrix[1][1] = sr*sp*sy+cr*cy;
matrix[2][1] = sr*cp;
matrix[0][2] = (cr*sp*cy+-sr*-sy);
matrix[1][2] = (cr*sp*sy+-sr*cy);
matrix[2][2] = cr*cp;
matrix[0][3] = 0.0;
matrix[1][3] = 0.0;
matrix[2][3] = 0.0;
}
void AngleIMatrix (const vec3_t angles, float matrix[3][4] )
{
float angle;
float sr, sp, sy, cr, cp, cy;
angle = angles[YAW] * (M_PI*2 / 360);
sy = sin(angle);
cy = cos(angle);
angle = angles[PITCH] * (M_PI*2 / 360);
sp = sin(angle);
cp = cos(angle);
angle = angles[ROLL] * (M_PI*2 / 360);
sr = sin(angle);
cr = cos(angle);
// matrix = (YAW * PITCH) * ROLL
matrix[0][0] = cp*cy;
matrix[0][1] = cp*sy;
matrix[0][2] = -sp;
matrix[1][0] = sr*sp*cy+cr*-sy;
matrix[1][1] = sr*sp*sy+cr*cy;
matrix[1][2] = sr*cp;
matrix[2][0] = (cr*sp*cy+-sr*-sy);
matrix[2][1] = (cr*sp*sy+-sr*cy);
matrix[2][2] = cr*cp;
matrix[0][3] = 0.0;
matrix[1][3] = 0.0;
matrix[2][3] = 0.0;
}
void NormalizeAngles( float *angles )
{
int i;
// Normalize angles
for ( i = 0; i < 3; i++ )
{
if ( angles[i] > 180.0 )
{
angles[i] -= 360.0;
}
else if ( angles[i] < -180.0 )
{
angles[i] += 360.0;
}
}
}
/*
===================
InterpolateAngles
Interpolate Euler angles.
FIXME: Use Quaternions to avoid discontinuities
Frac is 0.0 to 1.0 ( i.e., should probably be clamped, but doesn't have to be )
===================
*/
void InterpolateAngles( float *start, float *end, float *output, float frac )
{
int i;
float ang1, ang2;
float d;
NormalizeAngles( start );
NormalizeAngles( end );
for ( i = 0 ; i < 3 ; i++ )
{
ang1 = start[i];
ang2 = end[i];
d = ang2 - ang1;
if ( d > 180 )
{
d -= 360;
}
else if ( d < -180 )
{
d += 360;
}
output[i] = ang1 + d * frac;
}
NormalizeAngles( output );
}
/*
===================
AngleBetweenVectors
===================
*/
float AngleBetweenVectors( const vec3_t v1, const vec3_t v2 )
{
float angle;
float l1 = Length( v1 );
float l2 = Length( v2 );
if ( !l1 || !l2 )
return 0.0f;
angle = acos( DotProduct( v1, v2 ) ) / (l1*l2);
angle = ( angle * 180.0f ) / M_PI;
return angle;
}
void VectorTransform (const vec3_t in1, float in2[3][4], vec3_t out)
{
out[0] = DotProduct(in1, in2[0]) + in2[0][3];
out[1] = DotProduct(in1, in2[1]) + in2[1][3];
out[2] = DotProduct(in1, in2[2]) + in2[2][3];
}
int VectorCompare (const vec3_t v1, const vec3_t v2)
{
int i;
for (i=0 ; i<3 ; i++)
if (v1[i] != v2[i])
return 0;
return 1;
}
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 (vec3_t v1, vec3_t v2)
{
return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
}
void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
{
out[0] = veca[0]-vecb[0];
out[1] = veca[1]-vecb[1];
out[2] = veca[2]-vecb[2];
}
void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
{
out[0] = veca[0]+vecb[0];
out[1] = veca[1]+vecb[1];
out[2] = veca[2]+vecb[2];
}
void _VectorCopy (vec3_t in, vec3_t out)
{
out[0] = in[0];
out[1] = in[1];
out[2] = in[2];
}
void CrossProduct (const vec3_t v1, const vec3_t v2, vec3_t cross)
{
cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
}
double sqrt(double x);
float Length(const vec3_t v)
{
int i;
float length = 0.0f;
for (i=0 ; i< 3 ; i++)
length += v[i]*v[i];
length = sqrt (length); // FIXME
return length;
}
float Distance(const vec3_t v1, const vec3_t v2)
{
vec3_t d;
VectorSubtract(v2,v1,d);
return Length(d);
}
float VectorNormalize (vec3_t v)
{
float length, ilength;
length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
length = sqrt (length); // FIXME
if (length)
{
ilength = 1/length;
v[0] *= ilength;
v[1] *= ilength;
v[2] *= ilength;
}
return length;
}
void VectorInverse (vec3_t v)
{
v[0] = -v[0];
v[1] = -v[1];
v[2] = -v[2];
}
void VectorScale (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;
}
int Q_log2(int val)
{
int answer=0;
while (val>>=1)
answer++;
return answer;
}
void VectorMatrix( vec3_t forward, vec3_t right, vec3_t up)
{
vec3_t tmp;
if (forward[0] == 0 && forward[1] == 0)
{
right[0] = 1;
right[1] = 0;
right[2] = 0;
up[0] = -forward[2];
up[1] = 0;
up[2] = 0;
return;
}
tmp[0] = 0; tmp[1] = 0; tmp[2] = 1.0;
CrossProduct( forward, tmp, right );
VectorNormalize( right );
CrossProduct( right, forward, up );
VectorNormalize( up );
}
void VectorAngles( const vec3_t forward, vec3_t angles )
{
float tmp, yaw, pitch;
if (forward[1] == 0 && forward[0] == 0)
{
yaw = 0;
if (forward[2] > 0)
pitch = 90;
else
pitch = 270;
}
else
{
yaw = (atan2(forward[1], forward[0]) * 180 / M_PI);
if (yaw < 0)
yaw += 360;
tmp = sqrt (forward[0]*forward[0] + forward[1]*forward[1]);
pitch = (atan2(forward[2], tmp) * 180 / M_PI);
if (pitch < 0)
pitch += 360;
}
angles[0] = pitch;
angles[1] = yaw;
angles[2] = 0;
}