-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-Module: FGMatrix33.cpp\r
-Author: Tony Peden, Jon Berndt, Mathias Frolich\r
-Date started: 1998\r
-Purpose: FGMatrix33 class\r
-Called by: Various\r
-\r
-FUNCTIONAL DESCRIPTION\r
---------------------------------------------------------------------------------\r
-\r
-HISTORY\r
---------------------------------------------------------------------------------\r
-??/??/?? TP Created\r
-03/16/2000 JSB Added exception throwing\r
-\r
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-INCLUDES\r
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
-\r
-#include "FGMatrix33.h"\r
-#include "FGColumnVector3.h"\r
-\r
-namespace JSBSim {\r
-\r
-static const char *IdSrc = "$Id$";\r
-static const char *IdHdr = ID_MATRIX33;\r
-\r
-/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-CLASS IMPLEMENTATION\r
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33::FGMatrix33(void)\r
-{\r
- data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =\r
- data[6] = data[7] = data[8] = 0.0;\r
-\r
- Debug(0);\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-ostream& operator<<(ostream& os, const FGMatrix33& M)\r
-{\r
- for (unsigned int i=1; i<=M.Rows(); i++) {\r
- for (unsigned int j=1; j<=M.Cols(); j++) {\r
- if (i == M.Rows() && j == M.Cols())\r
- os << M(i,j);\r
- else\r
- os << M(i,j) << ", ";\r
- }\r
- }\r
- return os;\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-istream& operator>>(istream& is, FGMatrix33& M)\r
-{\r
- for (unsigned int i=1; i<=M.Rows(); i++) {\r
- for (unsigned int j=1; j<=M.Cols(); j++) {\r
- is >> M(i,j);\r
- }\r
- }\r
- return is;\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-double FGMatrix33::Determinant(void) const {\r
- return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)\r
- + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)\r
- - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33 FGMatrix33::Inverse(void) const {\r
- // Compute the inverse of a general matrix using Cramers rule.\r
- // I guess googling for cramers rule gives tons of references\r
- // for this. :)\r
- double rdet = 1.0/Determinant();\r
-\r
- double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));\r
- double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));\r
- double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));\r
- double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));\r
- double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));\r
- double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));\r
- double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));\r
- double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));\r
- double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));\r
-\r
- return FGMatrix33( i11, i12, i13,\r
- i21, i22, i23,\r
- i31, i32, i33 );\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-void FGMatrix33::InitMatrix(void)\r
-{\r
- data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =\r
- data[6] = data[7] = data[8] = 0.0;\r
-}\r
-\r
-// *****************************************************************************\r
-// binary operators ************************************************************\r
-// *****************************************************************************\r
-\r
-FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const\r
-{\r
- return FGMatrix33( Entry(1,1) - M(1,1),\r
- Entry(1,2) - M(1,2),\r
- Entry(1,3) - M(1,3),\r
- Entry(2,1) - M(2,1),\r
- Entry(2,2) - M(2,2),\r
- Entry(2,3) - M(2,3),\r
- Entry(3,1) - M(3,1),\r
- Entry(3,2) - M(3,2),\r
- Entry(3,3) - M(3,3) );\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)\r
-{\r
- data[0] -= M.data[0];\r
- data[1] -= M.data[1];\r
- data[2] -= M.data[2];\r
- data[3] -= M.data[3];\r
- data[4] -= M.data[4];\r
- data[5] -= M.data[5];\r
- data[6] -= M.data[6];\r
- data[7] -= M.data[7];\r
- data[8] -= M.data[8];\r
-\r
- return *this;\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const\r
-{\r
- return FGMatrix33( Entry(1,1) + M(1,1),\r
- Entry(1,2) + M(1,2),\r
- Entry(1,3) + M(1,3),\r
- Entry(2,1) + M(2,1),\r
- Entry(2,2) + M(2,2),\r
- Entry(2,3) + M(2,3),\r
- Entry(3,1) + M(3,1),\r
- Entry(3,2) + M(3,2),\r
- Entry(3,3) + M(3,3) );\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)\r
-{\r
- Entry(1,1) += M(1,1);\r
- Entry(1,2) += M(1,2);\r
- Entry(1,3) += M(1,3);\r
- Entry(2,1) += M(2,1);\r
- Entry(2,2) += M(2,2);\r
- Entry(2,3) += M(2,3);\r
- Entry(3,1) += M(3,1);\r
- Entry(3,2) += M(3,2);\r
- Entry(3,3) += M(3,3);\r
-\r
- return *this;\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33 FGMatrix33::operator*(const double scalar) const\r
-{\r
- return FGMatrix33( scalar * Entry(1,1),\r
- scalar * Entry(1,2),\r
- scalar * Entry(1,3),\r
- scalar * Entry(2,1),\r
- scalar * Entry(2,2),\r
- scalar * Entry(2,3),\r
- scalar * Entry(3,1),\r
- scalar * Entry(3,2),\r
- scalar * Entry(3,3) );\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33 operator*(double scalar, FGMatrix33 &M)\r
-{\r
- return FGMatrix33( scalar * M(1,1),\r
- scalar * M(1,2),\r
- scalar * M(1,3),\r
- scalar * M(2,1),\r
- scalar * M(2,2),\r
- scalar * M(2,3),\r
- scalar * M(3,1),\r
- scalar * M(3,2),\r
- scalar * M(3,3) );\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33& FGMatrix33::operator*=(const double scalar)\r
-{\r
- Entry(1,1) *= scalar;\r
- Entry(1,2) *= scalar;\r
- Entry(1,3) *= scalar;\r
- Entry(2,1) *= scalar;\r
- Entry(2,2) *= scalar;\r
- Entry(2,3) *= scalar;\r
- Entry(3,1) *= scalar;\r
- Entry(3,2) *= scalar;\r
- Entry(3,3) *= scalar;\r
-\r
- return *this;\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const\r
-{\r
- // FIXME: Make compiler friendlier\r
- FGMatrix33 Product;\r
-\r
- Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);\r
- Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);\r
- Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);\r
- Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);\r
- Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);\r
- Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);\r
- Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);\r
- Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);\r
- Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);\r
-\r
- return Product;\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)\r
-{\r
- // FIXME: Make compiler friendlier\r
- double a,b,c;\r
-\r
- a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);\r
- Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);\r
- Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);\r
- Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);\r
-\r
- a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);\r
- Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);\r
- Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);\r
- Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);\r
-\r
- a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);\r
- Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);\r
- Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);\r
- Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);\r
-\r
- return *this;\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33 FGMatrix33::operator/(const double scalar) const\r
-{\r
- FGMatrix33 Quot;\r
-\r
- if ( scalar != 0 ) {\r
- double tmp = 1.0/scalar;\r
- Quot(1,1) = Entry(1,1) * tmp;\r
- Quot(1,2) = Entry(1,2) * tmp;\r
- Quot(1,3) = Entry(1,3) * tmp;\r
- Quot(2,1) = Entry(2,1) * tmp;\r
- Quot(2,2) = Entry(2,2) * tmp;\r
- Quot(2,3) = Entry(2,3) * tmp;\r
- Quot(3,1) = Entry(3,1) * tmp;\r
- Quot(3,2) = Entry(3,2) * tmp;\r
- Quot(3,3) = Entry(3,3) * tmp;\r
- } else {\r
- MatrixException mE;\r
- mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";\r
- throw mE;\r
- }\r
- return Quot;\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGMatrix33& FGMatrix33::operator/=(const double scalar)\r
-{\r
- if ( scalar != 0 ) {\r
- double tmp = 1.0/scalar;\r
- Entry(1,1) *= tmp;\r
- Entry(1,2) *= tmp;\r
- Entry(1,3) *= tmp;\r
- Entry(2,1) *= tmp;\r
- Entry(2,2) *= tmp;\r
- Entry(2,3) *= tmp;\r
- Entry(3,1) *= tmp;\r
- Entry(3,2) *= tmp;\r
- Entry(3,3) *= tmp;\r
- } else {\r
- MatrixException mE;\r
- mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";\r
- throw mE;\r
- }\r
- return *this;\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-void FGMatrix33::T(void)\r
-{\r
- for (unsigned int i=1; i<=3; i++) {\r
- for (unsigned int j=i+1; j<=3; j++) {\r
- double tmp = Entry(i,j);\r
- Entry(i,j) = Entry(j,i);\r
- Entry(j,i) = tmp;\r
- }\r
- }\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-\r
-FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {\r
- double tmp1 = v(1)*Entry(1,1);\r
- double tmp2 = v(1)*Entry(2,1);\r
- double tmp3 = v(1)*Entry(3,1);\r
-\r
- tmp1 += v(2)*Entry(1,2);\r
- tmp2 += v(2)*Entry(2,2);\r
- tmp3 += v(2)*Entry(3,2);\r
-\r
- tmp1 += v(3)*Entry(1,3);\r
- tmp2 += v(3)*Entry(2,3);\r
- tmp3 += v(3)*Entry(3,3);\r
-\r
- return FGColumnVector3( tmp1, tmp2, tmp3 );\r
-}\r
-\r
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\r
-// The bitmasked value choices are as follows:\r
-// unset: In this case (the default) JSBSim would only print\r
-// out the normally expected messages, essentially echoing\r
-// the config files as they are read. If the environment\r
-// variable is not set, debug_lvl is set to 1 internally\r
-// 0: This requests JSBSim not to output any messages\r
-// whatsoever.\r
-// 1: This value explicity requests the normal JSBSim\r
-// startup messages\r
-// 2: This value asks for a message to be printed out when\r
-// a class is instantiated\r
-// 4: When this value is set, a message is displayed when a\r
-// FGModel object executes its Run() method\r
-// 8: When this value is set, various runtime state variables\r
-// are printed out periodically\r
-// 16: When set various parameters are sanity checked and\r
-// a message is printed out when they go out of bounds\r
-\r
-void FGMatrix33::Debug(int from)\r
-{\r
- if (debug_lvl <= 0) return;\r
-\r
- if (debug_lvl & 1) { // Standard console startup message output\r
- if (from == 0) { // Constructor\r
-\r
- }\r
- }\r
- if (debug_lvl & 2 ) { // Instantiation/Destruction notification\r
- if (from == 0) cout << "Instantiated: FGMatrix33" << endl;\r
- if (from == 1) cout << "Destroyed: FGMatrix33" << endl;\r
- }\r
- if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects\r
- }\r
- if (debug_lvl & 8 ) { // Runtime state variables\r
- }\r
- if (debug_lvl & 16) { // Sanity checking\r
- }\r
- if (debug_lvl & 64) {\r
- if (from == 0) { // Constructor\r
- cout << IdSrc << endl;\r
- cout << IdHdr << endl;\r
- }\r
- }\r
-}\r
-}\r
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+Module: FGMatrix33.cpp
+Author: Tony Peden, Jon Berndt, Mathias Frolich
+Date started: 1998
+Purpose: FGMatrix33 class
+Called by: Various
+
+FUNCTIONAL DESCRIPTION
+--------------------------------------------------------------------------------
+
+HISTORY
+--------------------------------------------------------------------------------
+??/??/?? TP Created
+03/16/2000 JSB Added exception throwing
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include "FGMatrix33.h"
+#include "FGColumnVector3.h"
+
+namespace JSBSim {
+
+static const char *IdSrc = "$Id$";
+static const char *IdHdr = ID_MATRIX33;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33::FGMatrix33(void)
+{
+ data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
+ data[6] = data[7] = data[8] = 0.0;
+
+ Debug(0);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ostream& operator<<(ostream& os, const FGMatrix33& M)
+{
+ for (unsigned int i=1; i<=M.Rows(); i++) {
+ for (unsigned int j=1; j<=M.Cols(); j++) {
+ if (i == M.Rows() && j == M.Cols())
+ os << M(i,j);
+ else
+ os << M(i,j) << ", ";
+ }
+ }
+ return os;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+istream& operator>>(istream& is, FGMatrix33& M)
+{
+ for (unsigned int i=1; i<=M.Rows(); i++) {
+ for (unsigned int j=1; j<=M.Cols(); j++) {
+ is >> M(i,j);
+ }
+ }
+ return is;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+double FGMatrix33::Determinant(void) const {
+ return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
+ + Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
+ - Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 FGMatrix33::Inverse(void) const {
+ // Compute the inverse of a general matrix using Cramers rule.
+ // I guess googling for cramers rule gives tons of references
+ // for this. :)
+ double rdet = 1.0/Determinant();
+
+ double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
+ double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
+ double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
+ double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
+ double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
+ double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
+ double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
+ double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
+ double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
+
+ return FGMatrix33( i11, i12, i13,
+ i21, i22, i23,
+ i31, i32, i33 );
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::InitMatrix(void)
+{
+ data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
+ data[6] = data[7] = data[8] = 0.0;
+}
+
+// *****************************************************************************
+// binary operators ************************************************************
+// *****************************************************************************
+
+FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
+{
+ return FGMatrix33( Entry(1,1) - M(1,1),
+ Entry(1,2) - M(1,2),
+ Entry(1,3) - M(1,3),
+ Entry(2,1) - M(2,1),
+ Entry(2,2) - M(2,2),
+ Entry(2,3) - M(2,3),
+ Entry(3,1) - M(3,1),
+ Entry(3,2) - M(3,2),
+ Entry(3,3) - M(3,3) );
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
+{
+ data[0] -= M.data[0];
+ data[1] -= M.data[1];
+ data[2] -= M.data[2];
+ data[3] -= M.data[3];
+ data[4] -= M.data[4];
+ data[5] -= M.data[5];
+ data[6] -= M.data[6];
+ data[7] -= M.data[7];
+ data[8] -= M.data[8];
+
+ return *this;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
+{
+ return FGMatrix33( Entry(1,1) + M(1,1),
+ Entry(1,2) + M(1,2),
+ Entry(1,3) + M(1,3),
+ Entry(2,1) + M(2,1),
+ Entry(2,2) + M(2,2),
+ Entry(2,3) + M(2,3),
+ Entry(3,1) + M(3,1),
+ Entry(3,2) + M(3,2),
+ Entry(3,3) + M(3,3) );
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
+{
+ Entry(1,1) += M(1,1);
+ Entry(1,2) += M(1,2);
+ Entry(1,3) += M(1,3);
+ Entry(2,1) += M(2,1);
+ Entry(2,2) += M(2,2);
+ Entry(2,3) += M(2,3);
+ Entry(3,1) += M(3,1);
+ Entry(3,2) += M(3,2);
+ Entry(3,3) += M(3,3);
+
+ return *this;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 FGMatrix33::operator*(const double scalar) const
+{
+ return FGMatrix33( scalar * Entry(1,1),
+ scalar * Entry(1,2),
+ scalar * Entry(1,3),
+ scalar * Entry(2,1),
+ scalar * Entry(2,2),
+ scalar * Entry(2,3),
+ scalar * Entry(3,1),
+ scalar * Entry(3,2),
+ scalar * Entry(3,3) );
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 operator*(double scalar, FGMatrix33 &M)
+{
+ return FGMatrix33( scalar * M(1,1),
+ scalar * M(1,2),
+ scalar * M(1,3),
+ scalar * M(2,1),
+ scalar * M(2,2),
+ scalar * M(2,3),
+ scalar * M(3,1),
+ scalar * M(3,2),
+ scalar * M(3,3) );
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33& FGMatrix33::operator*=(const double scalar)
+{
+ Entry(1,1) *= scalar;
+ Entry(1,2) *= scalar;
+ Entry(1,3) *= scalar;
+ Entry(2,1) *= scalar;
+ Entry(2,2) *= scalar;
+ Entry(2,3) *= scalar;
+ Entry(3,1) *= scalar;
+ Entry(3,2) *= scalar;
+ Entry(3,3) *= scalar;
+
+ return *this;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
+{
+ // FIXME: Make compiler friendlier
+ FGMatrix33 Product;
+
+ Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
+ Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
+ Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
+ Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
+ Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
+ Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
+ Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
+ Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
+ Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
+{
+ // FIXME: Make compiler friendlier
+ double a,b,c;
+
+ a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
+ Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
+ Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
+ Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
+
+ a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
+ Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
+ Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
+ Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
+
+ a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
+ Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
+ Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
+ Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
+
+ return *this;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 FGMatrix33::operator/(const double scalar) const
+{
+ FGMatrix33 Quot;
+
+ if ( scalar != 0 ) {
+ double tmp = 1.0/scalar;
+ Quot(1,1) = Entry(1,1) * tmp;
+ Quot(1,2) = Entry(1,2) * tmp;
+ Quot(1,3) = Entry(1,3) * tmp;
+ Quot(2,1) = Entry(2,1) * tmp;
+ Quot(2,2) = Entry(2,2) * tmp;
+ Quot(2,3) = Entry(2,3) * tmp;
+ Quot(3,1) = Entry(3,1) * tmp;
+ Quot(3,2) = Entry(3,2) * tmp;
+ Quot(3,3) = Entry(3,3) * tmp;
+ } else {
+ MatrixException mE;
+ mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
+ throw mE;
+ }
+ return Quot;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33& FGMatrix33::operator/=(const double scalar)
+{
+ if ( scalar != 0 ) {
+ double tmp = 1.0/scalar;
+ Entry(1,1) *= tmp;
+ Entry(1,2) *= tmp;
+ Entry(1,3) *= tmp;
+ Entry(2,1) *= tmp;
+ Entry(2,2) *= tmp;
+ Entry(2,3) *= tmp;
+ Entry(3,1) *= tmp;
+ Entry(3,2) *= tmp;
+ Entry(3,3) *= tmp;
+ } else {
+ MatrixException mE;
+ mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
+ throw mE;
+ }
+ return *this;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::T(void)
+{
+ for (unsigned int i=1; i<=3; i++) {
+ for (unsigned int j=i+1; j<=3; j++) {
+ double tmp = Entry(i,j);
+ Entry(i,j) = Entry(j,i);
+ Entry(j,i) = tmp;
+ }
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
+ double tmp1 = v(1)*Entry(1,1);
+ double tmp2 = v(1)*Entry(2,1);
+ double tmp3 = v(1)*Entry(3,1);
+
+ tmp1 += v(2)*Entry(1,2);
+ tmp2 += v(2)*Entry(2,2);
+ tmp3 += v(2)*Entry(3,2);
+
+ tmp1 += v(3)*Entry(1,3);
+ tmp2 += v(3)*Entry(2,3);
+ tmp3 += v(3)*Entry(3,3);
+
+ return FGColumnVector3( tmp1, tmp2, tmp3 );
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+// The bitmasked value choices are as follows:
+// unset: In this case (the default) JSBSim would only print
+// out the normally expected messages, essentially echoing
+// the config files as they are read. If the environment
+// variable is not set, debug_lvl is set to 1 internally
+// 0: This requests JSBSim not to output any messages
+// whatsoever.
+// 1: This value explicity requests the normal JSBSim
+// startup messages
+// 2: This value asks for a message to be printed out when
+// a class is instantiated
+// 4: When this value is set, a message is displayed when a
+// FGModel object executes its Run() method
+// 8: When this value is set, various runtime state variables
+// are printed out periodically
+// 16: When set various parameters are sanity checked and
+// a message is printed out when they go out of bounds
+
+void FGMatrix33::Debug(int from)
+{
+ if (debug_lvl <= 0) return;
+
+ if (debug_lvl & 1) { // Standard console startup message output
+ if (from == 0) { // Constructor
+
+ }
+ }
+ if (debug_lvl & 2 ) { // Instantiation/Destruction notification
+ if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
+ if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
+ }
+ if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
+ }
+ if (debug_lvl & 8 ) { // Runtime state variables
+ }
+ if (debug_lvl & 16) { // Sanity checking
+ }
+ if (debug_lvl & 64) {
+ if (from == 0) { // Constructor
+ cout << IdSrc << endl;
+ cout << IdHdr << endl;
+ }
+ }
+}
+}
projects / flightgear.git / blobdiff