libcamera: matrix: Add inverse() function

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

Signed-off-by: Stefan Klug <stefan.klug@ideasonboard.com>
Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com>
Reviewed-by: Paul Elder <paul.elder@ideasonboard.com>

include/libcamera/internal/matrix.h

@@ -19,6 +19,12 @@ namespace libcamera {
 
 LOG_DECLARE_CATEGORY(Matrix)
 
+#ifndef __DOXYGEN__
+template<typename T>
+bool matrixInvert(Span<const T> dataIn, Span<T> dataOut, unsigned int dim,
+		  Span<T> scratchBuffer, Span<unsigned int> swapBuffer);
+#endif /* __DOXYGEN__ */
+
 template<typename T, unsigned int Rows, unsigned int Cols>
 class Matrix
 {
@@ -91,6 +97,23 @@ public:
 		return *this;
 	}
 
+	Matrix<T, Rows, Cols> inverse(bool *ok = nullptr) const
+	{
+		static_assert(Rows == Cols, "Matrix must be square");
+
+		Matrix<T, Rows, Cols> inverse;
+		std::array<T, Rows * Cols * 2> scratchBuffer;
+		std::array<unsigned int, Rows> swapBuffer;
+		bool res = matrixInvert(Span<const T>(data_),
+					Span<T>(inverse.data_),
+					Rows,
+					Span<T>(scratchBuffer),
+					Span<unsigned int>(swapBuffer));
+		if (ok)
+			*ok = res;
+		return inverse;
+	}
+
 private:
 	/*
 	 * \todo The initializer is only necessary for the constructor to be