1 /*
2 Copyright (C) 2001-2006, William Joseph.
3 All Rights Reserved.
4
5 This file is part of GtkRadiant.
6
7 GtkRadiant is free software; you can redistribute it and/or modify
8 it under the terms of the GNU General Public License as published by
9 the Free Software Foundation; either version 2 of the License, or
10 (at your option) any later version.
11
12 GtkRadiant is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with GtkRadiant; if not, write to the Free Software
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
20 */
21
22 #if !defined( INCLUDED_MATH_MATRIX_H )
23 #define INCLUDED_MATH_MATRIX_H
24
25 /// \file
26 /// \brief Matrix data types and related operations.
27
28 #include "math/vector.h"
29
30 /// \brief A 4x4 matrix stored in single-precision floating-point.
31 class Matrix4
32 {
33 float m_elements[16];
34 public:
35
Matrix4()36 Matrix4(){
37 }
Matrix4(float xx_,float xy_,float xz_,float xw_,float yx_,float yy_,float yz_,float yw_,float zx_,float zy_,float zz_,float zw_,float tx_,float ty_,float tz_,float tw_)38 Matrix4( float xx_, float xy_, float xz_, float xw_,
39 float yx_, float yy_, float yz_, float yw_,
40 float zx_, float zy_, float zz_, float zw_,
41 float tx_, float ty_, float tz_, float tw_ ){
42 xx() = xx_;
43 xy() = xy_;
44 xz() = xz_;
45 xw() = xw_;
46 yx() = yx_;
47 yy() = yy_;
48 yz() = yz_;
49 yw() = yw_;
50 zx() = zx_;
51 zy() = zy_;
52 zz() = zz_;
53 zw() = zw_;
54 tx() = tx_;
55 ty() = ty_;
56 tz() = tz_;
57 tw() = tw_;
58 }
59
xx()60 float& xx(){
61 return m_elements[0];
62 }
xx()63 const float& xx() const {
64 return m_elements[0];
65 }
xy()66 float& xy(){
67 return m_elements[1];
68 }
xy()69 const float& xy() const {
70 return m_elements[1];
71 }
xz()72 float& xz(){
73 return m_elements[2];
74 }
xz()75 const float& xz() const {
76 return m_elements[2];
77 }
xw()78 float& xw(){
79 return m_elements[3];
80 }
xw()81 const float& xw() const {
82 return m_elements[3];
83 }
yx()84 float& yx(){
85 return m_elements[4];
86 }
yx()87 const float& yx() const {
88 return m_elements[4];
89 }
yy()90 float& yy(){
91 return m_elements[5];
92 }
yy()93 const float& yy() const {
94 return m_elements[5];
95 }
yz()96 float& yz(){
97 return m_elements[6];
98 }
yz()99 const float& yz() const {
100 return m_elements[6];
101 }
yw()102 float& yw(){
103 return m_elements[7];
104 }
yw()105 const float& yw() const {
106 return m_elements[7];
107 }
zx()108 float& zx(){
109 return m_elements[8];
110 }
zx()111 const float& zx() const {
112 return m_elements[8];
113 }
zy()114 float& zy(){
115 return m_elements[9];
116 }
zy()117 const float& zy() const {
118 return m_elements[9];
119 }
zz()120 float& zz(){
121 return m_elements[10];
122 }
zz()123 const float& zz() const {
124 return m_elements[10];
125 }
zw()126 float& zw(){
127 return m_elements[11];
128 }
zw()129 const float& zw() const {
130 return m_elements[11];
131 }
tx()132 float& tx(){
133 return m_elements[12];
134 }
tx()135 const float& tx() const {
136 return m_elements[12];
137 }
ty()138 float& ty(){
139 return m_elements[13];
140 }
ty()141 const float& ty() const {
142 return m_elements[13];
143 }
tz()144 float& tz(){
145 return m_elements[14];
146 }
tz()147 const float& tz() const {
148 return m_elements[14];
149 }
tw()150 float& tw(){
151 return m_elements[15];
152 }
tw()153 const float& tw() const {
154 return m_elements[15];
155 }
156
x()157 Vector4& x(){
158 return reinterpret_cast<Vector4&>( xx() );
159 }
x()160 const Vector4& x() const {
161 return reinterpret_cast<const Vector4&>( xx() );
162 }
y()163 Vector4& y(){
164 return reinterpret_cast<Vector4&>( yx() );
165 }
y()166 const Vector4& y() const {
167 return reinterpret_cast<const Vector4&>( yx() );
168 }
z()169 Vector4& z(){
170 return reinterpret_cast<Vector4&>( zx() );
171 }
z()172 const Vector4& z() const {
173 return reinterpret_cast<const Vector4&>( zx() );
174 }
t()175 Vector4& t(){
176 return reinterpret_cast<Vector4&>( tx() );
177 }
t()178 const Vector4& t() const {
179 return reinterpret_cast<const Vector4&>( tx() );
180 }
181
index(std::size_t i)182 const float& index( std::size_t i ) const {
183 return m_elements[i];
184 }
index(std::size_t i)185 float& index( std::size_t i ){
186 return m_elements[i];
187 }
188 const float& operator[]( std::size_t i ) const {
189 return m_elements[i];
190 }
191 float& operator[]( std::size_t i ){
192 return m_elements[i];
193 }
index(std::size_t r,std::size_t c)194 const float& index( std::size_t r, std::size_t c ) const {
195 return m_elements[( r << 2 ) + c];
196 }
index(std::size_t r,std::size_t c)197 float& index( std::size_t r, std::size_t c ){
198 return m_elements[( r << 2 ) + c];
199 }
200 };
201
202 /// \brief The 4x4 identity matrix.
203 const Matrix4 g_matrix4_identity(
204 1, 0, 0, 0,
205 0, 1, 0, 0,
206 0, 0, 1, 0,
207 0, 0, 0, 1
208 );
209
210
211 /// \brief Returns true if \p self and \p other are exactly element-wise equal.
212 inline bool operator==( const Matrix4& self, const Matrix4& other ){
213 return self.xx() == other.xx() && self.xy() == other.xy() && self.xz() == other.xz() && self.xw() == other.xw()
214 && self.yx() == other.yx() && self.yy() == other.yy() && self.yz() == other.yz() && self.yw() == other.yw()
215 && self.zx() == other.zx() && self.zy() == other.zy() && self.zz() == other.zz() && self.zw() == other.zw()
216 && self.tx() == other.tx() && self.ty() == other.ty() && self.tz() == other.tz() && self.tw() == other.tw();
217 }
218
219 /// \brief Returns true if \p self and \p other are exactly element-wise equal.
matrix4_equal(const Matrix4 & self,const Matrix4 & other)220 inline bool matrix4_equal( const Matrix4& self, const Matrix4& other ){
221 return self == other;
222 }
223
224 /// \brief Returns true if \p self and \p other are element-wise equal within \p epsilon.
matrix4_equal_epsilon(const Matrix4 & self,const Matrix4 & other,float epsilon)225 inline bool matrix4_equal_epsilon( const Matrix4& self, const Matrix4& other, float epsilon ){
226 return float_equal_epsilon( self.xx(), other.xx(), epsilon )
227 && float_equal_epsilon( self.xy(), other.xy(), epsilon )
228 && float_equal_epsilon( self.xz(), other.xz(), epsilon )
229 && float_equal_epsilon( self.xw(), other.xw(), epsilon )
230 && float_equal_epsilon( self.yx(), other.yx(), epsilon )
231 && float_equal_epsilon( self.yy(), other.yy(), epsilon )
232 && float_equal_epsilon( self.yz(), other.yz(), epsilon )
233 && float_equal_epsilon( self.yw(), other.yw(), epsilon )
234 && float_equal_epsilon( self.zx(), other.zx(), epsilon )
235 && float_equal_epsilon( self.zy(), other.zy(), epsilon )
236 && float_equal_epsilon( self.zz(), other.zz(), epsilon )
237 && float_equal_epsilon( self.zw(), other.zw(), epsilon )
238 && float_equal_epsilon( self.tx(), other.tx(), epsilon )
239 && float_equal_epsilon( self.ty(), other.ty(), epsilon )
240 && float_equal_epsilon( self.tz(), other.tz(), epsilon )
241 && float_equal_epsilon( self.tw(), other.tw(), epsilon );
242 }
243
244 /// \brief Returns true if \p self and \p other are exactly element-wise equal.
245 /// \p self and \p other must be affine.
matrix4_affine_equal(const Matrix4 & self,const Matrix4 & other)246 inline bool matrix4_affine_equal( const Matrix4& self, const Matrix4& other ){
247 return self[0] == other[0]
248 && self[1] == other[1]
249 && self[2] == other[2]
250 && self[4] == other[4]
251 && self[5] == other[5]
252 && self[6] == other[6]
253 && self[8] == other[8]
254 && self[9] == other[9]
255 && self[10] == other[10]
256 && self[12] == other[12]
257 && self[13] == other[13]
258 && self[14] == other[14];
259 }
260
261 enum Matrix4Handedness
262 {
263 MATRIX4_RIGHTHANDED = 0,
264 MATRIX4_LEFTHANDED = 1,
265 };
266
267 /// \brief Returns MATRIX4_RIGHTHANDED if \p self is right-handed, else returns MATRIX4_LEFTHANDED.
matrix4_handedness(const Matrix4 & self)268 inline Matrix4Handedness matrix4_handedness( const Matrix4& self ){
269 return (
270 vector3_dot(
271 vector3_cross( vector4_to_vector3( self.x() ), vector4_to_vector3( self.y() ) ),
272 vector4_to_vector3( self.z() )
273 )
274 < 0.0
275 ) ? MATRIX4_LEFTHANDED : MATRIX4_RIGHTHANDED;
276 }
277
278
279
280
281
282 /// \brief Returns \p self post-multiplied by \p other.
matrix4_multiplied_by_matrix4(const Matrix4 & self,const Matrix4 & other)283 inline Matrix4 matrix4_multiplied_by_matrix4( const Matrix4& self, const Matrix4& other ){
284 return Matrix4(
285 other[0] * self[0] + other[1] * self[4] + other[2] * self[8] + other[3] * self[12],
286 other[0] * self[1] + other[1] * self[5] + other[2] * self[9] + other[3] * self[13],
287 other[0] * self[2] + other[1] * self[6] + other[2] * self[10] + other[3] * self[14],
288 other[0] * self[3] + other[1] * self[7] + other[2] * self[11] + other[3] * self[15],
289 other[4] * self[0] + other[5] * self[4] + other[6] * self[8] + other[7] * self[12],
290 other[4] * self[1] + other[5] * self[5] + other[6] * self[9] + other[7] * self[13],
291 other[4] * self[2] + other[5] * self[6] + other[6] * self[10] + other[7] * self[14],
292 other[4] * self[3] + other[5] * self[7] + other[6] * self[11] + other[7] * self[15],
293 other[8] * self[0] + other[9] * self[4] + other[10] * self[8] + other[11] * self[12],
294 other[8] * self[1] + other[9] * self[5] + other[10] * self[9] + other[11] * self[13],
295 other[8] * self[2] + other[9] * self[6] + other[10] * self[10] + other[11] * self[14],
296 other[8] * self[3] + other[9] * self[7] + other[10] * self[11] + other[11] * self[15],
297 other[12] * self[0] + other[13] * self[4] + other[14] * self[8] + other[15] * self[12],
298 other[12] * self[1] + other[13] * self[5] + other[14] * self[9] + other[15] * self[13],
299 other[12] * self[2] + other[13] * self[6] + other[14] * self[10] + other[15] * self[14],
300 other[12] * self[3] + other[13] * self[7] + other[14] * self[11] + other[15] * self[15]
301 );
302 }
303
304 /// \brief Post-multiplies \p self by \p other in-place.
matrix4_multiply_by_matrix4(Matrix4 & self,const Matrix4 & other)305 inline void matrix4_multiply_by_matrix4( Matrix4& self, const Matrix4& other ){
306 self = matrix4_multiplied_by_matrix4( self, other );
307 }
308
309
310 /// \brief Returns \p self pre-multiplied by \p other.
matrix4_premultiplied_by_matrix4(const Matrix4 & self,const Matrix4 & other)311 inline Matrix4 matrix4_premultiplied_by_matrix4( const Matrix4& self, const Matrix4& other ){
312 #if 1
313 return matrix4_multiplied_by_matrix4( other, self );
314 #else
315 return Matrix4(
316 self[0] * other[0] + self[1] * other[4] + self[2] * other[8] + self[3] * other[12],
317 self[0] * other[1] + self[1] * other[5] + self[2] * other[9] + self[3] * other[13],
318 self[0] * other[2] + self[1] * other[6] + self[2] * other[10] + self[3] * other[14],
319 self[0] * other[3] + self[1] * other[7] + self[2] * other[11] + self[3] * other[15],
320 self[4] * other[0] + self[5] * other[4] + self[6] * other[8] + self[7] * other[12],
321 self[4] * other[1] + self[5] * other[5] + self[6] * other[9] + self[7] * other[13],
322 self[4] * other[2] + self[5] * other[6] + self[6] * other[10] + self[7] * other[14],
323 self[4] * other[3] + self[5] * other[7] + self[6] * other[11] + self[7] * other[15],
324 self[8] * other[0] + self[9] * other[4] + self[10] * other[8] + self[11] * other[12],
325 self[8] * other[1] + self[9] * other[5] + self[10] * other[9] + self[11] * other[13],
326 self[8] * other[2] + self[9] * other[6] + self[10] * other[10] + self[11] * other[14],
327 self[8] * other[3] + self[9] * other[7] + self[10] * other[11] + self[11] * other[15],
328 self[12] * other[0] + self[13] * other[4] + self[14] * other[8] + self[15] * other[12],
329 self[12] * other[1] + self[13] * other[5] + self[14] * other[9] + self[15] * other[13],
330 self[12] * other[2] + self[13] * other[6] + self[14] * other[10] + self[15] * other[14],
331 self[12] * other[3] + self[13] * other[7] + self[14] * other[11] + self[15] * other[15]
332 );
333 #endif
334 }
335
336 /// \brief Pre-multiplies \p self by \p other in-place.
matrix4_premultiply_by_matrix4(Matrix4 & self,const Matrix4 & other)337 inline void matrix4_premultiply_by_matrix4( Matrix4& self, const Matrix4& other ){
338 self = matrix4_premultiplied_by_matrix4( self, other );
339 }
340
341 /// \brief returns true if \p transform is affine.
matrix4_is_affine(const Matrix4 & transform)342 inline bool matrix4_is_affine( const Matrix4& transform ){
343 return transform[3] == 0 && transform[7] == 0 && transform[11] == 0 && transform[15] == 1;
344 }
345
346 /// \brief Returns \p self post-multiplied by \p other.
347 /// \p self and \p other must be affine.
matrix4_affine_multiplied_by_matrix4(const Matrix4 & self,const Matrix4 & other)348 inline Matrix4 matrix4_affine_multiplied_by_matrix4( const Matrix4& self, const Matrix4& other ){
349 return Matrix4(
350 other[0] * self[0] + other[1] * self[4] + other[2] * self[8],
351 other[0] * self[1] + other[1] * self[5] + other[2] * self[9],
352 other[0] * self[2] + other[1] * self[6] + other[2] * self[10],
353 0,
354 other[4] * self[0] + other[5] * self[4] + other[6] * self[8],
355 other[4] * self[1] + other[5] * self[5] + other[6] * self[9],
356 other[4] * self[2] + other[5] * self[6] + other[6] * self[10],
357 0,
358 other[8] * self[0] + other[9] * self[4] + other[10] * self[8],
359 other[8] * self[1] + other[9] * self[5] + other[10] * self[9],
360 other[8] * self[2] + other[9] * self[6] + other[10] * self[10],
361 0,
362 other[12] * self[0] + other[13] * self[4] + other[14] * self[8] + self[12],
363 other[12] * self[1] + other[13] * self[5] + other[14] * self[9] + self[13],
364 other[12] * self[2] + other[13] * self[6] + other[14] * self[10] + self[14],
365 1
366 );
367 }
368
369 /// \brief Post-multiplies \p self by \p other in-place.
370 /// \p self and \p other must be affine.
matrix4_affine_multiply_by_matrix4(Matrix4 & self,const Matrix4 & other)371 inline void matrix4_affine_multiply_by_matrix4( Matrix4& self, const Matrix4& other ){
372 self = matrix4_affine_multiplied_by_matrix4( self, other );
373 }
374
375 /// \brief Returns \p self pre-multiplied by \p other.
376 /// \p self and \p other must be affine.
matrix4_affine_premultiplied_by_matrix4(const Matrix4 & self,const Matrix4 & other)377 inline Matrix4 matrix4_affine_premultiplied_by_matrix4( const Matrix4& self, const Matrix4& other ){
378 #if 1
379 return matrix4_affine_multiplied_by_matrix4( other, self );
380 #else
381 return Matrix4(
382 self[0] * other[0] + self[1] * other[4] + self[2] * other[8],
383 self[0] * other[1] + self[1] * other[5] + self[2] * other[9],
384 self[0] * other[2] + self[1] * other[6] + self[2] * other[10],
385 0,
386 self[4] * other[0] + self[5] * other[4] + self[6] * other[8],
387 self[4] * other[1] + self[5] * other[5] + self[6] * other[9],
388 self[4] * other[2] + self[5] * other[6] + self[6] * other[10],
389 0,
390 self[8] * other[0] + self[9] * other[4] + self[10] * other[8],
391 self[8] * other[1] + self[9] * other[5] + self[10] * other[9],
392 self[8] * other[2] + self[9] * other[6] + self[10] * other[10],
393 0,
394 self[12] * other[0] + self[13] * other[4] + self[14] * other[8] + other[12],
395 self[12] * other[1] + self[13] * other[5] + self[14] * other[9] + other[13],
396 self[12] * other[2] + self[13] * other[6] + self[14] * other[10] + other[14],
397 1
398 )
399 );
400 #endif
401 }
402
403 /// \brief Pre-multiplies \p self by \p other in-place.
404 /// \p self and \p other must be affine.
matrix4_affine_premultiply_by_matrix4(Matrix4 & self,const Matrix4 & other)405 inline void matrix4_affine_premultiply_by_matrix4( Matrix4& self, const Matrix4& other ){
406 self = matrix4_affine_premultiplied_by_matrix4( self, other );
407 }
408
409 /// \brief Returns \p point transformed by \p self.
410 template<typename Element>
matrix4_transformed_point(const Matrix4 & self,const BasicVector3<Element> & point)411 inline BasicVector3<Element> matrix4_transformed_point( const Matrix4& self, const BasicVector3<Element>& point ){
412 return BasicVector3<Element>(
413 static_cast<Element>( self[0] * point[0] + self[4] * point[1] + self[8] * point[2] + self[12] ),
414 static_cast<Element>( self[1] * point[0] + self[5] * point[1] + self[9] * point[2] + self[13] ),
415 static_cast<Element>( self[2] * point[0] + self[6] * point[1] + self[10] * point[2] + self[14] )
416 );
417 }
418
419 /// \brief Transforms \p point by \p self in-place.
420 template<typename Element>
matrix4_transform_point(const Matrix4 & self,BasicVector3<Element> & point)421 inline void matrix4_transform_point( const Matrix4& self, BasicVector3<Element>& point ){
422 point = matrix4_transformed_point( self, point );
423 }
424
425 /// \brief Returns \p direction transformed by \p self.
426 template<typename Element>
matrix4_transformed_direction(const Matrix4 & self,const BasicVector3<Element> & direction)427 inline BasicVector3<Element> matrix4_transformed_direction( const Matrix4& self, const BasicVector3<Element>& direction ){
428 return BasicVector3<Element>(
429 static_cast<Element>( self[0] * direction[0] + self[4] * direction[1] + self[8] * direction[2] ),
430 static_cast<Element>( self[1] * direction[0] + self[5] * direction[1] + self[9] * direction[2] ),
431 static_cast<Element>( self[2] * direction[0] + self[6] * direction[1] + self[10] * direction[2] )
432 );
433 }
434
435 /// \brief Transforms \p direction by \p self in-place.
436 template<typename Element>
matrix4_transform_direction(const Matrix4 & self,BasicVector3<Element> & normal)437 inline void matrix4_transform_direction( const Matrix4& self, BasicVector3<Element>& normal ){
438 normal = matrix4_transformed_direction( self, normal );
439 }
440
441 /// \brief Returns \p vector4 transformed by \p self.
matrix4_transformed_vector4(const Matrix4 & self,const Vector4 & vector4)442 inline Vector4 matrix4_transformed_vector4( const Matrix4& self, const Vector4& vector4 ){
443 return Vector4(
444 self[0] * vector4[0] + self[4] * vector4[1] + self[8] * vector4[2] + self[12] * vector4[3],
445 self[1] * vector4[0] + self[5] * vector4[1] + self[9] * vector4[2] + self[13] * vector4[3],
446 self[2] * vector4[0] + self[6] * vector4[1] + self[10] * vector4[2] + self[14] * vector4[3],
447 self[3] * vector4[0] + self[7] * vector4[1] + self[11] * vector4[2] + self[15] * vector4[3]
448 );
449 }
450
451 /// \brief Transforms \p vector4 by \p self in-place.
matrix4_transform_vector4(const Matrix4 & self,Vector4 & vector4)452 inline void matrix4_transform_vector4( const Matrix4& self, Vector4& vector4 ){
453 vector4 = matrix4_transformed_vector4( self, vector4 );
454 }
455
456
457 /// \brief Transposes \p self in-place.
matrix4_transpose(Matrix4 & self)458 inline void matrix4_transpose( Matrix4& self ){
459 std::swap( self.xy(), self.yx() );
460 std::swap( self.xz(), self.zx() );
461 std::swap( self.xw(), self.tx() );
462 std::swap( self.yz(), self.zy() );
463 std::swap( self.yw(), self.ty() );
464 std::swap( self.zw(), self.tz() );
465 }
466
467 /// \brief Returns \p self transposed.
matrix4_transposed(const Matrix4 & self)468 inline Matrix4 matrix4_transposed( const Matrix4& self ){
469 return Matrix4(
470 self.xx(),
471 self.yx(),
472 self.zx(),
473 self.tx(),
474 self.xy(),
475 self.yy(),
476 self.zy(),
477 self.ty(),
478 self.xz(),
479 self.yz(),
480 self.zz(),
481 self.tz(),
482 self.xw(),
483 self.yw(),
484 self.zw(),
485 self.tw()
486 );
487 }
488
489
490 /// \brief Inverts an affine transform in-place.
491 /// Adapted from Graphics Gems 2.
matrix4_affine_inverse(const Matrix4 & self)492 inline Matrix4 matrix4_affine_inverse( const Matrix4& self ){
493 Matrix4 result;
494
495 // determinant of rotation submatrix
496 double det
497 = self[0] * ( self[5] * self[10] - self[9] * self[6] )
498 - self[1] * ( self[4] * self[10] - self[8] * self[6] )
499 + self[2] * ( self[4] * self[9] - self[8] * self[5] );
500
501 // throw exception here if (det*det < 1e-25)
502
503 // invert rotation submatrix
504 det = 1.0 / det;
505
506 result[0] = static_cast<float>( ( self[5] * self[10] - self[6] * self[9] ) * det );
507 result[1] = static_cast<float>( -( self[1] * self[10] - self[2] * self[9] ) * det );
508 result[2] = static_cast<float>( ( self[1] * self[6] - self[2] * self[5] ) * det );
509 result[3] = 0;
510 result[4] = static_cast<float>( -( self[4] * self[10] - self[6] * self[8] ) * det );
511 result[5] = static_cast<float>( ( self[0] * self[10] - self[2] * self[8] ) * det );
512 result[6] = static_cast<float>( -( self[0] * self[6] - self[2] * self[4] ) * det );
513 result[7] = 0;
514 result[8] = static_cast<float>( ( self[4] * self[9] - self[5] * self[8] ) * det );
515 result[9] = static_cast<float>( -( self[0] * self[9] - self[1] * self[8] ) * det );
516 result[10] = static_cast<float>( ( self[0] * self[5] - self[1] * self[4] ) * det );
517 result[11] = 0;
518
519 // multiply translation part by rotation
520 result[12] = -( self[12] * result[0] +
521 self[13] * result[4] +
522 self[14] * result[8] );
523 result[13] = -( self[12] * result[1] +
524 self[13] * result[5] +
525 self[14] * result[9] );
526 result[14] = -( self[12] * result[2] +
527 self[13] * result[6] +
528 self[14] * result[10] );
529 result[15] = 1;
530
531 return result;
532 }
533
matrix4_affine_invert(Matrix4 & self)534 inline void matrix4_affine_invert( Matrix4& self ){
535 self = matrix4_affine_inverse( self );
536 }
537
538 /// \brief A compile-time-constant integer.
539 template<int VALUE_>
540 struct IntegralConstant
541 {
542 enum unnamed_ { VALUE = VALUE_ };
543 };
544
545 /// \brief A compile-time-constant row/column index into a 4x4 matrix.
546 template<typename Row, typename Col>
547 class Matrix4Index
548 {
549 public:
550 typedef IntegralConstant<Row::VALUE> r;
551 typedef IntegralConstant<Col::VALUE> c;
552 typedef IntegralConstant<( r::VALUE * 4 ) + c::VALUE> i;
553 };
554
555 /// \brief A functor which returns the cofactor of a 3x3 submatrix obtained by ignoring a given row and column of a 4x4 matrix.
556 /// \param Row Defines the compile-time-constant integers x, y and z with values corresponding to the indices of the three rows to use.
557 /// \param Col Defines the compile-time-constant integers x, y and z with values corresponding to the indices of the three columns to use.
558 template<typename Row, typename Col>
559 class Matrix4Cofactor
560 {
561 public:
562 typedef typename Matrix4Index<typename Row::x, typename Col::x>::i xx;
563 typedef typename Matrix4Index<typename Row::x, typename Col::y>::i xy;
564 typedef typename Matrix4Index<typename Row::x, typename Col::z>::i xz;
565 typedef typename Matrix4Index<typename Row::y, typename Col::x>::i yx;
566 typedef typename Matrix4Index<typename Row::y, typename Col::y>::i yy;
567 typedef typename Matrix4Index<typename Row::y, typename Col::z>::i yz;
568 typedef typename Matrix4Index<typename Row::z, typename Col::x>::i zx;
569 typedef typename Matrix4Index<typename Row::z, typename Col::y>::i zy;
570 typedef typename Matrix4Index<typename Row::z, typename Col::z>::i zz;
apply(const Matrix4 & self)571 static double apply( const Matrix4& self ){
572 return self[xx::VALUE] * ( self[yy::VALUE] * self[zz::VALUE] - self[zy::VALUE] * self[yz::VALUE] )
573 - self[xy::VALUE] * ( self[yx::VALUE] * self[zz::VALUE] - self[zx::VALUE] * self[yz::VALUE] )
574 + self[xz::VALUE] * ( self[yx::VALUE] * self[zy::VALUE] - self[zx::VALUE] * self[yy::VALUE] );
575 }
576 };
577
578 /// \brief The cofactor element indices for a 4x4 matrix row or column.
579 /// \param Element The index of the element to ignore.
580 template<int Element>
581 class Cofactor4
582 {
583 public:
584 typedef IntegralConstant<( Element <= 0 ) ? 1 : 0> x;
585 typedef IntegralConstant<( Element <= 1 ) ? 2 : 1> y;
586 typedef IntegralConstant<( Element <= 2 ) ? 3 : 2> z;
587 };
588
589 /// \brief Returns the determinant of \p self.
matrix4_determinant(const Matrix4 & self)590 inline double matrix4_determinant( const Matrix4& self ){
591 return self.xx() * Matrix4Cofactor< Cofactor4<0>, Cofactor4<0> >::apply( self )
592 - self.xy() * Matrix4Cofactor< Cofactor4<0>, Cofactor4<1> >::apply( self )
593 + self.xz() * Matrix4Cofactor< Cofactor4<0>, Cofactor4<2> >::apply( self )
594 - self.xw() * Matrix4Cofactor< Cofactor4<0>, Cofactor4<3> >::apply( self );
595 }
596
597 /// \brief Returns the inverse of \p self using the Adjoint method.
598 /// \todo Throw an exception if the determinant is zero.
matrix4_full_inverse(const Matrix4 & self)599 inline Matrix4 matrix4_full_inverse( const Matrix4& self ){
600 double determinant = 1.0 / matrix4_determinant( self );
601
602 return Matrix4(
603 static_cast<float>( Matrix4Cofactor< Cofactor4<0>, Cofactor4<0> >::apply( self ) * determinant ),
604 static_cast<float>( -Matrix4Cofactor< Cofactor4<1>, Cofactor4<0> >::apply( self ) * determinant ),
605 static_cast<float>( Matrix4Cofactor< Cofactor4<2>, Cofactor4<0> >::apply( self ) * determinant ),
606 static_cast<float>( -Matrix4Cofactor< Cofactor4<3>, Cofactor4<0> >::apply( self ) * determinant ),
607 static_cast<float>( -Matrix4Cofactor< Cofactor4<0>, Cofactor4<1> >::apply( self ) * determinant ),
608 static_cast<float>( Matrix4Cofactor< Cofactor4<1>, Cofactor4<1> >::apply( self ) * determinant ),
609 static_cast<float>( -Matrix4Cofactor< Cofactor4<2>, Cofactor4<1> >::apply( self ) * determinant ),
610 static_cast<float>( Matrix4Cofactor< Cofactor4<3>, Cofactor4<1> >::apply( self ) * determinant ),
611 static_cast<float>( Matrix4Cofactor< Cofactor4<0>, Cofactor4<2> >::apply( self ) * determinant ),
612 static_cast<float>( -Matrix4Cofactor< Cofactor4<1>, Cofactor4<2> >::apply( self ) * determinant ),
613 static_cast<float>( Matrix4Cofactor< Cofactor4<2>, Cofactor4<2> >::apply( self ) * determinant ),
614 static_cast<float>( -Matrix4Cofactor< Cofactor4<3>, Cofactor4<2> >::apply( self ) * determinant ),
615 static_cast<float>( -Matrix4Cofactor< Cofactor4<0>, Cofactor4<3> >::apply( self ) * determinant ),
616 static_cast<float>( Matrix4Cofactor< Cofactor4<1>, Cofactor4<3> >::apply( self ) * determinant ),
617 static_cast<float>( -Matrix4Cofactor< Cofactor4<2>, Cofactor4<3> >::apply( self ) * determinant ),
618 static_cast<float>( Matrix4Cofactor< Cofactor4<3>, Cofactor4<3> >::apply( self ) * determinant )
619 );
620 }
621
622 /// \brief Inverts \p self in-place using the Adjoint method.
matrix4_full_invert(Matrix4 & self)623 inline void matrix4_full_invert( Matrix4& self ){
624 self = matrix4_full_inverse( self );
625 }
626
627
628 /// \brief Constructs a pure-translation matrix from \p translation.
matrix4_translation_for_vec3(const Vector3 & translation)629 inline Matrix4 matrix4_translation_for_vec3( const Vector3& translation ){
630 return Matrix4(
631 1, 0, 0, 0,
632 0, 1, 0, 0,
633 0, 0, 1, 0,
634 translation[0], translation[1], translation[2], 1
635 );
636 }
637
638 /// \brief Returns the translation part of \p self.
matrix4_get_translation_vec3(const Matrix4 & self)639 inline Vector3 matrix4_get_translation_vec3( const Matrix4& self ){
640 return vector4_to_vector3( self.t() );
641 }
642
643 /// \brief Concatenates \p self with \p translation.
644 /// The concatenated \p translation occurs before \p self.
matrix4_translate_by_vec3(Matrix4 & self,const Vector3 & translation)645 inline void matrix4_translate_by_vec3( Matrix4& self, const Vector3& translation ){
646 matrix4_multiply_by_matrix4( self, matrix4_translation_for_vec3( translation ) );
647 }
648
649 /// \brief Returns \p self Concatenated with \p translation.
650 /// The concatenated translation occurs before \p self.
matrix4_translated_by_vec3(const Matrix4 & self,const Vector3 & translation)651 inline Matrix4 matrix4_translated_by_vec3( const Matrix4& self, const Vector3& translation ){
652 return matrix4_multiplied_by_matrix4( self, matrix4_translation_for_vec3( translation ) );
653 }
654
655
656 #include "math/pi.h"
657
658 /// \brief Returns \p angle modulated by the range [0, 360).
659 /// \p angle must be in the range [-360, 360).
angle_modulate_degrees_range(float angle)660 inline float angle_modulate_degrees_range( float angle ){
661 return static_cast<float>( float_mod_range( angle, 360.0 ) );
662 }
663
664 /// \brief Returns \p euler angles converted from radians to degrees.
euler_radians_to_degrees(const Vector3 & euler)665 inline Vector3 euler_radians_to_degrees( const Vector3& euler ){
666 return Vector3(
667 static_cast<float>( radians_to_degrees( euler.x() ) ),
668 static_cast<float>( radians_to_degrees( euler.y() ) ),
669 static_cast<float>( radians_to_degrees( euler.z() ) )
670 );
671 }
672
673 /// \brief Returns \p euler angles converted from degrees to radians.
euler_degrees_to_radians(const Vector3 & euler)674 inline Vector3 euler_degrees_to_radians( const Vector3& euler ){
675 return Vector3(
676 static_cast<float>( degrees_to_radians( euler.x() ) ),
677 static_cast<float>( degrees_to_radians( euler.y() ) ),
678 static_cast<float>( degrees_to_radians( euler.z() ) )
679 );
680 }
681
682
683
684 /// \brief Constructs a pure-rotation matrix about the x axis from sin \p s and cosine \p c of an angle.
matrix4_rotation_for_sincos_x(float s,float c)685 inline Matrix4 matrix4_rotation_for_sincos_x( float s, float c ){
686 return Matrix4(
687 1, 0, 0, 0,
688 0, c, s, 0,
689 0,-s, c, 0,
690 0, 0, 0, 1
691 );
692 }
693
694 /// \brief Constructs a pure-rotation matrix about the x axis from an angle in radians.
matrix4_rotation_for_x(double x)695 inline Matrix4 matrix4_rotation_for_x( double x ){
696 return matrix4_rotation_for_sincos_x( static_cast<float>( sin( x ) ), static_cast<float>( cos( x ) ) );
697 }
698
699 /// \brief Constructs a pure-rotation matrix about the x axis from an angle in degrees.
matrix4_rotation_for_x_degrees(float x)700 inline Matrix4 matrix4_rotation_for_x_degrees( float x ){
701 return matrix4_rotation_for_x( degrees_to_radians( x ) );
702 }
703
704 /// \brief Constructs a pure-rotation matrix about the y axis from sin \p s and cosine \p c of an angle.
matrix4_rotation_for_sincos_y(float s,float c)705 inline Matrix4 matrix4_rotation_for_sincos_y( float s, float c ){
706 return Matrix4(
707 c, 0,-s, 0,
708 0, 1, 0, 0,
709 s, 0, c, 0,
710 0, 0, 0, 1
711 );
712 }
713
714 /// \brief Constructs a pure-rotation matrix about the y axis from an angle in radians.
matrix4_rotation_for_y(double y)715 inline Matrix4 matrix4_rotation_for_y( double y ){
716 return matrix4_rotation_for_sincos_y( static_cast<float>( sin( y ) ), static_cast<float>( cos( y ) ) );
717 }
718
719 /// \brief Constructs a pure-rotation matrix about the y axis from an angle in degrees.
matrix4_rotation_for_y_degrees(float y)720 inline Matrix4 matrix4_rotation_for_y_degrees( float y ){
721 return matrix4_rotation_for_y( degrees_to_radians( y ) );
722 }
723
724 /// \brief Constructs a pure-rotation matrix about the z axis from sin \p s and cosine \p c of an angle.
matrix4_rotation_for_sincos_z(float s,float c)725 inline Matrix4 matrix4_rotation_for_sincos_z( float s, float c ){
726 return Matrix4(
727 c, s, 0, 0,
728 -s, c, 0, 0,
729 0, 0, 1, 0,
730 0, 0, 0, 1
731 );
732 }
733
734 /// \brief Constructs a pure-rotation matrix about the z axis from an angle in radians.
matrix4_rotation_for_z(double z)735 inline Matrix4 matrix4_rotation_for_z( double z ){
736 return matrix4_rotation_for_sincos_z( static_cast<float>( sin( z ) ), static_cast<float>( cos( z ) ) );
737 }
738
739 /// \brief Constructs a pure-rotation matrix about the z axis from an angle in degrees.
matrix4_rotation_for_z_degrees(float z)740 inline Matrix4 matrix4_rotation_for_z_degrees( float z ){
741 return matrix4_rotation_for_z( degrees_to_radians( z ) );
742 }
743
744 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (x, y, z).
745 /*! \verbatim
746 clockwise rotation around X, Y, Z, facing along axis
747 1 0 0 cy 0 -sy cz sz 0
748 0 cx sx 0 1 0 -sz cz 0
749 0 -sx cx sy 0 cy 0 0 1
750
751 rows of Z by cols of Y
752 cy*cz -sy*cz+sz -sy*sz+cz
753 -sz*cy -sz*sy+cz
754
755 .. or something like that..
756
757 final rotation is Z * Y * X
758 cy*cz -sx*-sy*cz+cx*sz cx*-sy*sz+sx*cz
759 -cy*sz sx*sy*sz+cx*cz -cx*-sy*sz+sx*cz
760 sy -sx*cy cx*cy
761
762 transposed
763 cy.cz + 0.sz + sy.0 cy.-sz + 0 .cz + sy.0 cy.0 + 0 .0 + sy.1 |
764 sx.sy.cz + cx.sz + -sx.cy.0 sx.sy.-sz + cx.cz + -sx.cy.0 sx.sy.0 + cx.0 + -sx.cy.1 |
765 -cx.sy.cz + sx.sz + cx.cy.0 -cx.sy.-sz + sx.cz + cx.cy.0 -cx.sy.0 + 0 .0 + cx.cy.1 |
766 \endverbatim */
matrix4_rotation_for_euler_xyz(const Vector3 & euler)767 inline Matrix4 matrix4_rotation_for_euler_xyz( const Vector3& euler ){
768 #if 1
769
770 double cx = cos( euler[0] );
771 double sx = sin( euler[0] );
772 double cy = cos( euler[1] );
773 double sy = sin( euler[1] );
774 double cz = cos( euler[2] );
775 double sz = sin( euler[2] );
776
777 return Matrix4(
778 static_cast<float>( cy * cz ),
779 static_cast<float>( cy * sz ),
780 static_cast<float>( -sy ),
781 0,
782 static_cast<float>( sx * sy * cz + cx * -sz ),
783 static_cast<float>( sx * sy * sz + cx * cz ),
784 static_cast<float>( sx * cy ),
785 0,
786 static_cast<float>( cx * sy * cz + sx * sz ),
787 static_cast<float>( cx * sy * sz + -sx * cz ),
788 static_cast<float>( cx * cy ),
789 0,
790 0,
791 0,
792 0,
793 1
794 );
795
796 #else
797
798 return matrix4_premultiply_by_matrix4(
799 matrix4_premultiply_by_matrix4(
800 matrix4_rotation_for_x( euler[0] ),
801 matrix4_rotation_for_y( euler[1] )
802 ),
803 matrix4_rotation_for_z( euler[2] )
804 );
805
806 #endif
807 }
808
809 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (x, y, z).
matrix4_rotation_for_euler_xyz_degrees(const Vector3 & euler)810 inline Matrix4 matrix4_rotation_for_euler_xyz_degrees( const Vector3& euler ){
811 return matrix4_rotation_for_euler_xyz( euler_degrees_to_radians( euler ) );
812 }
813
814 /// \brief Concatenates \p self with the rotation transform produced by \p euler angles (degrees) in the order (x, y, z).
815 /// The concatenated rotation occurs before \p self.
matrix4_rotate_by_euler_xyz_degrees(Matrix4 & self,const Vector3 & euler)816 inline void matrix4_rotate_by_euler_xyz_degrees( Matrix4& self, const Vector3& euler ){
817 matrix4_multiply_by_matrix4( self, matrix4_rotation_for_euler_xyz_degrees( euler ) );
818 }
819
820
821 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (y, z, x).
matrix4_rotation_for_euler_yzx(const Vector3 & euler)822 inline Matrix4 matrix4_rotation_for_euler_yzx( const Vector3& euler ){
823 return matrix4_premultiplied_by_matrix4(
824 matrix4_premultiplied_by_matrix4(
825 matrix4_rotation_for_y( euler[1] ),
826 matrix4_rotation_for_z( euler[2] )
827 ),
828 matrix4_rotation_for_x( euler[0] )
829 );
830 }
831
832 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (y, z, x).
matrix4_rotation_for_euler_yzx_degrees(const Vector3 & euler)833 inline Matrix4 matrix4_rotation_for_euler_yzx_degrees( const Vector3& euler ){
834 return matrix4_rotation_for_euler_yzx( euler_degrees_to_radians( euler ) );
835 }
836
837 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (x, z, y).
matrix4_rotation_for_euler_xzy(const Vector3 & euler)838 inline Matrix4 matrix4_rotation_for_euler_xzy( const Vector3& euler ){
839 return matrix4_premultiplied_by_matrix4(
840 matrix4_premultiplied_by_matrix4(
841 matrix4_rotation_for_x( euler[0] ),
842 matrix4_rotation_for_z( euler[2] )
843 ),
844 matrix4_rotation_for_y( euler[1] )
845 );
846 }
847
848 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (x, z, y).
matrix4_rotation_for_euler_xzy_degrees(const Vector3 & euler)849 inline Matrix4 matrix4_rotation_for_euler_xzy_degrees( const Vector3& euler ){
850 return matrix4_rotation_for_euler_xzy( euler_degrees_to_radians( euler ) );
851 }
852
853 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (y, x, z).
854 /*! \verbatim
855 | cy.cz + sx.sy.-sz + -cx.sy.0 0.cz + cx.-sz + sx.0 sy.cz + -sx.cy.-sz + cx.cy.0 |
856 | cy.sz + sx.sy.cz + -cx.sy.0 0.sz + cx.cz + sx.0 sy.sz + -sx.cy.cz + cx.cy.0 |
857 | cy.0 + sx.sy.0 + -cx.sy.1 0.0 + cx.0 + sx.1 sy.0 + -sx.cy.0 + cx.cy.1 |
858 \endverbatim */
matrix4_rotation_for_euler_yxz(const Vector3 & euler)859 inline Matrix4 matrix4_rotation_for_euler_yxz( const Vector3& euler ){
860 #if 1
861
862 double cx = cos( euler[0] );
863 double sx = sin( euler[0] );
864 double cy = cos( euler[1] );
865 double sy = sin( euler[1] );
866 double cz = cos( euler[2] );
867 double sz = sin( euler[2] );
868
869 return Matrix4(
870 static_cast<float>( cy * cz + sx * sy * -sz ),
871 static_cast<float>( cy * sz + sx * sy * cz ),
872 static_cast<float>( -cx * sy ),
873 0,
874 static_cast<float>( cx * -sz ),
875 static_cast<float>( cx * cz ),
876 static_cast<float>( sx ),
877 0,
878 static_cast<float>( sy * cz + -sx * cy * -sz ),
879 static_cast<float>( sy * sz + -sx * cy * cz ),
880 static_cast<float>( cx * cy ),
881 0,
882 0,
883 0,
884 0,
885 1
886 );
887
888 #else
889
890 return matrix4_premultiply_by_matrix4(
891 matrix4_premultiply_by_matrix4(
892 matrix4_rotation_for_y( euler[1] ),
893 matrix4_rotation_for_x( euler[0] )
894 ),
895 matrix4_rotation_for_z( euler[2] )
896 );
897
898 #endif
899 }
900
901 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (y, x, z).
matrix4_rotation_for_euler_yxz_degrees(const Vector3 & euler)902 inline Matrix4 matrix4_rotation_for_euler_yxz_degrees( const Vector3& euler ){
903 return matrix4_rotation_for_euler_yxz( euler_degrees_to_radians( euler ) );
904 }
905
906 /// \brief Returns \p self concatenated with the rotation transform produced by \p euler angles (degrees) in the order (y, x, z).
907 /// The concatenated rotation occurs before \p self.
matrix4_rotated_by_euler_yxz_degrees(const Matrix4 & self,const Vector3 & euler)908 inline Matrix4 matrix4_rotated_by_euler_yxz_degrees( const Matrix4& self, const Vector3& euler ){
909 return matrix4_multiplied_by_matrix4( self, matrix4_rotation_for_euler_yxz_degrees( euler ) );
910 }
911
912 /// \brief Concatenates \p self with the rotation transform produced by \p euler angles (degrees) in the order (y, x, z).
913 /// The concatenated rotation occurs before \p self.
matrix4_rotate_by_euler_yxz_degrees(Matrix4 & self,const Vector3 & euler)914 inline void matrix4_rotate_by_euler_yxz_degrees( Matrix4& self, const Vector3& euler ){
915 self = matrix4_rotated_by_euler_yxz_degrees( self, euler );
916 }
917
918 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (z, x, y).
matrix4_rotation_for_euler_zxy(const Vector3 & euler)919 inline Matrix4 matrix4_rotation_for_euler_zxy( const Vector3& euler ){
920 #if 1
921 return matrix4_premultiplied_by_matrix4(
922 matrix4_premultiplied_by_matrix4(
923 matrix4_rotation_for_z( euler[2] ),
924 matrix4_rotation_for_x( euler[0] )
925 ),
926 matrix4_rotation_for_y( euler[1] )
927 );
928 #else
929 double cx = cos( euler[0] );
930 double sx = sin( euler[0] );
931 double cy = cos( euler[1] );
932 double sy = sin( euler[1] );
933 double cz = cos( euler[2] );
934 double sz = sin( euler[2] );
935
936 return Matrix4(
937 static_cast<float>( cz * cy + sz * sx * sy ),
938 static_cast<float>( sz * cx ),
939 static_cast<float>( cz * -sy + sz * sx * cy ),
940 0,
941 static_cast<float>( -sz * cy + cz * sx * sy ),
942 static_cast<float>( cz * cx ),
943 static_cast<float>( -sz * -sy + cz * cx * cy ),
944 0,
945 static_cast<float>( cx * sy ),
946 static_cast<float>( -sx ),
947 static_cast<float>( cx * cy ),
948 0,
949 0,
950 0,
951 0,
952 1
953 );
954 #endif
955 }
956
957 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degres=es) in the order (z, x, y).
matrix4_rotation_for_euler_zxy_degrees(const Vector3 & euler)958 inline Matrix4 matrix4_rotation_for_euler_zxy_degrees( const Vector3& euler ){
959 return matrix4_rotation_for_euler_zxy( euler_degrees_to_radians( euler ) );
960 }
961
962 /// \brief Returns \p self concatenated with the rotation transform produced by \p euler angles (degrees) in the order (z, x, y).
963 /// The concatenated rotation occurs before \p self.
matrix4_rotated_by_euler_zxy_degrees(const Matrix4 & self,const Vector3 & euler)964 inline Matrix4 matrix4_rotated_by_euler_zxy_degrees( const Matrix4& self, const Vector3& euler ){
965 return matrix4_multiplied_by_matrix4( self, matrix4_rotation_for_euler_zxy_degrees( euler ) );
966 }
967
968 /// \brief Concatenates \p self with the rotation transform produced by \p euler angles (degrees) in the order (z, x, y).
969 /// The concatenated rotation occurs before \p self.
matrix4_rotate_by_euler_zxy_degrees(Matrix4 & self,const Vector3 & euler)970 inline void matrix4_rotate_by_euler_zxy_degrees( Matrix4& self, const Vector3& euler ){
971 self = matrix4_rotated_by_euler_zxy_degrees( self, euler );
972 }
973
974 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (z, y, x).
matrix4_rotation_for_euler_zyx(const Vector3 & euler)975 inline Matrix4 matrix4_rotation_for_euler_zyx( const Vector3& euler ){
976 #if 1
977
978 double cx = cos( euler[0] );
979 double sx = sin( euler[0] );
980 double cy = cos( euler[1] );
981 double sy = sin( euler[1] );
982 double cz = cos( euler[2] );
983 double sz = sin( euler[2] );
984
985 return Matrix4(
986 static_cast<float>( cy * cz ),
987 static_cast<float>( sx * sy * cz + cx * sz ),
988 static_cast<float>( cx * -sy * cz + sx * sz ),
989 0,
990 static_cast<float>( cy * -sz ),
991 static_cast<float>( sx * sy * -sz + cx * cz ),
992 static_cast<float>( cx * -sy * -sz + sx * cz ),
993 0,
994 static_cast<float>( sy ),
995 static_cast<float>( -sx * cy ),
996 static_cast<float>( cx * cy ),
997 0,
998 0,
999 0,
1000 0,
1001 1
1002 );
1003
1004 #else
1005
1006 return matrix4_premultiply_by_matrix4(
1007 matrix4_premultiply_by_matrix4(
1008 matrix4_rotation_for_z( euler[2] ),
1009 matrix4_rotation_for_y( euler[1] )
1010 ),
1011 matrix4_rotation_for_x( euler[0] )
1012 );
1013
1014 #endif
1015 }
1016
1017 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (z, y, x).
matrix4_rotation_for_euler_zyx_degrees(const Vector3 & euler)1018 inline Matrix4 matrix4_rotation_for_euler_zyx_degrees( const Vector3& euler ){
1019 return matrix4_rotation_for_euler_zyx( euler_degrees_to_radians( euler ) );
1020 }
1021
1022
1023 /// \brief Calculates and returns a set of euler angles that produce the rotation component of \p self when applied in the order (x, y, z).
1024 /// \p self must be affine and orthonormal (unscaled) to produce a meaningful result.
matrix4_get_rotation_euler_xyz(const Matrix4 & self)1025 inline Vector3 matrix4_get_rotation_euler_xyz( const Matrix4& self ){
1026 double a = asin( -self[2] );
1027 double ca = cos( a );
1028
1029 if ( fabs( ca ) > 0.005 ) { // Gimbal lock?
1030 return Vector3(
1031 static_cast<float>( atan2( self[6] / ca, self[10] / ca ) ),
1032 static_cast<float>( a ),
1033 static_cast<float>( atan2( self[1] / ca, self[0] / ca ) )
1034 );
1035 }
1036 else // Gimbal lock has occurred
1037 {
1038 return Vector3(
1039 static_cast<float>( atan2( -self[9], self[5] ) ),
1040 static_cast<float>( a ),
1041 0
1042 );
1043 }
1044 }
1045
1046 /// \brief \copydoc matrix4_get_rotation_euler_xyz(const Matrix4&)
matrix4_get_rotation_euler_xyz_degrees(const Matrix4 & self)1047 inline Vector3 matrix4_get_rotation_euler_xyz_degrees( const Matrix4& self ){
1048 return euler_radians_to_degrees( matrix4_get_rotation_euler_xyz( self ) );
1049 }
1050
1051 /// \brief Calculates and returns a set of euler angles that produce the rotation component of \p self when applied in the order (y, x, z).
1052 /// \p self must be affine and orthonormal (unscaled) to produce a meaningful result.
matrix4_get_rotation_euler_yxz(const Matrix4 & self)1053 inline Vector3 matrix4_get_rotation_euler_yxz( const Matrix4& self ){
1054 double a = asin( self[6] );
1055 double ca = cos( a );
1056
1057 if ( fabs( ca ) > 0.005 ) { // Gimbal lock?
1058 return Vector3(
1059 static_cast<float>( a ),
1060 static_cast<float>( atan2( -self[2] / ca, self[10] / ca ) ),
1061 static_cast<float>( atan2( -self[4] / ca, self[5] / ca ) )
1062 );
1063 }
1064 else // Gimbal lock has occurred
1065 {
1066 return Vector3(
1067 static_cast<float>( a ),
1068 static_cast<float>( atan2( self[8], self[0] ) ),
1069 0
1070 );
1071 }
1072 }
1073
1074 /// \brief \copydoc matrix4_get_rotation_euler_yxz(const Matrix4&)
matrix4_get_rotation_euler_yxz_degrees(const Matrix4 & self)1075 inline Vector3 matrix4_get_rotation_euler_yxz_degrees( const Matrix4& self ){
1076 return euler_radians_to_degrees( matrix4_get_rotation_euler_yxz( self ) );
1077 }
1078
1079 /// \brief Calculates and returns a set of euler angles that produce the rotation component of \p self when applied in the order (z, x, y).
1080 /// \p self must be affine and orthonormal (unscaled) to produce a meaningful result.
matrix4_get_rotation_euler_zxy(const Matrix4 & self)1081 inline Vector3 matrix4_get_rotation_euler_zxy( const Matrix4& self ){
1082 double a = asin( -self[9] );
1083 double ca = cos( a );
1084
1085 if ( fabs( ca ) > 0.005 ) { // Gimbal lock?
1086 return Vector3(
1087 static_cast<float>( a ),
1088 static_cast<float>( atan2( self[8] / ca, self[10] / ca ) ),
1089 static_cast<float>( atan2( self[1] / ca, self[5] / ca ) )
1090 );
1091 }
1092 else // Gimbal lock has occurred
1093 {
1094 return Vector3(
1095 static_cast<float>( a ),
1096 0,
1097 static_cast<float>( atan2( -self[4], self[0] ) )
1098 );
1099 }
1100 }
1101
1102 /// \brief \copydoc matrix4_get_rotation_euler_zxy(const Matrix4&)
matrix4_get_rotation_euler_zxy_degrees(const Matrix4 & self)1103 inline Vector3 matrix4_get_rotation_euler_zxy_degrees( const Matrix4& self ){
1104 return euler_radians_to_degrees( matrix4_get_rotation_euler_zxy( self ) );
1105 }
1106
1107 /// \brief Calculates and returns a set of euler angles that produce the rotation component of \p self when applied in the order (z, y, x).
1108 /// \p self must be affine and orthonormal (unscaled) to produce a meaningful result.
matrix4_get_rotation_euler_zyx(const Matrix4 & self)1109 inline Vector3 matrix4_get_rotation_euler_zyx( const Matrix4& self ){
1110 double a = asin( self[8] );
1111 double ca = cos( a );
1112
1113 if ( fabs( ca ) > 0.005 ) { // Gimbal lock?
1114 return Vector3(
1115 static_cast<float>( atan2( -self[9] / ca, self[10] / ca ) ),
1116 static_cast<float>( a ),
1117 static_cast<float>( atan2( -self[4] / ca, self[0] / ca ) )
1118 );
1119 }
1120 else // Gimbal lock has occurred
1121 {
1122 return Vector3(
1123 0,
1124 static_cast<float>( a ),
1125 static_cast<float>( atan2( self[1], self[5] ) )
1126 );
1127 }
1128 }
1129
1130 /// \brief \copydoc matrix4_get_rotation_euler_zyx(const Matrix4&)
matrix4_get_rotation_euler_zyx_degrees(const Matrix4 & self)1131 inline Vector3 matrix4_get_rotation_euler_zyx_degrees( const Matrix4& self ){
1132 return euler_radians_to_degrees( matrix4_get_rotation_euler_zyx( self ) );
1133 }
1134
1135
1136 /// \brief Rotate \p self by \p euler angles (degrees) applied in the order (x, y, z), using \p pivotpoint.
matrix4_pivoted_rotate_by_euler_xyz_degrees(Matrix4 & self,const Vector3 & euler,const Vector3 & pivotpoint)1137 inline void matrix4_pivoted_rotate_by_euler_xyz_degrees( Matrix4& self, const Vector3& euler, const Vector3& pivotpoint ){
1138 matrix4_translate_by_vec3( self, pivotpoint );
1139 matrix4_rotate_by_euler_xyz_degrees( self, euler );
1140 matrix4_translate_by_vec3( self, vector3_negated( pivotpoint ) );
1141 }
1142
1143
1144 /// \brief Constructs a pure-scale matrix from \p scale.
matrix4_scale_for_vec3(const Vector3 & scale)1145 inline Matrix4 matrix4_scale_for_vec3( const Vector3& scale ){
1146 return Matrix4(
1147 scale[0], 0, 0, 0,
1148 0, scale[1], 0, 0,
1149 0, 0, scale[2], 0,
1150 0, 0, 0, 1
1151 );
1152 }
1153
1154 /// \brief Calculates and returns the (x, y, z) scale values that produce the scale component of \p self.
1155 /// \p self must be affine and orthogonal to produce a meaningful result.
matrix4_get_scale_vec3(const Matrix4 & self)1156 inline Vector3 matrix4_get_scale_vec3( const Matrix4& self ){
1157 return Vector3(
1158 static_cast<float>( vector3_length( vector4_to_vector3( self.x() ) ) ),
1159 static_cast<float>( vector3_length( vector4_to_vector3( self.y() ) ) ),
1160 static_cast<float>( vector3_length( vector4_to_vector3( self.z() ) ) )
1161 );
1162 }
1163
1164 /// \brief Scales \p self by \p scale.
matrix4_scale_by_vec3(Matrix4 & self,const Vector3 & scale)1165 inline void matrix4_scale_by_vec3( Matrix4& self, const Vector3& scale ){
1166 matrix4_multiply_by_matrix4( self, matrix4_scale_for_vec3( scale ) );
1167 }
1168
1169 /// \brief Scales \p self by \p scale, using \p pivotpoint.
matrix4_pivoted_scale_by_vec3(Matrix4 & self,const Vector3 & scale,const Vector3 & pivotpoint)1170 inline void matrix4_pivoted_scale_by_vec3( Matrix4& self, const Vector3& scale, const Vector3& pivotpoint ){
1171 matrix4_translate_by_vec3( self, pivotpoint );
1172 matrix4_scale_by_vec3( self, scale );
1173 matrix4_translate_by_vec3( self, vector3_negated( pivotpoint ) );
1174 }
1175
1176
1177 /// \brief Transforms \p self by \p translation, \p euler and \p scale.
1178 /// The transforms are combined in the order: scale, rotate-z, rotate-y, rotate-x, translate.
matrix4_transform_by_euler_xyz_degrees(Matrix4 & self,const Vector3 & translation,const Vector3 & euler,const Vector3 & scale)1179 inline void matrix4_transform_by_euler_xyz_degrees( Matrix4& self, const Vector3& translation, const Vector3& euler, const Vector3& scale ){
1180 matrix4_translate_by_vec3( self, translation );
1181 matrix4_rotate_by_euler_xyz_degrees( self, euler );
1182 matrix4_scale_by_vec3( self, scale );
1183 }
1184
1185 /// \brief Transforms \p self by \p translation, \p euler and \p scale, using \p pivotpoint.
matrix4_pivoted_transform_by_euler_xyz_degrees(Matrix4 & self,const Vector3 & translation,const Vector3 & euler,const Vector3 & scale,const Vector3 & pivotpoint)1186 inline void matrix4_pivoted_transform_by_euler_xyz_degrees( Matrix4& self, const Vector3& translation, const Vector3& euler, const Vector3& scale, const Vector3& pivotpoint ){
1187 matrix4_translate_by_vec3( self, pivotpoint + translation );
1188 matrix4_rotate_by_euler_xyz_degrees( self, euler );
1189 matrix4_scale_by_vec3( self, scale );
1190 matrix4_translate_by_vec3( self, vector3_negated( pivotpoint ) );
1191 }
1192
1193
1194 #endif
1195