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