From cbce83a1c8dcdae55993e7764ae8901f50ea540f Mon Sep 17 00:00:00 2001
From: Balduin <balduin@auterion.com>
Date: Tue, 10 Mar 2026 09:06:28 +0100
Subject: [PATCH] feat(matrix): add in-place nanToZero function

Adding this to the library, as with the new thrust setpoint definition
we can expect this to be needed in more and more places
---
 src/lib/matrix/matrix/Matrix.hpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp
index d17447b5b2..8df9ede9d9 100644
--- a/src/lib/matrix/matrix/Matrix.hpp
+++ b/src/lib/matrix/matrix/Matrix.hpp
@@ -604,6 +604,20 @@ public:
 		}
 	}
 
+	inline void nanToZero()
+	{
+
+		Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				if (std::isnan(self(i, j))) {
+					self(i, j) = static_cast<Type>(0.0f);
+				}
+			}
+		}
+	}
+
 	Matrix<Type, M, N> abs() const
 	{
 		Matrix<Type, M, N> r;