public float4x4(float4x4 m)
{
x = new float4(m.x);
y = new float4(m.y);
z = new float4(m.z);
w = new float4(m.w);
}
public static float4x4 MatrixTranspose(float4x4 m)
{
return new float4x4(m.x.x, m.y.x, m.z.x, m.w.x, m.x.y, m.y.y, m.z.y, m.w.y, m.x.z, m.y.z, m.z.z, m.w.z, m.x.w, m.y.w, m.z.w, m.w.w);
}
public static float4x4 MatrixLookAt(float3 eye, float3 at, float3 up)
{
float4x4 m = new float4x4();
m.w.w = 1.0f;
m.w.setxyz(eye);
m.z.setxyz(float3.normalize(eye - at));
m.x.setxyz(float3.normalize(float3.cross(up, m.z.xyz())));
m.y.setxyz(float3.cross(m.z.xyz(), m.x.xyz()));
return MatrixRigidInverse(m);
}
public static float4x4 MatrixRigidInverse(float4x4 m)
{
float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
float4x4 rot = new float4x4(m);
rot.w = new float4(0f, 0f, 0f, 1f);
return trans_inverse * MatrixTranspose(rot);
}
public static float4x4 Inverse(float4x4 m)
{
float4x4 d = new float4x4();
//float dst = d.x.x;
float[] tmp = new float[12]; // temp array for pairs
float[] src = new float[16]; // array of transpose source matrix
float det; // determinant
// transpose matrix
for (int i = 0; i < 4; i++)
{
src[i] = m[i].x;
src[i + 4] = m[i].y;
src[i + 8] = m[i].z;
src[i + 12] = m[i].w;
}
// calculate pairs for first 8 elements (cofactors)
tmp[0] = src[10] * src[15];
tmp[1] = src[11] * src[14];
tmp[2] = src[9] * src[15];
tmp[3] = src[11] * src[13];
tmp[4] = src[9] * src[14];
tmp[5] = src[10] * src[13];
tmp[6] = src[8] * src[15];
tmp[7] = src[11] * src[12];
tmp[8] = src[8] * src[14];
tmp[9] = src[10] * src[12];
tmp[10] = src[8] * src[13];
tmp[11] = src[9] * src[12];
// calculate first 8 elements (cofactors)
d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
// calculate pairs for second 8 elements (cofactors)
tmp[0] = src[2]*src[7];
tmp[1] = src[3]*src[6];
tmp[2] = src[1]*src[7];
tmp[3] = src[3]*src[5];
tmp[4] = src[1]*src[6];
tmp[5] = src[2]*src[5];
tmp[6] = src[0]*src[7];
tmp[7] = src[3]*src[4];
tmp[8] = src[0]*src[6];
tmp[9] = src[2]*src[4];
tmp[10] = src[0]*src[5];
tmp[11] = src[1]*src[4];
// calculate second 8 elements (cofactors)
d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
// calculate determinant
det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w;
// calculate matrix inverse
det = 1/det;
for (int j = 0; j < 4; j++)
d[j] *= det;
return d;
}
