forked from FWGS/Paranoia2
483 lines
14 KiB
C++
483 lines
14 KiB
C++
//=======================================================================
|
|
// Copyright (C) Shambler Team 2005
|
|
// vector.h - shared vector operations
|
|
//=======================================================================
|
|
#ifndef VECTOR_H
|
|
#define VECTOR_H
|
|
#include <port.h>
|
|
#include <math.h>
|
|
#include <float.h>
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <assert.h>
|
|
|
|
#define PITCH 0
|
|
#define YAW 1
|
|
#define ROLL 2
|
|
|
|
#define DIST_EPSILON (1.0f / 32.0f)
|
|
#define STOP_EPSILON 0.1f
|
|
#define ON_EPSILON 0.1f
|
|
|
|
#define BIT( n ) (1<<( n ))
|
|
#define RAD2DEG( x ) ((float)(x) * (float)(180.f / M_PI))
|
|
#define DEG2RAD( x ) ((float)(x) * (float)(M_PI / 180.f))
|
|
#define IS_NAN( x ) (((*(int *)&x) & (255<<23)) == (255<<23))
|
|
#define Q_rint( x ) ((x) < 0 ? ((int)((x)-0.5f)) : ((int)((x)+0.5f)))
|
|
#define Q_floor( a ) ((float)(long)(a))
|
|
#define Q_ceil( a ) ((float)(long)((a) + 1))
|
|
|
|
// global replacement engine <-> game
|
|
#define vec2_t Vector2D
|
|
#define vec3_t Vector
|
|
#define vec4_t Vector4D
|
|
#ifdef _MSC_VER
|
|
#pragma warning( disable : 4244 ) // disable 'possible loss of data converting float to int' warning message
|
|
#pragma warning( disable : 4305 ) // disable 'truncation from 'const double' to 'float' warning message
|
|
#endif
|
|
class NxVec3;
|
|
class Radian;
|
|
|
|
inline void SinCos( float angle, float *sine, float *cosine )
|
|
{
|
|
#if defined _MSC_VER && defined _M_I386
|
|
__asm
|
|
{
|
|
push ecx
|
|
fld dword ptr angle
|
|
fsincos
|
|
mov ecx, dword ptr[cosine]
|
|
fstp dword ptr [ecx]
|
|
mov ecx, dword ptr[sine]
|
|
fstp dword ptr [ecx]
|
|
pop ecx
|
|
}
|
|
#else
|
|
*sine = sin(angle);
|
|
*cosine = cos(angle);
|
|
#endif
|
|
}
|
|
|
|
inline float Q_rsqrt( float number )
|
|
{
|
|
const float threehalfs = 1.5F;
|
|
|
|
float x2 = number * 0.5F;
|
|
float y = number;
|
|
int i = *(long *)&y; // evil floating point bit level hacking
|
|
i = 0x5f3759df - (i>>1); // what the fuck?
|
|
y = * (float *)&i;
|
|
y = y * (1.5F - ( x2 * y * y )); // 1st iteration
|
|
|
|
assert( !IS_NAN( y ));
|
|
|
|
return y;
|
|
}
|
|
|
|
extern float HalfToFloat( unsigned short h );
|
|
|
|
//=========================================================
|
|
// 2DVector - used for many pathfinding and many other
|
|
// operations that are treated as planar rather than 3d.
|
|
//=========================================================
|
|
class Vector2D
|
|
{
|
|
public:
|
|
inline Vector2D(void) { }
|
|
inline Vector2D(float X, float Y) { x = X; y = Y; }
|
|
inline Vector2D( const float *rgfl ) { x = rgfl[0]; y = rgfl[1]; }
|
|
inline Vector2D(float rgfl[2]) { x = rgfl[0]; y = rgfl[1]; }
|
|
inline Vector2D operator+(const Vector2D& v) const { return Vector2D(x+v.x, y+v.y); }
|
|
inline Vector2D operator-(const Vector2D& v) const { return Vector2D(x-v.x, y-v.y); }
|
|
inline Vector2D operator*(float fl) const { return Vector2D(x*fl, y*fl); }
|
|
inline Vector2D operator/(float fl) const { return Vector2D(x/fl, y/fl); }
|
|
|
|
_forceinline Vector2D& operator+=(const Vector2D &v)
|
|
{
|
|
x+=v.x; y+=v.y;
|
|
return *this;
|
|
}
|
|
_forceinline Vector2D& operator-=(const Vector2D &v)
|
|
{
|
|
x-=v.x; y-=v.y;
|
|
return *this;
|
|
}
|
|
_forceinline Vector2D& operator*=(const Vector2D &v)
|
|
{
|
|
x *= v.x; y *= v.y;
|
|
return *this;
|
|
}
|
|
_forceinline Vector2D& operator*=(float s)
|
|
{
|
|
x *= s; y *= s;
|
|
return *this;
|
|
}
|
|
_forceinline Vector2D& operator/=(const Vector2D &v)
|
|
{
|
|
x /= v.x; y /= v.y;
|
|
return *this;
|
|
}
|
|
_forceinline Vector2D& operator/=(float s)
|
|
{
|
|
float oofl = 1.0f / s;
|
|
x *= oofl; y *= oofl;
|
|
return *this;
|
|
}
|
|
|
|
operator float *() { return &x; } // Vectors will now automatically convert to float * when needed
|
|
operator const float *() const { return &x; }
|
|
|
|
inline float Length(void) const { return sqrt(x*x + y*y ); }
|
|
inline Vector2D Normalize ( void ) const
|
|
{
|
|
Vector2D vec2;
|
|
|
|
float flLen = Length();
|
|
if ( flLen == 0 )
|
|
{
|
|
return Vector2D( 0, 0 );
|
|
}
|
|
else
|
|
{
|
|
flLen = 1 / flLen;
|
|
return Vector2D( x * flLen, y * flLen );
|
|
}
|
|
}
|
|
float x, y;
|
|
};
|
|
|
|
#define IS_NAN(x) (((*(int *)&x) & (255<<23)) == (255<<23))
|
|
|
|
inline float DotProduct(const Vector2D& a, const Vector2D& b) { return( a.x*b.x + a.y*b.y ); }
|
|
inline Vector2D operator*(float fl, const Vector2D& v) { return v * fl; }
|
|
|
|
class NxVec3;
|
|
|
|
//=========================================================
|
|
// 3D Vector
|
|
//=========================================================
|
|
struct Vector // same data-layout as engine's vec3_t,
|
|
{ // which is a float[3]
|
|
//public:
|
|
// Construction/destruction
|
|
inline Vector(void) { }
|
|
inline Vector(float X, float Y, float Z) { x = X; y = Y; z = Z; }
|
|
inline Vector(const Vector& v) { x = v.x; y = v.y; z = v.z; }
|
|
inline Vector( const float *rgfl ) { x = rgfl[0]; y = rgfl[1]; z = rgfl[2]; }
|
|
inline Vector(float rgfl[3]) { x = rgfl[0]; y = rgfl[1]; z = rgfl[2]; }
|
|
inline Vector( const unsigned short rgus[3] ) { x = HalfToFloat( rgus[0] ); y = HalfToFloat( rgus[1] ); z = HalfToFloat( rgus[2] ); }
|
|
inline Vector( char rgch[3] )
|
|
{
|
|
x = float( rgch[0] );
|
|
y = float( rgch[1] );
|
|
z = float( rgch[2] );
|
|
|
|
float flLen = Length();
|
|
|
|
if( flLen )
|
|
{
|
|
float flInvLen = 1.0f / flLen;
|
|
x *= flInvLen, y *= flInvLen, z *= flInvLen;
|
|
}
|
|
}
|
|
|
|
inline Vector( float fill ) { x = fill; y = fill; z = fill; }
|
|
Vector(const NxVec3& v);
|
|
|
|
// Initialization
|
|
void Init(float ix=0.0f, float iy=0.0f, float iz=0.0f){ x = ix; y = iy; z = iz; }
|
|
|
|
// Operators
|
|
inline Vector operator-(void) const { return Vector(-x,-y,-z); }
|
|
inline int operator==(const Vector& v) const { return x==v.x && y==v.y && z==v.z; }
|
|
inline int operator!=(const Vector& v) const { return !(*this==v); }
|
|
inline Vector operator+(const Vector& v) const { return Vector(x+v.x, y+v.y, z+v.z); }
|
|
inline Vector operator-(const Vector& v) const { return Vector(x-v.x, y-v.y, z-v.z); }
|
|
inline Vector operator+(float fl) const { return Vector(x+fl, y+fl, z+fl); }
|
|
inline Vector operator-(float fl) const { return Vector(x-fl, y-fl, z-fl); }
|
|
inline Vector operator*(float fl) const { return Vector(x*fl, y*fl, z*fl); }
|
|
inline Vector operator/(float fl) const { return Vector(x/fl, y/fl, z/fl); }
|
|
inline Vector operator*(const Vector& v) const { return Vector(x*v.x, y*v.y, z*v.z); }
|
|
const Vector& operator=(const NxVec3& v);
|
|
|
|
_forceinline Vector& operator+=(const Vector &v)
|
|
{
|
|
x+=v.x; y+=v.y; z += v.z;
|
|
return *this;
|
|
}
|
|
_forceinline Vector& operator-=(const Vector &v)
|
|
{
|
|
x-=v.x; y-=v.y; z -= v.z;
|
|
return *this;
|
|
}
|
|
_forceinline Vector& operator*=(const Vector &v)
|
|
{
|
|
x *= v.x; y *= v.y; z *= v.z;
|
|
return *this;
|
|
}
|
|
_forceinline Vector& operator*=(float s)
|
|
{
|
|
x *= s; y *= s; z *= s;
|
|
return *this;
|
|
}
|
|
_forceinline Vector& operator/=(const Vector &v)
|
|
{
|
|
x /= v.x; y /= v.y; z /= v.z;
|
|
return *this;
|
|
}
|
|
_forceinline Vector& operator/=(float s)
|
|
{
|
|
float oofl = 1.0f / s;
|
|
x *= oofl; y *= oofl; z *= oofl;
|
|
return *this;
|
|
}
|
|
|
|
_forceinline Vector& fixangle(void)
|
|
{
|
|
if (!IS_NAN( x ))
|
|
{
|
|
while ( x < 0 ) x += 360;
|
|
while ( x > 360 ) x -= 360;
|
|
}
|
|
if (!IS_NAN( y ))
|
|
{
|
|
while ( y < 0 ) y += 360;
|
|
while ( y > 360 ) y -= 360;
|
|
}
|
|
if (!IS_NAN( z ))
|
|
{
|
|
while ( z < 0 ) z += 360;
|
|
while ( z > 360 ) z -= 360;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
_forceinline Vector MA( float scale, const Vector &start, const Vector &direction ) const
|
|
{
|
|
return Vector(start.x + scale * direction.x, start.y + scale * direction.y, start.z + scale * direction.z) ;
|
|
}
|
|
|
|
_forceinline bool Compare( const Vector &vec, const float epsilon ) const
|
|
{
|
|
if( fabs( x - vec.x ) > epsilon ) return false;
|
|
if( fabs( y - vec.y ) > epsilon ) return false;
|
|
if( fabs( z - vec.z ) > epsilon ) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
// Methods
|
|
inline void CopyToArray( float *rgfl ) const { rgfl[0] = x, rgfl[1] = y, rgfl[2] = z; }
|
|
inline float Length(void) const { return sqrt( x*x + y*y + z*z ); }
|
|
inline float LengthSqr(void) const { return (x*x + y*y + z*z); }
|
|
operator float *() { return &x; } // Vectors will now automatically convert to float * when needed
|
|
operator const float *() const { return &x; }
|
|
|
|
inline Vector Normalize( void ) const
|
|
{
|
|
float flLen = Length();
|
|
|
|
if( flLen )
|
|
{
|
|
flLen = 1.0f / flLen;
|
|
return Vector( x * flLen, y * flLen, z * flLen );
|
|
}
|
|
|
|
return *this; // can't normalize
|
|
}
|
|
|
|
inline Vector NormalizeFast( void ) const
|
|
{
|
|
float ilength = Q_rsqrt( x * x + y * y + z * z );
|
|
return Vector( x * ilength, y * ilength, z * ilength );
|
|
}
|
|
|
|
inline float NormalizeLength( void )
|
|
{
|
|
float flLen = Length();
|
|
|
|
if( flLen )
|
|
{
|
|
float flInvLen = 1.0f / flLen;
|
|
x *= flInvLen, y *= flInvLen, z *= flInvLen;
|
|
}
|
|
|
|
return flLen;
|
|
}
|
|
|
|
inline Vector Abs( void ) const
|
|
{
|
|
return Vector( fabs( x ), fabs( y ), fabs( z ));
|
|
}
|
|
|
|
inline float Average( void ) const { return (x + y + z) / 3.0f; }
|
|
|
|
float Dot( Vector const& vOther ) const
|
|
{
|
|
return(x*vOther.x+y*vOther.y+z*vOther.z);
|
|
}
|
|
|
|
Vector Cross(const Vector &vOther) const
|
|
{
|
|
return Vector(y*vOther.z - z*vOther.y, z*vOther.x - x*vOther.z, x*vOther.y - y*vOther.x);
|
|
}
|
|
inline Vector2D Make2D ( void ) const
|
|
{
|
|
Vector2D Vec2;
|
|
Vec2.x = x;
|
|
Vec2.y = y;
|
|
return Vec2;
|
|
}
|
|
|
|
inline float Length2D(void) const { return sqrt(x*x + y*y); }
|
|
|
|
// Members
|
|
float x, y, z;
|
|
};
|
|
|
|
inline Vector operator* ( float fl, const Vector& v ) { return v * fl; }
|
|
inline float DotProduct(const Vector& a, const Vector& b ) { return( a.x * b.x + a.y * b.y + a.z * b.z ); }
|
|
inline float DotProductAbs( const Vector& a, const Vector& b ) { return( fabs( a.x * b.x ) + fabs( a.y * b.y ) + fabs( a.z * b.z )); }
|
|
inline Vector CrossProduct( const Vector& a, const Vector& b ) { return Vector( a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x ); }
|
|
inline void VectorLerp( const Vector& src1, float t, const Vector& src2, Vector& dest )
|
|
{
|
|
dest.x = src1.x + (src2.x - src1.x) * t;
|
|
dest.y = src1.y + (src2.y - src1.y) * t;
|
|
dest.z = src1.z + (src2.z - src1.z) * t;
|
|
}
|
|
|
|
//=========================================================
|
|
// 4D Vector - for matrix operations
|
|
//=========================================================
|
|
class Vector4D
|
|
{
|
|
public:
|
|
// Members
|
|
float x, y, z, w;
|
|
|
|
// Construction/destruction
|
|
inline Vector4D( void ) { }
|
|
inline Vector4D( float X, float Y, float Z, float W ) { x = X; y = Y; z = Z; w = W; }
|
|
inline Vector4D( const Vector4D& v ) { x = v.x; y = v.y; z = v.z, w = v.w; }
|
|
inline Vector4D( const float *pFloat ) { x = pFloat[0]; y = pFloat[1]; z = pFloat[2]; w = pFloat[3];}
|
|
inline Vector4D( const Vector& v ) { x = v.x; y = v.y; z = v.z; w = 1.0f; }
|
|
inline Vector4D( Radian const &angle ); // evil auto type promotion!!!
|
|
|
|
// Initialization
|
|
void Init( float ix = 0.0f, float iy = 0.0f, float iz = 0.0f, float iw = 0.0f )
|
|
{
|
|
x = ix; y = iy; z = iz; w = iw;
|
|
}
|
|
|
|
// Vectors will now automatically convert to float * when needed
|
|
operator float *() { return &x; }
|
|
operator const float *() const { return &x; }
|
|
|
|
// Vectors will now automatically convert to Vector when needed
|
|
operator Vector() { return Vector( x, y, z ); }
|
|
operator const Vector() const { return Vector( x, y, z ); }
|
|
|
|
inline float Length(void) const { return sqrt( x*x + y*y + z*z + w*w); }
|
|
inline float LengthSqr(void) const { return (x*x + y*y + z*z + w*w); }
|
|
|
|
inline Vector4D Normalize( void ) const
|
|
{
|
|
float flLen = Length();
|
|
|
|
if( flLen )
|
|
{
|
|
flLen = 1.0f / flLen;
|
|
return Vector4D( x * flLen, y * flLen, z * flLen, w * flLen );
|
|
}
|
|
|
|
return *this; // can't normalize
|
|
}
|
|
|
|
// equality
|
|
bool operator==(const Vector4D& v) const { return v.x==x && v.y==y && v.z==z && v.w==w; }
|
|
bool operator!=(const Vector4D& v) const { return !(*this==v); }
|
|
inline Vector4D operator+(const Vector4D& v) const { return Vector4D(x+v.x, y+v.y, z+v.z, w+v.w); }
|
|
inline Vector4D operator-(const Vector4D& v) const { return Vector4D(x-v.x, y-v.y, z-v.z, w-v.w); }
|
|
};
|
|
|
|
inline float DotProduct( const Vector4D& a, const Vector4D& b ) { return( a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w ); }
|
|
|
|
class Radian
|
|
{
|
|
public:
|
|
inline Radian( void ) { }
|
|
inline Radian( float X, float Y, float Z ) { x = X; y = Y; z = Z; }
|
|
inline Radian( Vector4D const &q ); // evil auto type promotion!!!
|
|
|
|
// initialization
|
|
inline void Init( float ix = 0.0f, float iy = 0.0f, float iz = 0.0f ) { x = ix; y = iy; z = iz; }
|
|
|
|
// Vectors will now automatically convert to float * when needed
|
|
operator float *() { return &x; }
|
|
operator const float *() const { return &x; }
|
|
|
|
// Operators
|
|
inline Radian operator-(void) const { return Radian(-x,-y,-z); }
|
|
inline int operator==(const Radian& v) const { return x==v.x && y==v.y && z==v.z; }
|
|
inline int operator!=(const Radian& v) const { return !(*this==v); }
|
|
inline Radian operator+(const Radian& v) const { return Radian(x+v.x, y+v.y, z+v.z); }
|
|
inline Radian operator-(const Radian& v) const { return Radian(x-v.x, y-v.y, z-v.z); }
|
|
inline Radian operator+(float fl) const { return Radian(x+fl, y+fl, z+fl); }
|
|
inline Radian operator-(float fl) const { return Radian(x-fl, y-fl, z-fl); }
|
|
inline Radian operator*(float fl) const { return Radian(x*fl, y*fl, z*fl); }
|
|
inline Radian operator/(float fl) const { return Radian(x/fl, y/fl, z/fl); }
|
|
inline Radian operator*(const Radian& v) const { return Radian(x*v.x, y*v.y, z*v.z); }
|
|
const Radian& operator=(const NxVec3& v);
|
|
|
|
_forceinline Radian& operator+=(const Radian &v)
|
|
{
|
|
x+=v.x; y+=v.y; z += v.z;
|
|
return *this;
|
|
}
|
|
_forceinline Radian& operator-=(const Radian &v)
|
|
{
|
|
x-=v.x; y-=v.y; z -= v.z;
|
|
return *this;
|
|
}
|
|
_forceinline Radian& operator*=(const Radian &v)
|
|
{
|
|
x *= v.x; y *= v.y; z *= v.z;
|
|
return *this;
|
|
}
|
|
_forceinline Radian& operator*=(float s)
|
|
{
|
|
x *= s; y *= s; z *= s;
|
|
return *this;
|
|
}
|
|
_forceinline Radian& operator/=(const Radian &v)
|
|
{
|
|
x /= v.x; y /= v.y; z /= v.z;
|
|
return *this;
|
|
}
|
|
_forceinline Radian& operator/=(float s)
|
|
{
|
|
float oofl = 1.0f / s;
|
|
x *= oofl; y *= oofl; z *= oofl;
|
|
return *this;
|
|
}
|
|
|
|
float x, y, z;
|
|
};
|
|
|
|
extern void AngleQuaternion( Radian const &angles, Vector4D &qt );
|
|
extern void QuaternionAngle( Vector4D const &q, Radian &angles );
|
|
|
|
inline Radian :: Radian( Vector4D const &q )
|
|
{
|
|
QuaternionAngle( q, *this );
|
|
}
|
|
|
|
inline Vector4D :: Vector4D( Radian const &angle )
|
|
{
|
|
AngleQuaternion( angle, *this );
|
|
}
|
|
|
|
extern const Vector g_vecZero;
|
|
extern const Radian g_radZero;
|
|
|
|
#endif//VECTOR_H
|