mirror of
https://port.numenaute.org/aleajactaest/khanat-opennel-code.git
synced 2024-12-16 22:38:42 +00:00
1569 lines
38 KiB
C++
1569 lines
38 KiB
C++
// NeL - MMORPG Framework <http://dev.ryzom.com/projects/nel/>
|
|
// Copyright (C) 2010 Winch Gate Property Limited
|
|
//
|
|
// This program is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU Affero General Public License as
|
|
// published by the Free Software Foundation, either version 3 of the
|
|
// License, or (at your option) any later version.
|
|
//
|
|
// This program 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 Affero General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU Affero General Public License
|
|
// along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
#include "stdmisc.h"
|
|
|
|
#include "nel/misc/matrix.h"
|
|
#include "nel/misc/plane.h"
|
|
#include "nel/misc/debug.h"
|
|
|
|
using namespace std;
|
|
|
|
|
|
#ifdef DEBUG_NEW
|
|
#define new DEBUG_NEW
|
|
#endif
|
|
|
|
namespace NLMISC
|
|
{
|
|
|
|
|
|
// ======================================================================================================
|
|
const CMatrix CMatrix::Identity;
|
|
|
|
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
|
|
|
|
// State Bits.
|
|
#define MAT_TRANS 1
|
|
#define MAT_ROT 2
|
|
#define MAT_SCALEUNI 4
|
|
#define MAT_SCALEANY 8
|
|
#define MAT_PROJ 16
|
|
// Validity bits. These means that the part may be yet identity, but is valid in the floats.
|
|
// NB: MAT_VALIDTRANS no more used for faster Pos access
|
|
#define MAT_VALIDROT 64
|
|
#define MAT_VALIDPROJ 128
|
|
#define MAT_VALIDALL (MAT_VALIDROT | MAT_VALIDPROJ)
|
|
// The identity is nothing.
|
|
#define MAT_IDENTITY 0
|
|
|
|
|
|
|
|
// Matrix elements.
|
|
#define a11 M[0]
|
|
#define a21 M[1]
|
|
#define a31 M[2]
|
|
#define a41 M[3]
|
|
#define a12 M[4]
|
|
#define a22 M[5]
|
|
#define a32 M[6]
|
|
#define a42 M[7]
|
|
#define a13 M[8]
|
|
#define a23 M[9]
|
|
#define a33 M[10]
|
|
#define a43 M[11]
|
|
#define a14 M[12]
|
|
#define a24 M[13]
|
|
#define a34 M[14]
|
|
#define a44 M[15]
|
|
|
|
|
|
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
|
|
|
|
|
|
// ======================================================================================================
|
|
bool CMatrix::hasScalePart() const
|
|
{
|
|
return (StateBit&(MAT_SCALEUNI|MAT_SCALEANY))!=0;
|
|
}
|
|
bool CMatrix::hasProjectionPart() const
|
|
{
|
|
return (StateBit&MAT_PROJ)!=0;
|
|
}
|
|
|
|
|
|
bool CMatrix::hasScaleUniform() const
|
|
{
|
|
return (StateBit & (MAT_SCALEUNI|MAT_SCALEANY))== MAT_SCALEUNI;
|
|
}
|
|
float CMatrix::getScaleUniform() const
|
|
{
|
|
if(hasScaleUniform())
|
|
return Scale33;
|
|
else
|
|
return 1;
|
|
}
|
|
|
|
|
|
|
|
// ======================================================================================================
|
|
inline bool CMatrix::hasRot() const
|
|
{
|
|
return (StateBit&(MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY))!=0;
|
|
}
|
|
inline bool CMatrix::hasTrans() const
|
|
{
|
|
return (StateBit&MAT_TRANS)!=0;
|
|
}
|
|
inline bool CMatrix::hasProj() const
|
|
{
|
|
return (StateBit&MAT_PROJ)!=0;
|
|
}
|
|
inline bool CMatrix::hasAll() const
|
|
{
|
|
return (hasRot() && hasTrans() && hasProj());
|
|
}
|
|
|
|
|
|
inline void CMatrix::testExpandRot() const
|
|
{
|
|
if(hasRot())
|
|
return;
|
|
if(!(StateBit&MAT_VALIDROT))
|
|
{
|
|
CMatrix *self= const_cast<CMatrix*>(this);
|
|
self->StateBit|=MAT_VALIDROT;
|
|
self->a11= 1; self->a12=0; self->a13=0;
|
|
self->a21= 0; self->a22=1; self->a23=0;
|
|
self->a31= 0; self->a32=0; self->a33=1;
|
|
self->Scale33= 1;
|
|
}
|
|
}
|
|
|
|
inline void CMatrix::testExpandProj() const
|
|
{
|
|
if(hasProj())
|
|
return;
|
|
if(!(StateBit&MAT_VALIDPROJ))
|
|
{
|
|
CMatrix *self= const_cast<CMatrix*>(this);
|
|
self->StateBit|=MAT_VALIDPROJ;
|
|
self->a41=0; self->a42=0; self->a43=0; self->a44=1;
|
|
}
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
CMatrix::CMatrix(const CMatrix &m)
|
|
{
|
|
(*this)= m;
|
|
}
|
|
// ======================================================================================================
|
|
CMatrix &CMatrix::operator=(const CMatrix &m)
|
|
{
|
|
StateBit= m.StateBit & ~MAT_VALIDALL;
|
|
if(hasAll())
|
|
{
|
|
memcpy(M, m.M, 16*sizeof(float));
|
|
Scale33= m.Scale33;
|
|
}
|
|
else
|
|
{
|
|
if(hasRot())
|
|
{
|
|
memcpy(&a11, &m.a11, 3*sizeof(float));
|
|
memcpy(&a12, &m.a12, 3*sizeof(float));
|
|
memcpy(&a13, &m.a13, 3*sizeof(float));
|
|
Scale33= m.Scale33;
|
|
}
|
|
if(hasProj())
|
|
{
|
|
a41= m.a41;
|
|
a42= m.a42;
|
|
a43= m.a43;
|
|
a44= m.a44;
|
|
}
|
|
// Must always copy Trans part.
|
|
memcpy(&a14, &m.a14, 3*sizeof(float));
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::identity()
|
|
{
|
|
StateBit= MAT_IDENTITY;
|
|
// Reset just Pos because must always be valid for faster getPos()
|
|
a14= a24= a34= 0;
|
|
// For optimisation it would be useful to keep MAT_VALID states.
|
|
// But this slows identity(), and this may not be interesting...
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::setRot(const CVector &i, const CVector &j, const CVector &k, bool hintNoScale)
|
|
{
|
|
StateBit|= MAT_ROT | MAT_SCALEANY;
|
|
if(hintNoScale)
|
|
StateBit&= ~(MAT_SCALEANY|MAT_SCALEUNI);
|
|
a11= i.x; a12= j.x; a13= k.x;
|
|
a21= i.y; a22= j.y; a23= k.y;
|
|
a31= i.z; a32= j.z; a33= k.z;
|
|
Scale33= 1.0f;
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::setRot(const float m33[9], bool hintNoScale)
|
|
{
|
|
StateBit|= MAT_ROT | MAT_SCALEANY;
|
|
if(hintNoScale)
|
|
StateBit&= ~(MAT_SCALEANY|MAT_SCALEUNI);
|
|
a11= m33[0]; a12= m33[3]; a13= m33[6];
|
|
a21= m33[1]; a22= m33[4]; a23= m33[7];
|
|
a31= m33[2]; a32= m33[5]; a33= m33[8];
|
|
Scale33= 1.0f;
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::setRot(const CVector &v, TRotOrder ro)
|
|
{
|
|
CMatrix rot;
|
|
rot.identity();
|
|
rot.rotate(v, ro);
|
|
float m33[9];
|
|
rot.getRot(m33);
|
|
setRot(m33, true);
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::setRot(const CMatrix &matrix)
|
|
{
|
|
// copy rotpart statebit from other.
|
|
StateBit&= ~(MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY);
|
|
StateBit|= matrix.StateBit & (MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY);
|
|
// copy values.
|
|
if(hasRot())
|
|
{
|
|
a11= matrix.a11; a12= matrix.a12; a13= matrix.a13;
|
|
a21= matrix.a21; a22= matrix.a22; a23= matrix.a23;
|
|
a31= matrix.a31; a32= matrix.a32; a33= matrix.a33;
|
|
// if has scale uniform, copy from matrix.
|
|
if(hasScaleUniform())
|
|
Scale33= matrix.Scale33;
|
|
}
|
|
else
|
|
{
|
|
// we are rot identity, with undefined values.
|
|
StateBit&= ~MAT_VALIDROT;
|
|
}
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::setPos(const CVector &v)
|
|
{
|
|
a14= v.x;
|
|
a24= v.y;
|
|
a34= v.z;
|
|
if(a14!=0 || a24!=0 || a34!=0)
|
|
StateBit|= MAT_TRANS;
|
|
else
|
|
// The trans is identity
|
|
StateBit&= ~MAT_TRANS;
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::movePos(const CVector &v)
|
|
{
|
|
a14+= v.x;
|
|
a24+= v.y;
|
|
a34+= v.z;
|
|
if(a14!=0 || a24!=0 || a34!=0)
|
|
StateBit|= MAT_TRANS;
|
|
else
|
|
// The trans is identity
|
|
StateBit&= ~MAT_TRANS;
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::setProj(const float proj[4])
|
|
{
|
|
a41= proj[0];
|
|
a42= proj[1];
|
|
a43= proj[2];
|
|
a44= proj[3];
|
|
|
|
// Check Proj state.
|
|
if(a41!=0 || a42!=0 || a43!=0 || a44!=1)
|
|
StateBit|= MAT_PROJ;
|
|
else
|
|
{
|
|
// The proj is identity, and is correcly setup!
|
|
StateBit&= ~MAT_PROJ;
|
|
StateBit|= MAT_VALIDPROJ;
|
|
}
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::resetProj()
|
|
{
|
|
a41= 0;
|
|
a42= 0;
|
|
a43= 0;
|
|
a44= 1;
|
|
// The proj is identity, and is correcly setup!
|
|
StateBit&= ~MAT_PROJ;
|
|
StateBit|= MAT_VALIDPROJ;
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::set(const float m44[16])
|
|
{
|
|
StateBit= MAT_IDENTITY;
|
|
|
|
StateBit|= MAT_ROT | MAT_SCALEANY;
|
|
memcpy(M, m44, 16*sizeof(float));
|
|
Scale33= 1.0f;
|
|
|
|
// Check Trans state.
|
|
if(a14!=0 || a24!=0 || a34!=0)
|
|
StateBit|= MAT_TRANS;
|
|
else
|
|
// The trans is identity
|
|
StateBit&= ~MAT_TRANS;
|
|
|
|
// Check Proj state.
|
|
if(a41!=0 || a42!=0 || a43!=0 || a44!=1)
|
|
StateBit|= MAT_PROJ;
|
|
else
|
|
{
|
|
// The proj is identity, and is correcly setup!
|
|
StateBit&= ~MAT_PROJ;
|
|
StateBit|= MAT_VALIDPROJ;
|
|
}
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::getRot(CVector &i, CVector &j, CVector &k) const
|
|
{
|
|
if(hasRot())
|
|
{
|
|
i.set(a11, a21, a31);
|
|
j.set(a12, a22, a32);
|
|
k.set(a13, a23, a33);
|
|
}
|
|
else
|
|
{
|
|
i.set(1, 0, 0);
|
|
j.set(0, 1, 0);
|
|
k.set(0, 0, 1);
|
|
}
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::getRot(float m33[9]) const
|
|
{
|
|
if(hasRot())
|
|
{
|
|
m33[0]= a11;
|
|
m33[1]= a21;
|
|
m33[2]= a31;
|
|
|
|
m33[3]= a12;
|
|
m33[4]= a22;
|
|
m33[5]= a32;
|
|
|
|
m33[6]= a13;
|
|
m33[7]= a23;
|
|
m33[8]= a33;
|
|
}
|
|
else
|
|
{
|
|
m33[0]= 1;
|
|
m33[1]= 0;
|
|
m33[2]= 0;
|
|
|
|
m33[3]= 0;
|
|
m33[4]= 1;
|
|
m33[5]= 0;
|
|
|
|
m33[6]= 0;
|
|
m33[7]= 0;
|
|
m33[8]= 1;
|
|
}
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::getProj(float proj[4]) const
|
|
{
|
|
if(hasProj())
|
|
{
|
|
proj[0]= a41;
|
|
proj[1]= a42;
|
|
proj[2]= a43;
|
|
proj[3]= a44;
|
|
}
|
|
else
|
|
{
|
|
proj[0]= 0;
|
|
proj[1]= 0;
|
|
proj[2]= 0;
|
|
proj[3]= 1;
|
|
}
|
|
}
|
|
// ======================================================================================================
|
|
CVector CMatrix::getI() const
|
|
{
|
|
if(hasRot())
|
|
return CVector(a11, a21, a31);
|
|
else
|
|
return CVector(1, 0, 0);
|
|
}
|
|
// ======================================================================================================
|
|
CVector CMatrix::getJ() const
|
|
{
|
|
if(hasRot())
|
|
return CVector(a12, a22, a32);
|
|
else
|
|
return CVector(0, 1, 0);
|
|
}
|
|
// ======================================================================================================
|
|
CVector CMatrix::getK() const
|
|
{
|
|
if(hasRot())
|
|
return CVector(a13, a23, a33);
|
|
else
|
|
return CVector(0, 0, 1);
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::get(float m44[16]) const
|
|
{
|
|
testExpandRot();
|
|
testExpandProj();
|
|
memcpy(m44, M, 16*sizeof(float));
|
|
}
|
|
// ======================================================================================================
|
|
const float *CMatrix::get() const
|
|
{
|
|
testExpandRot();
|
|
testExpandProj();
|
|
return M;
|
|
}
|
|
/*// ======================================================================================================
|
|
CVector CMatrix::toEuler(TRotOrder ro) const
|
|
{
|
|
|
|
}*/
|
|
|
|
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::translate(const CVector &v)
|
|
{
|
|
// SetTrans.
|
|
if( hasRot() )
|
|
{
|
|
a14+= a11*v.x + a12*v.y + a13*v.z;
|
|
a24+= a21*v.x + a22*v.y + a23*v.z;
|
|
a34+= a31*v.x + a32*v.y + a33*v.z;
|
|
}
|
|
else
|
|
{
|
|
a14+= v.x;
|
|
a24+= v.y;
|
|
a34+= v.z;
|
|
}
|
|
|
|
// SetProj.
|
|
if( hasProj() )
|
|
a44+= a41*v.x + a42*v.y + a43*v.z;
|
|
|
|
// Check Trans.
|
|
if(a14!=0 || a24!=0 || a34!=0)
|
|
StateBit|= MAT_TRANS;
|
|
else
|
|
// The trans is identity, and is correcly setup!
|
|
StateBit&= ~MAT_TRANS;
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::rotateX(float a)
|
|
{
|
|
|
|
if(a==0)
|
|
return;
|
|
double ca,sa;
|
|
ca=cos(a);
|
|
sa=sin(a);
|
|
|
|
// SetRot.
|
|
if( hasRot() )
|
|
{
|
|
float b12=a12, b22=a22, b32=a32;
|
|
float b13=a13, b23=a23, b33=a33;
|
|
a12= (float)(b12*ca + b13*sa);
|
|
a22= (float)(b22*ca + b23*sa);
|
|
a32= (float)(b32*ca + b33*sa);
|
|
a13= (float)(b13*ca - b12*sa);
|
|
a23= (float)(b23*ca - b22*sa);
|
|
a33= (float)(b33*ca - b32*sa);
|
|
}
|
|
else
|
|
{
|
|
testExpandRot();
|
|
a12= 0.0f; a22= (float)ca; a32= (float)sa;
|
|
a13= 0.0f; a23= (float)-sa; a33= (float)ca;
|
|
}
|
|
|
|
// SetProj.
|
|
if( hasProj() )
|
|
{
|
|
float b42=a42, b43=a43;
|
|
a42= (float)(b42*ca + b43*sa);
|
|
a43= (float)(b43*ca - b42*sa);
|
|
}
|
|
|
|
// set Rot.
|
|
StateBit|= MAT_ROT;
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::rotateY(float a)
|
|
{
|
|
|
|
if(a==0)
|
|
return;
|
|
double ca,sa;
|
|
ca=cos(a);
|
|
sa=sin(a);
|
|
|
|
// SetRot.
|
|
if( hasRot() )
|
|
{
|
|
float b11=a11, b21=a21, b31=a31;
|
|
float b13=a13, b23=a23, b33=a33;
|
|
a11= (float)(b11*ca - b13*sa);
|
|
a21= (float)(b21*ca - b23*sa);
|
|
a31= (float)(b31*ca - b33*sa);
|
|
a13= (float)(b13*ca + b11*sa);
|
|
a23= (float)(b23*ca + b21*sa);
|
|
a33= (float)(b33*ca + b31*sa);
|
|
}
|
|
else
|
|
{
|
|
testExpandRot();
|
|
a11= (float)ca; a21=0.0f; a31= (float)-sa;
|
|
a13= (float)sa; a23=0.0f; a33= (float)ca;
|
|
}
|
|
|
|
// SetProj.
|
|
if( hasProj() )
|
|
{
|
|
float b41=a41, b43=a43;
|
|
a41= (float)(b41*ca - b43*sa);
|
|
a43= (float)(b43*ca + b41*sa);
|
|
}
|
|
|
|
// set Rot.
|
|
StateBit|= MAT_ROT;
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::rotateZ(float a)
|
|
{
|
|
|
|
if(a==0)
|
|
return;
|
|
double ca,sa;
|
|
ca=cos(a);
|
|
sa=sin(a);
|
|
|
|
// SetRot.
|
|
if( StateBit & (MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY) )
|
|
{
|
|
float b11=a11, b21=a21, b31=a31;
|
|
float b12=a12, b22=a22, b32=a32;
|
|
a11= (float)(b11*ca + b12*sa);
|
|
a21= (float)(b21*ca + b22*sa);
|
|
a31= (float)(b31*ca + b32*sa);
|
|
a12= (float)(b12*ca - b11*sa);
|
|
a22= (float)(b22*ca - b21*sa);
|
|
a32= (float)(b32*ca - b31*sa);
|
|
}
|
|
else
|
|
{
|
|
testExpandRot();
|
|
a11= (float)ca; a21= (float)sa; a31=0.0f;
|
|
a12= (float)-sa; a22= (float)ca; a32=0.0f;
|
|
}
|
|
|
|
// SetProj.
|
|
if( hasProj() )
|
|
{
|
|
float b41=a41, b42=a42;
|
|
a41= (float)(b41*ca + b42*sa);
|
|
a42= (float)(b42*ca - b41*sa);
|
|
}
|
|
|
|
// set Rot.
|
|
StateBit|= MAT_ROT;
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::rotate(const CVector &v, TRotOrder ro)
|
|
{
|
|
CMatrix rot;
|
|
rot.identity();
|
|
switch(ro)
|
|
{
|
|
case XYZ: rot.rotateX(v.x); rot.rotateY(v.y); rot.rotateZ(v.z); break;
|
|
case XZY: rot.rotateX(v.x); rot.rotateZ(v.z); rot.rotateY(v.y); break;
|
|
case YXZ: rot.rotateY(v.y); rot.rotateX(v.x); rot.rotateZ(v.z); break;
|
|
case YZX: rot.rotateY(v.y); rot.rotateZ(v.z); rot.rotateX(v.x); break;
|
|
case ZXY: rot.rotateZ(v.z); rot.rotateX(v.x); rot.rotateY(v.y); break;
|
|
case ZYX: rot.rotateZ(v.z); rot.rotateY(v.y); rot.rotateX(v.x); break;
|
|
}
|
|
|
|
(*this)*= rot;
|
|
}
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::rotate(const CQuat &quat)
|
|
{
|
|
CMatrix rot;
|
|
rot.setRot(quat);
|
|
(*this)*= rot;
|
|
}
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::scale(float f)
|
|
{
|
|
|
|
if(f==1.0f) return;
|
|
if(StateBit & MAT_SCALEANY)
|
|
{
|
|
scale(CVector(f,f,f));
|
|
}
|
|
else
|
|
{
|
|
testExpandRot();
|
|
StateBit|= MAT_SCALEUNI;
|
|
Scale33*=f;
|
|
a11*= f; a12*=f; a13*=f;
|
|
a21*= f; a22*=f; a23*=f;
|
|
a31*= f; a32*=f; a33*=f;
|
|
|
|
// SetProj.
|
|
if( hasProj() )
|
|
{
|
|
a41*=f; a42*=f; a43*=f;
|
|
}
|
|
}
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::scale(const CVector &v)
|
|
{
|
|
|
|
if( v==CVector(1,1,1) ) return;
|
|
if( !(StateBit & MAT_SCALEANY) && v.x==v.y && v.x==v.z)
|
|
{
|
|
scale(v.x);
|
|
}
|
|
else
|
|
{
|
|
testExpandRot();
|
|
StateBit|=MAT_SCALEANY;
|
|
a11*= v.x; a12*=v.y; a13*=v.z;
|
|
a21*= v.x; a22*=v.y; a23*=v.z;
|
|
a31*= v.x; a32*=v.y; a33*=v.z;
|
|
|
|
// SetProj.
|
|
if( hasProj() )
|
|
{
|
|
a41*=v.x;
|
|
a42*=v.y;
|
|
a43*=v.z;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
|
|
|
|
// ***************************************************************************
|
|
void CMatrix::setMulMatrixNoProj(const CMatrix &m1, const CMatrix &m2)
|
|
{
|
|
/*
|
|
For a fast MulMatrix, it appears to be better to not take State bits into account (no test/if() overhead)
|
|
Just do heavy mul all the time (common case, and not so slow)
|
|
*/
|
|
|
|
// Ensure the src matrix have correct values in rot part
|
|
m1.testExpandRot();
|
|
m2.testExpandRot();
|
|
|
|
// Rot Mul
|
|
a11= m1.a11*m2.a11 + m1.a12*m2.a21 + m1.a13*m2.a31;
|
|
a12= m1.a11*m2.a12 + m1.a12*m2.a22 + m1.a13*m2.a32;
|
|
a13= m1.a11*m2.a13 + m1.a12*m2.a23 + m1.a13*m2.a33;
|
|
|
|
a21= m1.a21*m2.a11 + m1.a22*m2.a21 + m1.a23*m2.a31;
|
|
a22= m1.a21*m2.a12 + m1.a22*m2.a22 + m1.a23*m2.a32;
|
|
a23= m1.a21*m2.a13 + m1.a22*m2.a23 + m1.a23*m2.a33;
|
|
|
|
a31= m1.a31*m2.a11 + m1.a32*m2.a21 + m1.a33*m2.a31;
|
|
a32= m1.a31*m2.a12 + m1.a32*m2.a22 + m1.a33*m2.a32;
|
|
a33= m1.a31*m2.a13 + m1.a32*m2.a23 + m1.a33*m2.a33;
|
|
|
|
// Trans mul
|
|
a14= m1.a11*m2.a14 + m1.a12*m2.a24 + m1.a13*m2.a34 + m1.a14;
|
|
a24= m1.a21*m2.a14 + m1.a22*m2.a24 + m1.a23*m2.a34 + m1.a24;
|
|
a34= m1.a31*m2.a14 + m1.a32*m2.a24 + m1.a33*m2.a34 + m1.a34;
|
|
|
|
// Setup no proj at all, and force valid rot (still may be identity, but 0/1 are filled)
|
|
StateBit= (m1.StateBit | m2.StateBit | MAT_VALIDROT) & ~(MAT_PROJ|MAT_VALIDPROJ);
|
|
|
|
// Modify Scale. This test is important because Scale33 may be a #NAN if SCALEANY => avoid very slow mul.
|
|
if( hasScaleUniform() )
|
|
Scale33= m1.Scale33*m2.Scale33;
|
|
else
|
|
Scale33=1;
|
|
|
|
}
|
|
|
|
|
|
// ***************************************************************************
|
|
void CMatrix::setMulMatrix(const CMatrix &m1, const CMatrix &m2)
|
|
{
|
|
// Do *this= m1*m2
|
|
identity();
|
|
StateBit= m1.StateBit | m2.StateBit;
|
|
StateBit&= ~MAT_VALIDALL;
|
|
|
|
// Build Rot part.
|
|
//===============
|
|
bool M1Identity= ! m1.hasRot();
|
|
bool M2Identity= ! m2.hasRot();
|
|
bool M1ScaleOnly= ! (m1.StateBit & MAT_ROT);
|
|
bool M2ScaleOnly= ! (m2.StateBit & MAT_ROT);
|
|
bool MGeneralCase= !M1Identity && !M2Identity && !M1ScaleOnly && !M2ScaleOnly;
|
|
|
|
|
|
// Manage the most common general case first (optim the if ): blending of two rotations.
|
|
if( MGeneralCase )
|
|
{
|
|
a11= m1.a11*m2.a11 + m1.a12*m2.a21 + m1.a13*m2.a31;
|
|
a12= m1.a11*m2.a12 + m1.a12*m2.a22 + m1.a13*m2.a32;
|
|
a13= m1.a11*m2.a13 + m1.a12*m2.a23 + m1.a13*m2.a33;
|
|
|
|
a21= m1.a21*m2.a11 + m1.a22*m2.a21 + m1.a23*m2.a31;
|
|
a22= m1.a21*m2.a12 + m1.a22*m2.a22 + m1.a23*m2.a32;
|
|
a23= m1.a21*m2.a13 + m1.a22*m2.a23 + m1.a23*m2.a33;
|
|
|
|
a31= m1.a31*m2.a11 + m1.a32*m2.a21 + m1.a33*m2.a31;
|
|
a32= m1.a31*m2.a12 + m1.a32*m2.a22 + m1.a33*m2.a32;
|
|
a33= m1.a31*m2.a13 + m1.a32*m2.a23 + m1.a33*m2.a33;
|
|
}
|
|
// If one of the 3x3 matrix is an identity, just do a copy
|
|
else if( M1Identity || M2Identity )
|
|
{
|
|
// If both identity, then me too.
|
|
if( M1Identity && M2Identity )
|
|
{
|
|
// just expand me (important because validated below)
|
|
testExpandRot();
|
|
}
|
|
else
|
|
{
|
|
// Copy the non identity matrix.
|
|
const CMatrix *c= M2Identity? &m1 : &m2;
|
|
a11= c->a11; a12= c->a12; a13= c->a13;
|
|
a21= c->a21; a22= c->a22; a23= c->a23;
|
|
a31= c->a31; a32= c->a32; a33= c->a33;
|
|
}
|
|
}
|
|
// If two 3x3 matrix are just scaleOnly matrix, do a scaleFact.
|
|
else if( M1ScaleOnly && M2ScaleOnly )
|
|
{
|
|
// same process for scaleUni or scaleAny.
|
|
a11= m1.a11*m2.a11; a12= 0; a13= 0;
|
|
a21= 0; a22= m1.a22*m2.a22; a23= 0;
|
|
a31= 0; a32= 0; a33= m1.a33*m2.a33;
|
|
}
|
|
// If one of the matrix is a scaleOnly matrix, do a scale*Rot.
|
|
else if( M1ScaleOnly && !M2ScaleOnly )
|
|
{
|
|
a11= m1.a11*m2.a11; a12= m1.a11*m2.a12; a13= m1.a11*m2.a13;
|
|
a21= m1.a22*m2.a21; a22= m1.a22*m2.a22; a23= m1.a22*m2.a23;
|
|
a31= m1.a33*m2.a31; a32= m1.a33*m2.a32; a33= m1.a33*m2.a33;
|
|
}
|
|
else
|
|
{
|
|
// This must be this case
|
|
nlassert(!M1ScaleOnly && M2ScaleOnly);
|
|
a11= m1.a11*m2.a11; a12= m1.a12*m2.a22; a13= m1.a13*m2.a33;
|
|
a21= m1.a21*m2.a11; a22= m1.a22*m2.a22; a23= m1.a23*m2.a33;
|
|
a31= m1.a31*m2.a11; a32= m1.a32*m2.a22; a33= m1.a33*m2.a33;
|
|
}
|
|
|
|
// If M1 has translate and M2 has projective, rotation is modified.
|
|
if( m1.hasTrans() && m2.hasProj())
|
|
{
|
|
StateBit|= MAT_ROT|MAT_SCALEANY;
|
|
|
|
a11+= m1.a14*m2.a41;
|
|
a12+= m1.a14*m2.a42;
|
|
a13+= m1.a14*m2.a43;
|
|
|
|
a21+= m1.a24*m2.a41;
|
|
a22+= m1.a24*m2.a42;
|
|
a23+= m1.a24*m2.a43;
|
|
|
|
a31+= m1.a34*m2.a41;
|
|
a32+= m1.a34*m2.a42;
|
|
a33+= m1.a34*m2.a43;
|
|
}
|
|
|
|
// Modify Scale.
|
|
if( (StateBit & MAT_SCALEUNI) && !(StateBit & MAT_SCALEANY) )
|
|
{
|
|
// Must have correct Scale33
|
|
m1.testExpandRot();
|
|
m2.testExpandRot();
|
|
Scale33= m1.Scale33*m2.Scale33;
|
|
}
|
|
else
|
|
Scale33=1;
|
|
|
|
// In every case, I am valid now!
|
|
StateBit|=MAT_VALIDROT;
|
|
|
|
|
|
// Build Trans part.
|
|
//=================
|
|
if( StateBit & MAT_TRANS )
|
|
{
|
|
// Compose M2 part.
|
|
if( M1Identity )
|
|
{
|
|
a14= m2.a14;
|
|
a24= m2.a24;
|
|
a34= m2.a34;
|
|
}
|
|
else if (M1ScaleOnly )
|
|
{
|
|
a14= m1.a11*m2.a14;
|
|
a24= m1.a22*m2.a24;
|
|
a34= m1.a33*m2.a34;
|
|
}
|
|
else
|
|
{
|
|
a14= m1.a11*m2.a14 + m1.a12*m2.a24 + m1.a13*m2.a34;
|
|
a24= m1.a21*m2.a14 + m1.a22*m2.a24 + m1.a23*m2.a34;
|
|
a34= m1.a31*m2.a14 + m1.a32*m2.a24 + m1.a33*m2.a34;
|
|
}
|
|
// Compose M1 part.
|
|
if(m1.StateBit & MAT_TRANS)
|
|
{
|
|
if(m2.StateBit & MAT_PROJ)
|
|
{
|
|
a14+= m1.a14*m2.a44;
|
|
a24+= m1.a24*m2.a44;
|
|
a34+= m1.a34*m2.a44;
|
|
}
|
|
else
|
|
{
|
|
a14+= m1.a14;
|
|
a24+= m1.a24;
|
|
a34+= m1.a34;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// Build Proj part.
|
|
//=================
|
|
if( StateBit & MAT_PROJ )
|
|
{
|
|
// optimize nothing... (projection matrix are rare).
|
|
m1.testExpandRot();
|
|
m1.testExpandProj();
|
|
m2.testExpandRot();
|
|
m2.testExpandProj();
|
|
a41= m1.a41*m2.a11 + m1.a42*m2.a21 + m1.a43*m2.a31 + m1.a44*m2.a41;
|
|
a42= m1.a41*m2.a12 + m1.a42*m2.a22 + m1.a43*m2.a32 + m1.a44*m2.a42;
|
|
a43= m1.a41*m2.a13 + m1.a42*m2.a23 + m1.a43*m2.a33 + m1.a44*m2.a43;
|
|
a44= m1.a41*m2.a14 + m1.a42*m2.a24 + m1.a43*m2.a34 + m1.a44*m2.a44;
|
|
// The proj is valid now
|
|
StateBit|= MAT_VALIDPROJ;
|
|
}
|
|
else
|
|
{
|
|
// Don't copy proj part, and leave MAT_VALIDPROJ not set
|
|
}
|
|
}
|
|
// ======================================================================================================
|
|
void CMatrix::invert()
|
|
{
|
|
|
|
*this= inverted();
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::transpose3x3()
|
|
{
|
|
if(hasRot())
|
|
{
|
|
// swap values.
|
|
swap(a12, a21);
|
|
swap(a13, a31);
|
|
swap(a32, a23);
|
|
// Scale mode (none, uni, or any) is conserved. Scale33 too...
|
|
}
|
|
}
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::transpose()
|
|
{
|
|
transpose3x3();
|
|
if(hasTrans() || hasProj())
|
|
{
|
|
// if necessary, Get valid 0 on proj part.
|
|
testExpandProj();
|
|
// swap values
|
|
swap(a41, a14);
|
|
swap(a42, a24);
|
|
swap(a43, a34);
|
|
// swap StateBit flags, if not both were sets...
|
|
if(!hasTrans() || !hasProj())
|
|
{
|
|
// swap StateBit flags (swap trans with proj).
|
|
if(hasTrans())
|
|
{
|
|
StateBit&= ~MAT_TRANS;
|
|
StateBit|= MAT_PROJ;
|
|
}
|
|
else
|
|
{
|
|
StateBit&= ~MAT_PROJ;
|
|
StateBit|= MAT_TRANS;
|
|
}
|
|
}
|
|
// reset validity. NB, maybe not useful, but simpler, and bugfree.
|
|
StateBit&= ~(MAT_VALIDPROJ);
|
|
}
|
|
// NB: if no Trans or no Proj, do nothing, so don't need to modify VALIDTRANS and VALIDPROJ too.
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
bool CMatrix::fastInvert33(CMatrix &ret) const
|
|
{
|
|
// Fast invert of 3x3 rot matrix.
|
|
// Work if no scale and if MAT_SCALEUNI. doesn't work if MAT_SCALEANY.
|
|
|
|
if(StateBit & MAT_SCALEUNI)
|
|
{
|
|
if (Scale33 == 0.f) return false;
|
|
double s,S; // important for precision.
|
|
// Must divide the matrix by 1/Scale 2 times, to set unit, and to have a Scale=1/Scale.
|
|
S=1.0/Scale33;
|
|
ret.Scale33= (float)S;
|
|
s=S*S;
|
|
// The matrix is a base, so just transpose it.
|
|
ret.a11= (float)(a11*s); ret.a12= (float)(a21*s); ret.a13= (float)(a31*s);
|
|
ret.a21= (float)(a12*s); ret.a22= (float)(a22*s); ret.a23= (float)(a32*s);
|
|
ret.a31= (float)(a13*s); ret.a32= (float)(a23*s); ret.a33= (float)(a33*s);
|
|
}
|
|
else
|
|
{
|
|
ret.Scale33=1;
|
|
// The matrix is a base, so just transpose it.
|
|
ret.a11= a11; ret.a12= a21; ret.a13=a31;
|
|
ret.a21= a12; ret.a22= a22; ret.a23=a32;
|
|
ret.a31= a13; ret.a32= a23; ret.a33=a33;
|
|
}
|
|
return true;
|
|
// 15 cycles if no scale.
|
|
// 35 cycles if scale.
|
|
}
|
|
// ======================================================================================================
|
|
bool CMatrix::slowInvert33(CMatrix &ret) const
|
|
{
|
|
CVector invi,invj,invk;
|
|
CVector i,j,k;
|
|
double s;
|
|
|
|
i= getI();
|
|
j= getJ();
|
|
k= getK();
|
|
// Compute cofactors (minors *(-1)^(i+j)).
|
|
invi.x= j.y*k.z - k.y*j.z;
|
|
invi.y= j.z*k.x - k.z*j.x;
|
|
invi.z= j.x*k.y - k.x*j.y;
|
|
invj.x= k.y*i.z - i.y*k.z;
|
|
invj.y= k.z*i.x - i.z*k.x;
|
|
invj.z= k.x*i.y - i.x*k.y;
|
|
invk.x= i.y*j.z - j.y*i.z;
|
|
invk.y= i.z*j.x - j.z*i.x;
|
|
invk.z= i.x*j.y - j.x*i.y;
|
|
// compute determinant.
|
|
s= invi.x*i.x + invj.x*j.x + invk.x*k.x;
|
|
if(s==0)
|
|
return false;
|
|
// Transpose the Comatrice, and divide by determinant.
|
|
s=1.0/s;
|
|
ret.a11= (float)(invi.x*s); ret.a12= (float)(invi.y*s); ret.a13= (float)(invi.z*s);
|
|
ret.a21= (float)(invj.x*s); ret.a22= (float)(invj.y*s); ret.a23= (float)(invj.z*s);
|
|
ret.a31= (float)(invk.x*s); ret.a32= (float)(invk.y*s); ret.a33= (float)(invk.z*s);
|
|
|
|
return true;
|
|
// Roundly 82 cycles. (1Div=10 cycles).
|
|
}
|
|
// ======================================================================================================
|
|
bool CMatrix::slowInvert44(CMatrix &ret) const
|
|
{
|
|
sint i,j;
|
|
double s;
|
|
|
|
// Compute Cofactors
|
|
//==================
|
|
for(i=0;i<=3;i++)
|
|
{
|
|
for(j=0;j<=3;j++)
|
|
{
|
|
sint l1=0,l2=0,l3=0;
|
|
sint c1,c2,c3;
|
|
getCofactIndex(i,l1,l2,l3);
|
|
getCofactIndex(j,c1,c2,c3);
|
|
|
|
ret.mat(i,j)= 0;
|
|
ret.mat(i,j)+= mat(l1,c1) * mat(l2,c2) * mat(l3,c3);
|
|
ret.mat(i,j)+= mat(l1,c2) * mat(l2,c3) * mat(l3,c1);
|
|
ret.mat(i,j)+= mat(l1,c3) * mat(l2,c1) * mat(l3,c2);
|
|
|
|
ret.mat(i,j)-= mat(l1,c1) * mat(l2,c3) * mat(l3,c2);
|
|
ret.mat(i,j)-= mat(l1,c2) * mat(l2,c1) * mat(l3,c3);
|
|
ret.mat(i,j)-= mat(l1,c3) * mat(l2,c2) * mat(l3,c1);
|
|
|
|
if( (i+j)&1 )
|
|
ret.mat(i,j)=-ret.mat(i,j);
|
|
}
|
|
}
|
|
|
|
// Compute determinant.
|
|
//=====================
|
|
s= ret.mat(0,0) * mat(0,0) + ret.mat(0,1) * mat(0,1) + ret.mat(0,2) * mat(0,2) + ret.mat(0,3) * mat(0,3);
|
|
if(s==0)
|
|
return false;
|
|
|
|
// Divide by determinant.
|
|
//=======================
|
|
s=1.0/s;
|
|
for(i=0;i<=3;i++)
|
|
{
|
|
for(j=0;j<=3;j++)
|
|
ret.mat(i,j)= (float)(ret.mat(i,j)*s);
|
|
}
|
|
|
|
// Transpose the comatrice.
|
|
//=========================
|
|
for(i=0;i<=3;i++)
|
|
{
|
|
for(j=i+1;j<=3;j++)
|
|
{
|
|
swap(ret.mat(i,j), ret.mat(j,i));
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
// ======================================================================================================
|
|
CMatrix CMatrix::inverted() const
|
|
{
|
|
|
|
CMatrix ret;
|
|
|
|
testExpandRot();
|
|
testExpandProj();
|
|
|
|
// Do a conventionnal 44 inversion.
|
|
//=================================
|
|
if(StateBit & MAT_PROJ)
|
|
{
|
|
if(!slowInvert44(ret))
|
|
{
|
|
ret.identity();
|
|
return ret;
|
|
}
|
|
|
|
// Well, don't know what happens to matrix, so set all StateBit :).
|
|
ret.StateBit= MAT_TRANS|MAT_ROT|MAT_SCALEANY|MAT_PROJ;
|
|
|
|
// Check Trans state.
|
|
if(ret.a14!=0 || ret.a24!=0 || ret.a34!=0)
|
|
ret.StateBit|= MAT_TRANS;
|
|
else
|
|
ret.StateBit&= ~MAT_TRANS;
|
|
|
|
// Check Proj state.
|
|
if(ret.a41!=0 || ret.a42!=0 || ret.a43!=0 || ret.a44!=1)
|
|
ret.StateBit|= MAT_PROJ;
|
|
else
|
|
ret.StateBit&= ~MAT_PROJ;
|
|
}
|
|
|
|
// Do a speed 34 inversion.
|
|
//=========================
|
|
else
|
|
{
|
|
// Invert the rotation part.
|
|
if(StateBit & MAT_SCALEANY)
|
|
{
|
|
if(!slowInvert33(ret))
|
|
{
|
|
ret.identity();
|
|
return ret;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (!fastInvert33(ret))
|
|
{
|
|
ret.identity();
|
|
return ret;
|
|
}
|
|
}
|
|
// Scale33 is updated in fastInvert33().
|
|
|
|
// Invert the translation part.
|
|
if(StateBit & MAT_TRANS)
|
|
{
|
|
// Invert the translation part.
|
|
// This can only work if 4th line is 0 0 0 1.
|
|
// formula: InvVp= InvVi*(-Vp.x) + InvVj*(-Vp.y) + InvVk*(-Vp.z)
|
|
ret.a14= ret.a11*(-a14) + ret.a12*(-a24) + ret.a13*(-a34);
|
|
ret.a24= ret.a21*(-a14) + ret.a22*(-a24) + ret.a23*(-a34);
|
|
ret.a34= ret.a31*(-a14) + ret.a32*(-a24) + ret.a33*(-a34);
|
|
}
|
|
else
|
|
{
|
|
ret.a14= 0;
|
|
ret.a24= 0;
|
|
ret.a34= 0;
|
|
}
|
|
|
|
// The projection part is unmodified.
|
|
ret.a41= 0; ret.a42= 0; ret.a43= 0; ret.a44= 1;
|
|
|
|
// The matrix inverted keep the same state bits.
|
|
ret.StateBit= StateBit;
|
|
}
|
|
|
|
|
|
return ret;
|
|
}
|
|
// ======================================================================================================
|
|
bool CMatrix::normalize(TRotOrder ro)
|
|
{
|
|
|
|
CVector ti,tj,tk;
|
|
ti= getI();
|
|
tj= getJ();
|
|
tk= getK();
|
|
|
|
testExpandRot();
|
|
|
|
// Normalize with help of ro
|
|
switch(ro)
|
|
{
|
|
case XYZ:
|
|
ti.normalize();
|
|
tk= ti^tj;
|
|
tk.normalize();
|
|
tj= tk^ti;
|
|
break;
|
|
case XZY:
|
|
ti.normalize();
|
|
tj= tk^ti;
|
|
tj.normalize();
|
|
tk= ti^tj;
|
|
break;
|
|
case YXZ:
|
|
tj.normalize();
|
|
tk= ti^tj;
|
|
tk.normalize();
|
|
ti= tj^tk;
|
|
break;
|
|
case YZX:
|
|
tj.normalize();
|
|
ti= tj^tk;
|
|
ti.normalize();
|
|
tk= ti^tj;
|
|
break;
|
|
case ZXY:
|
|
tk.normalize();
|
|
tj= tk^ti;
|
|
tj.normalize();
|
|
ti= tj^tk;
|
|
break;
|
|
case ZYX:
|
|
tk.normalize();
|
|
ti= tj^tk;
|
|
ti.normalize();
|
|
tj= tk^ti;
|
|
break;
|
|
}
|
|
|
|
// Check, and set result.
|
|
if( ti.isNull() || tj.isNull() || tk.isNull() )
|
|
return false;
|
|
a11= ti.x; a12= tj.x; a13= tk.x;
|
|
a21= ti.y; a22= tj.y; a23= tk.y;
|
|
a31= ti.z; a32= tj.z; a33= tk.z;
|
|
// Scale is reseted.
|
|
StateBit&= ~(MAT_SCALEUNI|MAT_SCALEANY);
|
|
// Rot is setup...
|
|
StateBit|= MAT_ROT;
|
|
Scale33=1;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
|
|
|
|
// ======================================================================================================
|
|
CVector CMatrix::mulVector(const CVector &v) const
|
|
{
|
|
|
|
CVector ret;
|
|
|
|
if( hasRot() )
|
|
{
|
|
ret.x= a11*v.x + a12*v.y + a13*v.z;
|
|
ret.y= a21*v.x + a22*v.y + a23*v.z;
|
|
ret.z= a31*v.x + a32*v.y + a33*v.z;
|
|
return ret;
|
|
}
|
|
else
|
|
return v;
|
|
}
|
|
|
|
// ======================================================================================================
|
|
CVector CMatrix::mulPoint(const CVector &v) const
|
|
{
|
|
|
|
CVector ret;
|
|
|
|
if( hasRot() )
|
|
{
|
|
ret.x= a11*v.x + a12*v.y + a13*v.z;
|
|
ret.y= a21*v.x + a22*v.y + a23*v.z;
|
|
ret.z= a31*v.x + a32*v.y + a33*v.z;
|
|
}
|
|
else
|
|
{
|
|
ret= v;
|
|
}
|
|
if( hasTrans() )
|
|
{
|
|
ret.x+= a14;
|
|
ret.y+= a24;
|
|
ret.z+= a34;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
|
|
/*
|
|
* Multiply
|
|
*/
|
|
CVectorH CMatrix::operator*(const CVectorH& v) const
|
|
{
|
|
|
|
CVectorH ret;
|
|
|
|
testExpandRot();
|
|
testExpandProj();
|
|
|
|
ret.x= a11*v.x + a12*v.y + a13*v.z + a14*v.w;
|
|
ret.y= a21*v.x + a22*v.y + a23*v.z + a24*v.w;
|
|
ret.z= a31*v.x + a32*v.y + a33*v.z + a34*v.w;
|
|
ret.w= a41*v.x + a42*v.y + a43*v.z + a44*v.w;
|
|
return ret;
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
CPlane operator*(const CPlane &p, const CMatrix &m)
|
|
{
|
|
m.testExpandRot();
|
|
m.testExpandProj();
|
|
|
|
CPlane ret;
|
|
|
|
if( m.StateBit & (MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY|MAT_PROJ) )
|
|
{
|
|
// Compose with translation too.
|
|
ret.a= p.a*m.a11 + p.b*m.a21 + p.c*m.a31 + p.d*m.a41;
|
|
ret.b= p.a*m.a12 + p.b*m.a22 + p.c*m.a32 + p.d*m.a42;
|
|
ret.c= p.a*m.a13 + p.b*m.a23 + p.c*m.a33 + p.d*m.a43;
|
|
ret.d= p.a*m.a14 + p.b*m.a24 + p.c*m.a34 + p.d*m.a44;
|
|
return ret;
|
|
}
|
|
else if( m.StateBit & MAT_TRANS )
|
|
{
|
|
|
|
// Compose just with a translation.
|
|
ret.a= p.a;
|
|
ret.b= p.b;
|
|
ret.c= p.c;
|
|
ret.d= p.a*m.a14 + p.b*m.a24 + p.c*m.a34 + p.d*m.a44;
|
|
return ret;
|
|
}
|
|
else // Identity!!
|
|
return p;
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::setRot(const CQuat &quat)
|
|
{
|
|
// A quaternion do not have scale.
|
|
StateBit&= ~(MAT_ROT | MAT_SCALEANY|MAT_SCALEUNI);
|
|
Scale33= 1.0f;
|
|
if(quat.isIdentity())
|
|
{
|
|
a11= 1; a12= 0; a13= 0;
|
|
a21= 0; a22= 1; a23= 0;
|
|
a31= 0; a32= 0; a33= 1;
|
|
}
|
|
else
|
|
{
|
|
StateBit|= MAT_ROT;
|
|
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
|
|
|
|
// calculate coefficients
|
|
x2 = quat.x + quat.x; y2 = quat.y + quat.y;
|
|
z2 = quat.z + quat.z;
|
|
xx = quat.x * x2; xy = quat.x * y2; xz = quat.x * z2;
|
|
yy = quat.y * y2; yz = quat.y * z2; zz = quat.z * z2;
|
|
wx = quat.w * x2; wy = quat.w * y2; wz = quat.w * z2;
|
|
|
|
a11 = 1.0f - (yy + zz);
|
|
a12 = xy - wz;
|
|
a13 = xz + wy;
|
|
|
|
a21 = xy + wz;
|
|
a22 = 1.0f - (xx + zz);
|
|
a23 = yz - wx;
|
|
|
|
a31 = xz - wy;
|
|
a32 = yz + wx;
|
|
a33 = 1.0f - (xx + yy);
|
|
}
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::getRot(CQuat &quat) const
|
|
{
|
|
const CMatrix *pmat= this;
|
|
CMatrix MatNormed;
|
|
|
|
|
|
// Rot Indentity?
|
|
if(! (StateBit & MAT_ROT))
|
|
{
|
|
quat= CQuat::Identity;
|
|
return;
|
|
}
|
|
|
|
// Must normalize the matrix??
|
|
if(StateBit & (MAT_SCALEUNI | MAT_SCALEANY) )
|
|
{
|
|
MatNormed= *this;
|
|
MatNormed.normalize(ZYX);
|
|
pmat= &MatNormed;
|
|
}
|
|
|
|
// Compute quaternion.
|
|
float tr, s, q[4];
|
|
|
|
tr = pmat->a11 + pmat->a22 + pmat->a33;
|
|
|
|
// check the diagonal
|
|
if (tr > 0.0)
|
|
{
|
|
s = (float)sqrt (tr + 1.0f);
|
|
quat.w = s / 2.0f;
|
|
s = 0.5f / s;
|
|
quat.x = (pmat->a32 - pmat->a23) * s;
|
|
quat.y = (pmat->a13 - pmat->a31) * s;
|
|
quat.z = (pmat->a21 - pmat->a12) * s;
|
|
}
|
|
else
|
|
{
|
|
sint i, j, k;
|
|
sint nxt[3] = {1, 2, 0};
|
|
|
|
// diagonal is negative
|
|
i = 0;
|
|
if (pmat->a22 > pmat->a11) i = 1;
|
|
if (pmat->a33 > pmat->mat(i,i) ) i = 2;
|
|
j = nxt[i];
|
|
k = nxt[j];
|
|
|
|
s = (float) sqrt ( (pmat->mat(i,i) - (pmat->mat(j,j) + pmat->mat(k,k)) ) + 1.0);
|
|
|
|
q[i] = s * 0.5f;
|
|
|
|
if (s != 0.0f) s = 0.5f / s;
|
|
|
|
q[j] = (pmat->mat(j,i) + pmat->mat(i,j)) * s;
|
|
q[k] = (pmat->mat(k,i) + pmat->mat(i,k)) * s;
|
|
q[3] = (pmat->mat(k,j) - pmat->mat(j,k)) * s;
|
|
|
|
quat.x = q[0];
|
|
quat.y = q[1];
|
|
quat.z = q[2];
|
|
quat.w = q[3];
|
|
}
|
|
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
|
|
|
|
// ======================================================================================================
|
|
inline void CMatrix::setScaleUni(float scale)
|
|
{
|
|
// A Scale matrix do not have rotation.
|
|
StateBit&= ~(MAT_ROT | MAT_SCALEANY | MAT_SCALEUNI);
|
|
StateBit|= MAT_SCALEUNI | MAT_VALIDROT;
|
|
Scale33= scale;
|
|
a11= scale; a12= 0; a13= 0;
|
|
a21= 0; a22= scale; a23= 0;
|
|
a31= 0; a32= 0; a33= scale;
|
|
}
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::setScale(float scale)
|
|
{
|
|
setScaleUni(scale);
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::setScale(const CVector &v)
|
|
{
|
|
// actually a scale uniform?
|
|
if(v.x==v.y && v.x==v.z)
|
|
setScaleUni(v.x);
|
|
|
|
// A Scale matrix do not have rotation.
|
|
StateBit&= ~(MAT_ROT | MAT_SCALEANY | MAT_SCALEUNI);
|
|
StateBit|= MAT_SCALEANY | MAT_VALIDROT;
|
|
a11= v.x; a12= 0; a13= 0;
|
|
a21= 0; a22= v.y; a23= 0;
|
|
a31= 0; a32= 0; a33= v.z;
|
|
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
// ======================================================================================================
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::serial(IStream &f)
|
|
{
|
|
// Use versionning, maybe for futur improvement.
|
|
(void)f.serialVersion(0);
|
|
|
|
if(f.isReading())
|
|
identity();
|
|
f.serial(StateBit);
|
|
// avoid serial of random data
|
|
if(!f.isReading() && !hasScaleUniform())
|
|
{
|
|
float fs= 1.f;
|
|
f.serial(fs);
|
|
}
|
|
else
|
|
f.serial(Scale33);
|
|
if( hasRot() )
|
|
{
|
|
f.serial(a11, a12, a13);
|
|
f.serial(a21, a22, a23);
|
|
f.serial(a31, a32, a33);
|
|
}
|
|
if( hasTrans() )
|
|
{
|
|
f.serial(a14, a24, a34);
|
|
}
|
|
else if(f.isReading())
|
|
{
|
|
// must reset because Pos must always be valid
|
|
a14= a24= a34= 0;
|
|
}
|
|
if( hasProj() )
|
|
{
|
|
f.serial(a41, a42, a43, a44);
|
|
}
|
|
}
|
|
|
|
|
|
// ======================================================================================================
|
|
void CMatrix::setArbitraryRotI(const CVector &idir)
|
|
{
|
|
// avoid gimbal lock. if idir == nearly K, use another second lead vector
|
|
if( fabs(idir.z)<0.9f )
|
|
setRot(idir, CVector::J, CVector::K);
|
|
else
|
|
setRot(idir, CVector::J, CVector::I);
|
|
normalize(CMatrix::XZY);
|
|
}
|
|
|
|
void CMatrix::setArbitraryRotJ(const CVector &jdir)
|
|
{
|
|
// avoid gimbal lock. if jdir == nearly K, use another second lead vector
|
|
if(fabs(jdir.z)<0.9f)
|
|
setRot(CVector::I, jdir, CVector::K);
|
|
else
|
|
setRot(CVector::I, jdir, CVector::J);
|
|
normalize(CMatrix::YZX);
|
|
}
|
|
|
|
void CMatrix::setArbitraryRotK(const CVector &kdir)
|
|
{
|
|
// avoid gimbal lock. if kdir == nearly I, use another second lead vector
|
|
if( fabs(kdir.y)<0.9f )
|
|
setRot(CVector::I, CVector::J, kdir);
|
|
else
|
|
setRot(CVector::I, CVector::K, kdir);
|
|
normalize(CMatrix::ZYX);
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|