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