433 lines
18 KiB
C++
433 lines
18 KiB
C++
// Copyright (C) 2007 Id Software, Inc.
|
|
//
|
|
|
|
#ifndef __JOINTTRANSFORM_H__
|
|
#define __JOINTTRANSFORM_H__
|
|
|
|
/*
|
|
===============================================================================
|
|
|
|
Joint Quaternion
|
|
|
|
===============================================================================
|
|
*/
|
|
|
|
class idJointQuat {
|
|
public:
|
|
|
|
idQuat q;
|
|
idVec3 t;
|
|
float w;
|
|
};
|
|
|
|
// offsets for SIMD code
|
|
#define JOINTQUAT_SIZE (8*4) // sizeof( idJointQuat )
|
|
#define JOINTQUAT_SIZE_SHIFT 5 // log2( sizeof( idJointQuat ) )
|
|
#define JOINTQUAT_Q_OFFSET (0*4) // offsetof( idJointQuat, q )
|
|
#define JOINTQUAT_T_OFFSET (4*4) // offsetof( idJointQuat, t )
|
|
|
|
assert_sizeof( idJointQuat, JOINTQUAT_SIZE );
|
|
assert_sizeof( idJointQuat, (1<<JOINTQUAT_SIZE_SHIFT) );
|
|
assert_offsetof( idJointQuat, q, JOINTQUAT_Q_OFFSET );
|
|
assert_offsetof( idJointQuat, t, JOINTQUAT_T_OFFSET );
|
|
|
|
/*
|
|
===============================================================================
|
|
|
|
Compressed Joint Quaternion
|
|
|
|
===============================================================================
|
|
*/
|
|
|
|
class idCompressedJointQuat {
|
|
public:
|
|
static const int MAX_BONE_LENGTH = 256;
|
|
|
|
short q[3];
|
|
short t[3];
|
|
|
|
static short QuatToShort( const float c );
|
|
static short OffsetToShort( const float o );
|
|
static float ShortToQuat( const short c );
|
|
static float ShortToOffset( const short o );
|
|
|
|
void ClearQuat( void ) { q[0] = q[1] = q[2] = 0; }
|
|
void ClearOffset( void ) { t[0] = t[1] = t[2] = 0; }
|
|
|
|
idQuat ToQuat( void ) const;
|
|
idVec3 ToOffset( void ) const;
|
|
};
|
|
|
|
ID_INLINE short idCompressedJointQuat::QuatToShort( const float x ) {
|
|
return idMath::ClampShort( idMath::Ftoi( ( x < 0 ) ? ( x * 32767.0f - 0.5f ) : ( x * 32767.0f + 0.5f ) ) );
|
|
}
|
|
|
|
ID_INLINE short idCompressedJointQuat::OffsetToShort( const float x ) {
|
|
//assert( x > -MAX_BONE_LENGTH && x < MAX_BONE_LENGTH );
|
|
return idMath::ClampShort( idMath::Ftoi( ( x < 0 ) ? ( x * ( 32767.0f / MAX_BONE_LENGTH ) - 0.5f ) : ( x * ( 32767.0f / MAX_BONE_LENGTH ) + 0.5f ) ) );
|
|
}
|
|
|
|
ID_INLINE float idCompressedJointQuat::ShortToQuat( const short x ) {
|
|
return x * ( 1.0f / 32767.0f );
|
|
}
|
|
|
|
ID_INLINE float idCompressedJointQuat::ShortToOffset( const short x ) {
|
|
return x * ( 1.0f / ( 32767.0f / MAX_BONE_LENGTH ) );
|
|
}
|
|
|
|
ID_INLINE idQuat idCompressedJointQuat::ToQuat( void ) const {
|
|
idQuat quat;
|
|
quat.x = ShortToQuat( q[0] );
|
|
quat.y = ShortToQuat( q[1] );
|
|
quat.z = ShortToQuat( q[2] );
|
|
// take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
|
|
quat.w = idMath::Sqrt( idMath::Fabs( 1.0f - ( quat.x * quat.x + quat.y * quat.y + quat.z * quat.z ) ) );
|
|
return quat;
|
|
}
|
|
|
|
ID_INLINE idVec3 idCompressedJointQuat::ToOffset( void ) const {
|
|
return idVec3( ShortToOffset( t[0] ), ShortToOffset( t[1] ), ShortToOffset( t[2] ) );
|
|
}
|
|
|
|
/*
|
|
===============================================================================
|
|
|
|
Joint Matrix
|
|
|
|
idMat3 m;
|
|
idVec3 t;
|
|
|
|
m[0][0], m[1][0], m[2][0], t[0]
|
|
m[0][1], m[1][1], m[2][1], t[1]
|
|
m[0][2], m[1][2], m[2][2], t[2]
|
|
|
|
===============================================================================
|
|
*/
|
|
|
|
class idJointMat {
|
|
public:
|
|
|
|
void SetRotation( const idMat3 &m );
|
|
void SetTranslation( const idVec3 &t );
|
|
|
|
idVec3 operator*( const idVec3 &v ) const; // only rotate
|
|
idVec3 operator*( const idVec4 &v ) const; // rotate and translate
|
|
void Mul( idVec3 &out, const idVec3 &v ) const; // rotate and translate
|
|
|
|
idJointMat & operator*=( const idJointMat &a ); // transform
|
|
idJointMat & operator/=( const idJointMat &a ); // untransform
|
|
|
|
bool Compare( const idJointMat &a ) const; // exact compare, no epsilon
|
|
bool Compare( const idJointMat &a, const float epsilon ) const; // compare with epsilon
|
|
bool operator==( const idJointMat &a ) const; // exact compare, no epsilon
|
|
bool operator!=( const idJointMat &a ) const; // exact compare, no epsilon
|
|
|
|
void Identity( void );
|
|
void Invert( void );
|
|
|
|
idMat3 ToMat3( void ) const;
|
|
idVec3 ToVec3( void ) const;
|
|
idJointQuat ToJointQuat( void ) const;
|
|
void ToDualQuat( float dq[2][4] ) const;
|
|
const float * ToFloatPtr( void ) const;
|
|
float * ToFloatPtr( void );
|
|
|
|
static void Mul( idJointMat &result, const idJointMat &mat, const float s );
|
|
static void Mad( idJointMat &result, const idJointMat &mat, const float s );
|
|
static void Multiply( idJointMat &result, const idJointMat &m1, const idJointMat &m2 );
|
|
static void InverseMultiply( idJointMat &result, const idJointMat &m1, const idJointMat &m2 );
|
|
|
|
private:
|
|
float mat[3*4];
|
|
};
|
|
|
|
// offsets for SIMD code
|
|
#define JOINTMAT_SIZE (4*3*4) // sizeof( idJointMat )
|
|
|
|
assert_sizeof( idJointMat, JOINTMAT_SIZE );
|
|
|
|
|
|
ID_INLINE void idJointMat::SetRotation( const idMat3 &m ) {
|
|
// NOTE: idMat3 is transposed because it is column-major
|
|
mat[0 * 4 + 0] = m[0][0];
|
|
mat[0 * 4 + 1] = m[1][0];
|
|
mat[0 * 4 + 2] = m[2][0];
|
|
mat[1 * 4 + 0] = m[0][1];
|
|
mat[1 * 4 + 1] = m[1][1];
|
|
mat[1 * 4 + 2] = m[2][1];
|
|
mat[2 * 4 + 0] = m[0][2];
|
|
mat[2 * 4 + 1] = m[1][2];
|
|
mat[2 * 4 + 2] = m[2][2];
|
|
}
|
|
|
|
ID_INLINE void idJointMat::SetTranslation( const idVec3 &t ) {
|
|
mat[0 * 4 + 3] = t[0];
|
|
mat[1 * 4 + 3] = t[1];
|
|
mat[2 * 4 + 3] = t[2];
|
|
}
|
|
|
|
ID_INLINE idVec3 idJointMat::operator*( const idVec3 &v ) const {
|
|
return idVec3( mat[0 * 4 + 0] * v[0] + mat[0 * 4 + 1] * v[1] + mat[0 * 4 + 2] * v[2],
|
|
mat[1 * 4 + 0] * v[0] + mat[1 * 4 + 1] * v[1] + mat[1 * 4 + 2] * v[2],
|
|
mat[2 * 4 + 0] * v[0] + mat[2 * 4 + 1] * v[1] + mat[2 * 4 + 2] * v[2] );
|
|
}
|
|
|
|
ID_INLINE idVec3 idJointMat::operator*( const idVec4 &v ) const {
|
|
return idVec3( mat[0 * 4 + 0] * v[0] + mat[0 * 4 + 1] * v[1] + mat[0 * 4 + 2] * v[2] + mat[0 * 4 + 3] * v[3],
|
|
mat[1 * 4 + 0] * v[0] + mat[1 * 4 + 1] * v[1] + mat[1 * 4 + 2] * v[2] + mat[1 * 4 + 3] * v[3],
|
|
mat[2 * 4 + 0] * v[0] + mat[2 * 4 + 1] * v[1] + mat[2 * 4 + 2] * v[2] + mat[2 * 4 + 3] * v[3] );
|
|
}
|
|
|
|
ID_INLINE void idJointMat::Mul( idVec3 &o, const idVec3 &v ) const {
|
|
o.x = mat[0 * 4 + 0] * v[0] + mat[0 * 4 + 1] * v[1] + mat[0 * 4 + 2] * v[2] + mat[0 * 4 + 3];
|
|
o.y = mat[1 * 4 + 0] * v[0] + mat[1 * 4 + 1] * v[1] + mat[1 * 4 + 2] * v[2] + mat[1 * 4 + 3];
|
|
o.z = mat[2 * 4 + 0] * v[0] + mat[2 * 4 + 1] * v[1] + mat[2 * 4 + 2] * v[2] + mat[2 * 4 + 3];
|
|
}
|
|
|
|
|
|
ID_INLINE idJointMat &idJointMat::operator*=( const idJointMat &a ) {
|
|
float tmp[3];
|
|
|
|
tmp[0] = mat[0 * 4 + 0] * a.mat[0 * 4 + 0] + mat[1 * 4 + 0] * a.mat[0 * 4 + 1] + mat[2 * 4 + 0] * a.mat[0 * 4 + 2];
|
|
tmp[1] = mat[0 * 4 + 0] * a.mat[1 * 4 + 0] + mat[1 * 4 + 0] * a.mat[1 * 4 + 1] + mat[2 * 4 + 0] * a.mat[1 * 4 + 2];
|
|
tmp[2] = mat[0 * 4 + 0] * a.mat[2 * 4 + 0] + mat[1 * 4 + 0] * a.mat[2 * 4 + 1] + mat[2 * 4 + 0] * a.mat[2 * 4 + 2];
|
|
mat[0 * 4 + 0] = tmp[0];
|
|
mat[1 * 4 + 0] = tmp[1];
|
|
mat[2 * 4 + 0] = tmp[2];
|
|
|
|
tmp[0] = mat[0 * 4 + 1] * a.mat[0 * 4 + 0] + mat[1 * 4 + 1] * a.mat[0 * 4 + 1] + mat[2 * 4 + 1] * a.mat[0 * 4 + 2];
|
|
tmp[1] = mat[0 * 4 + 1] * a.mat[1 * 4 + 0] + mat[1 * 4 + 1] * a.mat[1 * 4 + 1] + mat[2 * 4 + 1] * a.mat[1 * 4 + 2];
|
|
tmp[2] = mat[0 * 4 + 1] * a.mat[2 * 4 + 0] + mat[1 * 4 + 1] * a.mat[2 * 4 + 1] + mat[2 * 4 + 1] * a.mat[2 * 4 + 2];
|
|
mat[0 * 4 + 1] = tmp[0];
|
|
mat[1 * 4 + 1] = tmp[1];
|
|
mat[2 * 4 + 1] = tmp[2];
|
|
|
|
tmp[0] = mat[0 * 4 + 2] * a.mat[0 * 4 + 0] + mat[1 * 4 + 2] * a.mat[0 * 4 + 1] + mat[2 * 4 + 2] * a.mat[0 * 4 + 2];
|
|
tmp[1] = mat[0 * 4 + 2] * a.mat[1 * 4 + 0] + mat[1 * 4 + 2] * a.mat[1 * 4 + 1] + mat[2 * 4 + 2] * a.mat[1 * 4 + 2];
|
|
tmp[2] = mat[0 * 4 + 2] * a.mat[2 * 4 + 0] + mat[1 * 4 + 2] * a.mat[2 * 4 + 1] + mat[2 * 4 + 2] * a.mat[2 * 4 + 2];
|
|
mat[0 * 4 + 2] = tmp[0];
|
|
mat[1 * 4 + 2] = tmp[1];
|
|
mat[2 * 4 + 2] = tmp[2];
|
|
|
|
tmp[0] = mat[0 * 4 + 3] * a.mat[0 * 4 + 0] + mat[1 * 4 + 3] * a.mat[0 * 4 + 1] + mat[2 * 4 + 3] * a.mat[0 * 4 + 2];
|
|
tmp[1] = mat[0 * 4 + 3] * a.mat[1 * 4 + 0] + mat[1 * 4 + 3] * a.mat[1 * 4 + 1] + mat[2 * 4 + 3] * a.mat[1 * 4 + 2];
|
|
tmp[2] = mat[0 * 4 + 3] * a.mat[2 * 4 + 0] + mat[1 * 4 + 3] * a.mat[2 * 4 + 1] + mat[2 * 4 + 3] * a.mat[2 * 4 + 2];
|
|
mat[0 * 4 + 3] = tmp[0];
|
|
mat[1 * 4 + 3] = tmp[1];
|
|
mat[2 * 4 + 3] = tmp[2];
|
|
|
|
mat[0 * 4 + 3] += a.mat[0 * 4 + 3];
|
|
mat[1 * 4 + 3] += a.mat[1 * 4 + 3];
|
|
mat[2 * 4 + 3] += a.mat[2 * 4 + 3];
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE idJointMat &idJointMat::operator/=( const idJointMat &a ) {
|
|
float tmp[3];
|
|
|
|
mat[0 * 4 + 3] -= a.mat[0 * 4 + 3];
|
|
mat[1 * 4 + 3] -= a.mat[1 * 4 + 3];
|
|
mat[2 * 4 + 3] -= a.mat[2 * 4 + 3];
|
|
|
|
tmp[0] = mat[0 * 4 + 0] * a.mat[0 * 4 + 0] + mat[1 * 4 + 0] * a.mat[1 * 4 + 0] + mat[2 * 4 + 0] * a.mat[2 * 4 + 0];
|
|
tmp[1] = mat[0 * 4 + 0] * a.mat[0 * 4 + 1] + mat[1 * 4 + 0] * a.mat[1 * 4 + 1] + mat[2 * 4 + 0] * a.mat[2 * 4 + 1];
|
|
tmp[2] = mat[0 * 4 + 0] * a.mat[0 * 4 + 2] + mat[1 * 4 + 0] * a.mat[1 * 4 + 2] + mat[2 * 4 + 0] * a.mat[2 * 4 + 2];
|
|
mat[0 * 4 + 0] = tmp[0];
|
|
mat[1 * 4 + 0] = tmp[1];
|
|
mat[2 * 4 + 0] = tmp[2];
|
|
|
|
tmp[0] = mat[0 * 4 + 1] * a.mat[0 * 4 + 0] + mat[1 * 4 + 1] * a.mat[1 * 4 + 0] + mat[2 * 4 + 1] * a.mat[2 * 4 + 0];
|
|
tmp[1] = mat[0 * 4 + 1] * a.mat[0 * 4 + 1] + mat[1 * 4 + 1] * a.mat[1 * 4 + 1] + mat[2 * 4 + 1] * a.mat[2 * 4 + 1];
|
|
tmp[2] = mat[0 * 4 + 1] * a.mat[0 * 4 + 2] + mat[1 * 4 + 1] * a.mat[1 * 4 + 2] + mat[2 * 4 + 1] * a.mat[2 * 4 + 2];
|
|
mat[0 * 4 + 1] = tmp[0];
|
|
mat[1 * 4 + 1] = tmp[1];
|
|
mat[2 * 4 + 1] = tmp[2];
|
|
|
|
tmp[0] = mat[0 * 4 + 2] * a.mat[0 * 4 + 0] + mat[1 * 4 + 2] * a.mat[1 * 4 + 0] + mat[2 * 4 + 2] * a.mat[2 * 4 + 0];
|
|
tmp[1] = mat[0 * 4 + 2] * a.mat[0 * 4 + 1] + mat[1 * 4 + 2] * a.mat[1 * 4 + 1] + mat[2 * 4 + 2] * a.mat[2 * 4 + 1];
|
|
tmp[2] = mat[0 * 4 + 2] * a.mat[0 * 4 + 2] + mat[1 * 4 + 2] * a.mat[1 * 4 + 2] + mat[2 * 4 + 2] * a.mat[2 * 4 + 2];
|
|
mat[0 * 4 + 2] = tmp[0];
|
|
mat[1 * 4 + 2] = tmp[1];
|
|
mat[2 * 4 + 2] = tmp[2];
|
|
|
|
tmp[0] = mat[0 * 4 + 3] * a.mat[0 * 4 + 0] + mat[1 * 4 + 3] * a.mat[1 * 4 + 0] + mat[2 * 4 + 3] * a.mat[2 * 4 + 0];
|
|
tmp[1] = mat[0 * 4 + 3] * a.mat[0 * 4 + 1] + mat[1 * 4 + 3] * a.mat[1 * 4 + 1] + mat[2 * 4 + 3] * a.mat[2 * 4 + 1];
|
|
tmp[2] = mat[0 * 4 + 3] * a.mat[0 * 4 + 2] + mat[1 * 4 + 3] * a.mat[1 * 4 + 2] + mat[2 * 4 + 3] * a.mat[2 * 4 + 2];
|
|
mat[0 * 4 + 3] = tmp[0];
|
|
mat[1 * 4 + 3] = tmp[1];
|
|
mat[2 * 4 + 3] = tmp[2];
|
|
|
|
return *this;
|
|
}
|
|
|
|
ID_INLINE bool idJointMat::Compare( const idJointMat &a ) const {
|
|
int i;
|
|
|
|
for ( i = 0; i < 12; i++ ) {
|
|
if ( mat[i] != a.mat[i] ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idJointMat::Compare( const idJointMat &a, const float epsilon ) const {
|
|
int i;
|
|
|
|
for ( i = 0; i < 12; i++ ) {
|
|
if ( idMath::Fabs( mat[i] - a.mat[i] ) > epsilon ) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
ID_INLINE bool idJointMat::operator==( const idJointMat &a ) const {
|
|
return Compare( a );
|
|
}
|
|
|
|
ID_INLINE bool idJointMat::operator!=( const idJointMat &a ) const {
|
|
return !Compare( a );
|
|
}
|
|
|
|
ID_INLINE void idJointMat::Identity( void ) {
|
|
mat[0 * 4 + 0] = 1.0f; mat[0 * 4 + 1] = 0.0f; mat[0 * 4 + 2] = 0.0f; mat[0 * 4 + 3] = 0.0f;
|
|
mat[0 * 4 + 0] = 0.0f; mat[0 * 4 + 1] = 1.0f; mat[0 * 4 + 2] = 0.0f; mat[0 * 4 + 3] = 0.0f;
|
|
mat[0 * 4 + 0] = 0.0f; mat[0 * 4 + 1] = 0.0f; mat[0 * 4 + 2] = 1.0f; mat[0 * 4 + 3] = 0.0f;
|
|
}
|
|
|
|
ID_INLINE void idJointMat::Invert( void ) {
|
|
float tmp[3];
|
|
|
|
// negate inverse rotated translation part
|
|
tmp[0] = mat[0 * 4 + 0] * mat[0 * 4 + 3] + mat[1 * 4 + 0] * mat[1 * 4 + 3] + mat[2 * 4 + 0] * mat[2 * 4 + 3];
|
|
tmp[1] = mat[0 * 4 + 1] * mat[0 * 4 + 3] + mat[1 * 4 + 1] * mat[1 * 4 + 3] + mat[2 * 4 + 1] * mat[2 * 4 + 3];
|
|
tmp[2] = mat[0 * 4 + 2] * mat[0 * 4 + 3] + mat[1 * 4 + 2] * mat[1 * 4 + 3] + mat[2 * 4 + 2] * mat[2 * 4 + 3];
|
|
mat[0 * 4 + 3] = -tmp[0];
|
|
mat[1 * 4 + 3] = -tmp[1];
|
|
mat[2 * 4 + 3] = -tmp[2];
|
|
|
|
// transpose rotation part
|
|
tmp[0] = mat[0 * 4 + 1];
|
|
mat[0 * 4 + 1] = mat[1 * 4 + 0];
|
|
mat[1 * 4 + 0] = tmp[0];
|
|
tmp[1] = mat[0 * 4 + 2];
|
|
mat[0 * 4 + 2] = mat[2 * 4 + 0];
|
|
mat[2 * 4 + 0] = tmp[1];
|
|
tmp[2] = mat[1 * 4 + 2];
|
|
mat[1 * 4 + 2] = mat[2 * 4 + 1];
|
|
mat[2 * 4 + 1] = tmp[2];
|
|
}
|
|
|
|
ID_INLINE idMat3 idJointMat::ToMat3( void ) const {
|
|
return idMat3( mat[0 * 4 + 0], mat[1 * 4 + 0], mat[2 * 4 + 0],
|
|
mat[0 * 4 + 1], mat[1 * 4 + 1], mat[2 * 4 + 1],
|
|
mat[0 * 4 + 2], mat[1 * 4 + 2], mat[2 * 4 + 2] );
|
|
}
|
|
|
|
ID_INLINE idVec3 idJointMat::ToVec3( void ) const {
|
|
return idVec3( mat[0 * 4 + 3], mat[1 * 4 + 3], mat[2 * 4 + 3] );
|
|
}
|
|
|
|
ID_INLINE const float *idJointMat::ToFloatPtr( void ) const {
|
|
return mat;
|
|
}
|
|
|
|
ID_INLINE float *idJointMat::ToFloatPtr( void ) {
|
|
return mat;
|
|
}
|
|
|
|
ID_INLINE void idJointMat::Mul( idJointMat &result, const idJointMat &mat, const float s ) {
|
|
result.mat[0 * 4 + 0] = s * mat.mat[0 * 4 + 0];
|
|
result.mat[0 * 4 + 1] = s * mat.mat[0 * 4 + 1];
|
|
result.mat[0 * 4 + 2] = s * mat.mat[0 * 4 + 2];
|
|
result.mat[0 * 4 + 3] = s * mat.mat[0 * 4 + 3];
|
|
result.mat[1 * 4 + 0] = s * mat.mat[1 * 4 + 0];
|
|
result.mat[1 * 4 + 1] = s * mat.mat[1 * 4 + 1];
|
|
result.mat[1 * 4 + 2] = s * mat.mat[1 * 4 + 2];
|
|
result.mat[1 * 4 + 3] = s * mat.mat[1 * 4 + 3];
|
|
result.mat[2 * 4 + 0] = s * mat.mat[2 * 4 + 0];
|
|
result.mat[2 * 4 + 1] = s * mat.mat[2 * 4 + 1];
|
|
result.mat[2 * 4 + 2] = s * mat.mat[2 * 4 + 2];
|
|
result.mat[2 * 4 + 3] = s * mat.mat[2 * 4 + 3];
|
|
}
|
|
|
|
ID_INLINE void idJointMat::Mad( idJointMat &result, const idJointMat &mat, const float s ) {
|
|
result.mat[0 * 4 + 0] += s * mat.mat[0 * 4 + 0];
|
|
result.mat[0 * 4 + 1] += s * mat.mat[0 * 4 + 1];
|
|
result.mat[0 * 4 + 2] += s * mat.mat[0 * 4 + 2];
|
|
result.mat[0 * 4 + 3] += s * mat.mat[0 * 4 + 3];
|
|
result.mat[1 * 4 + 0] += s * mat.mat[1 * 4 + 0];
|
|
result.mat[1 * 4 + 1] += s * mat.mat[1 * 4 + 1];
|
|
result.mat[1 * 4 + 2] += s * mat.mat[1 * 4 + 2];
|
|
result.mat[1 * 4 + 3] += s * mat.mat[1 * 4 + 3];
|
|
result.mat[2 * 4 + 0] += s * mat.mat[2 * 4 + 0];
|
|
result.mat[2 * 4 + 1] += s * mat.mat[2 * 4 + 1];
|
|
result.mat[2 * 4 + 2] += s * mat.mat[2 * 4 + 2];
|
|
result.mat[2 * 4 + 3] += s * mat.mat[2 * 4 + 3];
|
|
}
|
|
|
|
ID_INLINE void idJointMat::Multiply( idJointMat &result, const idJointMat &m1, const idJointMat &m2 ) {
|
|
result.mat[0 * 4 + 0] = m1.mat[0 * 4 + 0] * m2.mat[0 * 4 + 0] + m1.mat[0 * 4 + 1] * m2.mat[1 * 4 + 0] + m1.mat[0 * 4 + 2] * m2.mat[2 * 4 + 0];
|
|
result.mat[0 * 4 + 1] = m1.mat[0 * 4 + 0] * m2.mat[0 * 4 + 1] + m1.mat[0 * 4 + 1] * m2.mat[1 * 4 + 1] + m1.mat[0 * 4 + 2] * m2.mat[2 * 4 + 1];
|
|
result.mat[0 * 4 + 2] = m1.mat[0 * 4 + 0] * m2.mat[0 * 4 + 2] + m1.mat[0 * 4 + 1] * m2.mat[1 * 4 + 2] + m1.mat[0 * 4 + 2] * m2.mat[2 * 4 + 2];
|
|
result.mat[0 * 4 + 3] = m1.mat[0 * 4 + 0] * m2.mat[0 * 4 + 3] + m1.mat[0 * 4 + 1] * m2.mat[1 * 4 + 3] + m1.mat[0 * 4 + 2] * m2.mat[2 * 4 + 3] + m1.mat[0 * 4 + 3];
|
|
|
|
result.mat[1 * 4 + 0] = m1.mat[1 * 4 + 0] * m2.mat[0 * 4 + 0] + m1.mat[1 * 4 + 1] * m2.mat[1 * 4 + 0] + m1.mat[1 * 4 + 2] * m2.mat[2 * 4 + 0];
|
|
result.mat[1 * 4 + 1] = m1.mat[1 * 4 + 0] * m2.mat[0 * 4 + 1] + m1.mat[1 * 4 + 1] * m2.mat[1 * 4 + 1] + m1.mat[1 * 4 + 2] * m2.mat[2 * 4 + 1];
|
|
result.mat[1 * 4 + 2] = m1.mat[1 * 4 + 0] * m2.mat[0 * 4 + 2] + m1.mat[1 * 4 + 1] * m2.mat[1 * 4 + 2] + m1.mat[1 * 4 + 2] * m2.mat[2 * 4 + 2];
|
|
result.mat[1 * 4 + 3] = m1.mat[1 * 4 + 0] * m2.mat[0 * 4 + 3] + m1.mat[1 * 4 + 1] * m2.mat[1 * 4 + 3] + m1.mat[1 * 4 + 2] * m2.mat[2 * 4 + 3] + m1.mat[1 * 4 + 3];
|
|
|
|
result.mat[2 * 4 + 0] = m1.mat[2 * 4 + 0] * m2.mat[0 * 4 + 0] + m1.mat[2 * 4 + 1] * m2.mat[1 * 4 + 0] + m1.mat[2 * 4 + 2] * m2.mat[2 * 4 + 0];
|
|
result.mat[2 * 4 + 1] = m1.mat[2 * 4 + 0] * m2.mat[0 * 4 + 1] + m1.mat[2 * 4 + 1] * m2.mat[1 * 4 + 1] + m1.mat[2 * 4 + 2] * m2.mat[2 * 4 + 1];
|
|
result.mat[2 * 4 + 2] = m1.mat[2 * 4 + 0] * m2.mat[0 * 4 + 2] + m1.mat[2 * 4 + 1] * m2.mat[1 * 4 + 2] + m1.mat[2 * 4 + 2] * m2.mat[2 * 4 + 2];
|
|
result.mat[2 * 4 + 3] = m1.mat[2 * 4 + 0] * m2.mat[0 * 4 + 3] + m1.mat[2 * 4 + 1] * m2.mat[1 * 4 + 3] + m1.mat[2 * 4 + 2] * m2.mat[2 * 4 + 3] + m1.mat[2 * 4 + 3];
|
|
}
|
|
|
|
ID_INLINE void idJointMat::InverseMultiply( idJointMat &result, const idJointMat &m1, const idJointMat &m2 ) {
|
|
float dst[3];
|
|
|
|
result.mat[0 * 4 + 0] = m1.mat[0 * 4 + 0] * m2.mat[0 * 4 + 0] + m1.mat[0 * 4 + 1] * m2.mat[0 * 4 + 1] + m1.mat[0 * 4 + 2] * m2.mat[0 * 4 + 2];
|
|
result.mat[0 * 4 + 1] = m1.mat[0 * 4 + 0] * m2.mat[1 * 4 + 0] + m1.mat[0 * 4 + 1] * m2.mat[1 * 4 + 1] + m1.mat[0 * 4 + 2] * m2.mat[1 * 4 + 2];
|
|
result.mat[0 * 4 + 2] = m1.mat[0 * 4 + 0] * m2.mat[2 * 4 + 0] + m1.mat[0 * 4 + 1] * m2.mat[2 * 4 + 1] + m1.mat[0 * 4 + 2] * m2.mat[2 * 4 + 2];
|
|
|
|
result.mat[1 * 4 + 0] = m1.mat[1 * 4 + 0] * m2.mat[0 * 4 + 0] + m1.mat[1 * 4 + 1] * m2.mat[0 * 4 + 1] + m1.mat[1 * 4 + 2] * m2.mat[0 * 4 + 2];
|
|
result.mat[1 * 4 + 1] = m1.mat[1 * 4 + 0] * m2.mat[1 * 4 + 0] + m1.mat[1 * 4 + 1] * m2.mat[1 * 4 + 1] + m1.mat[1 * 4 + 2] * m2.mat[1 * 4 + 2];
|
|
result.mat[1 * 4 + 2] = m1.mat[1 * 4 + 0] * m2.mat[2 * 4 + 0] + m1.mat[1 * 4 + 1] * m2.mat[2 * 4 + 1] + m1.mat[1 * 4 + 2] * m2.mat[2 * 4 + 2];
|
|
|
|
result.mat[2 * 4 + 0] = m1.mat[2 * 4 + 0] * m2.mat[0 * 4 + 0] + m1.mat[2 * 4 + 1] * m2.mat[0 * 4 + 1] + m1.mat[2 * 4 + 2] * m2.mat[0 * 4 + 2];
|
|
result.mat[2 * 4 + 1] = m1.mat[2 * 4 + 0] * m2.mat[1 * 4 + 0] + m1.mat[2 * 4 + 1] * m2.mat[1 * 4 + 1] + m1.mat[2 * 4 + 2] * m2.mat[1 * 4 + 2];
|
|
result.mat[2 * 4 + 2] = m1.mat[2 * 4 + 0] * m2.mat[2 * 4 + 0] + m1.mat[2 * 4 + 1] * m2.mat[2 * 4 + 1] + m1.mat[2 * 4 + 2] * m2.mat[2 * 4 + 2];
|
|
|
|
dst[0] = - ( m2.mat[0 * 4 + 0] * m2.mat[0 * 4 + 3] + m2.mat[1 * 4 + 0] * m2.mat[1 * 4 + 3] + m2.mat[2 * 4 + 0] * m2.mat[2 * 4 + 3] );
|
|
dst[1] = - ( m2.mat[0 * 4 + 1] * m2.mat[0 * 4 + 3] + m2.mat[1 * 4 + 1] * m2.mat[1 * 4 + 3] + m2.mat[2 * 4 + 1] * m2.mat[2 * 4 + 3] );
|
|
dst[2] = - ( m2.mat[0 * 4 + 2] * m2.mat[0 * 4 + 3] + m2.mat[1 * 4 + 2] * m2.mat[1 * 4 + 3] + m2.mat[2 * 4 + 2] * m2.mat[2 * 4 + 3] );
|
|
|
|
result.mat[0 * 4 + 3] = m1.mat[0 * 4 + 0] * dst[0] + m1.mat[0 * 4 + 1] * dst[1] + m1.mat[0 * 4 + 2] * dst[2] + m1.mat[0 * 4 + 3];
|
|
result.mat[1 * 4 + 3] = m1.mat[1 * 4 + 0] * dst[0] + m1.mat[1 * 4 + 1] * dst[1] + m1.mat[1 * 4 + 2] * dst[2] + m1.mat[1 * 4 + 3];
|
|
result.mat[2 * 4 + 3] = m1.mat[2 * 4 + 0] * dst[0] + m1.mat[2 * 4 + 1] * dst[1] + m1.mat[2 * 4 + 2] * dst[2] + m1.mat[2 * 4 + 3];
|
|
}
|
|
|
|
|
|
/*
|
|
===============================================================================
|
|
|
|
Joint Frame & Camera Frame
|
|
|
|
===============================================================================
|
|
*/
|
|
|
|
struct jointFrame_t {
|
|
idCQuat q;
|
|
idVec3 t;
|
|
};
|
|
|
|
struct cameraFrame_t {
|
|
idCQuat q;
|
|
idVec3 t;
|
|
float fov;
|
|
};
|
|
|
|
#endif /* !__JOINTTRANSFORM_H__ */
|