src/FDM/JSBSim/math/FGMatrix33.h

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3 Header: FGMatrix33.h
   4 Author: Tony Peden, Jon Berndt, Mathias Frolich
   5 Date started: Unknown
   6
   7  ------------- Copyright (C) 2001  Jon S. Berndt (jon@jsbsim.org) -------------
   8
   9  This program is free software; you can redistribute it and/or modify it under
  10  the terms of the GNU Lesser General Public License as published by the Free Software
  11  Foundation; either version 2 of the License, or (at your option) any later
  12  version.
  13
  14  This program is distributed in the hope that it will be useful, but WITHOUT
  15  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  16  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  17  details.
  18
  19  You should have received a copy of the GNU Lesser General Public License along with
  20  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  21  Place - Suite 330, Boston, MA  02111-1307, USA.
  22
  23  Further information about the GNU Lesser General Public License can also be found on
  24  the world wide web at http://www.gnu.org.
  25
  26 HISTORY
  27 --------------------------------------------------------------------------------
  28 ??/??/??   TP   Created
  29 03/16/2000 JSB  Added exception throwing
  30 03/06/2004 MF   Rework of the code to make it a bit compiler friendlier
  31
  32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  33 SENTRY
  34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  35
  36 #ifndef FGMATRIX33_H
  37 #define FGMATRIX33_H
  38
  39 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  40 INCLUDES
  41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  42
  43 #include <string>
  44 #include <iosfwd>
  45
  46 #include "FGColumnVector3.h"
  47 #include "FGJSBBase.h"
  48
  49 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  50 DEFINITIONS
  51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  52
  53 #define ID_MATRIX33 "$Id: FGMatrix33.h,v 1.11 2010/06/30 03:13:40 jberndt Exp $"
  54
  55 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  56 FORWARD DECLARATIONS
  57 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  58
  59 namespace JSBSim {
  60
  61 class FGColumnVector3;
  62 class FGQuaternion;
  63
  64 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  65 CLASS DOCUMENTATION
  66 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  67
  68 /** Exception convenience class.
  69   */
  70
  71 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  72 DECLARATION: MatrixException
  73 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  74
  75 class MatrixException : public FGJSBBase
  76 {
  77 public:
  78   std::string Message;
  79 };
  80
  81 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  82 CLASS DOCUMENTATION
  83 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  84
  85   /** Handles matrix math operations.
  86       @author Tony Peden, Jon Berndt, Mathias Froelich
  87   */
  88
  89 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  90 DECLARATION: FGMatrix33
  91 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  92
  93 class FGMatrix33 : public FGJSBBase
  94 {
  95 public:
  96
  97   enum {
  98     eRows = 3,
  99     eColumns = 3
 100   };
 101
 102   /** Default initializer.
 103
 104       Create a zero matrix.
 105    */
 106   FGMatrix33(void);
 107
 108   /** Copy constructor.
 109
 110       @param M Matrix which is used for initialization.
 111
 112       Create copy of the matrix given in the argument.
 113    */
 114   FGMatrix33(const FGMatrix33& M) {
 115     data[0] = M.data[0];
 116     data[1] = M.data[1];
 117     data[2] = M.data[2];
 118     data[3] = M.data[3];
 119     data[4] = M.data[4];
 120     data[5] = M.data[5];
 121     data[6] = M.data[6];
 122     data[7] = M.data[7];
 123     data[8] = M.data[8];
 124
 125 //    Debug(0);
 126   }
 127
 128   /** Initialization by given values.
 129
 130       @param m11 value of the 1,1 Matrix element.
 131       @param m12 value of the 1,2 Matrix element.
 132       @param m13 value of the 1,3 Matrix element.
 133       @param m21 value of the 2,1 Matrix element.
 134       @param m22 value of the 2,2 Matrix element.
 135       @param m23 value of the 2,3 Matrix element.
 136       @param m31 value of the 3,1 Matrix element.
 137       @param m32 value of the 3,2 Matrix element.
 138       @param m33 value of the 3,3 Matrix element.
 139
 140       Create a matrix from the doubles given in the arguments.
 141    */
 142   FGMatrix33(double m11, double m12, double m13,
 143              double m21, double m22, double m23,
 144              double m31, double m32, double m33) {
 145     data[0] = m11;
 146     data[1] = m21;
 147     data[2] = m31;
 148     data[3] = m12;
 149     data[4] = m22;
 150     data[5] = m32;
 151     data[6] = m13;
 152     data[7] = m23;
 153     data[8] = m33;
 154
 155     // Debug(0);
 156   }
 157
 158   /** Destructor.
 159    */
 160   ~FGMatrix33(void) { /* Debug(1); */ }
 161
 162   /** Prints the contents of the matrix.
 163       @param delimeter the item separator (tab or comma)
 164       @return a string with the delimeter-separated contents of the matrix  */
 165   std::string Dump(const std::string& delimeter) const;
 166
 167   /** Prints the contents of the matrix.
 168       @param delimeter the item separator (tab or comma, etc.)
 169       @param prefix an additional prefix that is used to indent the 3X3 matrix printout
 170       @return a string with the delimeter-separated contents of the matrix  */
 171   std::string Dump(const std::string& delimiter, const std::string& prefix) const;
 172
 173   /** Read access the entries of the matrix.
 174       @param row Row index.
 175       @param col Column index.
 176
 177       @return the value of the matrix entry at the given row and
 178       column indices. Indices are counted starting with 1.
 179    */
 180   double operator()(unsigned int row, unsigned int col) const {
 181     return data[(col-1)*eRows+row-1];
 182   }
 183
 184   /** Write access the entries of the matrix.
 185       Note that the indices given in the arguments are unchecked.
 186
 187       @param row Row index.
 188       @param col Column index.
 189
 190       @return a reference to the matrix entry at the given row and
 191       column indices. Indices are counted starting with 1.
 192    */
 193   double& operator()(unsigned int row, unsigned int col) {
 194     return data[(col-1)*eRows+row-1];
 195   }
 196
 197   /** Read access the entries of the matrix.
 198       This function is just a shortcut for the <tt>double&
 199       operator()(unsigned int row, unsigned int col)</tt> function. It is
 200       used internally to access the elements in a more convenient way.
 201
 202       Note that the indices given in the arguments are unchecked.
 203
 204       @param row Row index.
 205       @param col Column index.
 206
 207       @return the value of the matrix entry at the given row and
 208       column indices. Indices are counted starting with 1.
 209    */
 210   double Entry(unsigned int row, unsigned int col) const {
 211     return data[(col-1)*eRows+row-1];
 212   }
 213
 214   /** Write access the entries of the matrix.
 215       This function is just a shortcut for the <tt>double&
 216       operator()(unsigned int row, unsigned int col)</tt> function. It is
 217       used internally to access the elements in a more convenient way.
 218
 219       Note that the indices given in the arguments are unchecked.
 220
 221       @param row Row index.
 222       @param col Column index.
 223
 224       @return a reference to the matrix entry at the given row and
 225       column indices. Indices are counted starting with 1.
 226    */
 227    double& Entry(unsigned int row, unsigned int col) {
 228      return data[(col-1)*eRows+row-1];
 229    }
 230
 231   /** Number of rows in the matrix.
 232       @return the number of rows in the matrix.
 233    */
 234    unsigned int Rows(void) const { return eRows; }
 235
 236   /** Number of cloumns in the matrix.
 237       @return the number of columns in the matrix.
 238    */
 239    unsigned int Cols(void) const { return eColumns; }
 240
 241   /** Transposed matrix.
 242       This function only returns the transpose of this matrix. This matrix itself
 243       remains unchanged.
 244       @return the transposed matrix.
 245    */
 246   FGMatrix33 Transposed(void) const {
 247     return FGMatrix33( data[0], data[1], data[2],
 248                        data[3], data[4], data[5],
 249                        data[6], data[7], data[8] );
 250   }
 251
 252   /** Transposes this matrix.
 253       This function only transposes this matrix. Nothing is returned.
 254    */
 255   void T(void);
 256
 257 /** Initialize the matrix.
 258     This function initializes a matrix to all 0.0.
 259  */
 260   void InitMatrix(void);
 261
 262 /** Initialize the matrix.
 263     This function initializes a matrix to user specified values.
 264  */
 265   void InitMatrix(double m11, double m12, double m13,
 266                   double m21, double m22, double m23,
 267                   double m31, double m32, double m33) {
 268     data[0] = m11;
 269     data[1] = m21;
 270     data[2] = m31;
 271     data[3] = m12;
 272     data[4] = m22;
 273     data[5] = m32;
 274     data[6] = m13;
 275     data[7] = m23;
 276     data[8] = m33;
 277   }
 278
 279   /** Returns the quaternion associated with this direction cosine (rotation) matrix.
 280   */
 281   FGQuaternion GetQuaternion(void);
 282
 283   /** Determinant of the matrix.
 284       @return the determinant of the matrix.
 285    */
 286   double Determinant(void) const;
 287
 288   /** Return if the matrix is invertible.
 289       Checks and returns if the matrix is nonsingular and thus
 290       invertible. This is done by simply computing the determinant and
 291       check if it is zero. Note that this test does not cover any
 292       instabilities caused by nearly singular matirces using finite
 293       arithmetics. It only checks exact singularity.
 294    */
 295   bool Invertible(void) const { return 0.0 != Determinant(); }
 296
 297   /** Return the inverse of the matrix.
 298       Computes and returns if the inverse of the matrix. It is computed
 299       by Cramers Rule. Also there are no checks performed if the matrix
 300       is invertible. If you are not sure that it really is check this
 301       with the @ref Invertible() call before.
 302    */
 303   FGMatrix33 Inverse(void) const;
 304
 305   /** Assignment operator.
 306
 307       @param A source matrix.
 308
 309       Copy the content of the matrix given in the argument into *this.
 310    */
 311   FGMatrix33& operator=(const FGMatrix33& A) {
 312     data[0] = A.data[0];
 313     data[1] = A.data[1];
 314     data[2] = A.data[2];
 315     data[3] = A.data[3];
 316     data[4] = A.data[4];
 317     data[5] = A.data[5];
 318     data[6] = A.data[6];
 319     data[7] = A.data[7];
 320     data[8] = A.data[8];
 321     return *this;
 322   }
 323
 324   /** Matrix vector multiplication.
 325
 326       @param v vector to multiply with.
 327       @return matric vector product.
 328
 329       Compute and return the product of the current matrix with the
 330       vector given in the argument.
 331    */
 332   FGColumnVector3 operator*(const FGColumnVector3& v) const;
 333
 334   /** Matrix subtraction.
 335
 336       @param B matrix to add to.
 337       @return difference of the matrices.
 338
 339       Compute and return the sum of the current matrix and the matrix
 340       B given in the argument.
 341   */
 342   FGMatrix33 operator-(const FGMatrix33& B) const;
 343
 344   /** Matrix addition.
 345
 346       @param B matrix to add to.
 347       @return sum of the matrices.
 348
 349       Compute and return the sum of the current matrix and the matrix
 350       B given in the argument.
 351   */
 352   FGMatrix33 operator+(const FGMatrix33& B) const;
 353
 354   /** Matrix product.
 355
 356       @param B matrix to add to.
 357       @return product of the matrices.
 358
 359       Compute and return the product of the current matrix and the matrix
 360       B given in the argument.
 361   */
 362   FGMatrix33 operator*(const FGMatrix33& B) const;
 363
 364   /** Multiply the matrix with a scalar.
 365
 366       @param scalar scalar factor to multiply with.
 367       @return scaled matrix.
 368
 369       Compute and return the product of the current matrix with the
 370       scalar value scalar given in the argument.
 371   */
 372   FGMatrix33 operator*(const double scalar) const;
 373
 374   /** Multiply the matrix with 1.0/scalar.
 375
 376       @param scalar scalar factor to divide through.
 377       @return scaled matrix.
 378
 379       Compute and return the product of the current matrix with the
 380       scalar value 1.0/scalar, where scalar is given in the argument.
 381   */
 382   FGMatrix33 operator/(const double scalar) const;
 383
 384   /** In place matrix subtraction.
 385
 386       @param B matrix to subtract.
 387       @return reference to the current matrix.
 388
 389       Compute the diffence from the current matrix and the matrix B
 390       given in the argument.
 391   */
 392   FGMatrix33& operator-=(const FGMatrix33 &B);
 393
 394   /** In place matrix addition.
 395
 396       @param B matrix to add.
 397       @return reference to the current matrix.
 398
 399       Compute the sum of the current matrix and the matrix B
 400       given in the argument.
 401   */
 402   FGMatrix33& operator+=(const FGMatrix33 &B);
 403
 404   /** In place matrix multiplication.
 405
 406       @param B matrix to multiply with.
 407       @return reference to the current matrix.
 408
 409       Compute the product of the current matrix and the matrix B
 410       given in the argument.
 411   */
 412   FGMatrix33& operator*=(const FGMatrix33 &B);
 413
 414   /** In place matrix scale.
 415
 416       @param scalar scalar value to multiply with.
 417       @return reference to the current matrix.
 418
 419       Compute the product of the current matrix and the scalar value scalar
 420       given in the argument.
 421   */
 422   FGMatrix33& operator*=(const double scalar);
 423
 424   /** In place matrix scale.
 425
 426       @param scalar scalar value to divide through.
 427       @return reference to the current matrix.
 428
 429       Compute the product of the current matrix and the scalar value
 430       1.0/scalar, where scalar is given in the argument.
 431   */
 432   FGMatrix33& operator/=(const double scalar);
 433
 434 private:
 435   double data[eRows*eColumns];
 436
 437   void Debug(int from);
 438 };
 439
 440 /** Scalar multiplication.
 441
 442     @param scalar scalar value to multiply with.
 443     @param A Matrix to multiply.
 444
 445     Multiply the Matrix with a scalar value.
 446 */
 447 inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
 448   // use already defined operation.
 449   return A*scalar;
 450 }
 451
 452 /** Write matrix to a stream.
 453
 454     @param os Stream to write to.
 455     @param M Matrix to write.
 456
 457     Write the matrix to a stream.
 458 */
 459 std::ostream& operator<<(std::ostream& os, const FGMatrix33& M);
 460
 461 /** Read matrix from a stream.
 462
 463     @param os Stream to read from.
 464     @param M Matrix to initialize with the values from the stream.
 465
 466     Read matrix from a stream.
 467 */
 468 std::istream& operator>>(std::istream& is, FGMatrix33& M);
 469
 470 } // namespace JSBSim
 471
 472 #include "FGQuaternion.h"
 473
 474 #endif