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