fly/flightgear
1
0
Fork 0
Code Activity

Initial revision.

Browse source
This commit is contained in:
curt 2001-10-05 20:16:16 +00:00
parent 85371ee361
commit 2297c706e8
8 changed files with 1679 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

322
src/FDM/JSBSim/FGColumnVector3.cpp Normal file
View file

@ -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;
}

122
src/FDM/JSBSim/FGColumnVector3.h Normal file
View file

@ -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 <stdlib.h>
#ifdef FGFS
# include <math.h>
# include <simgear/compiler.h>
# 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 <string>
# if defined(sgi) && !defined(__GNUC__)
# include <fstream.h>
# include <math.h>
# include <iostream.h>
# else
# include <fstream>
# include <cmath>
# include <iostream>
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

315
src/FDM/JSBSim/FGColumnVector4.cpp Normal file
View file

@ -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;
}

119
src/FDM/JSBSim/FGColumnVector4.h Normal file
View file

@ -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 <stdlib.h>
#ifdef FGFS
# include <math.h>
# include <simgear/compiler.h>
# 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 <string>
# if defined (sgi) && !defined(__GNUC__)
# include <fstream.h>
# include <math.h>
# include <iostream.h>
# else
# include <fstream>
# include <cmath>
# include <iostream>
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

60
src/FDM/JSBSim/FGJSBBase.cpp Normal file
View file

@ -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;

122
src/FDM/JSBSim/FGJSBBase.h Normal file
View file

@ -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 <simgear/compiler.h>
# include <math.h>
#else
# if defined(sgi) && !defined(__GNUC__)
# include <math.h>
# else
# include <cmath>
# endif
#endif
#ifndef M_PI
# include <simgear/constants.h>
# 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

481
src/FDM/JSBSim/FGMatrix33.cpp Normal file
View file

@ -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
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

138
src/FDM/JSBSim/FGMatrix33.h Normal file
View file

@ -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 <stdlib.h>
#ifdef FGFS
# include <math.h>
# include <simgear/compiler.h>
# 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 <string>
# if defined(sgi) && !defined(__GNUC__)
# include <fstream.h>
# include <math.h>
# include <iostream.h>
# else
# include <fstream>
# include <cmath>
# include <iostream>
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