mirrors/thug
1
Fork 0
mirror of https://github.com/thug1src/thug.git synced 2025-01-22 05:43:47 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity
master
thug/Code/Core/Math/vector.inl

1206 lines
35 KiB
Plaintext
Raw Permalink Normal View History

thug1src
2016-02-13 21:39:12 +00:00
/*****************************************************************************
** **
** Neversoft Entertainment **
** **
** Copyright (C) 1999 - All Rights Reserved **
** **
******************************************************************************
** **
** Project: Core Library **
** **
** Module: Math (Mth) **
** **
** File name: core/math/Vector.h **
** **
** Created: 11/29/99 - mjb **
** **
** Description: Vector Math Class **
** **
*****************************************************************************/
#ifndef __CORE_MATH_VECTOR_INL
#define __CORE_MATH_VECTOR_INL
#ifdef __PLAT_NGPS__
#include <libvu0.h>
#define __USE_VU0__
#ifdef DEBUG_UNINIT_VECTORS
#undef __USE_VU0__
#endif
#ifdef DEBUG_OVERFLOW_VECTORS
#undef __USE_VU0__
#endif
#endif
/*****************************************************************************
** Private Inline Functions **
*****************************************************************************/
namespace Mth
{
/*****************************************************************************
** Public Inline Functions **
*****************************************************************************/
// Unitialized vectors always need overwriting
// for now we debug this by setting them to a NaN value
// which we can check for later (and some platforms will check automatically)
#ifdef __INITIALIZE_VECTORS__
inline Vector::Vector ( )
{
col[X] = 0.0f;
col[Y] = 0.0f;
col[Z] = 0.0f;
col[W] = 1.0f;
}
#else
#ifdef DEBUG_UNINIT_VECTORS
inline Vector::Vector ( )
{
*((uint32*)(&col[X])) = 0x7F800001; // Positive NANS (Not A Number - Signaling)
*((uint32*)(&col[Y])) = 0x7F800002; // Positive NANS (Not A Number - Signaling)
*((uint32*)(&col[Z])) = 0x7F800003; // Positive NANS (Not A Number - Signaling)
*((uint32*)(&col[W])) = 0x7F800004; // Positive NANS (Not A Number - Signaling)
}
#else
inline Vector::Vector ( )
{
}
#endif
#endif
inline Vector::Vector ( ENoInitType )
{
// DO NOTHING
}
inline Vector::Vector ( float cx, float cy, float cz)
{
col[X] = cx;
col[Y] = cy;
col[Z] = cz;
col[W] = 1.0f;
}
inline Vector::Vector ( float cx, float cy, float cz, float cw )
{
col[X] = cx;
col[Y] = cy;
col[Z] = cz;
col[W] = cw;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
#ifndef DEBUG_UNINIT_VECTORS
#ifndef DEBUG_OVERFLOW_VECTORS
inline const float& Vector::operator[] ( sint i ) const
{
return col[i];
}
#endif
#endif
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float& Vector::operator[] ( sint i )
{
// Dbg_MsgAssert (( i >= X && i < NUM_ELEMENTS ),( "index out of range" ));
return col[i];
}
/******************************************************************/
/* */
/* */
/******************************************************************/
#ifndef DEBUG_UNINIT_VECTORS
inline Vector& Vector::operator= ( const Vector& v )
{
#ifdef __USE_VU0__
// Not really VU0, but it's hardware specific
*(uint128*)(this) = *(uint128*)(&v);
#else
col[X] = v[X];
col[Y] = v[Y];
col[Z] = v[Z];
col[W] = v[W];
#endif
return *this;
}
#endif
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector::Vector ( const Vector& v )
{
*this = v;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline bool Vector::operator== ( const Vector& v ) const
{
return (( col[X] == v[X] ) &&
( col[Y] == v[Y] ) &&
( col[Z] == v[Z] ) &&
( col[W] == v[W] ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline bool Vector::operator!= ( const Vector& v ) const
{
return !( *this == v );
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::operator+= ( const Vector& v )
{
col[X] += v[X];
col[Y] += v[Y];
col[Z] += v[Z];
col[W] += v[W];
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::operator-= ( const Vector& v )
{
col[X] -= v[X];
col[Y] -= v[Y];
col[Z] -= v[Z];
col[W] -= v[W];
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::operator*= ( const float s )
{
col[X] *= s;
col[Y] *= s;
col[Z] *= s;
col[W] *= s;
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::operator/= ( float s )
{
Dbg_MsgAssert( s != 0,( "Divide by zero" ));
s = 1.0f / s;
col[X] *= s;
col[Y] *= s;
col[Z] *= s;
col[W] *= s;
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
#ifdef __USE_VU0__
inline void xsceVu0MulVector(sceVu0FVECTOR v0, sceVu0FVECTOR v1, sceVu0FVECTOR v2)
{
asm __volatile__("
.set noreorder
lqc2 vf4,0x0(%1)
lqc2 vf5,0x0(%2)
vmul.xyzw vf6,vf4,vf5
sqc2 vf6,0x0(%0)
.set reorder
": : "r" (v0) , "r" (v1), "r" (v2));
}
#endif
inline Vector& Vector::operator*= ( const Vector& v )
{
#ifndef __USE_VU0__
col[X] *= v[X];
col[Y] *= v[Y];
col[Z] *= v[Z];
col[W] *= v[W];
# else
xsceVu0MulVector(col,(float*)v.col,col);
# endif
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::operator/= ( const Vector& v )
{
Dbg_MsgAssert( v[X] != 0,( "Divide by zero X" ));
Dbg_MsgAssert( v[Y] != 0,( "Divide by zero Y" ));
Dbg_MsgAssert( v[Z] != 0,( "Divide by zero Z" ));
Dbg_MsgAssert( v[W] != 0,( "Divide by zero W" ));
col[X] /= v[X];
col[Y] /= v[Y];
col[Z] /= v[Z];
col[W] /= v[W];
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::operator*= ( const Matrix& mat )
{
#ifdef __USE_VU0__
sceVu0ApplyMatrix((sceVu0FVECTOR)this,(sceVu0FMATRIX)&mat,(sceVu0FVECTOR)this);
#else
Vector v=*this;
col[X] = v[X] * mat[X][X] + v[Y] * mat[Y][X] + v[Z] * mat[Z][X] + v[W] * mat[W][X];
col[Y] = v[X] * mat[X][Y] + v[Y] * mat[Y][Y] + v[Z] * mat[Z][Y] + v[W] * mat[W][Y];
col[Z] = v[X] * mat[X][Z] + v[Y] * mat[Y][Z] + v[Z] * mat[Z][Z] + v[W] * mat[W][Z];
col[W] = v[X] * mat[X][W] + v[Y] * mat[Y][W] + v[Z] * mat[Z][W] + v[W] * mat[W][W];
#endif
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Set ( float cx, float cy, float cz, float cw )
{
col[X] = cx;
col[Y] = cy;
col[Z] = cz;
col[W] = cw;
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float Vector::LengthSqr ( void ) const
{
return (( col[X] * col[X] ) +
( col[Y] * col[Y] ) +
( col[Z] * col[Z] ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float Vector::Length ( void ) const
{
return sqrtf ( LengthSqr ());
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Negate ( void )
{
col[X] = -col[X];
col[Y] = -col[Y];
col[Z] = -col[Z];
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Negate ( const Vector& src )
{
col[X] = -src[X];
col[Y] = -src[Y];
col[Z] = -src[Z];
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
#ifdef __USE_VU0__
// currenntly this version seems to have problems with instrcutionre-ordering,
// so don't use it unless you figure it out.
// Note: it was originally a non-inline library function, so those problems would not occur
inline void xsceVu0Normalize(sceVu0FVECTOR v0, sceVu0FVECTOR v1)
{
asm __volatile__("
.set noreorder
lqc2 vf4,0x0(%1)
vmul.xyz vf5,vf4,vf4
vaddy.x vf5,vf5,vf5
vaddz.x vf5,vf5,vf5
vsqrt Q,vf5x
vwaitq
vaddq.x vf5x,vf0x,Q
vdiv Q,vf0w,vf5x
vsub.xyzw vf6,vf0,vf0 #vf6.xyzw=0;
vwaitq
vmulq.xyz vf6,vf4,Q
sqc2 vf6,0x0(%0)
.set reorder
": : "r" (v0) , "r" (v1));
}
#endif
inline Vector& Vector::Normalize( )
{
//# if 0
#ifdef __USE_VU0__
// currenntly inline (xsce) version seems to have problems with instrcutionre-ordering,
// so don't use it unless you figure it out.
sceVu0Normalize(col,col); // NOTE::: using the non-inline version (not xsce....)
# else
float l = Length();
if ( l > 0.0f )
{
l = 1.0f / l;
col[X] *= l;
col[Y] *= l;
col[Z] *= l;
}
else
{
// Dbg_MsgAssert(0,("Normalizing vector of zero length"));
}
# endif
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Normalize ( float len )
{
float l = Length();
if ( l > 0.0f )
{
l = len / l;
col[X] *= l;
col[Y] *= l;
col[Z] *= l;
}
else
{
// Dbg_MsgAssert(0,("Normalizing vector of zero length"));
}
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::ShearX ( float shy, float shz )
{
col[X] += (( col[Y] * shy ) + ( col[Z] * shz ));
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::ShearY ( float shx, float shz )
{
col[Y] += (( col[X] * shx ) + ( col[Z] * shz ));
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::ShearZ ( float shx, float shy )
{
col[Z] += (( col[X] * shx ) + ( col[Y] * shy ));
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::RotateX ( float s, float c )
{
float y = ( c * col[Y] ) - ( s * col[Z] );
float z = ( s * col[Y] ) + ( c * col[Z] );
col[Y] = y;
col[Z] = z;
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::RotateY ( float s, float c )
{
float x = ( c * col[X] ) + ( s * col[Z] );
float z = ( -s * col[X] ) + ( c * col[Z] );
col[X] = x;
col[Z] = z;
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::RotateZ ( float s, float c )
{
float x = ( c * col[X] ) - ( s * col[Y] );
float y = ( s * col[X] ) + ( c * col[Y] );
col[X] = x;
col[Y] = y;
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::RotateX ( float angle )
{
return RotateX ( sinf ( angle ), cosf ( angle ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::RotateY ( float angle )
{
return RotateY ( sinf ( angle ), cosf ( angle ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::RotateZ ( float angle )
{
return RotateZ ( sinf ( angle ), cosf ( angle ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Rotate ( const Quat& quat )
{
*this = quat.Rotate ( *this );
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Rotate ( const Matrix& mat )
{
*this = mat.Rotate ( *this );
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Rotate ( const Vector& axis, const float angle )
{
#if 0
Matrix mat( axis, angle );
*this = mat.Rotate( *this );
#else
Quat quat( axis, angle );
*this = quat.Rotate( *this );
#endif
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Scale ( float scale )
{
col[X] *= scale;
col[Y] *= scale;
col[Z] *= scale;
col[W] *= scale;
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Scale ( const Vector& scale )
{
col[X] *= scale[X];
col[Y] *= scale[Y];
col[Z] *= scale[Z];
col[W] *= scale[W];
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::ClampMin ( float min )
{
col[X] = Mth::ClampMin ( col[X], min );
col[Y] = Mth::ClampMin ( col[Y], min );
col[Z] = Mth::ClampMin ( col[Z], min );
col[W] = Mth::ClampMin ( col[W], min );
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::ClampMax ( float max )
{
col[X] = Mth::ClampMax ( col[X], max );
col[Y] = Mth::ClampMax ( col[Y], max );
col[Z] = Mth::ClampMax ( col[Z], max );
col[W] = Mth::ClampMax ( col[W], max );
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Clamp ( float min, float max )
{
col[X] = Mth::ClampMin ( Mth::ClampMax ( col[X], max ), min );
col[Y] = Mth::ClampMin ( Mth::ClampMax ( col[Y], max ), min );
col[Z] = Mth::ClampMin ( Mth::ClampMax ( col[Z], max ), min );
col[W] = Mth::ClampMin ( Mth::ClampMax ( col[W], max ), min );
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::ClampMin ( Vector& min )
{
col[X] = Mth::ClampMin ( col[X], min[X] );
col[Y] = Mth::ClampMin ( col[Y], min[Y] );
col[Z] = Mth::ClampMin ( col[Z], min[Z] );
col[W] = Mth::ClampMin ( col[W], min[W] );
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::ClampMax ( Vector& max )
{
col[X] = Mth::ClampMax ( col[X], max[X] );
col[Y] = Mth::ClampMax ( col[Y], max[Y] );
col[Z] = Mth::ClampMax ( col[Z], max[Z] );
col[W] = Mth::ClampMax ( col[W], max[W] );
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::Clamp ( Vector& min, Vector& max )
{
col[X] = Mth::ClampMin ( Mth::ClampMax ( col[X], max[X] ), min[X] );
col[Y] = Mth::ClampMin ( Mth::ClampMax ( col[Y], max[Y] ), min[Y] );
col[Z] = Mth::ClampMin ( Mth::ClampMax ( col[Z], max[Z] ), min[Z] );
col[W] = Mth::ClampMin ( Mth::ClampMax ( col[W], max[W] ), min[W] );
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float Vector::GetX( void ) const
{
return col[X];
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float Vector::GetY( void ) const
{
return col[Y];
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float Vector::GetZ( void ) const
{
return col[Z];
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float Vector::GetW( void ) const
{
return col[W];
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector operator+ ( const Vector& v1, const Vector& v2 )
{
return Vector ( v1[X] + v2[X], v1[Y] + v2[Y], v1[Z] + v2[Z], v1[W] + v2[W] );
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector operator- ( const Vector& v1, const Vector& v2 )
{
return Vector ( v1[X] - v2[X], v1[Y] - v2[Y], v1[Z] - v2[Z], v1[W] - v2[W] );
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector operator* ( const Vector& v, const float s )
{
return Vector ( v[X] * s, v[Y] * s, v[Z] * s, v[W] * s );
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector operator* ( const float s, const Vector& v )
{
return v * s;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector operator* ( const Vector& v, const Matrix& m )
{
#ifdef __USE_VU0__
// Mick:
Mth::Vector result;
sceVu0ApplyMatrix((sceVu0FVECTOR)&result,(sceVu0FMATRIX)&m,(sceVu0FVECTOR)&v);
return result;
#else
// Mick: This is REALLY SLOW, as it calls Mth::Matrix operator[]
return Vector ( v[X] * m[X][X] + v[Y] * m[Y][X] + v[Z] * m[Z][X] + v[W] * m[W][X],
v[X] * m[X][Y] + v[Y] * m[Y][Y] + v[Z] * m[Z][Y] + v[W] * m[W][Y],
v[X] * m[X][Z] + v[Y] * m[Y][Z] + v[Z] * m[Z][Z] + v[W] * m[W][Z],
v[X] * m[X][W] + v[Y] * m[Y][W] + v[Z] * m[Z][W] + v[W] * m[W][W]);
#endif
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector operator/ ( const Vector& v, const float s )
{
Dbg_MsgAssert (( s != 0.0f ),( "Divide by Zero" ));
float r = 1.0f / s;
return v * r;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector operator- ( const Vector& v )
{
return Vector ( -v[X], -v[Y], -v[Z], -v[W] );
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float DotProduct ( const Vector& v1, const Vector& v2 )
{
return ( v1[X] * v2[X] ) + ( v1[Y] * v2[Y] ) + ( v1[Z] * v2[Z] );
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector CrossProduct ( const Vector& v1, const Vector& v2 )
{
return Vector (( v1[Y] * v2[Z] ) - ( v1[Z] * v2[Y] ),
( v1[Z] * v2[X] ) - ( v1[X] * v2[Z] ),
( v1[X] * v2[Y] ) - ( v1[Y] * v2[X] ),
0.0f );
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector Clamp ( const Vector& v, float min, float max )
{
Vector f = v;
f.Clamp ( min, max );
return f;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector Clamp ( const Vector& v, Vector& min, Vector& max )
{
Vector f = v;
f.Clamp ( min, max );
return f;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float Distance ( const Vector& v1, const Vector& v2 )
{
Vector d = v1 - v2;
return sqrtf ( DotProduct ( d, d ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline float DistanceSqr ( const Vector& v1, const Vector& v2 )
{
Vector d = v1 - v2;
return ( DotProduct ( d, d ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline bool Equal ( const Vector& v1, const Vector& v2, const float tol )
{
return ( Equal ( v1[X], v2[X], tol ) &&
Equal ( v1[Y], v2[Y], tol ) &&
Equal ( v1[Z], v2[Z], tol ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector Lerp ( const Vector& v1, const Vector& v2, const float val )
{
return Vector ( v1 + ( v2 - v1 ) * val );
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector LinearMap ( const Vector& v1, const Vector& v2, float value, float value_min, float value_max )
{
return Vector ( v1 + ( v2 - v1 ) * ( (value - value_min) / (value_max - value_min) ) );
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector Min ( const Vector& v1, const Vector& v2 )
{
return Vector ( Min ( v1[X], v2[X] ),
Min ( v1[Y], v2[Y] ),
Min ( v1[Z], v2[Z] ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector Max ( const Vector& v1, const Vector& v2 )
{
return Vector ( Max ( v1[X], v2[X] ),
Max ( v1[Y], v2[Y] ),
Max ( v1[Z], v2[Z] ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector Min ( const Vector& v, float c )
{
return Min ( v, Vector ( c, c, c, 1.0f ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector Max ( const Vector& v, float c )
{
return Max ( v, Vector ( c, c, c, 0.0f ));
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline void Swap ( Vector& a, Vector& b )
{
Vector t = a;
a = b;
b = t;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline Vector& Vector::ZeroIfShorterThan ( float length )
{
if (LengthSqr() < length *length)
{
Set(0.0f,0.0f,0.0f,0.0f);
}
return *this;
}
/******************************************************************/
/* */
/* */
/******************************************************************/
inline ostream& operator<< ( ostream& os, const Vector& v )
{
return os << "( " << v[X] << ", " << v[Y] << ", " << v[Z] << ", " << v[W] << " )";
}
/******************************************************************/
/* */
/* */
/******************************************************************/
} // namespace Mth
#endif // __CORE_MATH_VECTOR_INL
Reference in New Issue Copy Permalink