/// <summary>
        /// each colomn is one vertex - CORRECTED 
        /// </summary>
        /// <param name="PointsModel1"></param>
        /// <param name="PointsModel2"></param>
        /// <param name="p_mRotationMatrix"></param>
        /// <param name="p_mTranslationMatrix"></param>
        public static void CalculateRotationAndTranslation(Matrix PointsModel1, Matrix PointsModel2, out Iridium.Numerics.LinearAlgebra.Matrix p_mRotationMatrix, out Iridium.Numerics.LinearAlgebra.Matrix p_mTranslationMatrix)
        {
            if (PointsModel1.ColumnCount != PointsModel2.ColumnCount || PointsModel1.RowCount != PointsModel2.RowCount)
                throw new Exception("Two Matrixes do not have the same count");

            Matrix MeanValModel1 = new Matrix(PointsModel1.RowCount, 1);
            Matrix MeanValModel2 = new Matrix(PointsModel1.RowCount, 1);

            for (int j = 0; j < PointsModel1.RowCount; j++)
            {
                for (int i = 0; i < PointsModel1.ColumnCount; i++)
                {
                    MeanValModel1[j, 0] += PointsModel1[j, i];
                    MeanValModel2[j, 0] += PointsModel2[j, i];
                }
            }
            MeanValModel1 *= (1.0f / PointsModel1.ColumnCount);
            MeanValModel2 *= (1.0f / PointsModel1.ColumnCount);

            Matrix CenteredModel1 = new Matrix(PointsModel1.RowCount, PointsModel1.ColumnCount);
            Matrix CenteredModel2 = new Matrix(PointsModel1.RowCount, PointsModel1.ColumnCount);

            for (int i = 0; i < PointsModel1.RowCount; i++)
            {
                for (int j = 0; j < PointsModel1.ColumnCount; j++)
                {
                    CenteredModel1[i, j] = PointsModel1[i, j] - MeanValModel1[i, 0];
                    CenteredModel2[i, j] = PointsModel2[i, j] - MeanValModel2[i, 0];
                }
            }

            CenteredModel2.Transpose();

            Iridium.Numerics.LinearAlgebra.Matrix Covariance = CenteredModel1 * CenteredModel2;

            Iridium.Numerics.LinearAlgebra.SingularValueDecomposition SVD = new Iridium.Numerics.LinearAlgebra.SingularValueDecomposition(Covariance);
            Iridium.Numerics.LinearAlgebra.Matrix U = SVD.LeftSingularVectors;
            Iridium.Numerics.LinearAlgebra.Matrix V = SVD.RightSingularVectors;

            Iridium.Numerics.LinearAlgebra.Matrix s = new Iridium.Numerics.LinearAlgebra.Matrix(PointsModel1.RowCount, 1.0);

            if (Covariance.Rank() < 2)
                throw new Exception("Cannot allign generic model (cov rank is less than 2)");

            if (Covariance.Rank() == 2) // m-1 where m is dimension space (3D)
            {
                double detU = Math.Round(U.Determinant());
                double detV = Math.Round(V.Determinant());
                double detC = Covariance.Determinant();
                if ((int)detU * (int)detV == 1)
                    s[PointsModel1.RowCount - 1, PointsModel1.RowCount - 1] = 1;
                else if ((int)detU * (int)detV == -1)
                    s[PointsModel1.RowCount - 1, PointsModel1.RowCount - 1] = -1;
                else
                    throw new Exception("Determinant of U and V are not in conditions");
            }
            else
            {
                if (Covariance.Determinant() < 0)
                    s[PointsModel1.RowCount - 1, PointsModel1.RowCount - 1] = -1;
            }

            V.Transpose();
            Iridium.Numerics.LinearAlgebra.Matrix Rotation = U * s * V;

            Iridium.Numerics.LinearAlgebra.Matrix Translation = MeanValModel1 - Rotation * MeanValModel2;

            p_mRotationMatrix = Rotation;
            p_mTranslationMatrix = Translation;
        }
        public static void CalculateRotationAndTranslation(List<Cl3DModel.Cl3DModelPointIterator> p_ListOfPointsFrom1Model, List<Cl3DModel.Cl3DModelPointIterator> p_ListOfPointsFrom2Model, out Iridium.Numerics.LinearAlgebra.Matrix p_mRotationMatrix, out Iridium.Numerics.LinearAlgebra.Matrix p_mTranslationMatrix)
        {
            if (p_ListOfPointsFrom1Model.Count != p_ListOfPointsFrom2Model.Count)
                throw new Exception("Both models should have the sime number of points");

            Matrix Model1 = new Matrix(3, p_ListOfPointsFrom1Model.Count);
            Matrix Model2 = new Matrix(3, p_ListOfPointsFrom2Model.Count);

            for (int i = 0; i < p_ListOfPointsFrom1Model.Count; i++)
            {
                Model1[0, i] = p_ListOfPointsFrom1Model[i].X;
                Model1[1, i] = p_ListOfPointsFrom1Model[i].Y;
                Model1[2, i] = p_ListOfPointsFrom1Model[i].Z;

                Model2[0, i] = p_ListOfPointsFrom2Model[i].X;
                Model2[1, i] = p_ListOfPointsFrom2Model[i].Y;
                Model2[2, i] = p_ListOfPointsFrom2Model[i].Z;
            }

            CalculateRotationAndTranslation(Model1, Model2, out p_mRotationMatrix, out p_mTranslationMatrix);
        }