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.11 2010/12/07 12:57:14 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
  66 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  67
  68 string FGMatrix33::Dump(const string& delimiter) const
  69 {
  70   ostringstream buffer;
  71   buffer << setw(12) << setprecision(10) << data[0] << delimiter;
  72   buffer << setw(12) << setprecision(10) << data[3] << delimiter;
  73   buffer << setw(12) << setprecision(10) << data[6] << delimiter;
  74   buffer << setw(12) << setprecision(10) << data[1] << delimiter;
  75   buffer << setw(12) << setprecision(10) << data[4] << delimiter;
  76   buffer << setw(12) << setprecision(10) << data[7] << delimiter;
  77   buffer << setw(12) << setprecision(10) << data[2] << delimiter;
  78   buffer << setw(12) << setprecision(10) << data[5] << delimiter;
  79   buffer << setw(12) << setprecision(10) << data[8];
  80   return buffer.str();
  81 }
  82
  83 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  84
  85 string FGMatrix33::Dump(const string& delimiter, const string& prefix) const
  86 {
  87   ostringstream buffer;
  88
  89   buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[0] << delimiter;
  90   buffer << right << fixed << setw(9) << setprecision(6) << data[3] << delimiter;
  91   buffer << right << fixed << setw(9) << setprecision(6) << data[6] << endl;
  92
  93   buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[1] << delimiter;
  94   buffer << right << fixed << setw(9) << setprecision(6) << data[4] << delimiter;
  95   buffer << right << fixed << setw(9) << setprecision(6) << data[7] << endl;
  96
  97   buffer << prefix << right << fixed << setw(9) << setprecision(6) << data[2] << delimiter;
  98   buffer << right << fixed << setw(9) << setprecision(6) << data[5] << delimiter;
  99   buffer << right << fixed << setw(9) << setprecision(6) << data[8];
 100
 101   buffer << setw(0) << left;
 102
 103   return buffer.str();
 104 }
 105
 106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 107
 108 FGQuaternion FGMatrix33::GetQuaternion(void)
 109 {
 110   FGQuaternion Q;
 111
 112   double tempQ[4];
 113   int idx;
 114
 115   tempQ[0] = 1.0 + data[0] + data[4] + data[8];
 116   tempQ[1] = 1.0 + data[0] - data[4] - data[8];
 117   tempQ[2] = 1.0 - data[0] + data[4] - data[8];
 118   tempQ[3] = 1.0 - data[0] - data[4] + data[8];
 119
 120   // Find largest of the above
 121   idx = 0;
 122   for (int i=1; i<4; i++) if (tempQ[i] > tempQ[idx]) idx = i;
 123
 124   switch(idx) {
 125     case 0:
 126       Q(1) = 0.50*sqrt(tempQ[0]);
 127       Q(2) = 0.25*(data[7] - data[5])/Q(1);
 128       Q(3) = 0.25*(data[2] - data[6])/Q(1);
 129       Q(4) = 0.25*(data[3] - data[1])/Q(1);
 130       break;
 131     case 1:
 132       Q(2) = 0.50*sqrt(tempQ[1]);
 133       Q(1) = 0.25*(data[7] - data[5])/Q(2);
 134       Q(3) = 0.25*(data[3] + data[1])/Q(2);
 135       Q(4) = 0.25*(data[2] + data[6])/Q(2);
 136       break;
 137     case 2:
 138       Q(3) = 0.50*sqrt(tempQ[2]);
 139       Q(1) = 0.25*(data[2] - data[6])/Q(3);
 140       Q(2) = 0.25*(data[3] + data[1])/Q(3);
 141       Q(4) = 0.25*(data[7] + data[5])/Q(3);
 142       break;
 143     case 3:
 144       Q(4) = 0.50*sqrt(tempQ[3]);
 145       Q(1) = 0.25*(data[3] - data[1])/Q(4);
 146       Q(2) = 0.25*(data[6] + data[2])/Q(4);
 147       Q(3) = 0.25*(data[7] + data[5])/Q(4);
 148       break;
 149     default:
 150       //error
 151       break;
 152   }
 153
 154   return (Q);
 155 }
 156
 157 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 158
 159 ostream& operator<<(ostream& os, const FGMatrix33& M)
 160 {
 161   for (unsigned int i=1; i<=M.Rows(); i++) {
 162     for (unsigned int j=1; j<=M.Cols(); j++) {
 163       if (i == M.Rows() && j == M.Cols())
 164         os << M(i,j);
 165       else
 166         os << M(i,j) << ", ";
 167     }
 168   }
 169   return os;
 170 }
 171
 172 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 173
 174 istream& operator>>(istream& is, FGMatrix33& M)
 175 {
 176   for (unsigned int i=1; i<=M.Rows(); i++) {
 177     for (unsigned int j=1; j<=M.Cols(); j++) {
 178       is >> M(i,j);
 179     }
 180   }
 181   return is;
 182 }
 183
 184 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 185
 186 double FGMatrix33::Determinant(void) const {
 187   return data[0]*data[4]*data[8] + data[3]*data[7]*data[2]
 188        + data[6]*data[1]*data[5] - data[6]*data[4]*data[2]
 189        - data[3]*data[1]*data[8] - data[7]*data[5]*data[0];
 190 }
 191
 192 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 193
 194 FGMatrix33 FGMatrix33::Inverse(void) const {
 195   // Compute the inverse of a general matrix using Cramers rule.
 196   // I guess googling for cramers rule gives tons of references
 197   // for this. :)
 198
 199   if (Determinant() != 0.0) {
 200     double rdet = 1.0/Determinant();
 201
 202     double i11 = rdet*(data[4]*data[8]-data[7]*data[5]);
 203     double i21 = rdet*(data[7]*data[2]-data[1]*data[8]);
 204     double i31 = rdet*(data[1]*data[5]-data[4]*data[2]);
 205     double i12 = rdet*(data[6]*data[5]-data[3]*data[8]);
 206     double i22 = rdet*(data[0]*data[8]-data[6]*data[2]);
 207     double i32 = rdet*(data[3]*data[2]-data[0]*data[5]);
 208     double i13 = rdet*(data[3]*data[7]-data[6]*data[4]);
 209     double i23 = rdet*(data[6]*data[1]-data[0]*data[7]);
 210     double i33 = rdet*(data[0]*data[4]-data[3]*data[1]);
 211
 212     return FGMatrix33( i11, i12, i13,
 213                        i21, i22, i23,
 214                        i31, i32, i33 );
 215   } else {
 216     return FGMatrix33( 0, 0, 0,
 217                        0, 0, 0,
 218                        0, 0, 0 );
 219   }
 220 }
 221
 222 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 223
 224 void FGMatrix33::InitMatrix(void)
 225 {
 226   data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
 227     data[6] = data[7] = data[8] = 0.0;
 228 }
 229
 230 // *****************************************************************************
 231 // binary operators ************************************************************
 232 // *****************************************************************************
 233
 234 FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
 235 {
 236   return FGMatrix33( data[0] - M.data[0],
 237                      data[3] - M.data[3],
 238                      data[6] - M.data[6],
 239                      data[1] - M.data[1],
 240                      data[4] - M.data[4],
 241                      data[7] - M.data[7],
 242                      data[2] - M.data[2],
 243                      data[5] - M.data[5],
 244                      data[8] - M.data[8] );
 245 }
 246
 247 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 248
 249 FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
 250 {
 251   data[0] -= M.data[0];
 252   data[1] -= M.data[1];
 253   data[2] -= M.data[2];
 254   data[3] -= M.data[3];
 255   data[4] -= M.data[4];
 256   data[5] -= M.data[5];
 257   data[6] -= M.data[6];
 258   data[7] -= M.data[7];
 259   data[8] -= M.data[8];
 260
 261   return *this;
 262 }
 263
 264 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 265
 266 FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
 267 {
 268   return FGMatrix33( data[0] + M.data[0],
 269                      data[3] + M.data[3],
 270                      data[6] + M.data[6],
 271                      data[1] + M.data[1],
 272                      data[4] + M.data[4],
 273                      data[7] + M.data[7],
 274                      data[2] + M.data[2],
 275                      data[5] + M.data[5],
 276                      data[8] + M.data[8] );
 277 }
 278
 279 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 280
 281 FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
 282 {
 283   data[0] += M.data[0];
 284   data[3] += M.data[3];
 285   data[6] += M.data[6];
 286   data[1] += M.data[1];
 287   data[4] += M.data[4];
 288   data[7] += M.data[7];
 289   data[2] += M.data[2];
 290   data[5] += M.data[5];
 291   data[8] += M.data[8];
 292
 293   return *this;
 294 }
 295
 296 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 297
 298 FGMatrix33 FGMatrix33::operator*(const double scalar) const
 299 {
 300   return FGMatrix33( scalar * data[0],
 301                      scalar * data[3],
 302                      scalar * data[6],
 303                      scalar * data[1],
 304                      scalar * data[4],
 305                      scalar * data[7],
 306                      scalar * data[2],
 307                      scalar * data[5],
 308                      scalar * data[8] );
 309 }
 310
 311 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 312 /*
 313 FGMatrix33 operator*(double scalar, FGMatrix33 &M)
 314 {
 315   return FGMatrix33( scalar * M(1,1),
 316                      scalar * M(1,2),
 317                      scalar * M(1,3),
 318                      scalar * M(2,1),
 319                      scalar * M(2,2),
 320                      scalar * M(2,3),
 321                      scalar * M(3,1),
 322                      scalar * M(3,2),
 323                      scalar * M(3,3) );
 324 }
 325 */
 326 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 327
 328 FGMatrix33& FGMatrix33::operator*=(const double scalar)
 329 {
 330   data[0] *= scalar;
 331   data[3] *= scalar;
 332   data[6] *= scalar;
 333   data[1] *= scalar;
 334   data[4] *= scalar;
 335   data[7] *= scalar;
 336   data[2] *= scalar;
 337   data[5] *= scalar;
 338   data[8] *= scalar;
 339
 340   return *this;
 341 }
 342
 343 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 344
 345 FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
 346 {
 347   FGMatrix33 Product;
 348
 349   Product.data[0] = data[0]*M.data[0] + data[3]*M.data[1] + data[6]*M.data[2];
 350   Product.data[3] = data[0]*M.data[3] + data[3]*M.data[4] + data[6]*M.data[5];
 351   Product.data[6] = data[0]*M.data[6] + data[3]*M.data[7] + data[6]*M.data[8];
 352   Product.data[1] = data[1]*M.data[0] + data[4]*M.data[1] + data[7]*M.data[2];
 353   Product.data[4] = data[1]*M.data[3] + data[4]*M.data[4] + data[7]*M.data[5];
 354   Product.data[7] = data[1]*M.data[6] + data[4]*M.data[7] + data[7]*M.data[8];
 355   Product.data[2] = data[2]*M.data[0] + data[5]*M.data[1] + data[8]*M.data[2];
 356   Product.data[5] = data[2]*M.data[3] + data[5]*M.data[4] + data[8]*M.data[5];
 357   Product.data[8] = data[2]*M.data[6] + data[5]*M.data[7] + data[8]*M.data[8];
 358
 359   return Product;
 360 }
 361
 362 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 363
 364 FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
 365 {
 366   // FIXME: Make compiler friendlier
 367   double a,b,c;
 368
 369   a = data[0]; b=data[3]; c=data[6];
 370   data[0] = a*M.data[0] + b*M.data[1] + c*M.data[2];
 371   data[3] = a*M.data[3] + b*M.data[4] + c*M.data[5];
 372   data[6] = a*M.data[6] + b*M.data[7] + c*M.data[8];
 373
 374   a = data[1]; b=data[4]; c=data[7];
 375   data[1] = a*M.data[0] + b*M.data[1] + c*M.data[2];
 376   data[4] = a*M.data[3] + b*M.data[4] + c*M.data[5];
 377   data[7] = a*M.data[6] + b*M.data[7] + c*M.data[8];
 378
 379   a = data[2]; b=data[5]; c=data[8];
 380   data[2] = a*M.data[0] + b*M.data[1] + c*M.data[2];
 381   data[5] = a*M.data[3] + b*M.data[4] + c*M.data[5];
 382   data[8] = a*M.data[6] + b*M.data[7] + c*M.data[8];
 383
 384   return *this;
 385 }
 386
 387 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 388
 389 FGMatrix33 FGMatrix33::operator/(const double scalar) const
 390 {
 391   FGMatrix33 Quot;
 392
 393   if ( scalar != 0 ) {
 394     double tmp = 1.0/scalar;
 395     Quot.data[0] = data[0] * tmp;
 396     Quot.data[3] = data[3] * tmp;
 397     Quot.data[6] = data[6] * tmp;
 398     Quot.data[1] = data[1] * tmp;
 399     Quot.data[4] = data[4] * tmp;
 400     Quot.data[7] = data[7] * tmp;
 401     Quot.data[2] = data[2] * tmp;
 402     Quot.data[5] = data[5] * tmp;
 403     Quot.data[8] = data[8] * tmp;
 404   } else {
 405     MatrixException mE;
 406     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
 407     throw mE;
 408   }
 409   return Quot;
 410 }
 411
 412 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 413
 414 FGMatrix33& FGMatrix33::operator/=(const double scalar)
 415 {
 416   if ( scalar != 0 ) {
 417     double tmp = 1.0/scalar;
 418     data[0] *= tmp;
 419     data[3] *= tmp;
 420     data[6] *= tmp;
 421     data[1] *= tmp;
 422     data[4] *= tmp;
 423     data[7] *= tmp;
 424     data[2] *= tmp;
 425     data[5] *= tmp;
 426     data[8] *= tmp;
 427   } else {
 428     MatrixException mE;
 429     mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
 430     throw mE;
 431   }
 432   return *this;
 433 }
 434
 435 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 436
 437 void FGMatrix33::T(void)
 438 {
 439   double tmp;
 440
 441   tmp = data[3];
 442   data[3] = data[1];
 443   data[1] = tmp;
 444
 445   tmp = data[6];
 446   data[6] = data[2];
 447   data[2] = tmp;
 448
 449   tmp = data[7];
 450   data[7] = data[5];
 451   data[5] = tmp;
 452 }
 453
 454 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 455
 456 FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const
 457 {
 458   double v1 = v(1);
 459   double v2 = v(2);
 460   double v3 = v(3);
 461
 462   double tmp1 = v1*data[0];  //[(col-1)*eRows+row-1]
 463   double tmp2 = v1*data[1];
 464   double tmp3 = v1*data[2];
 465
 466   tmp1 += v2*data[3];
 467   tmp2 += v2*data[4];
 468   tmp3 += v2*data[5];
 469
 470   tmp1 += v3*data[6];
 471   tmp2 += v3*data[7];
 472   tmp3 += v3*data[8];
 473
 474   return FGColumnVector3( tmp1, tmp2, tmp3 );
 475 }
 476
 477 }