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