From 63f45ec27e64387d6fa40f071ee2d4c97c86e6e2 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Mar 2023 15:39:43 +0100
Subject: [PATCH] added finite checks for vectors and matrices

---
 mission/controller/acs/util/MathOperations.h | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h
index f8537740..b344451a 100644
--- a/mission/controller/acs/util/MathOperations.h
+++ b/mission/controller/acs/util/MathOperations.h
@@ -404,6 +404,26 @@ class MathOperations {
     std::memcpy(inverse, identity, sizeof(identity));
     return 0;  // successful inversion
   }
+
+  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
+    for (uint8_t i = 0; i < size; i++) {
+      if (not isfinite(inputVector[i])) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
+    for (uint8_t col = 0; col < cols; col++) {
+      for (uint8_t row = 0; row < rows; row++) {
+        if (not isfinite(inputMatrix[row * cols + cols])) {
+          return false;
+        }
+      }
+    }
+    return true;
+  }
 };
 
 #endif /* ACS_MATH_MATHOPERATIONS_H_ */