1 // -*- Mode: C++; tab-width: 2; -*- 2 // vi: set ts=2: 3 // 4 // $Id: matrix44.h,v 1.55.14.1 2007/03/25 21:23:45 oliver Exp $ 5 // 6 7 #ifndef BALL_MATHS_MATRIX44_H 8 #define BALL_MATHS_MATRIX44_H 9 10 #ifndef BALL_COMMON_EXCEPTION_H 11 # include <BALL/COMMON/exception.h> 12 #endif 13 14 #include <cmath> 15 #include <iostream> 16 #include <cstdlib> 17 #include <iomanip> 18 19 #ifndef BALL_MATHS_ANGLE_H 20 # include <BALL/MATHS/angle.h> 21 #endif 22 23 #ifndef BALL_MATHS_VECTOR3_H 24 # include <BALL/MATHS/vector3.h> 25 #endif 26 27 #ifndef BALL_MATHS_VECTOR4_H 28 # include <BALL/MATHS/vector4.h> 29 #endif 30 31 namespace BALL 32 { 33 /** \defgroup Matrix44 4x4 Matrix 34 Matrix representing transformations: class \link TMatrix4x4 TMatrix4x4 \endlink and class \link Matrix4x4 Matrix4x4 \endlink 35 36 \ingroup Primitives 37 */ 38 //@{ 39 /// Default Type 40 template <typename T> 41 class TMatrix4x4; 42 43 /** @name Storers 44 */ 45 //@{ 46 /** Input Operator. 47 Read sixteen values of type <tt>T</tt> from an input stream. 48 @param s the input stream 49 @param m the matrix to read 50 */ 51 template <typename T> 52 std::istream& operator >> (std::istream& s, TMatrix4x4<T>& m) 53 ; 54 55 /** Output Operator 56 Writes sixteen values of type <tt>T</tt> to an output stream. 57 @param s the output stream 58 @param m the matrix to write 59 */ 60 template <typename T> 61 std::ostream& operator << (std::ostream& s, const TMatrix4x4<T>& m) 62 ; 63 //@} 64 65 /** Generic 4x4 Matrix Class. 66 */ 67 template <typename T> 68 class TMatrix4x4 69 { 70 public: 71 72 BALL_CREATE(TMatrix4x4) 73 74 /** @name Constructors and Destructors 75 */ 76 //@{ 77 78 /** Default constructor. 79 This method creates a new TMatrix4x4 object. The components 80 are initialized to <tt>0</tt>. 81 */ 82 TMatrix4x4() 83 ; 84 85 /** Array constructor. 86 This constructor creates a TMatrix4x4 object from the first 87 sixteen elements pointed to by <tt>ptr</tt>. 88 @param ptr the array to construct from 89 @exception NullPointer if <tt>ptr == 0</tt> 90 */ 91 TMatrix4x4(const T* ptr); 92 93 /** Array constructor. 94 This constructor creates a TMatrix4x4 object from the 95 sixteen elements in the array assigned by <tt>ptr</tt>. 96 @param ptr the array to construct from 97 @exception NullPointer if <tt>ptr == 0</tt> 98 */ 99 TMatrix4x4(const T ptr[4][4]); 100 101 /** Copy constructor. 102 Create a new TMatrix4x4 object from another. 103 @param TMatrix4x4 the TMatrix4x4 object to be copied 104 @param bool ignored (just for interface consistency) 105 */ 106 TMatrix4x4(const TMatrix4x4& m) 107 ; 108 109 /** Detailed constructor. 110 Create a new TMatrix4x4 object from four TVector4. 111 @param col1 assigned to the first column 112 @param col2 assigned to the second column 113 @param col3 assigned to the third column 114 @param col4 assigned to the fourth column 115 116 */ 117 TMatrix4x4 118 (const TVector4<T>& col1, const TVector4<T>& col2, 119 const TVector4<T>& col3, const TVector4<T>& col4) 120 ; 121 122 /** Detailed constructor. 123 Create a new TMatrix4x4 object from sixteen <tt>T</tt> values. 124 @param m11 - m44 assigned to the components 125 */ 126 TMatrix4x4 127 (const T& m11, const T& m12, const T& m13, const T& m14, 128 const T& m21, const T& m22, const T& m23, const T& m24, 129 const T& m31, const T& m32, const T& m33, const T& m34, 130 const T& m41, const T& m42, const T& m43, const T& m44) 131 ; 132 133 /** Destructor. 134 Destructs the TMatrix4x4 object. As there are no dynamic 135 data structures, nothing happens. 136 */ ~TMatrix4x4()137 virtual ~TMatrix4x4() 138 139 { 140 } 141 142 /** Clear method. 143 The values are set to 0. 144 */ 145 virtual void clear() 146 ; 147 148 //@} 149 /** @name Assignment 150 */ 151 //@{ 152 153 /** Assign from array-ptr. 154 Assign from the first sixteen elements pointed to by <tt>ptr</tt>. 155 @param ptr the array to construct from 156 @exception NullPointer if <tt>ptr == 0</tt> 157 */ 158 void set(const T* ptr); 159 160 /** Assign from the first sixteen elements. 161 pointed to by the array assigned by <tt>ptr</tt>. 162 @param ptr the array to construct from 163 @exception NullPointer if <tt>ptr == 0</tt> 164 */ 165 void set(const T ptr[4][4]); 166 167 /** Assign from another instance. 168 @param TMatrix4x4 the TMatrix4x4 object to assign from 169 */ 170 void set(const TMatrix4x4& m); 171 172 /** Assign from four TVector4. 173 @param col1 assigned to the first column 174 @param col2 assigned to the second column 175 @param col3 assigned to the third column 176 @param col4 assigned to the fourth column 177 */ 178 void set 179 (const TVector4<T>& col1, const TVector4<T>& col2, 180 const TVector4<T>& col3, const TVector4<T>& col4); 181 182 /** Assign from sixteen values of type T. 183 @param m11 - m44 assigned to the components 184 */ 185 void set 186 (const T& m11, const T& m12, const T& m13, const T& m14, 187 const T& m21, const T& m22, const T& m23, const T& m24, 188 const T& m31, const T& m32, const T& m33, const T& m34, 189 const T& m41, const T& m42, const T& m43, const T& m44); 190 191 /** Assignment operator. 192 Assign the components from the first 16 values assigned by <tt>ptr</tt>. 193 @param ptr the array to construct from 194 @exception NullPointer if <tt>ptr == 0</tt> 195 **/ 196 TMatrix4x4& operator = (const T* ptr); 197 198 /** Assignment operator. 199 Assign the components from the first 16 values assigned by <tt>ptr</tt>. 200 @param ptr the array to construct from 201 @exception NullPointer if <tt>ptr == 0</tt> 202 **/ 203 TMatrix4x4& operator = (const T ptr[4][4]); 204 205 /** Assignment operator. 206 Assign the components from another instance of TMatrix4x4. 207 @param TMatrix4x4 the TMatrix4x4 to assign from 208 **/ 209 TMatrix4x4& operator = (const TMatrix4x4& m); 210 211 /** Assign to an array. 212 Assigns the components to a pointer of an array of sixteen values of type <tt>T</tt>. 213 @param ptr the pointer to assign to 214 @exception NullPointer if <tt>ptr == 0</tt> 215 */ 216 void get(T* ptr) const; 217 218 /** Assign to an array. 219 Assigns the components to an array of sixteen values of type <tt>T</tt>. 220 @param ptr the array to assign to 221 @exception NullPointer if <tt>ptr == 0</tt> 222 */ 223 void get(T ptr[4][4]) const; 224 225 /** Assign to another instance. 226 Assigns the components to another TMatrix4x4. 227 @param TMatrix4x4 the TMatrix4x4 to be assigned to 228 */ 229 void get(TMatrix4x4& m) const; 230 231 /** Assign to four variables of type <b> TVector4 </b>. 232 @param col1 the TVector4 to obtain the values of the first column 233 @param col2 the TVector4 to obtain the values of the second column 234 @param col3 the TVector4 to obtain the values of the third column 235 @param col4 the TVector4 to obtain the values of the fourth column 236 */ 237 void get 238 (TVector4<T>& col1, TVector4<T>& col2, 239 TVector4<T>& col3, TVector4<T>& col4) const; 240 241 /** Assign to sixteen variables of type <tt>T</tt>. 242 @param m11 - m44 the variables to assign to 243 */ 244 void get 245 (T& m11, T& m12, T& m13, T& m14, 246 T& m21, T& m22, T& m23, T& m24, 247 T& m31, T& m32, T& m33, T& m34, 248 T& m41, T& m42, T& m43, T& m44) const; 249 250 /** Swap the contents of two instances of TMatrix4x4. 251 @param TMatrix4x4 the TMatrix4x4 to swap contents with 252 */ 253 void swap(TMatrix4x4& m); 254 255 //@} 256 /** @name Accessors 257 */ 258 //@{ 259 260 /** Compute the trace. 261 Get the sum of the diagonal elements (m11 + m22 + m33 + m44). 262 @return T the trace 263 */ 264 T getTrace() const; 265 266 /** Create a zero matrix. 267 A new matrix object is created and all elements set to 0. 268 */ 269 static const TMatrix4x4& getZero(); 270 271 /** Create an identity matrix. 272 A new matrix object is created and all elements but the diagonal are 273 set to zero. The diagonal elements are set to 1. 274 */ 275 static const TMatrix4x4& getIdentity(); 276 277 /** Set to an identity matrix. 278 m11, m22, m33, m44 = 1; 279 the other cells have the value 0; 280 */ 281 void setIdentity(); 282 283 /** Set the diagonal elements to the given value. 284 All other elements are set to 0. 285 @param T the value to fill with (default: 1) 286 */ 287 void set(const T& t = (T)1); 288 289 /** Mirror the Matrix at the diagonal. 290 All values are swaped by the mirrored value. 291 (I.e. m12 <=> m21 , m13 <=> m31 , ...) 292 */ 293 void transpose(); 294 295 /** Get a row of the matrix. 296 @param row the number of the row (0-3) 297 @return TVector4 the row 298 @exception IndexOverflow if <tt>row > 3</tt> 299 */ 300 TVector4<T> getRow(Position row) const; 301 302 /** Get a column of the matrix. 303 @param col the number of the column (0-3) 304 @return TVector4 the column 305 @exception IndexOverflow if <tt>col > 3</tt> 306 */ 307 TVector4<T> getColumn(Position col) const; 308 309 /** Set a row of the matrix. 310 @param row the number of the row (0-3) 311 @param row_value the new value of the row 312 @exception IndexOverflow if <tt>row > 3</tt> 313 */ 314 void setRow(Position row, const TVector4<T>& row_value); 315 316 /** Set a column of the matrix. 317 @param col the number of the column (0-3) 318 @param col_value the new value of the col 319 @exception IndexOverflow if <tt>col > 3</tt> 320 */ 321 void setColumn(Position col, const TVector4<T>& col_value); 322 323 /** Test whether two matrices are equal. 324 Two matrices are considered equal, if 325 \link Maths::isEqual Maths::isEqual \endlink returns <b>true</b> 326 for each pair of corresponding elements. 327 @param m the matrix to compare with 328 @return bool, <b>true</b> if all components are equal, <b>false</b> otherwise 329 */ 330 bool isEqual(const TMatrix4x4& m) const; 331 332 /** Get the diagonal of the matrix. 333 @return TVector4 the diagonal 334 */ 335 TVector4<T> getDiagonal() const; 336 337 /** Access operator of a cell. 338 @param row the number of the row (0-3) 339 @param col the number of the column (0-3) 340 @return T& a reference to the cell 341 @exception IndexOverflow if <tt>col >3 || row > 3</tt> 342 */ 343 T& operator () (Position row, Position col); 344 345 /** Constant access operator of a cell. 346 @param row the number of the row (0-3) 347 @param col the number of the column (0-3) 348 @return T& a const reference to the cell 349 @exception IndexOverflow if <tt>col ||row > 3</tt> 350 */ 351 const T& operator () (Position row, Position col) const; 352 353 /** Constant random access operator. 354 Access single elements of the matrix. <tt>index</tt> may assume 355 values in the range of 0 - 15. The elements of the matrix 356 are returned rows first, i.e., in the following order: <tt>m11</tt>, <tt>m12</tt>, <tt>m13</tt>... 357 @exception IndexOverflow if <tt>position > 15</tt> 358 */ 359 const T& operator [] (Position position) const; 360 361 /** Mutable random access operator. 362 @see operator[] 363 @exception IndexOverflow if <tt>position > 15</tt> 364 */ 365 T& operator [] (Position position); 366 367 /** Positive sign. 368 */ 369 TMatrix4x4 operator + () const; 370 371 /** Negative sign. 372 */ 373 TMatrix4x4 operator - () const; 374 375 /** Addition operator. 376 Adds another matrix to this matrix and return the result. 377 @param m the matrix to add 378 @return TMatrix4x4 the result 379 */ 380 TMatrix4x4 operator + (const TMatrix4x4& m) const; 381 382 /** Addition operator. 383 Adds another matrix to this matrix. 384 @param m the matrix to add 385 @return TMatrix4x4&, {\em *this} 386 */ 387 TMatrix4x4& operator += (const TMatrix4x4& m); 388 389 /** Subtraction operator. 390 Subtract another matrix from this matrix and return the result 391 @param m the matrix to subtract 392 @return TMatrix4x4 the result 393 */ 394 TMatrix4x4 operator - (const TMatrix4x4& m) const; 395 396 /** Subtraction operator. 397 Subtract another matrix from this matrix. 398 @param m the matrix to subtract 399 @return TMatrix4x4&, {\em *this} 400 */ 401 TMatrix4x4& operator -= (const TMatrix4x4& m); 402 403 /** Multiply by a scalar. 404 Operator for multiplying every cell value with a scalar value. 405 @return TMatrix4x4 the result 406 */ 407 TMatrix4x4 operator * (const T& scalar) const; 408 409 /** Multiply by a scalar. 410 Operator for multiplying every cell value with a scalar value. 411 @return TMatrix4x4&, {\em *this} 412 */ 413 TMatrix4x4& operator *= (const T& scalar); 414 415 /** Divide by a scalar. 416 Operator for dividing every cell value by a scalar value. 417 @return TMatrix4x4 the result 418 @exception DivisionByZero if <tt>scalar == 0</tt> 419 */ 420 TMatrix4x4 operator / (const T& scalar) const; 421 422 /** Divide by a scalar. 423 Operator for dividing every cell value by a scalar value. 424 @return TMatrix4x4&, {\em *this} 425 @exception DivisionByZero if <tt>scalar == 0</tt> 426 */ 427 TMatrix4x4& operator /= (const T& scalar); 428 429 /** Multiply two matrices. 430 @return TMatrix4x4 the result 431 */ 432 TMatrix4x4 operator * (const TMatrix4x4& m) const; 433 434 /** Multiply two matrices 435 @return TMatrix4x4&, {\em *this} 436 */ 437 TMatrix4x4& operator *= (const TMatrix4x4& m); 438 439 /** Multiplication by an instance of type <b> TVector4 </b>. 440 @return TMatrix4x4&, {\em *this} 441 */ 442 TVector4<T> operator * (const TVector4<T>& vector) const; 443 444 /** Invert the matrix. 445 Tests if the matrix can be inverted. 446 If possible, the result will be inverted and the result returned in <b> inverse </b>. 447 @param inverse is assigned the inverse matrix 448 @return bool true if the inverse matrix could be calculated, otherwise false. 449 */ 450 bool invert(TMatrix4x4& inverse) const; 451 452 /** Invert the matrix. 453 Tests if the matrix can be inverted. 454 If this is possible, the result is stored in the matrix. 455 @return bool true if the inverse matrix could be calculated, otherwise false. 456 */ 457 bool invert(); 458 459 /** Compute the determinant. 460 @return T the determinant. 461 */ 462 T getDeterminant() const; 463 464 /** Translate the matrix. 465 @param x the x-component of the translation 466 @param y the y-component of the translation 467 @param z the z-component of the translation 468 */ 469 void translate(const T &x, const T &y, const T &z); 470 471 /** Translate the matrix. 472 @param v the vector to translate with 473 */ 474 void translate(const TVector3<T>& v); 475 476 /** Set the matrix to a translation matrix. 477 @param x the x-component of the translation 478 @param y the y-component of the translation 479 @param z the z-component of the translation 480 */ 481 void setTranslation(const T& x, const T& y, const T& z); 482 483 /** Set the matrix to a translation matrix. 484 @param v the vector to translate with 485 */ 486 void setTranslation(const TVector3<T>& v); 487 488 /** Scale the matrix. 489 @param x_scale the x scale factor 490 @param y_scale the y scale factor 491 @param z_scale the z scale factor 492 */ 493 void scale(const T& x_scale, const T& y_scale, const T& z_scale); 494 495 /** Scale the matrix. 496 @param scale the scale factor 497 */ 498 void scale(const T& scale); 499 500 /** Scale the matrix. 501 @param v the vector with the scale factor 502 */ 503 void scale(const TVector3<T>& v); 504 505 /** Set the matrix to a scalation matrix. 506 @param x_scale the x scale factor 507 @param y_scale the y scale factor 508 @param z_scale the z scale factor 509 */ 510 void setScale(const T& x_scale, const T& y_scale, const T& z_scale); 511 512 /** Set the matrix to a scalation matrix. 513 @param scale the scale factor 514 */ 515 void setScale(const T& scale); 516 517 /** Set the matrix to a scalation matrix. 518 @param v the vector with the scale factor 519 */ 520 void setScale(const TVector3<T>& v); 521 522 /** Rotate the matrix around the x axis. 523 @param phi the rotation angle 524 */ 525 void rotateX(const TAngle<T>& phi); 526 527 /** Set the matrix to a x rotation matrix. 528 @param phi the rotation angle 529 */ 530 void setRotationX(const TAngle<T>& phi); 531 532 /** Rotate the matrix around the y axis. 533 @param phi the rotation angle 534 */ 535 void rotateY(const TAngle<T>& phi); 536 537 /** Set the matrix to a y rotation matrix. 538 @param phi the rotation angle 539 */ 540 void setRotationY(const TAngle<T>& phi); 541 542 /** Rotate the matrix around the z axis. 543 @param phi the rotation angle 544 */ 545 void rotateZ(const TAngle<T>& phi); 546 547 /** Set the matrix to a z rotation matrix. 548 @param phi the rotation angle 549 */ 550 void setRotationZ(const TAngle<T>& phi); 551 552 /** Rotate the matrix around a given axis. 553 @param phi the rotation angle 554 @param axis_x the x component of the axis 555 @param axis_y the y component of the axis 556 @param axis_z the z component of the axis 557 */ 558 void rotate(const TAngle<T>& phi, const T& axis_x, const T& axis_y, const T& axis_z); 559 560 /** Rotate the matrix around a given axis. 561 @param phi the rotation angle 562 @param axis the axis vector 563 */ 564 void rotate(const TAngle<T>& phi, const TVector3<T>& axis); 565 566 /** Rotate the matrix around a given axis. 567 @param phi the rotation angle 568 @param axis the axis vector, the fourth component of the vector is ignored 569 */ 570 void rotate(const TAngle<T>& phi, const TVector4<T>& axis); 571 572 /** Set the matrix to a rotation matrix. 573 @param phi the rotation angle 574 @param axis_x the x component of the axis 575 @param axis_y the y component of the axis 576 @param axis_z the z component of the axis 577 */ 578 void setRotation(const TAngle<T>& phi, const T& axis_x, const T& axis_y, const T& axis_z); 579 580 /** Set the matrix to a rotation matrix. 581 @param phi the rotation angle 582 @param axis the axis vector 583 */ 584 void setRotation(const TAngle<T>& phi, const TVector3<T>& axis); 585 586 /** Set the matrix to a rotation matrix. 587 @param phi the rotation angle 588 @param axis the axis vector, the fourth component of the vector is ignored 589 */ 590 void setRotation(const TAngle<T>& phi, const TVector4<T>& axis); 591 //@} 592 593 /** @name Predicates 594 */ 595 //@{ 596 597 /** Equality operator. 598 Instead of this operator isEqual should be used. 599 \link isEqual isEqual \endlink 600 @return bool, <b>true</b> if all components are equal, <b>false</b> otherwise 601 */ 602 bool operator == (const TMatrix4x4& m) const; 603 604 /** Inequality operator. 605 Instead of this operator isEqual should be used. 606 \link isEqual isEqual \endlink 607 @return bool, <b>true</b> if the two TMatrix4x4 differ in at least one component, <b>false</b> otherwise 608 */ 609 bool operator != (const TMatrix4x4& m) const; 610 611 /** Test whether this matrix is an identity matrix. 612 (I.e. m11, m22, m33, m44 = 1 and the other cells have the value 0) 613 @return bool, <b>true</b> if identity matrix, <b>false</b> otherwise 614 */ 615 bool isIdentity() const; 616 617 /** Test whether this matrix is regular. 618 @return bool, <b>true</b> if (Determinant != 0), <b>false</b> otherwise 619 */ 620 bool isRegular() const; 621 622 /** Test whether this matrix is singular. 623 @return bool, <b>true</b> if (Determinant == 0), <b>false</b> otherwise 624 */ 625 bool isSingular() const; 626 627 /** Test whether this matrix is symmetric. 628 (m12 = m21, m31 = m13, ...) 629 @return bool, <b>true</b> if symmatric, <b>false</b> otherwise 630 */ 631 bool isSymmetric() const; 632 633 /** Test whether the lower triangular is zero. 634 @return bool, <b>true</b> if (m12 = m13 = m14 = m23 = m24 = m34 = 0), <b>false</b> otherwise 635 */ 636 bool isLowerTriangular() const; 637 638 /** Test whether the upper triangular is zero. 639 @return bool, <b>true</b> if (m21 = m31 = m32 = m41 = m42 = m43 = 0), <b>false</b> otherwise 640 */ 641 bool isUpperTriangular() const; 642 643 /** Test whether all cells but the diagonal are zero. 644 @return bool, <b>true</b> or <b>false</b> 645 */ 646 bool isDiagonal() const; 647 //@} 648 649 /** @name Debugging and Diagnostics 650 */ 651 //@{ 652 653 /** Test whether instance is valid. 654 Always returns true. 655 @return bool <b>true</b> 656 */ 657 bool isValid() const; 658 659 /** Internal state dump. 660 Dump the current internal state of {\em *this} to 661 the output ostream <b> s </b> with dumping depth <b> depth </b>. 662 @param s - output stream where to output the internal state of {\em *this} 663 @param depth - the dumping depth 664 */ 665 void dump(std::ostream& s = std::cout, Size depth = 0) const; 666 //@} 667 668 /** @name Attributes 669 */ 670 //@{ 671 672 /// 1st cell in the 1st row 673 T m11; 674 675 /// 2nd cell in the 1st row 676 T m12; 677 678 /// 3rd cell in the 1st row 679 T m13; 680 681 /// 4th cell in the 1st row 682 T m14; 683 684 /// 1st cell in the 2nd row 685 T m21; 686 687 /// 2nd cell in the 2nd row 688 T m22; 689 690 /// 3rd cell in the 2nd row 691 T m23; 692 693 /// 4th cell in the 2nd row 694 T m24; 695 696 /// 1st cell in the 3rd row 697 T m31; 698 699 /// 2nd cell in the 3rd row 700 T m32; 701 702 /// 3rd cell in the 3rd row 703 T m33; 704 705 /// 4th cell in the 3rd row 706 T m34; 707 708 /// 1st cell in the 4th row 709 T m41; 710 711 /// 2nd cell in the 4th row 712 T m42; 713 714 /// 3rd cell in the 4th row 715 T m43; 716 717 /// 4th cell in the 4th row 718 T m44; 719 //@} 720 721 private: 722 initializeComponentPointers_()723 void initializeComponentPointers_() 724 { 725 T **ptr = (T **)comp_ptr_; 726 727 *ptr++ = &m11; *ptr++ = &m12; *ptr++ = &m13; *ptr++ = &m14; 728 *ptr++ = &m21; *ptr++ = &m22; *ptr++ = &m23; *ptr++ = &m24; 729 *ptr++ = &m31; *ptr++ = &m32; *ptr++ = &m33; *ptr++ = &m34; 730 *ptr++ = &m41; *ptr++ = &m42; *ptr++ = &m43; *ptr = &m44; 731 } 732 733 // pointers to the components of the matrix 734 T* comp_ptr_[16]; 735 }; 736 //@} 737 738 template <typename T> TMatrix4x4()739 TMatrix4x4<T>::TMatrix4x4() 740 : m11(0), m12(0), m13(0), m14(0), 741 m21(0), m22(0), m23(0), m24(0), 742 m31(0), m32(0), m33(0), m34(0), 743 m41(0), m42(0), m43(0), m44(0) 744 { 745 initializeComponentPointers_(); 746 } 747 748 template <typename T> TMatrix4x4(const T * ptr)749 TMatrix4x4<T>::TMatrix4x4( const T* ptr) 750 { 751 if (ptr == 0) 752 { 753 throw Exception::NullPointer(__FILE__, __LINE__); 754 } 755 756 m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++; 757 m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++; 758 m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++; 759 m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr; 760 761 initializeComponentPointers_(); 762 } 763 764 template <typename T> TMatrix4x4(const T array_ptr[4][4])765 TMatrix4x4<T>::TMatrix4x4(const T array_ptr[4][4]) 766 { 767 if (array_ptr == 0) 768 { 769 throw Exception::NullPointer(__FILE__, __LINE__); 770 } 771 772 const T *ptr = *array_ptr; 773 774 m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++; 775 m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++; 776 m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++; 777 m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr; 778 779 initializeComponentPointers_(); 780 } 781 782 template <typename T> TMatrix4x4(const TMatrix4x4<T> & m)783 TMatrix4x4<T>::TMatrix4x4(const TMatrix4x4<T>& m) 784 : m11(m.m11), m12(m.m12), m13(m.m13), m14(m.m14), 785 m21(m.m21), m22(m.m22), m23(m.m23), m24(m.m24), 786 m31(m.m31), m32(m.m32), m33(m.m33), m34(m.m34), 787 m41(m.m41), m42(m.m42), m43(m.m43), m44(m.m44) 788 { 789 initializeComponentPointers_(); 790 } 791 792 793 template <typename T> TMatrix4x4(const TVector4<T> & col1,const TVector4<T> & col2,const TVector4<T> & col3,const TVector4<T> & col4)794 TMatrix4x4<T>::TMatrix4x4 795 (const TVector4<T>& col1, const TVector4<T>& col2, 796 const TVector4<T>& col3,const TVector4<T>& col4) 797 : m11(col1.x), m12(col1.y), m13(col1.z), m14(col1.h), 798 m21(col2.x), m22(col2.y), m23(col2.z), m24(col2.h), 799 m31(col3.x), m32(col3.y), m33(col3.z), m34(col3.h), 800 m41(col4.x), m42(col4.y), m43(col4.z), m44(col4.h) 801 { 802 initializeComponentPointers_(); 803 } 804 805 template <typename T> TMatrix4x4(const T & m11,const T & m12,const T & m13,const T & m14,const T & m21,const T & m22,const T & m23,const T & m24,const T & m31,const T & m32,const T & m33,const T & m34,const T & m41,const T & m42,const T & m43,const T & m44)806 TMatrix4x4<T>::TMatrix4x4 807 (const T& m11, const T& m12, const T& m13, const T& m14, 808 const T& m21, const T& m22, const T& m23, const T& m24, 809 const T& m31, const T& m32, const T& m33, const T& m34, 810 const T& m41, const T& m42, const T& m43, const T& m44) 811 : m11(m11), m12(m12), m13(m13), m14(m14), 812 m21(m21), m22(m22), m23(m23), m24(m24), 813 m31(m31), m32(m32), m33(m33), m34(m34), 814 m41(m41), m42(m42), m43(m43), m44(m44) 815 { 816 initializeComponentPointers_(); 817 } 818 819 template <typename T> clear()820 void TMatrix4x4<T>::clear() 821 { 822 set((T)0); 823 } 824 825 template <typename T> set(const T * ptr)826 void TMatrix4x4<T>::set(const T* ptr) 827 { 828 if (ptr == 0) 829 { 830 throw Exception::NullPointer(__FILE__, __LINE__); 831 } 832 833 m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++; 834 m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++; 835 m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++; 836 m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr; 837 } 838 839 template <typename T> set(const T array_ptr[4][4])840 void TMatrix4x4<T>::set(const T array_ptr[4][4]) 841 { 842 if (array_ptr == 0) 843 { 844 throw Exception::NullPointer(__FILE__, __LINE__); 845 } 846 847 const T *ptr = *array_ptr; 848 849 m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++; 850 m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++; 851 m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++; 852 m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr; 853 } 854 855 template <typename T> set(const TMatrix4x4<T> & m)856 void TMatrix4x4<T>::set(const TMatrix4x4<T>& m) 857 { 858 m11 = m.m11; m12 = m.m12; m13 = m.m13; m14 = m.m14; 859 m21 = m.m21; m22 = m.m22; m23 = m.m23; m24 = m.m24; 860 m31 = m.m31; m32 = m.m32; m33 = m.m33; m34 = m.m34; 861 m41 = m.m41; m42 = m.m42; m43 = m.m43; m44 = m.m44; 862 } 863 864 template <typename T> set(const TVector4<T> & col1,const TVector4<T> & col2,const TVector4<T> & col3,const TVector4<T> & col4)865 void TMatrix4x4<T>::set 866 (const TVector4<T>& col1, const TVector4<T>& col2, 867 const TVector4<T>& col3, const TVector4<T>& col4) 868 { 869 m11 = col1.x; m12 = col1.y; m13 = col1.z; m14 = col1.h; 870 m21 = col2.x; m22 = col2.y; m23 = col2.z; m24 = col2.h; 871 m31 = col3.x; m32 = col3.y; m33 = col3.z; m34 = col3.h; 872 m41 = col4.x; m42 = col4.y; m43 = col4.z; m44 = col4.h; 873 } 874 875 template <typename T> set(const T & c11,const T & c12,const T & c13,const T & c14,const T & c21,const T & c22,const T & c23,const T & c24,const T & c31,const T & c32,const T & c33,const T & c34,const T & c41,const T & c42,const T & c43,const T & c44)876 void TMatrix4x4<T>::set 877 (const T& c11, const T& c12, const T& c13, const T& c14, 878 const T& c21, const T& c22, const T& c23, const T& c24, 879 const T& c31, const T& c32, const T& c33, const T& c34, 880 const T& c41, const T& c42, const T& c43, const T& c44) 881 { 882 m11 = c11; m12 = c12; m13 = c13; m14 = c14; 883 m21 = c21; m22 = c22; m23 = c23; m24 = c24; 884 m31 = c31; m32 = c32; m33 = c33; m34 = c34; 885 m41 = c41; m42 = c42; m43 = c43; m44 = c44; 886 } 887 888 template <typename T> 889 BALL_INLINE 890 TMatrix4x4<T>& TMatrix4x4<T>::operator = (const T* ptr) 891 { 892 set(ptr); 893 return *this; 894 } 895 896 template <typename T> 897 BALL_INLINE 898 TMatrix4x4<T>& TMatrix4x4<T>::operator = (const T array_ptr[4][4]) 899 { 900 set(array_ptr); 901 return *this; 902 } 903 904 template <typename T> 905 BALL_INLINE 906 TMatrix4x4<T>& TMatrix4x4<T>::operator = (const TMatrix4x4<T>& m) 907 { 908 set(m); 909 return *this; 910 } 911 912 template <typename T> get(T * ptr)913 void TMatrix4x4<T>::get(T* ptr) const 914 { 915 if (ptr == 0) 916 { 917 throw Exception::NullPointer(__FILE__, __LINE__); 918 } 919 920 *ptr++ = m11; *ptr++ = m12; *ptr++ = m13; *ptr++ = m14; 921 *ptr++ = m21; *ptr++ = m22; *ptr++ = m23; *ptr++ = m24; 922 *ptr++ = m31; *ptr++ = m32; *ptr++ = m33; *ptr++ = m34; 923 *ptr++ = m41; *ptr++ = m42; *ptr++ = m43; *ptr = m44; 924 } 925 926 template <typename T> get(T array_ptr[4][4])927 void TMatrix4x4<T>::get(T array_ptr[4][4]) const 928 { 929 if (array_ptr == 0) 930 { 931 throw Exception::NullPointer(__FILE__, __LINE__); 932 } 933 934 T *ptr = *array_ptr; 935 936 *ptr++ = m11; *ptr++ = m12; *ptr++ = m13; *ptr++ = m14; 937 *ptr++ = m21; *ptr++ = m22; *ptr++ = m23; *ptr++ = m24; 938 *ptr++ = m31; *ptr++ = m32; *ptr++ = m33; *ptr++ = m34; 939 *ptr++ = m41; *ptr++ = m42; *ptr++ = m43; *ptr = m44; 940 } 941 942 template <typename T> get(TMatrix4x4<T> & m)943 void TMatrix4x4<T>::get(TMatrix4x4<T>& m) const 944 { 945 m.set(*this); 946 } 947 948 template <typename T> get(TVector4<T> & col1,TVector4<T> & col2,TVector4<T> & col3,TVector4<T> & col4)949 void TMatrix4x4<T>::get 950 (TVector4<T>& col1, TVector4<T>& col2, 951 TVector4<T>& col3, TVector4<T>& col4) const 952 { 953 col1.x = m11; col1.y = m12; col1.z = m13; col1.h = m14; 954 col2.x = m21; col2.y = m22; col2.z = m23; col2.h = m24; 955 col3.x = m31; col3.y = m32; col3.z = m33; col3.h = m34; 956 col4.x = m41; col4.y = m42; col4.z = m43; col4.h = m44; 957 } 958 959 template <typename T> get(T & c11,T & c12,T & c13,T & c14,T & c21,T & c22,T & c23,T & c24,T & c31,T & c32,T & c33,T & c34,T & c41,T & c42,T & c43,T & c44)960 void TMatrix4x4<T>::get 961 (T& c11, T& c12, T& c13, T& c14, 962 T& c21, T& c22, T& c23, T& c24, 963 T& c31, T& c32, T& c33, T& c34, 964 T& c41, T& c42, T& c43, T& c44) const 965 { 966 c11 = m11; c12 = m12; c13 = m13; c14 = m14; 967 c21 = m21; c22 = m22; c23 = m23; c24 = m24; 968 c31 = m31; c32 = m32; c33 = m33; c34 = m34; 969 c41 = m41; c42 = m42; c43 = m43; c44 = m44; 970 } 971 972 template <typename T> 973 BALL_INLINE getTrace()974 T TMatrix4x4<T>::getTrace() const 975 { 976 return (m11 + m22 + m33 + m44); 977 } 978 979 template <typename T> 980 BALL_INLINE getZero()981 const TMatrix4x4<T>& TMatrix4x4<T>::getZero() 982 { 983 static TMatrix4x4<T> null_matrix 984 (0, 0, 0, 0, 985 0, 0, 0, 0, 986 0, 0, 0, 0, 987 0, 0, 0, 0); 988 989 return null_matrix; 990 } 991 992 993 template <typename T> 994 BALL_INLINE setIdentity()995 void TMatrix4x4<T>::setIdentity() 996 { 997 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 998 m11 = m22 = m33 = m44 = (T)1; 999 } 1000 template <typename T> 1001 BALL_INLINE getIdentity()1002 const TMatrix4x4<T>& TMatrix4x4<T>::getIdentity() 1003 { 1004 static TMatrix4x4<T> identity 1005 (1, 0, 0, 0, 1006 0, 1, 0, 0, 1007 0, 0, 1, 0, 1008 0, 0, 0, 1); 1009 1010 return identity; 1011 } 1012 1013 template <typename T> set(const T & t)1014 void TMatrix4x4<T>::set(const T& t) 1015 { 1016 m11 = m12 = m13 = m14 1017 = m21 = m22 = m23 = m24 1018 = m31 = m32 = m33 = m34 1019 = m41 = m42 = m43 = m44 1020 = t; 1021 } 1022 1023 template <typename T> swap(TMatrix4x4<T> & m)1024 void TMatrix4x4<T>::swap(TMatrix4x4<T>& m) 1025 { 1026 T tmp = m11; m11 = m.m11; m.m11 = tmp; 1027 tmp = m12; m12 = m.m12; m.m12 = tmp; 1028 tmp = m13; m13 = m.m13; m.m13 = tmp; 1029 tmp = m14; m14 = m.m14; m.m14 = tmp; 1030 tmp = m21; m21 = m.m21; m.m21 = tmp; 1031 tmp = m22; m22 = m.m22; m.m22 = tmp; 1032 tmp = m23; m23 = m.m23; m.m23 = tmp; 1033 tmp = m24; m24 = m.m24; m.m24 = tmp; 1034 tmp = m31; m31 = m.m31; m.m31 = tmp; 1035 tmp = m32; m32 = m.m32; m.m32 = tmp; 1036 tmp = m33; m33 = m.m33; m.m33 = tmp; 1037 tmp = m34; m34 = m.m34; m.m34 = tmp; 1038 tmp = m41; m41 = m.m41; m.m41 = tmp; 1039 tmp = m42; m42 = m.m42; m.m42 = tmp; 1040 tmp = m43; m43 = m.m43; m.m43 = tmp; 1041 tmp = m44; m44 = m.m44; m.m44 = tmp; 1042 } 1043 1044 template <typename T> transpose()1045 void TMatrix4x4<T>::transpose() 1046 { 1047 T tmp = m12; 1048 m12 = m21; 1049 m21 = tmp; 1050 1051 tmp = m13; 1052 m13 = m31; 1053 m31 = tmp; 1054 1055 tmp = m14; 1056 m14 = m41; 1057 m41 = tmp; 1058 1059 tmp = m23; 1060 m23 = m32; 1061 m32 = tmp; 1062 1063 tmp = m24; 1064 m24 = m42; 1065 m42 = tmp; 1066 1067 tmp = m34; 1068 m34 = m43; 1069 m43 = tmp; 1070 } 1071 1072 template <typename T> getRow(Position row)1073 TVector4<T> TMatrix4x4<T>::getRow(Position row) const 1074 { 1075 if (row > 3) 1076 { 1077 throw Exception::IndexOverflow(__FILE__, __LINE__, row, 3); 1078 } 1079 1080 // calculate the start of the row in the array 1081 const T* ptr = comp_ptr_[4 * row]; 1082 return TVector4<T> (ptr[0], ptr[1], ptr[2], ptr[3]); 1083 } 1084 1085 template <typename T> getColumn(Position col)1086 TVector4<T> TMatrix4x4<T>::getColumn(Position col) const 1087 { 1088 if (col > 3) 1089 { 1090 throw Exception::IndexOverflow(__FILE__, __LINE__, col, 3); 1091 } 1092 1093 const T* ptr = comp_ptr_[col]; 1094 1095 return TVector4<T> (ptr[0], ptr[4], ptr[8], ptr[12]); 1096 } 1097 1098 1099 template <typename T> setRow(Position row,const TVector4<T> & row_value)1100 void TMatrix4x4<T>::setRow(Position row, const TVector4<T>& row_value) 1101 { 1102 if (row > 3) 1103 { 1104 throw Exception::IndexOverflow(__FILE__, __LINE__, row, 3); 1105 } 1106 1107 // calculate a pointer to the start of the row 1108 T* ptr = comp_ptr_[4 * row]; 1109 1110 ptr[0] = row_value.x; 1111 ptr[1] = row_value.y; 1112 ptr[2] = row_value.z; 1113 ptr[3] = row_value.h; 1114 } 1115 1116 template <typename T> setColumn(Position col,const TVector4<T> & col_value)1117 void TMatrix4x4<T>::setColumn(Position col, const TVector4<T>& col_value) 1118 { 1119 if (col > 3) 1120 { 1121 throw Exception::IndexOverflow(__FILE__, __LINE__, col, 3); 1122 } 1123 1124 // calculate a pointer to the start of the column 1125 T* ptr = comp_ptr_[col]; 1126 1127 ptr[0] = col_value.x; 1128 ptr[4] = col_value.y; 1129 ptr[8] = col_value.z; 1130 ptr[12] = col_value.h; 1131 } 1132 1133 template <typename T> isEqual(const TMatrix4x4<T> & m)1134 bool TMatrix4x4<T>::isEqual(const TMatrix4x4<T>& m) const 1135 { 1136 // iterate over all component pointers 1137 // and compare the elements for approximate equality 1138 for (Position i = 0; i < 16; i++) 1139 { 1140 if (Maths::isEqual(*comp_ptr_[i], *m.comp_ptr_[i]) == false) 1141 { 1142 return false; 1143 } 1144 } 1145 1146 return true; 1147 } 1148 1149 template <typename T> getDiagonal()1150 TVector4<T>TMatrix4x4<T>::getDiagonal() const 1151 { 1152 return TVector4<T>(m11, m22, m33, m44); 1153 } 1154 1155 template <typename T> 1156 BALL_INLINE operator()1157 T& TMatrix4x4<T>::operator () (Position row, Position col) 1158 { 1159 if ((row > 3) || (col > 3)) 1160 { 1161 throw Exception::IndexOverflow(__FILE__, __LINE__, row + col, 3); 1162 } 1163 1164 return *comp_ptr_[4 * row + col]; 1165 } 1166 1167 template <typename T> 1168 BALL_INLINE operator()1169 const T& TMatrix4x4<T>::operator () (Position row, Position col) const 1170 { 1171 if ((row > 3) || (col > 3)) 1172 { 1173 throw Exception::IndexOverflow(__FILE__, __LINE__, row + col, 3); 1174 } 1175 1176 return *comp_ptr_[4 * row + col]; 1177 } 1178 1179 template <typename T> 1180 BALL_INLINE 1181 const T& TMatrix4x4<T>::operator [] (Position position) const 1182 { 1183 if (position > 15) 1184 { 1185 throw Exception::IndexOverflow(__FILE__, __LINE__, position, 15); 1186 } 1187 return *comp_ptr_[position]; 1188 } 1189 1190 template <typename T> 1191 BALL_INLINE 1192 T& TMatrix4x4<T>::operator [] (Position position) 1193 { 1194 if (position > 15) 1195 { 1196 throw Exception::IndexOverflow(__FILE__, __LINE__, position, 15); 1197 } 1198 return *comp_ptr_[position]; 1199 } 1200 1201 template <typename T> 1202 BALL_INLINE 1203 TMatrix4x4<T> TMatrix4x4<T>::operator + () const 1204 { 1205 return *this; 1206 } 1207 1208 template <typename T> 1209 BALL_INLINE TMatrix4x4<T> 1210 TMatrix4x4<T>::operator - () const 1211 { 1212 return TMatrix4x4<T> 1213 (-m11, -m12, -m13, -m14, 1214 -m21, -m22, -m23, -m24, 1215 -m31, -m32, -m33, -m34, 1216 -m41, -m42, -m43, -m44); 1217 } 1218 1219 template <typename T> 1220 TMatrix4x4<T> TMatrix4x4<T>::operator + (const TMatrix4x4<T>& m) const 1221 { 1222 return TMatrix4x4<T> 1223 (m11 + m.m11, m12 + m.m12, m13 + m.m13, m14 + m.m14, 1224 m21 + m.m21, m22 + m.m22, m23 + m.m23, m24 + m.m24, 1225 m31 + m.m31, m32 + m.m32, m33 + m.m33, m34 + m.m34, 1226 m41 + m.m41, m42 + m.m42, m43 + m.m43, m44 + m.m44); 1227 } 1228 1229 template <typename T> 1230 TMatrix4x4<T>& TMatrix4x4<T>::operator += (const TMatrix4x4<T>& m) 1231 { 1232 m11 += m.m11; 1233 m12 += m.m12; 1234 m13 += m.m13; 1235 m14 += m.m14; 1236 m21 += m.m21; 1237 m22 += m.m22; 1238 m23 += m.m23; 1239 m24 += m.m24; 1240 m31 += m.m31; 1241 m32 += m.m32; 1242 m33 += m.m33; 1243 m34 += m.m34; 1244 m41 += m.m41; 1245 m42 += m.m42; 1246 m43 += m.m43; 1247 m44 += m.m44; 1248 1249 return *this; 1250 } 1251 1252 template <typename T> 1253 TMatrix4x4<T> TMatrix4x4<T>::operator - (const TMatrix4x4<T>& m) const 1254 { 1255 return TMatrix4x4<T> 1256 (m11 - m.m11, m12 - m.m12, m13 - m.m13, m14 - m.m14, 1257 m21 - m.m21, m22 - m.m22, m23 - m.m23, m24 - m.m24, 1258 m31 - m.m31, m32 - m.m32, m33 - m.m33, m34 - m.m34, 1259 m41 - m.m41, m42 - m.m42, m43 - m.m43, m44 - m.m44); 1260 } 1261 1262 template <typename T> 1263 TMatrix4x4<T>& TMatrix4x4<T>::operator -= (const TMatrix4x4<T>& m) 1264 { 1265 m11 -= m.m11; 1266 m12 -= m.m12; 1267 m13 -= m.m13; 1268 m14 -= m.m14; 1269 m21 -= m.m21; 1270 m22 -= m.m22; 1271 m23 -= m.m23; 1272 m24 -= m.m24; 1273 m31 -= m.m31; 1274 m32 -= m.m32; 1275 m33 -= m.m33; 1276 m34 -= m.m34; 1277 m41 -= m.m41; 1278 m42 -= m.m42; 1279 m43 -= m.m43; 1280 m44 -= m.m44; 1281 1282 return *this; 1283 } 1284 1285 template <typename T> 1286 TMatrix4x4<T> TMatrix4x4<T>::operator * (const T& scalar) const 1287 { 1288 return TMatrix4x4<T> 1289 (m11 * scalar, m12 * scalar, m13 * scalar, m14 * scalar, 1290 m21 * scalar, m22 * scalar, m23 * scalar, m24 * scalar, 1291 m31 * scalar, m32 * scalar, m33 * scalar, m34 * scalar, 1292 m41 * scalar, m42 * scalar, m43 * scalar, m44 * scalar); 1293 } 1294 1295 template <typename T> 1296 TMatrix4x4<T> operator * (const T& scalar, const TMatrix4x4<T>& m) 1297 { 1298 return TMatrix4x4<T> 1299 (scalar * m.m11, scalar * m.m12, scalar * m.m13, scalar * m.m14, 1300 scalar * m.m21, scalar * m.m22, scalar * m.m23, scalar * m.m24, 1301 scalar * m.m31, scalar * m.m32, scalar * m.m33, scalar * m.m34, 1302 scalar * m.m41, scalar * m.m42, scalar * m.m43, scalar * m.m44); 1303 } 1304 1305 template <typename T> 1306 TMatrix4x4<T>& TMatrix4x4<T>::operator *= (const T& scalar) 1307 { 1308 m11 *= scalar; 1309 m12 *= scalar; 1310 m13 *= scalar; 1311 m14 *= scalar; 1312 m21 *= scalar; 1313 m22 *= scalar; 1314 m23 *= scalar; 1315 m24 *= scalar; 1316 m31 *= scalar; 1317 m32 *= scalar; 1318 m33 *= scalar; 1319 m34 *= scalar; 1320 m41 *= scalar; 1321 m42 *= scalar; 1322 m43 *= scalar; 1323 m44 *= scalar; 1324 1325 return *this; 1326 } 1327 1328 template <typename T> 1329 TVector3<T> operator *(const TMatrix4x4<T>& matrix, const TVector3<T>& vector) 1330 { 1331 return TVector3<T> 1332 (matrix.m11 * vector.x + matrix.m12 * vector.y + matrix.m13 * vector.z + matrix.m14, 1333 matrix.m21 * vector.x + matrix.m22 * vector.y + matrix.m23 * vector.z + matrix.m24, 1334 matrix.m31 * vector.x + matrix.m32 * vector.y + matrix.m33 * vector.z + matrix.m34); 1335 } 1336 1337 template <typename T> 1338 BALL_INLINE 1339 TMatrix4x4<T>TMatrix4x4<T>::operator / (const T& scalar) const 1340 { 1341 if (scalar == (T)0) 1342 { 1343 throw Exception::DivisionByZero(__FILE__, __LINE__); 1344 } 1345 1346 return (*this * ((T)1 / scalar)); 1347 } 1348 1349 template <typename T> 1350 BALL_INLINE 1351 TMatrix4x4<T>& TMatrix4x4<T>::operator /= (const T& scalar) 1352 { 1353 if (scalar == (T)0) 1354 { 1355 throw Exception::DivisionByZero(__FILE__, __LINE__); 1356 } 1357 1358 return (*this *= (T)1 / scalar); 1359 } 1360 1361 template <typename T> 1362 TMatrix4x4<T> TMatrix4x4<T>::operator * (const TMatrix4x4<T>& m) const 1363 { 1364 return TMatrix4x4<T> 1365 (m11 * m.m11 + m12 * m.m21 + m13 * m.m31 + m14 * m.m41, 1366 m11 * m.m12 + m12 * m.m22 + m13 * m.m32 + m14 * m.m42, 1367 m11 * m.m13 + m12 * m.m23 + m13 * m.m33 + m14 * m.m43, 1368 m11 * m.m14 + m12 * m.m24 + m13 * m.m34 + m14 * m.m44, 1369 1370 m21 * m.m11 + m22 * m.m21 + m23 * m.m31 + m24 * m.m41, 1371 m21 * m.m12 + m22 * m.m22 + m23 * m.m32 + m24 * m.m42, 1372 m21 * m.m13 + m22 * m.m23 + m23 * m.m33 + m24 * m.m43, 1373 m21 * m.m14 + m22 * m.m24 + m23 * m.m34 + m24 * m.m44, 1374 1375 m31 * m.m11 + m32 * m.m21 + m33 * m.m31 + m34 * m.m41, 1376 m31 * m.m12 + m32 * m.m22 + m33 * m.m32 + m34 * m.m42, 1377 m31 * m.m13 + m32 * m.m23 + m33 * m.m33 + m34 * m.m43, 1378 m31 * m.m14 + m32 * m.m24 + m33 * m.m34 + m34 * m.m44, 1379 1380 m41 * m.m11 + m42 * m.m21 + m43 * m.m31 + m44 * m.m41, 1381 m41 * m.m12 + m42 * m.m22 + m43 * m.m32 + m44 * m.m42, 1382 m41 * m.m13 + m42 * m.m23 + m43 * m.m33 + m44 * m.m43, 1383 m41 * m.m14 + m42 * m.m24 + m43 * m.m34 + m44 * m.m44); 1384 } 1385 1386 template <typename T> 1387 TMatrix4x4<T>& TMatrix4x4<T>::operator *= (const TMatrix4x4<T>& m) 1388 { 1389 set(m11 * m.m11 + m12 * m.m21 + m13 * m.m31 + m14 * m.m41, 1390 m11 * m.m12 + m12 * m.m22 + m13 * m.m32 + m14 * m.m42, 1391 m11 * m.m13 + m12 * m.m23 + m13 * m.m33 + m14 * m.m43, 1392 m11 * m.m14 + m12 * m.m24 + m13 * m.m34 + m14 * m.m44, 1393 1394 m21 * m.m11 + m22 * m.m21 + m23 * m.m31 + m24 * m.m41, 1395 m21 * m.m12 + m22 * m.m22 + m23 * m.m32 + m24 * m.m42, 1396 m21 * m.m13 + m22 * m.m23 + m23 * m.m33 + m24 * m.m43, 1397 m21 * m.m14 + m22 * m.m24 + m23 * m.m34 + m24 * m.m44, 1398 1399 m31 * m.m11 + m32 * m.m21 + m33 * m.m31 + m34 * m.m41, 1400 m31 * m.m12 + m32 * m.m22 + m33 * m.m32 + m34 * m.m42, 1401 m31 * m.m13 + m32 * m.m23 + m33 * m.m33 + m34 * m.m43, 1402 m31 * m.m14 + m32 * m.m24 + m33 * m.m34 + m34 * m.m44, 1403 1404 m41 * m.m11 + m42 * m.m21 + m43 * m.m31 + m44 * m.m41, 1405 m41 * m.m12 + m42 * m.m22 + m43 * m.m32 + m44 * m.m42, 1406 m41 * m.m13 + m42 * m.m23 + m43 * m.m33 + m44 * m.m43, 1407 m41 * m.m14 + m42 * m.m24 + m43 * m.m34 + m44 * m.m44); 1408 1409 return *this; 1410 } 1411 1412 template <typename T> 1413 TVector4<T> TMatrix4x4<T>::operator * (const TVector4<T>& v) const 1414 { 1415 return TVector4<T> 1416 (m11 * v.x + m12 * v.y + m13 * v.z + m14 * v.h, 1417 m21 * v.x + m22 * v.y + m23 * v.z + m24 * v.h, 1418 m31 * v.x + m32 * v.y + m33 * v.z + m34 * v.h, 1419 m41 * v.x + m42 * v.y + m43 * v.z + m44 * v.h); 1420 } 1421 1422 template <typename T> invert(TMatrix4x4<T> & inverse)1423 bool TMatrix4x4<T>::invert(TMatrix4x4<T>& inverse) const 1424 { 1425 /** First, we compute a QR decomposition, then we use it to solve 1426 * the system A*A^-1 = I <=> R * A^-1 = Q^t, where R is upper 1427 * triangular. 1428 * 1429 * This is based on the Householder transform algorithm given in 1430 * the Numerical Recipes. 1431 */ 1432 Index i, j, k; 1433 1434 T a[4][4] = // holds the matrix we want to invert 1435 { 1436 { m11, m12, m13, m14 }, 1437 { m21, m22, m23, m24 }, 1438 { m31, m32, m33, m34 }, 1439 { m41, m42, m43, m44 } 1440 }; 1441 1442 // holds the maximum in the part of A we still have to work with 1443 T scale, sum_of_squares, sigma, tau; 1444 T c[4], d[4]; 1445 for (k=0; k<3; k++) 1446 { 1447 scale = (T)0; 1448 // find the maximum in a 1449 for (i=k; i<4; i++) 1450 scale = Maths::max((T)fabs(a[i][k]), scale); 1451 1452 // is the matrix singular? 1453 if (scale == (T)0) 1454 return false; 1455 1456 // nope. we can normalize the remaining rows 1457 for (i=k; i<4; i++) 1458 a[i][k] /= scale; 1459 1460 sum_of_squares = (T)0; 1461 for (i=k; i<4; i++) 1462 sum_of_squares += a[i][k]*a[i][k]; 1463 1464 // shift the diagonal element 1465 sigma = (a[k][k] >= 0) ? sqrt(sum_of_squares) : -sqrt(sum_of_squares); 1466 a[k][k] += sigma; 1467 1468 c[k] = sigma*a[k][k]; 1469 d[k] = -scale*sigma; 1470 1471 for (j = k+1; j<4; j++) 1472 { 1473 // store the scalar product of a_[k] and a_[j] 1474 sum_of_squares = (T)0; 1475 for (i = k; i<4; i++) 1476 sum_of_squares += a[i][k] * a[i][j]; 1477 1478 tau = sum_of_squares / c[k]; 1479 1480 // prepare the matrix 1481 for (i=k; i<4; i++) 1482 a[i][j] -= tau*a[i][k]; 1483 } 1484 } 1485 d[3] = a[3][3]; 1486 1487 // is the matrix singular? 1488 if (d[3] == (T)0) 1489 return 1; 1490 1491 // now we have the QR decomposition. The upper triangle of A contains 1492 // R, except for the diagonal elements, which are stored in d. c contains 1493 // the values needed to compute the Householder matrices Q, and the vectors 1494 // u needed for the determination of the Qs are stored in the lower triangle 1495 // of A 1496 // 1497 // now we need to solve four linear systems of equations, one for each column 1498 // of the resulting matrix 1499 T result[4][4]; 1500 result[0][0] = 1; result[0][1] = 0; result[0][2] = 0; result[0][3] = 0; 1501 result[1][0] = 0; result[1][1] = 1; result[1][2] = 0; result[1][3] = 0; 1502 result[2][0] = 0; result[2][1] = 0; result[2][2] = 1; result[2][3] = 0; 1503 result[3][0] = 0; result[3][1] = 0; result[3][2] = 0; result[3][3] = 1; 1504 1505 for (k=0; k<4; k++) // k generates the k-th column of the inverse 1506 { 1507 // form the vector Q^t * b, which is simple, since b = e_k 1508 for (j=0; j<3; j++) 1509 { 1510 sum_of_squares = (T)0; 1511 for (i=j; i<4; i++) 1512 sum_of_squares += a[i][j]*result[i][k]; 1513 1514 tau = sum_of_squares / c[j]; 1515 1516 for (i=j; i<4; i++) 1517 result[i][k] -= tau*a[i][j]; 1518 } 1519 1520 // and solve the resulting system 1521 result[3][k] /= d[3]; 1522 for (i=2; i>=0; i--) 1523 { 1524 sum_of_squares = (T)0; 1525 for (j=i+1; j<4; j++) 1526 sum_of_squares += a[i][j] * result[j][k]; 1527 1528 result[i][k] = (result[i][k] - sum_of_squares) / d[i]; 1529 } 1530 } 1531 1532 T* k_ptr = *result; 1533 inverse.m11 = *k_ptr++; 1534 inverse.m12 = *k_ptr++; 1535 inverse.m13 = *k_ptr++; 1536 inverse.m14 = *k_ptr++; 1537 inverse.m21 = *k_ptr++; 1538 inverse.m22 = *k_ptr++; 1539 inverse.m23 = *k_ptr++; 1540 inverse.m24 = *k_ptr++; 1541 inverse.m31 = *k_ptr++; 1542 inverse.m32 = *k_ptr++; 1543 inverse.m33 = *k_ptr++; 1544 inverse.m34 = *k_ptr++; 1545 inverse.m41 = *k_ptr++; 1546 inverse.m42 = *k_ptr++; 1547 inverse.m43 = *k_ptr++; 1548 inverse.m44 = *k_ptr; 1549 1550 return true; 1551 } 1552 1553 template <typename T> invert()1554 BALL_INLINE bool TMatrix4x4<T>::invert() 1555 { 1556 return invert(*this); 1557 } 1558 1559 template <typename T> getDeterminant()1560 T TMatrix4x4<T>::getDeterminant() const 1561 { 1562 Position i; 1563 Position j; 1564 Position k; 1565 T submatrix[3][3]; 1566 T matrix[4][4] = 1567 { 1568 { m11, m12, m13, m14 }, 1569 { m21, m22, m23, m24 }, 1570 { m31, m32, m33, m34 }, 1571 { m41, m42, m43, m44 } 1572 }; 1573 T determinant = 0; 1574 1575 for (i = 0; i < 4; i++) 1576 { 1577 for (j = 0; j < 3; j++) 1578 { 1579 for (k = 0; k < 3; k++) 1580 { 1581 submatrix[j][k] = 1582 matrix[j + 1][(k < i) ? k : k + 1]; 1583 } 1584 } 1585 1586 determinant += matrix[0][i] * (T)(i / 2.0 == (i >> 1) ? 1 : -1) 1587 * (submatrix[0][0] * submatrix[1][1] * submatrix[2][2] 1588 + submatrix[0][1] * submatrix[1][2] * submatrix[2][0] 1589 + submatrix[0][2] * submatrix[1][0] * submatrix[2][1] 1590 - submatrix[0][2] * submatrix[1][1] * submatrix[2][0] 1591 - submatrix[0][0] * submatrix[1][2] * submatrix[2][1] 1592 - submatrix[0][1] * submatrix[1][0] * submatrix[2][2]); 1593 } 1594 1595 return determinant; 1596 } 1597 1598 template <typename T> translate(const T & x,const T & y,const T & z)1599 void TMatrix4x4<T>::translate(const T& x, const T& y, const T& z) 1600 { 1601 m14 += m11 * x + m12 * y + m13 * z; 1602 m24 += m21 * x + m22 * y + m23 * z; 1603 m34 += m31 * x + m32 * y + m33 * z; 1604 m44 += m41 * x + m42 * y + m43 * z; 1605 } 1606 1607 template <typename T> translate(const TVector3<T> & v)1608 void TMatrix4x4<T>::translate(const TVector3<T>& v) 1609 { 1610 m14 += m11 * v.x + m12 * v.y + m13 * v.z; 1611 m24 += m21 * v.x + m22 * v.y + m23 * v.z; 1612 m34 += m31 * v.x + m32 * v.y + m33 * v.z; 1613 m44 += m41 * v.x + m42 * v.y + m43 * v.z; 1614 } 1615 1616 template <typename T> setTranslation(const T & x,const T & y,const T & z)1617 void TMatrix4x4<T>::setTranslation(const T& x, const T& y, const T& z) 1618 { 1619 m11 = m22 = m33 = m44 = 1; 1620 1621 m12 = m13 = 1622 m21 = m23 = 1623 m31 = m32 = 1624 m41 = m42 = m43 = 0; 1625 1626 m14 = x; 1627 m24 = y; 1628 m34 = z; 1629 } 1630 1631 template <typename T> setTranslation(const TVector3<T> & v)1632 void TMatrix4x4<T>::setTranslation(const TVector3<T>& v) 1633 { 1634 m11 = m22 = m33 = m44 = 1; 1635 1636 m12 = m13 = 1637 m21 = m23 = 1638 m31 = m32 = 1639 m41 = m42 = m43 = 0; 1640 1641 m14 = v.x; 1642 m24 = v.y; 1643 m34 = v.z; 1644 } 1645 1646 template <typename T> scale(const T & x_scale,const T & y_scale,const T & z_scale)1647 void TMatrix4x4<T>::scale(const T& x_scale, const T& y_scale, const T& z_scale) 1648 { 1649 m11 *= x_scale; 1650 m21 *= x_scale; 1651 m31 *= x_scale; 1652 m41 *= x_scale; 1653 1654 m12 *= y_scale; 1655 m22 *= y_scale; 1656 m32 *= y_scale; 1657 m42 *= y_scale; 1658 1659 m13 *= z_scale; 1660 m23 *= z_scale; 1661 m33 *= z_scale; 1662 m43 *= z_scale; 1663 } 1664 1665 template <typename T> scale(const T & scale)1666 void TMatrix4x4<T>::scale(const T& scale) 1667 { 1668 m11 *= scale; 1669 m21 *= scale; 1670 m31 *= scale; 1671 m41 *= scale; 1672 1673 m12 *= scale; 1674 m22 *= scale; 1675 m32 *= scale; 1676 m42 *= scale; 1677 1678 m13 *= scale; 1679 m23 *= scale; 1680 m33 *= scale; 1681 m43 *= scale; 1682 } 1683 1684 template <typename T> scale(const TVector3<T> & v)1685 void TMatrix4x4<T>::scale(const TVector3<T>& v) 1686 { 1687 m11 *= v.x; 1688 m21 *= v.x; 1689 m31 *= v.x; 1690 m41 *= v.x; 1691 1692 m12 *= v.y; 1693 m22 *= v.y; 1694 m32 *= v.y; 1695 m42 *= v.y; 1696 1697 m13 *= v.z; 1698 m23 *= v.z; 1699 m33 *= v.z; 1700 m43 *= v.z; 1701 } 1702 1703 template <typename T> setScale(const T & x_scale,const T & y_scale,const T & z_scale)1704 void TMatrix4x4<T>::setScale(const T& x_scale, const T& y_scale, const T& z_scale) 1705 { 1706 m11 = x_scale; 1707 m22 = y_scale; 1708 m33 = z_scale; 1709 m44 = 1; 1710 1711 m12 = m13 = m14 = 1712 m21 = m23 = m24 = 1713 m31 = m32 = m34 = 1714 m41 = m42 = m43 = 0; 1715 } 1716 1717 template <typename T> setScale(const T & scale)1718 void TMatrix4x4<T>::setScale(const T& scale) 1719 { 1720 m11 = scale; 1721 m22 = scale; 1722 m33 = scale; 1723 m44 = 1; 1724 1725 m12 = m13 = m14 = 1726 m21 = m23 = m24 = 1727 m31 = m32 = m34 = 1728 m41 = m42 = m43 = 0; 1729 } 1730 1731 template <typename T> setScale(const TVector3<T> & v)1732 void TMatrix4x4<T>::setScale(const TVector3<T>& v) 1733 { 1734 m11 = v.x; 1735 m22 = v.y; 1736 m33 = v.z; 1737 m44 = 1; 1738 1739 m12 = m13 = m14 = 1740 m21 = m23 = m24 = 1741 m31 = m32 = m34 = 1742 m41 = m42 = m43 = 0; 1743 } 1744 1745 template <typename T> 1746 BALL_INLINE rotateX(const TAngle<T> & phi)1747 void TMatrix4x4<T>::rotateX(const TAngle<T>& phi) 1748 { 1749 TMatrix4x4<T> rotation; 1750 1751 rotation.setRotationX(phi); 1752 *this *= rotation; 1753 } 1754 1755 template <typename T> setRotationX(const TAngle<T> & phi)1756 void TMatrix4x4<T>::setRotationX(const TAngle<T>& phi) 1757 { 1758 m11 = m44 = 1; 1759 1760 m12 = m13 = m14 1761 = m21 = m24 1762 = m31 = m34 1763 = m41 = m42 = m43 1764 = 0; 1765 1766 m22 = m33 = cos(phi); 1767 m23 = -(m32 = sin(phi)); 1768 } 1769 1770 template <typename T> 1771 BALL_INLINE rotateY(const TAngle<T> & phi)1772 void TMatrix4x4<T>::rotateY(const TAngle<T>& phi) 1773 { 1774 TMatrix4x4<T> rotation; 1775 1776 rotation.setRotationY(phi); 1777 *this *= rotation; 1778 } 1779 1780 template <typename T> setRotationY(const TAngle<T> & phi)1781 void TMatrix4x4<T>::setRotationY(const TAngle<T>& phi) 1782 { 1783 m22 = m44 = 1; 1784 1785 m12 = m14 1786 = m21 = m23 = m24 1787 = m32 = m34 1788 = m41 = m42 = m43 1789 = 0; 1790 1791 m11 = m33 = cos(phi); 1792 m31 = -(m13 = sin(phi)); 1793 } 1794 1795 template <typename T> 1796 BALL_INLINE rotateZ(const TAngle<T> & phi)1797 void TMatrix4x4<T>::rotateZ(const TAngle<T>& phi) 1798 { 1799 TMatrix4x4<T> rotation; 1800 1801 rotation.setRotationZ(phi); 1802 *this *= rotation; 1803 } 1804 1805 template <typename T> setRotationZ(const TAngle<T> & phi)1806 void TMatrix4x4<T>::setRotationZ(const TAngle<T>& phi) 1807 { 1808 m33 = m44 = 1; 1809 1810 m13 = m14 = m23 = m24 = m31 = 1811 m32 = m34 = m41 = m42 = m43 = 0; 1812 1813 m11 = m22 = cos(phi); 1814 m12 = -(m21 = sin(phi)); 1815 } 1816 1817 template <typename T> 1818 BALL_INLINE rotate(const TAngle<T> & phi,const TVector3<T> & v)1819 void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const TVector3<T>& v) 1820 { 1821 rotate(phi, v.x, v.y, v.z); 1822 } 1823 1824 template <typename T> 1825 BALL_INLINE rotate(const TAngle<T> & phi,const TVector4<T> & v)1826 void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const TVector4<T>& v) 1827 { 1828 rotate(phi, v.x, v.y, v.z); 1829 } 1830 1831 // 1832 // Arbitrary axis rotation matrix. 1833 // 1834 // [Taken from the MESA-Library. But modified for additional Speed-Up.] 1835 // 1836 // This function was contributed by Erich Boleyn (erich@uruk.org). 1837 // 1838 // This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied 1839 // like so: Rz * Ry * T * Ry' * Rz'. T is the final rotation 1840 // (which is about the X-axis), and the two composite transforms 1841 // Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary 1842 // from the arbitrary axis to the X-axis then back. They are 1843 // all elementary rotations. 1844 // 1845 // Rz' is a rotation about the Z-axis, to bring the axis vector 1846 // into the x-z plane. Then Ry' is applied, rotating about the 1847 // Y-axis to bring the axis vector parallel with the X-axis. The 1848 // rotation about the X-axis is then performed. Ry and Rz are 1849 // simply the respective inverse transforms to bring the arbitrary 1850 // axis back to it's original orientation. The first transforms 1851 // Rz' and Ry' are considered inverses, since the data from the 1852 // arbitrary axis gives you info on how to get to it, not how 1853 // to get away from it, and an inverse must be applied. 1854 // 1855 // The basic calculation used is to recognize that the arbitrary 1856 // axis vector (x, y, z), since it is of unit length, actually 1857 // represents the sines and cosines of the angles to rotate the 1858 // X-axis to the same orientation, with theta being the angle about 1859 // Z and phi the angle about Y (in the order described above) 1860 // as follows: 1861 // 1862 // cos ( theta ) = x / sqrt ( 1 - z^2 ) 1863 // sin ( theta ) = y / sqrt ( 1 - z^2 ) 1864 // 1865 // cos ( phi ) = sqrt ( 1 - z^2 ) 1866 // sin ( phi ) = z 1867 // 1868 // Note that cos ( phi ) can further be inserted to the above 1869 // formulas: 1870 // 1871 // cos ( theta ) = x / cos ( phi ) 1872 // sin ( theta ) = y / sin ( phi ) 1873 // 1874 // ...etc. Because of those relations and the standard trigonometric 1875 // relations, it is pssible to reduce the transforms down to what 1876 // is used below. It may be that any primary axis chosen will give the 1877 // same results (modulo a sign convention) using thie method. 1878 // 1879 // Particularly nice is to notice that all divisions that might 1880 // have caused trouble when parallel to certain planes or 1881 // axis go away with care paid to reducing the expressions. 1882 // After checking, it does perform correctly under all cases, since 1883 // in all the cases of division where the denominator would have 1884 // been zero, the numerator would have been zero as well, giving 1885 // the expected result. 1886 1887 template <typename T> rotate(const TAngle<T> & phi,const T & ax,const T & ay,const T & az)1888 void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const T& ax, const T& ay, const T& az) 1889 { 1890 T xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c; 1891 T x = ax; 1892 T y = ay; 1893 T z = az; 1894 1895 double sin_angle = sin(phi); 1896 double cos_angle = cos(phi); 1897 1898 xx = x * x; 1899 yy = y * y; 1900 zz = z * z; 1901 1902 T mag = sqrt(xx + yy + zz); 1903 1904 if (mag == (T)0) 1905 { 1906 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 1907 m11 = m22 = m33 = m44 = (T)1; 1908 } 1909 1910 x /= mag; 1911 y /= mag; 1912 z /= mag; 1913 1914 // we need to recalculate xx, yy, zz due to the 1915 // normalization. recalculation is probably faster 1916 // than normalizing xx, yy, zz 1917 xx = x*x; 1918 yy = y*y; 1919 zz = z*z; 1920 1921 xy = x * y; 1922 yz = y * z; 1923 zx = z * x; 1924 xs = (T) (x * sin_angle); 1925 ys = (T) (y * sin_angle); 1926 zs = (T) (z * sin_angle); 1927 one_c = (T) (1 - cos_angle); 1928 1929 m11 = (T)( (one_c * xx) + cos_angle ); 1930 m12 = (one_c * xy) - zs; 1931 m13 = (one_c * zx) + ys; 1932 m14 = 0; 1933 1934 m21 = (one_c * xy) + zs; 1935 m22 = (T) ((one_c * yy) + cos_angle); 1936 m23 = (one_c * yz) - xs; 1937 m24 = 0; 1938 1939 m31 = (one_c * zx) - ys; 1940 m32 = (one_c * yz) + xs; 1941 m33 = (T) ((one_c * zz) + cos_angle); 1942 m34 = 0; 1943 1944 m41 = 0; 1945 m42 = 0; 1946 m43 = 0; 1947 m44 = 1; 1948 } 1949 1950 template <typename T> setRotation(const TAngle<T> & phi,const T & x,const T & y,const T & z)1951 void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const T& x, const T& y, const T& z) 1952 { 1953 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 1954 m11 = m22 = m33 = m44 = (T)1; 1955 rotate(phi, x, y, z); 1956 } 1957 1958 template <typename T> 1959 BALL_INLINE setRotation(const TAngle<T> & phi,const TVector3<T> & v)1960 void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const TVector3<T>& v) 1961 { 1962 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 1963 m11 = m22 = m33 = m44 = (T)1; 1964 rotate(phi, v.x, v.y, v.z); 1965 } 1966 1967 template <typename T> 1968 BALL_INLINE setRotation(const TAngle<T> & phi,const TVector4<T> & v)1969 void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const TVector4<T>& v) 1970 { 1971 m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0; 1972 m11 = m22 = m33 = m44 = (T)1; 1973 rotate(phi, v.x, v.y, v.z); 1974 } 1975 1976 template <typename T> 1977 bool TMatrix4x4<T>::operator == (const TMatrix4x4<T>& m) const 1978 { 1979 return 1980 ( m11 == m.m11 1981 && m12 == m.m12 1982 && m13 == m.m13 1983 && m14 == m.m14 1984 && m21 == m.m21 1985 && m22 == m.m22 1986 && m23 == m.m23 1987 && m24 == m.m24 1988 && m31 == m.m31 1989 && m32 == m.m32 1990 && m33 == m.m33 1991 && m34 == m.m34 1992 && m41 == m.m41 1993 && m42 == m.m42 1994 && m43 == m.m43 1995 && m44 == m.m44); 1996 } 1997 1998 template <typename T> 1999 bool TMatrix4x4<T>::operator != (const TMatrix4x4<T>& m) const 2000 { 2001 return 2002 ( m11 != m.m11 2003 || m12 != m.m12 2004 || m13 != m.m13 2005 || m14 != m.m14 2006 || m21 != m.m21 2007 || m22 != m.m22 2008 || m23 != m.m23 2009 || m24 != m.m24 2010 || m31 != m.m31 2011 || m32 != m.m32 2012 || m33 != m.m33 2013 || m34 != m.m34 2014 || m41 != m.m41 2015 || m42 != m.m42 2016 || m43 != m.m43 2017 || m44 != m.m44); 2018 } 2019 2020 template <typename T> isIdentity()2021 bool TMatrix4x4<T>::isIdentity() const 2022 { 2023 return 2024 ( m11 == (T)1 2025 && m12 == (T)0 2026 && m13 == (T)0 2027 && m14 == (T)0 2028 && m21 == (T)0 2029 && m22 == (T)1 2030 && m23 == (T)0 2031 && m24 == (T)0 2032 && m31 == (T)0 2033 && m32 == (T)0 2034 && m33 == (T)1 2035 && m34 == (T)0 2036 && m41 == (T)0 2037 && m42 == (T)0 2038 && m43 == (T)0 2039 && m44 == (T)1); 2040 } 2041 2042 template <typename T> 2043 BALL_INLINE isRegular()2044 bool TMatrix4x4<T>::isRegular() const 2045 { 2046 return (getDeterminant() != (T)0); 2047 } 2048 2049 template <typename T> 2050 BALL_INLINE isSingular()2051 bool TMatrix4x4<T>::isSingular() const 2052 { 2053 return (getDeterminant() == (T)0); 2054 } 2055 2056 template <typename T> isSymmetric()2057 bool TMatrix4x4<T>::isSymmetric() const 2058 { 2059 return ( m12 == m21 && m13 == m31 2060 && m14 == m41 && m23 == m32 2061 && m24 == m42 && m34 == m43); 2062 } 2063 2064 template <typename T> isLowerTriangular()2065 bool TMatrix4x4<T>::isLowerTriangular() const 2066 { 2067 return ( m12 == (T)0 2068 && m13 == (T)0 2069 && m14 == (T)0 2070 && m23 == (T)0 2071 && m24 == (T)0 2072 && m34 == (T)0); 2073 } 2074 2075 template <typename T> isUpperTriangular()2076 bool TMatrix4x4<T>::isUpperTriangular() const 2077 { 2078 return ( m21 == (T)0 2079 && m31 == (T)0 2080 && m32 == (T)0 2081 && m41 == (T)0 2082 && m42 == (T)0 2083 && m43 == (T)0); 2084 } 2085 2086 template <typename T> 2087 BALL_INLINE isDiagonal()2088 bool TMatrix4x4<T>::isDiagonal() const 2089 { 2090 return ( m12 == (T)0 2091 && m13 == (T)0 2092 && m14 == (T)0 2093 && m21 == (T)0 2094 && m23 == (T)0 2095 && m24 == (T)0 2096 && m31 == (T)0 2097 && m32 == (T)0 2098 && m34 == (T)0 2099 && m41 == (T)0 2100 && m42 == (T)0 2101 && m43 == (T)0); 2102 } 2103 2104 template <typename T> isValid()2105 bool TMatrix4x4<T>::isValid() const 2106 { 2107 T **ptr = (T **)comp_ptr_; 2108 2109 return ( *ptr++ == &m11 2110 && *ptr++ == &m12 2111 && *ptr++ == &m13 2112 && *ptr++ == &m14 2113 && *ptr++ == &m21 2114 && *ptr++ == &m22 2115 && *ptr++ == &m23 2116 && *ptr++ == &m24 2117 && *ptr++ == &m31 2118 && *ptr++ == &m32 2119 && *ptr++ == &m33 2120 && *ptr++ == &m34 2121 && *ptr++ == &m41 2122 && *ptr++ == &m42 2123 && *ptr++ == &m43 2124 && *ptr == &m44); 2125 } 2126 2127 template <typename T> 2128 std::istream& operator >> (std::istream& s, TMatrix4x4<T>& m) 2129 { 2130 char c; 2131 s >> c 2132 >> m.m11 >> m.m12 >> m.m13 >> m.m14 >> c >> c 2133 >> m.m21 >> m.m22 >> m.m23 >> m.m24 >> c >> c 2134 >> m.m31 >> m.m32 >> m.m33 >> m.m34 >> c >> c 2135 >> m.m41 >> m.m42 >> m.m43 >> m.m44 >> c; 2136 2137 return s; 2138 } 2139 2140 template <typename T> 2141 std::ostream& operator << (std::ostream& s, const TMatrix4x4<T>& m) 2142 { 2143 s << '/' << std::setw(14) << m.m11 << ' ' << std::setw(14) << m.m12 << ' ' << std::setw(14) << m.m13 << ' ' << std::setw(14) << m.m14 << " \\" << std::endl 2144 << '|' << std::setw(14) << m.m21 << ' ' << std::setw(14) << m.m22 << ' ' << std::setw(14) << m.m23 << ' ' << std::setw(14) << m.m24 << " |" << std::endl 2145 << '|' << std::setw(14) << m.m31 << ' ' << std::setw(14) << m.m32 << ' ' << std::setw(14) << m.m33 << ' ' << std::setw(14) << m.m34 << " |" << std::endl 2146 << '\\' << std::setw(14) << m.m41 << ' ' << std::setw(14) << m.m42 << ' ' << std::setw(14) << m.m43 << ' ' << std::setw(14) << m.m44 << " /" << std::endl; 2147 2148 return s; 2149 } 2150 2151 template <typename T> dump(std::ostream & s,Size depth)2152 void TMatrix4x4<T>::dump(std::ostream& s, Size depth) const 2153 { 2154 BALL_DUMP_STREAM_PREFIX(s); 2155 2156 BALL_DUMP_HEADER(s, this, this); 2157 2158 BALL_DUMP_DEPTH(s, depth); 2159 s << m11 << " " << m12 << " " << m13 << " " << m14 << std::endl; 2160 2161 BALL_DUMP_DEPTH(s, depth); 2162 s << m21 << " " << m22 << " " << m23 << " " << m24 << std::endl; 2163 2164 BALL_DUMP_DEPTH(s, depth); 2165 s << m31 << " " << m32 << " " << m33 << " " << m34 << std::endl; 2166 2167 BALL_DUMP_DEPTH(s, depth); 2168 s << m41 << " " << m42 << " " << m43 << " " << m44 << std::endl; 2169 2170 BALL_DUMP_STREAM_SUFFIX(s); 2171 } 2172 2173 /// 2174 template <typename T> 2175 TMatrix4x4<T> operator * (const T& scalar, const TMatrix4x4<T>& m); 2176 2177 /// 2178 template <typename T> 2179 TVector3<T> operator * (const TMatrix4x4<T>& matrix, const TVector3<T>& vector); 2180 2181 /** The Default TMatrix4x4 Type. 2182 This default is predefined for convenience for those cases where single precision is sufficient. 2183 \ingroup Matrix44 2184 */ 2185 typedef TMatrix4x4<float> Matrix4x4; 2186 2187 } // namespace BALL 2188 2189 #endif // BALL_MATHS_MATRIX44_H 2190