mirror of
https://github.com/dhewm/dhewm3.git
synced 2024-12-11 13:30:59 +00:00
2946 lines
91 KiB
C++
2946 lines
91 KiB
C++
/*
|
|
===========================================================================
|
|
|
|
Doom 3 GPL Source Code
|
|
Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
|
|
|
|
This file is part of the Doom 3 GPL Source Code ("Doom 3 Source Code").
|
|
|
|
Doom 3 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 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
Doom 3 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 Doom 3 Source Code. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code. If not, please request a copy in writing from id Software at the address below.
|
|
|
|
If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
|
|
|
|
===========================================================================
|
|
*/
|
|
|
|
#ifndef __MATH_MATRIX_H__
|
|
#define __MATH_MATRIX_H__
|
|
|
|
#include "idlib/math/Vector.h"
|
|
|
|
/*
|
|
===============================================================================
|
|
|
|
Matrix classes, all matrices are row-major except idMat3
|
|
|
|
===============================================================================
|
|
*/
|
|
|
|
#define MATRIX_INVERSE_EPSILON 1e-14
|
|
#define MATRIX_EPSILON 1e-6
|
|
|
|
class idAngles;
|
|
class idQuat;
|
|
class idCQuat;
|
|
class idRotation;
|
|
class idMat4;
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat2 - 2x2 matrix
|
|
//
|
|
//===============================================================
|
|
|
|
class idMat2 {
|
|
public:
|
|
idMat2( void );
|
|
explicit idMat2( const idVec2 &x, const idVec2 &y );
|
|
explicit idMat2( const float xx, const float xy, const float yx, const float yy );
|
|
explicit idMat2( const float src[ 2 ][ 2 ] );
|
|
|
|
const idVec2 & operator[]( int index ) const;
|
|
idVec2 & operator[]( int index );
|
|
idMat2 operator-() const;
|
|
idMat2 operator*( const float a ) const;
|
|
idVec2 operator*( const idVec2 &vec ) const;
|
|
idMat2 operator*( const idMat2 &a ) const;
|
|
idMat2 operator+( const idMat2 &a ) const;
|
|
idMat2 operator-( const idMat2 &a ) const;
|
|
idMat2 & operator*=( const float a );
|
|
idMat2 & operator*=( const idMat2 &a );
|
|
idMat2 & operator+=( const idMat2 &a );
|
|
idMat2 & operator-=( const idMat2 &a );
|
|
|
|
friend idMat2 operator*( const float a, const idMat2 &mat );
|
|
friend idVec2 operator*( const idVec2 &vec, const idMat2 &mat );
|
|
friend idVec2 & operator*=( idVec2 &vec, const idMat2 &mat );
|
|
|
|
bool Compare( const idMat2 &a ) const; // exact compare, no epsilon
|
|
bool Compare( const idMat2 &a, const float epsilon ) const; // compare with epsilon
|
|
bool operator==( const idMat2 &a ) const; // exact compare, no epsilon
|
|
bool operator!=( const idMat2 &a ) const; // exact compare, no epsilon
|
|
|
|
void Zero( void );
|
|
void Identity( void );
|
|
bool IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
|
|
float Trace( void ) const;
|
|
float Determinant( void ) const;
|
|
idMat2 Transpose( void ) const; // returns transpose
|
|
idMat2 & TransposeSelf( void );
|
|
idMat2 Inverse( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseSelf( void ); // returns false if determinant is zero
|
|
idMat2 InverseFast( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseFastSelf( void ); // returns false if determinant is zero
|
|
|
|
int GetDimension( void ) const;
|
|
|
|
const float * ToFloatPtr( void ) const;
|
|
float * ToFloatPtr( void );
|
|
const char * ToString( int precision = 2 ) const;
|
|
|
|
private:
|
|
idVec2 mat[ 2 ];
|
|
};
|
|
|
|
extern idMat2 mat2_zero;
|
|
extern idMat2 mat2_identity;
|
|
#define mat2_default mat2_identity
|
|
|
|
ID_INLINE idMat2::idMat2( void ) {
|
|
}
|
|
|
|
ID_INLINE idMat2::idMat2( const idVec2 &x, const idVec2 &y ) {
|
|
mat[ 0 ].x = x.x; mat[ 0 ].y = x.y;
|
|
mat[ 1 ].x = y.x; mat[ 1 ].y = y.y;
|
|
}
|
|
|
|
ID_INLINE idMat2::idMat2( const float xx, const float xy, const float yx, const float yy ) {
|
|
mat[ 0 ].x = xx; mat[ 0 ].y = xy;
|
|
mat[ 1 ].x = yx; mat[ 1 ].y = yy;
|
|
}
|
|
|
|
ID_INLINE idMat2::idMat2( const float src[ 2 ][ 2 ] ) {
|
|
memcpy( mat, src, 2 * 2 * sizeof( float ) );
|
|
}
|
|
|
|
ID_INLINE const idVec2 &idMat2::operator[]( int index ) const {
|
|
//assert( ( index >= 0 ) && ( index < 2 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idVec2 &idMat2::operator[]( int index ) {
|
|
//assert( ( index >= 0 ) && ( index < 2 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idMat2 idMat2::operator-() const {
|
|
return idMat2( -mat[0][0], -mat[0][1],
|
|
-mat[1][0], -mat[1][1] );
|
|
}
|
|
|
|
ID_INLINE idVec2 idMat2::operator*( const idVec2 &vec ) const {
|
|
return idVec2(
|
|
mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y,
|
|
mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y );
|
|
}
|
|
|
|
ID_INLINE idMat2 idMat2::operator*( const idMat2 &a ) const {
|
|
return idMat2(
|
|
mat[0].x * a[0].x + mat[0].y * a[1].x,
|
|
mat[0].x * a[0].y + mat[0].y * a[1].y,
|
|
mat[1].x * a[0].x + mat[1].y * a[1].x,
|
|
mat[1].x * a[0].y + mat[1].y * a[1].y );
|
|
}
|
|
|
|
ID_INLINE idMat2 idMat2::operator*( const float a ) const {
|
|
return idMat2(
|
|
mat[0].x * a, mat[0].y * a,
|
|
mat[1].x * a, mat[1].y * a );
|
|
}
|
|
|
|
ID_INLINE idMat2 idMat2::operator+( const idMat2 &a ) const {
|
|
return idMat2(
|
|
mat[0].x + a[0].x, mat[0].y + a[0].y,
|
|
mat[1].x + a[1].x, mat[1].y + a[1].y );
|
|
}
|
|
|
|
ID_INLINE idMat2 idMat2::operator-( const idMat2 &a ) const {
|
|
return idMat2(
|
|
mat[0].x - a[0].x, mat[0].y - a[0].y,
|
|
mat[1].x - a[1].x, mat[1].y - a[1].y );
|
|
}
|
|
|
|
ID_INLINE idMat2 &idMat2::operator*=( const float a ) {
|
|
mat[0].x *= a; mat[0].y *= a;
|
|
mat[1].x *= a; mat[1].y *= a;
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat2 &idMat2::operator*=( const idMat2 &a ) {
|
|
float x, y;
|
|
x = mat[0].x; y = mat[0].y;
|
|
mat[0].x = x * a[0].x + y * a[1].x;
|
|
mat[0].y = x * a[0].y + y * a[1].y;
|
|
x = mat[1].x; y = mat[1].y;
|
|
mat[1].x = x * a[0].x + y * a[1].x;
|
|
mat[1].y = x * a[0].y + y * a[1].y;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat2 &idMat2::operator+=( const idMat2 &a ) {
|
|
mat[0].x += a[0].x; mat[0].y += a[0].y;
|
|
mat[1].x += a[1].x; mat[1].y += a[1].y;
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat2 &idMat2::operator-=( const idMat2 &a ) {
|
|
mat[0].x -= a[0].x; mat[0].y -= a[0].y;
|
|
mat[1].x -= a[1].x; mat[1].y -= a[1].y;
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idVec2 operator*( const idVec2 &vec, const idMat2 &mat ) {
|
|
return mat * vec;
|
|
}
|
|
|
|
ID_INLINE idMat2 operator*( const float a, idMat2 const &mat ) {
|
|
return mat * a;
|
|
}
|
|
|
|
ID_INLINE idVec2 &operator*=( idVec2 &vec, const idMat2 &mat ) {
|
|
vec = mat * vec;
|
|
return vec;
|
|
}
|
|
|
|
ID_INLINE bool idMat2::Compare( const idMat2 &a ) const {
|
|
if ( mat[0].Compare( a[0] ) &&
|
|
mat[1].Compare( a[1] ) ) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
ID_INLINE bool idMat2::Compare( const idMat2 &a, const float epsilon ) const {
|
|
if ( mat[0].Compare( a[0], epsilon ) &&
|
|
mat[1].Compare( a[1], epsilon ) ) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
ID_INLINE bool idMat2::operator==( const idMat2 &a ) const {
|
|
return Compare( a );
|
|
}
|
|
|
|
ID_INLINE bool idMat2::operator!=( const idMat2 &a ) const {
|
|
return !Compare( a );
|
|
}
|
|
|
|
ID_INLINE void idMat2::Zero( void ) {
|
|
mat[0].Zero();
|
|
mat[1].Zero();
|
|
}
|
|
|
|
ID_INLINE void idMat2::Identity( void ) {
|
|
*this = mat2_identity;
|
|
}
|
|
|
|
ID_INLINE bool idMat2::IsIdentity( const float epsilon ) const {
|
|
return Compare( mat2_identity, epsilon );
|
|
}
|
|
|
|
ID_INLINE bool idMat2::IsSymmetric( const float epsilon ) const {
|
|
return ( idMath::Fabs( mat[0][1] - mat[1][0] ) < epsilon );
|
|
}
|
|
|
|
ID_INLINE bool idMat2::IsDiagonal( const float epsilon ) const {
|
|
if ( idMath::Fabs( mat[0][1] ) > epsilon ||
|
|
idMath::Fabs( mat[1][0] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE float idMat2::Trace( void ) const {
|
|
return ( mat[0][0] + mat[1][1] );
|
|
}
|
|
|
|
ID_INLINE float idMat2::Determinant( void ) const {
|
|
return mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
|
|
}
|
|
|
|
ID_INLINE idMat2 idMat2::Transpose( void ) const {
|
|
return idMat2( mat[0][0], mat[1][0],
|
|
mat[0][1], mat[1][1] );
|
|
}
|
|
|
|
ID_INLINE idMat2 &idMat2::TransposeSelf( void ) {
|
|
float tmp;
|
|
|
|
tmp = mat[0][1];
|
|
mat[0][1] = mat[1][0];
|
|
mat[1][0] = tmp;
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat2 idMat2::Inverse( void ) const {
|
|
idMat2 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE idMat2 idMat2::InverseFast( void ) const {
|
|
idMat2 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseFastSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE int idMat2::GetDimension( void ) const {
|
|
return 4;
|
|
}
|
|
|
|
ID_INLINE const float *idMat2::ToFloatPtr( void ) const {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
ID_INLINE float *idMat2::ToFloatPtr( void ) {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat3 - 3x3 matrix
|
|
//
|
|
// NOTE: matrix is column-major
|
|
//
|
|
//===============================================================
|
|
|
|
class idMat3 {
|
|
public:
|
|
idMat3( void );
|
|
explicit idMat3( const idVec3 &x, const idVec3 &y, const idVec3 &z );
|
|
explicit idMat3( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz );
|
|
explicit idMat3( const float src[ 3 ][ 3 ] );
|
|
|
|
const idVec3 & operator[]( int index ) const;
|
|
idVec3 & operator[]( int index );
|
|
idMat3 operator-() const;
|
|
idMat3 operator*( const float a ) const;
|
|
idVec3 operator*( const idVec3 &vec ) const;
|
|
idMat3 operator*( const idMat3 &a ) const;
|
|
idMat3 operator+( const idMat3 &a ) const;
|
|
idMat3 operator-( const idMat3 &a ) const;
|
|
idMat3 & operator*=( const float a );
|
|
idMat3 & operator*=( const idMat3 &a );
|
|
idMat3 & operator+=( const idMat3 &a );
|
|
idMat3 & operator-=( const idMat3 &a );
|
|
|
|
friend idMat3 operator*( const float a, const idMat3 &mat );
|
|
friend idVec3 operator*( const idVec3 &vec, const idMat3 &mat );
|
|
friend idVec3 & operator*=( idVec3 &vec, const idMat3 &mat );
|
|
|
|
bool Compare( const idMat3 &a ) const; // exact compare, no epsilon
|
|
bool Compare( const idMat3 &a, const float epsilon ) const; // compare with epsilon
|
|
bool operator==( const idMat3 &a ) const; // exact compare, no epsilon
|
|
bool operator!=( const idMat3 &a ) const; // exact compare, no epsilon
|
|
|
|
void Zero( void );
|
|
void Identity( void );
|
|
bool IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsRotated( void ) const;
|
|
|
|
void ProjectVector( const idVec3 &src, idVec3 &dst ) const;
|
|
void UnprojectVector( const idVec3 &src, idVec3 &dst ) const;
|
|
|
|
bool FixDegeneracies( void ); // fix degenerate axial cases
|
|
bool FixDenormals( void ); // change tiny numbers to zero
|
|
|
|
float Trace( void ) const;
|
|
float Determinant( void ) const;
|
|
idMat3 OrthoNormalize( void ) const;
|
|
idMat3 & OrthoNormalizeSelf( void );
|
|
idMat3 Transpose( void ) const; // returns transpose
|
|
idMat3 & TransposeSelf( void );
|
|
idMat3 Inverse( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseSelf( void ); // returns false if determinant is zero
|
|
idMat3 InverseFast( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseFastSelf( void ); // returns false if determinant is zero
|
|
idMat3 TransposeMultiply( const idMat3 &b ) const;
|
|
|
|
idMat3 InertiaTranslate( const float mass, const idVec3 ¢erOfMass, const idVec3 &translation ) const;
|
|
idMat3 & InertiaTranslateSelf( const float mass, const idVec3 ¢erOfMass, const idVec3 &translation );
|
|
idMat3 InertiaRotate( const idMat3 &rotation ) const;
|
|
idMat3 & InertiaRotateSelf( const idMat3 &rotation );
|
|
|
|
int GetDimension( void ) const;
|
|
|
|
idAngles ToAngles( void ) const;
|
|
idQuat ToQuat( void ) const;
|
|
idCQuat ToCQuat( void ) const;
|
|
idRotation ToRotation( void ) const;
|
|
idMat4 ToMat4( void ) const;
|
|
idVec3 ToAngularVelocity( void ) const;
|
|
const float * ToFloatPtr( void ) const;
|
|
float * ToFloatPtr( void );
|
|
const char * ToString( int precision = 2 ) const;
|
|
|
|
friend void TransposeMultiply( const idMat3 &inv, const idMat3 &b, idMat3 &dst );
|
|
friend idMat3 SkewSymmetric( idVec3 const &src );
|
|
|
|
private:
|
|
idVec3 mat[ 3 ];
|
|
};
|
|
|
|
extern idMat3 mat3_zero;
|
|
extern idMat3 mat3_identity;
|
|
#define mat3_default mat3_identity
|
|
|
|
ID_INLINE idMat3::idMat3( void ) {
|
|
}
|
|
|
|
ID_INLINE idMat3::idMat3( const idVec3 &x, const idVec3 &y, const idVec3 &z ) {
|
|
mat[ 0 ].x = x.x; mat[ 0 ].y = x.y; mat[ 0 ].z = x.z;
|
|
mat[ 1 ].x = y.x; mat[ 1 ].y = y.y; mat[ 1 ].z = y.z;
|
|
mat[ 2 ].x = z.x; mat[ 2 ].y = z.y; mat[ 2 ].z = z.z;
|
|
}
|
|
|
|
ID_INLINE idMat3::idMat3( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz ) {
|
|
mat[ 0 ].x = xx; mat[ 0 ].y = xy; mat[ 0 ].z = xz;
|
|
mat[ 1 ].x = yx; mat[ 1 ].y = yy; mat[ 1 ].z = yz;
|
|
mat[ 2 ].x = zx; mat[ 2 ].y = zy; mat[ 2 ].z = zz;
|
|
}
|
|
|
|
ID_INLINE idMat3::idMat3( const float src[ 3 ][ 3 ] ) {
|
|
memcpy( mat, src, 3 * 3 * sizeof( float ) );
|
|
}
|
|
|
|
ID_INLINE const idVec3 &idMat3::operator[]( int index ) const {
|
|
//assert( ( index >= 0 ) && ( index < 3 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idVec3 &idMat3::operator[]( int index ) {
|
|
//assert( ( index >= 0 ) && ( index < 3 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::operator-() const {
|
|
return idMat3( -mat[0][0], -mat[0][1], -mat[0][2],
|
|
-mat[1][0], -mat[1][1], -mat[1][2],
|
|
-mat[2][0], -mat[2][1], -mat[2][2] );
|
|
}
|
|
|
|
ID_INLINE idVec3 idMat3::operator*( const idVec3 &vec ) const {
|
|
return idVec3(
|
|
mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z,
|
|
mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z,
|
|
mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z );
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::operator*( const idMat3 &a ) const {
|
|
int i, j;
|
|
const float *m1Ptr, *m2Ptr;
|
|
float *dstPtr;
|
|
idMat3 dst;
|
|
|
|
m1Ptr = reinterpret_cast<const float *>(this);
|
|
m2Ptr = reinterpret_cast<const float *>(&a);
|
|
dstPtr = reinterpret_cast<float *>(&dst);
|
|
|
|
for ( i = 0; i < 3; i++ ) {
|
|
for ( j = 0; j < 3; j++ ) {
|
|
*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
|
|
+ m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
|
|
+ m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
|
|
dstPtr++;
|
|
}
|
|
m1Ptr += 3;
|
|
}
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::operator*( const float a ) const {
|
|
return idMat3(
|
|
mat[0].x * a, mat[0].y * a, mat[0].z * a,
|
|
mat[1].x * a, mat[1].y * a, mat[1].z * a,
|
|
mat[2].x * a, mat[2].y * a, mat[2].z * a );
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::operator+( const idMat3 &a ) const {
|
|
return idMat3(
|
|
mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z,
|
|
mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z,
|
|
mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z );
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::operator-( const idMat3 &a ) const {
|
|
return idMat3(
|
|
mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z,
|
|
mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z,
|
|
mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z );
|
|
}
|
|
|
|
ID_INLINE idMat3 &idMat3::operator*=( const float a ) {
|
|
mat[0].x *= a; mat[0].y *= a; mat[0].z *= a;
|
|
mat[1].x *= a; mat[1].y *= a; mat[1].z *= a;
|
|
mat[2].x *= a; mat[2].y *= a; mat[2].z *= a;
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat3 &idMat3::operator*=( const idMat3 &a ) {
|
|
int i, j;
|
|
const float *m2Ptr;
|
|
float *m1Ptr, dst[3];
|
|
|
|
m1Ptr = reinterpret_cast<float *>(this);
|
|
m2Ptr = reinterpret_cast<const float *>(&a);
|
|
|
|
for ( i = 0; i < 3; i++ ) {
|
|
for ( j = 0; j < 3; j++ ) {
|
|
dst[j] = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
|
|
+ m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
|
|
+ m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
|
|
}
|
|
m1Ptr[0] = dst[0]; m1Ptr[1] = dst[1]; m1Ptr[2] = dst[2];
|
|
m1Ptr += 3;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat3 &idMat3::operator+=( const idMat3 &a ) {
|
|
mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z;
|
|
mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z;
|
|
mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z;
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat3 &idMat3::operator-=( const idMat3 &a ) {
|
|
mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z;
|
|
mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z;
|
|
mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z;
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idVec3 operator*( const idVec3 &vec, const idMat3 &mat ) {
|
|
return mat * vec;
|
|
}
|
|
|
|
ID_INLINE idMat3 operator*( const float a, const idMat3 &mat ) {
|
|
return mat * a;
|
|
}
|
|
|
|
ID_INLINE idVec3 &operator*=( idVec3 &vec, const idMat3 &mat ) {
|
|
float x = mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z;
|
|
float y = mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z;
|
|
vec.z = mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z;
|
|
vec.x = x;
|
|
vec.y = y;
|
|
return vec;
|
|
}
|
|
|
|
ID_INLINE bool idMat3::Compare( const idMat3 &a ) const {
|
|
if ( mat[0].Compare( a[0] ) &&
|
|
mat[1].Compare( a[1] ) &&
|
|
mat[2].Compare( a[2] ) ) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
ID_INLINE bool idMat3::Compare( const idMat3 &a, const float epsilon ) const {
|
|
if ( mat[0].Compare( a[0], epsilon ) &&
|
|
mat[1].Compare( a[1], epsilon ) &&
|
|
mat[2].Compare( a[2], epsilon ) ) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
ID_INLINE bool idMat3::operator==( const idMat3 &a ) const {
|
|
return Compare( a );
|
|
}
|
|
|
|
ID_INLINE bool idMat3::operator!=( const idMat3 &a ) const {
|
|
return !Compare( a );
|
|
}
|
|
|
|
ID_INLINE void idMat3::Zero( void ) {
|
|
memset( mat, 0, sizeof( idMat3 ) );
|
|
}
|
|
|
|
ID_INLINE void idMat3::Identity( void ) {
|
|
*this = mat3_identity;
|
|
}
|
|
|
|
ID_INLINE bool idMat3::IsIdentity( const float epsilon ) const {
|
|
return Compare( mat3_identity, epsilon );
|
|
}
|
|
|
|
ID_INLINE bool idMat3::IsSymmetric( const float epsilon ) const {
|
|
if ( idMath::Fabs( mat[0][1] - mat[1][0] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
if ( idMath::Fabs( mat[0][2] - mat[2][0] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
if ( idMath::Fabs( mat[1][2] - mat[2][1] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat3::IsDiagonal( const float epsilon ) const {
|
|
if ( idMath::Fabs( mat[0][1] ) > epsilon ||
|
|
idMath::Fabs( mat[0][2] ) > epsilon ||
|
|
idMath::Fabs( mat[1][0] ) > epsilon ||
|
|
idMath::Fabs( mat[1][2] ) > epsilon ||
|
|
idMath::Fabs( mat[2][0] ) > epsilon ||
|
|
idMath::Fabs( mat[2][1] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat3::IsRotated( void ) const {
|
|
return !Compare( mat3_identity );
|
|
}
|
|
|
|
ID_INLINE void idMat3::ProjectVector( const idVec3 &src, idVec3 &dst ) const {
|
|
dst.x = src * mat[ 0 ];
|
|
dst.y = src * mat[ 1 ];
|
|
dst.z = src * mat[ 2 ];
|
|
}
|
|
|
|
ID_INLINE void idMat3::UnprojectVector( const idVec3 &src, idVec3 &dst ) const {
|
|
dst = mat[ 0 ] * src.x + mat[ 1 ] * src.y + mat[ 2 ] * src.z;
|
|
}
|
|
|
|
ID_INLINE bool idMat3::FixDegeneracies( void ) {
|
|
bool r = mat[0].FixDegenerateNormal();
|
|
r |= mat[1].FixDegenerateNormal();
|
|
r |= mat[2].FixDegenerateNormal();
|
|
return r;
|
|
}
|
|
|
|
ID_INLINE bool idMat3::FixDenormals( void ) {
|
|
bool r = mat[0].FixDenormals();
|
|
r |= mat[1].FixDenormals();
|
|
r |= mat[2].FixDenormals();
|
|
return r;
|
|
}
|
|
|
|
ID_INLINE float idMat3::Trace( void ) const {
|
|
return ( mat[0][0] + mat[1][1] + mat[2][2] );
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::OrthoNormalize( void ) const {
|
|
idMat3 ortho;
|
|
|
|
ortho = *this;
|
|
ortho[ 0 ].Normalize();
|
|
ortho[ 2 ].Cross( mat[ 0 ], mat[ 1 ] );
|
|
ortho[ 2 ].Normalize();
|
|
ortho[ 1 ].Cross( mat[ 2 ], mat[ 0 ] );
|
|
ortho[ 1 ].Normalize();
|
|
return ortho;
|
|
}
|
|
|
|
ID_INLINE idMat3 &idMat3::OrthoNormalizeSelf( void ) {
|
|
mat[ 0 ].Normalize();
|
|
mat[ 2 ].Cross( mat[ 0 ], mat[ 1 ] );
|
|
mat[ 2 ].Normalize();
|
|
mat[ 1 ].Cross( mat[ 2 ], mat[ 0 ] );
|
|
mat[ 1 ].Normalize();
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::Transpose( void ) const {
|
|
return idMat3( mat[0][0], mat[1][0], mat[2][0],
|
|
mat[0][1], mat[1][1], mat[2][1],
|
|
mat[0][2], mat[1][2], mat[2][2] );
|
|
}
|
|
|
|
ID_INLINE idMat3 &idMat3::TransposeSelf( void ) {
|
|
float tmp0, tmp1, tmp2;
|
|
|
|
tmp0 = mat[0][1];
|
|
mat[0][1] = mat[1][0];
|
|
mat[1][0] = tmp0;
|
|
tmp1 = mat[0][2];
|
|
mat[0][2] = mat[2][0];
|
|
mat[2][0] = tmp1;
|
|
tmp2 = mat[1][2];
|
|
mat[1][2] = mat[2][1];
|
|
mat[2][1] = tmp2;
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::Inverse( void ) const {
|
|
idMat3 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::InverseFast( void ) const {
|
|
idMat3 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseFastSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat3::TransposeMultiply( const idMat3 &b ) const {
|
|
return idMat3( mat[0].x * b[0].x + mat[1].x * b[1].x + mat[2].x * b[2].x,
|
|
mat[0].x * b[0].y + mat[1].x * b[1].y + mat[2].x * b[2].y,
|
|
mat[0].x * b[0].z + mat[1].x * b[1].z + mat[2].x * b[2].z,
|
|
mat[0].y * b[0].x + mat[1].y * b[1].x + mat[2].y * b[2].x,
|
|
mat[0].y * b[0].y + mat[1].y * b[1].y + mat[2].y * b[2].y,
|
|
mat[0].y * b[0].z + mat[1].y * b[1].z + mat[2].y * b[2].z,
|
|
mat[0].z * b[0].x + mat[1].z * b[1].x + mat[2].z * b[2].x,
|
|
mat[0].z * b[0].y + mat[1].z * b[1].y + mat[2].z * b[2].y,
|
|
mat[0].z * b[0].z + mat[1].z * b[1].z + mat[2].z * b[2].z );
|
|
}
|
|
|
|
ID_INLINE void TransposeMultiply( const idMat3 &transpose, const idMat3 &b, idMat3 &dst ) {
|
|
dst[0].x = transpose[0].x * b[0].x + transpose[1].x * b[1].x + transpose[2].x * b[2].x;
|
|
dst[0].y = transpose[0].x * b[0].y + transpose[1].x * b[1].y + transpose[2].x * b[2].y;
|
|
dst[0].z = transpose[0].x * b[0].z + transpose[1].x * b[1].z + transpose[2].x * b[2].z;
|
|
dst[1].x = transpose[0].y * b[0].x + transpose[1].y * b[1].x + transpose[2].y * b[2].x;
|
|
dst[1].y = transpose[0].y * b[0].y + transpose[1].y * b[1].y + transpose[2].y * b[2].y;
|
|
dst[1].z = transpose[0].y * b[0].z + transpose[1].y * b[1].z + transpose[2].y * b[2].z;
|
|
dst[2].x = transpose[0].z * b[0].x + transpose[1].z * b[1].x + transpose[2].z * b[2].x;
|
|
dst[2].y = transpose[0].z * b[0].y + transpose[1].z * b[1].y + transpose[2].z * b[2].y;
|
|
dst[2].z = transpose[0].z * b[0].z + transpose[1].z * b[1].z + transpose[2].z * b[2].z;
|
|
}
|
|
|
|
ID_INLINE idMat3 SkewSymmetric( idVec3 const &src ) {
|
|
return idMat3( 0.0f, -src.z, src.y, src.z, 0.0f, -src.x, -src.y, src.x, 0.0f );
|
|
}
|
|
|
|
ID_INLINE int idMat3::GetDimension( void ) const {
|
|
return 9;
|
|
}
|
|
|
|
ID_INLINE const float *idMat3::ToFloatPtr( void ) const {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
ID_INLINE float *idMat3::ToFloatPtr( void ) {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat4 - 4x4 matrix
|
|
//
|
|
//===============================================================
|
|
|
|
class idMat4 {
|
|
public:
|
|
idMat4( void );
|
|
explicit idMat4( const idVec4 &x, const idVec4 &y, const idVec4 &z, const idVec4 &w );
|
|
explicit idMat4(const float xx, const float xy, const float xz, const float xw,
|
|
const float yx, const float yy, const float yz, const float yw,
|
|
const float zx, const float zy, const float zz, const float zw,
|
|
const float wx, const float wy, const float wz, const float ww );
|
|
explicit idMat4( const idMat3 &rotation, const idVec3 &translation );
|
|
explicit idMat4( const float src[ 4 ][ 4 ] );
|
|
|
|
const idVec4 & operator[]( int index ) const;
|
|
idVec4 & operator[]( int index );
|
|
idMat4 operator*( const float a ) const;
|
|
idVec4 operator*( const idVec4 &vec ) const;
|
|
idVec3 operator*( const idVec3 &vec ) const;
|
|
idMat4 operator*( const idMat4 &a ) const;
|
|
idMat4 operator+( const idMat4 &a ) const;
|
|
idMat4 operator-( const idMat4 &a ) const;
|
|
idMat4 & operator*=( const float a );
|
|
idMat4 & operator*=( const idMat4 &a );
|
|
idMat4 & operator+=( const idMat4 &a );
|
|
idMat4 & operator-=( const idMat4 &a );
|
|
|
|
friend idMat4 operator*( const float a, const idMat4 &mat );
|
|
friend idVec4 operator*( const idVec4 &vec, const idMat4 &mat );
|
|
friend idVec3 operator*( const idVec3 &vec, const idMat4 &mat );
|
|
friend idVec4 & operator*=( idVec4 &vec, const idMat4 &mat );
|
|
friend idVec3 & operator*=( idVec3 &vec, const idMat4 &mat );
|
|
|
|
bool Compare( const idMat4 &a ) const; // exact compare, no epsilon
|
|
bool Compare( const idMat4 &a, const float epsilon ) const; // compare with epsilon
|
|
bool operator==( const idMat4 &a ) const; // exact compare, no epsilon
|
|
bool operator!=( const idMat4 &a ) const; // exact compare, no epsilon
|
|
|
|
void Zero( void );
|
|
void Identity( void );
|
|
bool IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsRotated( void ) const;
|
|
|
|
void ProjectVector( const idVec4 &src, idVec4 &dst ) const;
|
|
void UnprojectVector( const idVec4 &src, idVec4 &dst ) const;
|
|
|
|
float Trace( void ) const;
|
|
float Determinant( void ) const;
|
|
idMat4 Transpose( void ) const; // returns transpose
|
|
idMat4 & TransposeSelf( void );
|
|
idMat4 Inverse( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseSelf( void ); // returns false if determinant is zero
|
|
idMat4 InverseFast( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseFastSelf( void ); // returns false if determinant is zero
|
|
idMat4 TransposeMultiply( const idMat4 &b ) const;
|
|
|
|
int GetDimension( void ) const;
|
|
|
|
const float * ToFloatPtr( void ) const;
|
|
float * ToFloatPtr( void );
|
|
const char * ToString( int precision = 2 ) const;
|
|
|
|
private:
|
|
idVec4 mat[ 4 ];
|
|
};
|
|
|
|
extern idMat4 mat4_zero;
|
|
extern idMat4 mat4_identity;
|
|
#define mat4_default mat4_identity
|
|
|
|
ID_INLINE idMat4::idMat4( void ) {
|
|
}
|
|
|
|
ID_INLINE idMat4::idMat4( const idVec4 &x, const idVec4 &y, const idVec4 &z, const idVec4 &w ) {
|
|
mat[ 0 ] = x;
|
|
mat[ 1 ] = y;
|
|
mat[ 2 ] = z;
|
|
mat[ 3 ] = w;
|
|
}
|
|
|
|
ID_INLINE idMat4::idMat4( const float xx, const float xy, const float xz, const float xw,
|
|
const float yx, const float yy, const float yz, const float yw,
|
|
const float zx, const float zy, const float zz, const float zw,
|
|
const float wx, const float wy, const float wz, const float ww ) {
|
|
mat[0][0] = xx; mat[0][1] = xy; mat[0][2] = xz; mat[0][3] = xw;
|
|
mat[1][0] = yx; mat[1][1] = yy; mat[1][2] = yz; mat[1][3] = yw;
|
|
mat[2][0] = zx; mat[2][1] = zy; mat[2][2] = zz; mat[2][3] = zw;
|
|
mat[3][0] = wx; mat[3][1] = wy; mat[3][2] = wz; mat[3][3] = ww;
|
|
}
|
|
|
|
ID_INLINE idMat4::idMat4( const idMat3 &rotation, const idVec3 &translation ) {
|
|
// NOTE: idMat3 is transposed because it is column-major
|
|
mat[ 0 ][ 0 ] = rotation[0][0];
|
|
mat[ 0 ][ 1 ] = rotation[1][0];
|
|
mat[ 0 ][ 2 ] = rotation[2][0];
|
|
mat[ 0 ][ 3 ] = translation[0];
|
|
mat[ 1 ][ 0 ] = rotation[0][1];
|
|
mat[ 1 ][ 1 ] = rotation[1][1];
|
|
mat[ 1 ][ 2 ] = rotation[2][1];
|
|
mat[ 1 ][ 3 ] = translation[1];
|
|
mat[ 2 ][ 0 ] = rotation[0][2];
|
|
mat[ 2 ][ 1 ] = rotation[1][2];
|
|
mat[ 2 ][ 2 ] = rotation[2][2];
|
|
mat[ 2 ][ 3 ] = translation[2];
|
|
mat[ 3 ][ 0 ] = 0.0f;
|
|
mat[ 3 ][ 1 ] = 0.0f;
|
|
mat[ 3 ][ 2 ] = 0.0f;
|
|
mat[ 3 ][ 3 ] = 1.0f;
|
|
}
|
|
|
|
ID_INLINE idMat4::idMat4( const float src[ 4 ][ 4 ] ) {
|
|
memcpy( mat, src, 4 * 4 * sizeof( float ) );
|
|
}
|
|
|
|
ID_INLINE const idVec4 &idMat4::operator[]( int index ) const {
|
|
//assert( ( index >= 0 ) && ( index < 4 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idVec4 &idMat4::operator[]( int index ) {
|
|
//assert( ( index >= 0 ) && ( index < 4 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idMat4 idMat4::operator*( const float a ) const {
|
|
return idMat4(
|
|
mat[0].x * a, mat[0].y * a, mat[0].z * a, mat[0].w * a,
|
|
mat[1].x * a, mat[1].y * a, mat[1].z * a, mat[1].w * a,
|
|
mat[2].x * a, mat[2].y * a, mat[2].z * a, mat[2].w * a,
|
|
mat[3].x * a, mat[3].y * a, mat[3].z * a, mat[3].w * a );
|
|
}
|
|
|
|
ID_INLINE idVec4 idMat4::operator*( const idVec4 &vec ) const {
|
|
return idVec4(
|
|
mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w * vec.w,
|
|
mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w * vec.w,
|
|
mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w * vec.w,
|
|
mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w * vec.w );
|
|
}
|
|
|
|
ID_INLINE idVec3 idMat4::operator*( const idVec3 &vec ) const {
|
|
float s = mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w;
|
|
if ( s == 0.0f ) {
|
|
return idVec3( 0.0f, 0.0f, 0.0f );
|
|
}
|
|
if ( s == 1.0f ) {
|
|
return idVec3(
|
|
mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w,
|
|
mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w,
|
|
mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w );
|
|
}
|
|
else {
|
|
float invS = 1.0f / s;
|
|
return idVec3(
|
|
(mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w) * invS,
|
|
(mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w) * invS,
|
|
(mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w) * invS );
|
|
}
|
|
}
|
|
|
|
ID_INLINE idMat4 idMat4::operator*( const idMat4 &a ) const {
|
|
int i, j;
|
|
const float *m1Ptr, *m2Ptr;
|
|
float *dstPtr;
|
|
idMat4 dst;
|
|
|
|
m1Ptr = reinterpret_cast<const float *>(this);
|
|
m2Ptr = reinterpret_cast<const float *>(&a);
|
|
dstPtr = reinterpret_cast<float *>(&dst);
|
|
|
|
for ( i = 0; i < 4; i++ ) {
|
|
for ( j = 0; j < 4; j++ ) {
|
|
*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 4 + j ]
|
|
+ m1Ptr[1] * m2Ptr[ 1 * 4 + j ]
|
|
+ m1Ptr[2] * m2Ptr[ 2 * 4 + j ]
|
|
+ m1Ptr[3] * m2Ptr[ 3 * 4 + j ];
|
|
dstPtr++;
|
|
}
|
|
m1Ptr += 4;
|
|
}
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE idMat4 idMat4::operator+( const idMat4 &a ) const {
|
|
return idMat4(
|
|
mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z, mat[0].w + a[0].w,
|
|
mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z, mat[1].w + a[1].w,
|
|
mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z, mat[2].w + a[2].w,
|
|
mat[3].x + a[3].x, mat[3].y + a[3].y, mat[3].z + a[3].z, mat[3].w + a[3].w );
|
|
}
|
|
|
|
ID_INLINE idMat4 idMat4::operator-( const idMat4 &a ) const {
|
|
return idMat4(
|
|
mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z, mat[0].w - a[0].w,
|
|
mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z, mat[1].w - a[1].w,
|
|
mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z, mat[2].w - a[2].w,
|
|
mat[3].x - a[3].x, mat[3].y - a[3].y, mat[3].z - a[3].z, mat[3].w - a[3].w );
|
|
}
|
|
|
|
ID_INLINE idMat4 &idMat4::operator*=( const float a ) {
|
|
mat[0].x *= a; mat[0].y *= a; mat[0].z *= a; mat[0].w *= a;
|
|
mat[1].x *= a; mat[1].y *= a; mat[1].z *= a; mat[1].w *= a;
|
|
mat[2].x *= a; mat[2].y *= a; mat[2].z *= a; mat[2].w *= a;
|
|
mat[3].x *= a; mat[3].y *= a; mat[3].z *= a; mat[3].w *= a;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat4 &idMat4::operator*=( const idMat4 &a ) {
|
|
*this = (*this) * a;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat4 &idMat4::operator+=( const idMat4 &a ) {
|
|
mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z; mat[0].w += a[0].w;
|
|
mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z; mat[1].w += a[1].w;
|
|
mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z; mat[2].w += a[2].w;
|
|
mat[3].x += a[3].x; mat[3].y += a[3].y; mat[3].z += a[3].z; mat[3].w += a[3].w;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat4 &idMat4::operator-=( const idMat4 &a ) {
|
|
mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z; mat[0].w -= a[0].w;
|
|
mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z; mat[1].w -= a[1].w;
|
|
mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z; mat[2].w -= a[2].w;
|
|
mat[3].x -= a[3].x; mat[3].y -= a[3].y; mat[3].z -= a[3].z; mat[3].w -= a[3].w;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat4 operator*( const float a, const idMat4 &mat ) {
|
|
return mat * a;
|
|
}
|
|
|
|
ID_INLINE idVec4 operator*( const idVec4 &vec, const idMat4 &mat ) {
|
|
return mat * vec;
|
|
}
|
|
|
|
ID_INLINE idVec3 operator*( const idVec3 &vec, const idMat4 &mat ) {
|
|
return mat * vec;
|
|
}
|
|
|
|
ID_INLINE idVec4 &operator*=( idVec4 &vec, const idMat4 &mat ) {
|
|
vec = mat * vec;
|
|
return vec;
|
|
}
|
|
|
|
ID_INLINE idVec3 &operator*=( idVec3 &vec, const idMat4 &mat ) {
|
|
vec = mat * vec;
|
|
return vec;
|
|
}
|
|
|
|
ID_INLINE bool idMat4::Compare( const idMat4 &a ) const {
|
|
dword i;
|
|
const float *ptr1, *ptr2;
|
|
|
|
ptr1 = reinterpret_cast<const float *>(mat);
|
|
ptr2 = reinterpret_cast<const float *>(a.mat);
|
|
for ( i = 0; i < 4*4; i++ ) {
|
|
if ( ptr1[i] != ptr2[i] ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat4::Compare( const idMat4 &a, const float epsilon ) const {
|
|
dword i;
|
|
const float *ptr1, *ptr2;
|
|
|
|
ptr1 = reinterpret_cast<const float *>(mat);
|
|
ptr2 = reinterpret_cast<const float *>(a.mat);
|
|
for ( i = 0; i < 4*4; i++ ) {
|
|
if ( idMath::Fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat4::operator==( const idMat4 &a ) const {
|
|
return Compare( a );
|
|
}
|
|
|
|
ID_INLINE bool idMat4::operator!=( const idMat4 &a ) const {
|
|
return !Compare( a );
|
|
}
|
|
|
|
ID_INLINE void idMat4::Zero( void ) {
|
|
memset( mat, 0, sizeof( idMat4 ) );
|
|
}
|
|
|
|
ID_INLINE void idMat4::Identity( void ) {
|
|
*this = mat4_identity;
|
|
}
|
|
|
|
ID_INLINE bool idMat4::IsIdentity( const float epsilon ) const {
|
|
return Compare( mat4_identity, epsilon );
|
|
}
|
|
|
|
ID_INLINE bool idMat4::IsSymmetric( const float epsilon ) const {
|
|
for ( int i = 1; i < 4; i++ ) {
|
|
for ( int j = 0; j < i; j++ ) {
|
|
if ( idMath::Fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat4::IsDiagonal( const float epsilon ) const {
|
|
for ( int i = 0; i < 4; i++ ) {
|
|
for ( int j = 0; j < 4; j++ ) {
|
|
if ( i != j && idMath::Fabs( mat[i][j] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat4::IsRotated( void ) const {
|
|
if ( !mat[ 0 ][ 1 ] && !mat[ 0 ][ 2 ] &&
|
|
!mat[ 1 ][ 0 ] && !mat[ 1 ][ 2 ] &&
|
|
!mat[ 2 ][ 0 ] && !mat[ 2 ][ 1 ] ) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE void idMat4::ProjectVector( const idVec4 &src, idVec4 &dst ) const {
|
|
dst.x = src * mat[ 0 ];
|
|
dst.y = src * mat[ 1 ];
|
|
dst.z = src * mat[ 2 ];
|
|
dst.w = src * mat[ 3 ];
|
|
}
|
|
|
|
ID_INLINE void idMat4::UnprojectVector( const idVec4 &src, idVec4 &dst ) const {
|
|
dst = mat[ 0 ] * src.x + mat[ 1 ] * src.y + mat[ 2 ] * src.z + mat[ 3 ] * src.w;
|
|
}
|
|
|
|
ID_INLINE float idMat4::Trace( void ) const {
|
|
return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] );
|
|
}
|
|
|
|
ID_INLINE idMat4 idMat4::Inverse( void ) const {
|
|
idMat4 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE idMat4 idMat4::InverseFast( void ) const {
|
|
idMat4 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseFastSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE idMat4 idMat3::ToMat4( void ) const {
|
|
// NOTE: idMat3 is transposed because it is column-major
|
|
return idMat4( mat[0][0], mat[1][0], mat[2][0], 0.0f,
|
|
mat[0][1], mat[1][1], mat[2][1], 0.0f,
|
|
mat[0][2], mat[1][2], mat[2][2], 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f );
|
|
}
|
|
|
|
ID_INLINE int idMat4::GetDimension( void ) const {
|
|
return 16;
|
|
}
|
|
|
|
ID_INLINE const float *idMat4::ToFloatPtr( void ) const {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
ID_INLINE float *idMat4::ToFloatPtr( void ) {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat5 - 5x5 matrix
|
|
//
|
|
//===============================================================
|
|
|
|
class idMat5 {
|
|
public:
|
|
idMat5( void );
|
|
explicit idMat5( const idVec5 &v0, const idVec5 &v1, const idVec5 &v2, const idVec5 &v3, const idVec5 &v4 );
|
|
explicit idMat5( const float src[ 5 ][ 5 ] );
|
|
|
|
const idVec5 & operator[]( int index ) const;
|
|
idVec5 & operator[]( int index );
|
|
idMat5 operator*( const float a ) const;
|
|
idVec5 operator*( const idVec5 &vec ) const;
|
|
idMat5 operator*( const idMat5 &a ) const;
|
|
idMat5 operator+( const idMat5 &a ) const;
|
|
idMat5 operator-( const idMat5 &a ) const;
|
|
idMat5 & operator*=( const float a );
|
|
idMat5 & operator*=( const idMat5 &a );
|
|
idMat5 & operator+=( const idMat5 &a );
|
|
idMat5 & operator-=( const idMat5 &a );
|
|
|
|
friend idMat5 operator*( const float a, const idMat5 &mat );
|
|
friend idVec5 operator*( const idVec5 &vec, const idMat5 &mat );
|
|
friend idVec5 & operator*=( idVec5 &vec, const idMat5 &mat );
|
|
|
|
bool Compare( const idMat5 &a ) const; // exact compare, no epsilon
|
|
bool Compare( const idMat5 &a, const float epsilon ) const; // compare with epsilon
|
|
bool operator==( const idMat5 &a ) const; // exact compare, no epsilon
|
|
bool operator!=( const idMat5 &a ) const; // exact compare, no epsilon
|
|
|
|
void Zero( void );
|
|
void Identity( void );
|
|
bool IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
|
|
float Trace( void ) const;
|
|
float Determinant( void ) const;
|
|
idMat5 Transpose( void ) const; // returns transpose
|
|
idMat5 & TransposeSelf( void );
|
|
idMat5 Inverse( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseSelf( void ); // returns false if determinant is zero
|
|
idMat5 InverseFast( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseFastSelf( void ); // returns false if determinant is zero
|
|
|
|
int GetDimension( void ) const;
|
|
|
|
const float * ToFloatPtr( void ) const;
|
|
float * ToFloatPtr( void );
|
|
const char * ToString( int precision = 2 ) const;
|
|
|
|
private:
|
|
idVec5 mat[ 5 ];
|
|
};
|
|
|
|
extern idMat5 mat5_zero;
|
|
extern idMat5 mat5_identity;
|
|
#define mat5_default mat5_identity
|
|
|
|
ID_INLINE idMat5::idMat5( void ) {
|
|
}
|
|
|
|
ID_INLINE idMat5::idMat5( const float src[ 5 ][ 5 ] ) {
|
|
memcpy( mat, src, 5 * 5 * sizeof( float ) );
|
|
}
|
|
|
|
ID_INLINE idMat5::idMat5( const idVec5 &v0, const idVec5 &v1, const idVec5 &v2, const idVec5 &v3, const idVec5 &v4 ) {
|
|
mat[0] = v0;
|
|
mat[1] = v1;
|
|
mat[2] = v2;
|
|
mat[3] = v3;
|
|
mat[4] = v4;
|
|
}
|
|
|
|
ID_INLINE const idVec5 &idMat5::operator[]( int index ) const {
|
|
//assert( ( index >= 0 ) && ( index < 5 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idVec5 &idMat5::operator[]( int index ) {
|
|
//assert( ( index >= 0 ) && ( index < 5 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idMat5 idMat5::operator*( const idMat5 &a ) const {
|
|
int i, j;
|
|
const float *m1Ptr, *m2Ptr;
|
|
float *dstPtr;
|
|
idMat5 dst;
|
|
|
|
m1Ptr = reinterpret_cast<const float *>(this);
|
|
m2Ptr = reinterpret_cast<const float *>(&a);
|
|
dstPtr = reinterpret_cast<float *>(&dst);
|
|
|
|
for ( i = 0; i < 5; i++ ) {
|
|
for ( j = 0; j < 5; j++ ) {
|
|
*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 5 + j ]
|
|
+ m1Ptr[1] * m2Ptr[ 1 * 5 + j ]
|
|
+ m1Ptr[2] * m2Ptr[ 2 * 5 + j ]
|
|
+ m1Ptr[3] * m2Ptr[ 3 * 5 + j ]
|
|
+ m1Ptr[4] * m2Ptr[ 4 * 5 + j ];
|
|
dstPtr++;
|
|
}
|
|
m1Ptr += 5;
|
|
}
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE idMat5 idMat5::operator*( const float a ) const {
|
|
return idMat5(
|
|
idVec5( mat[0][0] * a, mat[0][1] * a, mat[0][2] * a, mat[0][3] * a, mat[0][4] * a ),
|
|
idVec5( mat[1][0] * a, mat[1][1] * a, mat[1][2] * a, mat[1][3] * a, mat[1][4] * a ),
|
|
idVec5( mat[2][0] * a, mat[2][1] * a, mat[2][2] * a, mat[2][3] * a, mat[2][4] * a ),
|
|
idVec5( mat[3][0] * a, mat[3][1] * a, mat[3][2] * a, mat[3][3] * a, mat[3][4] * a ),
|
|
idVec5( mat[4][0] * a, mat[4][1] * a, mat[4][2] * a, mat[4][3] * a, mat[4][4] * a ) );
|
|
}
|
|
|
|
ID_INLINE idVec5 idMat5::operator*( const idVec5 &vec ) const {
|
|
return idVec5(
|
|
mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2] + mat[0][3] * vec[3] + mat[0][4] * vec[4],
|
|
mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2] + mat[1][3] * vec[3] + mat[1][4] * vec[4],
|
|
mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2] + mat[2][3] * vec[3] + mat[2][4] * vec[4],
|
|
mat[3][0] * vec[0] + mat[3][1] * vec[1] + mat[3][2] * vec[2] + mat[3][3] * vec[3] + mat[3][4] * vec[4],
|
|
mat[4][0] * vec[0] + mat[4][1] * vec[1] + mat[4][2] * vec[2] + mat[4][3] * vec[3] + mat[4][4] * vec[4] );
|
|
}
|
|
|
|
ID_INLINE idMat5 idMat5::operator+( const idMat5 &a ) const {
|
|
return idMat5(
|
|
idVec5( mat[0][0] + a[0][0], mat[0][1] + a[0][1], mat[0][2] + a[0][2], mat[0][3] + a[0][3], mat[0][4] + a[0][4] ),
|
|
idVec5( mat[1][0] + a[1][0], mat[1][1] + a[1][1], mat[1][2] + a[1][2], mat[1][3] + a[1][3], mat[1][4] + a[1][4] ),
|
|
idVec5( mat[2][0] + a[2][0], mat[2][1] + a[2][1], mat[2][2] + a[2][2], mat[2][3] + a[2][3], mat[2][4] + a[2][4] ),
|
|
idVec5( mat[3][0] + a[3][0], mat[3][1] + a[3][1], mat[3][2] + a[3][2], mat[3][3] + a[3][3], mat[3][4] + a[3][4] ),
|
|
idVec5( mat[4][0] + a[4][0], mat[4][1] + a[4][1], mat[4][2] + a[4][2], mat[4][3] + a[4][3], mat[4][4] + a[4][4] ) );
|
|
}
|
|
|
|
ID_INLINE idMat5 idMat5::operator-( const idMat5 &a ) const {
|
|
return idMat5(
|
|
idVec5( mat[0][0] - a[0][0], mat[0][1] - a[0][1], mat[0][2] - a[0][2], mat[0][3] - a[0][3], mat[0][4] - a[0][4] ),
|
|
idVec5( mat[1][0] - a[1][0], mat[1][1] - a[1][1], mat[1][2] - a[1][2], mat[1][3] - a[1][3], mat[1][4] - a[1][4] ),
|
|
idVec5( mat[2][0] - a[2][0], mat[2][1] - a[2][1], mat[2][2] - a[2][2], mat[2][3] - a[2][3], mat[2][4] - a[2][4] ),
|
|
idVec5( mat[3][0] - a[3][0], mat[3][1] - a[3][1], mat[3][2] - a[3][2], mat[3][3] - a[3][3], mat[3][4] - a[3][4] ),
|
|
idVec5( mat[4][0] - a[4][0], mat[4][1] - a[4][1], mat[4][2] - a[4][2], mat[4][3] - a[4][3], mat[4][4] - a[4][4] ) );
|
|
}
|
|
|
|
ID_INLINE idMat5 &idMat5::operator*=( const float a ) {
|
|
mat[0][0] *= a; mat[0][1] *= a; mat[0][2] *= a; mat[0][3] *= a; mat[0][4] *= a;
|
|
mat[1][0] *= a; mat[1][1] *= a; mat[1][2] *= a; mat[1][3] *= a; mat[1][4] *= a;
|
|
mat[2][0] *= a; mat[2][1] *= a; mat[2][2] *= a; mat[2][3] *= a; mat[2][4] *= a;
|
|
mat[3][0] *= a; mat[3][1] *= a; mat[3][2] *= a; mat[3][3] *= a; mat[3][4] *= a;
|
|
mat[4][0] *= a; mat[4][1] *= a; mat[4][2] *= a; mat[4][3] *= a; mat[4][4] *= a;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat5 &idMat5::operator*=( const idMat5 &a ) {
|
|
*this = *this * a;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat5 &idMat5::operator+=( const idMat5 &a ) {
|
|
mat[0][0] += a[0][0]; mat[0][1] += a[0][1]; mat[0][2] += a[0][2]; mat[0][3] += a[0][3]; mat[0][4] += a[0][4];
|
|
mat[1][0] += a[1][0]; mat[1][1] += a[1][1]; mat[1][2] += a[1][2]; mat[1][3] += a[1][3]; mat[1][4] += a[1][4];
|
|
mat[2][0] += a[2][0]; mat[2][1] += a[2][1]; mat[2][2] += a[2][2]; mat[2][3] += a[2][3]; mat[2][4] += a[2][4];
|
|
mat[3][0] += a[3][0]; mat[3][1] += a[3][1]; mat[3][2] += a[3][2]; mat[3][3] += a[3][3]; mat[3][4] += a[3][4];
|
|
mat[4][0] += a[4][0]; mat[4][1] += a[4][1]; mat[4][2] += a[4][2]; mat[4][3] += a[4][3]; mat[4][4] += a[4][4];
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat5 &idMat5::operator-=( const idMat5 &a ) {
|
|
mat[0][0] -= a[0][0]; mat[0][1] -= a[0][1]; mat[0][2] -= a[0][2]; mat[0][3] -= a[0][3]; mat[0][4] -= a[0][4];
|
|
mat[1][0] -= a[1][0]; mat[1][1] -= a[1][1]; mat[1][2] -= a[1][2]; mat[1][3] -= a[1][3]; mat[1][4] -= a[1][4];
|
|
mat[2][0] -= a[2][0]; mat[2][1] -= a[2][1]; mat[2][2] -= a[2][2]; mat[2][3] -= a[2][3]; mat[2][4] -= a[2][4];
|
|
mat[3][0] -= a[3][0]; mat[3][1] -= a[3][1]; mat[3][2] -= a[3][2]; mat[3][3] -= a[3][3]; mat[3][4] -= a[3][4];
|
|
mat[4][0] -= a[4][0]; mat[4][1] -= a[4][1]; mat[4][2] -= a[4][2]; mat[4][3] -= a[4][3]; mat[4][4] -= a[4][4];
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idVec5 operator*( const idVec5 &vec, const idMat5 &mat ) {
|
|
return mat * vec;
|
|
}
|
|
|
|
ID_INLINE idMat5 operator*( const float a, idMat5 const &mat ) {
|
|
return mat * a;
|
|
}
|
|
|
|
ID_INLINE idVec5 &operator*=( idVec5 &vec, const idMat5 &mat ) {
|
|
vec = mat * vec;
|
|
return vec;
|
|
}
|
|
|
|
ID_INLINE bool idMat5::Compare( const idMat5 &a ) const {
|
|
dword i;
|
|
const float *ptr1, *ptr2;
|
|
|
|
ptr1 = reinterpret_cast<const float *>(mat);
|
|
ptr2 = reinterpret_cast<const float *>(a.mat);
|
|
for ( i = 0; i < 5*5; i++ ) {
|
|
if ( ptr1[i] != ptr2[i] ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat5::Compare( const idMat5 &a, const float epsilon ) const {
|
|
dword i;
|
|
const float *ptr1, *ptr2;
|
|
|
|
ptr1 = reinterpret_cast<const float *>(mat);
|
|
ptr2 = reinterpret_cast<const float *>(a.mat);
|
|
for ( i = 0; i < 5*5; i++ ) {
|
|
if ( idMath::Fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat5::operator==( const idMat5 &a ) const {
|
|
return Compare( a );
|
|
}
|
|
|
|
ID_INLINE bool idMat5::operator!=( const idMat5 &a ) const {
|
|
return !Compare( a );
|
|
}
|
|
|
|
ID_INLINE void idMat5::Zero( void ) {
|
|
memset( mat, 0, sizeof( idMat5 ) );
|
|
}
|
|
|
|
ID_INLINE void idMat5::Identity( void ) {
|
|
*this = mat5_identity;
|
|
}
|
|
|
|
ID_INLINE bool idMat5::IsIdentity( const float epsilon ) const {
|
|
return Compare( mat5_identity, epsilon );
|
|
}
|
|
|
|
ID_INLINE bool idMat5::IsSymmetric( const float epsilon ) const {
|
|
for ( int i = 1; i < 5; i++ ) {
|
|
for ( int j = 0; j < i; j++ ) {
|
|
if ( idMath::Fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat5::IsDiagonal( const float epsilon ) const {
|
|
for ( int i = 0; i < 5; i++ ) {
|
|
for ( int j = 0; j < 5; j++ ) {
|
|
if ( i != j && idMath::Fabs( mat[i][j] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE float idMat5::Trace( void ) const {
|
|
return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] + mat[4][4] );
|
|
}
|
|
|
|
ID_INLINE idMat5 idMat5::Inverse( void ) const {
|
|
idMat5 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE idMat5 idMat5::InverseFast( void ) const {
|
|
idMat5 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseFastSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE int idMat5::GetDimension( void ) const {
|
|
return 25;
|
|
}
|
|
|
|
ID_INLINE const float *idMat5::ToFloatPtr( void ) const {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
ID_INLINE float *idMat5::ToFloatPtr( void ) {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat6 - 6x6 matrix
|
|
//
|
|
//===============================================================
|
|
|
|
class idMat6 {
|
|
public:
|
|
idMat6( void );
|
|
explicit idMat6( const idVec6 &v0, const idVec6 &v1, const idVec6 &v2, const idVec6 &v3, const idVec6 &v4, const idVec6 &v5 );
|
|
explicit idMat6( const idMat3 &m0, const idMat3 &m1, const idMat3 &m2, const idMat3 &m3 );
|
|
explicit idMat6( const float src[ 6 ][ 6 ] );
|
|
|
|
const idVec6 & operator[]( int index ) const;
|
|
idVec6 & operator[]( int index );
|
|
idMat6 operator*( const float a ) const;
|
|
idVec6 operator*( const idVec6 &vec ) const;
|
|
idMat6 operator*( const idMat6 &a ) const;
|
|
idMat6 operator+( const idMat6 &a ) const;
|
|
idMat6 operator-( const idMat6 &a ) const;
|
|
idMat6 & operator*=( const float a );
|
|
idMat6 & operator*=( const idMat6 &a );
|
|
idMat6 & operator+=( const idMat6 &a );
|
|
idMat6 & operator-=( const idMat6 &a );
|
|
|
|
friend idMat6 operator*( const float a, const idMat6 &mat );
|
|
friend idVec6 operator*( const idVec6 &vec, const idMat6 &mat );
|
|
friend idVec6 & operator*=( idVec6 &vec, const idMat6 &mat );
|
|
|
|
bool Compare( const idMat6 &a ) const; // exact compare, no epsilon
|
|
bool Compare( const idMat6 &a, const float epsilon ) const; // compare with epsilon
|
|
bool operator==( const idMat6 &a ) const; // exact compare, no epsilon
|
|
bool operator!=( const idMat6 &a ) const; // exact compare, no epsilon
|
|
|
|
void Zero( void );
|
|
void Identity( void );
|
|
bool IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
|
|
idMat3 SubMat3( int n ) const;
|
|
float Trace( void ) const;
|
|
float Determinant( void ) const;
|
|
idMat6 Transpose( void ) const; // returns transpose
|
|
idMat6 & TransposeSelf( void );
|
|
idMat6 Inverse( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseSelf( void ); // returns false if determinant is zero
|
|
idMat6 InverseFast( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseFastSelf( void ); // returns false if determinant is zero
|
|
|
|
int GetDimension( void ) const;
|
|
|
|
const float * ToFloatPtr( void ) const;
|
|
float * ToFloatPtr( void );
|
|
const char * ToString( int precision = 2 ) const;
|
|
|
|
private:
|
|
idVec6 mat[ 6 ];
|
|
};
|
|
|
|
extern idMat6 mat6_zero;
|
|
extern idMat6 mat6_identity;
|
|
#define mat6_default mat6_identity
|
|
|
|
ID_INLINE idMat6::idMat6( void ) {
|
|
}
|
|
|
|
ID_INLINE idMat6::idMat6( const idMat3 &m0, const idMat3 &m1, const idMat3 &m2, const idMat3 &m3 ) {
|
|
mat[0] = idVec6( m0[0][0], m0[0][1], m0[0][2], m1[0][0], m1[0][1], m1[0][2] );
|
|
mat[1] = idVec6( m0[1][0], m0[1][1], m0[1][2], m1[1][0], m1[1][1], m1[1][2] );
|
|
mat[2] = idVec6( m0[2][0], m0[2][1], m0[2][2], m1[2][0], m1[2][1], m1[2][2] );
|
|
mat[3] = idVec6( m2[0][0], m2[0][1], m2[0][2], m3[0][0], m3[0][1], m3[0][2] );
|
|
mat[4] = idVec6( m2[1][0], m2[1][1], m2[1][2], m3[1][0], m3[1][1], m3[1][2] );
|
|
mat[5] = idVec6( m2[2][0], m2[2][1], m2[2][2], m3[2][0], m3[2][1], m3[2][2] );
|
|
}
|
|
|
|
ID_INLINE idMat6::idMat6( const idVec6 &v0, const idVec6 &v1, const idVec6 &v2, const idVec6 &v3, const idVec6 &v4, const idVec6 &v5 ) {
|
|
mat[0] = v0;
|
|
mat[1] = v1;
|
|
mat[2] = v2;
|
|
mat[3] = v3;
|
|
mat[4] = v4;
|
|
mat[5] = v5;
|
|
}
|
|
|
|
ID_INLINE idMat6::idMat6( const float src[ 6 ][ 6 ] ) {
|
|
memcpy( mat, src, 6 * 6 * sizeof( float ) );
|
|
}
|
|
|
|
ID_INLINE const idVec6 &idMat6::operator[]( int index ) const {
|
|
//assert( ( index >= 0 ) && ( index < 6 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idVec6 &idMat6::operator[]( int index ) {
|
|
//assert( ( index >= 0 ) && ( index < 6 ) );
|
|
return mat[ index ];
|
|
}
|
|
|
|
ID_INLINE idMat6 idMat6::operator*( const idMat6 &a ) const {
|
|
int i, j;
|
|
const float *m1Ptr, *m2Ptr;
|
|
float *dstPtr;
|
|
idMat6 dst;
|
|
|
|
m1Ptr = reinterpret_cast<const float *>(this);
|
|
m2Ptr = reinterpret_cast<const float *>(&a);
|
|
dstPtr = reinterpret_cast<float *>(&dst);
|
|
|
|
for ( i = 0; i < 6; i++ ) {
|
|
for ( j = 0; j < 6; j++ ) {
|
|
*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 6 + j ]
|
|
+ m1Ptr[1] * m2Ptr[ 1 * 6 + j ]
|
|
+ m1Ptr[2] * m2Ptr[ 2 * 6 + j ]
|
|
+ m1Ptr[3] * m2Ptr[ 3 * 6 + j ]
|
|
+ m1Ptr[4] * m2Ptr[ 4 * 6 + j ]
|
|
+ m1Ptr[5] * m2Ptr[ 5 * 6 + j ];
|
|
dstPtr++;
|
|
}
|
|
m1Ptr += 6;
|
|
}
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE idMat6 idMat6::operator*( const float a ) const {
|
|
return idMat6(
|
|
idVec6( mat[0][0] * a, mat[0][1] * a, mat[0][2] * a, mat[0][3] * a, mat[0][4] * a, mat[0][5] * a ),
|
|
idVec6( mat[1][0] * a, mat[1][1] * a, mat[1][2] * a, mat[1][3] * a, mat[1][4] * a, mat[1][5] * a ),
|
|
idVec6( mat[2][0] * a, mat[2][1] * a, mat[2][2] * a, mat[2][3] * a, mat[2][4] * a, mat[2][5] * a ),
|
|
idVec6( mat[3][0] * a, mat[3][1] * a, mat[3][2] * a, mat[3][3] * a, mat[3][4] * a, mat[3][5] * a ),
|
|
idVec6( mat[4][0] * a, mat[4][1] * a, mat[4][2] * a, mat[4][3] * a, mat[4][4] * a, mat[4][5] * a ),
|
|
idVec6( mat[5][0] * a, mat[5][1] * a, mat[5][2] * a, mat[5][3] * a, mat[5][4] * a, mat[5][5] * a ) );
|
|
}
|
|
|
|
ID_INLINE idVec6 idMat6::operator*( const idVec6 &vec ) const {
|
|
return idVec6(
|
|
mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2] + mat[0][3] * vec[3] + mat[0][4] * vec[4] + mat[0][5] * vec[5],
|
|
mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2] + mat[1][3] * vec[3] + mat[1][4] * vec[4] + mat[1][5] * vec[5],
|
|
mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2] + mat[2][3] * vec[3] + mat[2][4] * vec[4] + mat[2][5] * vec[5],
|
|
mat[3][0] * vec[0] + mat[3][1] * vec[1] + mat[3][2] * vec[2] + mat[3][3] * vec[3] + mat[3][4] * vec[4] + mat[3][5] * vec[5],
|
|
mat[4][0] * vec[0] + mat[4][1] * vec[1] + mat[4][2] * vec[2] + mat[4][3] * vec[3] + mat[4][4] * vec[4] + mat[4][5] * vec[5],
|
|
mat[5][0] * vec[0] + mat[5][1] * vec[1] + mat[5][2] * vec[2] + mat[5][3] * vec[3] + mat[5][4] * vec[4] + mat[5][5] * vec[5] );
|
|
}
|
|
|
|
ID_INLINE idMat6 idMat6::operator+( const idMat6 &a ) const {
|
|
return idMat6(
|
|
idVec6( mat[0][0] + a[0][0], mat[0][1] + a[0][1], mat[0][2] + a[0][2], mat[0][3] + a[0][3], mat[0][4] + a[0][4], mat[0][5] + a[0][5] ),
|
|
idVec6( mat[1][0] + a[1][0], mat[1][1] + a[1][1], mat[1][2] + a[1][2], mat[1][3] + a[1][3], mat[1][4] + a[1][4], mat[1][5] + a[1][5] ),
|
|
idVec6( mat[2][0] + a[2][0], mat[2][1] + a[2][1], mat[2][2] + a[2][2], mat[2][3] + a[2][3], mat[2][4] + a[2][4], mat[2][5] + a[2][5] ),
|
|
idVec6( mat[3][0] + a[3][0], mat[3][1] + a[3][1], mat[3][2] + a[3][2], mat[3][3] + a[3][3], mat[3][4] + a[3][4], mat[3][5] + a[3][5] ),
|
|
idVec6( mat[4][0] + a[4][0], mat[4][1] + a[4][1], mat[4][2] + a[4][2], mat[4][3] + a[4][3], mat[4][4] + a[4][4], mat[4][5] + a[4][5] ),
|
|
idVec6( mat[5][0] + a[5][0], mat[5][1] + a[5][1], mat[5][2] + a[5][2], mat[5][3] + a[5][3], mat[5][4] + a[5][4], mat[5][5] + a[5][5] ) );
|
|
}
|
|
|
|
ID_INLINE idMat6 idMat6::operator-( const idMat6 &a ) const {
|
|
return idMat6(
|
|
idVec6( mat[0][0] - a[0][0], mat[0][1] - a[0][1], mat[0][2] - a[0][2], mat[0][3] - a[0][3], mat[0][4] - a[0][4], mat[0][5] - a[0][5] ),
|
|
idVec6( mat[1][0] - a[1][0], mat[1][1] - a[1][1], mat[1][2] - a[1][2], mat[1][3] - a[1][3], mat[1][4] - a[1][4], mat[1][5] - a[1][5] ),
|
|
idVec6( mat[2][0] - a[2][0], mat[2][1] - a[2][1], mat[2][2] - a[2][2], mat[2][3] - a[2][3], mat[2][4] - a[2][4], mat[2][5] - a[2][5] ),
|
|
idVec6( mat[3][0] - a[3][0], mat[3][1] - a[3][1], mat[3][2] - a[3][2], mat[3][3] - a[3][3], mat[3][4] - a[3][4], mat[3][5] - a[3][5] ),
|
|
idVec6( mat[4][0] - a[4][0], mat[4][1] - a[4][1], mat[4][2] - a[4][2], mat[4][3] - a[4][3], mat[4][4] - a[4][4], mat[4][5] - a[4][5] ),
|
|
idVec6( mat[5][0] - a[5][0], mat[5][1] - a[5][1], mat[5][2] - a[5][2], mat[5][3] - a[5][3], mat[5][4] - a[5][4], mat[5][5] - a[5][5] ) );
|
|
}
|
|
|
|
ID_INLINE idMat6 &idMat6::operator*=( const float a ) {
|
|
mat[0][0] *= a; mat[0][1] *= a; mat[0][2] *= a; mat[0][3] *= a; mat[0][4] *= a; mat[0][5] *= a;
|
|
mat[1][0] *= a; mat[1][1] *= a; mat[1][2] *= a; mat[1][3] *= a; mat[1][4] *= a; mat[1][5] *= a;
|
|
mat[2][0] *= a; mat[2][1] *= a; mat[2][2] *= a; mat[2][3] *= a; mat[2][4] *= a; mat[2][5] *= a;
|
|
mat[3][0] *= a; mat[3][1] *= a; mat[3][2] *= a; mat[3][3] *= a; mat[3][4] *= a; mat[3][5] *= a;
|
|
mat[4][0] *= a; mat[4][1] *= a; mat[4][2] *= a; mat[4][3] *= a; mat[4][4] *= a; mat[4][5] *= a;
|
|
mat[5][0] *= a; mat[5][1] *= a; mat[5][2] *= a; mat[5][3] *= a; mat[5][4] *= a; mat[5][5] *= a;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat6 &idMat6::operator*=( const idMat6 &a ) {
|
|
*this = *this * a;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat6 &idMat6::operator+=( const idMat6 &a ) {
|
|
mat[0][0] += a[0][0]; mat[0][1] += a[0][1]; mat[0][2] += a[0][2]; mat[0][3] += a[0][3]; mat[0][4] += a[0][4]; mat[0][5] += a[0][5];
|
|
mat[1][0] += a[1][0]; mat[1][1] += a[1][1]; mat[1][2] += a[1][2]; mat[1][3] += a[1][3]; mat[1][4] += a[1][4]; mat[1][5] += a[1][5];
|
|
mat[2][0] += a[2][0]; mat[2][1] += a[2][1]; mat[2][2] += a[2][2]; mat[2][3] += a[2][3]; mat[2][4] += a[2][4]; mat[2][5] += a[2][5];
|
|
mat[3][0] += a[3][0]; mat[3][1] += a[3][1]; mat[3][2] += a[3][2]; mat[3][3] += a[3][3]; mat[3][4] += a[3][4]; mat[3][5] += a[3][5];
|
|
mat[4][0] += a[4][0]; mat[4][1] += a[4][1]; mat[4][2] += a[4][2]; mat[4][3] += a[4][3]; mat[4][4] += a[4][4]; mat[4][5] += a[4][5];
|
|
mat[5][0] += a[5][0]; mat[5][1] += a[5][1]; mat[5][2] += a[5][2]; mat[5][3] += a[5][3]; mat[5][4] += a[5][4]; mat[5][5] += a[5][5];
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMat6 &idMat6::operator-=( const idMat6 &a ) {
|
|
mat[0][0] -= a[0][0]; mat[0][1] -= a[0][1]; mat[0][2] -= a[0][2]; mat[0][3] -= a[0][3]; mat[0][4] -= a[0][4]; mat[0][5] -= a[0][5];
|
|
mat[1][0] -= a[1][0]; mat[1][1] -= a[1][1]; mat[1][2] -= a[1][2]; mat[1][3] -= a[1][3]; mat[1][4] -= a[1][4]; mat[1][5] -= a[1][5];
|
|
mat[2][0] -= a[2][0]; mat[2][1] -= a[2][1]; mat[2][2] -= a[2][2]; mat[2][3] -= a[2][3]; mat[2][4] -= a[2][4]; mat[2][5] -= a[2][5];
|
|
mat[3][0] -= a[3][0]; mat[3][1] -= a[3][1]; mat[3][2] -= a[3][2]; mat[3][3] -= a[3][3]; mat[3][4] -= a[3][4]; mat[3][5] -= a[3][5];
|
|
mat[4][0] -= a[4][0]; mat[4][1] -= a[4][1]; mat[4][2] -= a[4][2]; mat[4][3] -= a[4][3]; mat[4][4] -= a[4][4]; mat[4][5] -= a[4][5];
|
|
mat[5][0] -= a[5][0]; mat[5][1] -= a[5][1]; mat[5][2] -= a[5][2]; mat[5][3] -= a[5][3]; mat[5][4] -= a[5][4]; mat[5][5] -= a[5][5];
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idVec6 operator*( const idVec6 &vec, const idMat6 &mat ) {
|
|
return mat * vec;
|
|
}
|
|
|
|
ID_INLINE idMat6 operator*( const float a, idMat6 const &mat ) {
|
|
return mat * a;
|
|
}
|
|
|
|
ID_INLINE idVec6 &operator*=( idVec6 &vec, const idMat6 &mat ) {
|
|
vec = mat * vec;
|
|
return vec;
|
|
}
|
|
|
|
ID_INLINE bool idMat6::Compare( const idMat6 &a ) const {
|
|
dword i;
|
|
const float *ptr1, *ptr2;
|
|
|
|
ptr1 = reinterpret_cast<const float *>(mat);
|
|
ptr2 = reinterpret_cast<const float *>(a.mat);
|
|
for ( i = 0; i < 6*6; i++ ) {
|
|
if ( ptr1[i] != ptr2[i] ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat6::Compare( const idMat6 &a, const float epsilon ) const {
|
|
dword i;
|
|
const float *ptr1, *ptr2;
|
|
|
|
ptr1 = reinterpret_cast<const float *>(mat);
|
|
ptr2 = reinterpret_cast<const float *>(a.mat);
|
|
for ( i = 0; i < 6*6; i++ ) {
|
|
if ( idMath::Fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat6::operator==( const idMat6 &a ) const {
|
|
return Compare( a );
|
|
}
|
|
|
|
ID_INLINE bool idMat6::operator!=( const idMat6 &a ) const {
|
|
return !Compare( a );
|
|
}
|
|
|
|
ID_INLINE void idMat6::Zero( void ) {
|
|
memset( mat, 0, sizeof( idMat6 ) );
|
|
}
|
|
|
|
ID_INLINE void idMat6::Identity( void ) {
|
|
*this = mat6_identity;
|
|
}
|
|
|
|
ID_INLINE bool idMat6::IsIdentity( const float epsilon ) const {
|
|
return Compare( mat6_identity, epsilon );
|
|
}
|
|
|
|
ID_INLINE bool idMat6::IsSymmetric( const float epsilon ) const {
|
|
for ( int i = 1; i < 6; i++ ) {
|
|
for ( int j = 0; j < i; j++ ) {
|
|
if ( idMath::Fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMat6::IsDiagonal( const float epsilon ) const {
|
|
for ( int i = 0; i < 6; i++ ) {
|
|
for ( int j = 0; j < 6; j++ ) {
|
|
if ( i != j && idMath::Fabs( mat[i][j] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE idMat3 idMat6::SubMat3( int n ) const {
|
|
assert( n >= 0 && n < 4 );
|
|
int b0 = ((n & 2) >> 1) * 3;
|
|
int b1 = (n & 1) * 3;
|
|
return idMat3(
|
|
mat[b0 + 0][b1 + 0], mat[b0 + 0][b1 + 1], mat[b0 + 0][b1 + 2],
|
|
mat[b0 + 1][b1 + 0], mat[b0 + 1][b1 + 1], mat[b0 + 1][b1 + 2],
|
|
mat[b0 + 2][b1 + 0], mat[b0 + 2][b1 + 1], mat[b0 + 2][b1 + 2] );
|
|
}
|
|
|
|
ID_INLINE float idMat6::Trace( void ) const {
|
|
return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] + mat[4][4] + mat[5][5] );
|
|
}
|
|
|
|
ID_INLINE idMat6 idMat6::Inverse( void ) const {
|
|
idMat6 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE idMat6 idMat6::InverseFast( void ) const {
|
|
idMat6 invMat;
|
|
|
|
invMat = *this;
|
|
int r id_attribute((unused)) = invMat.InverseFastSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE int idMat6::GetDimension( void ) const {
|
|
return 36;
|
|
}
|
|
|
|
ID_INLINE const float *idMat6::ToFloatPtr( void ) const {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
ID_INLINE float *idMat6::ToFloatPtr( void ) {
|
|
return mat[0].ToFloatPtr();
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMatX - arbitrary sized dense real matrix
|
|
//
|
|
// The matrix lives on 16 byte aligned and 16 byte padded memory.
|
|
//
|
|
// NOTE: due to the temporary memory pool idMatX cannot be used by multiple threads.
|
|
//
|
|
//===============================================================
|
|
|
|
#define MATX_MAX_TEMP 1024
|
|
#define MATX_QUAD( x ) ( ( ( ( x ) + 3 ) & ~3 ) * sizeof( float ) )
|
|
#define MATX_CLEAREND() int s = numRows * numColumns; while( s < ( ( s + 3 ) & ~3 ) ) { mat[s++] = 0.0f; }
|
|
#define MATX_ALLOCA( n ) ( (float *) _alloca16( MATX_QUAD( n ) ) )
|
|
#define MATX_SIMD
|
|
|
|
class idMatX {
|
|
public:
|
|
idMatX( void );
|
|
explicit idMatX( int rows, int columns );
|
|
explicit idMatX( int rows, int columns, float *src );
|
|
~idMatX( void );
|
|
|
|
void Set( int rows, int columns, const float *src );
|
|
void Set( const idMat3 &m1, const idMat3 &m2 );
|
|
void Set( const idMat3 &m1, const idMat3 &m2, const idMat3 &m3, const idMat3 &m4 );
|
|
|
|
const float * operator[]( int index ) const;
|
|
float * operator[]( int index );
|
|
idMatX & operator=( const idMatX &a );
|
|
idMatX operator*( const float a ) const;
|
|
idVecX operator*( const idVecX &vec ) const;
|
|
idMatX operator*( const idMatX &a ) const;
|
|
idMatX operator+( const idMatX &a ) const;
|
|
idMatX operator-( const idMatX &a ) const;
|
|
idMatX & operator*=( const float a );
|
|
idMatX & operator*=( const idMatX &a );
|
|
idMatX & operator+=( const idMatX &a );
|
|
idMatX & operator-=( const idMatX &a );
|
|
|
|
friend idMatX operator*( const float a, const idMatX &m );
|
|
friend idVecX operator*( const idVecX &vec, const idMatX &m );
|
|
friend idVecX & operator*=( idVecX &vec, const idMatX &m );
|
|
|
|
bool Compare( const idMatX &a ) const; // exact compare, no epsilon
|
|
bool Compare( const idMatX &a, const float epsilon ) const; // compare with epsilon
|
|
bool operator==( const idMatX &a ) const; // exact compare, no epsilon
|
|
bool operator!=( const idMatX &a ) const; // exact compare, no epsilon
|
|
|
|
void SetSize( int rows, int columns ); // set the number of rows/columns
|
|
void ChangeSize( int rows, int columns, bool makeZero = false ); // change the size keeping data intact where possible
|
|
int GetNumRows( void ) const { return numRows; } // get the number of rows
|
|
int GetNumColumns( void ) const { return numColumns; } // get the number of columns
|
|
void SetData( int rows, int columns, float *data ); // set float array pointer
|
|
void Zero( void ); // clear matrix
|
|
void Zero( int rows, int columns ); // set size and clear matrix
|
|
void Identity( void ); // clear to identity matrix
|
|
void Identity( int rows, int columns ); // set size and clear to identity matrix
|
|
void Diag( const idVecX &v ); // create diagonal matrix from vector
|
|
void Random( int seed, float l = 0.0f, float u = 1.0f ); // fill matrix with random values
|
|
void Random( int rows, int columns, int seed, float l = 0.0f, float u = 1.0f );
|
|
void Negate( void ); // (*this) = - (*this)
|
|
void Clamp( float min, float max ); // clamp all values
|
|
idMatX & SwapRows( int r1, int r2 ); // swap rows
|
|
idMatX & SwapColumns( int r1, int r2 ); // swap columns
|
|
idMatX & SwapRowsColumns( int r1, int r2 ); // swap rows and columns
|
|
idMatX & RemoveRow( int r ); // remove a row
|
|
idMatX & RemoveColumn( int r ); // remove a column
|
|
idMatX & RemoveRowColumn( int r ); // remove a row and column
|
|
void ClearUpperTriangle( void ); // clear the upper triangle
|
|
void ClearLowerTriangle( void ); // clear the lower triangle
|
|
void SquareSubMatrix( const idMatX &m, int size ); // get square sub-matrix from 0,0 to size,size
|
|
float MaxDifference( const idMatX &m ) const; // return maximum element difference between this and m
|
|
|
|
bool IsSquare( void ) const { return ( numRows == numColumns ); }
|
|
bool IsZero( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsIdentity( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsDiagonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsTriDiagonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetric( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsOrthogonal( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsOrthonormal( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsPMatrix( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsZMatrix( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsPositiveDefinite( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetricPositiveDefinite( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsPositiveSemiDefinite( const float epsilon = MATRIX_EPSILON ) const;
|
|
bool IsSymmetricPositiveSemiDefinite( const float epsilon = MATRIX_EPSILON ) const;
|
|
|
|
float Trace( void ) const; // returns product of diagonal elements
|
|
float Determinant( void ) const; // returns determinant of matrix
|
|
idMatX Transpose( void ) const; // returns transpose
|
|
idMatX & TransposeSelf( void ); // transposes the matrix itself
|
|
idMatX Inverse( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseSelf( void ); // returns false if determinant is zero
|
|
idMatX InverseFast( void ) const; // returns the inverse ( m * m.Inverse() = identity )
|
|
bool InverseFastSelf( void ); // returns false if determinant is zero
|
|
|
|
bool LowerTriangularInverse( void ); // in-place inversion, returns false if determinant is zero
|
|
bool UpperTriangularInverse( void ); // in-place inversion, returns false if determinant is zero
|
|
|
|
idVecX Multiply( const idVecX &vec ) const; // (*this) * vec
|
|
idVecX TransposeMultiply( const idVecX &vec ) const; // this->Transpose() * vec
|
|
|
|
idMatX Multiply( const idMatX &a ) const; // (*this) * a
|
|
idMatX TransposeMultiply( const idMatX &a ) const; // this->Transpose() * a
|
|
|
|
void Multiply( idVecX &dst, const idVecX &vec ) const; // dst = (*this) * vec
|
|
void MultiplyAdd( idVecX &dst, const idVecX &vec ) const; // dst += (*this) * vec
|
|
void MultiplySub( idVecX &dst, const idVecX &vec ) const; // dst -= (*this) * vec
|
|
void TransposeMultiply( idVecX &dst, const idVecX &vec ) const; // dst = this->Transpose() * vec
|
|
void TransposeMultiplyAdd( idVecX &dst, const idVecX &vec ) const; // dst += this->Transpose() * vec
|
|
void TransposeMultiplySub( idVecX &dst, const idVecX &vec ) const; // dst -= this->Transpose() * vec
|
|
|
|
void Multiply( idMatX &dst, const idMatX &a ) const; // dst = (*this) * a
|
|
void TransposeMultiply( idMatX &dst, const idMatX &a ) const; // dst = this->Transpose() * a
|
|
|
|
int GetDimension( void ) const; // returns total number of values in matrix
|
|
|
|
const idVec6 & SubVec6( int row ) const; // interpret beginning of row as a const idVec6
|
|
idVec6 & SubVec6( int row ); // interpret beginning of row as an idVec6
|
|
const idVecX SubVecX( int row ) const; // interpret complete row as a const idVecX
|
|
idVecX SubVecX( int row ); // interpret complete row as an idVecX
|
|
const float * ToFloatPtr( void ) const; // pointer to const matrix float array
|
|
float * ToFloatPtr( void ); // pointer to matrix float array
|
|
const char * ToString( int precision = 2 ) const;
|
|
|
|
void Update_RankOne( const idVecX &v, const idVecX &w, float alpha );
|
|
void Update_RankOneSymmetric( const idVecX &v, float alpha );
|
|
void Update_RowColumn( const idVecX &v, const idVecX &w, int r );
|
|
void Update_RowColumnSymmetric( const idVecX &v, int r );
|
|
void Update_Increment( const idVecX &v, const idVecX &w );
|
|
void Update_IncrementSymmetric( const idVecX &v );
|
|
void Update_Decrement( int r );
|
|
|
|
bool Inverse_GaussJordan( void ); // invert in-place with Gauss-Jordan elimination
|
|
bool Inverse_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha );
|
|
bool Inverse_UpdateRowColumn( const idVecX &v, const idVecX &w, int r );
|
|
bool Inverse_UpdateIncrement( const idVecX &v, const idVecX &w );
|
|
bool Inverse_UpdateDecrement( const idVecX &v, const idVecX &w, int r );
|
|
void Inverse_Solve( idVecX &x, const idVecX &b ) const;
|
|
|
|
bool LU_Factor( int *index, float *det = NULL ); // factor in-place: L * U
|
|
bool LU_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha, int *index );
|
|
bool LU_UpdateRowColumn( const idVecX &v, const idVecX &w, int r, int *index );
|
|
bool LU_UpdateIncrement( const idVecX &v, const idVecX &w, int *index );
|
|
bool LU_UpdateDecrement( const idVecX &v, const idVecX &w, const idVecX &u, int r, int *index );
|
|
void LU_Solve( idVecX &x, const idVecX &b, const int *index ) const;
|
|
void LU_Inverse( idMatX &inv, const int *index ) const;
|
|
void LU_UnpackFactors( idMatX &L, idMatX &U ) const;
|
|
void LU_MultiplyFactors( idMatX &m, const int *index ) const;
|
|
|
|
bool QR_Factor( idVecX &c, idVecX &d ); // factor in-place: Q * R
|
|
bool QR_UpdateRankOne( idMatX &R, const idVecX &v, const idVecX &w, float alpha );
|
|
bool QR_UpdateRowColumn( idMatX &R, const idVecX &v, const idVecX &w, int r );
|
|
bool QR_UpdateIncrement( idMatX &R, const idVecX &v, const idVecX &w );
|
|
bool QR_UpdateDecrement( idMatX &R, const idVecX &v, const idVecX &w, int r );
|
|
void QR_Solve( idVecX &x, const idVecX &b, const idVecX &c, const idVecX &d ) const;
|
|
void QR_Solve( idVecX &x, const idVecX &b, const idMatX &R ) const;
|
|
void QR_Inverse( idMatX &inv, const idVecX &c, const idVecX &d ) const;
|
|
void QR_UnpackFactors( idMatX &Q, idMatX &R, const idVecX &c, const idVecX &d ) const;
|
|
void QR_MultiplyFactors( idMatX &m, const idVecX &c, const idVecX &d ) const;
|
|
|
|
bool SVD_Factor( idVecX &w, idMatX &V ); // factor in-place: U * Diag(w) * V.Transpose()
|
|
void SVD_Solve( idVecX &x, const idVecX &b, const idVecX &w, const idMatX &V ) const;
|
|
void SVD_Inverse( idMatX &inv, const idVecX &w, const idMatX &V ) const;
|
|
void SVD_MultiplyFactors( idMatX &m, const idVecX &w, const idMatX &V ) const;
|
|
|
|
bool Cholesky_Factor( void ); // factor in-place: L * L.Transpose()
|
|
bool Cholesky_UpdateRankOne( const idVecX &v, float alpha, int offset = 0 );
|
|
bool Cholesky_UpdateRowColumn( const idVecX &v, int r );
|
|
bool Cholesky_UpdateIncrement( const idVecX &v );
|
|
bool Cholesky_UpdateDecrement( const idVecX &v, int r );
|
|
void Cholesky_Solve( idVecX &x, const idVecX &b ) const;
|
|
void Cholesky_Inverse( idMatX &inv ) const;
|
|
void Cholesky_MultiplyFactors( idMatX &m ) const;
|
|
|
|
bool LDLT_Factor( void ); // factor in-place: L * D * L.Transpose()
|
|
bool LDLT_UpdateRankOne( const idVecX &v, float alpha, int offset = 0 );
|
|
bool LDLT_UpdateRowColumn( const idVecX &v, int r );
|
|
bool LDLT_UpdateIncrement( const idVecX &v );
|
|
bool LDLT_UpdateDecrement( const idVecX &v, int r );
|
|
void LDLT_Solve( idVecX &x, const idVecX &b ) const;
|
|
void LDLT_Inverse( idMatX &inv ) const;
|
|
void LDLT_UnpackFactors( idMatX &L, idMatX &D ) const;
|
|
void LDLT_MultiplyFactors( idMatX &m ) const;
|
|
|
|
void TriDiagonal_ClearTriangles( void );
|
|
bool TriDiagonal_Solve( idVecX &x, const idVecX &b ) const;
|
|
void TriDiagonal_Inverse( idMatX &inv ) const;
|
|
|
|
bool Eigen_SolveSymmetricTriDiagonal( idVecX &eigenValues );
|
|
bool Eigen_SolveSymmetric( idVecX &eigenValues );
|
|
bool Eigen_Solve( idVecX &realEigenValues, idVecX &imaginaryEigenValues );
|
|
void Eigen_SortIncreasing( idVecX &eigenValues );
|
|
void Eigen_SortDecreasing( idVecX &eigenValues );
|
|
|
|
static void Test( void );
|
|
|
|
private:
|
|
int numRows; // number of rows
|
|
int numColumns; // number of columns
|
|
int alloced; // floats allocated, if -1 then mat points to data set with SetData
|
|
float * mat; // memory the matrix is stored
|
|
|
|
static float temp[MATX_MAX_TEMP+4]; // used to store intermediate results
|
|
static float * tempPtr; // pointer to 16 byte aligned temporary memory
|
|
static int tempIndex; // index into memory pool, wraps around
|
|
|
|
private:
|
|
void SetTempSize( int rows, int columns );
|
|
float DeterminantGeneric( void ) const;
|
|
bool InverseSelfGeneric( void );
|
|
void QR_Rotate( idMatX &R, int i, float a, float b );
|
|
float Pythag( float a, float b ) const;
|
|
void SVD_BiDiag( idVecX &w, idVecX &rv1, float &anorm );
|
|
void SVD_InitialWV( idVecX &w, idMatX &V, idVecX &rv1 );
|
|
void HouseholderReduction( idVecX &diag, idVecX &subd );
|
|
bool QL( idVecX &diag, idVecX &subd );
|
|
void HessenbergReduction( idMatX &H );
|
|
void ComplexDivision( float xr, float xi, float yr, float yi, float &cdivr, float &cdivi );
|
|
bool HessenbergToRealSchur( idMatX &H, idVecX &realEigenValues, idVecX &imaginaryEigenValues );
|
|
};
|
|
|
|
ID_INLINE idMatX::idMatX( void ) {
|
|
numRows = numColumns = alloced = 0;
|
|
mat = NULL;
|
|
}
|
|
|
|
ID_INLINE idMatX::~idMatX( void ) {
|
|
// if not temp memory
|
|
if ( mat != NULL && ( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP ) && alloced != -1 ) {
|
|
Mem_Free16( mat );
|
|
}
|
|
}
|
|
|
|
ID_INLINE idMatX::idMatX( int rows, int columns ) {
|
|
numRows = numColumns = alloced = 0;
|
|
mat = NULL;
|
|
SetSize( rows, columns );
|
|
}
|
|
|
|
ID_INLINE idMatX::idMatX( int rows, int columns, float *src ) {
|
|
numRows = numColumns = alloced = 0;
|
|
mat = NULL;
|
|
SetData( rows, columns, src );
|
|
}
|
|
|
|
ID_INLINE void idMatX::Set( int rows, int columns, const float *src ) {
|
|
SetSize( rows, columns );
|
|
memcpy( this->mat, src, rows * columns * sizeof( float ) );
|
|
}
|
|
|
|
ID_INLINE void idMatX::Set( const idMat3 &m1, const idMat3 &m2 ) {
|
|
int i, j;
|
|
|
|
SetSize( 3, 6 );
|
|
for ( i = 0; i < 3; i++ ) {
|
|
for ( j = 0; j < 3; j++ ) {
|
|
mat[(i+0) * numColumns + (j+0)] = m1[i][j];
|
|
mat[(i+0) * numColumns + (j+3)] = m2[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
ID_INLINE void idMatX::Set( const idMat3 &m1, const idMat3 &m2, const idMat3 &m3, const idMat3 &m4 ) {
|
|
int i, j;
|
|
|
|
SetSize( 6, 6 );
|
|
for ( i = 0; i < 3; i++ ) {
|
|
for ( j = 0; j < 3; j++ ) {
|
|
mat[(i+0) * numColumns + (j+0)] = m1[i][j];
|
|
mat[(i+0) * numColumns + (j+3)] = m2[i][j];
|
|
mat[(i+3) * numColumns + (j+0)] = m3[i][j];
|
|
mat[(i+3) * numColumns + (j+3)] = m4[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
ID_INLINE const float *idMatX::operator[]( int index ) const {
|
|
assert( ( index >= 0 ) && ( index < numRows ) );
|
|
return mat + index * numColumns;
|
|
}
|
|
|
|
ID_INLINE float *idMatX::operator[]( int index ) {
|
|
assert( ( index >= 0 ) && ( index < numRows ) );
|
|
return mat + index * numColumns;
|
|
}
|
|
|
|
ID_INLINE idMatX &idMatX::operator=( const idMatX &a ) {
|
|
SetSize( a.numRows, a.numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->Copy16( mat, a.mat, a.numRows * a.numColumns );
|
|
#else
|
|
memcpy( mat, a.mat, a.numRows * a.numColumns * sizeof( float ) );
|
|
#endif
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMatX idMatX::operator*( const float a ) const {
|
|
idMatX m;
|
|
|
|
m.SetTempSize( numRows, numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->Mul16( m.mat, mat, a, numRows * numColumns );
|
|
#else
|
|
int i, s;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
m.mat[i] = mat[i] * a;
|
|
}
|
|
#endif
|
|
return m;
|
|
}
|
|
|
|
ID_INLINE idVecX idMatX::operator*( const idVecX &vec ) const {
|
|
idVecX dst;
|
|
|
|
assert( numColumns == vec.GetSize() );
|
|
|
|
dst.SetTempSize( numRows );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_MultiplyVecX( dst, *this, vec );
|
|
#else
|
|
Multiply( dst, vec );
|
|
#endif
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE idMatX idMatX::operator*( const idMatX &a ) const {
|
|
idMatX dst;
|
|
|
|
assert( numColumns == a.numRows );
|
|
|
|
dst.SetTempSize( numRows, a.numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_MultiplyMatX( dst, *this, a );
|
|
#else
|
|
Multiply( dst, a );
|
|
#endif
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE idMatX idMatX::operator+( const idMatX &a ) const {
|
|
idMatX m;
|
|
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
m.SetTempSize( numRows, numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->Add16( m.mat, mat, a.mat, numRows * numColumns );
|
|
#else
|
|
int i, s;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
m.mat[i] = mat[i] + a.mat[i];
|
|
}
|
|
#endif
|
|
return m;
|
|
}
|
|
|
|
ID_INLINE idMatX idMatX::operator-( const idMatX &a ) const {
|
|
idMatX m;
|
|
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
m.SetTempSize( numRows, numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->Sub16( m.mat, mat, a.mat, numRows * numColumns );
|
|
#else
|
|
int i, s;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
m.mat[i] = mat[i] - a.mat[i];
|
|
}
|
|
#endif
|
|
return m;
|
|
}
|
|
|
|
ID_INLINE idMatX &idMatX::operator*=( const float a ) {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MulAssign16( mat, a, numRows * numColumns );
|
|
#else
|
|
int i, s;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
mat[i] *= a;
|
|
}
|
|
#endif
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMatX &idMatX::operator*=( const idMatX &a ) {
|
|
*this = *this * a;
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMatX &idMatX::operator+=( const idMatX &a ) {
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->AddAssign16( mat, a.mat, numRows * numColumns );
|
|
#else
|
|
int i, s;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
mat[i] += a.mat[i];
|
|
}
|
|
#endif
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMatX &idMatX::operator-=( const idMatX &a ) {
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->SubAssign16( mat, a.mat, numRows * numColumns );
|
|
#else
|
|
int i, s;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
mat[i] -= a.mat[i];
|
|
}
|
|
#endif
|
|
idMatX::tempIndex = 0;
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMatX operator*( const float a, idMatX const &m ) {
|
|
return m * a;
|
|
}
|
|
|
|
ID_INLINE idVecX operator*( const idVecX &vec, const idMatX &m ) {
|
|
return m * vec;
|
|
}
|
|
|
|
ID_INLINE idVecX &operator*=( idVecX &vec, const idMatX &m ) {
|
|
vec = m * vec;
|
|
return vec;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::Compare( const idMatX &a ) const {
|
|
int i, s;
|
|
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
if ( mat[i] != a.mat[i] ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::Compare( const idMatX &a, const float epsilon ) const {
|
|
int i, s;
|
|
|
|
assert( numRows == a.numRows && numColumns == a.numColumns );
|
|
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
if ( idMath::Fabs( mat[i] - a.mat[i] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::operator==( const idMatX &a ) const {
|
|
return Compare( a );
|
|
}
|
|
|
|
ID_INLINE bool idMatX::operator!=( const idMatX &a ) const {
|
|
return !Compare( a );
|
|
}
|
|
|
|
ID_INLINE void idMatX::SetSize( int rows, int columns ) {
|
|
assert( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP );
|
|
int alloc = ( rows * columns + 3 ) & ~3;
|
|
if ( alloc > alloced && alloced != -1 ) {
|
|
if ( mat != NULL ) {
|
|
Mem_Free16( mat );
|
|
}
|
|
mat = (float *) Mem_Alloc16( alloc * sizeof( float ) );
|
|
alloced = alloc;
|
|
}
|
|
numRows = rows;
|
|
numColumns = columns;
|
|
MATX_CLEAREND();
|
|
}
|
|
|
|
ID_INLINE void idMatX::SetTempSize( int rows, int columns ) {
|
|
int newSize;
|
|
|
|
newSize = ( rows * columns + 3 ) & ~3;
|
|
assert( newSize < MATX_MAX_TEMP );
|
|
if ( idMatX::tempIndex + newSize > MATX_MAX_TEMP ) {
|
|
idMatX::tempIndex = 0;
|
|
}
|
|
mat = idMatX::tempPtr + idMatX::tempIndex;
|
|
idMatX::tempIndex += newSize;
|
|
alloced = newSize;
|
|
numRows = rows;
|
|
numColumns = columns;
|
|
MATX_CLEAREND();
|
|
}
|
|
|
|
ID_INLINE void idMatX::SetData( int rows, int columns, float *data ) {
|
|
assert( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP );
|
|
if ( mat != NULL && alloced != -1 ) {
|
|
Mem_Free16( mat );
|
|
}
|
|
assert( ( ( (uintptr_t) data ) & 15 ) == 0 ); // data must be 16 byte aligned
|
|
mat = data;
|
|
alloced = -1;
|
|
numRows = rows;
|
|
numColumns = columns;
|
|
MATX_CLEAREND();
|
|
}
|
|
|
|
ID_INLINE void idMatX::Zero( void ) {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->Zero16( mat, numRows * numColumns );
|
|
#else
|
|
memset( mat, 0, numRows * numColumns * sizeof( float ) );
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::Zero( int rows, int columns ) {
|
|
SetSize( rows, columns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->Zero16( mat, numRows * numColumns );
|
|
#else
|
|
memset( mat, 0, rows * columns * sizeof( float ) );
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::Identity( void ) {
|
|
assert( numRows == numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->Zero16( mat, numRows * numColumns );
|
|
#else
|
|
memset( mat, 0, numRows * numColumns * sizeof( float ) );
|
|
#endif
|
|
for ( int i = 0; i < numRows; i++ ) {
|
|
mat[i * numColumns + i] = 1.0f;
|
|
}
|
|
}
|
|
|
|
ID_INLINE void idMatX::Identity( int rows, int columns ) {
|
|
assert( rows == columns );
|
|
SetSize( rows, columns );
|
|
idMatX::Identity();
|
|
}
|
|
|
|
ID_INLINE void idMatX::Diag( const idVecX &v ) {
|
|
Zero( v.GetSize(), v.GetSize() );
|
|
for ( int i = 0; i < v.GetSize(); i++ ) {
|
|
mat[i * numColumns + i] = v[i];
|
|
}
|
|
}
|
|
|
|
ID_INLINE void idMatX::Random( int seed, float l, float u ) {
|
|
int i, s;
|
|
float c;
|
|
idRandom rnd(seed);
|
|
|
|
c = u - l;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
mat[i] = l + rnd.RandomFloat() * c;
|
|
}
|
|
}
|
|
|
|
ID_INLINE void idMatX::Random( int rows, int columns, int seed, float l, float u ) {
|
|
int i, s;
|
|
float c;
|
|
idRandom rnd(seed);
|
|
|
|
SetSize( rows, columns );
|
|
c = u - l;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
mat[i] = l + rnd.RandomFloat() * c;
|
|
}
|
|
}
|
|
|
|
ID_INLINE void idMatX::Negate( void ) {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->Negate16( mat, numRows * numColumns );
|
|
#else
|
|
int i, s;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
mat[i] = -mat[i];
|
|
}
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::Clamp( float min, float max ) {
|
|
int i, s;
|
|
s = numRows * numColumns;
|
|
for ( i = 0; i < s; i++ ) {
|
|
if ( mat[i] < min ) {
|
|
mat[i] = min;
|
|
} else if ( mat[i] > max ) {
|
|
mat[i] = max;
|
|
}
|
|
}
|
|
}
|
|
|
|
ID_INLINE idMatX &idMatX::SwapRows( int r1, int r2 ) {
|
|
float *ptr;
|
|
|
|
ptr = (float *) _alloca16( numColumns * sizeof( float ) );
|
|
memcpy( ptr, mat + r1 * numColumns, numColumns * sizeof( float ) );
|
|
memcpy( mat + r1 * numColumns, mat + r2 * numColumns, numColumns * sizeof( float ) );
|
|
memcpy( mat + r2 * numColumns, ptr, numColumns * sizeof( float ) );
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMatX &idMatX::SwapColumns( int r1, int r2 ) {
|
|
int i;
|
|
float tmp, *ptr;
|
|
|
|
for ( i = 0; i < numRows; i++ ) {
|
|
ptr = mat + i * numColumns;
|
|
tmp = ptr[r1];
|
|
ptr[r1] = ptr[r2];
|
|
ptr[r2] = tmp;
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMatX &idMatX::SwapRowsColumns( int r1, int r2 ) {
|
|
|
|
SwapRows( r1, r2 );
|
|
SwapColumns( r1, r2 );
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE void idMatX::ClearUpperTriangle( void ) {
|
|
assert( numRows == numColumns );
|
|
for ( int i = numRows-2; i >= 0; i-- ) {
|
|
memset( mat + i * numColumns + i + 1, 0, (numColumns - 1 - i) * sizeof(float) );
|
|
}
|
|
}
|
|
|
|
ID_INLINE void idMatX::ClearLowerTriangle( void ) {
|
|
assert( numRows == numColumns );
|
|
for ( int i = 1; i < numRows; i++ ) {
|
|
memset( mat + i * numColumns, 0, i * sizeof(float) );
|
|
}
|
|
}
|
|
|
|
ID_INLINE void idMatX::SquareSubMatrix( const idMatX &m, int size ) {
|
|
int i;
|
|
assert( size <= m.numRows && size <= m.numColumns );
|
|
SetSize( size, size );
|
|
for ( i = 0; i < size; i++ ) {
|
|
memcpy( mat + i * numColumns, m.mat + i * m.numColumns, size * sizeof( float ) );
|
|
}
|
|
}
|
|
|
|
ID_INLINE float idMatX::MaxDifference( const idMatX &m ) const {
|
|
int i, j;
|
|
float diff, maxDiff;
|
|
|
|
assert( numRows == m.numRows && numColumns == m.numColumns );
|
|
|
|
maxDiff = -1.0f;
|
|
for ( i = 0; i < numRows; i++ ) {
|
|
for ( j = 0; j < numColumns; j++ ) {
|
|
diff = idMath::Fabs( mat[ i * numColumns + j ] - m[i][j] );
|
|
if ( maxDiff < 0.0f || diff > maxDiff ) {
|
|
maxDiff = diff;
|
|
}
|
|
}
|
|
}
|
|
return maxDiff;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::IsZero( const float epsilon ) const {
|
|
// returns true if (*this) == Zero
|
|
for ( int i = 0; i < numRows; i++ ) {
|
|
for ( int j = 0; j < numColumns; j++ ) {
|
|
if ( idMath::Fabs( mat[i * numColumns + j] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::IsIdentity( const float epsilon ) const {
|
|
// returns true if (*this) == Identity
|
|
assert( numRows == numColumns );
|
|
for ( int i = 0; i < numRows; i++ ) {
|
|
for ( int j = 0; j < numColumns; j++ ) {
|
|
if ( idMath::Fabs( mat[i * numColumns + j] - (float)( i == j ) ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::IsDiagonal( const float epsilon ) const {
|
|
// returns true if all elements are zero except for the elements on the diagonal
|
|
assert( numRows == numColumns );
|
|
for ( int i = 0; i < numRows; i++ ) {
|
|
for ( int j = 0; j < numColumns; j++ ) {
|
|
if ( i != j && idMath::Fabs( mat[i * numColumns + j] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::IsTriDiagonal( const float epsilon ) const {
|
|
// returns true if all elements are zero except for the elements on the diagonal plus or minus one column
|
|
|
|
if ( numRows != numColumns ) {
|
|
return false;
|
|
}
|
|
for ( int i = 0; i < numRows-2; i++ ) {
|
|
for ( int j = i+2; j < numColumns; j++ ) {
|
|
if ( idMath::Fabs( (*this)[i][j] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
if ( idMath::Fabs( (*this)[j][i] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::IsSymmetric( const float epsilon ) const {
|
|
// (*this)[i][j] == (*this)[j][i]
|
|
if ( numRows != numColumns ) {
|
|
return false;
|
|
}
|
|
for ( int i = 0; i < numRows; i++ ) {
|
|
for ( int j = 0; j < numColumns; j++ ) {
|
|
if ( idMath::Fabs( mat[ i * numColumns + j ] - mat[ j * numColumns + i ] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE float idMatX::Trace( void ) const {
|
|
float trace = 0.0f;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
// sum of elements on the diagonal
|
|
for ( int i = 0; i < numRows; i++ ) {
|
|
trace += mat[i * numRows + i];
|
|
}
|
|
return trace;
|
|
}
|
|
|
|
ID_INLINE float idMatX::Determinant( void ) const {
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
switch( numRows ) {
|
|
case 1:
|
|
return mat[0];
|
|
case 2:
|
|
return reinterpret_cast<const idMat2 *>(mat)->Determinant();
|
|
case 3:
|
|
return reinterpret_cast<const idMat3 *>(mat)->Determinant();
|
|
case 4:
|
|
return reinterpret_cast<const idMat4 *>(mat)->Determinant();
|
|
case 5:
|
|
return reinterpret_cast<const idMat5 *>(mat)->Determinant();
|
|
case 6:
|
|
return reinterpret_cast<const idMat6 *>(mat)->Determinant();
|
|
default:
|
|
return DeterminantGeneric();
|
|
}
|
|
return 0.0f;
|
|
}
|
|
|
|
ID_INLINE idMatX idMatX::Transpose( void ) const {
|
|
idMatX transpose;
|
|
int i, j;
|
|
|
|
transpose.SetTempSize( numColumns, numRows );
|
|
|
|
for ( i = 0; i < numRows; i++ ) {
|
|
for ( j = 0; j < numColumns; j++ ) {
|
|
transpose.mat[j * transpose.numColumns + i] = mat[i * numColumns + j];
|
|
}
|
|
}
|
|
|
|
return transpose;
|
|
}
|
|
|
|
ID_INLINE idMatX &idMatX::TransposeSelf( void ) {
|
|
*this = Transpose();
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idMatX idMatX::Inverse( void ) const {
|
|
idMatX invMat;
|
|
|
|
invMat.SetTempSize( numRows, numColumns );
|
|
memcpy( invMat.mat, mat, numRows * numColumns * sizeof( float ) );
|
|
int r id_attribute((unused)) = invMat.InverseSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::InverseSelf( void ) {
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
switch( numRows ) {
|
|
case 1:
|
|
if ( idMath::Fabs( mat[0] ) < MATRIX_INVERSE_EPSILON ) {
|
|
return false;
|
|
}
|
|
mat[0] = 1.0f / mat[0];
|
|
return true;
|
|
case 2:
|
|
return reinterpret_cast<idMat2 *>(mat)->InverseSelf();
|
|
case 3:
|
|
return reinterpret_cast<idMat3 *>(mat)->InverseSelf();
|
|
case 4:
|
|
return reinterpret_cast<idMat4 *>(mat)->InverseSelf();
|
|
case 5:
|
|
return reinterpret_cast<idMat5 *>(mat)->InverseSelf();
|
|
case 6:
|
|
return reinterpret_cast<idMat6 *>(mat)->InverseSelf();
|
|
default:
|
|
return InverseSelfGeneric();
|
|
}
|
|
}
|
|
|
|
ID_INLINE idMatX idMatX::InverseFast( void ) const {
|
|
idMatX invMat;
|
|
|
|
invMat.SetTempSize( numRows, numColumns );
|
|
memcpy( invMat.mat, mat, numRows * numColumns * sizeof( float ) );
|
|
int r id_attribute((unused)) = invMat.InverseFastSelf();
|
|
assert( r );
|
|
return invMat;
|
|
}
|
|
|
|
ID_INLINE bool idMatX::InverseFastSelf( void ) {
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
switch( numRows ) {
|
|
case 1:
|
|
if ( idMath::Fabs( mat[0] ) < MATRIX_INVERSE_EPSILON ) {
|
|
return false;
|
|
}
|
|
mat[0] = 1.0f / mat[0];
|
|
return true;
|
|
case 2:
|
|
return reinterpret_cast<idMat2 *>(mat)->InverseFastSelf();
|
|
case 3:
|
|
return reinterpret_cast<idMat3 *>(mat)->InverseFastSelf();
|
|
case 4:
|
|
return reinterpret_cast<idMat4 *>(mat)->InverseFastSelf();
|
|
case 5:
|
|
return reinterpret_cast<idMat5 *>(mat)->InverseFastSelf();
|
|
case 6:
|
|
return reinterpret_cast<idMat6 *>(mat)->InverseFastSelf();
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return InverseSelfGeneric();
|
|
}
|
|
|
|
ID_INLINE idVecX idMatX::Multiply( const idVecX &vec ) const {
|
|
idVecX dst;
|
|
|
|
assert( numColumns == vec.GetSize() );
|
|
|
|
dst.SetTempSize( numRows );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_MultiplyVecX( dst, *this, vec );
|
|
#else
|
|
Multiply( dst, vec );
|
|
#endif
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE idMatX idMatX::Multiply( const idMatX &a ) const {
|
|
idMatX dst;
|
|
|
|
assert( numColumns == a.numRows );
|
|
|
|
dst.SetTempSize( numRows, a.numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_MultiplyMatX( dst, *this, a );
|
|
#else
|
|
Multiply( dst, a );
|
|
#endif
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE idVecX idMatX::TransposeMultiply( const idVecX &vec ) const {
|
|
idVecX dst;
|
|
|
|
assert( numRows == vec.GetSize() );
|
|
|
|
dst.SetTempSize( numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_TransposeMultiplyVecX( dst, *this, vec );
|
|
#else
|
|
TransposeMultiply( dst, vec );
|
|
#endif
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE idMatX idMatX::TransposeMultiply( const idMatX &a ) const {
|
|
idMatX dst;
|
|
|
|
assert( numRows == a.numRows );
|
|
|
|
dst.SetTempSize( numColumns, a.numColumns );
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_TransposeMultiplyMatX( dst, *this, a );
|
|
#else
|
|
TransposeMultiply( dst, a );
|
|
#endif
|
|
return dst;
|
|
}
|
|
|
|
ID_INLINE void idMatX::Multiply( idVecX &dst, const idVecX &vec ) const {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_MultiplyVecX( dst, *this, vec );
|
|
#else
|
|
int i, j;
|
|
const float *mPtr, *vPtr;
|
|
float *dstPtr;
|
|
|
|
mPtr = mat;
|
|
vPtr = vec.ToFloatPtr();
|
|
dstPtr = dst.ToFloatPtr();
|
|
for ( i = 0; i < numRows; i++ ) {
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for ( j = 1; j < numColumns; j++ ) {
|
|
sum += mPtr[j] * vPtr[j];
|
|
}
|
|
dstPtr[i] = sum;
|
|
mPtr += numColumns;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::MultiplyAdd( idVecX &dst, const idVecX &vec ) const {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_MultiplyAddVecX( dst, *this, vec );
|
|
#else
|
|
int i, j;
|
|
const float *mPtr, *vPtr;
|
|
float *dstPtr;
|
|
|
|
mPtr = mat;
|
|
vPtr = vec.ToFloatPtr();
|
|
dstPtr = dst.ToFloatPtr();
|
|
for ( i = 0; i < numRows; i++ ) {
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for ( j = 1; j < numColumns; j++ ) {
|
|
sum += mPtr[j] * vPtr[j];
|
|
}
|
|
dstPtr[i] += sum;
|
|
mPtr += numColumns;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::MultiplySub( idVecX &dst, const idVecX &vec ) const {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_MultiplySubVecX( dst, *this, vec );
|
|
#else
|
|
int i, j;
|
|
const float *mPtr, *vPtr;
|
|
float *dstPtr;
|
|
|
|
mPtr = mat;
|
|
vPtr = vec.ToFloatPtr();
|
|
dstPtr = dst.ToFloatPtr();
|
|
for ( i = 0; i < numRows; i++ ) {
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for ( j = 1; j < numColumns; j++ ) {
|
|
sum += mPtr[j] * vPtr[j];
|
|
}
|
|
dstPtr[i] -= sum;
|
|
mPtr += numColumns;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::TransposeMultiply( idVecX &dst, const idVecX &vec ) const {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_TransposeMultiplyVecX( dst, *this, vec );
|
|
#else
|
|
int i, j;
|
|
const float *mPtr, *vPtr;
|
|
float *dstPtr;
|
|
|
|
vPtr = vec.ToFloatPtr();
|
|
dstPtr = dst.ToFloatPtr();
|
|
for ( i = 0; i < numColumns; i++ ) {
|
|
mPtr = mat + i;
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for ( j = 1; j < numRows; j++ ) {
|
|
mPtr += numColumns;
|
|
sum += mPtr[0] * vPtr[j];
|
|
}
|
|
dstPtr[i] = sum;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::TransposeMultiplyAdd( idVecX &dst, const idVecX &vec ) const {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_TransposeMultiplyAddVecX( dst, *this, vec );
|
|
#else
|
|
int i, j;
|
|
const float *mPtr, *vPtr;
|
|
float *dstPtr;
|
|
|
|
vPtr = vec.ToFloatPtr();
|
|
dstPtr = dst.ToFloatPtr();
|
|
for ( i = 0; i < numColumns; i++ ) {
|
|
mPtr = mat + i;
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for ( j = 1; j < numRows; j++ ) {
|
|
mPtr += numColumns;
|
|
sum += mPtr[0] * vPtr[j];
|
|
}
|
|
dstPtr[i] += sum;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::TransposeMultiplySub( idVecX &dst, const idVecX &vec ) const {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_TransposeMultiplySubVecX( dst, *this, vec );
|
|
#else
|
|
int i, j;
|
|
const float *mPtr, *vPtr;
|
|
float *dstPtr;
|
|
|
|
vPtr = vec.ToFloatPtr();
|
|
dstPtr = dst.ToFloatPtr();
|
|
for ( i = 0; i < numColumns; i++ ) {
|
|
mPtr = mat + i;
|
|
float sum = mPtr[0] * vPtr[0];
|
|
for ( j = 1; j < numRows; j++ ) {
|
|
mPtr += numColumns;
|
|
sum += mPtr[0] * vPtr[j];
|
|
}
|
|
dstPtr[i] -= sum;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::Multiply( idMatX &dst, const idMatX &a ) const {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_MultiplyMatX( dst, *this, a );
|
|
#else
|
|
int i, j, k, l, n;
|
|
float *dstPtr;
|
|
const float *m1Ptr, *m2Ptr;
|
|
double sum;
|
|
|
|
assert( numColumns == a.numRows );
|
|
|
|
dstPtr = dst.ToFloatPtr();
|
|
m1Ptr = ToFloatPtr();
|
|
m2Ptr = a.ToFloatPtr();
|
|
k = numRows;
|
|
l = a.GetNumColumns();
|
|
|
|
for ( i = 0; i < k; i++ ) {
|
|
for ( j = 0; j < l; j++ ) {
|
|
m2Ptr = a.ToFloatPtr() + j;
|
|
sum = m1Ptr[0] * m2Ptr[0];
|
|
for ( n = 1; n < numColumns; n++ ) {
|
|
m2Ptr += l;
|
|
sum += m1Ptr[n] * m2Ptr[0];
|
|
}
|
|
*dstPtr++ = sum;
|
|
}
|
|
m1Ptr += numColumns;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE void idMatX::TransposeMultiply( idMatX &dst, const idMatX &a ) const {
|
|
#ifdef MATX_SIMD
|
|
SIMDProcessor->MatX_TransposeMultiplyMatX( dst, *this, a );
|
|
#else
|
|
int i, j, k, l, n;
|
|
float *dstPtr;
|
|
const float *m1Ptr, *m2Ptr;
|
|
double sum;
|
|
|
|
assert( numRows == a.numRows );
|
|
|
|
dstPtr = dst.ToFloatPtr();
|
|
m1Ptr = ToFloatPtr();
|
|
k = numColumns;
|
|
l = a.numColumns;
|
|
|
|
for ( i = 0; i < k; i++ ) {
|
|
for ( j = 0; j < l; j++ ) {
|
|
m1Ptr = ToFloatPtr() + i;
|
|
m2Ptr = a.ToFloatPtr() + j;
|
|
sum = m1Ptr[0] * m2Ptr[0];
|
|
for ( n = 1; n < numRows; n++ ) {
|
|
m1Ptr += numColumns;
|
|
m2Ptr += a.numColumns;
|
|
sum += m1Ptr[0] * m2Ptr[0];
|
|
}
|
|
*dstPtr++ = sum;
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
ID_INLINE int idMatX::GetDimension( void ) const {
|
|
return numRows * numColumns;
|
|
}
|
|
|
|
ID_INLINE const idVec6 &idMatX::SubVec6( int row ) const {
|
|
assert( numColumns >= 6 && row >= 0 && row < numRows );
|
|
return *reinterpret_cast<const idVec6 *>(mat + row * numColumns);
|
|
}
|
|
|
|
ID_INLINE idVec6 &idMatX::SubVec6( int row ) {
|
|
assert( numColumns >= 6 && row >= 0 && row < numRows );
|
|
return *reinterpret_cast<idVec6 *>(mat + row * numColumns);
|
|
}
|
|
|
|
ID_INLINE const idVecX idMatX::SubVecX( int row ) const {
|
|
idVecX v;
|
|
assert( row >= 0 && row < numRows );
|
|
v.SetData( numColumns, mat + row * numColumns );
|
|
return v;
|
|
}
|
|
|
|
ID_INLINE idVecX idMatX::SubVecX( int row ) {
|
|
idVecX v;
|
|
assert( row >= 0 && row < numRows );
|
|
v.SetData( numColumns, mat + row * numColumns );
|
|
return v;
|
|
}
|
|
|
|
ID_INLINE const float *idMatX::ToFloatPtr( void ) const {
|
|
return mat;
|
|
}
|
|
|
|
ID_INLINE float *idMatX::ToFloatPtr( void ) {
|
|
return mat;
|
|
}
|
|
|
|
#endif /* !__MATH_MATRIX_H__ */
|