673 lines
19 KiB
C++
673 lines
19 KiB
C++
//=======================================================================
|
|
// Copyright (C) Shambler Team 2006
|
|
// vmatrix.h - shared matrix operations
|
|
//=======================================================================
|
|
|
|
#ifndef MATRIX_H
|
|
#define MATRIX_H
|
|
|
|
#include <windows.h>
|
|
#include "basetypes.h"
|
|
#include "vector.h"
|
|
#include "plane.h"
|
|
#include "const.h"
|
|
|
|
struct matrix3x4_t
|
|
{
|
|
matrix3x4_t() {}
|
|
matrix3x4_t(
|
|
float m00, float m01, float m02, float m03,
|
|
float m10, float m11, float m12, float m13,
|
|
float m20, float m21, float m22, float m23 )
|
|
{
|
|
m_flMatVal[0][0] = m00; m_flMatVal[0][1] = m01; m_flMatVal[0][2] = m02; m_flMatVal[0][3] = m03;
|
|
m_flMatVal[1][0] = m10; m_flMatVal[1][1] = m11; m_flMatVal[1][2] = m12; m_flMatVal[1][3] = m13;
|
|
m_flMatVal[2][0] = m20; m_flMatVal[2][1] = m21; m_flMatVal[2][2] = m22; m_flMatVal[2][3] = m23;
|
|
}
|
|
|
|
float *operator[]( int i ) { return m_flMatVal[i]; }
|
|
const float *operator[]( int i ) const { return m_flMatVal[i]; }
|
|
float *Base() { return &m_flMatVal[0][0]; }
|
|
const float *Base() const { return &m_flMatVal[0][0]; }
|
|
|
|
float m_flMatVal[3][4];
|
|
};
|
|
|
|
class VMatrix
|
|
{
|
|
public:
|
|
VMatrix ();
|
|
VMatrix(
|
|
vec_t m00, vec_t m01, vec_t m02, vec_t m03,
|
|
vec_t m10, vec_t m11, vec_t m12, vec_t m13,
|
|
vec_t m20, vec_t m21, vec_t m22, vec_t m23,
|
|
vec_t m30, vec_t m31, vec_t m32, vec_t m33
|
|
);
|
|
|
|
// Creates a matrix where the X axis = forward
|
|
// the Y axis = left, and the Z axis = up
|
|
VMatrix( const Vector& forward, const Vector& left, const Vector& up );
|
|
|
|
// Construct from a 3x4 matrix
|
|
VMatrix( const matrix3x4_t& matrix3x4 );
|
|
|
|
// Set the values in the matrix.
|
|
void Init(
|
|
vec_t m00, vec_t m01, vec_t m02, vec_t m03,
|
|
vec_t m10, vec_t m11, vec_t m12, vec_t m13,
|
|
vec_t m20, vec_t m21, vec_t m22, vec_t m23,
|
|
vec_t m30, vec_t m31, vec_t m32, vec_t m33
|
|
);
|
|
|
|
// Initialize from a 3x4
|
|
void Init( const matrix3x4_t& matrix3x4 );
|
|
|
|
// array access
|
|
float* operator[](int i) { return m[i]; }
|
|
float const* operator[](int i) const { return m[i]; }
|
|
|
|
// Access the basis vectors.
|
|
Vector GetLeft() const;
|
|
Vector GetUp() const;
|
|
Vector GetForward() const;
|
|
|
|
void SetLeft(const Vector &vLeft);
|
|
void SetUp(const Vector &vUp);
|
|
void SetForward(const Vector &vForward);
|
|
|
|
void GetBasisVectors(Vector &vForward, Vector &vLeft, Vector &vUp) const;
|
|
void SetBasisVectors(const Vector &vForward, const Vector &vLeft, const Vector &vUp);
|
|
|
|
// Get/set the translation.
|
|
Vector GetTranslation() const;
|
|
void SetTranslation(const Vector &vTrans);
|
|
|
|
void PreTranslate(const Vector &vTrans);
|
|
void PostTranslate(const Vector &vTrans);
|
|
|
|
matrix3x4_t& As3x4();
|
|
const matrix3x4_t& As3x4() const;
|
|
void CopyFrom3x4( const matrix3x4_t &m3x4 );
|
|
void Set3x4( matrix3x4_t& matrix3x4 ) const;
|
|
|
|
// Matrix->vector operations.
|
|
public:
|
|
|
|
// Multiply by a vector (divides by w, assumes input w is 1).
|
|
Vector operator*(const Vector &vVec) const;
|
|
|
|
// Multiply by the upper 3x3 part of the matrix (ie: only apply rotation).
|
|
Vector VMul3x3(const Vector &vVec) const;
|
|
|
|
// Apply the inverse (transposed) rotation (only works on pure rotation matrix)
|
|
Vector VMul3x3Transpose(const Vector &vVec) const;
|
|
|
|
// Multiply by the upper 3 rows.
|
|
Vector VMul4x3(const Vector &vVec) const;
|
|
|
|
// Apply the inverse (transposed) transformation (only works on pure rotation/translation)
|
|
Vector VMul4x3Transpose(const Vector &vVec) const;
|
|
|
|
// Multiply by a 3D vector (same as operator*).
|
|
void V3Mul(const Vector &vIn, Vector &vOut) const;
|
|
|
|
// Multiply by a 4D vector.
|
|
void V4Mul(const Vector4D &vIn, Vector4D &vOut) const;
|
|
|
|
// Applies the rotation (ignores translation in the matrix). (This just calls VMul3x3).
|
|
Vector ApplyRotation(const Vector &vVec) const;
|
|
|
|
|
|
// Matrix->plane operations.
|
|
public:
|
|
// Transform the plane. The matrix can only contain translation and rotation.
|
|
void TransformPlane( const VPlane &inPlane, VPlane &outPlane ) const;
|
|
|
|
// Just calls TransformPlane and returns the result.
|
|
VPlane operator*(const VPlane &thePlane) const;
|
|
|
|
|
|
// Matrix->matrix operations.
|
|
public:
|
|
|
|
VMatrix& operator=(const VMatrix &mOther);
|
|
|
|
// Multiply two matrices (out = this * vm).
|
|
void MatrixMul( const VMatrix &vm, VMatrix &out ) const;
|
|
|
|
// Just calls MatrixMul and returns the result.
|
|
VMatrix operator*(const VMatrix &mOther) const;
|
|
|
|
// Add two matrices.
|
|
VMatrix const& operator+=(const VMatrix &other);
|
|
|
|
// Add/Subtract two matrices.
|
|
VMatrix operator+(const VMatrix &other) const;
|
|
VMatrix operator-(const VMatrix &other) const;
|
|
|
|
// Negation.
|
|
VMatrix operator-() const;
|
|
|
|
// Return inverse matrix. Be careful because the results are undefined
|
|
// if the matrix doesn't have an inverse (ie: InverseGeneral returns false).
|
|
VMatrix operator~() const;
|
|
|
|
|
|
// Matrix operations.
|
|
public:
|
|
|
|
// Set to identity.
|
|
void Identity();
|
|
|
|
// Setup a matrix for origin and angles.
|
|
void SetupMatrixOrgAngles( const Vector &origin, const Vector &vAngles );
|
|
|
|
// General inverse. This may fail so check the return!
|
|
bool InverseGeneral(VMatrix &vInverse) const;
|
|
|
|
// Does a fast inverse, assuming the matrix only contains translation and rotation.
|
|
void InverseTR( VMatrix &mRet ) const;
|
|
VMatrix InverseTR() const; // This calls the other InverseTR and returns the result.
|
|
|
|
// Get the scale of the matrix's basis vectors.
|
|
Vector GetScale() const;
|
|
|
|
// (Fast) multiply by a scaling matrix setup from vScale.
|
|
VMatrix Scale(const Vector &vScale);
|
|
|
|
// Normalize the basis vectors.
|
|
VMatrix NormalizeBasisVectors() const;
|
|
|
|
// Transpose.
|
|
VMatrix Transpose() const;
|
|
|
|
// Transpose upper-left 3x3.
|
|
VMatrix Transpose3x3() const;
|
|
|
|
// Usually used for debug checks. Returns true if the upper 3x3 contains
|
|
// unit vectors and they are all orthogonal.
|
|
bool IsRotationMatrix() const;
|
|
|
|
|
|
public:
|
|
|
|
// The matrix.
|
|
vec_t m[4][4];
|
|
};
|
|
|
|
// ------------------------------------------------------------------------------------------- //
|
|
// Helper functions.
|
|
// ------------------------------------------------------------------------------------------- //
|
|
|
|
|
|
// Setup an identity matrix.
|
|
VMatrix SetupMatrixIdentity();
|
|
|
|
// Setup as a scaling matrix.
|
|
VMatrix SetupMatrixScale(const Vector &vScale);
|
|
|
|
// Setup a translation matrix.
|
|
VMatrix SetupMatrixTranslation(const Vector &vTranslation);
|
|
|
|
// Setup a matrix to reflect around the plane.
|
|
VMatrix SetupMatrixReflection(const VPlane &thePlane);
|
|
|
|
// Setup a matrix to project from vOrigin onto thePlane.
|
|
VMatrix SetupMatrixProjection(const Vector &vOrigin, const VPlane &thePlane);
|
|
|
|
// Setup a matrix to rotate the specified amount around the specified axis.
|
|
VMatrix SetupMatrixAxisRot(const Vector &vAxis, vec_t fDegrees);
|
|
|
|
// Setup a matrix from euler angles. Just sets identity and calls MatrixAngles.
|
|
VMatrix SetupMatrixAngles(const Vector &vAngles);
|
|
|
|
// Setup a matrix for origin and angles.
|
|
VMatrix SetupMatrixOrgAngles(const Vector &origin, const Vector &vAngles);
|
|
|
|
// Returns the point at the intersection on the 3 planes.
|
|
// Returns false if it can't be solved (2 or more planes are parallel).
|
|
bool PlaneIntersection(
|
|
const VPlane &vp1,
|
|
const VPlane &vp2,
|
|
const VPlane &vp3,
|
|
Vector &vOut
|
|
);
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// These methods are faster. Use them if you want faster code
|
|
//-----------------------------------------------------------------------------
|
|
|
|
void MatrixSetIdentity( VMatrix &dst );
|
|
void MatrixTranspose( const VMatrix& src, VMatrix& dst );
|
|
void MatrixCopy( const VMatrix& src, VMatrix& dst );
|
|
void MatrixCopy( const matrix3x4_t &in, matrix3x4_t &out );
|
|
void MatrixInvert( const matrix3x4_t& in, matrix3x4_t& out );
|
|
void MatrixMultiply( const VMatrix& src1, const VMatrix& src2, VMatrix& dst );
|
|
|
|
void ConcatTransforms (const matrix3x4_t& in1, const matrix3x4_t& in2, matrix3x4_t& out);
|
|
|
|
// Accessors
|
|
void MatrixGetColumn( const VMatrix &src, int nCol, Vector *pColumn );
|
|
void MatrixSetColumn( VMatrix &src, int nCol, const Vector &column );
|
|
|
|
// Vector3DMultiply treats src2 as if it's a direction vector
|
|
void Vector3DMultiply( const VMatrix& src1, const Vector& src2, Vector& dst );
|
|
|
|
// Vector3DMultiplyPosition treats src2 as if it's a point (adds the translation)
|
|
inline void Vector3DMultiplyPosition( const VMatrix& src1, const Vector src2, Vector& dst );
|
|
|
|
// Vector3DMultiplyPositionProjective treats src2 as if it's a point
|
|
// and does the perspective divide at the end
|
|
void Vector3DMultiplyPositionProjective( const VMatrix& src1, const Vector &src2, Vector& dst );
|
|
|
|
// Vector3DMultiplyPosition treats src2 as if it's a direction
|
|
// and does the perspective divide at the end
|
|
// NOTE: src1 had better be an inverse transpose to use this correctly
|
|
void Vector3DMultiplyProjective( const VMatrix& src1, const Vector &src2, Vector& dst );
|
|
|
|
void Vector4DMultiply( const VMatrix& src1, const Vector4D& src2, Vector4D& dst );
|
|
|
|
// Multiplies the vector by the transpose of the matrix
|
|
void Vector3DMultiplyTranspose( const VMatrix& src1, const Vector& src2, Vector& dst );
|
|
void Vector4DMultiplyTranspose( const VMatrix& src1, const Vector4D& src2, Vector4D& dst );
|
|
|
|
void MatrixBuildTranslation( VMatrix& dst, float x, float y, float z );
|
|
void MatrixBuildTranslation( VMatrix& dst, const Vector &translation );
|
|
|
|
void MatrixBuildRotationAboutAxis( VMatrix& dst, const Vector& vAxisOfRot, float angleDegrees );
|
|
void MatrixBuildRotateZ( VMatrix& dst, float angleDegrees );
|
|
|
|
// Builds a rotation matrix that rotates one direction vector into another
|
|
void MatrixBuildRotation( VMatrix &dst, const Vector& initialDirection, const Vector& finalDirection );
|
|
|
|
// Builds a scale matrix
|
|
void MatrixBuildScale( VMatrix &dst, float x, float y, float z );
|
|
void MatrixBuildScale( VMatrix &dst, const Vector& scale );
|
|
|
|
// Setup a matrix from euler angles.
|
|
void MatrixFromAngles( const Vector& vAngles, VMatrix& dst );
|
|
|
|
// Creates euler angles from a matrix
|
|
void MatrixToAngles( const VMatrix& src, Vector& vAngles );
|
|
|
|
// Does a fast inverse, assuming the matrix only contains translation and rotation.
|
|
void MatrixInverseTR( const VMatrix& src, VMatrix &dst );
|
|
|
|
// Inverts any matrix at all
|
|
bool MatrixInverseGeneral(const VMatrix& src, VMatrix& dst);
|
|
|
|
// Computes the inverse transpose
|
|
void MatrixInverseTranspose( const VMatrix& src, VMatrix& dst );
|
|
|
|
// ------------------------------------------------------------------------------------------- //
|
|
// VMatrix inlines.
|
|
// ------------------------------------------------------------------------------------------- //
|
|
inline VMatrix::VMatrix()
|
|
{
|
|
}
|
|
|
|
inline VMatrix::VMatrix(
|
|
vec_t m00, vec_t m01, vec_t m02, vec_t m03,
|
|
vec_t m10, vec_t m11, vec_t m12, vec_t m13,
|
|
vec_t m20, vec_t m21, vec_t m22, vec_t m23,
|
|
vec_t m30, vec_t m31, vec_t m32, vec_t m33)
|
|
{
|
|
Init(
|
|
m00, m01, m02, m03,
|
|
m10, m11, m12, m13,
|
|
m20, m21, m22, m23,
|
|
m30, m31, m32, m33
|
|
);
|
|
}
|
|
|
|
|
|
inline VMatrix::VMatrix( const matrix3x4_t& matrix3x4 )
|
|
{
|
|
Init( matrix3x4 );
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Creates a matrix where the X axis = forward
|
|
// the Y axis = left, and the Z axis = up
|
|
//-----------------------------------------------------------------------------
|
|
|
|
inline VMatrix::VMatrix( const Vector& xAxis, const Vector& yAxis, const Vector& zAxis )
|
|
{
|
|
Init(
|
|
xAxis.x, yAxis.x, zAxis.x, 0.0f,
|
|
xAxis.y, yAxis.y, zAxis.y, 0.0f,
|
|
xAxis.z, yAxis.z, zAxis.z, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f
|
|
);
|
|
}
|
|
|
|
|
|
inline void VMatrix::Init(
|
|
vec_t m00, vec_t m01, vec_t m02, vec_t m03,
|
|
vec_t m10, vec_t m11, vec_t m12, vec_t m13,
|
|
vec_t m20, vec_t m21, vec_t m22, vec_t m23,
|
|
vec_t m30, vec_t m31, vec_t m32, vec_t m33
|
|
)
|
|
{
|
|
m[0][0] = m00;
|
|
m[0][1] = m01;
|
|
m[0][2] = m02;
|
|
m[0][3] = m03;
|
|
|
|
m[1][0] = m10;
|
|
m[1][1] = m11;
|
|
m[1][2] = m12;
|
|
m[1][3] = m13;
|
|
|
|
m[2][0] = m20;
|
|
m[2][1] = m21;
|
|
m[2][2] = m22;
|
|
m[2][3] = m23;
|
|
|
|
m[3][0] = m30;
|
|
m[3][1] = m31;
|
|
m[3][2] = m32;
|
|
m[3][3] = m33;
|
|
}
|
|
|
|
// Initialize from a 3x4
|
|
inline void VMatrix::Init( const matrix3x4_t& matrix3x4 )
|
|
{
|
|
memcpy(m, matrix3x4.Base(), sizeof( matrix3x4_t ) );
|
|
|
|
m[3][0] = 0.0f;
|
|
m[3][1] = 0.0f;
|
|
m[3][2] = 0.0f;
|
|
m[3][3] = 1.0f;
|
|
|
|
}
|
|
|
|
inline Vector VMatrix::GetForward() const
|
|
{
|
|
return Vector(m[0][0], m[1][0], m[2][0]);
|
|
}
|
|
|
|
inline Vector VMatrix::GetLeft() const
|
|
{
|
|
return Vector(m[0][1], m[1][1], m[2][1]);
|
|
}
|
|
|
|
inline Vector VMatrix::GetUp() const
|
|
{
|
|
return Vector(m[0][2], m[1][2], m[2][2]);
|
|
}
|
|
|
|
inline void VMatrix::SetForward(const Vector &vForward)
|
|
{
|
|
m[0][0] = vForward.x;
|
|
m[1][0] = vForward.y;
|
|
m[2][0] = vForward.z;
|
|
}
|
|
|
|
inline void VMatrix::SetLeft(const Vector &vLeft)
|
|
{
|
|
m[0][1] = vLeft.x;
|
|
m[1][1] = vLeft.y;
|
|
m[2][1] = vLeft.z;
|
|
}
|
|
|
|
inline void VMatrix::SetUp(const Vector &vUp)
|
|
{
|
|
m[0][2] = vUp.x;
|
|
m[1][2] = vUp.y;
|
|
m[2][2] = vUp.z;
|
|
}
|
|
|
|
inline void VMatrix::GetBasisVectors(Vector &vForward, Vector &vLeft, Vector &vUp) const
|
|
{
|
|
vForward = GetForward();
|
|
vLeft = GetLeft();
|
|
vUp = GetUp();
|
|
}
|
|
|
|
inline void VMatrix::SetBasisVectors(const Vector &vForward, const Vector &vLeft, const Vector &vUp)
|
|
{
|
|
SetForward(vForward);
|
|
SetLeft(vLeft);
|
|
SetUp(vUp);
|
|
}
|
|
|
|
inline Vector VMatrix::GetTranslation() const
|
|
{
|
|
return Vector(m[0][3], m[1][3], m[2][3]);
|
|
}
|
|
|
|
inline void VMatrix::SetTranslation(const Vector &vTrans)
|
|
{
|
|
m[0][3] = vTrans.x;
|
|
m[1][3] = vTrans.y;
|
|
m[2][3] = vTrans.z;
|
|
}
|
|
|
|
// appply translation to this matrix in the input space
|
|
inline void VMatrix::PreTranslate(const Vector &vTrans)
|
|
{
|
|
Vector tmp = VMul4x3( vTrans );
|
|
m[0][3] = tmp.x;
|
|
m[1][3] = tmp.y;
|
|
m[2][3] = tmp.z;
|
|
}
|
|
|
|
// appply translation to this matrix in the output space
|
|
inline void VMatrix::PostTranslate(const Vector &vTrans)
|
|
{
|
|
m[0][3] += vTrans.x;
|
|
m[1][3] += vTrans.y;
|
|
m[2][3] += vTrans.z;
|
|
}
|
|
inline const matrix3x4_t& VMatrix::As3x4() const
|
|
{
|
|
return *((const matrix3x4_t*)this);
|
|
}
|
|
|
|
inline matrix3x4_t& VMatrix::As3x4()
|
|
{
|
|
return *((matrix3x4_t*)this);
|
|
}
|
|
|
|
inline void VMatrix::CopyFrom3x4( const matrix3x4_t &m3x4 )
|
|
{
|
|
memcpy( m, m3x4.Base(), sizeof( matrix3x4_t ) );
|
|
m[3][0] = m[3][1] = m[3][2] = 0;
|
|
m[3][3] = 1;
|
|
}
|
|
|
|
inline void VMatrix::Set3x4( matrix3x4_t& matrix3x4 ) const
|
|
{
|
|
memcpy(matrix3x4.Base(), m, sizeof( matrix3x4_t ) );
|
|
}
|
|
|
|
inline Vector VMatrix::operator*(const Vector &vVec) const
|
|
{
|
|
Vector vRet;
|
|
vRet.x = m[0][0]*vVec.x + m[0][1]*vVec.y + m[0][2]*vVec.z + m[0][3];
|
|
vRet.y = m[1][0]*vVec.x + m[1][1]*vVec.y + m[1][2]*vVec.z + m[1][3];
|
|
vRet.z = m[2][0]*vVec.x + m[2][1]*vVec.y + m[2][2]*vVec.z + m[2][3];
|
|
|
|
return vRet;
|
|
}
|
|
|
|
inline const VMatrix& VMatrix::operator+=(const VMatrix &other)
|
|
{
|
|
for(int i=0; i < 4; i++)
|
|
{
|
|
for(int j=0; j < 4; j++)
|
|
{
|
|
m[i][j] += other.m[i][j];
|
|
}
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
inline VMatrix VMatrix::operator+(const VMatrix &other) const
|
|
{
|
|
VMatrix ret;
|
|
for(int i=0; i < 16; i++)
|
|
{
|
|
((float*)ret.m)[i] = ((float*)m)[i] + ((float*)other.m)[i];
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
inline VMatrix VMatrix::operator-(const VMatrix &other) const
|
|
{
|
|
VMatrix ret;
|
|
|
|
for(int i=0; i < 4; i++)
|
|
{
|
|
for(int j=0; j < 4; j++)
|
|
{
|
|
ret.m[i][j] = m[i][j] - other.m[i][j];
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
inline VMatrix VMatrix::operator-() const
|
|
{
|
|
VMatrix ret;
|
|
for( int i=0; i < 16; i++ )
|
|
{
|
|
((float*)ret.m)[i] = ((float*)m)[i];
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
inline Vector VMatrix::VMul4x3(const Vector &vVec) const
|
|
{
|
|
return Vector(
|
|
m[0][0]*vVec.x + m[0][1]*vVec.y + m[0][2]*vVec.z + m[0][3],
|
|
m[1][0]*vVec.x + m[1][1]*vVec.y + m[1][2]*vVec.z + m[1][3],
|
|
m[2][0]*vVec.x + m[2][1]*vVec.y + m[2][2]*vVec.z + m[2][3]
|
|
);
|
|
}
|
|
|
|
|
|
inline Vector VMatrix::VMul4x3Transpose(const Vector &vVec) const
|
|
{
|
|
Vector tmp = vVec;
|
|
tmp.x -= m[0][3];
|
|
tmp.y -= m[1][3];
|
|
tmp.z -= m[2][3];
|
|
|
|
return Vector(
|
|
m[0][0]*tmp.x + m[1][0]*tmp.y + m[2][0]*tmp.z,
|
|
m[0][1]*tmp.x + m[1][1]*tmp.y + m[2][1]*tmp.z,
|
|
m[0][2]*tmp.x + m[1][2]*tmp.y + m[2][2]*tmp.z
|
|
);
|
|
}
|
|
|
|
inline Vector VMatrix::VMul3x3(const Vector &vVec) const
|
|
{
|
|
return Vector(
|
|
m[0][0]*vVec.x + m[0][1]*vVec.y + m[0][2]*vVec.z,
|
|
m[1][0]*vVec.x + m[1][1]*vVec.y + m[1][2]*vVec.z,
|
|
m[2][0]*vVec.x + m[2][1]*vVec.y + m[2][2]*vVec.z
|
|
);
|
|
}
|
|
|
|
inline Vector VMatrix::VMul3x3Transpose(const Vector &vVec) const
|
|
{
|
|
return Vector(
|
|
m[0][0]*vVec.x + m[1][0]*vVec.y + m[2][0]*vVec.z,
|
|
m[0][1]*vVec.x + m[1][1]*vVec.y + m[2][1]*vVec.z,
|
|
m[0][2]*vVec.x + m[1][2]*vVec.y + m[2][2]*vVec.z
|
|
);
|
|
}
|
|
|
|
inline void VMatrix::V3Mul(const Vector &vIn, Vector &vOut) const
|
|
{
|
|
vec_t rw;
|
|
|
|
rw = 1.0f / (m[3][0]*vIn.x + m[3][1]*vIn.y + m[3][2]*vIn.z + m[3][3]);
|
|
vOut.x = (m[0][0]*vIn.x + m[0][1]*vIn.y + m[0][2]*vIn.z + m[0][3]) * rw;
|
|
vOut.y = (m[1][0]*vIn.x + m[1][1]*vIn.y + m[1][2]*vIn.z + m[1][3]) * rw;
|
|
vOut.z = (m[2][0]*vIn.x + m[2][1]*vIn.y + m[2][2]*vIn.z + m[2][3]) * rw;
|
|
}
|
|
|
|
inline void VMatrix::V4Mul(const Vector4D &vIn, Vector4D &vOut) const
|
|
{
|
|
vOut[0] = m[0][0]*vIn[0] + m[0][1]*vIn[1] + m[0][2]*vIn[2] + m[0][3]*vIn[3];
|
|
vOut[1] = m[1][0]*vIn[0] + m[1][1]*vIn[1] + m[1][2]*vIn[2] + m[1][3]*vIn[3];
|
|
vOut[2] = m[2][0]*vIn[0] + m[2][1]*vIn[1] + m[2][2]*vIn[2] + m[2][3]*vIn[3];
|
|
vOut[3] = m[3][0]*vIn[0] + m[3][1]*vIn[1] + m[3][2]*vIn[2] + m[3][3]*vIn[3];
|
|
}
|
|
|
|
inline Vector VMatrix::ApplyRotation(const Vector &vVec) const
|
|
{
|
|
return VMul3x3(vVec);
|
|
}
|
|
|
|
inline void VMatrix::TransformPlane( const VPlane &inPlane, VPlane &outPlane ) const
|
|
{
|
|
Vector vPlanePt, vTemp;
|
|
|
|
outPlane.m_Normal = VMul3x3( inPlane.m_Normal );
|
|
|
|
vPlanePt = inPlane.GetPointOnPlane();
|
|
vTemp = VMul4x3( vPlanePt );
|
|
outPlane.m_Dist = DotProduct( outPlane.m_Normal, vTemp );
|
|
}
|
|
|
|
inline VMatrix VMatrix::operator~() const
|
|
{
|
|
VMatrix mRet;
|
|
InverseGeneral(mRet);
|
|
return mRet;
|
|
}
|
|
|
|
inline void VMatrix::Identity()
|
|
{
|
|
*this = SetupMatrixIdentity();
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Accessors
|
|
//-----------------------------------------------------------------------------
|
|
inline void MatrixGetColumn( const VMatrix &src, int nCol, Vector *pColumn )
|
|
{
|
|
(*pColumn)[0] = src[0][nCol];
|
|
(*pColumn)[1] = src[1][nCol];
|
|
(*pColumn)[2] = src[2][nCol];
|
|
}
|
|
|
|
inline void MatrixSetColumn( VMatrix &src, int nCol, const Vector &column )
|
|
{
|
|
src.m[0][nCol] = column.x;
|
|
src.m[1][nCol] = column.y;
|
|
src.m[2][nCol] = column.z;
|
|
}
|
|
|
|
inline void MatrixSetColumn( const Vector &in, int column, matrix3x4_t& out )
|
|
{
|
|
out[0][column] = in.x;
|
|
out[1][column] = in.y;
|
|
out[2][column] = in.z;
|
|
}
|
|
|
|
//misc utils
|
|
void VectorITransform (const float *in1, const matrix3x4_t& in2, float *out);
|
|
void VectorTransform (const float *in1, const matrix3x4_t& in2, float *out);
|
|
void VectorIRotate( const float *in1, const matrix3x4_t& in2, float *out );
|
|
void VectorRotate( const float *in1, const matrix3x4_t& in2, float *out );
|
|
void SetIdentityMatrix( matrix3x4_t& matrix );
|
|
|
|
void MatrixAngles( const matrix3x4_t & matrix, float *angles ); // !!!!
|
|
void AngleMatrix( const Vector &angles, matrix3x4_t& matrix );
|
|
void AngleMatrix( const Vector &angles, const Vector &position, matrix3x4_t& matrix );
|
|
|
|
inline void MatrixAngles( const matrix3x4_t &matrix, Vector &angles )
|
|
{
|
|
MatrixAngles( matrix, &angles.x );
|
|
}
|
|
|
|
#endif
|