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 (jsb@hal-pc.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 <stdlib.h>
  44 #ifdef FGFS
  45 #  include <math.h>
  46 #  include <simgear/compiler.h>
  47 #  include STL_STRING
  48 #  include STL_FSTREAM
  49 #  include STL_IOSTREAM
  50    SG_USING_STD(string);
  51    SG_USING_STD(ostream);
  52    SG_USING_STD(istream);
  53    SG_USING_STD(cerr);
  54    SG_USING_STD(cout);
  55    SG_USING_STD(endl);
  56 #else
  57 #  include <string>
  58 #  if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
  59      include <fstream.h>
  60      include <iostream.h>
  61 #    include <math.h>
  62 #  else
  63 #    include <fstream>
  64 #    include <iostream>
  65 #    if defined(sgi) && !defined(__GNUC__)
  66 #      include <math.h>
  67 #    else
  68 #      include <cmath>
  69 #    endif
  70      using std::ostream;
  71      using std::istream;
  72      using std::cerr;
  73      using std::cout;
  74      using std::endl;
  75 #  endif
  76    using std::string;
  77 #endif
  78
  79 #include "FGColumnVector3.h"
  80 #include "FGJSBBase.h"
  81
  82 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  83 DEFINITIONS
  84 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  85
  86 #define ID_MATRIX33 "$Id$"
  87
  88 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  89 FORWARD DECLARATIONS
  90 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  91
  92 namespace JSBSim {
  93
  94 class FGColumnVector3;
  95
  96 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  97 CLASS DOCUMENTATION
  98 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  99
 100 /** Exception convenience class.
 101   */
 102
 103 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 104 DECLARATION: MatrixException
 105 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 106
 107 class MatrixException : public FGJSBBase
 108 {
 109 public:
 110   string Message;
 111 };
 112
 113 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 114 CLASS DOCUMENTATION
 115 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 116
 117   /** Handles matrix math operations.
 118       @author Tony Peden, Jon Berndt, Mathias Froelich
 119   */
 120
 121 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 122 DECLARATION: FGMatrix33
 123 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 124
 125 class FGMatrix33 : public FGJSBBase
 126 {
 127 public:
 128
 129   enum {
 130     eRows = 3,
 131     eColumns = 3
 132   };
 133
 134   /** Default initializer.
 135
 136       Create a zero matrix.
 137    */
 138   FGMatrix33(void);
 139
 140   /** Copy constructor.
 141
 142       @param M Matrix which is used for initialization.
 143
 144       Create copy of the matrix given in the argument.
 145    */
 146   FGMatrix33(const FGMatrix33& M) {
 147     Entry(1,1) = M.Entry(1,1);
 148     Entry(2,1) = M.Entry(2,1);
 149     Entry(3,1) = M.Entry(3,1);
 150     Entry(1,2) = M.Entry(1,2);
 151     Entry(2,2) = M.Entry(2,2);
 152     Entry(3,2) = M.Entry(3,2);
 153     Entry(1,3) = M.Entry(1,3);
 154     Entry(2,3) = M.Entry(2,3);
 155     Entry(3,3) = M.Entry(3,3);
 156
 157     Debug(0);
 158   }
 159
 160   /** Initialization by given values.
 161
 162       @param m11 value of the 1,1 Matrix element.
 163       @param m12 value of the 1,2 Matrix element.
 164       @param m13 value of the 1,3 Matrix element.
 165       @param m21 value of the 2,1 Matrix element.
 166       @param m22 value of the 2,2 Matrix element.
 167       @param m23 value of the 2,3 Matrix element.
 168       @param m31 value of the 3,1 Matrix element.
 169       @param m32 value of the 3,2 Matrix element.
 170       @param m33 value of the 3,3 Matrix element.
 171
 172       Create a matrix from the doubles given in the arguments.
 173    */
 174   FGMatrix33(double m11, double m12, double m13,
 175              double m21, double m22, double m23,
 176              double m31, double m32, double m33) {
 177     Entry(1,1) = m11;
 178     Entry(2,1) = m21;
 179     Entry(3,1) = m31;
 180     Entry(1,2) = m12;
 181     Entry(2,2) = m22;
 182     Entry(3,2) = m32;
 183     Entry(1,3) = m13;
 184     Entry(2,3) = m23;
 185     Entry(3,3) = m33;
 186
 187     Debug(0);
 188   }
 189
 190   /** Destructor.
 191    */
 192   ~FGMatrix33(void) { Debug(1); }
 193
 194   /** Read access the entries of the matrix.
 195       @param row Row index.
 196       @param col Column index.
 197
 198       @return the value of the matrix entry at the given row and
 199       column indices. Indices are counted starting with 1.
 200    */
 201   double operator()(unsigned int row, unsigned int col) const {
 202     return Entry(row, col);
 203   }
 204
 205   /** Write access the entries of the matrix.
 206       Note that the indices given in the arguments are unchecked.
 207
 208       @param row Row index.
 209       @param col Column index.
 210
 211       @return a reference to the matrix entry at the given row and
 212       column indices. Indices are counted starting with 1.
 213    */
 214   double& operator()(unsigned int row, unsigned int col) {
 215     return Entry(row, col);
 216   }
 217
 218   /** Read access the entries of the matrix.
 219       This function is just a shortcut for the @ref double&
 220       operator()(unsigned int row, unsigned int col) function. It is
 221       used internally to access the elements in a more convenient way.
 222
 223       Note that the indices given in the arguments are unchecked.
 224
 225       @param row Row index.
 226       @param col Column index.
 227
 228       @return the value of the matrix entry at the given row and
 229       column indices. Indices are counted starting with 1.
 230    */
 231   double Entry(unsigned int row, unsigned int col) const {
 232     return data[(col-1)*eRows+row-1];
 233   }
 234
 235   /** Write access the entries of the matrix.
 236       This function is just a shortcut for the @ref double&
 237       operator()(unsigned int row, unsigned int col) function. It is
 238       used internally to access the elements in a more convenient way.
 239
 240       Note that the indices given in the arguments are unchecked.
 241
 242       @param row Row index.
 243       @param col Column index.
 244
 245       @return a reference to the matrix entry at the given row and
 246       column indices. Indices are counted starting with 1.
 247    */
 248    double& Entry(unsigned int row, unsigned int col) {
 249      return data[(col-1)*eRows+row-1];
 250    }
 251
 252   /** Number of rows in the matrix.
 253       @return the number of rows in the matrix.
 254    */
 255    unsigned int Rows(void) const { return eRows; }
 256
 257   /** Number of cloumns in the matrix.
 258       @return the number of columns in the matrix.
 259    */
 260    unsigned int Cols(void) const { return eColumns; }
 261
 262   /** Transposed matrix.
 263       This function only returns the transpose of this matrix. This matrix itself
 264       remains unchanged.
 265       @return the transposed matrix.
 266    */
 267   FGMatrix33 Transposed(void) const {
 268     return FGMatrix33( Entry(1,1), Entry(2,1), Entry(3,1),
 269                        Entry(1,2), Entry(2,2), Entry(3,2),
 270                        Entry(1,3), Entry(2,3), Entry(3,3) );
 271   }
 272
 273   /** Transposes this matrix.
 274       This function only transposes this matrix. Nothing is returned.
 275    */
 276   void T(void);
 277
 278 /** Initialize the matrix.
 279     This function initializes a matrix to all 0.0.
 280  */
 281   void InitMatrix(void);
 282
 283 /** Initialize the matrix.
 284     This function initializes a matrix to user specified values.
 285  */
 286   void InitMatrix(double m11, double m12, double m13,
 287                   double m21, double m22, double m23,
 288                   double m31, double m32, double m33) {
 289     Entry(1,1) = m11;
 290     Entry(2,1) = m21;
 291     Entry(3,1) = m31;
 292     Entry(1,2) = m12;
 293     Entry(2,2) = m22;
 294     Entry(3,2) = m32;
 295     Entry(1,3) = m13;
 296     Entry(2,3) = m23;
 297     Entry(3,3) = m33;
 298   }
 299
 300   /** Determinant of the matrix.
 301       @return the determinant of the matrix.
 302    */
 303   double Determinant(void) const;
 304
 305   /** Return if the matrix is invertible.
 306       Checks and returns if the matrix is nonsingular and thus
 307       invertible. This is done by simply computing the determinant and
 308       check if it is zero. Note that this test does not cover any
 309       instabilities caused by nearly singular matirces using finite
 310       arithmetics. It only checks exact singularity.
 311    */
 312   bool Invertible(void) const { return 0.0 != Determinant(); }
 313
 314   /** Return the inverse of the matrix.
 315       Computes and returns if the inverse of the matrix. It is computed
 316       by Cramers Rule. Also there are no checks performed if the matrix
 317       is invertible. If you are not sure that it really is check this
 318       with the @ref Invertible() call before.
 319    */
 320   FGMatrix33 Inverse(void) const;
 321
 322   /** Assignment operator.
 323
 324       @param A source matrix.
 325
 326       Copy the content of the matrix given in the argument into *this.
 327    */
 328   FGMatrix33& operator=(const FGMatrix33& A) {
 329     data[0] = A.data[0];
 330     data[1] = A.data[1];
 331     data[2] = A.data[2];
 332     data[3] = A.data[3];
 333     data[4] = A.data[4];
 334     data[5] = A.data[5];
 335     data[6] = A.data[6];
 336     data[7] = A.data[7];
 337     data[8] = A.data[8];
 338     return *this;
 339   }
 340
 341   /** Matrix vector multiplication.
 342
 343       @param v vector to multiply with.
 344       @return matric vector product.
 345
 346       Compute and return the product of the current matrix with the
 347       vector given in the argument.
 348    */
 349   FGColumnVector3 operator*(const FGColumnVector3& v) const;
 350
 351   /** Matrix subtraction.
 352
 353       @param B matrix to add to.
 354       @return difference of the matrices.
 355
 356       Compute and return the sum of the current matrix and the matrix
 357       B given in the argument.
 358   */
 359   FGMatrix33 operator-(const FGMatrix33& B) const;
 360
 361   /** Matrix addition.
 362
 363       @param B matrix to add to.
 364       @return sum of the matrices.
 365
 366       Compute and return the sum of the current matrix and the matrix
 367       B given in the argument.
 368   */
 369   FGMatrix33 operator+(const FGMatrix33& B) const;
 370
 371   /** Matrix product.
 372
 373       @param B matrix to add to.
 374       @return product of the matrices.
 375
 376       Compute and return the product of the current matrix and the matrix
 377       B given in the argument.
 378   */
 379   FGMatrix33 operator*(const FGMatrix33& B) const;
 380
 381   /** Multiply the matrix with a scalar.
 382
 383       @param scalar scalar factor to multiply with.
 384       @return scaled matrix.
 385
 386       Compute and return the product of the current matrix with the
 387       scalar value scalar given in the argument.
 388   */
 389   FGMatrix33 operator*(const double scalar) const;
 390
 391   /** Multiply the matrix with 1.0/scalar.
 392
 393       @param scalar scalar factor to divide through.
 394       @return scaled matrix.
 395
 396       Compute and return the product of the current matrix with the
 397       scalar value 1.0/scalar, where scalar is given in the argument.
 398   */
 399   FGMatrix33 operator/(const double scalar) const;
 400
 401   /** In place matrix subtraction.
 402
 403       @param B matrix to subtract.
 404       @return reference to the current matrix.
 405
 406       Compute the diffence from the current matrix and the matrix B
 407       given in the argument.
 408   */
 409   FGMatrix33& operator-=(const FGMatrix33 &B);
 410
 411   /** In place matrix addition.
 412
 413       @param B matrix to add.
 414       @return reference to the current matrix.
 415
 416       Compute the sum of the current matrix and the matrix B
 417       given in the argument.
 418   */
 419   FGMatrix33& operator+=(const FGMatrix33 &B);
 420
 421   /** In place matrix multiplication.
 422
 423       @param B matrix to multiply with.
 424       @return reference to the current matrix.
 425
 426       Compute the product of the current matrix and the matrix B
 427       given in the argument.
 428   */
 429   FGMatrix33& operator*=(const FGMatrix33 &B);
 430
 431   /** In place matrix scale.
 432
 433       @param scalar scalar value to multiply with.
 434       @return reference to the current matrix.
 435
 436       Compute the product of the current matrix and the scalar value scalar
 437       given in the argument.
 438   */
 439   FGMatrix33& operator*=(const double scalar);
 440
 441   /** In place matrix scale.
 442
 443       @param scalar scalar value to divide through.
 444       @return reference to the current matrix.
 445
 446       Compute the product of the current matrix and the scalar value
 447       1.0/scalar, where scalar is given in the argument.
 448   */
 449   FGMatrix33& operator/=(const double scalar);
 450
 451 private:
 452   double data[eRows*eColumns];
 453
 454   void Debug(int from);
 455 };
 456
 457 /** Scalar multiplication.
 458
 459     @param scalar scalar value to multiply with.
 460     @param A Matrix to multiply.
 461
 462     Multiply the Matrix with a scalar value.
 463 */
 464 inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
 465   // use already defined operation.
 466   return A*scalar;
 467 }
 468
 469 /** Write matrix to a stream.
 470
 471     @param os Stream to write to.
 472     @param M Matrix to write.
 473
 474     Write the matrix to a stream.
 475 */
 476 ostream& operator<<(ostream& os, const FGMatrix33& M);
 477
 478 /** Read matrix from a stream.
 479
 480     @param os Stream to read from.
 481     @param M Matrix to initialize with the values from the stream.
 482
 483     Read matrix from a stream.
 484 */
 485 istream& operator>>(istream& is, FGMatrix33& M);
 486
 487 } // namespace JSBSim
 488
 489 #endif