/// <summary>
/// Calculates a rotation and translation that needs to be applied
/// on pSetA to match pSetB.
/// The process expects the matching points from sets having the same index.
/// </summary>
private static CRigidTransform CalculateRigidTransform(List <Vector3> pSetA, List <Vector3> pSetB)
{
//prevent modification of the input parameters
List <Vector3> setA = CUtils.GetCopy(pSetA);
List <Vector3> setB = CUtils.GetCopy(pSetB);
//setA and setB will be modified
List <Vector3> setAorig = CUtils.GetCopy(pSetA);
List <Vector3> setBorig = CUtils.GetCopy(pSetB);
Vector3 centroid_A = CUtils.GetAverage(setA);
Vector3 centroid_B = CUtils.GetAverage(setB);
CUtils.MovePointsBy(ref setA, -centroid_A);
CUtils.MovePointsBy(ref setB, -centroid_B);
Matrix mA = CreateMatrix(setA);
Matrix mB = CreateMatrix(setB);
Matrix A_multiply_B = mA.MultiplyTransposeBy(mB);
double[,] H = ConvertToDouble(A_multiply_B);
double[] w = new double[3];
double[,] u = new double[3, 3];
double[,] vt = new double[3, 3];
//U, S, Vt = linalg.svd(H)
//viz https://radfiz.org.ua/files/temp/Lab1_16/alglib-3.4.0.csharp/csharp/manual.csharp.html
const int u_needed = 2;
const int vt_needed = 2;
const int additional_memory = 2;
alglib.svd.rmatrixsvd(H, 3, 3, u_needed, vt_needed, additional_memory,
ref w, ref u, ref vt, null);
Matrix Vt = new Matrix(vt);
Matrix U = new Matrix(u);
Matrix rotation = Vt.MultiplyTransposeBy(U.Transpose(true));
//R = Vt.T * U.T
if (GetDeterminant(ConvertToDouble(rotation), 3) < 0)
{
//CDebug.Warning("Reflection detected");
//for(int i = 0; i < R.Rows; i++)
//{
// R[i, 2] *= -1; // NOT THE SAME!
//}
vt[2, 0] *= -1;
vt[2, 1] *= -1;
vt[2, 2] *= -1;
//Matrices need reinitialization - they are modified
Vt = new Matrix(vt);
U = new Matrix(u);
rotation = Vt.MultiplyTransposeBy(U.Transpose(true));
}
Matrix centerA =
new Matrix(new double[, ] {
{ centroid_A.X, centroid_A.Y, centroid_A.Z }
});
Matrix centerB =
new Matrix(new double[, ] {
{ centroid_B.X, centroid_B.Y, centroid_B.Z }
});
//create a copy - operations affects the original object
Matrix rotationCopy = new Matrix(rotation);
Matrix _R_mult_centerA = rotationCopy.Negate().MultiplyByTranspose(centerA);
Matrix translation = _R_mult_centerA + centerB.Transpose(true);
//CDebug.WriteLine("Rotation = ");
//CDebug.WriteLine("" + R);
//CDebug.WriteLine("Translation = ");
//CDebug.WriteLine("" + T);
float offset = GetOffset(setAorig, setBorig, rotation, translation);
return(new CRigidTransform(rotation, translation, offset));
}
