public static float4Matrix SubMatrix(float4Matrix m1, float4Matrix m2) { return(new float4Matrix( Round(new float4(m1.r0.x - m2.r0.x, m1.r0.y - m2.r0.y, m1.r0.z - m2.r0.z, m1.r0.w - m2.r0.w)), Round(new float4(m1.r1.x - m2.r1.x, m1.r1.y - m2.r1.y, m1.r1.z - m2.r1.z, m1.r1.w - m2.r1.w)), Round(new float4(m1.r2.x - m2.r2.x, m1.r2.y - m2.r2.y, m1.r2.z - m2.r2.z, m1.r2.w - m2.r2.w)), Round(new float4(m1.r3.x - m2.r3.x, m1.r3.y - m2.r3.y, m1.r3.z - m2.r3.z, m1.r3.w - m2.r3.w)) )); }
public static Matrix <float> BuildMatrix(float4Matrix m) { return(Matrix <float> .Build.DenseOfArray(new float[, ] { { m.r0.x, m.r0.y, m.r0.z, m.r0.w }, { m.r1.x, m.r1.y, m.r1.z, m.r1.w }, { m.r2.x, m.r2.y, m.r2.z, m.r2.w }, { m.r3.x, m.r3.y, m.r3.z, m.r3.w }, })); }
private quaternion[] GetRFramesWithMatrix(quaternion q0, quaternion q1) { Debug.Log("===========R-FRAMES begin ==========="); //two frames of p0 & p1: quaternion F01h = m.normalizesafe(q0); quaternion F02h = m.normalizesafe(new quaternion(-F01h.value.y, F01h.value.x, 0f, 0f)); quaternion F03h = H.Mult(F01h, F02h); //Debug.Log(F01 + " | " + F02 + " | " + F03); quaternion F11 = m.normalizesafe(q1); quaternion F12 = m.normalizesafe(new quaternion(-F11.value.y, F11.value.x, 0f, 0f)); quaternion F13 = H.Mult(F11, F12); //Debug.Log(F11 + " | " + F12 + " | " + F13); //transform to float4 wxyz: float4 F01 = H.TransformToQWYZ(H.QuatToFloat4(F01h)); float4 F02 = H.TransformToQWYZ(H.QuatToFloat4(F02h)); float4 F03 = H.TransformToQWYZ(H.QuatToFloat4(F03h)); //Debug.Log(H.Matrix4of4ToMatrixBuild(H.MulPQ(H.QuatToFloat4(F01)))); float4Matrix m1 = H.SubMatrix(H.MulQP(H.iif), H.MulPQ(F01)); Matrix <float> M1 = H.BuildMatrix(m1); Debug.Log(H.iifm); Debug.Log(H.BuildMatrix(H.MulQP(H.iifm))); Debug.Log(H.BuildMatrix(H.MulPQ(F01))); Debug.Log(M1); float4Matrix m2 = H.SubMatrix(H.MulQP(H.jjf), H.MulPQ(F02)); Matrix <float> M2 = H.BuildMatrix(m2); Debug.Log(H.jjfm); Debug.Log(H.BuildMatrix(H.MulQP(H.jjfm))); Debug.Log(H.BuildMatrix(H.MulPQ(F02))); //Debug.Log(M2); float4Matrix m3 = H.SubMatrix(H.MulQP(H.kkf), H.MulPQ(F03)); Matrix <float> M3 = H.BuildMatrix(m3); Debug.Log(H.kkfm); Debug.Log(H.BuildMatrix(H.MulQP(H.kkfm))); Debug.Log(H.BuildMatrix(H.MulPQ(F03))); //Debug.Log(M3); // Debug.Log("############# converted FULL MATRIX ############"); var BigM = Matrix <double> .Build.DenseOfArray(new double[, ] { { m1.r0.x, m1.r0.y, m1.r0.z, m1.r0.w }, { m1.r1.x, m1.r1.y, m1.r1.z, m1.r1.w }, { m1.r2.x, m1.r2.y, m1.r2.z, m1.r2.w }, { m1.r3.x, m1.r3.y, m1.r3.z, m1.r3.w }, { m2.r0.x, m2.r0.y, m2.r0.z, m2.r0.w }, { m2.r1.x, m2.r1.y, m2.r1.z, m2.r1.w }, { m2.r2.x, m2.r2.y, m2.r2.z, m2.r2.w }, { m2.r3.x, m2.r3.y, m2.r3.z, m2.r3.w }, { m3.r0.x, m3.r0.y, m3.r0.z, m3.r0.w }, { m3.r1.x, m3.r1.y, m3.r1.z, m3.r1.w }, { m3.r2.x, m3.r2.y, m3.r2.z, m3.r2.w }, { m3.r3.x, m3.r3.y, m3.r3.z, m3.r3.w }, }); //Debug.Log("Rank: " + BigM.Rank()); //Debug.Log("Solutions Count: " + BigM.Kernel().Length); // Debug.Log("Solutions Count: " + BigM.Kernel()[0]); //var solution = BigM.Solve(Vector<double>.Build.Dense(new double[] { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 })); //Debug.Log(solution); Debug.Log(BigM); Debug.Log("############# converted FULL MATRIX ############"); var MM = Matrix <float> .Build.DenseOfArray(new float[, ] { { m1.r0.x, m1.r0.y, m1.r0.z, m1.r0.w }, { m2.r0.x, m2.r0.y, m2.r0.z, m2.r0.w }, { m3.r0.x, m3.r0.y, m3.r0.z, m3.r0.w }, }); Debug.Log("Rank: " + MM.Rank()); Debug.Log("Solutions Count: " + MM.Kernel().Length); if (MM.Kernel().Length > 0) { Debug.Log("Solutions: " + MM.Kernel()[0][0]); } // var solution = MM.Solve(Vector<float>.Build.Dense(new float[] { 0, 0, 0, 0 })); //Debug.Log(solution); Debug.Log(MM); Debug.Log("===========R-FRAMES ends ==========="); return(null); }