Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
Matrix.h
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
7#include <Jolt/Math/Vector.h>
9
11
13template <uint Rows, uint Cols>
14class [[nodiscard]] Matrix
15{
16public:
18 inline Matrix() = default;
19 inline Matrix(const Matrix &inM2) { *this = inM2; }
20
22 inline uint GetRows() const { return Rows; }
23 inline uint GetCols() const { return Cols; }
24
26 inline void SetZero()
27 {
28 for (uint c = 0; c < Cols; ++c)
29 mCol[c].SetZero();
30 }
31
32 inline static Matrix sZero() { Matrix m; m.SetZero(); return m; }
33
35 inline bool IsZero() const
36 {
37 for (uint c = 0; c < Cols; ++c)
38 if (!mCol[c].IsZero())
39 return false;
40
41 return true;
42 }
43
45 inline void SetIdentity()
46 {
47 // Clear matrix
48 SetZero();
49
50 // Set diagonal to 1
51 for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc)
52 mCol[rc].mF32[rc] = 1.0f;
53 }
54
55 inline static Matrix sIdentity() { Matrix m; m.SetIdentity(); return m; }
56
58 bool IsIdentity() const { return *this == sIdentity(); }
59
61 inline void SetDiagonal(const Vector<Rows < Cols? Rows : Cols> &inV)
62 {
63 // Clear matrix
64 SetZero();
65
66 // Set diagonal
67 for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc)
68 mCol[rc].mF32[rc] = inV[rc];
69 }
70
71 inline static Matrix sDiagonal(const Vector<Rows < Cols? Rows : Cols> &inV)
72 {
73 Matrix m;
74 m.SetDiagonal(inV);
75 return m;
76 }
77
79 template <class OtherMatrix>
80 void CopyPart(const OtherMatrix &inM, uint inSourceRow, uint inSourceCol, uint inNumRows, uint inNumCols, uint inDestRow, uint inDestCol)
81 {
82 for (uint c = 0; c < inNumCols; ++c)
83 for (uint r = 0; r < inNumRows; ++r)
84 mCol[inDestCol + c].mF32[inDestRow + r] = inM(inSourceRow + r, inSourceCol + c);
85 }
86
88 inline float operator () (uint inRow, uint inColumn) const
89 {
90 JPH_ASSERT(inRow < Rows);
91 JPH_ASSERT(inColumn < Cols);
92 return mCol[inColumn].mF32[inRow];
93 }
94
95 inline float & operator () (uint inRow, uint inColumn)
96 {
97 JPH_ASSERT(inRow < Rows);
98 JPH_ASSERT(inColumn < Cols);
99 return mCol[inColumn].mF32[inRow];
100 }
101
103 inline bool operator == (const Matrix &inM2) const
104 {
105 for (uint c = 0; c < Cols; ++c)
106 if (mCol[c] != inM2.mCol[c])
107 return false;
108 return true;
109 }
110
111 inline bool operator != (const Matrix &inM2) const
112 {
113 for (uint c = 0; c < Cols; ++c)
114 if (mCol[c] != inM2.mCol[c])
115 return true;
116 return false;
117 }
118
120 inline Matrix & operator = (const Matrix &inM2)
121 {
122 for (uint c = 0; c < Cols; ++c)
123 mCol[c] = inM2.mCol[c];
124 return *this;
125 }
126
128 template <uint OtherCols>
130 {
132 for (uint c = 0; c < OtherCols; ++c)
133 for (uint r = 0; r < Rows; ++r)
134 {
135 float dot = 0.0f;
136 for (uint i = 0; i < Cols; ++i)
137 dot += mCol[i].mF32[r] * inM.mCol[c].mF32[i];
138 m.mCol[c].mF32[r] = dot;
139 }
140 return m;
141 }
142
144 inline Vector<Rows> operator * (const Vector<Cols> &inV) const
145 {
146 Vector<Rows> v;
147 for (uint r = 0; r < Rows; ++r)
148 {
149 float dot = 0.0f;
150 for (uint c = 0; c < Cols; ++c)
151 dot += mCol[c].mF32[r] * inV.mF32[c];
152 v.mF32[r] = dot;
153 }
154 return v;
155 }
156
158 inline Matrix operator * (float inV) const
159 {
160 Matrix m;
161 for (uint c = 0; c < Cols; ++c)
162 m.mCol[c] = mCol[c] * inV;
163 return m;
164 }
165
166 inline friend Matrix operator * (float inV, const Matrix &inM)
167 {
168 return inM * inV;
169 }
170
172 inline Matrix operator + (const Matrix &inM) const
173 {
174 Matrix m;
175 for (uint c = 0; c < Cols; ++c)
176 m.mCol[c] = mCol[c] + inM.mCol[c];
177 return m;
178 }
179
181 inline Matrix operator - (const Matrix &inM) const
182 {
183 Matrix m;
184 for (uint c = 0; c < Cols; ++c)
185 m.mCol[c] = mCol[c] - inM.mCol[c];
186 return m;
187 }
188
191 {
193 for (uint r = 0; r < Rows; ++r)
194 for (uint c = 0; c < Cols; ++c)
195 m.mCol[r].mF32[c] = mCol[c].mF32[r];
196 return m;
197 }
198
200 bool SetInversed(const Matrix &inM)
201 {
202 if constexpr (Rows != Cols) JPH_ASSERT(false);
203 Matrix copy(inM);
204 SetIdentity();
205 return GaussianElimination(copy, *this);
206 }
207
208 inline Matrix Inversed() const
209 {
210 Matrix m;
211 m.SetInversed(*this);
212 return m;
213 }
214
216 friend ostream & operator << (ostream &inStream, const Matrix &inM)
217 {
218 for (uint i = 0; i < Cols - 1; ++i)
219 inStream << inM.mCol[i] << ", ";
220 inStream << inM.mCol[Cols - 1];
221 return inStream;
222 }
223
225 const Vector<Rows> & GetColumn(int inIdx) const { return mCol[inIdx]; }
226 Vector<Rows> & GetColumn(int inIdx) { return mCol[inIdx]; }
227
228 Vector<Rows> mCol[Cols];
229};
230
231// The template specialization doesn't sit well with Doxygen
232#ifndef JPH_PLATFORM_DOXYGEN
233
235template <>
236inline bool Matrix<2, 2>::SetInversed(const Matrix<2, 2> &inM)
237{
238 // Fetch elements
239 float a = inM.mCol[0].mF32[0];
240 float b = inM.mCol[1].mF32[0];
241 float c = inM.mCol[0].mF32[1];
242 float d = inM.mCol[1].mF32[1];
243
244 // Calculate determinant
245 float det = a * d - b * c;
246 if (det == 0.0f)
247 return false;
248
249 // Construct inverse
250 mCol[0].mF32[0] = d / det;
251 mCol[1].mF32[0] = -b / det;
252 mCol[0].mF32[1] = -c / det;
253 mCol[1].mF32[1] = a / det;
254 return true;
255}
256
257#endif // !JPH_PLATFORM_DOXYGEN
258
unsigned int uint
Definition: Core.h:439
#define JPH_NAMESPACE_END
Definition: Core.h:367
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
DVec3 operator*(double inV1, DVec3Arg inV2)
Definition: DVec3.inl:447
JPH_NAMESPACE_BEGIN bool GaussianElimination(MatrixA &ioA, MatrixB &ioB, float inTolerance=1.0e-16f)
Definition: GaussianElimination.h:19
#define JPH_ASSERT(...)
Definition: IssueReporting.h:33
Templatized matrix class.
Definition: Matrix.h:15
static Matrix sZero()
Definition: Matrix.h:32
bool SetInversed(const Matrix &inM)
Inverse matrix.
Definition: Matrix.h:200
uint GetRows() const
Dimensions.
Definition: Matrix.h:22
bool IsIdentity() const
Check if this matrix is identity.
Definition: Matrix.h:58
uint GetCols() const
Definition: Matrix.h:23
Vector< Rows > mCol[Cols]
Column.
Definition: Matrix.h:228
void CopyPart(const OtherMatrix &inM, uint inSourceRow, uint inSourceCol, uint inNumRows, uint inNumCols, uint inDestRow, uint inDestCol)
Copy a (part) of another matrix into this matrix.
Definition: Matrix.h:80
Matrix()=default
Constructor.
void SetZero()
Zero matrix.
Definition: Matrix.h:26
static Matrix sIdentity()
Definition: Matrix.h:55
Vector< Rows > & GetColumn(int inIdx)
Definition: Matrix.h:226
bool IsZero() const
Check if this matrix consists of all zeros.
Definition: Matrix.h:35
Matrix(const Matrix &inM2)
Definition: Matrix.h:19
const Vector< Rows > & GetColumn(int inIdx) const
Column access.
Definition: Matrix.h:225
void SetIdentity()
Identity matrix.
Definition: Matrix.h:45
void SetDiagonal(const Vector< Rows< Cols? Rows :Cols > &inV)
Diagonal matrix.
Definition: Matrix.h:61
static Matrix sDiagonal(const Vector< Rows< Cols? Rows :Cols > &inV)
Definition: Matrix.h:71
Matrix Inversed() const
Definition: Matrix.h:208
Matrix< Cols, Rows > Transposed() const
Transpose matrix.
Definition: Matrix.h:190
Templatized vector class.
Definition: Vector.h:12
float mF32[Rows]
Definition: Vector.h:213