1//
2// Redistribution and use in source and binary forms, with or without
3// modification, are permitted provided that the following conditions
4// are met:
5// * Redistributions of source code must retain the above copyright
6// notice, this list of conditions and the following disclaimer.
7// * Redistributions in binary form must reproduce the above copyright
8// notice, this list of conditions and the following disclaimer in the
9// documentation and/or other materials provided with the distribution.
10// * Neither the name of NVIDIA CORPORATION nor the names of its
11// contributors may be used to endorse or promote products derived
12// from this software without specific prior written permission.
13//
14// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
15// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
17// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
18// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
19// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
20// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
21// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
22// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25//
26// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
27// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
28// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
29
30#ifndef PXFOUNDATION_PXMAT33_H
31#define PXFOUNDATION_PXMAT33_H
32/** \addtogroup foundation
33@{
34*/
35
36#include "foundation/PxVec3.h"
37#include "foundation/PxQuat.h"
38
39#if !PX_DOXYGEN
40namespace physx
41{
42#endif
43/*!
44\brief 3x3 matrix class
45
46Some clarifications, as there have been much confusion about matrix formats etc in the past.
47
48Short:
49- Matrix have base vectors in columns (vectors are column matrices, 3x1 matrices).
50- Matrix is physically stored in column major format
51- Matrices are concaternated from left
52
53Long:
54Given three base vectors a, b and c the matrix is stored as
55
56|a.x b.x c.x|
57|a.y b.y c.y|
58|a.z b.z c.z|
59
60Vectors are treated as columns, so the vector v is
61
62|x|
63|y|
64|z|
65
66And matrices are applied _before_ the vector (pre-multiplication)
67v' = M*v
68
69|x'| |a.x b.x c.x| |x| |a.x*x + b.x*y + c.x*z|
70|y'| = |a.y b.y c.y| * |y| = |a.y*x + b.y*y + c.y*z|
71|z'| |a.z b.z c.z| |z| |a.z*x + b.z*y + c.z*z|
72
73
74Physical storage and indexing:
75To be compatible with popular 3d rendering APIs (read D3d and OpenGL)
76the physical indexing is
77
78|0 3 6|
79|1 4 7|
80|2 5 8|
81
82index = column*3 + row
83
84which in C++ translates to M[column][row]
85
86The mathematical indexing is M_row,column and this is what is used for _-notation
87so _12 is 1st row, second column and operator(row, column)!
88
89*/
90class PxMat33
91{
92 public:
93 //! Default constructor
94 PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33()
95 {
96 }
97
98 //! identity constructor
99 PX_CUDA_CALLABLE PX_INLINE PxMat33(PxIDENTITY r)
100 : column0(1.0f, 0.0f, 0.0f), column1(0.0f, 1.0f, 0.0f), column2(0.0f, 0.0f, 1.0f)
101 {
102 PX_UNUSED(r);
103 }
104
105 //! zero constructor
106 PX_CUDA_CALLABLE PX_INLINE PxMat33(PxZERO r) : column0(0.0f), column1(0.0f), column2(0.0f)
107 {
108 PX_UNUSED(r);
109 }
110
111 //! Construct from three base vectors
112 PX_CUDA_CALLABLE PxMat33(const PxVec3& col0, const PxVec3& col1, const PxVec3& col2)
113 : column0(col0), column1(col1), column2(col2)
114 {
115 }
116
117 //! constructor from a scalar, which generates a multiple of the identity matrix
118 explicit PX_CUDA_CALLABLE PX_INLINE PxMat33(float r)
119 : column0(r, 0.0f, 0.0f), column1(0.0f, r, 0.0f), column2(0.0f, 0.0f, r)
120 {
121 }
122
123 //! Construct from float[9]
124 explicit PX_CUDA_CALLABLE PX_INLINE PxMat33(float values[])
125 : column0(values[0], values[1], values[2])
126 , column1(values[3], values[4], values[5])
127 , column2(values[6], values[7], values[8])
128 {
129 }
130
131 //! Construct from a quaternion
132 explicit PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33(const PxQuat& q)
133 {
134 const float x = q.x;
135 const float y = q.y;
136 const float z = q.z;
137 const float w = q.w;
138
139 const float x2 = x + x;
140 const float y2 = y + y;
141 const float z2 = z + z;
142
143 const float xx = x2 * x;
144 const float yy = y2 * y;
145 const float zz = z2 * z;
146
147 const float xy = x2 * y;
148 const float xz = x2 * z;
149 const float xw = x2 * w;
150
151 const float yz = y2 * z;
152 const float yw = y2 * w;
153 const float zw = z2 * w;
154
155 column0 = PxVec3(1.0f - yy - zz, xy + zw, xz - yw);
156 column1 = PxVec3(xy - zw, 1.0f - xx - zz, yz + xw);
157 column2 = PxVec3(xz + yw, yz - xw, 1.0f - xx - yy);
158 }
159
160 //! Copy constructor
161 PX_CUDA_CALLABLE PX_INLINE PxMat33(const PxMat33& other)
162 : column0(other.column0), column1(other.column1), column2(other.column2)
163 {
164 }
165
166 //! Assignment operator
167 PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33& operator=(const PxMat33& other)
168 {
169 column0 = other.column0;
170 column1 = other.column1;
171 column2 = other.column2;
172 return *this;
173 }
174
175 //! Construct from diagonal, off-diagonals are zero.
176 PX_CUDA_CALLABLE PX_INLINE static const PxMat33 createDiagonal(const PxVec3& d)
177 {
178 return PxMat33(PxVec3(d.x, 0.0f, 0.0f), PxVec3(0.0f, d.y, 0.0f), PxVec3(0.0f, 0.0f, d.z));
179 }
180
181 /**
182 \brief returns true if the two matrices are exactly equal
183 */
184 PX_CUDA_CALLABLE PX_INLINE bool operator==(const PxMat33& m) const
185 {
186 return column0 == m.column0 && column1 == m.column1 && column2 == m.column2;
187 }
188
189 //! Get transposed matrix
190 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxMat33 getTranspose() const
191 {
192 const PxVec3 v0(column0.x, column1.x, column2.x);
193 const PxVec3 v1(column0.y, column1.y, column2.y);
194 const PxVec3 v2(column0.z, column1.z, column2.z);
195
196 return PxMat33(v0, v1, v2);
197 }
198
199 //! Get the real inverse
200 PX_CUDA_CALLABLE PX_INLINE const PxMat33 getInverse() const
201 {
202 const float det = getDeterminant();
203 PxMat33 inverse;
204
205 if(det != 0)
206 {
207 const float invDet = 1.0f / det;
208
209 inverse.column0.x = invDet * (column1.y * column2.z - column2.y * column1.z);
210 inverse.column0.y = invDet * -(column0.y * column2.z - column2.y * column0.z);
211 inverse.column0.z = invDet * (column0.y * column1.z - column0.z * column1.y);
212
213 inverse.column1.x = invDet * -(column1.x * column2.z - column1.z * column2.x);
214 inverse.column1.y = invDet * (column0.x * column2.z - column0.z * column2.x);
215 inverse.column1.z = invDet * -(column0.x * column1.z - column0.z * column1.x);
216
217 inverse.column2.x = invDet * (column1.x * column2.y - column1.y * column2.x);
218 inverse.column2.y = invDet * -(column0.x * column2.y - column0.y * column2.x);
219 inverse.column2.z = invDet * (column0.x * column1.y - column1.x * column0.y);
220
221 return inverse;
222 }
223 else
224 {
225 return PxMat33(PxIdentity);
226 }
227 }
228
229 //! Get determinant
230 PX_CUDA_CALLABLE PX_INLINE float getDeterminant() const
231 {
232 return column0.dot(v: column1.cross(v: column2));
233 }
234
235 //! Unary minus
236 PX_CUDA_CALLABLE PX_INLINE const PxMat33 operator-() const
237 {
238 return PxMat33(-column0, -column1, -column2);
239 }
240
241 //! Add
242 PX_CUDA_CALLABLE PX_INLINE const PxMat33 operator+(const PxMat33& other) const
243 {
244 return PxMat33(column0 + other.column0, column1 + other.column1, column2 + other.column2);
245 }
246
247 //! Subtract
248 PX_CUDA_CALLABLE PX_INLINE const PxMat33 operator-(const PxMat33& other) const
249 {
250 return PxMat33(column0 - other.column0, column1 - other.column1, column2 - other.column2);
251 }
252
253 //! Scalar multiplication
254 PX_CUDA_CALLABLE PX_INLINE const PxMat33 operator*(float scalar) const
255 {
256 return PxMat33(column0 * scalar, column1 * scalar, column2 * scalar);
257 }
258
259 friend PxMat33 operator*(float, const PxMat33&);
260
261 //! Matrix vector multiplication (returns 'this->transform(vec)')
262 PX_CUDA_CALLABLE PX_INLINE const PxVec3 operator*(const PxVec3& vec) const
263 {
264 return transform(other: vec);
265 }
266
267 // a <op>= b operators
268
269 //! Matrix multiplication
270 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxMat33 operator*(const PxMat33& other) const
271 {
272 // Rows from this <dot> columns from other
273 // column0 = transform(other.column0) etc
274 return PxMat33(transform(other: other.column0), transform(other: other.column1), transform(other: other.column2));
275 }
276
277 //! Equals-add
278 PX_CUDA_CALLABLE PX_INLINE PxMat33& operator+=(const PxMat33& other)
279 {
280 column0 += other.column0;
281 column1 += other.column1;
282 column2 += other.column2;
283 return *this;
284 }
285
286 //! Equals-sub
287 PX_CUDA_CALLABLE PX_INLINE PxMat33& operator-=(const PxMat33& other)
288 {
289 column0 -= other.column0;
290 column1 -= other.column1;
291 column2 -= other.column2;
292 return *this;
293 }
294
295 //! Equals scalar multiplication
296 PX_CUDA_CALLABLE PX_INLINE PxMat33& operator*=(float scalar)
297 {
298 column0 *= scalar;
299 column1 *= scalar;
300 column2 *= scalar;
301 return *this;
302 }
303
304 //! Equals matrix multiplication
305 PX_CUDA_CALLABLE PX_INLINE PxMat33& operator*=(const PxMat33& other)
306 {
307 *this = *this * other;
308 return *this;
309 }
310
311 //! Element access, mathematical way!
312 PX_CUDA_CALLABLE PX_FORCE_INLINE float operator()(unsigned int row, unsigned int col) const
313 {
314 return (*this)[col][row];
315 }
316
317 //! Element access, mathematical way!
318 PX_CUDA_CALLABLE PX_FORCE_INLINE float& operator()(unsigned int row, unsigned int col)
319 {
320 return (*this)[col][row];
321 }
322
323 // Transform etc
324
325 //! Transform vector by matrix, equal to v' = M*v
326 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3 transform(const PxVec3& other) const
327 {
328 return column0 * other.x + column1 * other.y + column2 * other.z;
329 }
330
331 //! Transform vector by matrix transpose, v' = M^t*v
332 PX_CUDA_CALLABLE PX_INLINE const PxVec3 transformTranspose(const PxVec3& other) const
333 {
334 return PxVec3(column0.dot(v: other), column1.dot(v: other), column2.dot(v: other));
335 }
336
337 PX_CUDA_CALLABLE PX_FORCE_INLINE const float* front() const
338 {
339 return &column0.x;
340 }
341
342 PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3& operator[](unsigned int num)
343 {
344 return (&column0)[num];
345 }
346 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3& operator[](unsigned int num) const
347 {
348 return (&column0)[num];
349 }
350
351 // Data, see above for format!
352
353 PxVec3 column0, column1, column2; // the three base vectors
354};
355
356// implementation from PxQuat.h
357PX_CUDA_CALLABLE PX_INLINE PxQuat::PxQuat(const PxMat33& m)
358{
359 if(m.column2.z < 0)
360 {
361 if(m.column0.x > m.column1.y)
362 {
363 float t = 1 + m.column0.x - m.column1.y - m.column2.z;
364 *this = PxQuat(t, m.column0.y + m.column1.x, m.column2.x + m.column0.z, m.column1.z - m.column2.y) *
365 (0.5f / PxSqrt(a: t));
366 }
367 else
368 {
369 float t = 1 - m.column0.x + m.column1.y - m.column2.z;
370 *this = PxQuat(m.column0.y + m.column1.x, t, m.column1.z + m.column2.y, m.column2.x - m.column0.z) *
371 (0.5f / PxSqrt(a: t));
372 }
373 }
374 else
375 {
376 if(m.column0.x < -m.column1.y)
377 {
378 float t = 1 - m.column0.x - m.column1.y + m.column2.z;
379 *this = PxQuat(m.column2.x + m.column0.z, m.column1.z + m.column2.y, t, m.column0.y - m.column1.x) *
380 (0.5f / PxSqrt(a: t));
381 }
382 else
383 {
384 float t = 1 + m.column0.x + m.column1.y + m.column2.z;
385 *this = PxQuat(m.column1.z - m.column2.y, m.column2.x - m.column0.z, m.column0.y - m.column1.x, t) *
386 (0.5f / PxSqrt(a: t));
387 }
388 }
389}
390
391#if !PX_DOXYGEN
392} // namespace physx
393#endif
394
395/** @} */
396#endif // #ifndef PXFOUNDATION_PXMAT33_H
397

source code of qtquick3dphysics/src/3rdparty/PhysX/pxshared/include/foundation/PxMat33.h