Complete Vector3f, Vector4f, Matrix3f and Matrix4f as in Aventura

VectorMatrixLib/GaussJordanSolver.h

@@ -0,0 +1,106 @@
+//
+// MIT License
+// Copyright(c) 2021 - 2026 Olivier BARRY
+// 
+// This file is part of the C++ Aventura Project
+// 
+// VectorMatrix Math Library
+//
+
+#ifndef GAUSSJORDANSOLVER_H
+#define GAUSSJORDANSOLVER_H
+
+#include <cmath>
+#include "Exceptions.h"
+
+namespace vectormatrix
+{
+	// Matrix inversion by Gauss-Jordan elimination with partial pivoting,
+	// shared by Matrix3f and Matrix4f (same algorithm as Aventura GaussJordanSolver)
+	class GaussJordanSolver
+	{
+	public:
+		// Invert the N x N matrix a into result.
+		// Throws NotInvertibleMatrixException if a pivot is smaller than epsilon (singular matrix).
+		template <int N>
+		static void invert(const float (&a)[N][N], float (&result)[N][N], float epsilon)
+		{
+			float matrix[N][N];
+			for (int i = 0; i < N; i++)
+				for (int j = 0; j < N; j++)
+				{
+					matrix[i][j] = a[i][j];
+					result[i][j] = (i == j) ? 1.0f : 0.0f;
+				}
+
+			int r = 0; // last pivot row
+
+			// Browsing columns one by one
+			for (int j = 0; j < N; j++)
+			{
+				int k = indiceOfMaxRowInColumn(matrix, j, r);
+				float pivot = matrix[k][j];
+
+				if (std::fabs(pivot) < epsilon) throw NotInvertibleMatrixException();
+
+				// Divide all the row by the pivot to reduce the pivot to 1
+				for (int col = 0; col < N; col++)
+				{
+					matrix[k][col] /= pivot;
+					result[k][col] /= pivot;
+				}
+				// Swap the rows k and r
+				if (r != k)
+				{
+					swapRows(matrix, r, k);
+					swapRows(result, r, k);
+				}
+				for (int i = 0; i < N; i++)
+				{
+					if (i != r)
+					{
+						float matrix_ij = matrix[i][j];
+						for (int col = 0; col < N; col++)
+						{
+							matrix[i][col] -= matrix[r][col] * matrix_ij;
+							result[i][col] -= result[r][col] * matrix_ij;
+						}
+					}
+				}
+				r++;
+			}
+		}
+
+	private:
+		template <int N>
+		static void swapRows(float (&a)[N][N], int r1, int r2)
+		{
+			for (int col = 0; col < N; col++)
+			{
+				float tmp = a[r1][col];
+				a[r1][col] = a[r2][col];
+				a[r2][col] = tmp;
+			}
+		}
+
+		// Row index (from pivot row down) of the greatest absolute value in column col
+		template <int N>
+		static int indiceOfMaxRowInColumn(const float (&m)[N][N], int col, int pivot)
+		{
+			float max = std::fabs(m[pivot][col]);
+			int indiceMax = pivot;
+			for (int i = pivot + 1; i < N; i++)
+			{
+				float val = std::fabs(m[i][col]);
+				if (max < val)
+				{
+					max = val;
+					indiceMax = i;
+				}
+			}
+			return indiceMax;
+		}
+	};
+}
+
+#endif // GAUSSJORDANSOLVER_H

VectorMatrixLib/Matrix3f.cpp

@@ -8,9 +8,12 @@
 //
 
 #include <iostream>
+#include <string>
 #include "Matrix3f.h"
 #include "Matrix4f.h"
 #include "MathTools.h"
+#include "Exceptions.h"
+#include "GaussJordanSolver.h"
 
 namespace vectormatrix
 {
@@ -138,6 +141,106 @@ namespace vectormatrix
 		array_[x][y] = a;
 	}
 
