src/FDM/JSBSim/math/FGMatrix33.cpp

   1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   2
   3 Module: FGMatrix33.cpp
   4 Author: Tony Peden, Jon Berndt, Mathias Frolich
   5 Date started: 1998
   6 Purpose: FGMatrix33 class
   7 Called by: Various
   8
   9  ------------- Copyright (C) 1998 by the authors above -------------
  10
  11  This program is free software; you can redistribute it and/or modify it under
  12  the terms of the GNU Lesser General Public License as published by the Free Software
  13  Foundation; either version 2 of the License, or (at your option) any later
  14  version.
  15
  16  This program is distributed in the hope that it will be useful, but WITHOUT
  17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  18  FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
  19  details.
  20
  21  You should have received a copy of the GNU Lesser General Public License along with
  22  this program; if not, write to the Free Software Foundation, Inc., 59 Temple
  23  Place - Suite 330, Boston, MA  02111-1307, USA.
  24
  25  Further information about the GNU Lesser General Public License can also be found on
  26  the world wide web at http://www.gnu.org.
  27
  28 FUNCTIONAL DESCRIPTION
  29 --------------------------------------------------------------------------------
  30
  31 HISTORY
  32 --------------------------------------------------------------------------------
  33 ??/??/??   TP   Created
  34 03/16/2000 JSB  Added exception throwing
  35
  36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  37 INCLUDES
  38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  39
  40 #include "FGMatrix33.h"
  41 #include "FGColumnVector3.h"
  42 #include <sstream>
  43 #include <iomanip>
  44
  45 #include <iostream>
  46
  47 using namespace std;
  48
  49 namespace JSBSim {
  50
  51 static const char *IdSrc = "$Id$";
  52 static const char *IdHdr = ID_MATRIX33;
  53
  54 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  55 CLASS IMPLEMENTATION
  56 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  57
  58 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  59
  60 FGMatrix33::FGMatrix33(void)
  61 {
  62   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
  63     data[6] = data[7] = data[8] = 0.0;
  64
  65   // Debug(0);
  66 }
  67
  68 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  69
  70 string FGMatrix33::Dump(const string& delimiter) const
  71 {
  72   ostringstream buffer;
  73   buffer << std::setw(18) << std::setprecision(16) << Entry(1,1) << delimiter;
  74   buffer << std::setw(18) << std::setprecision(16) << Entry(1,2) << delimiter;
  75   buffer << std::setw(18) << std::setprecision(16) << Entry(1,3) << delimiter;
  76   buffer << std::setw(18) << std::setprecision(16) << Entry(2,1) << delimiter;
  77   buffer << std::setw(18) << std::setprecision(16) << Entry(2,2) << delimiter;
  78   buffer << std::setw(18) << std::setprecision(16) << Entry(2,3) << delimiter;
  79   buffer << std::setw(18) << std::setprecision(16) << Entry(3,1) << delimiter;
  80   buffer << std::setw(18) << std::setprecision(16) << Entry(3,2) << delimiter;
  81   buffer << std::setw(18) << std::setprecision(16) << Entry(3,3);
  82   return buffer.str();
  83 }
  84
  85 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  86
  87 ostream& operator<<(ostream& os, const FGMatrix33& M)
  88 {
  89   for (unsigned int i=1; i<=M.Rows(); i++) {
  90     for (unsigned int j=1; j<=M.Cols(); j++) {
  91       if (i == M.Rows() && j == M.Cols())
  92         os << M(i,j);
  93       else
  94         os << M(i,j) << ", ";
  95     }
  96   }
  97   return os;
  98 }
  99
 100 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 101
 102 istream& operator>>(istream& is, FGMatrix33& M)
 103 {
 104   for (unsigned int i=1; i<=M.Rows(); i++) {
 105     for (unsigned int j=1; j<=M.Cols(); j++) {
 106       is >> M(i,j);
 107     }
 108   }
 109   return is;
 110 }
 111
 112 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 113
 114 double FGMatrix33::Determinant(void) const {
 115   return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
 116     + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
 117     - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
 118 }
 119
 120 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 121
 122 FGMatrix33 FGMatrix33::Inverse(void) const {
 123   // Compute the inverse of a general matrix using Cramers rule.
 124   // I guess googling for cramers rule gives tons of references
 125   // for this. :)
 126   double rdet = 1.0/Determinant();
 127
 128   double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
 129   double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
 130   double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
 131   double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
 132   double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
 133   double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
 134   double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
 135   double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
 136   double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
 137
 138   return FGMatrix33( i11, i12, i13,
 139                      i21, i22, i23,
 140                      i31, i32, i33 );
 141 }
 142
 143 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 144
 145 void FGMatrix33::InitMatrix(void)
 146 {
 147   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
 148     data[6] = data[7] = data[8] = 0.0;
 149 }
 150
 151 // *****************************************************************************
 152 // binary operators ************************************************************
 153 // *****************************************************************************
 154
 155 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
 156 {
 157   return FGMatrix33( Entry(1,1) - M(1,1),
 158                      Entry(1,2) - M(1,2),
 159                      Entry(1,3) - M(1,3),
 160                      Entry(2,1) - M(2,1),
 161                      Entry(2,2) - M(2,2),
 162                      Entry(2,3) - M(2,3),
 163                      Entry(3,1) - M(3,1),
 164                      Entry(3,2) - M(3,2),
 165                      Entry(3,3) - M(3,3) );
 166 }
 167
 168 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 169
 170 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
 171 {
 172   data[0] -= M.data[0];
 173   data[1] -= M.data[1];
 174   data[2] -= M.data[2];
 175   data[3] -= M.data[3];
 176   data[4] -= M.data[4];
 177   data[5] -= M.data[5];
 178   data[6] -= M.data[6];
 179   data[7] -= M.data[7];
 180   data[8] -= M.data[8];
 181
 182   return *this;
 183 }
 184
 185 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 186
 187 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
 188 {
 189   return FGMatrix33( Entry(1,1) + M(1,1),
 190                      Entry(1,2) + M(1,2),
 191                      Entry(1,3) + M(1,3),
 192                      Entry(2,1) + M(2,1),
 193                      Entry(2,2) + M(2,2),
 194                      Entry(2,3) + M(2,3),
 195                      Entry(3,1) + M(3,1),
 196                      Entry(3,2) + M(3,2),
 197                      Entry(3,3) + M(3,3) );
 198 }
 199
 200 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 201
 202 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
 203 {
 204   Entry(1,1) += M(1,1);
 205   Entry(1,2) += M(1,2);
 206   Entry(1,3) += M(1,3);
 207   Entry(2,1) += M(2,1);
 208   Entry(2,2) += M(2,2);
 209   Entry(2,3) += M(2,3);
 210   Entry(3,1) += M(3,1);
 211   Entry(3,2) += M(3,2);
 212   Entry(3,3) += M(3,3);
 213
 214   return *this;
 215 }
 216
 217 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 218
 219 FGMatrix33 FGMatrix33::operator*(const double scalar) const
 220 {
 221   return FGMatrix33( scalar * Entry(1,1),
 222                      scalar * Entry(1,2),
 223                      scalar * Entry(1,3),
 224                      scalar * Entry(2,1),
 225                      scalar * Entry(2,2),
 226                      scalar * Entry(2,3),
 227                      scalar * Entry(3,1),
 228                      scalar * Entry(3,2),
 229                      scalar * Entry(3,3) );
 230 }
 231
 232 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 233
 234 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
 235 {
 236   return FGMatrix33( scalar * M(1,1),
 237                      scalar * M(1,2),
 238                      scalar * M(1,3),
 239                      scalar * M(2,1),
 240                      scalar * M(2,2),
 241                      scalar * M(2,3),
 242                      scalar * M(3,1),
 243                      scalar * M(3,2),
 244                      scalar * M(3,3) );
 245 }
 246
 247 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 248
 249 FGMatrix33& FGMatrix33::operator*=(const double scalar)
 250 {
 251   Entry(1,1) *= scalar;
 252   Entry(1,2) *= scalar;
 253   Entry(1,3) *= scalar;
 254   Entry(2,1) *= scalar;
 255   Entry(2,2) *= scalar;
 256   Entry(2,3) *= scalar;
 257   Entry(3,1) *= scalar;
 258   Entry(3,2) *= scalar;
 259   Entry(3,3) *= scalar;
 260
 261   return *this;
 262 }
 263
 264 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 265
 266 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
 267 {
 268   // FIXME: Make compiler friendlier
 269   FGMatrix33 Product;
 270
 271   Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
 272   Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
 273   Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
 274   Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
 275   Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
 276   Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
 277   Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
 278   Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
 279   Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
 280
 281   return Product;
 282 }
 283
 284 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 285
 286 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
 287 {
 288   // FIXME: Make compiler friendlier
 289   double a,b,c;
 290
 291   a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
 292   Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 293   Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 294   Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 295
 296   a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
 297   Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 298   Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 299   Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 300
 301   a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
 302   Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
 303   Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
 304   Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
 305
 306   return *this;
 307 }
 308
 309 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 310
 311 FGMatrix33 FGMatrix33::operator/(const double scalar) const
 312 {
 313   FGMatrix33 Quot;
 314
 315   if ( scalar != 0 ) {
 316     double tmp = 1.0/scalar;
 317     Quot(1,1) = Entry(1,1) * tmp;
 318     Quot(1,2) = Entry(1,2) * tmp;
 319     Quot(1,3) = Entry(1,3) * tmp;
 320     Quot(2,1) = Entry(2,1) * tmp;
 321     Quot(2,2) = Entry(2,2) * tmp;
 322     Quot(2,3) = Entry(2,3) * tmp;
 323     Quot(3,1) = Entry(3,1) * tmp;
 324     Quot(3,2) = Entry(3,2) * tmp;
 325     Quot(3,3) = Entry(3,3) * tmp;
 326   } else {
 327     MatrixException mE;
 328     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
 329     throw mE;
 330   }
 331   return Quot;
 332 }
 333
 334 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 335
 336 FGMatrix33& FGMatrix33::operator/=(const double scalar)
 337 {
 338   if ( scalar != 0 ) {
 339     double tmp = 1.0/scalar;
 340     Entry(1,1) *= tmp;
 341     Entry(1,2) *= tmp;
 342     Entry(1,3) *= tmp;
 343     Entry(2,1) *= tmp;
 344     Entry(2,2) *= tmp;
 345     Entry(2,3) *= tmp;
 346     Entry(3,1) *= tmp;
 347     Entry(3,2) *= tmp;
 348     Entry(3,3) *= tmp;
 349   } else {
 350     MatrixException mE;
 351     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
 352     throw mE;
 353   }
 354   return *this;
 355 }
 356
 357 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 358
 359 void FGMatrix33::T(void)
 360 {
 361   for (unsigned int i=1; i<=3; i++) {
 362     for (unsigned int j=i+1; j<=3; j++) {
 363       double tmp = Entry(i,j);
 364       Entry(i,j) = Entry(j,i);
 365       Entry(j,i) = tmp;
 366     }
 367   }
 368 }
 369
 370 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 371
 372 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
 373   double tmp1 = v(1)*Entry(1,1);
 374   double tmp2 = v(1)*Entry(2,1);
 375   double tmp3 = v(1)*Entry(3,1);
 376
 377   tmp1 += v(2)*Entry(1,2);
 378   tmp2 += v(2)*Entry(2,2);
 379   tmp3 += v(2)*Entry(3,2);
 380
 381   tmp1 += v(3)*Entry(1,3);
 382   tmp2 += v(3)*Entry(2,3);
 383   tmp3 += v(3)*Entry(3,3);
 384
 385   return FGColumnVector3( tmp1, tmp2, tmp3 );
 386 }
 387
 388 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 389 //    The bitmasked value choices are as follows:
 390 //    unset: In this case (the default) JSBSim would only print
 391 //       out the normally expected messages, essentially echoing
 392 //       the config files as they are read. If the environment
 393 //       variable is not set, debug_lvl is set to 1 internally
 394 //    0: This requests JSBSim not to output any messages
 395 //       whatsoever.
 396 //    1: This value explicity requests the normal JSBSim
 397 //       startup messages
 398 //    2: This value asks for a message to be printed out when
 399 //       a class is instantiated
 400 //    4: When this value is set, a message is displayed when a
 401 //       FGModel object executes its Run() method
 402 //    8: When this value is set, various runtime state variables
 403 //       are printed out periodically
 404 //    16: When set various parameters are sanity checked and
 405 //       a message is printed out when they go out of bounds
 406
 407 void FGMatrix33::Debug(int from)
 408 {
 409   if (debug_lvl <= 0) return;
 410
 411   if (debug_lvl & 1) { // Standard console startup message output
 412     if (from == 0) { // Constructor
 413
 414     }
 415   }
 416   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 417     if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
 418     if (from == 1) cout << "Destroyed:    FGMatrix33" << endl;
 419   }
 420   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 421   }
 422   if (debug_lvl & 8 ) { // Runtime state variables
 423   }
 424   if (debug_lvl & 16) { // Sanity checking
 425   }
 426   if (debug_lvl & 64) {
 427     if (from == 0) { // Constructor
 428       cout << IdSrc << endl;
 429       cout << IdHdr << endl;
 430     }
 431   }
 432 }
 433 }