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: FGMatrix33.cpp,v 1.10 2010/07/01 23:13:19 jberndt Exp $";
  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 << setw(12) << setprecision(10) << data[0] << delimiter;
  74   buffer << setw(12) << setprecision(10) << data[3] << delimiter;
  75   buffer << setw(12) << setprecision(10) << data[6] << delimiter;
  76   buffer << setw(12) << setprecision(10) << data[1] << delimiter;
  77   buffer << setw(12) << setprecision(10) << data[4] << delimiter;
  78   buffer << setw(12) << setprecision(10) << data[7] << delimiter;
  79   buffer << setw(12) << setprecision(10) << data[2] << delimiter;
  80   buffer << setw(12) << setprecision(10) << data[5] << delimiter;
  81   buffer << setw(12) << setprecision(10) << data[8];
  82   return buffer.str();
  83 }
  84
  85 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  86
  87 string FGMatrix33::Dump(const string& delimiter, const string& prefix) const
  88 {
  89   ostringstream buffer;
  90
  91   buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[0] << delimiter;
  92   buffer << right << fixed << setw(9) << setprecision(6) << data[3] << delimiter;
  93   buffer << right << fixed << setw(9) << setprecision(6) << data[6] << endl;
  94
  95   buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[1] << delimiter;
  96   buffer << right << fixed << setw(9) << setprecision(6) << data[4] << delimiter;
  97   buffer << right << fixed << setw(9) << setprecision(6) << data[7] << endl;
  98
  99   buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[2] << delimiter;
 100   buffer << right << fixed << setw(9) << setprecision(6) << data[5] << delimiter;
 101   buffer << right << fixed << setw(9) << setprecision(6) << data[8];
 102
 103   buffer << setw(0) << left;
 104
 105   return buffer.str();
 106 }
 107
 108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 109
 110 FGQuaternion FGMatrix33::GetQuaternion(void)
 111 {
 112   FGQuaternion Q;
 113
 114   double tempQ[4];
 115   int idx;
 116
 117   tempQ[0] = 1.0 + data[0] + data[4] + data[8];
 118   tempQ[1] = 1.0 + data[0] - data[4] - data[8];
 119   tempQ[2] = 1.0 - data[0] + data[4] - data[8];
 120   tempQ[3] = 1.0 - data[0] - data[4] + data[8];
 121
 122   // Find largest of the above
 123   idx = 0;
 124   for (int i=1; i<4; i++) if (tempQ[i] > tempQ[idx]) idx = i;
 125
 126   switch(idx) {
 127     case 0:
 128       Q(1) = 0.50*sqrt(tempQ[0]);
 129       Q(2) = 0.25*(data[7] - data[5])/Q(1);
 130       Q(3) = 0.25*(data[2] - data[6])/Q(1);
 131       Q(4) = 0.25*(data[3] - data[1])/Q(1);
 132       break;
 133     case 1:
 134       Q(2) = 0.50*sqrt(tempQ[1]);
 135       Q(1) = 0.25*(data[7] - data[5])/Q(2);
 136       Q(3) = 0.25*(data[3] + data[1])/Q(2);
 137       Q(4) = 0.25*(data[2] + data[6])/Q(2);
 138       break;
 139     case 2:
 140       Q(3) = 0.50*sqrt(tempQ[2]);
 141       Q(1) = 0.25*(data[2] - data[6])/Q(3);
 142       Q(2) = 0.25*(data[3] + data[1])/Q(3);
 143       Q(4) = 0.25*(data[7] + data[5])/Q(3);
 144       break;
 145     case 3:
 146       Q(4) = 0.50*sqrt(tempQ[3]);
 147       Q(1) = 0.25*(data[3] - data[1])/Q(4);
 148       Q(2) = 0.25*(data[6] + data[2])/Q(4);
 149       Q(3) = 0.25*(data[7] + data[5])/Q(4);
 150       break;
 151     default:
 152       //error
 153       break;
 154   }
 155
 156   return (Q);
 157 }
 158
 159 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 160
 161 ostream& operator<<(ostream& os, const FGMatrix33& M)
 162 {
 163   for (unsigned int i=1; i<=M.Rows(); i++) {
 164     for (unsigned int j=1; j<=M.Cols(); j++) {
 165       if (i == M.Rows() && j == M.Cols())
 166         os << M(i,j);
 167       else
 168         os << M(i,j) << ", ";
 169     }
 170   }
 171   return os;
 172 }
 173
 174 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 175
 176 istream& operator>>(istream& is, FGMatrix33& M)
 177 {
 178   for (unsigned int i=1; i<=M.Rows(); i++) {
 179     for (unsigned int j=1; j<=M.Cols(); j++) {
 180       is >> M(i,j);
 181     }
 182   }
 183   return is;
 184 }
 185
 186 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 187
 188 double FGMatrix33::Determinant(void) const {
 189   return data[0]*data[4]*data[8] + data[3]*data[7]*data[2]
 190        + data[6]*data[1]*data[5] - data[6]*data[4]*data[2]
 191        - data[3]*data[1]*data[8] - data[7]*data[5]*data[0];
 192 }
 193
 194 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 195
 196 FGMatrix33 FGMatrix33::Inverse(void) const {
 197   // Compute the inverse of a general matrix using Cramers rule.
 198   // I guess googling for cramers rule gives tons of references
 199   // for this. :)
 200
 201   if (Determinant() != 0.0) {
 202     double rdet = 1.0/Determinant();
 203
 204     double i11 = rdet*(data[4]*data[8]-data[7]*data[5]);
 205     double i21 = rdet*(data[7]*data[2]-data[1]*data[8]);
 206     double i31 = rdet*(data[1]*data[5]-data[4]*data[2]);
 207     double i12 = rdet*(data[6]*data[5]-data[3]*data[8]);
 208     double i22 = rdet*(data[0]*data[8]-data[6]*data[2]);
 209     double i32 = rdet*(data[3]*data[2]-data[0]*data[5]);
 210     double i13 = rdet*(data[3]*data[7]-data[6]*data[4]);
 211     double i23 = rdet*(data[6]*data[1]-data[0]*data[7]);
 212     double i33 = rdet*(data[0]*data[4]-data[3]*data[1]);
 213
 214     return FGMatrix33( i11, i12, i13,
 215                        i21, i22, i23,
 216                        i31, i32, i33 );
 217   } else {
 218     return FGMatrix33( 0, 0, 0,
 219                        0, 0, 0,
 220                        0, 0, 0 );
 221   }
 222 }
 223
 224 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 225
 226 void FGMatrix33::InitMatrix(void)
 227 {
 228   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
 229     data[6] = data[7] = data[8] = 0.0;
 230 }
 231
 232 // *****************************************************************************
 233 // binary operators ************************************************************
 234 // *****************************************************************************
 235
 236 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
 237 {
 238   return FGMatrix33( data[0] - M.data[0],
 239                      data[3] - M.data[3],
 240                      data[6] - M.data[6],
 241                      data[1] - M.data[1],
 242                      data[4] - M.data[4],
 243                      data[7] - M.data[7],
 244                      data[2] - M.data[2],
 245                      data[5] - M.data[5],
 246                      data[8] - M.data[8] );
 247 }
 248
 249 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 250
 251 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
 252 {
 253   data[0] -= M.data[0];
 254   data[1] -= M.data[1];
 255   data[2] -= M.data[2];
 256   data[3] -= M.data[3];
 257   data[4] -= M.data[4];
 258   data[5] -= M.data[5];
 259   data[6] -= M.data[6];
 260   data[7] -= M.data[7];
 261   data[8] -= M.data[8];
 262
 263   return *this;
 264 }
 265
 266 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 267
 268 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
 269 {
 270   return FGMatrix33( data[0] + M.data[0],
 271                      data[3] + M.data[3],
 272                      data[6] + M.data[6],
 273                      data[1] + M.data[1],
 274                      data[4] + M.data[4],
 275                      data[7] + M.data[7],
 276                      data[2] + M.data[2],
 277                      data[5] + M.data[5],
 278                      data[8] + M.data[8] );
 279 }
 280
 281 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 282
 283 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
 284 {
 285   data[0] += M.data[0];
 286   data[3] += M.data[3];
 287   data[6] += M.data[6];
 288   data[1] += M.data[1];
 289   data[4] += M.data[4];
 290   data[7] += M.data[7];
 291   data[2] += M.data[2];
 292   data[5] += M.data[5];
 293   data[8] += M.data[8];
 294
 295   return *this;
 296 }
 297
 298 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 299
 300 FGMatrix33 FGMatrix33::operator*(const double scalar) const
 301 {
 302   return FGMatrix33( scalar * data[0],
 303                      scalar * data[3],
 304                      scalar * data[6],
 305                      scalar * data[1],
 306                      scalar * data[4],
 307                      scalar * data[7],
 308                      scalar * data[2],
 309                      scalar * data[5],
 310                      scalar * data[8] );
 311 }
 312
 313 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 314 /*
 315 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
 316 {
 317   return FGMatrix33( scalar * M(1,1),
 318                      scalar * M(1,2),
 319                      scalar * M(1,3),
 320                      scalar * M(2,1),
 321                      scalar * M(2,2),
 322                      scalar * M(2,3),
 323                      scalar * M(3,1),
 324                      scalar * M(3,2),
 325                      scalar * M(3,3) );
 326 }
 327 */
 328 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 329
 330 FGMatrix33& FGMatrix33::operator*=(const double scalar)
 331 {
 332   data[0] *= scalar;
 333   data[3] *= scalar;
 334   data[6] *= scalar;
 335   data[1] *= scalar;
 336   data[4] *= scalar;
 337   data[7] *= scalar;
 338   data[2] *= scalar;
 339   data[5] *= scalar;
 340   data[8] *= scalar;
 341
 342   return *this;
 343 }
 344
 345 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 346
 347 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
 348 {
 349   FGMatrix33 Product;
 350
 351   Product.data[0] = data[0]*M.data[0] + data[3]*M.data[1] + data[6]*M.data[2];
 352   Product.data[3] = data[0]*M.data[3] + data[3]*M.data[4] + data[6]*M.data[5];
 353   Product.data[6] = data[0]*M.data[6] + data[3]*M.data[7] + data[6]*M.data[8];
 354   Product.data[1] = data[1]*M.data[0] + data[4]*M.data[1] + data[7]*M.data[2];
 355   Product.data[4] = data[1]*M.data[3] + data[4]*M.data[4] + data[7]*M.data[5];
 356   Product.data[7] = data[1]*M.data[6] + data[4]*M.data[7] + data[7]*M.data[8];
 357   Product.data[2] = data[2]*M.data[0] + data[5]*M.data[1] + data[8]*M.data[2];
 358   Product.data[5] = data[2]*M.data[3] + data[5]*M.data[4] + data[8]*M.data[5];
 359   Product.data[8] = data[2]*M.data[6] + data[5]*M.data[7] + data[8]*M.data[8];
 360
 361   return Product;
 362 }
 363
 364 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 365
 366 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
 367 {
 368   // FIXME: Make compiler friendlier
 369   double a,b,c;
 370
 371   a = data[0]; b=data[3]; c=data[6];
 372   data[0] = a*M.data[0] + b*M.data[1] + c*M.data[2];
 373   data[3] = a*M.data[3] + b*M.data[4] + c*M.data[5];
 374   data[6] = a*M.data[6] + b*M.data[7] + c*M.data[8];
 375
 376   a = data[1]; b=data[4]; c=data[7];
 377   data[1] = a*M.data[0] + b*M.data[1] + c*M.data[2];
 378   data[4] = a*M.data[3] + b*M.data[4] + c*M.data[5];
 379   data[7] = a*M.data[6] + b*M.data[7] + c*M.data[8];
 380
 381   a = data[2]; b=data[5]; c=data[8];
 382   data[2] = a*M.data[0] + b*M.data[1] + c*M.data[2];
 383   data[5] = a*M.data[3] + b*M.data[4] + c*M.data[5];
 384   data[8] = a*M.data[6] + b*M.data[7] + c*M.data[8];
 385
 386   return *this;
 387 }
 388
 389 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 390
 391 FGMatrix33 FGMatrix33::operator/(const double scalar) const
 392 {
 393   FGMatrix33 Quot;
 394
 395   if ( scalar != 0 ) {
 396     double tmp = 1.0/scalar;
 397     Quot.data[0] = data[0] * tmp;
 398     Quot.data[3] = data[3] * tmp;
 399     Quot.data[6] = data[6] * tmp;
 400     Quot.data[1] = data[1] * tmp;
 401     Quot.data[4] = data[4] * tmp;
 402     Quot.data[7] = data[7] * tmp;
 403     Quot.data[2] = data[2] * tmp;
 404     Quot.data[5] = data[5] * tmp;
 405     Quot.data[8] = data[8] * tmp;
 406   } else {
 407     MatrixException mE;
 408     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
 409     throw mE;
 410   }
 411   return Quot;
 412 }
 413
 414 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 415
 416 FGMatrix33& FGMatrix33::operator/=(const double scalar)
 417 {
 418   if ( scalar != 0 ) {
 419     double tmp = 1.0/scalar;
 420     data[0] *= tmp;
 421     data[3] *= tmp;
 422     data[6] *= tmp;
 423     data[1] *= tmp;
 424     data[4] *= tmp;
 425     data[7] *= tmp;
 426     data[2] *= tmp;
 427     data[5] *= tmp;
 428     data[8] *= tmp;
 429   } else {
 430     MatrixException mE;
 431     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
 432     throw mE;
 433   }
 434   return *this;
 435 }
 436
 437 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 438
 439 void FGMatrix33::T(void)
 440 {
 441   double tmp;
 442
 443   tmp = data[3];
 444   data[3] = data[1];
 445   data[1] = tmp;
 446
 447   tmp = data[6];
 448   data[6] = data[2];
 449   data[2] = tmp;
 450
 451   tmp = data[7];
 452   data[7] = data[5];
 453   data[5] = tmp;
 454 }
 455
 456 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 457
 458 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const
 459 {
 460   double v1 = v(1);
 461   double v2 = v(2);
 462   double v3 = v(3);
 463
 464   double tmp1 = v1*data[0];  //[(col-1)*eRows+row-1]
 465   double tmp2 = v1*data[1];
 466   double tmp3 = v1*data[2];
 467
 468   tmp1 += v2*data[3];
 469   tmp2 += v2*data[4];
 470   tmp3 += v2*data[5];
 471
 472   tmp1 += v3*data[6];
 473   tmp2 += v3*data[7];
 474   tmp3 += v3*data[8];
 475
 476   return FGColumnVector3( tmp1, tmp2, tmp3 );
 477 }
 478
 479 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 480 //    The bitmasked value choices are as follows:
 481 //    unset: In this case (the default) JSBSim would only print
 482 //       out the normally expected messages, essentially echoing
 483 //       the config files as they are read. If the environment
 484 //       variable is not set, debug_lvl is set to 1 internally
 485 //    0: This requests JSBSim not to output any messages
 486 //       whatsoever.
 487 //    1: This value explicity requests the normal JSBSim
 488 //       startup messages
 489 //    2: This value asks for a message to be printed out when
 490 //       a class is instantiated
 491 //    4: When this value is set, a message is displayed when a
 492 //       FGModel object executes its Run() method
 493 //    8: When this value is set, various runtime state variables
 494 //       are printed out periodically
 495 //    16: When set various parameters are sanity checked and
 496 //       a message is printed out when they go out of bounds
 497
 498 void FGMatrix33::Debug(int from)
 499 {
 500   if (debug_lvl <= 0) return;
 501
 502   if (debug_lvl & 1) { // Standard console startup message output
 503     if (from == 0) { // Constructor
 504
 505     }
 506   }
 507   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
 508     if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
 509     if (from == 1) cout << "Destroyed:    FGMatrix33" << endl;
 510   }
 511   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
 512   }
 513   if (debug_lvl & 8 ) { // Runtime state variables
 514   }
 515   if (debug_lvl & 16) { // Sanity checking
 516   }
 517   if (debug_lvl & 64) {
 518     if (from == 0) { // Constructor
 519       cout << IdSrc << endl;
 520       cout << IdHdr << endl;
 521     }
 522   }
 523 }
 524 }