bool isEqual(const Cc3dMatrix4&mat1,const Cc3dMatrix4&mat2,float eps){
for(int i=0;i<16;i++){
float d=fabsf(mat1.getAt(i)-mat2.getAt(i));
if(d>eps)return false;
}
return true;
}
void Cc3dTransform::setRmat(const Cc3dMatrix4&Rmat){
assert(Rmat.getAt(3)==0);
assert(Rmat.getAt(7)==0);
assert(Rmat.getAt(11)==0);
assert(Rmat.getAt(15)==1);
float x=m_RTmat.getAt(12);
float y=m_RTmat.getAt(13);
float z=m_RTmat.getAt(14);
m_RTmat=Rmat;
m_RTmat.setAt(12, x);
m_RTmat.setAt(13, y);
m_RTmat.setAt(14, z);
}
Cc3dMatrix4 orthogonalization3x3(const Cc3dMatrix4&mat)
//??mat??3*3??????�ۜZ??????
//*****?????????????????????????????????��?????*****
{
//????<,>???????
//
//b1=a1 u1=b1/|b1|
//b2=a2-<a2,u1>u1 u2=b2/|b2|
//b3=a3-<a3,u1>u1-<a3,u2>u2 u3=b3/|b3|
//??b1,b2,b3?????????u1,u2,u3??????????
//
float x=mat.getAt(12);
float y=mat.getAt(13);
float z=mat.getAt(14);
//???�ۜZ??????
Cc3dVector4 a1(mat.getAt(0),mat.getAt(1),mat.getAt(2),0);
Cc3dVector4 a2(mat.getAt(4),mat.getAt(5),mat.getAt(6),0);
Cc3dVector4 a3(mat.getAt(8),mat.getAt(9),mat.getAt(10),0);
Cc3dVector4 u1,u2,u3;
{
Cc3dVector4 b1,b2,b3;
//????b1,u1
{
b1=a1;
u1=b1;
u1=normalize(u1);
}
//????b2,u2
{
float k=dot(a2,u1);
Cc3dVector4 ku1=u1*k;
b2=a2-ku1;
u2=b2;
u2=normalize(u2);
}
//????b3,u3
{
float k=dot(a3,u1);
float _k=dot(a3,u2);
Cc3dVector4 ku1,_ku2;
ku1=u1*k;
_ku2=u2*_k;
Cc3dVector4 temp;//ku1+_ku2
temp=ku1+_ku2;
b3=a3-temp;
u3=b3;
u3=normalize(u3);
}
}
//???u1,u2,u3
//??u1~3??????rsmat
Cc3dMatrix4 rsmat;
rsmat.init(u1.x(),u1.y(),u1.z(),0,//col 1
u2.x(),u2.y(),u2.z(),0,
u3.x(),u3.y(),u3.z(),0,
x,y,z,1);
return rsmat;
}
Cc3dMatrix4 Cc3dTransform::getRTSmat()const{
//apply scale to RTmat
Cc3dMatrix4 RTSmat;
RTSmat=m_RTmat;
RTSmat.setAt(0, RTSmat.getAt(0)*m_scaleX);
RTSmat.setAt(1, RTSmat.getAt(1)*m_scaleX);
RTSmat.setAt(2, RTSmat.getAt(2)*m_scaleX);
RTSmat.setAt(4, RTSmat.getAt(4)*m_scaleY);
RTSmat.setAt(5, RTSmat.getAt(5)*m_scaleY);
RTSmat.setAt(6, RTSmat.getAt(6)*m_scaleY);
RTSmat.setAt(8, RTSmat.getAt(8)*m_scaleZ);
RTSmat.setAt(9, RTSmat.getAt(9)*m_scaleZ);
RTSmat.setAt(10, RTSmat.getAt(10)*m_scaleZ);
return RTSmat;
}
Cc3dMatrix4 inverse(const Cc3dMatrix4&mat){
//this code is copy from: http://stackoverflow.com/questions/1148309/inverting-a-4x4-matrix
float m[16];
for(int i=0;i<16;i++)m[i]=mat.getAt(i);
float invOut[16];
float inv[16], det;
int i;
inv[0] = m[5] * m[10] * m[15] -
m[5] * m[11] * m[14] -
m[9] * m[6] * m[15] +
m[9] * m[7] * m[14] +
m[13] * m[6] * m[11] -
m[13] * m[7] * m[10];
inv[4] = -m[4] * m[10] * m[15] +
m[4] * m[11] * m[14] +
m[8] * m[6] * m[15] -
m[8] * m[7] * m[14] -
m[12] * m[6] * m[11] +
m[12] * m[7] * m[10];
inv[8] = m[4] * m[9] * m[15] -
m[4] * m[11] * m[13] -
m[8] * m[5] * m[15] +
m[8] * m[7] * m[13] +
m[12] * m[5] * m[11] -
m[12] * m[7] * m[9];
inv[12] = -m[4] * m[9] * m[14] +
m[4] * m[10] * m[13] +
m[8] * m[5] * m[14] -
m[8] * m[6] * m[13] -
m[12] * m[5] * m[10] +
m[12] * m[6] * m[9];
inv[1] = -m[1] * m[10] * m[15] +
m[1] * m[11] * m[14] +
m[9] * m[2] * m[15] -
m[9] * m[3] * m[14] -
m[13] * m[2] * m[11] +
m[13] * m[3] * m[10];
inv[5] = m[0] * m[10] * m[15] -
m[0] * m[11] * m[14] -
m[8] * m[2] * m[15] +
m[8] * m[3] * m[14] +
m[12] * m[2] * m[11] -
m[12] * m[3] * m[10];
inv[9] = -m[0] * m[9] * m[15] +
m[0] * m[11] * m[13] +
m[8] * m[1] * m[15] -
m[8] * m[3] * m[13] -
m[12] * m[1] * m[11] +
m[12] * m[3] * m[9];
inv[13] = m[0] * m[9] * m[14] -
m[0] * m[10] * m[13] -
m[8] * m[1] * m[14] +
m[8] * m[2] * m[13] +
m[12] * m[1] * m[10] -
m[12] * m[2] * m[9];
inv[2] = m[1] * m[6] * m[15] -
m[1] * m[7] * m[14] -
m[5] * m[2] * m[15] +
m[5] * m[3] * m[14] +
m[13] * m[2] * m[7] -
m[13] * m[3] * m[6];
inv[6] = -m[0] * m[6] * m[15] +
m[0] * m[7] * m[14] +
m[4] * m[2] * m[15] -
m[4] * m[3] * m[14] -
m[12] * m[2] * m[7] +
m[12] * m[3] * m[6];
inv[10] = m[0] * m[5] * m[15] -
m[0] * m[7] * m[13] -
m[4] * m[1] * m[15] +
m[4] * m[3] * m[13] +
m[12] * m[1] * m[7] -
m[12] * m[3] * m[5];
inv[14] = -m[0] * m[5] * m[14] +
m[0] * m[6] * m[13] +
m[4] * m[1] * m[14] -
m[4] * m[2] * m[13] -
m[12] * m[1] * m[6] +
m[12] * m[2] * m[5];
inv[3] = -m[1] * m[6] * m[11] +
m[1] * m[7] * m[10] +
m[5] * m[2] * m[11] -
m[5] * m[3] * m[10] -
m[9] * m[2] * m[7] +
m[9] * m[3] * m[6];
inv[7] = m[0] * m[6] * m[11] -
m[0] * m[7] * m[10] -
m[4] * m[2] * m[11] +
m[4] * m[3] * m[10] +
m[8] * m[2] * m[7] -
m[8] * m[3] * m[6];
inv[11] = -m[0] * m[5] * m[11] +
m[0] * m[7] * m[9] +
m[4] * m[1] * m[11] -
m[4] * m[3] * m[9] -
m[8] * m[1] * m[7] +
m[8] * m[3] * m[5];
inv[15] = m[0] * m[5] * m[10] -
m[0] * m[6] * m[9] -
m[4] * m[1] * m[10] +
m[4] * m[2] * m[9] +
m[8] * m[1] * m[6] -
m[8] * m[2] * m[5];
det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
if (det == 0)
//return false;
assert(false);
det = 1.0 / det;
for (i = 0; i < 16; i++)
invOut[i] = inv[i] * det;
// return true;
return Cc3dMatrix4(invOut);
}