/// <summary> /// /// </summary> /// <param name="p"></param> /// <param name="q"></param> /// <returns></returns> public static Matrix3D Rigid3D(IList <Vector3D> p, IList <Vector3D> q) { int n = p.Count; double[] m = new double[9]; for (int i = 0; i < n; i++) { m[0] += p[i].X * q[i].X; m[1] += p[i].X * q[i].Y; m[2] += p[i].X * q[i].Z; m[3] += p[i].Y * q[i].X; m[4] += p[i].Y * q[i].Y; m[5] += p[i].Y * q[i].Z; m[6] += p[i].Z * q[i].X; m[7] += p[i].Z * q[i].Y; m[8] += p[i].Z * q[i].Z; } var M = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(3, 3, m); //double[] S = new double[3]; //double[] U = new double[9]; //double[] VT = new double[9]; //mklSolver.SingularValueDecomposition(true, M.Data, 3, 3, S, U, VT); //var RR = new Matrix3D() //{ // M11 = U[0] * VT[0] + U[1] * VT[3] * U[2] * VT[6], // M12 = U[0] * VT[1] + U[1] * VT[4] * U[2] * VT[7], // M13 = U[0] * VT[2] + U[1] * VT[5] * U[2] * VT[8], // M21 = U[3] * VT[0] + U[4] * VT[3] * U[5] * VT[6], // M22 = U[3] * VT[1] + U[4] * VT[4] * U[5] * VT[7], // M23 = U[3] * VT[2] + U[4] * VT[5] * U[5] * VT[8], // M31 = U[6] * VT[0] + U[7] * VT[3] * U[8] * VT[6], // M32 = U[6] * VT[1] + U[7] * VT[4] * U[8] * VT[7], // M33 = U[6] * VT[2] + U[7] * VT[5] * U[8] * VT[8], //}; //return RR; // R = V*UT <==> RT = VT*U var svd = M.Svd(true); var RT = svd.U().Multiply(svd.VT()); var R = new Matrix3D() { M11 = RT[0, 0], M12 = RT[1, 0], M13 = RT[2, 0], M21 = RT[0, 1], M22 = RT[1, 1], M23 = RT[2, 1], M31 = RT[0, 2], M32 = RT[1, 2], M33 = RT[2, 2], //M11 = RT[0, 0], //M12 = RT[0, 1], //M13 = RT[0, 2], //M21 = RT[1, 0], //M22 = RT[1, 1], //M23 = RT[1, 2], //M31 = RT[2, 0], //M32 = RT[2, 1], //M33 = RT[2, 2], M44 = 1.0, }; return(R); }
/// <summary> /// /// </summary> /// <param name="p"></param> /// <param name="q"></param> /// <returns></returns> public static Matrix3D Rigid3D(IList<Vector3D> p, IList<Vector3D> q) { int n = p.Count; double[] m = new double[9]; for (int i = 0; i < n; i++) { m[0] += p[i].X * q[i].X; m[1] += p[i].X * q[i].Y; m[2] += p[i].X * q[i].Z; m[3] += p[i].Y * q[i].X; m[4] += p[i].Y * q[i].Y; m[5] += p[i].Y * q[i].Z; m[6] += p[i].Z * q[i].X; m[7] += p[i].Z * q[i].Y; m[8] += p[i].Z * q[i].Z; } var M = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(3, 3, m); //double[] S = new double[3]; //double[] U = new double[9]; //double[] VT = new double[9]; //mklSolver.SingularValueDecomposition(true, M.Data, 3, 3, S, U, VT); //var RR = new Matrix3D() //{ // M11 = U[0] * VT[0] + U[1] * VT[3] * U[2] * VT[6], // M12 = U[0] * VT[1] + U[1] * VT[4] * U[2] * VT[7], // M13 = U[0] * VT[2] + U[1] * VT[5] * U[2] * VT[8], // M21 = U[3] * VT[0] + U[4] * VT[3] * U[5] * VT[6], // M22 = U[3] * VT[1] + U[4] * VT[4] * U[5] * VT[7], // M23 = U[3] * VT[2] + U[4] * VT[5] * U[5] * VT[8], // M31 = U[6] * VT[0] + U[7] * VT[3] * U[8] * VT[6], // M32 = U[6] * VT[1] + U[7] * VT[4] * U[8] * VT[7], // M33 = U[6] * VT[2] + U[7] * VT[5] * U[8] * VT[8], //}; //return RR; // R = V*UT <==> RT = VT*U var svd = M.Svd(true); var RT = svd.U().Multiply(svd.VT()); var R = new Matrix3D() { M11 = RT[0, 0], M12 = RT[1, 0], M13 = RT[2, 0], M21 = RT[0, 1], M22 = RT[1, 1], M23 = RT[2, 1], M31 = RT[0, 2], M32 = RT[1, 2], M33 = RT[2, 2], //M11 = RT[0, 0], //M12 = RT[0, 1], //M13 = RT[0, 2], //M21 = RT[1, 0], //M22 = RT[1, 1], //M23 = RT[1, 2], //M31 = RT[2, 0], //M32 = RT[2, 1], //M33 = RT[2, 2], M44 = 1.0, }; return R; }
public void ConeGestureFinished() { ConeGestureInProgress = false; GetComponent <LineRenderer>().enabled = false; // Process cone gesture data here // Start by converting (local) XYZ to TNB coordinates w.r.t the curve var startIdx = coneGesturePositionData.Count / 10; var endIdx = coneGesturePositionData.Count * 9 / 10; var n = endIdx - startIdx; double[] posData = new double[3 * n]; double[] velData = new double[3 * n]; double[] accData = new double[3 * n]; for (int i = startIdx; i < endIdx; ++i) { var pos = coneGesturePositionData[i]; var vel = coneGestureVelocityData[i]; var acc = coneGestureAccelerationData[i]; emissionCurve.GetClosestPoint(pos, out Vector3 closest, out int bezierIdx, out float param); var tangent = emissionCurve.Tangent(bezierIdx, param); var normal = emissionCurve.Normal(bezierIdx, param); var binormal = Vector3.Cross(tangent, normal); var delta = (pos - closest); var time = emissionCurve.GetTimeFromSplineParam(bezierIdx, param); // For points, first coordinate is time, and the other two given by dot product pos = new Vector3(time, Vector3.Dot(delta, normal), Vector3.Dot(delta, binormal)); // For vectors, all three coordinates given by projecting to the new frame (dot product) vel = new Vector3(Vector3.Dot(vel, tangent), Vector3.Dot(vel, normal), Vector3.Dot(vel, binormal)); acc = new Vector3(Vector3.Dot(acc, tangent), Vector3.Dot(acc, normal), Vector3.Dot(acc, binormal)); for (var j = 0; j < 3; ++j) { var idx = n * j + i - startIdx; posData[idx] = pos[j]; velData[idx] = vel[j]; accData[idx] = acc[j]; } } // Perform SVD to get principal components Matrix <double> P = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(n, 3, posData); Matrix <double> V = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(n, 3, velData); Matrix <double> A = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(n, 3, accData); var accSvd = A.Svd(true); var SA = accSvd.S; var VA = accSvd.VT; if (SA[0] / SA[1] > Globals.CONE_GESTURE_CLASSIFICATION_SINGULAR_VALUE_RATIO) { ProcessEmissionNoiseGesture(P, VA); } else { ProcessSpiralForceGesture(P, V); } Debug.Log("Delsecting because cone gesture finished"); animScript.Deselected(); }