diff --git a/OpenRA.Game/Graphics/Util.cs b/OpenRA.Game/Graphics/Util.cs index d0efeadc3e..1d08371ddb 100644 --- a/OpenRA.Game/Graphics/Util.cs +++ b/OpenRA.Game/Graphics/Util.cs @@ -53,5 +53,181 @@ namespace OpenRA.Graphics destOffset += destSkip; } } + + public static float[] IdentityMatrix() + { + return Exts.MakeArray(16, j => (j % 5 == 0) ? 1.0f : 0); + } + + public static float[] ScaleMatrix(float sx, float sy, float sz) + { + var mtx = IdentityMatrix(); + mtx[0] = sx; + mtx[5] = sy; + mtx[10] = sz; + return mtx; + } + + public static float[] TranslationMatrix(float x, float y, float z) + { + var mtx = IdentityMatrix(); + mtx[12] = x; + mtx[13] = y; + mtx[14] = z; + return mtx; + } + + public static float[] MatrixMultiply(float[] lhs, float[] rhs) + { + var mtx = new float[16]; + for (var i = 0; i < 4; i++) + for (var j = 0; j < 4; j++) + { + mtx[4*i + j] = 0; + for (var k = 0; k < 4; k++) + mtx[4*i + j] += lhs[4*k + j]*rhs[4*i + k]; + } + + return mtx; + } + + public static float[] MatrixVectorMultiply(float[] mtx, float[] vec) + { + var ret = new float[4]; + for (var j = 0; j < 4; j++) + { + ret[j] = 0; + for (var k = 0; k < 4; k++) + ret[j] += mtx[4*k + j]*vec[k]; + } + + return ret; + } + + public static float[] MatrixInverse(float[] m) + { + var mtx = new float[16]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + mtx[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]; + + var det = m[0]*mtx[0] + m[1]*mtx[4] + m[2]*mtx[8] + m[3]*mtx[12]; + if (det == 0) + return null; + + for (var i = 0; i < 16; i++) + mtx[i] *= 1/det; + + return mtx; + } } }