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_PXMAT44_H |
31 | #define PXFOUNDATION_PXMAT44_H |
32 | /** \addtogroup foundation |
33 | @{ |
34 | */ |
35 | |
36 | #include "foundation/PxQuat.h" |
37 | #include "foundation/PxVec4.h" |
38 | #include "foundation/PxMat33.h" |
39 | #include "foundation/PxTransform.h" |
40 | |
41 | #if !PX_DOXYGEN |
42 | namespace physx |
43 | { |
44 | #endif |
45 | |
46 | /*! |
47 | \brief 4x4 matrix class |
48 | |
49 | This class is layout-compatible with D3D and OpenGL matrices. More notes on layout are given in the PxMat33 |
50 | |
51 | @see PxMat33 PxTransform |
52 | */ |
53 | |
54 | class PxMat44 |
55 | { |
56 | public: |
57 | //! Default constructor |
58 | PX_CUDA_CALLABLE PX_INLINE PxMat44() |
59 | { |
60 | } |
61 | |
62 | //! identity constructor |
63 | PX_CUDA_CALLABLE PX_INLINE PxMat44(PxIDENTITY r) |
64 | : column0(1.0f, 0.0f, 0.0f, 0.0f) |
65 | , column1(0.0f, 1.0f, 0.0f, 0.0f) |
66 | , column2(0.0f, 0.0f, 1.0f, 0.0f) |
67 | , column3(0.0f, 0.0f, 0.0f, 1.0f) |
68 | { |
69 | PX_UNUSED(r); |
70 | } |
71 | |
72 | //! zero constructor |
73 | PX_CUDA_CALLABLE PX_INLINE PxMat44(PxZERO r) : column0(PxZero), column1(PxZero), column2(PxZero), column3(PxZero) |
74 | { |
75 | PX_UNUSED(r); |
76 | } |
77 | |
78 | //! Construct from four 4-vectors |
79 | PX_CUDA_CALLABLE PxMat44(const PxVec4& col0, const PxVec4& col1, const PxVec4& col2, const PxVec4& col3) |
80 | : column0(col0), column1(col1), column2(col2), column3(col3) |
81 | { |
82 | } |
83 | |
84 | //! constructor that generates a multiple of the identity matrix |
85 | explicit PX_CUDA_CALLABLE PX_INLINE PxMat44(float r) |
86 | : column0(r, 0.0f, 0.0f, 0.0f) |
87 | , column1(0.0f, r, 0.0f, 0.0f) |
88 | , column2(0.0f, 0.0f, r, 0.0f) |
89 | , column3(0.0f, 0.0f, 0.0f, r) |
90 | { |
91 | } |
92 | |
93 | //! Construct from three base vectors and a translation |
94 | PX_CUDA_CALLABLE PxMat44(const PxVec3& col0, const PxVec3& col1, const PxVec3& col2, const PxVec3& col3) |
95 | : column0(col0, 0), column1(col1, 0), column2(col2, 0), column3(col3, 1.0f) |
96 | { |
97 | } |
98 | |
99 | //! Construct from float[16] |
100 | explicit PX_CUDA_CALLABLE PX_INLINE PxMat44(float values[]) |
101 | : column0(values[0], values[1], values[2], values[3]) |
102 | , column1(values[4], values[5], values[6], values[7]) |
103 | , column2(values[8], values[9], values[10], values[11]) |
104 | , column3(values[12], values[13], values[14], values[15]) |
105 | { |
106 | } |
107 | |
108 | //! Construct from a quaternion |
109 | explicit PX_CUDA_CALLABLE PX_INLINE PxMat44(const PxQuat& q) |
110 | { |
111 | const float x = q.x; |
112 | const float y = q.y; |
113 | const float z = q.z; |
114 | const float w = q.w; |
115 | |
116 | const float x2 = x + x; |
117 | const float y2 = y + y; |
118 | const float z2 = z + z; |
119 | |
120 | const float xx = x2 * x; |
121 | const float yy = y2 * y; |
122 | const float zz = z2 * z; |
123 | |
124 | const float xy = x2 * y; |
125 | const float xz = x2 * z; |
126 | const float xw = x2 * w; |
127 | |
128 | const float yz = y2 * z; |
129 | const float yw = y2 * w; |
130 | const float zw = z2 * w; |
131 | |
132 | column0 = PxVec4(1.0f - yy - zz, xy + zw, xz - yw, 0.0f); |
133 | column1 = PxVec4(xy - zw, 1.0f - xx - zz, yz + xw, 0.0f); |
134 | column2 = PxVec4(xz + yw, yz - xw, 1.0f - xx - yy, 0.0f); |
135 | column3 = PxVec4(0.0f, 0.0f, 0.0f, 1.0f); |
136 | } |
137 | |
138 | //! Construct from a diagonal vector |
139 | explicit PX_CUDA_CALLABLE PX_INLINE PxMat44(const PxVec4& diagonal) |
140 | : column0(diagonal.x, 0.0f, 0.0f, 0.0f) |
141 | , column1(0.0f, diagonal.y, 0.0f, 0.0f) |
142 | , column2(0.0f, 0.0f, diagonal.z, 0.0f) |
143 | , column3(0.0f, 0.0f, 0.0f, diagonal.w) |
144 | { |
145 | } |
146 | |
147 | //! Construct from Mat33 and a translation |
148 | PX_CUDA_CALLABLE PxMat44(const PxMat33& axes, const PxVec3& position) |
149 | : column0(axes.column0, 0.0f), column1(axes.column1, 0.0f), column2(axes.column2, 0.0f), column3(position, 1.0f) |
150 | { |
151 | } |
152 | |
153 | PX_CUDA_CALLABLE PxMat44(const PxTransform& t) |
154 | { |
155 | *this = PxMat44(PxMat33(t.q), t.p); |
156 | } |
157 | |
158 | /** |
159 | \brief returns true if the two matrices are exactly equal |
160 | */ |
161 | PX_CUDA_CALLABLE PX_INLINE bool operator==(const PxMat44& m) const |
162 | { |
163 | return column0 == m.column0 && column1 == m.column1 && column2 == m.column2 && column3 == m.column3; |
164 | } |
165 | |
166 | //! Copy constructor |
167 | PX_CUDA_CALLABLE PX_INLINE PxMat44(const PxMat44& other) |
168 | : column0(other.column0), column1(other.column1), column2(other.column2), column3(other.column3) |
169 | { |
170 | } |
171 | |
172 | //! Assignment operator |
173 | PX_CUDA_CALLABLE PX_INLINE PxMat44& operator=(const PxMat44& other) |
174 | { |
175 | column0 = other.column0; |
176 | column1 = other.column1; |
177 | column2 = other.column2; |
178 | column3 = other.column3; |
179 | return *this; |
180 | } |
181 | |
182 | //! Get transposed matrix |
183 | PX_CUDA_CALLABLE PX_INLINE const PxMat44 getTranspose() const |
184 | { |
185 | return PxMat44( |
186 | PxVec4(column0.x, column1.x, column2.x, column3.x), PxVec4(column0.y, column1.y, column2.y, column3.y), |
187 | PxVec4(column0.z, column1.z, column2.z, column3.z), PxVec4(column0.w, column1.w, column2.w, column3.w)); |
188 | } |
189 | |
190 | //! Unary minus |
191 | PX_CUDA_CALLABLE PX_INLINE const PxMat44 operator-() const |
192 | { |
193 | return PxMat44(-column0, -column1, -column2, -column3); |
194 | } |
195 | |
196 | //! Add |
197 | PX_CUDA_CALLABLE PX_INLINE const PxMat44 operator+(const PxMat44& other) const |
198 | { |
199 | return PxMat44(column0 + other.column0, column1 + other.column1, column2 + other.column2, |
200 | column3 + other.column3); |
201 | } |
202 | |
203 | //! Subtract |
204 | PX_CUDA_CALLABLE PX_INLINE const PxMat44 operator-(const PxMat44& other) const |
205 | { |
206 | return PxMat44(column0 - other.column0, column1 - other.column1, column2 - other.column2, |
207 | column3 - other.column3); |
208 | } |
209 | |
210 | //! Scalar multiplication |
211 | PX_CUDA_CALLABLE PX_INLINE const PxMat44 operator*(float scalar) const |
212 | { |
213 | return PxMat44(column0 * scalar, column1 * scalar, column2 * scalar, column3 * scalar); |
214 | } |
215 | |
216 | friend PxMat44 operator*(float, const PxMat44&); |
217 | |
218 | //! Matrix multiplication |
219 | PX_CUDA_CALLABLE PX_INLINE const PxMat44 operator*(const PxMat44& other) const |
220 | { |
221 | // Rows from this <dot> columns from other |
222 | // column0 = transform(other.column0) etc |
223 | return PxMat44(transform(other: other.column0), transform(other: other.column1), transform(other: other.column2), |
224 | transform(other: other.column3)); |
225 | } |
226 | |
227 | // a <op>= b operators |
228 | |
229 | //! Equals-add |
230 | PX_CUDA_CALLABLE PX_INLINE PxMat44& operator+=(const PxMat44& other) |
231 | { |
232 | column0 += other.column0; |
233 | column1 += other.column1; |
234 | column2 += other.column2; |
235 | column3 += other.column3; |
236 | return *this; |
237 | } |
238 | |
239 | //! Equals-sub |
240 | PX_CUDA_CALLABLE PX_INLINE PxMat44& operator-=(const PxMat44& other) |
241 | { |
242 | column0 -= other.column0; |
243 | column1 -= other.column1; |
244 | column2 -= other.column2; |
245 | column3 -= other.column3; |
246 | return *this; |
247 | } |
248 | |
249 | //! Equals scalar multiplication |
250 | PX_CUDA_CALLABLE PX_INLINE PxMat44& operator*=(float scalar) |
251 | { |
252 | column0 *= scalar; |
253 | column1 *= scalar; |
254 | column2 *= scalar; |
255 | column3 *= scalar; |
256 | return *this; |
257 | } |
258 | |
259 | //! Equals matrix multiplication |
260 | PX_CUDA_CALLABLE PX_INLINE PxMat44& operator*=(const PxMat44& other) |
261 | { |
262 | *this = *this * other; |
263 | return *this; |
264 | } |
265 | |
266 | //! Element access, mathematical way! |
267 | PX_CUDA_CALLABLE PX_FORCE_INLINE float operator()(unsigned int row, unsigned int col) const |
268 | { |
269 | return (*this)[col][row]; |
270 | } |
271 | |
272 | //! Element access, mathematical way! |
273 | PX_CUDA_CALLABLE PX_FORCE_INLINE float& operator()(unsigned int row, unsigned int col) |
274 | { |
275 | return (*this)[col][row]; |
276 | } |
277 | |
278 | //! Transform vector by matrix, equal to v' = M*v |
279 | PX_CUDA_CALLABLE PX_INLINE const PxVec4 transform(const PxVec4& other) const |
280 | { |
281 | return column0 * other.x + column1 * other.y + column2 * other.z + column3 * other.w; |
282 | } |
283 | |
284 | //! Transform vector by matrix, equal to v' = M*v |
285 | PX_CUDA_CALLABLE PX_INLINE const PxVec3 transform(const PxVec3& other) const |
286 | { |
287 | return transform(other: PxVec4(other, 1.0f)).getXYZ(); |
288 | } |
289 | |
290 | //! Rotate vector by matrix, equal to v' = M*v |
291 | PX_CUDA_CALLABLE PX_INLINE const PxVec4 rotate(const PxVec4& other) const |
292 | { |
293 | return column0 * other.x + column1 * other.y + column2 * other.z; // + column3*0; |
294 | } |
295 | |
296 | //! Rotate vector by matrix, equal to v' = M*v |
297 | PX_CUDA_CALLABLE PX_INLINE const PxVec3 rotate(const PxVec3& other) const |
298 | { |
299 | return rotate(other: PxVec4(other, 1.0f)).getXYZ(); |
300 | } |
301 | |
302 | PX_CUDA_CALLABLE PX_INLINE const PxVec3 getBasis(int num) const |
303 | { |
304 | PX_SHARED_ASSERT(num >= 0 && num < 3); |
305 | return (&column0)[num].getXYZ(); |
306 | } |
307 | |
308 | PX_CUDA_CALLABLE PX_INLINE const PxVec3 getPosition() const |
309 | { |
310 | return column3.getXYZ(); |
311 | } |
312 | |
313 | PX_CUDA_CALLABLE PX_INLINE void setPosition(const PxVec3& position) |
314 | { |
315 | column3.x = position.x; |
316 | column3.y = position.y; |
317 | column3.z = position.z; |
318 | } |
319 | |
320 | PX_CUDA_CALLABLE PX_FORCE_INLINE const float* front() const |
321 | { |
322 | return &column0.x; |
323 | } |
324 | |
325 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec4& operator[](unsigned int num) |
326 | { |
327 | return (&column0)[num]; |
328 | } |
329 | PX_CUDA_CALLABLE PX_FORCE_INLINE const PxVec4& operator[](unsigned int num) const |
330 | { |
331 | return (&column0)[num]; |
332 | } |
333 | |
334 | PX_CUDA_CALLABLE PX_INLINE void scale(const PxVec4& p) |
335 | { |
336 | column0 *= p.x; |
337 | column1 *= p.y; |
338 | column2 *= p.z; |
339 | column3 *= p.w; |
340 | } |
341 | |
342 | PX_CUDA_CALLABLE PX_INLINE const PxMat44 inverseRT(void) const |
343 | { |
344 | PxVec3 r0(column0.x, column1.x, column2.x), r1(column0.y, column1.y, column2.y), |
345 | r2(column0.z, column1.z, column2.z); |
346 | |
347 | return PxMat44(r0, r1, r2, -(r0 * column3.x + r1 * column3.y + r2 * column3.z)); |
348 | } |
349 | |
350 | PX_CUDA_CALLABLE PX_INLINE bool isFinite() const |
351 | { |
352 | return column0.isFinite() && column1.isFinite() && column2.isFinite() && column3.isFinite(); |
353 | } |
354 | |
355 | // Data, see above for format! |
356 | |
357 | PxVec4 column0, column1, column2, column3; // the four base vectors |
358 | }; |
359 | |
360 | // implementation from PxTransform.h |
361 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxTransform::PxTransform(const PxMat44& m) |
362 | { |
363 | PxVec3 column0 = PxVec3(m.column0.x, m.column0.y, m.column0.z); |
364 | PxVec3 column1 = PxVec3(m.column1.x, m.column1.y, m.column1.z); |
365 | PxVec3 column2 = PxVec3(m.column2.x, m.column2.y, m.column2.z); |
366 | |
367 | q = PxQuat(PxMat33(column0, column1, column2)); |
368 | p = PxVec3(m.column3.x, m.column3.y, m.column3.z); |
369 | } |
370 | |
371 | #if !PX_DOXYGEN |
372 | } // namespace physx |
373 | #endif |
374 | |
375 | /** @} */ |
376 | #endif // #ifndef PXFOUNDATION_PXMAT44_H |
377 | |