From 7c1860f2861354f909f1275ee7eafb0210f2a1f4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 21 Aug 2021 19:12:34 +0200
Subject: [PATCH] geninv(): pass result as argument

depending on usage, this reduces stack usage a bit.
---
 matrix/PseudoInverse.hpp | 13 ++++++++-----
 test/pseudoInverse.cpp   | 27 ++++++++++++++++++++-------
 2 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/matrix/PseudoInverse.hpp b/matrix/PseudoInverse.hpp
index a2df21b5d2..1590efecfd 100644
--- a/matrix/PseudoInverse.hpp
+++ b/matrix/PseudoInverse.hpp
@@ -21,7 +21,7 @@ namespace matrix
  * Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809
  */
 template<typename Type, size_t M, size_t N>
-Matrix<Type, N, M> geninv(const Matrix<Type, M, N> & G)
+bool geninv(const Matrix<Type, M, N> & G, Matrix<Type, N, M>& res)
 {
     size_t rank;
     if (M <= N) {
@@ -31,11 +31,12 @@ Matrix<Type, N, M> geninv(const Matrix<Type, M, N> & G)
         A = L.transpose() * L;
         SquareMatrix<Type, M> X;
         if (!inv(A, X, rank)) {
-            return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
+            res = Matrix<Type, N, M>();
+            return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
         }
         // doing an intermediate assignment reduces stack usage
         A = X * X * L.transpose();
-        return G.transpose() * (L * A);
+        res = G.transpose() * (L * A);
 
     } else {
         SquareMatrix<Type, N> A = G.transpose() * G;
@@ -44,12 +45,14 @@ Matrix<Type, N, M> geninv(const Matrix<Type, M, N> & G)
         A = L.transpose() * L;
         SquareMatrix<Type, N> X;
         if(!inv(A, X, rank)) {
-            return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
+            res = Matrix<Type, N, M>();
+            return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
         }
         // doing an intermediate assignment reduces stack usage
         A = X * X * L.transpose();
-        return (L * A) * G.transpose();
+        res = (L * A) * G.transpose();
     }
+    return true;
 }
 
 
diff --git a/test/pseudoInverse.cpp b/test/pseudoInverse.cpp
index e7d8b10187..a313a58bcd 100644
--- a/test/pseudoInverse.cpp
+++ b/test/pseudoInverse.cpp
@@ -22,7 +22,9 @@ int main()
     };
 
     Matrix<float, 3, 4> A0(data0);
-    Matrix<float, 4, 3> A0_I = geninv(A0);
+    Matrix<float, 4, 3> A0_I;
+    bool ret = geninv(A0, A0_I);
+    TEST(ret);
     Matrix<float, 4, 3> A0_I_check(data0_check);
 
     TEST((A0_I - A0_I_check).abs().max() < 1e-5);
@@ -42,7 +44,9 @@ int main()
     };
 
     Matrix<float, 4, 3> A1(data1);
-    Matrix<float, 3, 4> A1_I = geninv(A1);
+    Matrix<float, 3, 4> A1_I;
+    ret = geninv(A1, A1_I);
+    TEST(ret);
     Matrix<float, 3, 4> A1_I_check(data1_check);
 
     TEST((A1_I - A1_I_check).abs().max() < 1e-5);
@@ -53,7 +57,8 @@ int main()
     Matrix<float, n_large - 1, n_large> A_large_I;
 
     for (size_t i = 0; i < n_large; i++) {
-        A_large_I = geninv(A_large);
+        ret = geninv(A_large, A_large_I);
+        TEST(ret);
         TEST(isEqual(A_large, A_large_I.T()));
     }
 
@@ -69,13 +74,17 @@ int main()
     };
 
     SquareMatrix<float, 3> A2(data2);
-    SquareMatrix<float, 3> A2_I = geninv(A2);
+    SquareMatrix<float, 3> A2_I;
+    ret = geninv(A2, A2_I);
+    TEST(ret);
     SquareMatrix<float, 3> A2_I_check(data2_check);
     TEST((A2_I - A2_I_check).abs().max() < 1e-3);
 
     // Null matrix test
     Matrix<float, 6, 16> A3;
-    Matrix<float, 16, 6> A3_I = geninv(A3);
+    Matrix<float, 16, 6> A3_I;
+    ret = geninv(A3, A3_I);
+    TEST(ret);
     Matrix<float, 16, 6> A3_I_check;
     TEST((A3_I - A3_I_check).abs().max() < 1e-5);
 
@@ -108,7 +117,9 @@ int main()
         { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
     };
     Matrix<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
-    Matrix<float, 16, 6> A = geninv(B);
+    Matrix<float, 16, 6> A;
+    ret = geninv(B, A);
+    TEST(ret);
     TEST((A - A_check).abs().max() < 1e-5);
 
     // Real-world test case
@@ -120,7 +131,9 @@ int main()
         { 0.607814, -0.607814,  0.607814, -0.607814,  1.0000,  1.0000}
     };
     Matrix<float, 5, 6> real ( real_alloc);
-    Matrix<float, 6, 5> real_pinv = geninv(real);
+    Matrix<float, 6, 5> real_pinv;
+    ret = geninv(real, real_pinv);
+    TEST(ret);
 
     // from SVD-based inverse
     const float real_pinv_expected_alloc[6][5] = {