1/*
2 * Copyright (c) 2008-2015, NVIDIA CORPORATION. All rights reserved.
3 *
4 * NVIDIA CORPORATION and its licensors retain all intellectual property
5 * and proprietary rights in and to this software, related documentation
6 * and any modifications thereto. Any use, reproduction, disclosure or
7 * distribution of this software and related documentation without an express
8 * license agreement from NVIDIA CORPORATION is strictly prohibited.
9 */
10// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
11// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
12
13
14#ifndef PX_FOUNDATION_PX_MAT33_H
15#define PX_FOUNDATION_PX_MAT33_H
16/** \addtogroup foundation
17@{
18*/
19
20#include "foundation/PxVec3.h"
21#include "foundation/PxQuat.h"
22
23#ifndef PX_DOXYGEN
24namespace physx
25{
26#endif
27/*!
28\brief 3x3 matrix class
29
30Some clarifications, as there have been much confusion about matrix formats etc in the past.
31
32Short:
33- Matrix have base vectors in columns (vectors are column matrices, 3x1 matrices).
34- Matrix is physically stored in column major format
35- Matrices are concaternated from left
36
37Long:
38Given three base vectors a, b and c the matrix is stored as
39
40|a.x b.x c.x|
41|a.y b.y c.y|
42|a.z b.z c.z|
43
44Vectors are treated as columns, so the vector v is
45
46|x|
47|y|
48|z|
49
50And matrices are applied _before_ the vector (pre-multiplication)
51v' = M*v
52
53|x'| |a.x b.x c.x| |x| |a.x*x + b.x*y + c.x*z|
54|y'| = |a.y b.y c.y| * |y| = |a.y*x + b.y*y + c.y*z|
55|z'| |a.z b.z c.z| |z| |a.z*x + b.z*y + c.z*z|
56
57
58Physical storage and indexing:
59To be compatible with popular 3d rendering APIs (read D3d and OpenGL)
60the physical indexing is
61
62|0 3 6|
63|1 4 7|
64|2 5 8|
65
66index = column*3 + row
67
68which in C++ translates to M[column][row]
69
70The mathematical indexing is M_row,column and this is what is used for _-notation
71so _12 is 1st row, second column and operator(row, column)!
72
73*/
74class PxMat33
75{
76public:
77 //! Default constructor
78 PX_CUDA_CALLABLE PX_INLINE PxMat33()
79 {}
80
81 //! identity constructor
82 PX_CUDA_CALLABLE PX_INLINE PxMat33(PxIDENTITY r)
83 : column0(1.0f,0.0f,0.0f), column1(0.0f,1.0f,0.0f), column2(0.0f,0.0f,1.0f)
84 {
85 PX_UNUSED(r);
86 }
87
88 //! zero constructor
89 PX_CUDA_CALLABLE PX_INLINE PxMat33(PxZERO r)
90 : column0(0.0f), column1(0.0f), column2(0.0f)
91 {
92 PX_UNUSED(r);
93 }
94
95
96 //! Construct from three base vectors
97 PX_CUDA_CALLABLE PxMat33(const PxVec3& col0, const PxVec3& col1, const PxVec3& col2)
98 : column0(col0), column1(col1), column2(col2)
99 {}
100
101
102 //! constructor from a scalar, which generates a multiple of the identity matrix
103 explicit PX_CUDA_CALLABLE PX_INLINE PxMat33(PxReal r)
104 : column0(r,0.0f,0.0f), column1(0.0f,r,0.0f), column2(0.0f,0.0f,r)
105 {}
106
107
108 //! Construct from float[9]
109 explicit PX_CUDA_CALLABLE PX_INLINE PxMat33(PxReal values[]):
110 column0(values[0],values[1],values[2]),
111 column1(values[3],values[4],values[5]),
112 column2(values[6],values[7],values[8])
113 {
114 }
115
116 //! Construct from a quaternion
117 explicit PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33(const PxQuat& q)
118 {
119 const PxReal x = q.x;
120 const PxReal y = q.y;
121 const PxReal z = q.z;
122 const PxReal w = q.w;
123
124 const PxReal x2 = x + x;
125 const PxReal y2 = y + y;
126 const PxReal z2 = z + z;
127
128 const PxReal xx = x2*x;
129 const PxReal yy = y2*y;
130 const PxReal zz = z2*z;
131
132 const PxReal xy = x2*y;
133 const PxReal xz = x2*z;
134 const PxReal xw = x2*w;
135
136 const PxReal yz = y2*z;
137 const PxReal yw = y2*w;
138 const PxReal zw = z2*w;
139
140 column0 = PxVec3(1.0f - yy - zz, xy + zw, xz - yw);
141 column1 = PxVec3(xy - zw, 1.0f - xx - zz, yz + xw);
142 column2 = PxVec3(xz + yw, yz - xw, 1.0f - xx - yy);
143 }
144
145 //! Copy constructor
146 PX_CUDA_CALLABLE PX_INLINE PxMat33(const PxMat33& other)
147 : column0(other.column0), column1(other.column1), column2(other.column2)
148 {}
149
150 //! Assignment operator
151 PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33& operator=(const PxMat33& other)
152 {
153 column0 = other.column0;
154 column1 = other.column1;
155 column2 = other.column2;
156 return *this;
157 }
158
159 //! \deprecated Set to identity matrix. Deprecated. use PxMat33(PxIdentity)
160 PX_DEPRECATED PX_CUDA_CALLABLE PX_INLINE static PxMat33 createIdentity()
161 {
162 return PxMat33(PxIdentity);
163 }
164
165 //! \deprecated Set to zero matrix. Deprecated. use PxMat33(PxZero).
166 PX_DEPRECATED PX_CUDA_CALLABLE PX_INLINE static PxMat33 createZero()
167 {
168 return PxMat33(PxZero); // PxMat33(0) is ambiguous, it can either be the array constructor or the scalar constructor
169 }
170
171 //! Construct from diagonal, off-diagonals are zero.
172 PX_CUDA_CALLABLE PX_INLINE static PxMat33 createDiagonal(const PxVec3& d)
173 {
174 return PxMat33(PxVec3(d.x,0.0f,0.0f), PxVec3(0.0f,d.y,0.0f), PxVec3(0.0f,0.0f,d.z));
175 }
176
177 /**
178 \brief returns true if the two matrices are exactly equal
179 */
180 PX_CUDA_CALLABLE PX_INLINE bool operator==(const PxMat33& m) const { return column0 == m.column0 && column1 == m.column1 && column2 == m.column2; }
181
182
183
184 //! Get transposed matrix
185 PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 getTranspose() const
186 {
187 const PxVec3 v0(column0.x, column1.x, column2.x);
188 const PxVec3 v1(column0.y, column1.y, column2.y);
189 const PxVec3 v2(column0.z, column1.z, column2.z);
190
191 return PxMat33(v0,v1,v2);
192 }
193
194 //! Get the real inverse
195 PX_CUDA_CALLABLE PX_INLINE PxMat33 getInverse() const
196 {
197 const PxReal det = getDeterminant();
198 PxMat33 inverse;
199
200 if(det != 0)
201 {
202 const PxReal invDet = 1.0f/det;
203
204 inverse.column0[0] = invDet * (column1[1]*column2[2] - column2[1]*column1[2]);
205 inverse.column0[1] = invDet *-(column0[1]*column2[2] - column2[1]*column0[2]);
206 inverse.column0[2] = invDet * (column0[1]*column1[2] - column0[2]*column1[1]);
207
208 inverse.column1[0] = invDet *-(column1[0]*column2[2] - column1[2]*column2[0]);
209 inverse.column1[1] = invDet * (column0[0]*column2[2] - column0[2]*column2[0]);
210 inverse.column1[2] = invDet *-(column0[0]*column1[2] - column0[2]*column1[0]);
211
212 inverse.column2[0] = invDet * (column1[0]*column2[1] - column1[1]*column2[0]);
213 inverse.column2[1] = invDet *-(column0[0]*column2[1] - column0[1]*column2[0]);
214 inverse.column2[2] = invDet * (column0[0]*column1[1] - column1[0]*column0[1]);
215
216 return inverse;
217 }
218 else
219 {
220 return createIdentity();
221 }
222 }
223
224 //! Get determinant
225 PX_CUDA_CALLABLE PX_INLINE PxReal getDeterminant() const
226 {
227 return column0.dot(column1.cross(column2));
228 }
229
230 //! Unary minus
231 PX_CUDA_CALLABLE PX_INLINE PxMat33 operator-() const
232 {
233 return PxMat33(-column0, -column1, -column2);
234 }
235
236 //! Add
237 PX_CUDA_CALLABLE PX_INLINE PxMat33 operator+(const PxMat33& other) const
238 {
239 return PxMat33( column0+other.column0,
240 column1+other.column1,
241 column2+other.column2);
242 }
243
244 //! Subtract
245 PX_CUDA_CALLABLE PX_INLINE PxMat33 operator-(const PxMat33& other) const
246 {
247 return PxMat33( column0-other.column0,
248 column1-other.column1,
249 column2-other.column2);
250 }
251
252 //! Scalar multiplication
253 PX_CUDA_CALLABLE PX_INLINE PxMat33 operator*(PxReal scalar) const
254 {
255 return PxMat33(column0*scalar, column1*scalar, column2*scalar);
256 }
257
258 friend PxMat33 operator*(PxReal, const PxMat33&);
259
260 //! Matrix vector multiplication (returns 'this->transform(vec)')
261 PX_CUDA_CALLABLE PX_INLINE PxVec3 operator*(const PxVec3& vec) const
262 {
263 return transform(vec);
264 }
265
266
267 // a <op>= b operators
268
269 //! Matrix multiplication
270 PX_CUDA_CALLABLE PX_FORCE_INLINE 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.column0), transform(other.column1), transform(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*=(PxReal 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
312 //! Element access, mathematical way!
313 PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal operator()(unsigned int row, unsigned int col) const
314 {
315 return (*this)[col][row];
316 }
317
318 //! Element access, mathematical way!
319 PX_CUDA_CALLABLE PX_FORCE_INLINE PxReal& operator()(unsigned int row, unsigned int col)
320 {
321 return (*this)[col][row];
322 }
323
324 // Transform etc
325
326 //! Transform vector by matrix, equal to v' = M*v
327 PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 transform(const PxVec3& other) const
328 {
329 return column0*other.x + column1*other.y + column2*other.z;
330 }
331
332 //! Transform vector by matrix transpose, v' = M^t*v
333 PX_CUDA_CALLABLE PX_INLINE PxVec3 transformTranspose(const PxVec3& other) const
334 {
335 return PxVec3( column0.dot(other),
336 column1.dot(other),
337 column2.dot(other));
338 }
339
340 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxReal* front() const
341 {
342 return &column0.x;
343 }
344
345 PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3& operator[](unsigned int num) {return (&column0)[num];}
346 PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3& operator[](unsigned int num) const {return (&column0)[num];}
347
348 PX_DEPRECATED PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3& operator[](int num) {return (&column0)[num];}
349 PX_DEPRECATED PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec3& operator[](int num) const {return (&column0)[num];}
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 PxReal tr = m(0,0) + m(1,1) + m(2,2), h;
360 if(tr >= 0)
361 {
362 h = PxSqrt(tr +1);
363 w = 0.5f * h;
364 h = 0.5f / h;
365
366 x = (m(2,1) - m(1,2)) * h;
367 y = (m(0,2) - m(2,0)) * h;
368 z = (m(1,0) - m(0,1)) * h;
369 }
370 else
371 {
372 unsigned int i = 0;
373 if (m(1,1) > m(0,0))
374 i = 1;
375 if (m(2,2) > m(i,i))
376 i = 2;
377 switch (i)
378 {
379 case 0:
380 h = PxSqrt((m(0,0) - (m(1,1) + m(2,2))) + 1);
381 x = 0.5f * h;
382 h = 0.5f / h;
383
384 y = (m(0,1) + m(1,0)) * h;
385 z = (m(2,0) + m(0,2)) * h;
386 w = (m(2,1) - m(1,2)) * h;
387 break;
388 case 1:
389 h = PxSqrt((m(1,1) - (m(2,2) + m(0,0))) + 1);
390 y = 0.5f * h;
391 h = 0.5f / h;
392
393 z = (m(1,2) + m(2,1)) * h;
394 x = (m(0,1) + m(1,0)) * h;
395 w = (m(0,2) - m(2,0)) * h;
396 break;
397 case 2:
398 h = PxSqrt((m(2,2) - (m(0,0) + m(1,1))) + 1);
399 z = 0.5f * h;
400 h = 0.5f / h;
401
402 x = (m(2,0) + m(0,2)) * h;
403 y = (m(1,2) + m(2,1)) * h;
404 w = (m(1,0) - m(0,1)) * h;
405 break;
406 default: // Make compiler happy
407 x = y = z = w = 0;
408 break;
409 }
410 }
411}
412
413#ifndef PX_DOXYGEN
414} // namespace physx
415#endif
416
417/** @} */
418#endif // PX_FOUNDATION_PX_MAT33_H
419