From 421ab87cb1d86e39bbeabecda5d20422ebcace8b Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Thu, 23 Sep 2021 10:33:47 -0400 Subject: [PATCH 01/17] Initial commit I think the link Micakel sent is the right way to do it :) So we need to include https://github.com/martinbis11/polar-decomposition-3x3. --- src/Utils/Rotation.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 25639364..338c2431 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -316,6 +316,7 @@ void utils::Rotation::checkUnitary() + sqrtNorm + ", but should be equal to 3. Alternatively, you " "can compile with SKIP_ASSERT set to ON to turn off " "this error message"); + //Ajout du ckeck ici :) #endif #endif } From e32d88f4e802f52d94f4521a58ef8d1c7b53845b Mon Sep 17 00:00:00 2001 From: Pariterre Date: Thu, 23 Sep 2021 17:08:14 -0400 Subject: [PATCH 02/17] Added the unit check for speed --- src/Utils/Rotation.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 338c2431..15309601 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -311,12 +311,18 @@ void utils::Rotation::checkUnitary() #ifndef BIORBD_USE_CASADI_MATH #ifndef SKIP_ASSERT double sqrtNorm = static_cast(this->squaredNorm()); - utils::Error::check(fabs(sqrtNorm - 3.) < 1e-4, + bool checkUnit(fabs(sqrtNorm - 3.) < 1e-4); + if (!checkUnit){ + // Make calulation here + utils::Matrix3d mat; + + utils::Error::check(false, utils::String("The Rotation matrix square norm is equal to ") - + sqrtNorm + ", but should be equal to 3. Alternatively, you " + + sqrtNorm + ", but should be equal to 3. " + mat(0, 0) + "Alternatively, you " "can compile with SKIP_ASSERT set to ON to turn off " "this error message"); - //Ajout du ckeck ici :) + } + #endif #endif } From ca224b22e0fbad1b5bf8b7d47b2813c89ee7efa7 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 11:31:28 -0400 Subject: [PATCH 03/17] Added the calculations --- src/Utils/Rotation.cpp | 334 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 327 insertions(+), 7 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 15309601..152d428e 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -312,15 +312,335 @@ void utils::Rotation::checkUnitary() #ifndef SKIP_ASSERT double sqrtNorm = static_cast(this->squaredNorm()); bool checkUnit(fabs(sqrtNorm - 3.) < 1e-4); - if (!checkUnit){ - // Make calulation here - utils::Matrix3d mat; + if (!checkUnit) + { + + // Code addapted from https://github.com/brainexcerpts/3x3_polar_decomposition/blob/master/src/polar_decomposition_3x3.inl + + // Placer inf_norm and one norm in a function + // créer fonction polar_decomposition et déplacer dans Matrix3d + + float tolerance = 1e-6; + utils::Matrix3d M = this->block(0, 0, 3, 3); + utils::Matrix3d R; + utils::Matrix3d S; + utils::Matrix3d Mk; + utils::Matrix3d Ek; + float det, E_one_norm; + bool use_svd = false; + + + // Mk = mat^T + Mk = M.transpose(); + + //M one-norm + float M_one_norm = 0.0f; + for (int i = 0; i < 3; i++) + { + float col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + if (col_abs_sum > M_one_norm) + M_one_norm = col_abs_sum; + } + + //M infinity-norm + float M_inf_norm = 0.0; + for (int i = 0; i < 3; i++) + { + float row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + if (row_sum > M_inf_norm) + M_inf_norm = row_sum; + } + + do + { + utils::Matrix3d M_adj_Tk; + + /* + // row 2 x row 3 + cross(&(Mk[3]), &(Mk[6]), &M_adj_Tk(0, 0))); + // row 3 x row 1 + cross_product(&(Mk[6]), &(Mk[0]), &(M_adj_Tk[3])); + // row 1 x row 2 + cross_product(&(Mk[0]), &(Mk[3]), &(M_adj_Tk[6])); + */ + + det = Mk(0, 0) * M_adj_Tk(0, 0) + Mk(0, 1) * M_adj_Tk(0, 1) + Mk(0, 2) * M_adj_Tk(0, 2); + + if( det <= 1e-6 ) + { + use_svd = true; + std::cout << "Oups, SVD serait utile!\n"; + break; + } + + if( det == 0.0 ) + { + printf("Warning (polarDecomposition) : zero determinant encountered.\n"); + break; + } + + //mat_adj_T one-norm + float MadjT_one_norm = 0.0f; + for (int i = 0; i < 3; i++) + { + float col_abs_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); + if (col_abs_sum > MadjT_one_norm) + MadjT_one_norm = col_abs_sum; + } + + //mat infinity-norm + float MadjT_inf_norm = 0.0; + for (int i = 0; i < 3; i++) + { + float row_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); + if (row_sum > M_inf_norm) + M_inf_norm = row_sum; + } + + + float gamma = sqrtf(sqrtf((MadjT_one_norm * MadjT_inf_norm) / (M_one_norm * M_inf_norm * det * det))); + float g1 = gamma * 0.5f; + float g2 = 0.5f / (gamma * det); + + for(int i = 0; i < 3; i++) + { + for(int j = 0; i < 3; j++) + { + Ek(i, j) = Mk(i, j); + Mk(i, j) = g1 * Mk(i, j) + g2 * M_adj_Tk(i, j); + Ek(i, j) -= Mk(i, j); + } + } + + //Ek one-norm + float E_one_norm = 0.0f; + for (int i = 0; i < 3; i++) + { + float col_abs_sum = fabs(Ek(i, 0)) + fabs(Ek(i, 1)) + fabs(Ek(i, 2)); + if (col_abs_sum > E_one_norm) + E_one_norm = col_abs_sum; + } + + //M one-norm + float M_one_norm = 0.0f; + for (int i = 0; i < 3; i++) + { + float col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + if (col_abs_sum > M_one_norm) + M_one_norm = col_abs_sum; + } + + //M infinity-norm + float M_inf_norm = 0.0; + for (int i = 0; i < 3; i++) + { + float row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + if (row_sum > M_inf_norm) + M_inf_norm = row_sum; + } + + } while ( E_one_norm > M_one_norm * tolerance ); + + + if(use_svd) // && force_rotation + { + // use the SVD algorithm to compute R + utils::Matrix3d Mm = M; + utils::Matrix3d Um, Vm; + utils::Vector3d lambda; + utils::Vector3d lambda_inverse; + int modified_SVD = 1; + + + //SVD_3x3(Mm, Um, lambda, Vm, tolerance, modified_SVD); + + // F = Mm + // U = Um + // sigma = lambda + // V = Vm + // eps = tolerance + + // form F^T F and do eigendecomposition + utils::Matrix3d normalEq = Mm.transpose() * Mm; + utils::Vector3d eigen_vals; + utils::Matrix3d eigen_vecs; + + ///////////////////// + //eigen_sym(normalEq, eigen_vals, eigen_vecs); + + Vm = eigen_vecs.transpose(); + + // Handle situation: + // det(Vm) == -1 + // - multiply the first column of V by -1 + if (Vm.determinant() < 0.0) + { + // convert V into a rotation (multiply column 1 by -1) + Vm(0,0) *= -1.0; + Vm(1,0) *= -1.0; + Vm(2,0) *= -1.0; + } + + lambda(0) = (eigen_vals(0) > 0.0f) ? sqrtf(eigen_vals(0)) : 0.0f; + lambda(1) = (eigen_vals(1) > 0.0f) ? sqrtf(eigen_vals(1)) : 0.0f; + lambda(2) = (eigen_vals(2) > 0.0f) ? sqrtf(eigen_vals(2)) : 0.0f; + + // compute inverse of singular values + // also check if singular values are close to zero + lambda_inverse(0) = (lambda(0) > tolerance) ? (1.0f / lambda(0)) : 0.0f; + lambda_inverse(1) = (lambda(1) > tolerance) ? (1.0f / lambda(1)) : 0.0f; + lambda_inverse(2) = (lambda(2) > tolerance) ? (1.0f / lambda(2)) : 0.0f; + + // compute U using the formula: + // U = F * V * diag(SigmaInverse) + Um = Mm * Vm; + ///////////////////////////////// + //Um = details::multiply_diag_right(Um, sigma_inverse); + + // In theory, U is now orthonormal, U^T U = U U^T = I .. + // it may be a rotation or a reflection, depending on F. + // But in practice, if singular values are small or zero, + // it may not be orthonormal, so we need to fix it. + // Handle situation: + // 2. An entry of Sigma is near zero + // --------------------------------------------------------- + + if ((lambda(0) < tolerance) && (lambda(1) < tolerance) && (lambda(2) < tolerance)) + { + // extreme case, all singular values are small, + // material has collapsed almost to a point + // see [Irving 04], p. 4 + Um(0, 0) = 1.0; + Um(0, 1) = 0.0; + Um(0, 2) = 0.0; + Um(1, 0) = 0.0; + Um(1, 1) = 1.0; + Um(1, 2) = 0.0; + Um(2, 0) = 0.0; + Um(2, 1) = 0.0; + Um(2, 2) = 1.0; + + } + else + { + // handle the case where two singular values are small, + // but the third one is not handle it by computing + // two (arbitrary) vectors orthogonal to the eigenvector + // for the large singular value + int done = 0; + for(int dim = 0; dim < 3; dim++) + { + int dim_a = dim; + int dim_b = (dim + 1) % 3; + int dim_c = (dim + 2) % 3; + if ((lambda(dim_b) < tolerance) && (lambda(dim_c) < tolerance)) + { + // only the column dimA can be trusted, + // columns dimB and dimC correspond to tiny singular values + utils::Vector3d vec1(Um(0,dim_a), Um(1,dim_a), Um(2,dim_a)); // column dimA + utils::Vector3d vec2; + /////////////////////// + //vec2 = details::find_orthonormal_vec(vec1); + utils::Vector3d vec3 = (vec1.cross(vec2)).normalized(); + Um(0, dim_b) = vec2(0); + Um(1, dim_b) = vec2(1); + Um(2, dim_b) = vec2(2); + Um(0, dim_c) = vec3(0); + Um(1, dim_c) = vec3(1); + Um(2, dim_c) = vec3(2); + if (Um.determinant() < 0.0) + { + Um(0, dim_b) *= -1.0; + Um(1, dim_b) *= -1.0; + Um(2, dim_b) *= -1.0; + } + done = 1; + break; // out of for + } + } + + // handle the case where one singular value is small, + // but the other two are not + // handle it by computing the cross product of the two eigenvectors + // for the two large singular values + if(!done) + { + for(int dim = 0; dim < 3; dim++) + { + int dim_a = dim; + int dim_b = (dim + 1) % 3; + int dim_c = (dim + 2) % 3; + + if(lambda(dim_a) < tolerance) + { + // columns dimB and dimC are both good, + // but column dimA corresponds to a tiny singular value + + utils::Vector3d vec1(Um(0,dim_b), Um(1,dim_b), Um(2,dim_b)); // column dimB + utils::Vector3d vec2(Um(0,dim_c), Um(1,dim_c), Um(2,dim_c)); // column dimC + utils::Vector3d vec3 = (vec1.cross(vec2)).normalized(); + Um(0, dim_a) = vec3(0); + Um(1, dim_a) = vec3(1); + Um(2, dim_a) = vec3(2); + if(Um.determinant() < 0.0) + { + Um(0, dim_a) *= -1.0; + Um(1, dim_a) *= -1.0; + Um(2, dim_a) *= -1.0; + } + done = 1; + break; // out of for + } + } + } + + if ((!done) && (modified_SVD == 1)) + { + // Handle situation: + // negative determinant (Tet is inverted in solid mechanics) + // - check if det(U) == -1 + // - If yes, then negate the minimal element of Sigma + // and the corresponding column of U + + float det_U = Um.determinant(); + if (det_U < 0.0) + { + // negative determinant + // find the smallest singular value (they are all non-negative) + int smallest_singular_value_idx = 0; + for(int dim=1; dim<3; dim++) + if (lambda(dim) < lambda(smallest_singular_value_idx)) + smallest_singular_value_idx = dim; + + // negate the smallest singular value + lambda(smallest_singular_value_idx) *= -1.0; + Um(0, smallest_singular_value_idx) *= -1.0; + Um(1, smallest_singular_value_idx) *= -1.0; + Um(2, smallest_singular_value_idx) *= -1.0; + } + } + } + R = Um * Vm.transpose(); + } + else + { + // R = Mk^T + R = Mk.transpose(); + } + + + std::cout << "On s'est rendu, youpi!\n"; + + // utils::Matrix3d mat; utils::Error::check(false, - utils::String("The Rotation matrix square norm is equal to ") - + sqrtNorm + ", but should be equal to 3. " + mat(0, 0) + "Alternatively, you " - "can compile with SKIP_ASSERT set to ON to turn off " - "this error message"); + utils::String("The Rotation matrix square norm is equal to ") + + sqrtNorm + ", but should be equal to 3. Consider replacing the RT matrix by this closest orthonormal matrix \n[" + + R(0, 0) + ", " + R(0, 1) + ", " + R(0, 2) + "\n" + + R(1, 0) + ", " + R(1, 1) + ", " + R(1, 2) + "\n" + + R(2, 0) + ", " + R(2, 1) + ", " + R(2, 2) + "]\n" + + "Alternatively, you can compile with SKIP_ASSERT set to ON to turn off this error message"); + } #endif From d8531c392100b4034cdac2c4d8676623f183439d Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 12:13:20 -0400 Subject: [PATCH 04/17] included eigen_sym --- src/Utils/Rotation.cpp | 251 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 241 insertions(+), 10 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 152d428e..36f21549 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -451,16 +451,7 @@ void utils::Rotation::checkUnitary() utils::Vector3d lambda_inverse; int modified_SVD = 1; - - //SVD_3x3(Mm, Um, lambda, Vm, tolerance, modified_SVD); - - // F = Mm - // U = Um - // sigma = lambda - // V = Vm - // eps = tolerance - - // form F^T F and do eigendecomposition + // form Um^T Um and do eigendecomposition utils::Matrix3d normalEq = Mm.transpose() * Mm; utils::Vector3d eigen_vals; utils::Matrix3d eigen_vecs; @@ -468,6 +459,246 @@ void utils::Rotation::checkUnitary() ///////////////////// //eigen_sym(normalEq, eigen_vals, eigen_vecs); + utils::Matrix3d V; + utils::Vector3d d; + + int n = 3; + + utils::Vector3d e; + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + V(i, j) = normalEq(i, j); + } + } + + + // Symmetric Householder reduction to tridiagonal form. (tred2) + for (int j = 0; j < n; j++) { + d(j) = V(n-1, j); + } + + // Householder reduction to tridiagonal form. + + for (int i = n-1; i > 0; i--) { + + // Scale to avoid under/overflow. + + float scale = 0.0; + float h = 0.0; + for (int k = 0; k < i; k++) { + scale = scale + fabs(d(k)); + } + if (scale == 0.0) { + e(i) = d(i-1); + for (int j = 0; j < i; j++) { + d(j) = V(i-1, j); + V(i, j) = 0.0; + V(j, i) = 0.0; + } + } else { + + // Generate Householder vector. + for (int k = 0; k < i; k++) { + d(k) /= scale; + h += d(k) * d(k); + } + float f = d[i-1]; + float g = sqrtf(h); + if (f > 0) { + g = -g; + } + e(i) = scale * g; + h = h - f * g; + d(i-1) = f - g; + for (int j = 0; j < i; j++) { + e(j) = 0.0; + } + + // Apply similarity transformation to remaining columns. + for (int j = 0; j < i; j++) { + f = d(j); + V(j, i) = f; + g = e(j) + V(j, j) * f; + for (int k = j+1; k <= i-1; k++) { + g += V(k, j) * d(k); + e(k) += V(k, j) * f; + } + e(j) = g; + } + f = 0.0; + for (int j = 0; j < i; j++) { + e(j) /= h; + f += e(j) * d(j); + } + float hh = f / (h + h); + for (int j = 0; j < i; j++) { + e[j] -= hh * d(j); + } + for (int j = 0; j < i; j++) { + f = d(j); + g = e(j); + for (int k = j; k <= i-1; k++) { + V(k, j) -= (f * e(k) + g * d(k)); + } + d(j) = V(i-1, j); + V(i, j) = 0.0; + } + } + d(i) = h; + } + + // Accumulate transformations + for (int i = 0; i < n-1; i++) { + V(n-1, i) = V(i, i); + V(i, i) = 1.0; + float h = d(i+1); + if (h != 0.0) { + for (int k = 0; k <= i; k++) { + d(k) = V(k, i+1) / h; + } + for (int j = 0; j <= i; j++) { + float g = 0.0; + for (int k = 0; k <= i; k++) { + g += V(k, i+1) * V(k, j); + } + for (int k = 0; k <= i; k++) { + V(k, j) -= g * d(k); + } + } + } + for (int k = 0; k <= i; k++) { + V(k, i+1) = 0.0; + } + } + for (int j = 0; j < n; j++) { + d(j) = V(-1, j); + V(n-1, j) = 0.0; + } + V(n-1, n-1) = 1.0; + e(0) = 0.0; + + + // Symmetric tridiagonal QL algorithm. (td12) + for (int i = 1; i < n; i++) { + e(i-1) = e(i); + } + e(n-1) = 0.0; + + float f = 0.0f; + float tst1 = 0.0f; + float eps = 1e-16; + for (int l = 0; l < n; l++) { + + // Find small subdiagonal element + tst1 = fmax(tst1, fabs(d(l)) + fabs(e(l))); + int m = l; + while (m < n) { + if (fabs(e(m)) <= eps*tst1) { + break; + } + m++; + } + + // If m == l, d[l] is an eigenvalue, + // otherwise, iterate. + if (m > l) { + int iter = 0; + do { + iter = iter + 1; // (Could check iteration count here.) + + // Compute implicit shift + float g = d(l); + float p = (d(l+1) - g) / (2.0f * e(l)); + + float r = sqrtf(p*p+1.0f*1.0f); + if (p < 0) { + r = -r; + } + d(l) = e(l) / (p + r); + d(l+1) = e(l) * (p + r); + float dl1 = d(l+1); + float h = g - d(l); + for (int i = l+2; i < n; i++) { + d(i) -= h; + } + f = f + h; + + // Implicit QL transformation. + p = d(m); + float c = 1.0; + float c2 = c; + float c3 = c; + float el1 = e(l+1); + float s = 0.0; + float s2 = 0.0; + for (int i = m-1; i >= l; i--) { + c3 = c2; + c2 = c; + s2 = s; + g = c * e(i); + h = c * p; + r = sqrtf(p*p+e(i)*e(i)); + e(i+1) = s * r; + s = e(i) / r; + c = p / r; + p = c * d(i) - s * g; + d(i+1) = h + s * (c * g + s * d(i)); + + // Accumulate transformation. + for (int k = 0; k < n; k++) { + h = V(k, i+1); + V(k, i+1) = s * V(k, i) + c * h; + V(k, i) = c * V(k, i) - s * h; + } + } + p = -s * s2 * c3 * el1 * e(l) / dl1; + e(l) = s * p; + d(l) = c * p; + + // Check for convergence. + + } while (fabs(e(l)) > eps*tst1); + } + d(l) = d(l) + f; + e(l) = 0.0; + } + + // Sort eigenvalues and corresponding vectors. + for (int i = 0; i < n-1; i++) { + int k = i; + float p = d(i); + for (int j = i+1; j < n; j++) { + if (d(j) < p) { + k = j; + p = d(j); + } + } + if (k != i) { + d(k) = d(i); + d(i) = p; + for (int j = 0; j < n; j++) { + p = V(j, i); + V(j, i) = V(j, k); + V(j, k) = p; + } + } + } + + eigen_vals(0) = d(2); + eigen_vals(1) = d(1); + eigen_vals(2) = d(0); + + eigen_vecs(0, 0) = V(0, 2); + eigen_vecs(0, 1) = V(1, 2); + eigen_vecs(0, 2) = V(2, 2); + eigen_vecs(1, 0) = V(0, 1); + eigen_vecs(1, 1) = V(1, 1); + eigen_vecs(1, 2) = V(2, 1); + eigen_vecs(2, 0) = V(0, 0); + eigen_vecs(2, 1) = V(1, 0); + eigen_vecs(2, 2) = V(2, 0); + + Vm = eigen_vecs.transpose(); // Handle situation: From 7e6d7c8534a8146d1f9a61779a6bd499c675eba6 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 13:33:54 -0400 Subject: [PATCH 05/17] replaced multiply_diag_right --- src/Utils/Rotation.cpp | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 36f21549..3d16c4ca 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -725,8 +725,30 @@ void utils::Rotation::checkUnitary() // compute U using the formula: // U = F * V * diag(SigmaInverse) Um = Mm * Vm; - ///////////////////////////////// - //Um = details::multiply_diag_right(Um, sigma_inverse); + + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + result(i, j) = Um(i, j); + } + } + + result(0,0) *= sigma_inverse(0); + result(1,0) *= sigma_inverse(0); + result(2,0) *= sigma_inverse(0); + + result(0,1) *= sigma_inverse(1); + result(1,1) *= sigma_inverse(1); + result(2,1) *= sigma_inverse(1); + + result(0,2) *= sigma_inverse(2); + result(1,2) *= sigma_inverse(2); + result(2,2) *= sigma_inverse(2); + + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + Um(i, j) = result(i, j); + } + } // In theory, U is now orthonormal, U^T U = U U^T = I .. // it may be a rotation or a reflection, depending on F. From 5fb52792c56bbe9dda99506bb641e53d57d14e90 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 13:39:09 -0400 Subject: [PATCH 06/17] replaced find_orthogonal_vect --- src/Utils/Rotation.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 3d16c4ca..6f8dd9e8 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -792,8 +792,19 @@ void utils::Rotation::checkUnitary() // columns dimB and dimC correspond to tiny singular values utils::Vector3d vec1(Um(0,dim_a), Um(1,dim_a), Um(2,dim_a)); // column dimA utils::Vector3d vec2; - /////////////////////// - //vec2 = details::find_orthonormal_vec(vec1); + + // find smallest abs component of v + int smallest_idx = 0; + for(int dim = 1; dim < 3; dim++) + if (fabs(vec1(dim)) < fabs(vec1(smallest_idx))) + smallest_idx = dim; + + utils::Vector3d axis; + axis(smallest_idx) = 1.0; + + // this cross-product will be non-zero (as long as v is not zero) + Vec2 = (Vec1.cross(axis)).normalized(); + utils::Vector3d vec3 = (vec1.cross(vec2)).normalized(); Um(0, dim_b) = vec2(0); Um(1, dim_b) = vec2(1); From 3489ff33d1b85a8dd0323e7677334a7b46ceb9e1 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 13:43:24 -0400 Subject: [PATCH 07/17] float to scalar as requested --- src/Utils/Rotation.cpp | 88 ++++++++++++++++++++---------------------- 1 file changed, 42 insertions(+), 46 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 6f8dd9e8..960c3d63 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -320,13 +320,13 @@ void utils::Rotation::checkUnitary() // Placer inf_norm and one norm in a function // créer fonction polar_decomposition et déplacer dans Matrix3d - float tolerance = 1e-6; + utils::Scalar tolerance = 1e-6; utils::Matrix3d M = this->block(0, 0, 3, 3); utils::Matrix3d R; utils::Matrix3d S; utils::Matrix3d Mk; utils::Matrix3d Ek; - float det, E_one_norm; + utils::Scalar det, E_one_norm; bool use_svd = false; @@ -334,19 +334,19 @@ void utils::Rotation::checkUnitary() Mk = M.transpose(); //M one-norm - float M_one_norm = 0.0f; + utils::Scalar M_one_norm = 0.0f; for (int i = 0; i < 3; i++) { - float col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); if (col_abs_sum > M_one_norm) M_one_norm = col_abs_sum; } //M infinity-norm - float M_inf_norm = 0.0; + utils::Scalar M_inf_norm = 0.0; for (int i = 0; i < 3; i++) { - float row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + utils::Scalar row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); if (row_sum > M_inf_norm) M_inf_norm = row_sum; } @@ -380,27 +380,27 @@ void utils::Rotation::checkUnitary() } //mat_adj_T one-norm - float MadjT_one_norm = 0.0f; + utils::Scalar MadjT_one_norm = 0.0f; for (int i = 0; i < 3; i++) { - float col_abs_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); + utils::Scalar col_abs_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); if (col_abs_sum > MadjT_one_norm) MadjT_one_norm = col_abs_sum; } //mat infinity-norm - float MadjT_inf_norm = 0.0; + utils::Scalar MadjT_inf_norm = 0.0; for (int i = 0; i < 3; i++) { - float row_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); + utils::Scalar row_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); if (row_sum > M_inf_norm) M_inf_norm = row_sum; } - float gamma = sqrtf(sqrtf((MadjT_one_norm * MadjT_inf_norm) / (M_one_norm * M_inf_norm * det * det))); - float g1 = gamma * 0.5f; - float g2 = 0.5f / (gamma * det); + utils::Scalar gamma = sqrtf(sqrtf((MadjT_one_norm * MadjT_inf_norm) / (M_one_norm * M_inf_norm * det * det))); + utils::Scalar g1 = gamma * 0.5f; + utils::Scalar g2 = 0.5f / (gamma * det); for(int i = 0; i < 3; i++) { @@ -413,28 +413,28 @@ void utils::Rotation::checkUnitary() } //Ek one-norm - float E_one_norm = 0.0f; + utils::Scalar E_one_norm = 0.0f; for (int i = 0; i < 3; i++) { - float col_abs_sum = fabs(Ek(i, 0)) + fabs(Ek(i, 1)) + fabs(Ek(i, 2)); + utils::Scalar col_abs_sum = fabs(Ek(i, 0)) + fabs(Ek(i, 1)) + fabs(Ek(i, 2)); if (col_abs_sum > E_one_norm) E_one_norm = col_abs_sum; } //M one-norm - float M_one_norm = 0.0f; + utils::Scalar M_one_norm = 0.0f; for (int i = 0; i < 3; i++) { - float col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); if (col_abs_sum > M_one_norm) M_one_norm = col_abs_sum; } //M infinity-norm - float M_inf_norm = 0.0; + utils::Scalar M_inf_norm = 0.0; for (int i = 0; i < 3; i++) { - float row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + utils::Scalar row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); if (row_sum > M_inf_norm) M_inf_norm = row_sum; } @@ -455,10 +455,6 @@ void utils::Rotation::checkUnitary() utils::Matrix3d normalEq = Mm.transpose() * Mm; utils::Vector3d eigen_vals; utils::Matrix3d eigen_vecs; - - ///////////////////// - //eigen_sym(normalEq, eigen_vals, eigen_vecs); - utils::Matrix3d V; utils::Vector3d d; @@ -483,8 +479,8 @@ void utils::Rotation::checkUnitary() // Scale to avoid under/overflow. - float scale = 0.0; - float h = 0.0; + utils::Scalar scale = 0.0; + utils::Scalar h = 0.0; for (int k = 0; k < i; k++) { scale = scale + fabs(d(k)); } @@ -502,8 +498,8 @@ void utils::Rotation::checkUnitary() d(k) /= scale; h += d(k) * d(k); } - float f = d[i-1]; - float g = sqrtf(h); + utils::Scalar f = d[i-1]; + utils::Scalar g = sqrtf(h); if (f > 0) { g = -g; } @@ -530,7 +526,7 @@ void utils::Rotation::checkUnitary() e(j) /= h; f += e(j) * d(j); } - float hh = f / (h + h); + utils::Scalar hh = f / (h + h); for (int j = 0; j < i; j++) { e[j] -= hh * d(j); } @@ -551,13 +547,13 @@ void utils::Rotation::checkUnitary() for (int i = 0; i < n-1; i++) { V(n-1, i) = V(i, i); V(i, i) = 1.0; - float h = d(i+1); + utils::Scalar h = d(i+1); if (h != 0.0) { for (int k = 0; k <= i; k++) { d(k) = V(k, i+1) / h; } for (int j = 0; j <= i; j++) { - float g = 0.0; + utils::Scalar g = 0.0; for (int k = 0; k <= i; k++) { g += V(k, i+1) * V(k, j); } @@ -584,9 +580,9 @@ void utils::Rotation::checkUnitary() } e(n-1) = 0.0; - float f = 0.0f; - float tst1 = 0.0f; - float eps = 1e-16; + utils::Scalar f = 0.0f; + utils::Scalar tst1 = 0.0f; + utils::Scalar eps = 1e-16; for (int l = 0; l < n; l++) { // Find small subdiagonal element @@ -607,17 +603,17 @@ void utils::Rotation::checkUnitary() iter = iter + 1; // (Could check iteration count here.) // Compute implicit shift - float g = d(l); - float p = (d(l+1) - g) / (2.0f * e(l)); + utils::Scalar g = d(l); + utils::Scalar p = (d(l+1) - g) / (2.0f * e(l)); - float r = sqrtf(p*p+1.0f*1.0f); + utils::Scalar r = sqrtf(p*p+1.0f*1.0f); if (p < 0) { r = -r; } d(l) = e(l) / (p + r); d(l+1) = e(l) * (p + r); - float dl1 = d(l+1); - float h = g - d(l); + utils::Scalar dl1 = d(l+1); + utils::Scalar h = g - d(l); for (int i = l+2; i < n; i++) { d(i) -= h; } @@ -625,12 +621,12 @@ void utils::Rotation::checkUnitary() // Implicit QL transformation. p = d(m); - float c = 1.0; - float c2 = c; - float c3 = c; - float el1 = e(l+1); - float s = 0.0; - float s2 = 0.0; + utils::Scalar c = 1.0; + utils::Scalar c2 = c; + utils::Scalar c3 = c; + utils::Scalar el1 = e(l+1); + utils::Scalar s = 0.0; + utils::Scalar s2 = 0.0; for (int i = m-1; i >= l; i--) { c3 = c2; c2 = c; @@ -666,7 +662,7 @@ void utils::Rotation::checkUnitary() // Sort eigenvalues and corresponding vectors. for (int i = 0; i < n-1; i++) { int k = i; - float p = d(i); + utils::Scalar p = d(i); for (int j = i+1; j < n; j++) { if (d(j) < p) { k = j; @@ -866,7 +862,7 @@ void utils::Rotation::checkUnitary() // - If yes, then negate the minimal element of Sigma // and the corresponding column of U - float det_U = Um.determinant(); + utils::Scalar det_U = Um.determinant(); if (det_U < 0.0) { // negative determinant From 07b9d4f21ac9cb6143d7391654adbe5576bdff8e Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 13:46:51 -0400 Subject: [PATCH 08/17] sorry small fixes, forgot to compile! --- src/Utils/Rotation.cpp | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 960c3d63..af094a7a 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -722,23 +722,25 @@ void utils::Rotation::checkUnitary() // U = F * V * diag(SigmaInverse) Um = Mm * Vm; + utils::Matrix3d result; + for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { result(i, j) = Um(i, j); } } - result(0,0) *= sigma_inverse(0); - result(1,0) *= sigma_inverse(0); - result(2,0) *= sigma_inverse(0); + result(0,0) *= lambda_inverse(0); + result(1,0) *= lambda_inverse(0); + result(2,0) *= lambda_inverse(0); - result(0,1) *= sigma_inverse(1); - result(1,1) *= sigma_inverse(1); - result(2,1) *= sigma_inverse(1); + result(0,1) *= lambda_inverse(1); + result(1,1) *= lambda_inverse(1); + result(2,1) *= lambda_inverse(1); - result(0,2) *= sigma_inverse(2); - result(1,2) *= sigma_inverse(2); - result(2,2) *= sigma_inverse(2); + result(0,2) *= lambda_inverse(2); + result(1,2) *= lambda_inverse(2); + result(2,2) *= lambda_inverse(2); for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { @@ -799,7 +801,7 @@ void utils::Rotation::checkUnitary() axis(smallest_idx) = 1.0; // this cross-product will be non-zero (as long as v is not zero) - Vec2 = (Vec1.cross(axis)).normalized(); + vec2 = (vec1.cross(axis)).normalized(); utils::Vector3d vec3 = (vec1.cross(vec2)).normalized(); Um(0, dim_b) = vec2(0); From 4614d697e63de60c0268d12c911c3f0a722e2447 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 14:36:01 -0400 Subject: [PATCH 09/17] Made changes to norm calculations --- src/Utils/Rotation.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index af094a7a..1afc2554 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -333,20 +333,20 @@ void utils::Rotation::checkUnitary() // Mk = mat^T Mk = M.transpose(); - //M one-norm + //M one-norm (column) utils::Scalar M_one_norm = 0.0f; for (int i = 0; i < 3; i++) { - utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + utils::Scalar col_abs_sum = sum(fabs(M.block(i, 0, 1, 3))); if (col_abs_sum > M_one_norm) M_one_norm = col_abs_sum; } - //M infinity-norm + //M infinity-norm (row) utils::Scalar M_inf_norm = 0.0; for (int i = 0; i < 3; i++) { - utils::Scalar row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + utils::Scalar row_sum = sum(fabs(M.block(0, i, 3, 1))); if (row_sum > M_inf_norm) M_inf_norm = row_sum; } @@ -383,7 +383,7 @@ void utils::Rotation::checkUnitary() utils::Scalar MadjT_one_norm = 0.0f; for (int i = 0; i < 3; i++) { - utils::Scalar col_abs_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); + utils::Scalar col_abs_sum = sum(fabs(M_adj_Tk.block(0, i, 3, 1))); if (col_abs_sum > MadjT_one_norm) MadjT_one_norm = col_abs_sum; } @@ -392,7 +392,7 @@ void utils::Rotation::checkUnitary() utils::Scalar MadjT_inf_norm = 0.0; for (int i = 0; i < 3; i++) { - utils::Scalar row_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); + utils::Scalar row_sum = fabs(M_adj_Tk.block(i, 0, 1, 3))); if (row_sum > M_inf_norm) M_inf_norm = row_sum; } @@ -416,7 +416,7 @@ void utils::Rotation::checkUnitary() utils::Scalar E_one_norm = 0.0f; for (int i = 0; i < 3; i++) { - utils::Scalar col_abs_sum = fabs(Ek(i, 0)) + fabs(Ek(i, 1)) + fabs(Ek(i, 2)); + utils::Scalar col_abs_sum = fabs(Ek.block(0, i, 3, 1))); if (col_abs_sum > E_one_norm) E_one_norm = col_abs_sum; } @@ -425,7 +425,7 @@ void utils::Rotation::checkUnitary() utils::Scalar M_one_norm = 0.0f; for (int i = 0; i < 3; i++) { - utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + utils::Scalar col_abs_sum = fabs(M.block(0, i, 3, 1))); if (col_abs_sum > M_one_norm) M_one_norm = col_abs_sum; } @@ -434,7 +434,7 @@ void utils::Rotation::checkUnitary() utils::Scalar M_inf_norm = 0.0; for (int i = 0; i < 3; i++) { - utils::Scalar row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); + utils::Scalar row_sum = fabs(M.block(i, 0, 1, 3))); if (row_sum > M_inf_norm) M_inf_norm = row_sum; } From 3f9a60074d102fce348182b3338cb176ead56b50 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 14:43:29 -0400 Subject: [PATCH 10/17] Changed for loops to use size_t --- src/Utils/Rotation.cpp | 92 +++++++++++++++++++++--------------------- 1 file changed, 46 insertions(+), 46 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 1afc2554..16fc0f2c 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -335,7 +335,7 @@ void utils::Rotation::checkUnitary() //M one-norm (column) utils::Scalar M_one_norm = 0.0f; - for (int i = 0; i < 3; i++) + for (size_t i = 0; i < 3; ++i) { utils::Scalar col_abs_sum = sum(fabs(M.block(i, 0, 1, 3))); if (col_abs_sum > M_one_norm) @@ -344,7 +344,7 @@ void utils::Rotation::checkUnitary() //M infinity-norm (row) utils::Scalar M_inf_norm = 0.0; - for (int i = 0; i < 3; i++) + for (size_t i = 0; i < 3; ++i) { utils::Scalar row_sum = sum(fabs(M.block(0, i, 3, 1))); if (row_sum > M_inf_norm) @@ -381,7 +381,7 @@ void utils::Rotation::checkUnitary() //mat_adj_T one-norm utils::Scalar MadjT_one_norm = 0.0f; - for (int i = 0; i < 3; i++) + for (size_t i = 0; i < 3; ++i) { utils::Scalar col_abs_sum = sum(fabs(M_adj_Tk.block(0, i, 3, 1))); if (col_abs_sum > MadjT_one_norm) @@ -390,7 +390,7 @@ void utils::Rotation::checkUnitary() //mat infinity-norm utils::Scalar MadjT_inf_norm = 0.0; - for (int i = 0; i < 3; i++) + for (size_t i = 0; i < 3; ++i) { utils::Scalar row_sum = fabs(M_adj_Tk.block(i, 0, 1, 3))); if (row_sum > M_inf_norm) @@ -402,9 +402,9 @@ void utils::Rotation::checkUnitary() utils::Scalar g1 = gamma * 0.5f; utils::Scalar g2 = 0.5f / (gamma * det); - for(int i = 0; i < 3; i++) + for(size_t i = 0; i < 3; ++i) { - for(int j = 0; i < 3; j++) + for(size_t j = 0; i < 3; ++j) { Ek(i, j) = Mk(i, j); Mk(i, j) = g1 * Mk(i, j) + g2 * M_adj_Tk(i, j); @@ -414,7 +414,7 @@ void utils::Rotation::checkUnitary() //Ek one-norm utils::Scalar E_one_norm = 0.0f; - for (int i = 0; i < 3; i++) + for (size_t i = 0; i < 3; ++i) { utils::Scalar col_abs_sum = fabs(Ek.block(0, i, 3, 1))); if (col_abs_sum > E_one_norm) @@ -423,7 +423,7 @@ void utils::Rotation::checkUnitary() //M one-norm utils::Scalar M_one_norm = 0.0f; - for (int i = 0; i < 3; i++) + for (size_t i = 0; i < 3; ++i) { utils::Scalar col_abs_sum = fabs(M.block(0, i, 3, 1))); if (col_abs_sum > M_one_norm) @@ -432,7 +432,7 @@ void utils::Rotation::checkUnitary() //M infinity-norm utils::Scalar M_inf_norm = 0.0; - for (int i = 0; i < 3; i++) + for (size_t i = 0; i < 3; ++i) { utils::Scalar row_sum = fabs(M.block(i, 0, 1, 3))); if (row_sum > M_inf_norm) @@ -461,32 +461,32 @@ void utils::Rotation::checkUnitary() int n = 3; utils::Vector3d e; - for (int i = 0; i < n; i++) { - for (int j = 0; j < n; j++) { + for (size_t i = 0; i < n; ++i) { + for (size_t j = 0; j < n; ++j) { V(i, j) = normalEq(i, j); } } // Symmetric Householder reduction to tridiagonal form. (tred2) - for (int j = 0; j < n; j++) { + for (size_t j = 0; j < n; ++j) { d(j) = V(n-1, j); } // Householder reduction to tridiagonal form. - for (int i = n-1; i > 0; i--) { + for (size_t = n-1; i > 0; --i) { // Scale to avoid under/overflow. utils::Scalar scale = 0.0; utils::Scalar h = 0.0; - for (int k = 0; k < i; k++) { + for (size_t k = 0; k < i; ++k) { scale = scale + fabs(d(k)); } if (scale == 0.0) { e(i) = d(i-1); - for (int j = 0; j < i; j++) { + for (size_t j = 0; j < i; ++j) { d(j) = V(i-1, j); V(i, j) = 0.0; V(j, i) = 0.0; @@ -494,7 +494,7 @@ void utils::Rotation::checkUnitary() } else { // Generate Householder vector. - for (int k = 0; k < i; k++) { + for (size_t k = 0; k < i; ++k) { d(k) /= scale; h += d(k) * d(k); } @@ -506,34 +506,34 @@ void utils::Rotation::checkUnitary() e(i) = scale * g; h = h - f * g; d(i-1) = f - g; - for (int j = 0; j < i; j++) { + for (size_t j = 0; j < i; ++j) { e(j) = 0.0; } // Apply similarity transformation to remaining columns. - for (int j = 0; j < i; j++) { + for (size_t j = 0; j < i; ++j) { f = d(j); V(j, i) = f; g = e(j) + V(j, j) * f; - for (int k = j+1; k <= i-1; k++) { + for (size_t k = j+1; k <= i-1; ++k) { g += V(k, j) * d(k); e(k) += V(k, j) * f; } e(j) = g; } f = 0.0; - for (int j = 0; j < i; j++) { + for (size_t j = 0; j < i; ++j) { e(j) /= h; f += e(j) * d(j); } utils::Scalar hh = f / (h + h); - for (int j = 0; j < i; j++) { + for (size_t j = 0; j < i; ++j) { e[j] -= hh * d(j); } - for (int j = 0; j < i; j++) { + for (size_t j = 0; j < i; ++j) { f = d(j); g = e(j); - for (int k = j; k <= i-1; k++) { + for (size_t k = j; k <= i-1; ++k) { V(k, j) -= (f * e(k) + g * d(k)); } d(j) = V(i-1, j); @@ -544,29 +544,29 @@ void utils::Rotation::checkUnitary() } // Accumulate transformations - for (int i = 0; i < n-1; i++) { + for (size_t i = 0; i < n-1; ++i) { V(n-1, i) = V(i, i); V(i, i) = 1.0; utils::Scalar h = d(i+1); if (h != 0.0) { - for (int k = 0; k <= i; k++) { + for (size_t k = 0; k <= i; ++k) { d(k) = V(k, i+1) / h; } - for (int j = 0; j <= i; j++) { + for (size_t j = 0; j <= i; ++j) { utils::Scalar g = 0.0; - for (int k = 0; k <= i; k++) { + for (size_t k = 0; k <= i; ++k) { g += V(k, i+1) * V(k, j); } - for (int k = 0; k <= i; k++) { + for (size_t k = 0; k <= i; ++k) { V(k, j) -= g * d(k); } } } - for (int k = 0; k <= i; k++) { + for (size_t k = 0; k <= i; ++k) { V(k, i+1) = 0.0; } } - for (int j = 0; j < n; j++) { + for (size_t j = 0; j < n; ++j) { d(j) = V(-1, j); V(n-1, j) = 0.0; } @@ -575,7 +575,7 @@ void utils::Rotation::checkUnitary() // Symmetric tridiagonal QL algorithm. (td12) - for (int i = 1; i < n; i++) { + for (size_t i = 1; i < n; ++i) { e(i-1) = e(i); } e(n-1) = 0.0; @@ -583,7 +583,7 @@ void utils::Rotation::checkUnitary() utils::Scalar f = 0.0f; utils::Scalar tst1 = 0.0f; utils::Scalar eps = 1e-16; - for (int l = 0; l < n; l++) { + for (size_t l = 0; l < n; ++l) { // Find small subdiagonal element tst1 = fmax(tst1, fabs(d(l)) + fabs(e(l))); @@ -614,7 +614,7 @@ void utils::Rotation::checkUnitary() d(l+1) = e(l) * (p + r); utils::Scalar dl1 = d(l+1); utils::Scalar h = g - d(l); - for (int i = l+2; i < n; i++) { + for (size_t i = l+2; i < n; ++i) { d(i) -= h; } f = f + h; @@ -627,7 +627,7 @@ void utils::Rotation::checkUnitary() utils::Scalar el1 = e(l+1); utils::Scalar s = 0.0; utils::Scalar s2 = 0.0; - for (int i = m-1; i >= l; i--) { + for (size_t i = m-1; i >= l; --i) { c3 = c2; c2 = c; s2 = s; @@ -641,7 +641,7 @@ void utils::Rotation::checkUnitary() d(i+1) = h + s * (c * g + s * d(i)); // Accumulate transformation. - for (int k = 0; k < n; k++) { + for (size_t k = 0; k < n; ++k) { h = V(k, i+1); V(k, i+1) = s * V(k, i) + c * h; V(k, i) = c * V(k, i) - s * h; @@ -660,10 +660,10 @@ void utils::Rotation::checkUnitary() } // Sort eigenvalues and corresponding vectors. - for (int i = 0; i < n-1; i++) { + for (size_t i = 0; i < n-1; ++i) { int k = i; utils::Scalar p = d(i); - for (int j = i+1; j < n; j++) { + for (size_t j = i+1; j < n; ++j) { if (d(j) < p) { k = j; p = d(j); @@ -672,7 +672,7 @@ void utils::Rotation::checkUnitary() if (k != i) { d(k) = d(i); d(i) = p; - for (int j = 0; j < n; j++) { + for (size_t j = 0; j < n; ++j) { p = V(j, i); V(j, i) = V(j, k); V(j, k) = p; @@ -724,8 +724,8 @@ void utils::Rotation::checkUnitary() utils::Matrix3d result; - for (int i = 0; i < n; i++) { - for (int j = 0; j < n; j++) { + for (size_t i = 0; i < n; ++i) { + for (size_t j = 0; j < n; ++j) { result(i, j) = Um(i, j); } } @@ -742,8 +742,8 @@ void utils::Rotation::checkUnitary() result(1,2) *= lambda_inverse(2); result(2,2) *= lambda_inverse(2); - for (int i = 0; i < n; i++) { - for (int j = 0; j < n; j++) { + for (size_t i = 0; i < n; ++i) { + for (size_t j = 0; j < n; ++j) { Um(i, j) = result(i, j); } } @@ -779,7 +779,7 @@ void utils::Rotation::checkUnitary() // two (arbitrary) vectors orthogonal to the eigenvector // for the large singular value int done = 0; - for(int dim = 0; dim < 3; dim++) + for(size_t dim = 0; dim < 3; ++dim) { int dim_a = dim; int dim_b = (dim + 1) % 3; @@ -793,7 +793,7 @@ void utils::Rotation::checkUnitary() // find smallest abs component of v int smallest_idx = 0; - for(int dim = 1; dim < 3; dim++) + for(size_t dim = 1; dim < 3; ++dim) if (fabs(vec1(dim)) < fabs(vec1(smallest_idx))) smallest_idx = dim; @@ -827,7 +827,7 @@ void utils::Rotation::checkUnitary() // for the two large singular values if(!done) { - for(int dim = 0; dim < 3; dim++) + for(size_t dim = 0; dim < 3; ++dim) { int dim_a = dim; int dim_b = (dim + 1) % 3; @@ -870,7 +870,7 @@ void utils::Rotation::checkUnitary() // negative determinant // find the smallest singular value (they are all non-negative) int smallest_singular_value_idx = 0; - for(int dim=1; dim<3; dim++) + for(size_t dim=1; dim<3; ++dim) if (lambda(dim) < lambda(smallest_singular_value_idx)) smallest_singular_value_idx = dim; From 1a374f22653f4edb60439c27bfa309f1f82fbe79 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 14:52:10 -0400 Subject: [PATCH 11/17] Changed the warning for det=0 --- src/Utils/Rotation.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 16fc0f2c..9026a24f 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -369,13 +369,12 @@ void utils::Rotation::checkUnitary() if( det <= 1e-6 ) { use_svd = true; - std::cout << "Oups, SVD serait utile!\n"; break; } - if( det == 0.0 ) + utils::Error::check(det == 0.0, "Warning zero determinant encountered in polar decomposition"); + if (det == 0.0) { - printf("Warning (polarDecomposition) : zero determinant encountered.\n"); break; } From 841328da8118a8762effb604ef3d9edea9528a46 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 16:49:44 -0400 Subject: [PATCH 12/17] replaced warnings and matrix declaration --- src/Utils/Rotation.cpp | 59 ++++++++++++------------------------------ 1 file changed, 16 insertions(+), 43 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index 9026a24f..c1228eb0 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -368,13 +368,14 @@ void utils::Rotation::checkUnitary() if( det <= 1e-6 ) { + utils::Error::warning("Warning determinant <= 1e-6 encountered in polar decomposition"); use_svd = true; break; } - utils::Error::check(det == 0.0, "Warning zero determinant encountered in polar decomposition"); if (det == 0.0) { + utils::Error::warning("Warning zero determinant encountered in polar decomposition"); break; } @@ -401,15 +402,9 @@ void utils::Rotation::checkUnitary() utils::Scalar g1 = gamma * 0.5f; utils::Scalar g2 = 0.5f / (gamma * det); - for(size_t i = 0; i < 3; ++i) - { - for(size_t j = 0; i < 3; ++j) - { - Ek(i, j) = Mk(i, j); - Mk(i, j) = g1 * Mk(i, j) + g2 * M_adj_Tk(i, j); - Ek(i, j) -= Mk(i, j); - } - } + Ek = Mk; + Mk = g1 * Mk + g2 * M_adj_Tk; + Ek -= Mk; //Ek one-norm utils::Scalar E_one_norm = 0.0f; @@ -460,11 +455,7 @@ void utils::Rotation::checkUnitary() int n = 3; utils::Vector3d e; - for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j < n; ++j) { - V(i, j) = normalEq(i, j); - } - } + V = normalEq; // Symmetric Householder reduction to tridiagonal form. (tred2) @@ -721,31 +712,17 @@ void utils::Rotation::checkUnitary() // U = F * V * diag(SigmaInverse) Um = Mm * Vm; - utils::Matrix3d result; + Um(0,0) *= lambda_inverse(0); + Um(1,0) *= lambda_inverse(0); + Um(2,0) *= lambda_inverse(0); - for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j < n; ++j) { - result(i, j) = Um(i, j); - } - } + Um(0,1) *= lambda_inverse(1); + Um(1,1) *= lambda_inverse(1); + Um(2,1) *= lambda_inverse(1); - result(0,0) *= lambda_inverse(0); - result(1,0) *= lambda_inverse(0); - result(2,0) *= lambda_inverse(0); - - result(0,1) *= lambda_inverse(1); - result(1,1) *= lambda_inverse(1); - result(2,1) *= lambda_inverse(1); - - result(0,2) *= lambda_inverse(2); - result(1,2) *= lambda_inverse(2); - result(2,2) *= lambda_inverse(2); - - for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j < n; ++j) { - Um(i, j) = result(i, j); - } - } + Um(0,2) *= lambda_inverse(2); + Um(1,2) *= lambda_inverse(2); + Um(2,2) *= lambda_inverse(2); // In theory, U is now orthonormal, U^T U = U U^T = I .. // it may be a rotation or a reflection, depending on F. @@ -889,13 +866,9 @@ void utils::Rotation::checkUnitary() R = Mk.transpose(); } - - std::cout << "On s'est rendu, youpi!\n"; - // utils::Matrix3d mat; - utils::Error::check(false, - utils::String("The Rotation matrix square norm is equal to ") + utils::Error::raise(utils::String("The Rotation matrix square norm is equal to ") + sqrtNorm + ", but should be equal to 3. Consider replacing the RT matrix by this closest orthonormal matrix \n[" + R(0, 0) + ", " + R(0, 1) + ", " + R(0, 2) + "\n" + R(1, 0) + ", " + R(1, 1) + ", " + R(1, 2) + "\n" From 2c251152d3d0bce70d4e5794fe3b1aa944b5a164 Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 17:09:11 -0400 Subject: [PATCH 13/17] Matrix3d do not have abs func --- src/Utils/Rotation.cpp | 29 ++++++++++++----------------- 1 file changed, 12 insertions(+), 17 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index c1228eb0..efc5df1d 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -337,7 +337,7 @@ void utils::Rotation::checkUnitary() utils::Scalar M_one_norm = 0.0f; for (size_t i = 0; i < 3; ++i) { - utils::Scalar col_abs_sum = sum(fabs(M.block(i, 0, 1, 3))); + utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); if (col_abs_sum > M_one_norm) M_one_norm = col_abs_sum; } @@ -346,7 +346,7 @@ void utils::Rotation::checkUnitary() utils::Scalar M_inf_norm = 0.0; for (size_t i = 0; i < 3; ++i) { - utils::Scalar row_sum = sum(fabs(M.block(0, i, 3, 1))); + utils::Scalar row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); if (row_sum > M_inf_norm) M_inf_norm = row_sum; } @@ -383,7 +383,7 @@ void utils::Rotation::checkUnitary() utils::Scalar MadjT_one_norm = 0.0f; for (size_t i = 0; i < 3; ++i) { - utils::Scalar col_abs_sum = sum(fabs(M_adj_Tk.block(0, i, 3, 1))); + utils::Scalar col_abs_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); if (col_abs_sum > MadjT_one_norm) MadjT_one_norm = col_abs_sum; } @@ -392,7 +392,7 @@ void utils::Rotation::checkUnitary() utils::Scalar MadjT_inf_norm = 0.0; for (size_t i = 0; i < 3; ++i) { - utils::Scalar row_sum = fabs(M_adj_Tk.block(i, 0, 1, 3))); + utils::Scalar row_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); if (row_sum > M_inf_norm) M_inf_norm = row_sum; } @@ -410,7 +410,7 @@ void utils::Rotation::checkUnitary() utils::Scalar E_one_norm = 0.0f; for (size_t i = 0; i < 3; ++i) { - utils::Scalar col_abs_sum = fabs(Ek.block(0, i, 3, 1))); + utils::Scalar col_abs_sum = fabs(Ek(i, 0)) + fabs(Ek(i, 1)) + fabs(Ek(i, 2)); if (col_abs_sum > E_one_norm) E_one_norm = col_abs_sum; } @@ -419,7 +419,7 @@ void utils::Rotation::checkUnitary() utils::Scalar M_one_norm = 0.0f; for (size_t i = 0; i < 3; ++i) { - utils::Scalar col_abs_sum = fabs(M.block(0, i, 3, 1))); + utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); if (col_abs_sum > M_one_norm) M_one_norm = col_abs_sum; } @@ -428,7 +428,7 @@ void utils::Rotation::checkUnitary() utils::Scalar M_inf_norm = 0.0; for (size_t i = 0; i < 3; ++i) { - utils::Scalar row_sum = fabs(M.block(i, 0, 1, 3))); + utils::Scalar row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); if (row_sum > M_inf_norm) M_inf_norm = row_sum; } @@ -459,13 +459,10 @@ void utils::Rotation::checkUnitary() // Symmetric Householder reduction to tridiagonal form. (tred2) - for (size_t j = 0; j < n; ++j) { - d(j) = V(n-1, j); - } + V.block(n-1, 0, 1, 3); // Householder reduction to tridiagonal form. - - for (size_t = n-1; i > 0; --i) { + for (size_t i = n-1; i > 0; --i) { // Scale to avoid under/overflow. @@ -561,14 +558,12 @@ void utils::Rotation::checkUnitary() V(n-1, j) = 0.0; } V(n-1, n-1) = 1.0; - e(0) = 0.0; // Symmetric tridiagonal QL algorithm. (td12) - for (size_t i = 1; i < n; ++i) { - e(i-1) = e(i); - } - e(n-1) = 0.0; + e(0) = e(1); + e(1) = e(2); + e(2) = 0.0; utils::Scalar f = 0.0f; utils::Scalar tst1 = 0.0f; From 6105d2f362fa8a0b8b82c9fa64435f28901a7bbf Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Mon, 27 Sep 2021 17:29:32 -0400 Subject: [PATCH 14/17] error warnign --- src/Utils/Rotation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index efc5df1d..d4cdb2d7 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -368,14 +368,14 @@ void utils::Rotation::checkUnitary() if( det <= 1e-6 ) { - utils::Error::warning("Warning determinant <= 1e-6 encountered in polar decomposition"); + utils::Error::warning(false, utils::String("Warning determinant <= 1e-6 encountered in polar decomposition")); use_svd = true; break; } if (det == 0.0) { - utils::Error::warning("Warning zero determinant encountered in polar decomposition"); + utils::Error::warning(false, utils::String("Warning zero determinant encountered in polar decomposition")); break; } From b4df00f199b69e837ff92f0f38291fa152d4b00f Mon Sep 17 00:00:00 2001 From: EveCharbie Date: Tue, 28 Sep 2021 16:00:01 -0400 Subject: [PATCH 15/17] Fix du mardi PM :) --- src/Utils/Rotation.cpp | 37 +++++++++++++++---------------------- 1 file changed, 15 insertions(+), 22 deletions(-) diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index d4cdb2d7..cf912792 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -334,7 +334,7 @@ void utils::Rotation::checkUnitary() Mk = M.transpose(); //M one-norm (column) - utils::Scalar M_one_norm = 0.0f; + utils::Scalar M_one_norm = 0.0; for (size_t i = 0; i < 3; ++i) { utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); @@ -351,7 +351,7 @@ void utils::Rotation::checkUnitary() M_inf_norm = row_sum; } - do + do //for { utils::Matrix3d M_adj_Tk; @@ -365,6 +365,7 @@ void utils::Rotation::checkUnitary() */ det = Mk(0, 0) * M_adj_Tk(0, 0) + Mk(0, 1) * M_adj_Tk(0, 1) + Mk(0, 2) * M_adj_Tk(0, 2); + utils::Error::error(det != 0.0, utils::String("Warning zero determinant encountered in polar decomposition")); if( det <= 1e-6 ) { @@ -373,14 +374,8 @@ void utils::Rotation::checkUnitary() break; } - if (det == 0.0) - { - utils::Error::warning(false, utils::String("Warning zero determinant encountered in polar decomposition")); - break; - } - //mat_adj_T one-norm - utils::Scalar MadjT_one_norm = 0.0f; + utils::Scalar MadjT_one_norm = 0.0; for (size_t i = 0; i < 3; ++i) { utils::Scalar col_abs_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); @@ -399,7 +394,7 @@ void utils::Rotation::checkUnitary() utils::Scalar gamma = sqrtf(sqrtf((MadjT_one_norm * MadjT_inf_norm) / (M_one_norm * M_inf_norm * det * det))); - utils::Scalar g1 = gamma * 0.5f; + utils::Scalar g1 = gamma * 0.5; utils::Scalar g2 = 0.5f / (gamma * det); Ek = Mk; @@ -407,7 +402,7 @@ void utils::Rotation::checkUnitary() Ek -= Mk; //Ek one-norm - utils::Scalar E_one_norm = 0.0f; + utils::Scalar E_one_norm = 0.0; for (size_t i = 0; i < 3; ++i) { utils::Scalar col_abs_sum = fabs(Ek(i, 0)) + fabs(Ek(i, 1)) + fabs(Ek(i, 2)); @@ -416,7 +411,7 @@ void utils::Rotation::checkUnitary() } //M one-norm - utils::Scalar M_one_norm = 0.0f; + utils::Scalar M_one_norm = 0.0; for (size_t i = 0; i < 3; ++i) { utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); @@ -565,8 +560,8 @@ void utils::Rotation::checkUnitary() e(1) = e(2); e(2) = 0.0; - utils::Scalar f = 0.0f; - utils::Scalar tst1 = 0.0f; + utils::Scalar f = 0.0; + utils::Scalar tst1 = 0.0; utils::Scalar eps = 1e-16; for (size_t l = 0; l < n; ++l) { @@ -693,15 +688,15 @@ void utils::Rotation::checkUnitary() Vm(2,0) *= -1.0; } - lambda(0) = (eigen_vals(0) > 0.0f) ? sqrtf(eigen_vals(0)) : 0.0f; - lambda(1) = (eigen_vals(1) > 0.0f) ? sqrtf(eigen_vals(1)) : 0.0f; - lambda(2) = (eigen_vals(2) > 0.0f) ? sqrtf(eigen_vals(2)) : 0.0f; + lambda(0) = (eigen_vals(0) > 0.0) ? sqrtf(eigen_vals(0)) : 0.0; + lambda(1) = (eigen_vals(1) > 0.0) ? sqrtf(eigen_vals(1)) : 0.0; + lambda(2) = (eigen_vals(2) > 0.0) ? sqrtf(eigen_vals(2)) : 0.0; // compute inverse of singular values // also check if singular values are close to zero - lambda_inverse(0) = (lambda(0) > tolerance) ? (1.0f / lambda(0)) : 0.0f; - lambda_inverse(1) = (lambda(1) > tolerance) ? (1.0f / lambda(1)) : 0.0f; - lambda_inverse(2) = (lambda(2) > tolerance) ? (1.0f / lambda(2)) : 0.0f; + lambda_inverse(0) = (lambda(0) > tolerance) ? (1.0f / lambda(0)) : 0.0; + lambda_inverse(1) = (lambda(1) > tolerance) ? (1.0f / lambda(1)) : 0.0; + lambda_inverse(2) = (lambda(2) > tolerance) ? (1.0f / lambda(2)) : 0.0; // compute U using the formula: // U = F * V * diag(SigmaInverse) @@ -861,8 +856,6 @@ void utils::Rotation::checkUnitary() R = Mk.transpose(); } - // utils::Matrix3d mat; - utils::Error::raise(utils::String("The Rotation matrix square norm is equal to ") + sqrtNorm + ", but should be equal to 3. Consider replacing the RT matrix by this closest orthonormal matrix \n[" + R(0, 0) + ", " + R(0, 1) + ", " + R(0, 2) + "\n" From 5b5a6f292d8d53ced0d802b77e926256556362df Mon Sep 17 00:00:00 2001 From: Pariterre Date: Tue, 28 Sep 2021 17:38:06 -0400 Subject: [PATCH 16/17] Changed the polar to SVD --- include/Utils/Matrix3d.h | 23 ++ src/Utils/Matrix3d.cpp | 35 +++ src/Utils/Rotation.cpp | 543 +-------------------------------------- test/test_utils.cpp | 32 +++ 4 files changed, 91 insertions(+), 542 deletions(-) diff --git a/include/Utils/Matrix3d.h b/include/Utils/Matrix3d.h index 8d9e724b..e6e3614c 100644 --- a/include/Utils/Matrix3d.h +++ b/include/Utils/Matrix3d.h @@ -50,6 +50,29 @@ class BIORBD_API Matrix3d : public RigidBodyDynamics::Math::Matrix3d other) : RigidBodyDynamics::Math::Matrix3d(other) {} #endif + + /// + /// \brief Return the 1-norm of the matrix + /// \return The 1-norm of the matrix + /// + utils::Scalar normOne() const; + + /// + /// \brief Return the inf-norm of the matrix + /// \return The inf-norm of the matrix + /// + utils::Scalar normInf() const; + +#ifndef BIORBD_USE_CASADI_MATH + /// + /// \brief Produce an orthonormal matrix from the current matrix + /// \return The othonormal matrix + /// + /// Code adapted from https://github.com/brainexcerpts/3x3_polar_decomposition/blob/master/src/polar_decomposition_3x3.inl + /// + utils::Matrix3d orthoNormalize() const; +#endif + #ifdef BIORBD_USE_CASADI_MATH /// diff --git a/src/Utils/Matrix3d.cpp b/src/Utils/Matrix3d.cpp index ceaccd48..47e10696 100644 --- a/src/Utils/Matrix3d.cpp +++ b/src/Utils/Matrix3d.cpp @@ -20,6 +20,41 @@ utils::Matrix3d::Matrix3d( } +utils::Scalar utils::Matrix3d::normOne() const +{ + utils::Scalar value = 0.0; + for (size_t i = 0; i < 3; ++i) + { + utils::Scalar col_sum = fabs((*this)(0, i)) + fabs((*this)(1, i)) + fabs((*this)(2, i)); + if (col_sum > value) + value = col_sum; + } + return value; +} + +utils::Scalar utils::Matrix3d::normInf() const +{ + utils::Scalar value = 0.0; + for (size_t i = 0; i < 3; ++i) + { + utils::Scalar row_sum = fabs((*this)(i, 0)) + fabs((*this)(i, 1)) + fabs((*this)(i, 2)); + if (row_sum > value) + value = row_sum; + } + return value; +} + +#ifndef BIORBD_USE_CASADI_MATH +utils::Matrix3d utils::Matrix3d::orthoNormalize() const +{ +#ifdef BIORBD_USE_EIGEN3_MATH + Eigen::JacobiSVD svd(*this, Eigen::ComputeFullU | Eigen::ComputeFullV); + return svd.matrixU() * svd.matrixV().transpose(); +#else +#error "SVD decomposition not implemented for non-eigen3 backend" +#endif +} +#endif #ifdef BIORBD_USE_CASADI_MATH diff --git a/src/Utils/Rotation.cpp b/src/Utils/Rotation.cpp index cf912792..beb42689 100644 --- a/src/Utils/Rotation.cpp +++ b/src/Utils/Rotation.cpp @@ -314,548 +314,7 @@ void utils::Rotation::checkUnitary() bool checkUnit(fabs(sqrtNorm - 3.) < 1e-4); if (!checkUnit) { - - // Code addapted from https://github.com/brainexcerpts/3x3_polar_decomposition/blob/master/src/polar_decomposition_3x3.inl - - // Placer inf_norm and one norm in a function - // créer fonction polar_decomposition et déplacer dans Matrix3d - - utils::Scalar tolerance = 1e-6; - utils::Matrix3d M = this->block(0, 0, 3, 3); - utils::Matrix3d R; - utils::Matrix3d S; - utils::Matrix3d Mk; - utils::Matrix3d Ek; - utils::Scalar det, E_one_norm; - bool use_svd = false; - - - // Mk = mat^T - Mk = M.transpose(); - - //M one-norm (column) - utils::Scalar M_one_norm = 0.0; - for (size_t i = 0; i < 3; ++i) - { - utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); - if (col_abs_sum > M_one_norm) - M_one_norm = col_abs_sum; - } - - //M infinity-norm (row) - utils::Scalar M_inf_norm = 0.0; - for (size_t i = 0; i < 3; ++i) - { - utils::Scalar row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); - if (row_sum > M_inf_norm) - M_inf_norm = row_sum; - } - - do //for - { - utils::Matrix3d M_adj_Tk; - - /* - // row 2 x row 3 - cross(&(Mk[3]), &(Mk[6]), &M_adj_Tk(0, 0))); - // row 3 x row 1 - cross_product(&(Mk[6]), &(Mk[0]), &(M_adj_Tk[3])); - // row 1 x row 2 - cross_product(&(Mk[0]), &(Mk[3]), &(M_adj_Tk[6])); - */ - - det = Mk(0, 0) * M_adj_Tk(0, 0) + Mk(0, 1) * M_adj_Tk(0, 1) + Mk(0, 2) * M_adj_Tk(0, 2); - utils::Error::error(det != 0.0, utils::String("Warning zero determinant encountered in polar decomposition")); - - if( det <= 1e-6 ) - { - utils::Error::warning(false, utils::String("Warning determinant <= 1e-6 encountered in polar decomposition")); - use_svd = true; - break; - } - - //mat_adj_T one-norm - utils::Scalar MadjT_one_norm = 0.0; - for (size_t i = 0; i < 3; ++i) - { - utils::Scalar col_abs_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); - if (col_abs_sum > MadjT_one_norm) - MadjT_one_norm = col_abs_sum; - } - - //mat infinity-norm - utils::Scalar MadjT_inf_norm = 0.0; - for (size_t i = 0; i < 3; ++i) - { - utils::Scalar row_sum = fabs(M_adj_Tk(i, 0)) + fabs(M_adj_Tk(i, 1)) + fabs(M_adj_Tk(i, 2)); - if (row_sum > M_inf_norm) - M_inf_norm = row_sum; - } - - - utils::Scalar gamma = sqrtf(sqrtf((MadjT_one_norm * MadjT_inf_norm) / (M_one_norm * M_inf_norm * det * det))); - utils::Scalar g1 = gamma * 0.5; - utils::Scalar g2 = 0.5f / (gamma * det); - - Ek = Mk; - Mk = g1 * Mk + g2 * M_adj_Tk; - Ek -= Mk; - - //Ek one-norm - utils::Scalar E_one_norm = 0.0; - for (size_t i = 0; i < 3; ++i) - { - utils::Scalar col_abs_sum = fabs(Ek(i, 0)) + fabs(Ek(i, 1)) + fabs(Ek(i, 2)); - if (col_abs_sum > E_one_norm) - E_one_norm = col_abs_sum; - } - - //M one-norm - utils::Scalar M_one_norm = 0.0; - for (size_t i = 0; i < 3; ++i) - { - utils::Scalar col_abs_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); - if (col_abs_sum > M_one_norm) - M_one_norm = col_abs_sum; - } - - //M infinity-norm - utils::Scalar M_inf_norm = 0.0; - for (size_t i = 0; i < 3; ++i) - { - utils::Scalar row_sum = fabs(M(i, 0)) + fabs(M(i, 1)) + fabs(M(i, 2)); - if (row_sum > M_inf_norm) - M_inf_norm = row_sum; - } - - } while ( E_one_norm > M_one_norm * tolerance ); - - - if(use_svd) // && force_rotation - { - // use the SVD algorithm to compute R - utils::Matrix3d Mm = M; - utils::Matrix3d Um, Vm; - utils::Vector3d lambda; - utils::Vector3d lambda_inverse; - int modified_SVD = 1; - - // form Um^T Um and do eigendecomposition - utils::Matrix3d normalEq = Mm.transpose() * Mm; - utils::Vector3d eigen_vals; - utils::Matrix3d eigen_vecs; - utils::Matrix3d V; - utils::Vector3d d; - - int n = 3; - - utils::Vector3d e; - V = normalEq; - - - // Symmetric Householder reduction to tridiagonal form. (tred2) - V.block(n-1, 0, 1, 3); - - // Householder reduction to tridiagonal form. - for (size_t i = n-1; i > 0; --i) { - - // Scale to avoid under/overflow. - - utils::Scalar scale = 0.0; - utils::Scalar h = 0.0; - for (size_t k = 0; k < i; ++k) { - scale = scale + fabs(d(k)); - } - if (scale == 0.0) { - e(i) = d(i-1); - for (size_t j = 0; j < i; ++j) { - d(j) = V(i-1, j); - V(i, j) = 0.0; - V(j, i) = 0.0; - } - } else { - - // Generate Householder vector. - for (size_t k = 0; k < i; ++k) { - d(k) /= scale; - h += d(k) * d(k); - } - utils::Scalar f = d[i-1]; - utils::Scalar g = sqrtf(h); - if (f > 0) { - g = -g; - } - e(i) = scale * g; - h = h - f * g; - d(i-1) = f - g; - for (size_t j = 0; j < i; ++j) { - e(j) = 0.0; - } - - // Apply similarity transformation to remaining columns. - for (size_t j = 0; j < i; ++j) { - f = d(j); - V(j, i) = f; - g = e(j) + V(j, j) * f; - for (size_t k = j+1; k <= i-1; ++k) { - g += V(k, j) * d(k); - e(k) += V(k, j) * f; - } - e(j) = g; - } - f = 0.0; - for (size_t j = 0; j < i; ++j) { - e(j) /= h; - f += e(j) * d(j); - } - utils::Scalar hh = f / (h + h); - for (size_t j = 0; j < i; ++j) { - e[j] -= hh * d(j); - } - for (size_t j = 0; j < i; ++j) { - f = d(j); - g = e(j); - for (size_t k = j; k <= i-1; ++k) { - V(k, j) -= (f * e(k) + g * d(k)); - } - d(j) = V(i-1, j); - V(i, j) = 0.0; - } - } - d(i) = h; - } - - // Accumulate transformations - for (size_t i = 0; i < n-1; ++i) { - V(n-1, i) = V(i, i); - V(i, i) = 1.0; - utils::Scalar h = d(i+1); - if (h != 0.0) { - for (size_t k = 0; k <= i; ++k) { - d(k) = V(k, i+1) / h; - } - for (size_t j = 0; j <= i; ++j) { - utils::Scalar g = 0.0; - for (size_t k = 0; k <= i; ++k) { - g += V(k, i+1) * V(k, j); - } - for (size_t k = 0; k <= i; ++k) { - V(k, j) -= g * d(k); - } - } - } - for (size_t k = 0; k <= i; ++k) { - V(k, i+1) = 0.0; - } - } - for (size_t j = 0; j < n; ++j) { - d(j) = V(-1, j); - V(n-1, j) = 0.0; - } - V(n-1, n-1) = 1.0; - - - // Symmetric tridiagonal QL algorithm. (td12) - e(0) = e(1); - e(1) = e(2); - e(2) = 0.0; - - utils::Scalar f = 0.0; - utils::Scalar tst1 = 0.0; - utils::Scalar eps = 1e-16; - for (size_t l = 0; l < n; ++l) { - - // Find small subdiagonal element - tst1 = fmax(tst1, fabs(d(l)) + fabs(e(l))); - int m = l; - while (m < n) { - if (fabs(e(m)) <= eps*tst1) { - break; - } - m++; - } - - // If m == l, d[l] is an eigenvalue, - // otherwise, iterate. - if (m > l) { - int iter = 0; - do { - iter = iter + 1; // (Could check iteration count here.) - - // Compute implicit shift - utils::Scalar g = d(l); - utils::Scalar p = (d(l+1) - g) / (2.0f * e(l)); - - utils::Scalar r = sqrtf(p*p+1.0f*1.0f); - if (p < 0) { - r = -r; - } - d(l) = e(l) / (p + r); - d(l+1) = e(l) * (p + r); - utils::Scalar dl1 = d(l+1); - utils::Scalar h = g - d(l); - for (size_t i = l+2; i < n; ++i) { - d(i) -= h; - } - f = f + h; - - // Implicit QL transformation. - p = d(m); - utils::Scalar c = 1.0; - utils::Scalar c2 = c; - utils::Scalar c3 = c; - utils::Scalar el1 = e(l+1); - utils::Scalar s = 0.0; - utils::Scalar s2 = 0.0; - for (size_t i = m-1; i >= l; --i) { - c3 = c2; - c2 = c; - s2 = s; - g = c * e(i); - h = c * p; - r = sqrtf(p*p+e(i)*e(i)); - e(i+1) = s * r; - s = e(i) / r; - c = p / r; - p = c * d(i) - s * g; - d(i+1) = h + s * (c * g + s * d(i)); - - // Accumulate transformation. - for (size_t k = 0; k < n; ++k) { - h = V(k, i+1); - V(k, i+1) = s * V(k, i) + c * h; - V(k, i) = c * V(k, i) - s * h; - } - } - p = -s * s2 * c3 * el1 * e(l) / dl1; - e(l) = s * p; - d(l) = c * p; - - // Check for convergence. - - } while (fabs(e(l)) > eps*tst1); - } - d(l) = d(l) + f; - e(l) = 0.0; - } - - // Sort eigenvalues and corresponding vectors. - for (size_t i = 0; i < n-1; ++i) { - int k = i; - utils::Scalar p = d(i); - for (size_t j = i+1; j < n; ++j) { - if (d(j) < p) { - k = j; - p = d(j); - } - } - if (k != i) { - d(k) = d(i); - d(i) = p; - for (size_t j = 0; j < n; ++j) { - p = V(j, i); - V(j, i) = V(j, k); - V(j, k) = p; - } - } - } - - eigen_vals(0) = d(2); - eigen_vals(1) = d(1); - eigen_vals(2) = d(0); - - eigen_vecs(0, 0) = V(0, 2); - eigen_vecs(0, 1) = V(1, 2); - eigen_vecs(0, 2) = V(2, 2); - eigen_vecs(1, 0) = V(0, 1); - eigen_vecs(1, 1) = V(1, 1); - eigen_vecs(1, 2) = V(2, 1); - eigen_vecs(2, 0) = V(0, 0); - eigen_vecs(2, 1) = V(1, 0); - eigen_vecs(2, 2) = V(2, 0); - - - Vm = eigen_vecs.transpose(); - - // Handle situation: - // det(Vm) == -1 - // - multiply the first column of V by -1 - if (Vm.determinant() < 0.0) - { - // convert V into a rotation (multiply column 1 by -1) - Vm(0,0) *= -1.0; - Vm(1,0) *= -1.0; - Vm(2,0) *= -1.0; - } - - lambda(0) = (eigen_vals(0) > 0.0) ? sqrtf(eigen_vals(0)) : 0.0; - lambda(1) = (eigen_vals(1) > 0.0) ? sqrtf(eigen_vals(1)) : 0.0; - lambda(2) = (eigen_vals(2) > 0.0) ? sqrtf(eigen_vals(2)) : 0.0; - - // compute inverse of singular values - // also check if singular values are close to zero - lambda_inverse(0) = (lambda(0) > tolerance) ? (1.0f / lambda(0)) : 0.0; - lambda_inverse(1) = (lambda(1) > tolerance) ? (1.0f / lambda(1)) : 0.0; - lambda_inverse(2) = (lambda(2) > tolerance) ? (1.0f / lambda(2)) : 0.0; - - // compute U using the formula: - // U = F * V * diag(SigmaInverse) - Um = Mm * Vm; - - Um(0,0) *= lambda_inverse(0); - Um(1,0) *= lambda_inverse(0); - Um(2,0) *= lambda_inverse(0); - - Um(0,1) *= lambda_inverse(1); - Um(1,1) *= lambda_inverse(1); - Um(2,1) *= lambda_inverse(1); - - Um(0,2) *= lambda_inverse(2); - Um(1,2) *= lambda_inverse(2); - Um(2,2) *= lambda_inverse(2); - - // In theory, U is now orthonormal, U^T U = U U^T = I .. - // it may be a rotation or a reflection, depending on F. - // But in practice, if singular values are small or zero, - // it may not be orthonormal, so we need to fix it. - // Handle situation: - // 2. An entry of Sigma is near zero - // --------------------------------------------------------- - - if ((lambda(0) < tolerance) && (lambda(1) < tolerance) && (lambda(2) < tolerance)) - { - // extreme case, all singular values are small, - // material has collapsed almost to a point - // see [Irving 04], p. 4 - Um(0, 0) = 1.0; - Um(0, 1) = 0.0; - Um(0, 2) = 0.0; - Um(1, 0) = 0.0; - Um(1, 1) = 1.0; - Um(1, 2) = 0.0; - Um(2, 0) = 0.0; - Um(2, 1) = 0.0; - Um(2, 2) = 1.0; - - } - else - { - // handle the case where two singular values are small, - // but the third one is not handle it by computing - // two (arbitrary) vectors orthogonal to the eigenvector - // for the large singular value - int done = 0; - for(size_t dim = 0; dim < 3; ++dim) - { - int dim_a = dim; - int dim_b = (dim + 1) % 3; - int dim_c = (dim + 2) % 3; - if ((lambda(dim_b) < tolerance) && (lambda(dim_c) < tolerance)) - { - // only the column dimA can be trusted, - // columns dimB and dimC correspond to tiny singular values - utils::Vector3d vec1(Um(0,dim_a), Um(1,dim_a), Um(2,dim_a)); // column dimA - utils::Vector3d vec2; - - // find smallest abs component of v - int smallest_idx = 0; - for(size_t dim = 1; dim < 3; ++dim) - if (fabs(vec1(dim)) < fabs(vec1(smallest_idx))) - smallest_idx = dim; - - utils::Vector3d axis; - axis(smallest_idx) = 1.0; - - // this cross-product will be non-zero (as long as v is not zero) - vec2 = (vec1.cross(axis)).normalized(); - - utils::Vector3d vec3 = (vec1.cross(vec2)).normalized(); - Um(0, dim_b) = vec2(0); - Um(1, dim_b) = vec2(1); - Um(2, dim_b) = vec2(2); - Um(0, dim_c) = vec3(0); - Um(1, dim_c) = vec3(1); - Um(2, dim_c) = vec3(2); - if (Um.determinant() < 0.0) - { - Um(0, dim_b) *= -1.0; - Um(1, dim_b) *= -1.0; - Um(2, dim_b) *= -1.0; - } - done = 1; - break; // out of for - } - } - - // handle the case where one singular value is small, - // but the other two are not - // handle it by computing the cross product of the two eigenvectors - // for the two large singular values - if(!done) - { - for(size_t dim = 0; dim < 3; ++dim) - { - int dim_a = dim; - int dim_b = (dim + 1) % 3; - int dim_c = (dim + 2) % 3; - - if(lambda(dim_a) < tolerance) - { - // columns dimB and dimC are both good, - // but column dimA corresponds to a tiny singular value - - utils::Vector3d vec1(Um(0,dim_b), Um(1,dim_b), Um(2,dim_b)); // column dimB - utils::Vector3d vec2(Um(0,dim_c), Um(1,dim_c), Um(2,dim_c)); // column dimC - utils::Vector3d vec3 = (vec1.cross(vec2)).normalized(); - Um(0, dim_a) = vec3(0); - Um(1, dim_a) = vec3(1); - Um(2, dim_a) = vec3(2); - if(Um.determinant() < 0.0) - { - Um(0, dim_a) *= -1.0; - Um(1, dim_a) *= -1.0; - Um(2, dim_a) *= -1.0; - } - done = 1; - break; // out of for - } - } - } - - if ((!done) && (modified_SVD == 1)) - { - // Handle situation: - // negative determinant (Tet is inverted in solid mechanics) - // - check if det(U) == -1 - // - If yes, then negate the minimal element of Sigma - // and the corresponding column of U - - utils::Scalar det_U = Um.determinant(); - if (det_U < 0.0) - { - // negative determinant - // find the smallest singular value (they are all non-negative) - int smallest_singular_value_idx = 0; - for(size_t dim=1; dim<3; ++dim) - if (lambda(dim) < lambda(smallest_singular_value_idx)) - smallest_singular_value_idx = dim; - - // negate the smallest singular value - lambda(smallest_singular_value_idx) *= -1.0; - Um(0, smallest_singular_value_idx) *= -1.0; - Um(1, smallest_singular_value_idx) *= -1.0; - Um(2, smallest_singular_value_idx) *= -1.0; - } - } - } - R = Um * Vm.transpose(); - } - else - { - // R = Mk^T - R = Mk.transpose(); - } - + utils::Matrix3d R(orthoNormalize()); utils::Error::raise(utils::String("The Rotation matrix square norm is equal to ") + sqrtNorm + ", but should be equal to 3. Consider replacing the RT matrix by this closest orthonormal matrix \n[" + R(0, 0) + ", " + R(0, 1) + ", " + R(0, 2) + "\n" diff --git a/test/test_utils.cpp b/test/test_utils.cpp index a7f46f06..26eff495 100644 --- a/test/test_utils.cpp +++ b/test/test_utils.cpp @@ -507,6 +507,38 @@ TEST(Matrix, unitTest) } } +TEST(matrix3d, unitTest){ + utils::Matrix3d R(1, 2, 3, + 4, 5, 6, + 7, 8, 9); + { + SCALAR_TO_DOUBLE(one, R.normOne()); + EXPECT_NEAR(one, 18, requiredPrecision); + } + + { + SCALAR_TO_DOUBLE(inf, R.normInf()); + EXPECT_NEAR(inf, 24, requiredPrecision); + } + + { + utils::Matrix3d orthoNorm = R.orthoNormalize(); + std::vector expectedMult({ + -0.419386184128578, -0.2775187761147384, 0.8643486318991006, + -0.2775187761147384, 0.9457394527596399, 0.16899768163401963, + 0.8643486318991006, 0.16899768163401974, 0.47364673136893787 + }); + int cmp(0); + for (unsigned int i=0; i<3; ++i) { + for (unsigned int j=0; j<3; ++j) { + SCALAR_TO_DOUBLE(m, orthoNorm(i, j)); + EXPECT_NEAR(m, expectedMult[cmp++], requiredPrecision); + } + } + } + +} + TEST(Rotation, unitTest) { utils::Rotation rot1( From a9fd7d55ebfddc14cc83d2cde330bf60263440bb Mon Sep 17 00:00:00 2001 From: Pariterre Date: Tue, 28 Sep 2021 18:19:16 -0400 Subject: [PATCH 17/17] Removed some method for casadi --- include/Utils/Matrix3d.h | 2 +- src/Utils/Matrix3d.cpp | 2 +- test/test_utils.cpp | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/include/Utils/Matrix3d.h b/include/Utils/Matrix3d.h index e6e3614c..60175e6f 100644 --- a/include/Utils/Matrix3d.h +++ b/include/Utils/Matrix3d.h @@ -51,6 +51,7 @@ class BIORBD_API Matrix3d : public RigidBodyDynamics::Math::Matrix3d RigidBodyDynamics::Math::Matrix3d(other) {} #endif +#ifndef BIORBD_USE_CASADI_MATH /// /// \brief Return the 1-norm of the matrix /// \return The 1-norm of the matrix @@ -63,7 +64,6 @@ class BIORBD_API Matrix3d : public RigidBodyDynamics::Math::Matrix3d /// utils::Scalar normInf() const; -#ifndef BIORBD_USE_CASADI_MATH /// /// \brief Produce an orthonormal matrix from the current matrix /// \return The othonormal matrix diff --git a/src/Utils/Matrix3d.cpp b/src/Utils/Matrix3d.cpp index 47e10696..173ddd23 100644 --- a/src/Utils/Matrix3d.cpp +++ b/src/Utils/Matrix3d.cpp @@ -20,6 +20,7 @@ utils::Matrix3d::Matrix3d( } +#ifndef BIORBD_USE_CASADI_MATH utils::Scalar utils::Matrix3d::normOne() const { utils::Scalar value = 0.0; @@ -44,7 +45,6 @@ utils::Scalar utils::Matrix3d::normInf() const return value; } -#ifndef BIORBD_USE_CASADI_MATH utils::Matrix3d utils::Matrix3d::orthoNormalize() const { #ifdef BIORBD_USE_EIGEN3_MATH diff --git a/test/test_utils.cpp b/test/test_utils.cpp index 26eff495..6ac0ca42 100644 --- a/test/test_utils.cpp +++ b/test/test_utils.cpp @@ -507,6 +507,7 @@ TEST(Matrix, unitTest) } } +#ifndef BIORBD_USE_CASADI_MATH TEST(matrix3d, unitTest){ utils::Matrix3d R(1, 2, 3, 4, 5, 6, @@ -538,6 +539,7 @@ TEST(matrix3d, unitTest){ } } +#endif TEST(Rotation, unitTest) {