mirror of
https://github.com/thug1src/thug.git
synced 2024-12-02 12:56:45 +00:00
352 lines
10 KiB
C++
352 lines
10 KiB
C++
|
/********************************************************************************
|
||
|
|
* *
|
||
|
|
* Module: *
|
||
|
|
* NsMatrix *
|
||
|
|
* Description: *
|
||
|
|
* Matrix functionality. *
|
||
|
|
* Written by: *
|
||
|
|
* Paul Robinson *
|
||
|
|
* Copyright: *
|
||
|
|
* 2001 Neversoft Entertainment - All rights reserved. *
|
||
|
|
* *
|
||
|
|
********************************************************************************/
|
||
|
|
|
||
|
|
/********************************************************************************
|
||
|
|
* Includes. *
|
||
|
|
********************************************************************************/
|
||
|
|
#include <math.h>
|
||
|
|
#include <string.h>
|
||
|
|
#include "p_frame.h"
|
||
|
|
#include "p_matrix.h"
|
||
|
|
#include <core/defines.h>
|
||
|
|
#include <core/math/math.h>
|
||
|
|
|
||
|
|
/********************************************************************************
|
||
|
|
* Defines. *
|
||
|
|
********************************************************************************/
|
||
|
|
|
||
|
|
/********************************************************************************
|
||
|
|
* Structures. *
|
||
|
|
********************************************************************************/
|
||
|
|
|
||
|
|
/********************************************************************************
|
||
|
|
* Local variables. *
|
||
|
|
********************************************************************************/
|
||
|
|
|
||
|
|
/********************************************************************************
|
||
|
|
* Forward references. *
|
||
|
|
********************************************************************************/
|
||
|
|
|
||
|
|
/********************************************************************************
|
||
|
|
* Externs. *
|
||
|
|
********************************************************************************/
|
||
|
|
|
||
|
|
void NsMatrix::translate ( NsVector * v, NsMatrix_Combine c )
|
||
|
|
{
|
||
|
|
Mtx temp;
|
||
|
|
switch (c) {
|
||
|
|
case NsMatrix_Combine_Replace:
|
||
|
|
MTXTrans( m_matrix, v->x, v->y, v->z );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Pre:
|
||
|
|
MTXTrans( temp, v->x, v->y, v->z );
|
||
|
|
MTXConcat( m_matrix, temp, m_matrix );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Post:
|
||
|
|
MTXTrans( temp, v->x, v->y, v->z );
|
||
|
|
MTXConcat( temp, m_matrix, m_matrix );
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::rotate ( NsVector * axis, float angle, NsMatrix_Combine c )
|
||
|
|
{
|
||
|
|
Mtx temp;
|
||
|
|
|
||
|
|
switch (c) {
|
||
|
|
case NsMatrix_Combine_Replace:
|
||
|
|
MTXRotAxisDeg( m_matrix, (Vec*)axis, angle );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Pre:
|
||
|
|
MTXRotAxisDeg( temp, (Vec*)axis, angle );
|
||
|
|
MTXConcat( m_matrix, temp, m_matrix );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Post:
|
||
|
|
MTXRotAxisDeg( temp, (Vec*)axis, angle );
|
||
|
|
MTXConcat( temp, m_matrix, m_matrix );
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::rotateX( float degrees, NsMatrix_Combine c )
|
||
|
|
{
|
||
|
|
Mtx temp;
|
||
|
|
|
||
|
|
switch (c) {
|
||
|
|
case NsMatrix_Combine_Replace:
|
||
|
|
MTXRotDeg(m_matrix, 'x', degrees );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Pre:
|
||
|
|
MTXRotDeg(temp, 'x', degrees );
|
||
|
|
MTXConcat( m_matrix, temp, m_matrix );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Post:
|
||
|
|
MTXRotDeg(temp, 'x', degrees );
|
||
|
|
MTXConcat( temp, m_matrix, m_matrix );
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::rotateY( float degrees, NsMatrix_Combine c )
|
||
|
|
{
|
||
|
|
Mtx temp;
|
||
|
|
|
||
|
|
switch (c) {
|
||
|
|
case NsMatrix_Combine_Replace:
|
||
|
|
MTXRotDeg(m_matrix, 'y', degrees );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Pre:
|
||
|
|
MTXRotDeg(temp, 'y', degrees );
|
||
|
|
MTXConcat( m_matrix, temp, m_matrix );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Post:
|
||
|
|
MTXRotDeg(temp, 'y', degrees );
|
||
|
|
MTXConcat( temp, m_matrix, m_matrix );
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::rotateZ( float degrees, NsMatrix_Combine c )
|
||
|
|
{
|
||
|
|
Mtx temp;
|
||
|
|
|
||
|
|
switch (c) {
|
||
|
|
case NsMatrix_Combine_Replace:
|
||
|
|
MTXRotDeg(m_matrix, 'z', degrees );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Pre:
|
||
|
|
MTXRotDeg(temp, 'z', degrees );
|
||
|
|
MTXConcat( m_matrix, temp, m_matrix );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Post:
|
||
|
|
MTXRotDeg(temp, 'z', degrees );
|
||
|
|
MTXConcat( temp, m_matrix, m_matrix );
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::scale ( NsVector * v, NsMatrix_Combine c )
|
||
|
|
{
|
||
|
|
Mtx temp;
|
||
|
|
switch (c) {
|
||
|
|
case NsMatrix_Combine_Replace:
|
||
|
|
MTXScale( m_matrix, v->x, v->y, v->z );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Pre:
|
||
|
|
MTXScale( temp, v->x, v->y, v->z );
|
||
|
|
MTXConcat( m_matrix, temp, m_matrix );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Post:
|
||
|
|
MTXScale( temp, v->x, v->y, v->z );
|
||
|
|
MTXConcat( temp, m_matrix, m_matrix );
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::identity ( void )
|
||
|
|
{
|
||
|
|
MTXIdentity( m_matrix );
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::getRotation( NsVector * axis, float * angle, NsVector * center )
|
||
|
|
{
|
||
|
|
float nTwoSinTheta, nTwoCosTheta;
|
||
|
|
NsVector vTwoSinThetaAxis;
|
||
|
|
float nRadians;
|
||
|
|
// Mtx mISubMat;
|
||
|
|
// Mtx mISubMatInverse;
|
||
|
|
|
||
|
|
|
||
|
|
nTwoCosTheta = getRight()->x + getUp()->y + getAt()->z - 1.0f;
|
||
|
|
|
||
|
|
// Dave 08/07/01 - Unsure as to exactly why the negation of the axis fixes the slerping problem
|
||
|
|
// (getRotation() is called from NsSlerp::SetMatrices(), but it does. Investigate further at some point...
|
||
|
|
// vTwoSinThetaAxis.x = getUp()->z - getAt()->y;
|
||
|
|
// vTwoSinThetaAxis.y = getAt()->x - getRight()->z;
|
||
|
|
// vTwoSinThetaAxis.z = getRight()->y - getUp()->x;
|
||
|
|
vTwoSinThetaAxis.x = getAt()->y - getUp()->z;
|
||
|
|
vTwoSinThetaAxis.y = getRight()->z - getAt()->x;
|
||
|
|
vTwoSinThetaAxis.z = getUp()->x - getRight()->y;
|
||
|
|
|
||
|
|
nTwoSinTheta = vTwoSinThetaAxis.length();
|
||
|
|
if (nTwoSinTheta > 0.0f)
|
||
|
|
{
|
||
|
|
float recipLength = (1.0f / (nTwoSinTheta));
|
||
|
|
|
||
|
|
axis->scale( vTwoSinThetaAxis, recipLength );
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
axis->set( 0.0f, 0.0f, 0.0f );
|
||
|
|
}
|
||
|
|
nRadians = atan2(nTwoSinTheta, nTwoCosTheta);
|
||
|
|
(*angle) = nRadians * (((float) 180) / ((float) Mth::PI));
|
||
|
|
if ((nTwoSinTheta <= 0.01f) && (nTwoCosTheta <= 0.0f))
|
||
|
|
{
|
||
|
|
/*
|
||
|
|
* sin theta is 0; cos theta is -1; theta is 180 degrees
|
||
|
|
* vTwoSinThetaAxis was degenerate
|
||
|
|
* axis will have to be found another way.
|
||
|
|
*/
|
||
|
|
|
||
|
|
NsVector vTwoSinThetaAxis;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Matrix is:
|
||
|
|
* [ [ 2 a_x^2 - 1, 2 a_x a_y, 2 a_x a_z, 0 ]
|
||
|
|
* [ 2 a_x a_y, 2 a_y^2 - 1, 2 a_y a_z, 0 ]
|
||
|
|
* [ 2 a_x a_z, 2 a_y a_z, 2 a_z^2 - 1, 0 ]
|
||
|
|
* [ 0, 0, 0, 1 ] ]
|
||
|
|
* Build axis scaled by 4 * component of maximum absolute value
|
||
|
|
*/
|
||
|
|
if (getRight()->x > getUp()->y)
|
||
|
|
{
|
||
|
|
if (getRight()->x > getAt()->z)
|
||
|
|
{
|
||
|
|
vTwoSinThetaAxis.x = 1.0f + getRight()->x;
|
||
|
|
vTwoSinThetaAxis.x = vTwoSinThetaAxis.x + vTwoSinThetaAxis.x;
|
||
|
|
vTwoSinThetaAxis.y = getRight()->y + getUp()->x;
|
||
|
|
vTwoSinThetaAxis.z = getRight()->z + getAt()->x;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
vTwoSinThetaAxis.z = 1.0f + getAt()->z;
|
||
|
|
vTwoSinThetaAxis.z = vTwoSinThetaAxis.z + vTwoSinThetaAxis.z;
|
||
|
|
vTwoSinThetaAxis.x = getAt()->x + getRight()->z;
|
||
|
|
vTwoSinThetaAxis.y = getAt()->y + getUp()->z;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
if (getUp()->y > getAt()->z)
|
||
|
|
{
|
||
|
|
vTwoSinThetaAxis.y = 1.0f + getUp()->y;
|
||
|
|
vTwoSinThetaAxis.y = vTwoSinThetaAxis.y + vTwoSinThetaAxis.y;
|
||
|
|
vTwoSinThetaAxis.z = getUp()->z + getAt()->y;
|
||
|
|
vTwoSinThetaAxis.x = getUp()->x + getRight()->y;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
vTwoSinThetaAxis.z = 1.0f + getAt()->z;
|
||
|
|
vTwoSinThetaAxis.z = (vTwoSinThetaAxis.z + vTwoSinThetaAxis.z);
|
||
|
|
vTwoSinThetaAxis.x = getAt()->x + getRight()->z;
|
||
|
|
vTwoSinThetaAxis.y = getAt()->y + getUp()->z;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*
|
||
|
|
* and normalize the axis
|
||
|
|
*/
|
||
|
|
|
||
|
|
axis->normalize( vTwoSinThetaAxis );
|
||
|
|
}
|
||
|
|
|
||
|
|
// /*
|
||
|
|
// * Find center of line of rotation:
|
||
|
|
// * [ v - c ] * R + c = v * R + s
|
||
|
|
// * -> v * R + c - c * R = v * R + s
|
||
|
|
// * -> c * [ I - R ] = s
|
||
|
|
// * -> c = s * [ I - R ] ^ -1
|
||
|
|
// */
|
||
|
|
//
|
||
|
|
// MtxSetIdentity(&mISubMat);
|
||
|
|
// /*
|
||
|
|
// * Find [ I - R ] ^ -1
|
||
|
|
// */
|
||
|
|
// VecSubMacro(&mISubMat.right, &mISubMat.right, &matrix->right);
|
||
|
|
// VecSubMacro(&mISubMat.up, &mISubMat.up, &matrix->up);
|
||
|
|
// VecSubMacro(&mISubMat.at, &mISubMat.at, &matrix->at);
|
||
|
|
// MtxSetFlags(&mISubMat, GENERIC_FLAGS(&mISubMatInverse));
|
||
|
|
// MtxSetFlags(&mISubMatInverse, GENERIC_FLAGS(&mISubMatInverse));
|
||
|
|
// /* MatrixInvertOrthoNormalized(&mISubMatInverse, &mISubMat); */
|
||
|
|
// MatrixInvertGeneric(&mISubMatInverse, &mISubMat);
|
||
|
|
// /*
|
||
|
|
// * Find s * [ I - R ] ^ -1
|
||
|
|
// */
|
||
|
|
// *center = mISubMatInverse.pos;
|
||
|
|
// VecIncrementScaledMacro(center, &mISubMatInverse.right, m[0][3]);
|
||
|
|
// VecIncrementScaledMacro(center, &mISubMatInverse.up, m[1][3]);
|
||
|
|
// VecIncrementScaledMacro(center, &mISubMatInverse.at, m[2][3]);
|
||
|
|
//
|
||
|
|
// RWRETURN(matrix);
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::transform ( NsMatrix& a, NsMatrix_Combine c )
|
||
|
|
{
|
||
|
|
switch (c) {
|
||
|
|
case NsMatrix_Combine_Replace:
|
||
|
|
copy( a );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Pre:
|
||
|
|
MTXConcat( m_matrix, a.m_matrix, m_matrix );
|
||
|
|
break;
|
||
|
|
case NsMatrix_Combine_Post:
|
||
|
|
MTXConcat( a.m_matrix, m_matrix, m_matrix );
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::cat ( NsMatrix& a, NsMatrix& b )
|
||
|
|
{
|
||
|
|
MTXConcat( a.m_matrix, b.m_matrix, m_matrix );
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::invert( void )
|
||
|
|
{
|
||
|
|
MTXInverse( m_matrix, m_matrix );
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::invert( NsMatrix& source )
|
||
|
|
{
|
||
|
|
MTXInverse( source.m_matrix, m_matrix );
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::copy( NsMatrix& source )
|
||
|
|
{
|
||
|
|
memcpy( this, &source, sizeof( NsMatrix ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::lookAt ( NsVector * pPos, NsVector * pUp, NsVector * pTarget )
|
||
|
|
{
|
||
|
|
MTXLookAt( m_matrix, (Vec*)pPos, (Vec*)pUp, (Vec*)pTarget );
|
||
|
|
}
|
||
|
|
|
||
|
|
//void NsMatrix::fromQuat ( NsQuat * pQuat )
|
||
|
|
//{
|
||
|
|
// MTXQuat ( m_matrix, &pQuat->m_quat );
|
||
|
|
//}
|
||
|
|
|
||
|
|
void NsMatrix::multiply( NsVector * pSourceBase, NsVector * pDestBase )
|
||
|
|
{
|
||
|
|
MTXMultVec( m_matrix, (Vec*)pSourceBase, (Vec*)pDestBase );
|
||
|
|
}
|
||
|
|
|
||
|
|
void NsMatrix::multiply( NsVector * pSourceBase, NsVector * pDestBase, int count )
|
||
|
|
{
|
||
|
|
MTXMultVecArray( m_matrix, (Vec*)pSourceBase, (Vec*)pDestBase, count );
|
||
|
|
}
|