diff --git a/src/FDM/JSBSim/FGColumnVector3.cpp b/src/FDM/JSBSim/FGColumnVector3.cpp new file mode 100644 index 000000000..45fe14c71 --- /dev/null +++ b/src/FDM/JSBSim/FGColumnVector3.cpp @@ -0,0 +1,322 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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 "FGColumnVector3.h" +#include "FGMatrix33.h" + + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_COLUMNVECTOR3; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +FGColumnVector3::FGColumnVector3(void) +{ + data = new double[4]; + rowCtr = 1; + //cout << "Allocated: " << data << endl; + //if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3::FGColumnVector3(int m) +{ + data = new double[4]; + rowCtr = 1; + data[1]=0;data[2]=0;data[3]=0; + //cout << "Allocated: " << data << endl; + //if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3::~FGColumnVector3(void) +{ + //cout << "Freed: " << data << endl; + delete[] data; + data = NULL; + if (debug_lvl & 2) cout << "Destroyed: FGColumnVector3" << endl; +} + + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3::FGColumnVector3(const FGColumnVector3& b) +{ + data = new double[4]; + data[1] = b.data[1]; + data[2] = b.data[2]; + data[3] = b.data[3]; + rowCtr = 1; + + if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::operator=(const FGColumnVector3& b) +{ + data = new double[4]; + data[1] = b.data[1]; + data[2] = b.data[2]; + data[3] = b.data[3]; + rowCtr = 1; + + if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl; + + return *this; +} + + +/* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +double& FGColumnVector3::operator()(int m) const +{ + return data[m]; +} + */ + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +/* FGColumnVector3 operator*(const FGMatrix33& Mat, FGColumnVector3& Col) +{ + FGColumnVector3 Product; + + Product(1) = Col(1)*Mat(1,1) + Col(2)*Mat(1,2) + Col(3)*Mat(1,3); + Product(2) = Col(1)*Mat(2,1) + Col(2)*Mat(2,2) + Col(3)*Mat(2,3); + Product(3) = Col(1)*Mat(3,1) + Col(2)*Mat(3,2) + Col(3)*Mat(3,3); + + return Product; +} + */ + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::operator+(const FGColumnVector3& C) +{ + FGColumnVector3 Sum; + Sum(1) = C(1) + data[1]; + Sum(2) = C(2) + data[2]; + Sum(3) = C(3) + data[3]; + + return Sum; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector3::operator+=(const FGColumnVector3& C) +{ + data[1] += C(1); + data[2] += C(2); + data[3] += C(3); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::operator*(const double scalar) +{ + FGColumnVector3 Product; + + Product(1) = scalar * data[1]; + Product(2) = scalar * data[2]; + Product(3) = scalar * data[3]; + + return Product; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector3::operator*=(const double scalar) +{ + data[1] *= scalar; + data[2] *= scalar; + data[3] *= scalar; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::operator-(const FGColumnVector3& V) +{ + + FGColumnVector3 Diff; + + Diff(1) = data[1] - V(1); + Diff(2) = data[2] - V(2); + Diff(3) = data[3] - V(3); + + return Diff; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector3::operator-=(const FGColumnVector3& V) +{ + data[1] -= V(1); + data[2] -= V(2); + data[3] -= V(3); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::operator/(const double scalar) +{ + FGColumnVector3 Quotient; + + if (scalar != 0) { + double tmp = 1.0/scalar; + Quotient(1) = data[1] * tmp; + Quotient(2) = data[2] * tmp; + Quotient(3) = data[3] * tmp; + } else { + cerr << "Attempt to divide by zero in method FGColumnVector3::operator/(const double scalar), object " << this << endl; + } + return Quotient; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector3::operator/=(const double scalar) +{ + FGColumnVector3 Quotient; + + if (scalar != 0) { + double tmp = 1.0/scalar; + data[1] *= tmp; + data[2] *= tmp; + data[3] *= tmp; + } else { + cerr << "Attempt to divide by zero in method FGColumnVector3::operator/=(const double scalar), object " << this << endl; + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 operator*(const double scalar, const FGColumnVector3& C) +{ + FGColumnVector3 Product; + + Product(1) = scalar * C(1); + Product(2) = scalar * C(2); + Product(3) = scalar * C(3); + + return Product; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +float FGColumnVector3::Magnitude(void) +{ + double num; + + if ((data[1] == 0.00) && + (data[2] == 0.00) && + (data[3] == 0.00)) + { + return 0.00; + } else { + num = data[1]*data[1]; + num += data[2]*data[2]; + num += data[3]*data[3]; + return sqrt(num); + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::Normalize(void) +{ + double Mag = Magnitude(); + + if (Mag != 0) { + Mag = 1.0/Mag; + data[1] *= Mag; + data[2] *= Mag; + data[3] *= Mag; + } + + return *this; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::operator*(const FGColumnVector3& V) +{ + FGColumnVector3 Product; + + Product(1) = data[2] * V(3) - data[3] * V(2); + Product(2) = data[3] * V(1) - data[1] * V(3); + Product(3) = data[1] * V(2) - data[2] * V(1); + + return Product; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector3::operator*=(const FGColumnVector3& V) +{ + double a,b,c; + a = data[1]; b=data[2]; c=data[3]; + + data[1] = b * V(3) - c * V(2); + data[2] = c * V(1) - a * V(3); + data[3] = a * V(2) - b * V(1); + +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V) +{ + FGColumnVector3 Product; + + Product(1) = data[1] * V(1); + Product(2) = data[2] * V(2); + Product(3) = data[3] * V(3); + + return Product; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector3::Debug(void) +{ + //TODO: Add your source code here +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +ostream& operator<<(ostream& os, const FGColumnVector3& col) +{ + os << col(1) << " , " << col(2) << " , " << col(3); + return os; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector3& FGColumnVector3::operator<<(const float ff) +{ + data[rowCtr] = ff; + if (++rowCtr > 3 ) + rowCtr = 1; + return *this; +} diff --git a/src/FDM/JSBSim/FGColumnVector3.h b/src/FDM/JSBSim/FGColumnVector3.h new file mode 100644 index 000000000..cae83b6f0 --- /dev/null +++ b/src/FDM/JSBSim/FGColumnVector3.h @@ -0,0 +1,122 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +Header: FGMatrix33.h +Author: Originally by Tony Peden [formatted and adapted here by Jon Berndt] +Date started: Unknown + +HISTORY +-------------------------------------------------------------------------------- +??/??/?? TP Created +03/16/2000 JSB Added exception throwing + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGCOLUMNVECTOR3_H +#define FGCOLUMNVECTOR3_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include +#ifdef FGFS +# include +# include +# include STL_STRING +# include STL_FSTREAM +# include STL_IOSTREAM + SG_USING_STD(string); +# if !defined(SG_HAVE_NATIVE_SGI_COMPILERS) + SG_USING_STD(ostream); + SG_USING_STD(istream); + SG_USING_STD(cerr); + SG_USING_STD(cout); + SG_USING_STD(endl); +# endif +#else +# include +# if defined(sgi) && !defined(__GNUC__) +# include +# include +# include +# else +# include +# include +# include + using std::ostream; + using std::istream; + using std::cerr; + using std::cout; + using std::endl; +# endif + using std::string; +#endif + +#include "FGMatrix33.h" +#include "FGJSBBase.h" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_COLUMNVECTOR3 "$Id$" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGMatrix33; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DECLARATION: FGColumnVector3 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGColumnVector3 : public FGJSBBase +{ +public: + FGColumnVector3(void); + FGColumnVector3(int m); + FGColumnVector3(const FGColumnVector3& b); + ~FGColumnVector3(void); + + FGColumnVector3 operator=(const FGColumnVector3& b); + + FGColumnVector3 operator*(const double scalar); + FGColumnVector3 operator*(const FGColumnVector3& V); // Cross product operator + FGColumnVector3 operator/(const double scalar); + FGColumnVector3 operator+(const FGColumnVector3& B); // must not return reference + FGColumnVector3 operator-(const FGColumnVector3& B); + + void operator-=(const FGColumnVector3 &B); + void operator+=(const FGColumnVector3 &B); + void operator*=(const FGColumnVector3 &B); + void operator*=(const double scalar); + void operator/=(const double scalar); + + FGColumnVector3& operator<<(const float ff); + + inline void InitMatrix(void) { data[1]=0; data[2]=0; data[3]=0; } + inline void InitMatrix(float ff) { data[1]=ff; data[2]=ff; data[3]=ff; } + + float Magnitude(void); + FGColumnVector3 Normalize(void); + + friend FGColumnVector3 operator*(const double scalar, const FGColumnVector3& A); + //friend FGColumnVector3 operator*(const FGMatrix33& M, FGColumnVector3& V); + + friend ostream& operator<<(ostream& os, const FGColumnVector3& col); + + inline double& operator()(int m) const { return data[m]; } + + FGColumnVector3 multElementWise(const FGColumnVector3& V); + +private: + double *data; + int rowCtr; + void Debug(void); +}; + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +#endif diff --git a/src/FDM/JSBSim/FGColumnVector4.cpp b/src/FDM/JSBSim/FGColumnVector4.cpp new file mode 100644 index 000000000..5a0e73868 --- /dev/null +++ b/src/FDM/JSBSim/FGColumnVector4.cpp @@ -0,0 +1,315 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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 "FGColumnVector4.h" + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_COLUMNVECTOR4; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +FGColumnVector4::FGColumnVector4(void) +{ + data = new double[5]; + rowCtr = 1; + //cout << "Allocated: " << data << endl; + if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4::FGColumnVector4(int m) +{ + data = new double[5]; + rowCtr = 1; + data[1]=0;data[2]=0;data[3]=0;data[4]=0; + //cout << "Allocated: " << data << endl; + if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4::~FGColumnVector4(void) +{ + //cout << "Freed: " << data << endl; + delete[] data; + data = NULL; + if (debug_lvl & 2) cout << "Destroyed: FGColumnVector4" << endl; +} + + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4::FGColumnVector4(const FGColumnVector4& b) +{ + data = new double[5]; + data[1] = b.data[1]; + data[2] = b.data[2]; + data[3] = b.data[3]; + data[4] = b.data[4]; + + rowCtr = 1; + + if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4 FGColumnVector4::operator=(const FGColumnVector4& b) +{ + data = new double[5]; + data[1] = b.data[1]; + data[2] = b.data[2]; + data[3] = b.data[3]; + data[4] = b.data[4]; + rowCtr = 1; + + if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl; + + return *this; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4 FGColumnVector4::operator+(const FGColumnVector4& C) +{ + FGColumnVector4 Sum; + + Sum(1) = C(1) + data[1]; + Sum(2) = C(2) + data[2]; + Sum(3) = C(3) + data[3]; + Sum(4) = C(4) + data[4]; + return Sum; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector4::operator+=(const FGColumnVector4& C) +{ + data[1] += C(1); + data[2] += C(2); + data[3] += C(3); + data[4] += C(4); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4 FGColumnVector4::operator*(const double scalar) +{ + FGColumnVector4 Product; + + Product(1) = scalar * data[1]; + Product(2) = scalar * data[2]; + Product(3) = scalar * data[3]; + Product(4) = scalar * data[4]; + + return Product; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector4::operator*=(const double scalar) +{ + data[1] *= scalar; + data[2] *= scalar; + data[3] *= scalar; + data[4] *= scalar; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4 FGColumnVector4::operator-(const FGColumnVector4& V) +{ + + FGColumnVector4 Diff; + + Diff(1) = data[1] - V(1); + Diff(2) = data[2] - V(2); + Diff(3) = data[3] - V(3); + Diff(4) = data[4] - V(4); + + return Diff; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector4::operator-=(const FGColumnVector4& V) +{ + data[1] -= V(1); + data[2] -= V(2); + data[3] -= V(3); + data[4] -= V(4); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4 FGColumnVector4::operator/(const double scalar) +{ + FGColumnVector4 Quotient; + + if (scalar != 0) { + double tmp = 1.0/scalar; + Quotient(1) = data[1] * tmp; + Quotient(2) = data[2] * tmp; + Quotient(3) = data[3] * tmp; + Quotient(4) = data[4] * tmp; + } else { + cerr << "Attempt to divide by zero in method FGColumnVector4::operator/(const double scalar), object " << this << endl; + } + return Quotient; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector4::operator/=(const double scalar) +{ + FGColumnVector4 Quotient; + + if (scalar != 0) { + double tmp = 1.0/scalar; + data[1] *= tmp; + data[2] *= tmp; + data[3] *= tmp; + data[4] *= tmp; + } else { + cerr << "Attempt to divide by zero in method FGColumnVector4::operator/=(const double scalar), object " << this << endl; + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4 operator*(const double scalar, const FGColumnVector4& C) +{ + FGColumnVector4 Product; + + Product(1) = scalar * C(1); + Product(2) = scalar * C(2); + Product(3) = scalar * C(3); + Product(4) = scalar * C(4); + return Product; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +float FGColumnVector4::Magnitude(void) +{ + double num; + + if ((data[1] == 0.00) && + (data[2] == 0.00) && + (data[3] == 0.00) && + (data[4] == 0.00)) + { + return 0.00; + } else { + num = data[1]*data[1]; + num += data[2]*data[2]; + num += data[3]*data[3]; + num += data[4]*data[4]; + return sqrt(num); + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4 FGColumnVector4::Normalize(void) +{ + double Mag = Magnitude(); + + if (Mag != 0) { + Mag = 1.0/Mag; + data[1] *= Mag; + data[2] *= Mag; + data[3] *= Mag; + data[4] *= Mag; + } + + return *this; +} + +/* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4 FGColumnVector4::operator*(const FGColumnVector4& V) +{ + FGColumnVector4 Product; + + Product(1) = data[2] * V(3) - data[3] * V(2); + Product(2) = data[3] * V(1) - data[1] * V(3); + Product(3) = data[1] * V(2) - data[2] * V(1); + + return Product; +} */ + + +/* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector4::operator*=(const FGColumnVector4& V) +{ + double a,b,c; + a = data[1]; b=data[2]; c=data[3]; + + data[1] = b * V(3) - c * V(2); + data[2] = c * V(1) - a * V(3); + data[3] = a * V(2) - b * V(1); + +} + */ + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4 FGColumnVector4::multElementWise(const FGColumnVector4& V) +{ + FGColumnVector4 Product; + + Product(1) = data[1] * V(1); + Product(2) = data[2] * V(2); + Product(3) = data[3] * V(3); + Product(4) = data[4] * V(4); + + return Product; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGColumnVector4::Debug(void) +{ + //TODO: Add your source code here +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +ostream& operator<<(ostream& os, FGColumnVector4& col) +{ + os << col(1) << " , " << col(2) << " , " << col(3) << " , " << col(4); + return os; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGColumnVector4& FGColumnVector4::operator<<(const float ff) +{ + data[rowCtr] = ff; + if (++rowCtr > 4 ) + rowCtr = 1; + return *this; +} + diff --git a/src/FDM/JSBSim/FGColumnVector4.h b/src/FDM/JSBSim/FGColumnVector4.h new file mode 100644 index 000000000..a90c021c2 --- /dev/null +++ b/src/FDM/JSBSim/FGColumnVector4.h @@ -0,0 +1,119 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +Header: FGMatrix33.h +Author: Originally by Tony Peden [formatted and adapted here by Jon Berndt] +Date started: Unknown + +HISTORY +-------------------------------------------------------------------------------- +??/??/?? TP Created +03/16/2000 JSB Added exception throwing + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGCOLUMNVECTOR4_H +#define FGCOLUMNVECTOR4_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include +#ifdef FGFS +# include +# include +# include STL_STRING +# include STL_FSTREAM +# include STL_IOSTREAM + SG_USING_STD(string); +# if !defined(SG_HAVE_NATIVE_SGI_COMPILERS) + SG_USING_STD(ostream); + SG_USING_STD(istream); + SG_USING_STD(cerr); + SG_USING_STD(cout); + SG_USING_STD(endl); +# endif +#else +# include +# if defined (sgi) && !defined(__GNUC__) +# include +# include +# include +# else +# include +# include +# include + using std::ostream; + using std::istream; + using std::cerr; + using std::cout; + using std::endl; +# endif + using std::string; +#endif + +#include "FGJSBBase.h" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_COLUMNVECTOR4 "$Id$" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DECLARATION: FGColumnVector4 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGColumnVector4 : public FGJSBBase +{ +public: + FGColumnVector4(void); + FGColumnVector4(int m); + FGColumnVector4(const FGColumnVector4& b); + ~FGColumnVector4(void); + + FGColumnVector4 operator=(const FGColumnVector4& b); + + FGColumnVector4 operator*(const double scalar); + //FGColumnVector4 operator*(const FGColumnVector4& V); // Cross product operator + FGColumnVector4 operator/(const double scalar); + FGColumnVector4 operator+(const FGColumnVector4& B); // must not return reference + FGColumnVector4 operator-(const FGColumnVector4& B); + + void operator-=(const FGColumnVector4 &B); + void operator+=(const FGColumnVector4 &B); + //void operator*=(const FGColumnVector4 &B); + void operator*=(const double scalar); + void operator/=(const double scalar); + + inline double& operator()(int m) const { return data[m]; } + + FGColumnVector4& operator<<(const float ff); + + inline void InitMatrix(void) { data[1]=0; data[2]=0; data[3]=0; } + inline void InitMatrix(float ff) { data[1]=ff; data[2]=ff; data[3]=ff; } + + float Magnitude(void); + FGColumnVector4 Normalize(void); + + friend FGColumnVector4 operator*(const double scalar, const FGColumnVector4& A); + + friend ostream& operator<<(ostream& os, FGColumnVector4& col); + + + FGColumnVector4 multElementWise(const FGColumnVector4& V); + +private: + double *data; + int rowCtr; + void Debug(void); +}; + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +#endif diff --git a/src/FDM/JSBSim/FGJSBBase.cpp b/src/FDM/JSBSim/FGJSBBase.cpp new file mode 100644 index 000000000..1b8a36d49 --- /dev/null +++ b/src/FDM/JSBSim/FGJSBBase.cpp @@ -0,0 +1,60 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Module: FGJSBBase.cpp + Author: Jon S. Berndt + Date started: 07/01/01 + Purpose: Encapsulates the JSBBase object + + ------------- Copyright (C) 2001 Jon S. Berndt (jsb@hal-pc.org) ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU 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 General Public License for more + details. + + You should have received a copy of the GNU 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 General Public License can also be found on + the world wide web at http://www.gnu.org. + +FUNCTIONAL DESCRIPTION +-------------------------------------------------------------------------------- + +HISTORY +-------------------------------------------------------------------------------- +07/01/01 JSB Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include "FGJSBBase.h" + +static const char *IdSrc = "$Id$"; +static const char *IdHdr = ID_JSBBASE; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS IMPLEMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +char FGJSBBase::highint[5] = {27, '[', '1', 'm', '\0' }; +char FGJSBBase::halfint[5] = {27, '[', '2', 'm', '\0' }; +char FGJSBBase::normint[6] = {27, '[', '2', '2', 'm', '\0' }; +char FGJSBBase::reset[5] = {27, '[', '0', 'm', '\0' }; +char FGJSBBase::underon[5] = {27, '[', '4', 'm', '\0' }; +char FGJSBBase::underoff[6] = {27, '[', '2', '4', 'm', '\0' }; +char FGJSBBase::fgblue[6] = {27, '[', '3', '4', 'm', '\0' }; +char FGJSBBase::fgcyan[6] = {27, '[', '3', '6', 'm', '\0' }; +char FGJSBBase::fgred[6] = {27, '[', '3', '1', 'm', '\0' }; +char FGJSBBase::fggreen[6] = {27, '[', '3', '2', 'm', '\0' }; +char FGJSBBase::fgdef[6] = {27, '[', '3', '9', 'm', '\0' }; + +short FGJSBBase::debug_lvl = 0; + diff --git a/src/FDM/JSBSim/FGJSBBase.h b/src/FDM/JSBSim/FGJSBBase.h new file mode 100644 index 000000000..a852258e0 --- /dev/null +++ b/src/FDM/JSBSim/FGJSBBase.h @@ -0,0 +1,122 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Header: FGJSBBase.h + Author: Jon S. Berndt + Date started: 07/01/01 + + ------------- Copyright (C) 2001 Jon S. Berndt (jsb@hal-pc.org) ------------- + + This program is free software; you can redistribute it and/or modify it under + the terms of the GNU 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 General Public License for more + details. + + You should have received a copy of the GNU 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 General Public License can also be found on + the world wide web at http://www.gnu.org. + +HISTORY +-------------------------------------------------------------------------------- +07/01/01 JSB Created + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGJSBBASE_H +#define FGJSBBASE_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifdef FGFS +# include +# include +#else +# if defined(sgi) && !defined(__GNUC__) +# include +# else +# include +# endif +#endif + +#ifndef M_PI +# include +# define M_PI SG_PI +#endif + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_JSBBASE "$Id$" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs] +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/** JSBSim Base class. + @author Jon S. Berndt + @version $Id$ +*/ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DECLARATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGJSBBase { +public: + /// Constructor for FGJSBBase. + FGJSBBase() {}; + + /// Destructor for FGJSBBase + virtual ~FGJSBBase() {}; + + enum {eL = 1, eM, eN }; + enum {eP = 1, eQ, eR }; + enum {eU = 1, eV, eW }; + enum {eX = 1, eY, eZ }; + enum {ePhi = 1, eTht, ePsi }; + enum {eDrag = 1, eSide, eLift }; + enum {eRoll = 1, ePitch, eYaw }; + enum {eNorth = 1, eEast, eDown }; + + static char highint[5]; + static char halfint[5]; + static char normint[6]; + static char reset[5]; + static char underon[5]; + static char underoff[6]; + static char fgblue[6]; + static char fgcyan[6]; + static char fgred[6]; + static char fggreen[6]; + static char fgdef[6]; + +protected: + virtual void Debug(void) {}; + + static short debug_lvl; + static int frame; +}; + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +#endif + diff --git a/src/FDM/JSBSim/FGMatrix33.cpp b/src/FDM/JSBSim/FGMatrix33.cpp new file mode 100644 index 000000000..0c69dd7a8 --- /dev/null +++ b/src/FDM/JSBSim/FGMatrix33.cpp @@ -0,0 +1,481 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +double** FGalloc(void) +{ + double **A; + + A = new double *[4]; + if (!A) return NULL; + + double *tmp; + tmp = new double [16]; + + if (!tmp) { + delete A; + return NULL; + } + A[0] = tmp; + A[1] = tmp + 4; + A[2] = tmp + 8; + A[3] = tmp + 12; +#if 0 + A[0] = new double [4]; + if (!A[0]) return NULL; + A[1] = new double [4]; + if (!A[1]) return NULL; + A[2] = new double [4]; + if (!A[2]) return NULL; + A[3] = new double [4]; + if (!A[3]) return NULL; +#endif + + return A; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void dealloc(double **A) +{ + delete[] A[0]; + delete[] A; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33::FGMatrix33(void) +{ + data=FGalloc(); + InitMatrix(); + rowCtr = colCtr = 1; + + if (debug_lvl & 2) cout << "Instantiated: FGMatrix33" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33::FGMatrix33(int r, int c) +{ + data=FGalloc(); + InitMatrix(); + rowCtr = colCtr = 1; + + if (debug_lvl & 2) cout << "Instantiated: FGMatrix33" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33::FGMatrix33(const FGMatrix33& M) +{ + rowCtr = colCtr = 1; + width = M.width; + prec = M.prec; + delim = M.delim; + origin = M.origin; + + data=FGalloc(); + + 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]; + + if (debug_lvl & 2) cout << "Instantiated: FGMatrix33" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGMatrix33::~FGMatrix33(void) +{ + dealloc(data); + rowCtr = colCtr = 1; + + if (debug_lvl & 2) cout << "Destroyed: FGMatrix33" << endl; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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 float 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) { + if (data != NULL) dealloc(data); + + width = M.width; + prec = M.prec; + delim = M.delim; + origin = M.origin; + + data=FGalloc(); + + 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::SetOParams(char delim,int width,int prec,int origin) +{ + FGMatrix33::delim = delim; + FGMatrix33::width = width; + FGMatrix33::prec = prec; + FGMatrix33::origin = origin; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +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) +{ + float 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; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGMatrix33::Debug(void) +{ + //TODO: Add your source code here +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + diff --git a/src/FDM/JSBSim/FGMatrix33.h b/src/FDM/JSBSim/FGMatrix33.h new file mode 100644 index 000000000..be38a1b28 --- /dev/null +++ b/src/FDM/JSBSim/FGMatrix33.h @@ -0,0 +1,138 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +Header: FGMatrix33.h +Author: Originally by Tony Peden [formatted and adapted here by Jon Berndt] +Date started: Unknown + +HISTORY +-------------------------------------------------------------------------------- +??/??/?? TP Created +03/16/2000 JSB Added exception throwing + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef FGMATRIX33_H +#define FGMATRIX33_H + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include +#ifdef FGFS +# include +# include +# include STL_STRING +# include STL_FSTREAM +# include STL_IOSTREAM + SG_USING_STD(string); +# if !defined(SG_HAVE_NATIVE_SGI_COMPILERS) + SG_USING_STD(ostream); + SG_USING_STD(istream); + SG_USING_STD(cerr); + SG_USING_STD(cout); + SG_USING_STD(endl); +# endif +#else +# include +# if defined(sgi) && !defined(__GNUC__) +# include +# include +# include +# else +# include +# include +# include + using std::ostream; + using std::istream; + using std::cerr; + using std::cout; + using std::endl; +# endif + using std::string; +#endif + +#include "FGColumnVector3.h" +#include "FGJSBBase.h" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_MATRIX33 "$Id$" + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGColumnVector3; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DECLARATION: MatrixException +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class MatrixException : public FGJSBBase +{ +public: + string Message; +}; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DECLARATION: FGMatrix33 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGMatrix33 : public FGJSBBase +{ +public: + FGMatrix33(void); + FGMatrix33(int r, int c); + FGMatrix33(const FGMatrix33& A); + ~FGMatrix33(void); + + FGMatrix33& operator=(const FGMatrix33& A); + inline double& operator()(unsigned int row, unsigned int col) const {return data[row][col];} + + FGColumnVector3 operator*(const FGColumnVector3& Col); + + inline unsigned int Rows(void) const { return 3; } + inline unsigned int Cols(void) const { return 3; } + + void T(void); + void InitMatrix(void); + void InitMatrix(double value); + + //friend FGMatrix33 operator*(double scalar,FGMatrix33& A); + + FGMatrix33 operator-(const FGMatrix33& B); + FGMatrix33 operator+(const FGMatrix33& B); + FGMatrix33 operator*(const FGMatrix33& B); + FGMatrix33 operator*(const double scalar); + FGMatrix33 operator/(const double scalar); + FGMatrix33& operator<<(const float ff); + + friend ostream& operator<<(ostream& os, const FGMatrix33& M); + friend istream& operator>>(istream& is, FGMatrix33& M); + + void operator-=(const FGMatrix33 &B); + void operator+=(const FGMatrix33 &B); + void operator*=(const FGMatrix33 &B); + void operator*=(const double scalar); + void operator/=(const double scalar); + + + void SetOParams(char delim,int width,int prec, int origin=0); + +protected: + double **data; + +private: + char delim; + int width,prec,origin; + void TransposeSquare(void); + unsigned int rowCtr, colCtr; + void Debug(void); +}; + +#endif