437 lines
12 KiB
C++
437 lines
12 KiB
C++
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
Module: FGMatrix33.cpp
|
|
Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
|
|
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"
|
|
|
|
static const char *IdSrc = "$Id$";
|
|
static const char *IdHdr = ID_MATRIX33;
|
|
|
|
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
CLASS IMPLEMENTATION
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33::FGMatrix33(void)
|
|
{
|
|
InitMatrix();
|
|
rowCtr = colCtr = 1;
|
|
|
|
Debug(0);
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33::FGMatrix33(int r, int c)
|
|
{
|
|
InitMatrix();
|
|
rowCtr = colCtr = 1;
|
|
|
|
Debug(0);
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33::FGMatrix33(const FGMatrix33& M)
|
|
{
|
|
rowCtr = colCtr = 1;
|
|
|
|
data[1][1] = M.data[1][1];
|
|
data[1][2] = M.data[1][2];
|
|
data[1][3] = M.data[1][3];
|
|
data[2][1] = M.data[2][1];
|
|
data[2][2] = M.data[2][2];
|
|
data[2][3] = M.data[2][3];
|
|
data[3][1] = M.data[3][1];
|
|
data[3][2] = M.data[3][2];
|
|
data[3][3] = M.data[3][3];
|
|
|
|
Debug(0);
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33::~FGMatrix33(void)
|
|
{
|
|
rowCtr = colCtr = 1;
|
|
|
|
Debug(1);
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
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.data[i][j];
|
|
else
|
|
os << M.data[i][j] << ", ";
|
|
}
|
|
}
|
|
return os;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33& FGMatrix33::operator<<(const double ff)
|
|
{
|
|
data[rowCtr][colCtr] = ff;
|
|
if (++colCtr > Cols()) {
|
|
colCtr = 1;
|
|
if (++rowCtr > Rows())
|
|
rowCtr = 1;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
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.data[i][j];
|
|
}
|
|
}
|
|
return is;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33& FGMatrix33::operator=(const FGMatrix33& M)
|
|
{
|
|
if (&M != this) {
|
|
data[1][1] = M.data[1][1];
|
|
data[1][2] = M.data[1][2];
|
|
data[1][3] = M.data[1][3];
|
|
data[2][1] = M.data[2][1];
|
|
data[2][2] = M.data[2][2];
|
|
data[2][3] = M.data[2][3];
|
|
data[3][1] = M.data[3][1];
|
|
data[3][2] = M.data[3][2];
|
|
data[3][3] = M.data[3][3];
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
void FGMatrix33::InitMatrix(double value)
|
|
{
|
|
if (data) {
|
|
data[1][1] = value;
|
|
data[1][2] = value;
|
|
data[1][3] = value;
|
|
data[2][1] = value;
|
|
data[2][2] = value;
|
|
data[2][3] = value;
|
|
data[3][1] = value;
|
|
data[3][2] = value;
|
|
data[3][3] = value;
|
|
}
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
void FGMatrix33::InitMatrix(void)
|
|
{
|
|
this->InitMatrix(0);
|
|
}
|
|
|
|
// *****************************************************************************
|
|
// binary operators ************************************************************
|
|
// *****************************************************************************
|
|
|
|
FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M)
|
|
{
|
|
FGMatrix33 Diff;
|
|
|
|
Diff(1,1) = data[1][1] - M(1,1);
|
|
Diff(1,2) = data[1][2] - M(1,2);
|
|
Diff(1,3) = data[1][3] - M(1,3);
|
|
Diff(2,1) = data[2][1] - M(2,1);
|
|
Diff(2,2) = data[2][2] - M(2,2);
|
|
Diff(2,3) = data[2][3] - M(2,3);
|
|
Diff(3,1) = data[3][1] - M(3,1);
|
|
Diff(3,2) = data[3][2] - M(3,2);
|
|
Diff(3,3) = data[3][3] - M(3,3);
|
|
|
|
return Diff;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
void FGMatrix33::operator-=(const FGMatrix33 &M)
|
|
{
|
|
data[1][1] -= M(1,1);
|
|
data[1][2] -= M(1,2);
|
|
data[1][3] -= M(1,3);
|
|
data[2][1] -= M(2,1);
|
|
data[2][2] -= M(2,2);
|
|
data[2][3] -= M(2,3);
|
|
data[3][1] -= M(3,1);
|
|
data[3][2] -= M(3,2);
|
|
data[3][3] -= M(3,3);
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M)
|
|
{
|
|
FGMatrix33 Sum;
|
|
|
|
Sum(1,1) = data[1][1] + M(1,1);
|
|
Sum(1,2) = data[1][2] + M(1,2);
|
|
Sum(1,3) = data[1][3] + M(1,3);
|
|
Sum(2,1) = data[2][1] + M(2,1);
|
|
Sum(2,2) = data[2][2] + M(2,2);
|
|
Sum(2,3) = data[2][3] + M(2,3);
|
|
Sum(3,1) = data[3][1] + M(3,1);
|
|
Sum(3,2) = data[3][2] + M(3,2);
|
|
Sum(3,3) = data[3][3] + M(3,3);
|
|
|
|
return Sum;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
void FGMatrix33::operator+=(const FGMatrix33 &M)
|
|
{
|
|
data[1][1] += M(1,1);
|
|
data[1][2] += M(1,2);
|
|
data[1][3] += M(1,3);
|
|
data[2][1] += M(2,1);
|
|
data[2][2] += M(2,2);
|
|
data[2][3] += M(2,3);
|
|
data[3][1] += M(3,1);
|
|
data[3][2] += M(3,2);
|
|
data[3][3] += M(3,3);
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33 FGMatrix33::operator*(const double scalar)
|
|
{
|
|
FGMatrix33 Product;
|
|
|
|
Product(1,1) = data[1][1] * scalar;
|
|
Product(1,2) = data[1][2] * scalar;
|
|
Product(1,3) = data[1][3] * scalar;
|
|
Product(2,1) = data[2][1] * scalar;
|
|
Product(2,2) = data[2][2] * scalar;
|
|
Product(2,3) = data[2][3] * scalar;
|
|
Product(3,1) = data[3][1] * scalar;
|
|
Product(3,2) = data[3][2] * scalar;
|
|
Product(3,3) = data[3][3] * scalar;
|
|
|
|
return Product;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33 operator*(double scalar, FGMatrix33 &M)
|
|
{
|
|
FGMatrix33 Product;
|
|
|
|
Product(1,1) = M(1,1) * scalar;
|
|
Product(1,2) = M(1,2) * scalar;
|
|
Product(1,3) = M(1,3) * scalar;
|
|
Product(2,1) = M(2,1) * scalar;
|
|
Product(2,2) = M(2,2) * scalar;
|
|
Product(2,3) = M(2,3) * scalar;
|
|
Product(3,1) = M(3,1) * scalar;
|
|
Product(3,2) = M(3,2) * scalar;
|
|
Product(3,3) = M(3,3) * scalar;
|
|
|
|
return Product;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
void FGMatrix33::operator*=(const double scalar)
|
|
{
|
|
data[1][1] *= scalar;
|
|
data[1][2] *= scalar;
|
|
data[1][3] *= scalar;
|
|
data[2][1] *= scalar;
|
|
data[2][2] *= scalar;
|
|
data[2][3] *= scalar;
|
|
data[3][1] *= scalar;
|
|
data[3][2] *= scalar;
|
|
data[3][3] *= scalar;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M)
|
|
{
|
|
FGMatrix33 Product;
|
|
|
|
Product(1,1) = data[1][1]*M(1,1) + data[1][2]*M(2,1) + data[1][3]*M(3,1);
|
|
Product(1,2) = data[1][1]*M(1,2) + data[1][2]*M(2,2) + data[1][3]*M(3,2);
|
|
Product(1,3) = data[1][1]*M(1,3) + data[1][2]*M(2,3) + data[1][3]*M(3,3);
|
|
Product(2,1) = data[2][1]*M(1,1) + data[2][2]*M(2,1) + data[2][3]*M(3,1);
|
|
Product(2,2) = data[2][1]*M(1,2) + data[2][2]*M(2,2) + data[2][3]*M(3,2);
|
|
Product(2,3) = data[2][1]*M(1,3) + data[2][2]*M(2,3) + data[2][3]*M(3,3);
|
|
Product(3,1) = data[3][1]*M(1,1) + data[3][2]*M(2,1) + data[3][3]*M(3,1);
|
|
Product(3,2) = data[3][1]*M(1,2) + data[3][2]*M(2,2) + data[3][3]*M(3,2);
|
|
Product(3,3) = data[3][1]*M(1,3) + data[3][2]*M(2,3) + data[3][3]*M(3,3);
|
|
|
|
return Product;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
void FGMatrix33::operator*=(const FGMatrix33& M)
|
|
{
|
|
double a,b,c;
|
|
|
|
a = data[1][1]; b=data[1][2]; c=data[1][3];
|
|
data[1][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
|
|
data[1][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
|
|
data[1][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
|
|
|
|
a = data[2][1]; b=data[2][2]; c=data[2][3];
|
|
data[2][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
|
|
data[2][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
|
|
data[2][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
|
|
|
|
a = data[3][1]; b=data[3][2]; c=data[3][3];
|
|
data[3][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
|
|
data[3][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
|
|
data[3][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGMatrix33 FGMatrix33::operator/(const double scalar)
|
|
{
|
|
FGMatrix33 Quot;
|
|
|
|
if( scalar != 0 ) {
|
|
double tmp = 1.0/scalar;
|
|
Quot(1,1) = data[1][1] * tmp;
|
|
Quot(1,2) = data[1][2] * tmp;
|
|
Quot(1,3) = data[1][3] * tmp;
|
|
Quot(2,1) = data[2][1] * tmp;
|
|
Quot(2,2) = data[2][2] * tmp;
|
|
Quot(2,3) = data[2][3] * tmp;
|
|
Quot(3,1) = data[3][1] * tmp;
|
|
Quot(3,2) = data[3][2] * tmp;
|
|
Quot(3,3) = data[3][3] * tmp;
|
|
} else {
|
|
MatrixException mE;
|
|
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
|
|
throw mE;
|
|
}
|
|
return Quot;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
void FGMatrix33::operator/=(const double scalar)
|
|
{
|
|
if( scalar != 0 ) {
|
|
double tmp = 1.0/scalar;
|
|
data[1][1] *= tmp;
|
|
data[1][2] *= tmp;
|
|
data[1][3] *= tmp;
|
|
data[2][1] *= tmp;
|
|
data[2][2] *= tmp;
|
|
data[2][3] *= tmp;
|
|
data[3][1] *= tmp;
|
|
data[3][2] *= tmp;
|
|
data[3][3] *= tmp;
|
|
} else {
|
|
MatrixException mE;
|
|
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
|
|
throw mE;
|
|
}
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
void FGMatrix33::T(void)
|
|
{
|
|
for (unsigned int i=1; i<=3; i++) {
|
|
for (unsigned int j=i+1; j<=3; j++) {
|
|
double tmp = data[i][j];
|
|
data[i][j] = data[j][i];
|
|
data[j][i] = tmp;
|
|
}
|
|
}
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& Col)
|
|
{
|
|
FGColumnVector3 Product;
|
|
|
|
Product(1) = data[1][1]*Col(1) + data[1][2]*Col(2) + data[1][3]*Col(3);
|
|
Product(2) = data[2][1]*Col(1) + data[2][2]*Col(2) + data[2][3]*Col(3);
|
|
Product(3) = data[3][1]*Col(1) + data[3][2]*Col(2) + data[3][3]*Col(3);
|
|
|
|
return Product;
|
|
}
|
|
|
|
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
// 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
|
|
}
|
|
}
|
|
|