+	Matrix3f Matrix3f::identity()
+	{
+		Matrix3f r;
+		r.setDiagonal(1);
+		return r;
+	}
+
+	void Matrix3f::setDiagonal(float v)
+	{
+		for (int i = 0; i < SIZE3; i++)
+			array_[i][i] = v;
+	}
+
+	Vector3f Matrix3f::getRow(int r) const
+	{
+		if (r < 0 || r >= SIZE3) throw IndexOutOfBoundException("Index out of bound while getting Row (" + std::to_string(r) + ") of Matrix3f");
+		return Vector3f(r, *this);
+	}
+
+	Vector3f Matrix3f::getColumn(int c) const
+	{
+		if (c < 0 || c >= SIZE3) throw IndexOutOfBoundException("Index out of bound while getting Column (" + std::to_string(c) + ") of Matrix3f");
+		return Vector3f(*this, c);
+	}
+
+	void Matrix3f::setRow(int r, const Vector3f& v)
+	{
+		if (r < 0 || r >= SIZE3) throw IndexOutOfBoundException("Index out of bound while setting Row (" + std::to_string(r) + ") of Matrix3f");
+		for (int j = 0; j < SIZE3; j++)
+			array_[r][j] = v.get(j);
+	}
+
+	void Matrix3f::setColumn(int c, const Vector3f& v)
+	{
+		if (c < 0 || c >= SIZE3) throw IndexOutOfBoundException("Index out of bound while setting Column (" + std::to_string(c) + ") of Matrix3f");
+		for (int i = 0; i < SIZE3; i++)
+			array_[i][c] = v.get(i);
+	}
+
+	std::vector<std::vector<float>> Matrix3f::getArray() const
+	{
+		std::vector<std::vector<float>> a(SIZE3, std::vector<float>(SIZE3));
+		for (int i = 0; i < SIZE3; i++)
+			for (int j = 0; j < SIZE3; j++)
+				a[i][j] = array_[i][j];
+		return a;
+	}
+
+	void Matrix3f::setArray(const std::vector<std::vector<float>>& a)
+	{
+		if (a.size() != SIZE3) throw MatrixArrayWrongSizeException("Wrong array row size (" + std::to_string(a.size()) + ") while setting Matrix3f from array");
+		for (int i = 0; i < SIZE3; i++)
+			if (a[i].size() != SIZE3) throw MatrixArrayWrongSizeException("Wrong array column size (" + std::to_string(a[i].size()) + ") while setting Matrix3f from array");
+		for (int i = 0; i < SIZE3; i++)
+			for (int j = 0; j < SIZE3; j++)
+				array_[i][j] = a[i][j];
+	}
+
+	float Matrix3f::trace() const
+	{
+		float t = 0;
+		for (int i = 0; i < SIZE3; i++)
+			t += array_[i][i];
+		return t;
+	}
+
+	bool Matrix3f::isIdentity() const
+	{
+		return *this == identity();
+	}
+
+	float Matrix3f::determinant() const
+	{
+		return array_[0][0] * (array_[1][1] * array_[2][2] - array_[1][2] * array_[2][1])
+			- array_[0][1] * (array_[1][0] * array_[2][2] - array_[1][2] * array_[2][0])
+			+ array_[0][2] * (array_[1][0] * array_[2][1] - array_[1][1] * array_[2][0]);
+	}
+
+	Matrix3f Matrix3f::transpose() const
+	{
+		Matrix3f r;
+		for (int i = 0; i < SIZE3; i++)
+			for (int j = 0; j < SIZE3; j++)
+				r.array_[i][j] = array_[j][i];
+		return r;
+	}
+
+	Matrix3f& Matrix3f::transposeEquals()
+	{
+		*this = transpose();
+		return *this;
+	}
+
+	Matrix3f Matrix3f::inverse() const
+	{
+		Matrix3f r;
+		GaussJordanSolver::invert<SIZE3>(array_, r.array_, EPSILON);
+		return r;
+	}
+
 	std::ostream& operator<<(std::ostream& strm, const Matrix3f& m)
 	{
 		strm << "Matrix3f(" << std::endl;

VectorMatrixLib/Matrix3f.h

@@ -11,6 +11,7 @@
 #define MATRIX3F_H
 
 #include <iostream>
+#include <vector>
 #include "Vector3f.h"
 
 namespace vectormatrix
@@ -23,6 +24,9 @@ namespace vectormatrix
 		// Constants
 		static const int SIZE3 = 3;
 
+		// Identity matrix (a new copy is returned each time)
+		static Matrix3f identity();
+
 		// Constructors
 		Matrix3f();
 		explicit Matrix3f(float a);
@@ -47,6 +51,23 @@ namespace vectormatrix
 		// Getters and Setters
 		float get(int x, int y) const;
 		void set(int x, int y, float a);
+		void setDiagonal(float v);
+		// Rows and columns (throw IndexOutOfBoundException if the index is out of bound)
+		Vector3f getRow(int r) const;
+		Vector3f getColumn(int c) const;
+		void setRow(int r, const Vector3f& v);
+		void setColumn(int c, const Vector3f& v);
+		// Copy of the elements, and set from an array (throws MatrixArrayWrongSizeException if not 3x3)
+		std::vector<std::vector<float>> getArray() const;
+		void setArray(const std::vector<std::vector<float>>& a);
+		// Other methods
+		float trace() const;
+		bool isIdentity() const;
+		float determinant() const;
+		Matrix3f transpose() const;
+		Matrix3f& transposeEquals();
+		// Throws NotInvertibleMatrixException if this matrix is singular
+		Matrix3f inverse() const;
 
 	private:
 		float array_[SIZE3][SIZE3];