fly/flightgear
1
0
Fork 0
Code Activity
flightgear/src/FDM/JSBSim/math/FGMatrix33.cpp
timoore bfc1967529 Revert most iostream-related changes to JSBSim 
So many files in JSBSim use cout that it is not productive to remove
iostream from header files.
2008-06-06 21:21:57 +00:00

410 lines
13 KiB
C++
Raw Blame History

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGMatrix33.cpp
Author: Tony Peden, Jon Berndt, Mathias Frolich
Date started: 1998
Purpose: FGMatrix33 class
Called by: Various
------------- Copyright (C) 1998 by the authors above -------------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
details.
You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU Lesser General Public License can also be found on
the world wide web at http://www.gnu.org.
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;
}
}
}
}
View git blame Copy permalink