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 |
40 | namespace physx |
41 | { |
42 | #endif |
43 | /*! |
44 | \brief 3x3 matrix class |
45 | |
46 | Some clarifications, as there have been much confusion about matrix formats etc in the past. |
47 | |
48 | Short: |
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 | |
53 | Long: |
54 | Given 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 | |
60 | Vectors are treated as columns, so the vector v is |
61 | |
62 | |x| |
63 | |y| |
64 | |z| |
65 | |
66 | And matrices are applied _before_ the vector (pre-multiplication) |
67 | v' = 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 | |
74 | Physical storage and indexing: |
75 | To be compatible with popular 3d rendering APIs (read D3d and OpenGL) |
76 | the physical indexing is |
77 | |
78 | |0 3 6| |
79 | |1 4 7| |
80 | |2 5 8| |
81 | |
82 | index = column*3 + row |
83 | |
84 | which in C++ translates to M[column][row] |
85 | |
86 | The mathematical indexing is M_row,column and this is what is used for _-notation |
87 | so _12 is 1st row, second column and operator(row, column)! |
88 | |
89 | */ |
90 | class 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 |
357 | PX_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 | |