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;
}
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 Cc3dMatrix4::operator*(const Cc3dMatrix4&mat)const{
const float *a=this->getArray();
const float *b=mat.getArray();
float r[16];//result
r[0]=b[0]*a[0]+b[1]*a[4]+b[2]*a[8]+b[3]*a[12];
r[1]=b[0]*a[1]+b[1]*a[5]+b[2]*a[9]+b[3]*a[13];
r[2]=b[0]*a[2]+b[1]*a[6]+b[2]*a[10]+b[3]*a[14];
r[3]=b[0]*a[3]+b[1]*a[7]+b[2]*a[11]+b[3]*a[15];
r[4]=b[4]*a[0]+b[5]*a[4]+b[6]*a[8]+b[7]*a[12];
r[5]=b[4]*a[1]+b[5]*a[5]+b[6]*a[9]+b[7]*a[13];
r[6]=b[4]*a[2]+b[5]*a[6]+b[6]*a[10]+b[7]*a[14];
r[7]=b[4]*a[3]+b[5]*a[7]+b[6]*a[11]+b[7]*a[15];
r[8]=b[8]*a[0]+b[9]*a[4]+b[10]*a[8]+b[11]*a[12];
r[9]=b[8]*a[1]+b[9]*a[5]+b[10]*a[9]+b[11]*a[13];
r[10]=b[8]*a[2]+b[9]*a[6]+b[10]*a[10]+b[11]*a[14];
r[11]=b[8]*a[3]+b[9]*a[7]+b[10]*a[11]+b[11]*a[15];
r[12]=b[12]*a[0]+b[13]*a[4]+b[14]*a[8]+b[15]*a[12];
r[13]=b[12]*a[1]+b[13]*a[5]+b[14]*a[9]+b[15]*a[13];
r[14]=b[12]*a[2]+b[13]*a[6]+b[14]*a[10]+b[15]*a[14];
r[15]=b[12]*a[3]+b[13]*a[7]+b[14]*a[11]+b[15]*a[15];
return Cc3dMatrix4(r);
}
Cc3dMatrix4 Cc3dMatrix4::operator+(const Cc3dMatrix4&mat)const{
const float *a=this->getArray();
const float *b=mat.getArray();
float r[16];//result
for(int i=0;i<16;i++){
r[i]=a[i]+b[i];
}
return Cc3dMatrix4(r);
}
Cc3dMatrix4 transpose(const Cc3dMatrix4&mat){
const float* m=mat.getArray();
float rs[16]={
m[0],m[4],m[8],m[12],//col 1
m[1],m[5],m[9],m[13],//col 2
m[2],m[6],m[10],m[14],//col 3
m[3],m[7],m[11],m[15]//col 4
};
return Cc3dMatrix4(rs);
}
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);
}
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;
}
void Cc3dSubMesh::drawC3D(Renderer *renderer, const Cc3dMatrix4& modelMat){
_customCommand.init(_globalZOrder);
_customCommand.func = CC_CALLBACK_0(Cc3dSubMesh::onDraw,this, Mat4(modelMat.getArray()), 0);
renderer->addCommand(&_customCommand);
}