public void SetFromRotationMatrix(Matrix3d rot)
{
SetFromRotationMatrix(ref rot);
}
public Quaterniond(Matrix3d mat)
{
x = y = z = 0; w = 1;
SetFromRotationMatrix(mat);
}
/// <summary>
/// Evaluate second-order FWN approximation at point q, relative to center c
/// </summary>
public static double EvaluateOrder2Approx(ref Vector3d center, ref Vector3d order1Coeff, ref Matrix3d order2Coeff, ref Vector3d q)
{
Vector3d dpq = (center - q);
double len = dpq.Length;
double len3 = len * len * len;
double fourPi_len3 = 1.0 / (MathUtil.FourPI * len3);
double order1 = fourPi_len3 * order1Coeff.Dot(ref dpq);
// second-order hessian \grad^2(G)
double c = -3.0 / (MathUtil.FourPI * len3 * len * len);
// expanded-out version below avoids extra constructors
//Matrix3d xqxq = new Matrix3d(ref dpq, ref dpq);
//Matrix3d hessian = new Matrix3d(fourPi_len3, fourPi_len3, fourPi_len3) - c * xqxq;
Matrix3d hessian = new Matrix3d(
fourPi_len3 + c * dpq.x * dpq.x, c * dpq.x * dpq.y, c * dpq.x * dpq.z,
c * dpq.y * dpq.x, fourPi_len3 + c * dpq.y * dpq.y, c * dpq.y * dpq.z,
c * dpq.z * dpq.x, c * dpq.z * dpq.y, fourPi_len3 + c * dpq.z * dpq.z);
double order2 = order2Coeff.InnerProduct(ref hessian);
return(order1 + order2);
}
public bool EpsilonEqual(Matrix3d m2, double epsilon)
{
return(Row0.EpsilonEqual(m2.Row0, epsilon) &&
Row1.EpsilonEqual(m2.Row1, epsilon) &&
Row2.EpsilonEqual(m2.Row2, epsilon));
}
/// <summary>
/// Compute U * S * V^T, useful for error-checking
/// </summary>
public Matrix3d ReconstructMatrix()
{
Matrix3d svdS = new Matrix3d(S[0], S[1], S[2]);
return(U.ToRotationMatrix() * svdS * V.Conjugate().ToRotationMatrix());
}
public FastQuaternionSVD(Matrix3d matrix, double epsilon = MathUtil.Epsilon, int jacobiIters = 4)
{
Solve(matrix, epsilon, jacobiIters);
}
/// <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
Matrix3d RotUpdateM = new Matrix3d(RotUpdate);
Quaterniond RotUpdateQ = new Quaterniond(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();
}