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