initial commit, 4.5 stable

thirdparty/jolt_physics/Jolt/Math/Matrix.h

@@ -0,0 +1,259 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#pragma once
+
+#include <Jolt/Math/Vector.h>
+#include <Jolt/Math/GaussianElimination.h>
+
+JPH_NAMESPACE_BEGIN
+
+/// Templatized matrix class
+template <uint Rows, uint Cols>
+class [[nodiscard]] Matrix
+{
+public:
+	/// Constructor
+	inline									Matrix() = default;
+	inline									Matrix(const Matrix &inM2)								{ *this = inM2; }
+
+	/// Dimensions
+	inline uint								GetRows() const											{ return Rows; }
+	inline uint								GetCols() const											{ return Cols; }
+
+	/// Zero matrix
+	inline void								SetZero()
+	{
+		for (uint c = 0; c < Cols; ++c)
+			mCol[c].SetZero();
+	}
+
+	inline static Matrix					sZero()													{ Matrix m; m.SetZero(); return m; }
+
+	/// Check if this matrix consists of all zeros
+	inline bool								IsZero() const
+	{
+		for (uint c = 0; c < Cols; ++c)
+			if (!mCol[c].IsZero())
+				return false;
+
+		return true;
+	}
+
+	/// Identity matrix
+	inline void								SetIdentity()
+	{
+		// Clear matrix
+		SetZero();
+
+		// Set diagonal to 1
+		for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc)
+			mCol[rc].mF32[rc] = 1.0f;
+	}
+
+	inline static Matrix					sIdentity()												{ Matrix m; m.SetIdentity(); return m; }
+
+	/// Check if this matrix is identity
+	bool									IsIdentity() const										{ return *this == sIdentity(); }
+
+	/// Diagonal matrix
+	inline void								SetDiagonal(const Vector<Rows < Cols? Rows : Cols> &inV)
+	{
+		// Clear matrix
+		SetZero();
+
+		// Set diagonal
+		for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc)
+			mCol[rc].mF32[rc] = inV[rc];
+	}
+
+	inline static Matrix					sDiagonal(const Vector<Rows < Cols? Rows : Cols> &inV)
+	{
+		Matrix m;
+		m.SetDiagonal(inV);
+		return m;
+	}
+
+	/// Copy a (part) of another matrix into this matrix
+	template <class OtherMatrix>
+		void								CopyPart(const OtherMatrix &inM, uint inSourceRow, uint inSourceCol, uint inNumRows, uint inNumCols, uint inDestRow, uint inDestCol)
+		{
+			for (uint c = 0; c < inNumCols; ++c)
+				for (uint r = 0; r < inNumRows; ++r)
+					mCol[inDestCol + c].mF32[inDestRow + r] = inM(inSourceRow + r, inSourceCol + c);
+		}
+
+	/// Get float component by element index
+	inline float							operator () (uint inRow, uint inColumn) const
+	{
+		JPH_ASSERT(inRow < Rows);
+		JPH_ASSERT(inColumn < Cols);
+		return mCol[inColumn].mF32[inRow];
+	}
+
+	inline float &							operator () (uint inRow, uint inColumn)
+	{
+		JPH_ASSERT(inRow < Rows);
+		JPH_ASSERT(inColumn < Cols);
+		return mCol[inColumn].mF32[inRow];
+	}
+
+	/// Comparison
+	inline bool								operator == (const Matrix &inM2) const
+	{
+		for (uint c = 0; c < Cols; ++c)
+			if (mCol[c] != inM2.mCol[c])
+				return false;
+		return true;
+	}
+
+	inline bool								operator != (const Matrix &inM2) const
+	{
+		for (uint c = 0; c < Cols; ++c)
+			if (mCol[c] != inM2.mCol[c])
+				return true;
+		return false;
+	}
+
+	/// Assignment
+	inline Matrix &							operator = (const Matrix &inM2)
+	{
+		for (uint c = 0; c < Cols; ++c)
+			mCol[c] = inM2.mCol[c];
+		return *this;
+	}
+
+	/// Multiply matrix by matrix
+	template <uint OtherCols>
+	inline Matrix<Rows, OtherCols>	operator * (const Matrix<Cols, OtherCols> &inM) const
+	{
+		Matrix<Rows, OtherCols> m;
+		for (uint c = 0; c < OtherCols; ++c)
+			for (uint r = 0; r < Rows; ++r)
+			{
+				float dot = 0.0f;
+				for (uint i = 0; i < Cols; ++i)
+					dot += mCol[i].mF32[r] * inM.mCol[c].mF32[i];
+				m.mCol[c].mF32[r] = dot;
+			}
+		return m;
+	}
+
+	/// Multiply vector by matrix
+	inline Vector<Rows>						operator * (const Vector<Cols> &inV) const
+	{
+		Vector<Rows> v;
+		for (uint r = 0; r < Rows; ++r)
+		{
+			float dot = 0.0f;
+			for (uint c = 0; c < Cols; ++c)
+				dot += mCol[c].mF32[r] * inV.mF32[c];
+			v.mF32[r] = dot;
+		}
+		return v;
+	}
+
+	/// Multiply matrix with float
+	inline Matrix							operator * (float inV) const
+	{
+		Matrix m;
+		for (uint c = 0; c < Cols; ++c)
+			m.mCol[c] = mCol[c] * inV;
+		return m;
+	}
+
+	inline friend Matrix					operator * (float inV, const Matrix &inM)
+	{
+		return inM * inV;
+	}
+
+	/// Per element addition of matrix
+	inline Matrix							operator + (const Matrix &inM) const
+	{
+		Matrix m;
+		for (uint c = 0; c < Cols; ++c)
+			m.mCol[c] = mCol[c] + inM.mCol[c];
+		return m;
+	}
+
+	/// Per element subtraction of matrix
+	inline Matrix							operator - (const Matrix &inM) const
+	{
+		Matrix m;
+		for (uint c = 0; c < Cols; ++c)
+			m.mCol[c] = mCol[c] - inM.mCol[c];
+		return m;
+	}
+
+	/// Transpose matrix
+	inline Matrix<Cols, Rows>				Transposed() const
+	{
+		Matrix<Cols, Rows> m;
+		for (uint r = 0; r < Rows; ++r)
+			for (uint c = 0; c < Cols; ++c)
+				m.mCol[r].mF32[c] = mCol[c].mF32[r];
+		return m;
+	}
+
+	/// Inverse matrix
+	bool									SetInversed(const Matrix &inM)
+	{
+		if constexpr (Rows != Cols) JPH_ASSERT(false);
+		Matrix copy(inM);
+		SetIdentity();
+		return GaussianElimination(copy, *this);
+	}
+
+	inline Matrix							Inversed() const
+	{
+		Matrix m;
+		m.SetInversed(*this);
+		return m;
+	}
+
+	/// To String
+	friend ostream &						operator << (ostream &inStream, const Matrix &inM)
+	{
+		for (uint i = 0; i < Cols - 1; ++i)
+			inStream << inM.mCol[i] << ", ";
+		inStream << inM.mCol[Cols - 1];
+		return inStream;
+	}
+
+	/// Column access
+	const Vector<Rows> &					GetColumn(int inIdx) const					{ return mCol[inIdx]; }
+	Vector<Rows> &							GetColumn(int inIdx)						{ return mCol[inIdx]; }
+
+	Vector<Rows>							mCol[Cols];									///< Column
+};
+
+// The template specialization doesn't sit well with Doxygen
+#ifndef JPH_PLATFORM_DOXYGEN
+
+/// Specialization of SetInversed for 2x2 matrix
+template <>
+inline bool Matrix<2, 2>::SetInversed(const Matrix<2, 2> &inM)
+{
+	// Fetch elements
+	float a = inM.mCol[0].mF32[0];
+	float b = inM.mCol[1].mF32[0];
+	float c = inM.mCol[0].mF32[1];
+	float d = inM.mCol[1].mF32[1];
+
+	// Calculate determinant
+	float det = a * d - b * c;
+	if (det == 0.0f)
+		return false;
+
+	// Construct inverse
+	mCol[0].mF32[0] = d / det;
+	mCol[1].mF32[0] = -b / det;
+	mCol[0].mF32[1] = -c / det;
+	mCol[1].mF32[1] = a / det;
+	return true;
+}
+
+#endif // !JPH_PLATFORM_DOXYGEN
+
+JPH_NAMESPACE_END