[PATCH 03/10] libcamera: matrix: Add inverse() function

[Stefan Klug]

For calculations in upcoming algorithm patches, the inverse of a matrix
is required. Add an implementation of the inverse() function for 3x3
matrices.

Signed-off-by: Stefan Klug <stefan.klug at ideasonboard.com>
---
 include/libcamera/internal/matrix.h | 24 ++++++++++++++++++++++++
 src/libcamera/matrix.cpp            | 10 ++++++++++
 2 files changed, 34 insertions(+)

diff --git a/include/libcamera/internal/matrix.h b/include/libcamera/internal/matrix.h
index ca81dcda93c4..053336e9cfa4 100644
--- a/include/libcamera/internal/matrix.h
+++ b/include/libcamera/internal/matrix.h
@@ -90,6 +90,30 @@ public:
 		return *this;
 	}
 
+	Matrix<T, Rows, Cols> inverse() const
+	{
+		static_assert(Rows == 3 && Cols == 3, "Matrix must be 3x3");
+
+		const auto &m = *this;
+		double det = m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) -
+			     m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) +
+			     m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
+
+		double invdet = 1 / det;
+
+		Matrix<T, Rows, Cols> ret;
+		ret[0][0] = (m[1][1] * m[2][2] - m[2][1] * m[1][2]) * invdet;
+		ret[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) * invdet;
+		ret[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * invdet;
+		ret[1][0] = (m[1][2] * m[2][0] - m[1][0] * m[2][2]) * invdet;
+		ret[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * invdet;
+		ret[1][2] = (m[1][0] * m[0][2] - m[0][0] * m[1][2]) * invdet;
+		ret[2][0] = (m[1][0] * m[2][1] - m[2][0] * m[1][1]) * invdet;
+		ret[2][1] = (m[2][0] * m[0][1] - m[0][0] * m[2][1]) * invdet;
+		ret[2][2] = (m[0][0] * m[1][1] - m[1][0] * m[0][1]) * invdet;
+		return ret;
+	}
+
 	template<typename T2>
 	Matrix<T2, Rows, Cols> cast() const
 	{
diff --git a/src/libcamera/matrix.cpp b/src/libcamera/matrix.cpp
index 91a3f16405a3..b17a1938f1ba 100644
--- a/src/libcamera/matrix.cpp
+++ b/src/libcamera/matrix.cpp
@@ -77,6 +77,16 @@ LOG_DEFINE_CATEGORY(Matrix)
  * \return Row \a i from the matrix, as a Span
  */
 
+/**
+ * \fn Matrix::inverse() const
+ * \brief Compute the inverse of the matrix
+ *
+ * This function computes the inverse of the matrix. It is only implemented for
+ * 3x3 matrices.
+ *
+ * \return The inverse of the matrix
+ */
+
 /**
  * \fn template<typename T2> Matrix<T2, Rows, Cols> Matrix::cast() const
  * \brief Cast the matrix to a different type
-- 
2.43.0