mirror of
https://github.com/thug1src/thug.git
synced 2025-01-21 21:33:46 +00:00
1054 lines
33 KiB
C++
1054 lines
33 KiB
C++
/*****************************************************************************
|
||
** **
|
||
** Neversoft Entertainment **
|
||
** **
|
||
** Copyright (C) 1999 - All Rights Reserved **
|
||
** **
|
||
******************************************************************************
|
||
** **
|
||
** Project: Core Library **
|
||
** **
|
||
** Module: Math (Mth) **
|
||
** **
|
||
** File name: core/math/Matrix.inl **
|
||
** **
|
||
** Created: 11/23/99 - mjb **
|
||
** **
|
||
** Description: 4x4 Matrix Math Class **
|
||
** **
|
||
*****************************************************************************/
|
||
|
||
#ifndef __CORE_MATH_MATRIX_INL
|
||
#define __CORE_MATH_MATRIX_INL
|
||
|
||
namespace Mth
|
||
{
|
||
|
||
|
||
|
||
/*****************************************************************************
|
||
** Private Inline Functions **
|
||
*****************************************************************************/
|
||
|
||
|
||
/*****************************************************************************
|
||
** Public Inline Functions **
|
||
*****************************************************************************/
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Ident ( void )
|
||
{
|
||
row[X][X] = 1.0f;
|
||
row[X][Y] = 0.0f;
|
||
row[X][Z] = 0.0f;
|
||
row[X][W] = 0.0f;
|
||
|
||
row[Y][X] = 0.0f;
|
||
row[Y][Y] = 1.0f;
|
||
row[Y][Z] = 0.0f;
|
||
row[Y][W] = 0.0f;
|
||
|
||
row[Z][X] = 0.0f;
|
||
row[Z][Y] = 0.0f;
|
||
row[Z][Z] = 1.0f;
|
||
row[Z][W] = 0.0f;
|
||
|
||
row[W][X] = 0.0f;
|
||
row[W][Y] = 0.0f;
|
||
row[W][Z] = 0.0f;
|
||
row[W][W] = 1.0f;
|
||
|
||
return *this;
|
||
}
|
||
|
||
|
||
inline Matrix::Matrix( void )
|
||
{
|
||
|
||
#ifdef DEBUG_UNINIT_VECTORS
|
||
// For debugging uninitialized vectors, we set them to a magic value
|
||
// and then the vector code should assert when they are accessed
|
||
// via the [] operator, which casts a row to a vector
|
||
*((uint32*)(&row[X][X])) = 0x7F800010; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[X][Y])) = 0x7F800011; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[X][Z])) = 0x7F800012; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[X][W])) = 0x7F800013; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[Y][X])) = 0x7F800014; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[Y][Y])) = 0x7F800015; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[Y][Z])) = 0x7F800016; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[Y][W])) = 0x7F800017; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[Z][X])) = 0x7F800018; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[Z][Y])) = 0x7F800019; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[Z][Z])) = 0x7F80001a; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[Z][W])) = 0x7F80001b; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[W][X])) = 0x7F80001c; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[W][Y])) = 0x7F80001d; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[W][Z])) = 0x7F80001e; // Positive NANS (Not A Number - Signaling)
|
||
*((uint32*)(&row[W][W])) = 0x7F80001f; // Positive NANS (Not A Number - Signaling)
|
||
#else
|
||
#ifdef __INITIALIZE_VECTORS__
|
||
Ident();
|
||
|
||
#endif
|
||
#endif
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix::Matrix( const Vector& axis, const float angle )
|
||
{
|
||
|
||
|
||
CreateRotateMatrix( *this, axis, angle );
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix::Matrix( int axis, const float angle )
|
||
{
|
||
|
||
|
||
CreateRotateMatrix( *this, axis, angle );
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix::Matrix( const Mth::Quat &orientation )
|
||
{
|
||
orientation.GetMatrix(*this);
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline const Vector& Matrix::operator[] ( sint i ) const
|
||
{
|
||
// Dbg_MsgAssert (( i >= X && i < NUM_ELEMENTS ),( "index out of range" ));
|
||
return *(Vector*)(row[i]);
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Vector& Matrix::operator[] ( sint i )
|
||
{
|
||
// Dbg_MsgAssert (( i >= X && i < NUM_ELEMENTS ),( "index out of range" ));
|
||
return *(Mth::Vector*)(&row[i]);
|
||
}
|
||
|
||
|
||
/*
|
||
#ifdef __USE_VU0__
|
||
inline void asm_copy(void* m0, void* m1)
|
||
{
|
||
asm __volatile__("
|
||
lq $6,0x0(%1)
|
||
lq $7,0x10(%1)
|
||
lq $8,0x20(%1)
|
||
lq $9,0x30(%1)
|
||
sq $6,0x0(%0)
|
||
sq $7,0x10(%0)
|
||
sq $8,0x20(%0)
|
||
sq $9,0x30(%0)
|
||
": : "r" (m0) , "r" (m1):"$6","$7","$8","$9");
|
||
}
|
||
#endif
|
||
*/
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::operator= ( const Matrix& src )
|
||
{
|
||
#ifdef __USE_VU0__
|
||
// Mick: This actually is faster than the asm_copy method as
|
||
// the compiler can optimize it better
|
||
*((uint128 *)&row[0])=*((uint128 *)&src[0]);
|
||
*((uint128 *)&row[1])=*((uint128 *)&src[1]);
|
||
*((uint128 *)&row[2])=*((uint128 *)&src[2]);
|
||
*((uint128 *)&row[3])=*((uint128 *)&src[3]);
|
||
// asm_copy(this,(void*)&src[0]);
|
||
#else
|
||
*(Vector*)row[X] = *(Vector*)src.row[X];
|
||
*(Vector*)row[Y] = *(Vector*)src.row[Y];
|
||
*(Vector*)row[Z] = *(Vector*)src.row[Z];
|
||
*(Vector*)row[W] = *(Vector*)src.row[W];
|
||
#endif
|
||
return *this;
|
||
}
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix::Matrix( const Matrix& src )
|
||
{
|
||
*this = src;
|
||
}
|
||
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline void Swap ( Matrix& a, Matrix& b )
|
||
{
|
||
Matrix t = a;
|
||
a = b;
|
||
b = t;
|
||
}
|
||
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::operator*= ( const Matrix& n )
|
||
{
|
||
# ifdef __PLAT_XBOX__
|
||
D3DXMatrixMultiply((D3DXMATRIX*)this, (D3DXMATRIX*)this, (D3DXMATRIX*)&n );
|
||
# else
|
||
#ifdef __USE_VU0__
|
||
sceVu0MulMatrix((sceVu0FVECTOR*)this,(sceVu0FVECTOR*)&n,(sceVu0FVECTOR*)this);
|
||
# else
|
||
|
||
Vector vec;
|
||
|
||
vec[X] = row[X][X] * n[X][X] + row[X][Y] * n[Y][X] + row[X][Z] * n[Z][X] + row[X][W] * n[W][X];
|
||
vec[Y] = row[X][X] * n[X][Y] + row[X][Y] * n[Y][Y] + row[X][Z] * n[Z][Y] + row[X][W] * n[W][Y];
|
||
vec[Z] = row[X][X] * n[X][Z] + row[X][Y] * n[Y][Z] + row[X][Z] * n[Z][Z] + row[X][W] * n[W][Z];
|
||
vec[W] = row[X][X] * n[X][W] + row[X][Y] * n[Y][W] + row[X][Z] * n[Z][W] + row[X][W] * n[W][W];
|
||
*(Vector*)row[X] = vec;
|
||
|
||
vec[X] = row[Y][X] * n[X][X] + row[Y][Y] * n[Y][X] + row[Y][Z] * n[Z][X] + row[Y][W] * n[W][X];
|
||
vec[Y] = row[Y][X] * n[X][Y] + row[Y][Y] * n[Y][Y] + row[Y][Z] * n[Z][Y] + row[Y][W] * n[W][Y];
|
||
vec[Z] = row[Y][X] * n[X][Z] + row[Y][Y] * n[Y][Z] + row[Y][Z] * n[Z][Z] + row[Y][W] * n[W][Z];
|
||
vec[W] = row[Y][X] * n[X][W] + row[Y][Y] * n[Y][W] + row[Y][Z] * n[Z][W] + row[Y][W] * n[W][W];
|
||
*(Vector*)row[Y] = vec;
|
||
|
||
vec[X] = row[Z][X] * n[X][X] + row[Z][Y] * n[Y][X] + row[Z][Z] * n[Z][X] + row[Z][W] * n[W][X];
|
||
vec[Y] = row[Z][X] * n[X][Y] + row[Z][Y] * n[Y][Y] + row[Z][Z] * n[Z][Y] + row[Z][W] * n[W][Y];
|
||
vec[Z] = row[Z][X] * n[X][Z] + row[Z][Y] * n[Y][Z] + row[Z][Z] * n[Z][Z] + row[Z][W] * n[W][Z];
|
||
vec[W] = row[Z][X] * n[X][W] + row[Z][Y] * n[Y][W] + row[Z][Z] * n[Z][W] + row[Z][W] * n[W][W];
|
||
*(Vector*)row[Z] = vec;
|
||
|
||
vec[X] = row[W][X] * n[X][X] + row[W][Y] * n[Y][X] + row[W][Z] * n[Z][X] + row[W][W] * n[W][X];
|
||
vec[Y] = row[W][X] * n[X][Y] + row[W][Y] * n[Y][Y] + row[W][Z] * n[Z][Y] + row[W][W] * n[W][Y];
|
||
vec[Z] = row[W][X] * n[X][Z] + row[W][Y] * n[Y][Z] + row[W][Z] * n[Z][Z] + row[W][W] * n[W][Z];
|
||
vec[W] = row[W][X] * n[X][W] + row[W][Y] * n[Y][W] + row[W][Z] * n[Z][W] + row[W][W] * n[W][W];
|
||
*(Vector*)row[W] = vec;
|
||
# endif
|
||
# endif
|
||
|
||
return *this;
|
||
}
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix operator* ( const Matrix& m1, const Matrix& m2 )
|
||
{
|
||
|
||
|
||
Matrix result = m1;
|
||
result *= m2;
|
||
|
||
return result;
|
||
}
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::operator*= ( const float s )
|
||
{
|
||
|
||
|
||
if ( s != 1.0f )
|
||
{
|
||
*(Vector*)(row[X]) *= s;
|
||
*(Vector*)(row[Y]) *= s;
|
||
*(Vector*)(row[Z]) *= s;
|
||
*(Vector*)(row[W]) *= s;
|
||
}
|
||
|
||
return *this;
|
||
}
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::operator+= ( const Matrix& n )
|
||
{
|
||
row[X][X] += n[X][X];
|
||
row[X][Y] += n[X][Y];
|
||
row[X][Z] += n[X][Z];
|
||
row[X][W] += n[X][W];
|
||
|
||
row[Y][X] += n[Y][X];
|
||
row[Y][Y] += n[Y][Y];
|
||
row[Y][Z] += n[Y][Z];
|
||
row[Y][W] += n[Y][W];
|
||
|
||
row[Z][X] += n[Z][X];
|
||
row[Z][Y] += n[Z][Y];
|
||
row[Z][Z] += n[Z][Z];
|
||
row[Z][W] += n[Z][W];
|
||
|
||
row[W][X] += n[W][X];
|
||
row[W][Y] += n[W][Y];
|
||
row[W][Z] += n[W][Z];
|
||
row[W][W] += n[W][W];
|
||
|
||
return *this;
|
||
}
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline bool Matrix::operator== ( const Matrix& n ) const
|
||
{
|
||
|
||
|
||
for ( sint i = 0; i < NUM_ELEMENTS; i++ )
|
||
{
|
||
for ( sint j = 0; j < NUM_ELEMENTS; j++ )
|
||
{
|
||
if ( row[i][j] != n[i][j] )
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline bool Matrix::operator!= ( const Matrix& n ) const
|
||
{
|
||
|
||
|
||
return !( *this == n );
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline float Matrix::Determinant ( void ) const
|
||
{
|
||
|
||
|
||
return row[X][X] * Determinant3 ( row[Y][Y], row[Z][Y], row[W][Y], row[Y][Z], row[Z][Z], row[W][Z], row[Y][W], row[Z][W], row[W][W] )
|
||
- row[X][Y] * Determinant3 ( row[Y][X], row[Z][X], row[W][X], row[Y][Z], row[Z][Z], row[W][Z], row[Y][W], row[Z][W], row[W][W] )
|
||
+ row[X][Z] * Determinant3 ( row[Y][X], row[Z][X], row[W][X], row[Y][Y], row[Z][Y], row[W][Y], row[Y][W], row[Z][W], row[W][W] )
|
||
- row[X][W] * Determinant3 ( row[Y][X], row[Z][X], row[W][X], row[Y][Y], row[Z][Y], row[W][Y], row[Y][Z], row[Z][Z], row[W][Z] );
|
||
}
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline bool Matrix::IsIdent ( void ) const
|
||
{
|
||
|
||
|
||
return ( row[X][X] == 1.0f && row[X][Y] == 0.0f && row[X][Z] == 0.0f && row[X][W] == 0.0f &&
|
||
row[Y][X] == 0.0f && row[Y][Y] == 1.0f && row[Y][Z] == 0.0f && row[Y][W] == 0.0f &&
|
||
row[Z][X] == 0.0f && row[Z][Y] == 0.0f && row[Z][Z] == 1.0f && row[Z][W] == 0.0f &&
|
||
row[W][X] == 0.0f && row[W][Y] == 0.0f && row[W][Z] == 0.0f && row[W][W] == 1.0f );
|
||
}
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Identity ( void )
|
||
{
|
||
row[X][X] = 1.0f;
|
||
row[X][Y] = 0.0f;
|
||
row[X][Z] = 0.0f;
|
||
row[X][W] = 0.0f;
|
||
|
||
row[Y][X] = 0.0f;
|
||
row[Y][Y] = 1.0f;
|
||
row[Y][Z] = 0.0f;
|
||
row[Y][W] = 0.0f;
|
||
|
||
row[Z][X] = 0.0f;
|
||
row[Z][Y] = 0.0f;
|
||
row[Z][Z] = 1.0f;
|
||
row[Z][W] = 0.0f;
|
||
|
||
row[W][X] = 0.0f;
|
||
row[W][Y] = 0.0f;
|
||
row[W][Z] = 0.0f;
|
||
row[W][W] = 1.0f;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Zero ( void )
|
||
{
|
||
row[X][X] = 0.0f;
|
||
row[X][Y] = 0.0f;
|
||
row[X][Z] = 0.0f;
|
||
row[X][W] = 0.0f;
|
||
|
||
row[Y][X] = 0.0f;
|
||
row[Y][Y] = 0.0f;
|
||
row[Y][Z] = 0.0f;
|
||
row[Y][W] = 0.0f;
|
||
|
||
row[Z][X] = 0.0f;
|
||
row[Z][Y] = 0.0f;
|
||
row[Z][Z] = 0.0f;
|
||
row[Z][W] = 0.0f;
|
||
|
||
row[W][X] = 0.0f;
|
||
row[W][Y] = 0.0f;
|
||
row[W][Z] = 0.0f;
|
||
row[W][W] = 0.0f;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Adjoint ( const Matrix& src )
|
||
{
|
||
|
||
|
||
row[X][X] = Determinant3 ( src[Y][Y], src[Z][Y], src[W][Y], src[Y][Z], src[Z][Z], src[W][Z], src[Y][W], src[Z][W], src[W][W] );
|
||
row[Y][X] = -Determinant3 ( src[Y][X], src[Z][X], src[W][X], src[Y][Z], src[Z][Z], src[W][Z], src[Y][W], src[Z][W], src[W][W] );
|
||
row[Z][X] = Determinant3 ( src[Y][X], src[Z][X], src[W][X], src[Y][Y], src[Z][Y], src[W][Y], src[Y][W], src[Z][W], src[W][W] );
|
||
row[W][X] = -Determinant3 ( src[Y][X], src[Z][X], src[W][X], src[Y][Y], src[Z][Y], src[W][Y], src[Y][Z], src[Z][Z], src[W][Z] );
|
||
|
||
row[X][Y] = -Determinant3 ( src[X][Y], src[Z][Y], src[W][Y], src[X][Z], src[Z][Z], src[W][Z], src[X][W], src[Z][W], src[W][W] );
|
||
row[Y][Y] = Determinant3 ( src[X][X], src[Z][X], src[W][X], src[X][Z], src[Z][Z], src[W][Z], src[X][W], src[Z][W], src[W][W] );
|
||
row[Z][Y] = -Determinant3 ( src[X][X], src[Z][X], src[W][X], src[X][Y], src[Z][Y], src[W][Y], src[X][W], src[Z][W], src[W][W] );
|
||
row[W][Y] = Determinant3 ( src[X][X], src[Z][X], src[W][X], src[X][Y], src[Z][Y], src[W][Y], src[X][Z], src[Z][Z], src[W][Z] );
|
||
|
||
row[X][Z] = Determinant3 ( src[X][Y], src[Y][Y], src[W][Y], src[X][Z], src[Y][Z], src[W][Z], src[X][W], src[Y][W], src[W][W] );
|
||
row[Y][Z] = -Determinant3 ( src[X][X], src[Y][X], src[W][X], src[X][Z], src[Y][Z], src[W][Z], src[X][W], src[Y][W], src[W][W] );
|
||
row[Z][Z] = Determinant3 ( src[X][X], src[Y][X], src[W][X], src[X][Y], src[Y][Y], src[W][Y], src[X][W], src[Y][W], src[W][W] );
|
||
row[W][Z] = -Determinant3 ( src[X][X], src[Y][X], src[W][X], src[X][Y], src[Y][Y], src[W][Y], src[X][Z], src[Y][Z], src[W][Z] );
|
||
|
||
row[X][W] = -Determinant3 ( src[X][Y], src[Y][Y], src[Z][Y], src[X][Z], src[Y][Z], src[Z][Z], src[X][W], src[Y][W], src[Z][W] );
|
||
row[Y][W] = Determinant3 ( src[X][X], src[Y][X], src[Z][X], src[X][Z], src[Y][Z], src[Z][Z], src[X][W], src[Y][W], src[Z][W] );
|
||
row[Z][W] = -Determinant3 ( src[X][X], src[Y][X], src[Z][X], src[X][Y], src[Y][Y], src[Z][Y], src[X][W], src[Y][W], src[Z][W] );
|
||
row[W][W] = Determinant3 ( src[X][X], src[Y][X], src[Z][X], src[X][Y], src[Y][Y], src[Z][Y], src[X][Z], src[Y][Z], src[Z][Z] );
|
||
|
||
return *this;
|
||
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Invert ( const Matrix& src )
|
||
{
|
||
|
||
|
||
Adjoint (src);
|
||
|
||
float det = src.Determinant();
|
||
|
||
if ( det != 1.0f )
|
||
{
|
||
*this *= ( 1.0f / det );
|
||
}
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Invert ( void )
|
||
{
|
||
|
||
|
||
Matrix d;
|
||
|
||
d.Invert( *this );
|
||
|
||
*this = d;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
#ifdef __USE_VU0__
|
||
// K: Copied from sceVu0InversMatrix, just tweaked to take one param & be inline.
|
||
// This is about 18% faster than in C. (1000000 calls take .121 s instead of .148)
|
||
inline void Vu0Invert(sceVu0FMATRIX m0)
|
||
{
|
||
asm __volatile__("
|
||
lq $8,0x0000(%0)
|
||
lq $9,0x0010(%0)
|
||
lq $10,0x0020(%0)
|
||
lqc2 vf4,0x0030(%0)
|
||
|
||
vmove.xyzw vf5,vf4
|
||
vsub.xyz vf4,vf4,vf4 #vf4.xyz=0;
|
||
vmove.xyzw vf9,vf4
|
||
qmfc2 $11,vf4
|
||
|
||
#<EFBFBD>]<5D>u
|
||
pextlw $12,$9,$8
|
||
pextuw $13,$9,$8
|
||
pextlw $14,$11,$10
|
||
pextuw $15,$11,$10
|
||
pcpyld $8,$14,$12
|
||
pcpyud $9,$12,$14
|
||
pcpyld $10,$15,$13
|
||
|
||
qmtc2 $8,vf6
|
||
qmtc2 $9,vf7
|
||
qmtc2 $10,vf8
|
||
|
||
#<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
vmulax.xyz ACC, vf6,vf5
|
||
vmadday.xyz ACC, vf7,vf5
|
||
vmaddz.xyz vf4,vf8,vf5
|
||
vsub.xyz vf4,vf9,vf4
|
||
|
||
sq $8,0x0000(%0)
|
||
sq $9,0x0010(%0)
|
||
sq $10,0x0020(%0)
|
||
sqc2 vf4,0x0030(%0)
|
||
": : "r" (m0) :"$8","$9","$10","$11","$12","$13","$14","$15");
|
||
}
|
||
#endif
|
||
|
||
inline Matrix& Matrix::InvertUniform ()
|
||
{
|
||
|
||
|
||
// Only works for orthonormal Matrix
|
||
|
||
#if 0 // (Mike) this version is buggy!
|
||
|
||
Transpose ( src );
|
||
|
||
row[POS][X] = -DotProduct ( src[POS], src[RIGHT] );
|
||
row[POS][Y] = -DotProduct ( src[POS], src[UP] );
|
||
row[POS][Z] = -DotProduct ( src[POS], src[AT] );
|
||
|
||
return *this;
|
||
|
||
#else // (Mike) try this one instead...
|
||
|
||
// (Dan) actually, don't use the vu0; it returns before the correct values are safely stored in memory; if you access the results too
|
||
// quickly, you get the wrong answers; this is because the compiler may reorder instructions
|
||
#if 0
|
||
// (Ken) or try this one, uses vu0
|
||
Vu0Invert((sceVu0FMATRIX)this);
|
||
return *this;
|
||
#else
|
||
|
||
// need to copy the old row,
|
||
// or else it will change on us as we're doing
|
||
// our dot products...
|
||
|
||
Mth::Vector oldPos = *(Vector*)row[POS];
|
||
|
||
row[POS][X] = -DotProduct ( oldPos, *(Vector*)row[RIGHT] );
|
||
row[POS][Y] = -DotProduct ( oldPos, *(Vector*)row[UP] );
|
||
row[POS][Z] = -DotProduct ( oldPos, *(Vector*)row[AT] );
|
||
|
||
Swap ( row[X][Y], row[Y][X] );
|
||
Swap ( row[X][Z], row[Z][X] );
|
||
Swap ( row[Y][Z], row[Z][Y] );
|
||
|
||
return *this;
|
||
|
||
#endif
|
||
|
||
#endif
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Transpose ( void )
|
||
{
|
||
|
||
|
||
Swap ( row[X][Y], row[Y][X] );
|
||
Swap ( row[X][Z], row[Z][X] );
|
||
Swap ( row[X][W], row[W][X] );
|
||
Swap ( row[Y][Z], row[Z][Y] );
|
||
Swap ( row[Y][W], row[W][Y] );
|
||
Swap ( row[Z][W], row[W][Z] );
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Transpose ( const Matrix& src )
|
||
{
|
||
|
||
|
||
if ( &src == this )
|
||
{
|
||
return Transpose();
|
||
}
|
||
|
||
row[X][X] = src[X][X];
|
||
row[X][Y] = src[Y][X];
|
||
row[X][Z] = src[Z][X];
|
||
row[X][W] = src[W][X];
|
||
row[Y][X] = src[X][Y];
|
||
row[Y][Y] = src[Y][Y];
|
||
row[Y][Z] = src[Z][Y];
|
||
row[Y][W] = src[W][Y];
|
||
row[Z][X] = src[X][Z];
|
||
row[Z][Y] = src[Y][Z];
|
||
row[Z][Z] = src[Z][Z];
|
||
row[Z][W] = src[W][Z];
|
||
row[W][X] = src[X][W];
|
||
row[W][Y] = src[Y][W];
|
||
row[W][Z] = src[Z][W];
|
||
row[W][W] = src[W][W];
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Vector Matrix::Transform ( const Vector& src ) const
|
||
{
|
||
|
||
|
||
Vector v;
|
||
|
||
#ifdef __USE_VU0__
|
||
sceVu0ApplyMatrix((float*)&v,(sceVu0FVECTOR*)this,(float*)&src);
|
||
#else
|
||
|
||
v[X] = src[X] * row[RIGHT][X] + src[Y] * row[UP][X] + src[Z] * row[AT][X] + src[W] * row[POS][X];
|
||
v[Y] = src[X] * row[RIGHT][Y] + src[Y] * row[UP][Y] + src[Z] * row[AT][Y] + src[W] * row[POS][Y];
|
||
v[Z] = src[X] * row[RIGHT][Z] + src[Y] * row[UP][Z] + src[Z] * row[AT][Z] + src[W] * row[POS][Z];
|
||
v[W] = src[X] * row[RIGHT][W] + src[Y] * row[UP][W] + src[Z] * row[AT][W] + src[W] * row[POS][W];
|
||
|
||
#endif
|
||
|
||
return v;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Vector Matrix::TransformAsPos ( const Vector& src ) const
|
||
{
|
||
|
||
Vector v;
|
||
|
||
v[X] = src[X] * row[RIGHT][X] + src[Y] * row[UP][X] + src[Z] * row[AT][X] + row[POS][X];
|
||
v[Y] = src[X] * row[RIGHT][Y] + src[Y] * row[UP][Y] + src[Z] * row[AT][Y] + row[POS][Y];
|
||
v[Z] = src[X] * row[RIGHT][Z] + src[Y] * row[UP][Z] + src[Z] * row[AT][Z] + row[POS][Z];
|
||
v[W] = src[X] * row[RIGHT][W] + src[Y] * row[UP][W] + src[Z] * row[AT][W] + row[POS][W];
|
||
|
||
return v;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Vector Matrix::Rotate ( const Vector& src ) const
|
||
{
|
||
|
||
|
||
Vector v;
|
||
|
||
v[X] = src[X] * row[RIGHT][X] + src[Y] * row[UP][X] + src[Z] * row[AT][X];
|
||
v[Y] = src[X] * row[RIGHT][Y] + src[Y] * row[UP][Y] + src[Z] * row[AT][Y];
|
||
v[Z] = src[X] * row[RIGHT][Z] + src[Y] * row[UP][Z] + src[Z] * row[AT][Z];
|
||
v[W] = src[W];
|
||
|
||
return v;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Translate( const Vector& trans )
|
||
{
|
||
|
||
|
||
*(Vector*)(row[POS]) += trans;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
// Ken: Added cos I needed it.
|
||
inline void Matrix::SetPos( const Vector& trans )
|
||
{
|
||
*(Vector*)(row[POS]) = trans;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::TranslateLocal( const Vector& trans )
|
||
{
|
||
|
||
|
||
Vector& vec = *(Vector*)(row[POS]);
|
||
|
||
vec[X] += (( trans[X] * row[RIGHT][X] ) +
|
||
( trans[Y] * row[UP][X] ) +
|
||
( trans[Z] * row[AT][X] ));
|
||
|
||
vec[Y] += (( trans[X] * row[RIGHT][Y] ) +
|
||
( trans[Y] * row[UP][Y] ) +
|
||
( trans[Z] * row[AT][Y] ));
|
||
|
||
vec[Z] += (( trans[X] * row[RIGHT][Z] ) +
|
||
( trans[Y] * row[UP][Z] ) +
|
||
( trans[Z] * row[AT][Z] ));
|
||
|
||
return *this;
|
||
}
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Rotate( const Vector& axis, const float angle )
|
||
{
|
||
|
||
|
||
Matrix rot_mat( axis, angle );
|
||
|
||
*this *= rot_mat;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::RotateLocal( const Vector& axis, const float angle )
|
||
{
|
||
|
||
|
||
Matrix rot_mat( axis, angle );
|
||
|
||
*this = rot_mat * *this;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::RotateLocal( int axis, const float angle )
|
||
{
|
||
|
||
|
||
Matrix rot_mat( axis, angle );
|
||
|
||
*this = rot_mat * *this;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::RotateX( const float angle )
|
||
{
|
||
|
||
Matrix rotX;
|
||
|
||
*this *= CreateRotateXMatrix( rotX, angle );
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::RotateXLocal( const float angle )
|
||
{
|
||
|
||
Matrix rotX;
|
||
|
||
*this = CreateRotateXMatrix( rotX, angle ) * *this;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::RotateY( const float angle )
|
||
{
|
||
|
||
Matrix rotY;
|
||
|
||
*this *= CreateRotateYMatrix( rotY, angle );
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::RotateYLocal( const float angle )
|
||
{
|
||
|
||
#ifdef __USE_VU0__
|
||
|
||
// Use the end of SCache for a quick and easy speed increase.
|
||
// (Mick, changed to use the end, so start can be used for permanent stuff)
|
||
Matrix *p_rot_y=(Matrix*)(0x70004000-4*4*4);
|
||
|
||
*this = CreateRotateYMatrix( *p_rot_y, angle ) * *this;
|
||
|
||
#else
|
||
|
||
Matrix rotY;
|
||
*this = CreateRotateYMatrix( rotY, angle ) * *this;
|
||
|
||
#endif
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::RotateZ( const float angle )
|
||
{
|
||
|
||
|
||
Matrix rotZ;
|
||
|
||
*this *= CreateRotateZMatrix( rotZ, angle );
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::RotateZLocal( const float angle )
|
||
{
|
||
|
||
|
||
Matrix rotZ;
|
||
|
||
*this = CreateRotateZMatrix( rotZ, angle ) * *this;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::Scale( const Vector& scale )
|
||
{
|
||
|
||
|
||
*(Vector*)(row[RIGHT]) *= scale;
|
||
*(Vector*)(row[UP]) *= scale;
|
||
*(Vector*)(row[AT]) *= scale;
|
||
*(Vector*)(row[POS]) *= scale;
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::ScaleLocal( const Vector& scale )
|
||
{
|
||
|
||
|
||
*(Vector*)(row[RIGHT]) *= scale[X];
|
||
*(Vector*)(row[UP]) *= scale[Y];
|
||
*(Vector*)(row[AT]) *= scale[Z];
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::SetColumn ( sint i, const Vector& v )
|
||
{
|
||
|
||
|
||
// Dbg_MsgAssert (( i >= X && i < NUM_ELEMENTS ),( "index out of range" ));
|
||
|
||
row[X][i] = v[X];
|
||
row[Y][i] = v[Y];
|
||
row[Z][i] = v[Z];
|
||
row[W][i] = v[W];
|
||
|
||
return *this;
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Vector Matrix::GetColumn ( sint i ) const
|
||
{
|
||
|
||
|
||
// Dbg_MsgAssert (( i >= X && i < NUM_ELEMENTS ),( "index out of range" ));
|
||
|
||
return Vector ( row[X][i], row[Y][i], row[Z][i], row[W][i] );
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline ostream& operator<< ( ostream& os, const Matrix& m )
|
||
{
|
||
return os << "{ " << m[X] << ",\n " << m[Y] << ",\n " << m[Z] << ",\n " << m[W] << " }";
|
||
}
|
||
|
||
/******************************************************************/
|
||
/* */
|
||
/* */
|
||
/******************************************************************/
|
||
|
||
inline Matrix& Matrix::SetFromAngles ( const Vector& angles )
|
||
{
|
||
Identity();
|
||
if (angles[X] != 0.0f || angles[Y] != 0.0f || angles[Z] != 0.0f)
|
||
{
|
||
RotateX(angles[X]);
|
||
RotateY(angles[Y]);
|
||
RotateZ(angles[Z]);
|
||
}
|
||
return *this;
|
||
}
|
||
|
||
} // namespace Mth
|
||
|
||
#endif // __CORE_MATH_MATRIX_INL
|
||
|