public static Quaternion OptimalRotationWeighted(Vector[] p, Vector[] x, Vector up, Vector ux, double[] w) { MatrixByArr cov = CovarianceWeighted(p, x, up, ux, w); double tr = cov[0, 0] + cov[1, 1] + cov[2, 2]; double A23 = cov[1, 2] - cov[2, 1]; double A31 = cov[2, 0] - cov[0, 2]; double A12 = cov[0, 1] - cov[1, 0]; MatrixByArr mat = new double[4, 4]; mat[0, 0] = tr; mat[1, 0] = mat[0, 1] = A23; mat[2, 0] = mat[0, 2] = A31; mat[3, 0] = mat[0, 3] = A12; cov = cov + cov.Tr(); cov = cov - LinAlg.Eye(3, tr); mat[1, 1] = cov[0, 0]; mat[1, 2] = cov[1, 0]; mat[1, 3] = cov[2, 0]; mat[2, 1] = cov[0, 1]; mat[2, 2] = cov[1, 1]; mat[2, 3] = cov[2, 1]; mat[3, 1] = cov[0, 2]; mat[3, 2] = cov[1, 2]; mat[3, 3] = cov[2, 2]; LinAlg.Eigen eigen = new LinAlg.Eigen(mat); double[,] eigenD = eigen.getD(); double[,] eigenV = eigen.getV(); int index_max = 0; if (eigenD[1, 1] > eigenD[index_max, index_max]) { index_max = 1; } if (eigenD[2, 2] > eigenD[index_max, index_max]) { index_max = 2; } if (eigenD[3, 3] > eigenD[index_max, index_max]) { index_max = 3; } Quaternion quater = new Quaternion( eigenV[0, index_max], eigenV[1, index_max], eigenV[2, index_max], eigenV[3, index_max] ); return(quater); }
public static Vector FlattenCol(this MatrixByArr mat) /* <summary> flatten toward column direction </summary>*/ { return(mat.Tr().Flatten()); }