public Frame3f(Vector3f origin, Vector3f x, Vector3f y, Vector3f z) { this.origin = origin; Matrix3f m = new Matrix3f(x, y, z, false); this.rotation = m.ToQuaternion(); }
public Matrix3f ToRotationMatrix() { float twoX = 2 * x; float twoY = 2 * y; float twoZ = 2 * z; float twoWX = twoX * w; float twoWY = twoY * w; float twoWZ = twoZ * w; float twoXX = twoX * x; float twoXY = twoY * x; float twoXZ = twoZ * x; float twoYY = twoY * y; float twoYZ = twoZ * y; float twoZZ = twoZ * z; Matrix3f m = Matrix3f.Zero; m[0, 0] = 1 - (twoYY + twoZZ); m[0, 1] = twoXY - twoWZ; m[0, 2] = twoXZ + twoWY; m[1, 0] = twoXY + twoWZ; m[1, 1] = 1 - (twoXX + twoZZ); m[1, 2] = twoYZ - twoWX; m[2, 0] = twoXZ - twoWY; m[2, 1] = twoYZ + twoWX; m[2, 2] = 1 - (twoXX + twoYY); return(m); }
public static void print_compare(string prefix, Matrix4x4 v1, g3.Matrix3f v2) { double dErr = 0; for (int r = 0; r < 3; ++r) { for (int c = 0; c < 3; ++c) { dErr += Math.Abs(v1[r, c] - v2[r, c]); } } DebugUtil.Log(2, "{0} {1} {2} err {3}", prefix, v1.ToString("F3"), v2.ToString("F3"), dErr); }
public void SetFromRotationMatrix(Matrix3f rot) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternion Calculus and Fast Animation". Index3i next = new Index3i(1, 2, 0); float trace = rot[0, 0] + rot[1, 1] + rot[2, 2]; float root; if (trace > 0) { // |w| > 1/2, may as well choose w > 1/2 root = (float)Math.Sqrt(trace + (float)1); // 2w w = ((float)0.5) * root; root = ((float)0.5) / root; // 1/(4w) x = (rot[2, 1] - rot[1, 2]) * root; y = (rot[0, 2] - rot[2, 0]) * root; z = (rot[1, 0] - rot[0, 1]) * root; } else { // |w| <= 1/2 int i = 0; if (rot[1, 1] > rot[0, 0]) { i = 1; } if (rot[2, 2] > rot[i, i]) { i = 2; } int j = next[i]; int k = next[j]; root = (float)Math.Sqrt(rot[i, i] - rot[j, j] - rot[k, k] + (float)1); Vector3f quat = new Vector3f(x, y, z); quat[i] = ((float)0.5) * root; root = ((float)0.5) / root; w = (rot[k, j] - rot[j, k]) * root; quat[j] = (rot[j, i] + rot[i, j]) * root; quat[k] = (rot[k, i] + rot[i, k]) * root; x = quat.x; y = quat.y; z = quat.z; } Normalize(); // we prefer normalized quaternions... }
public bool EpsilonEqual(Matrix3f m2, float epsilon) { return(Row0.EpsilonEqual(m2.Row0, epsilon) && Row1.EpsilonEqual(m2.Row1, epsilon) && Row2.EpsilonEqual(m2.Row2, epsilon)); }
public Quaternionf(Matrix3f mat) { x = y = z = 0; w = 1; SetFromRotationMatrix(mat); }
/// <summary> /// Solve for Translate/Rotate update, based on current From and To /// Note: From and To are invalidated, will need to be re-computed after calling this /// </summary> void update_transformation() { int N = From.Length; // normalize weights double wSum = 0; for (int i = 0; i < N; ++i) { wSum += Weights[i]; } double wSumInv = 1.0 / wSum; // compute means Vector3d MeanX = Vector3d.Zero; Vector3d MeanY = Vector3d.Zero; for (int i = 0; i < N; ++i) { MeanX += (Weights[i] * wSumInv) * From[i]; MeanY += (Weights[i] * wSumInv) * To[i]; } // subtract means for (int i = 0; i < N; ++i) { From[i] -= MeanX; To[i] -= MeanY; } // construct matrix 3x3 Matrix From*Transpose(To) // (vectors are columns) double[] M = new double[9]; for (int k = 0; k < 3; ++k) { int r = 3 * k; for (int i = 0; i < N; ++i) { double lhs = (Weights[i] * wSumInv) * From[i][k]; M[r + 0] += lhs * To[i].x; M[r + 1] += lhs * To[i].y; M[r + 2] += lhs * To[i].z; } } // compute SVD of M SingularValueDecomposition svd = new SingularValueDecomposition(3, 3, 100); uint ok = svd.Solve(M, -1); // sort in decreasing order, like Eigen Debug.Assert(ok < 9999999); double[] U = new double[9], V = new double[9], Tmp = new double[9];; svd.GetU(U); svd.GetV(V); // this is our rotation update double[] RotUpdate = new double[9]; // U*V gives us desired rotation double detU = MatrixUtil.Determinant3x3(U); double detV = MatrixUtil.Determinant3x3(V); if (detU * detV < 0) { double[] S = MatrixUtil.MakeDiagonal3x3(1, 1, -1); MatrixUtil.Multiply3x3(V, S, Tmp); MatrixUtil.Transpose3x3(U); MatrixUtil.Multiply3x3(Tmp, U, RotUpdate); } else { MatrixUtil.Transpose3x3(U); MatrixUtil.Multiply3x3(V, U, RotUpdate); } // convert matrix to quaternion Matrix3f RotUpdateM = new Matrix3f(RotUpdate); Quaternionf RotUpdateQ = new Quaternionf(RotUpdateM); // [TODO] is this right? We are solving for a translation and // rotation of the current From points, but when we fold these // into Translation & Rotation variables, we have essentially // changed the order of operations... // figure out translation update Vector3d TransUpdate = MeanY - RotUpdateQ * MeanX; Translation += TransUpdate; // and rotation Rotation = RotUpdateQ * Rotation; // above was ported from this code...still not supporting weights though. // need to figure out exactly what this code does (like, what does // transpose() do to a vector??) // /// Normalize weight vector //Eigen::VectorXd w_normalized = w/w.sum(); ///// De-mean //Eigen::Vector3d X_mean, Y_mean; //for(int i=0; i<3; ++i) { // X_mean(i) = (X.row(i).array()*w_normalized.transpose().array()).sum(); // Y_mean(i) = (Y.row(i).array()*w_normalized.transpose().array()).sum(); //Eigen::Matrix3d sigma = X * w_normalized.asDiagonal() * Y.transpose(); }