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);
}
