Fix an issue where math.h doesn't define sqrt in namespace std (at least on MSVC) and remove DOS line-endings.
This commit is contained in:
ehofman
parent
d77e8c75a1
commit
c37874e140
11 changed files with 3051 additions and 3051 deletions
|
|
@ -1,145 +1,145 @@
|
|||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGColumnVector3.cpp
|
||||
Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
|
||||
Date started: 1998
|
||||
Purpose: FGColumnVector3 class
|
||||
Called by: Various
|
||||
|
||||
FUNCTIONAL DESCRIPTION
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
HISTORY
|
||||
--------------------------------------------------------------------------------
|
||||
??/??/?? TP Created
|
||||
03/16/2000 JSB Added exception throwing
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#include "FGColumnVector3.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_COLUMNVECTOR3;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
FGColumnVector3::FGColumnVector3(void)
|
||||
{
|
||||
data[0] = data[1] = data[2] = 0.0;
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3 FGColumnVector3::operator/(const double scalar) const
|
||||
{
|
||||
if (scalar != 0.0)
|
||||
return operator*( 1.0/scalar );
|
||||
|
||||
cerr << "Attempt to divide by zero in method "
|
||||
"FGColumnVector3::operator/(const double scalar), "
|
||||
"object " << this << endl;
|
||||
return FGColumnVector3();
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3& FGColumnVector3::operator/=(const double scalar)
|
||||
{
|
||||
if (scalar != 0.0)
|
||||
operator*=( 1.0/scalar );
|
||||
else
|
||||
cerr << "Attempt to divide by zero in method "
|
||||
"FGColumnVector3::operator/=(const double scalar), "
|
||||
"object " << this << endl;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
double FGColumnVector3::Magnitude(void) const
|
||||
{
|
||||
if (data[1] == 0.0 && data[2] == 0.0 && data[3] == 0.0)
|
||||
return 0.0;
|
||||
else
|
||||
return sqrt( Entry(1)*Entry(1) + Entry(2)*Entry(2) + Entry(3)*Entry(3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3& FGColumnVector3::Normalize(void)
|
||||
{
|
||||
double Mag = Magnitude();
|
||||
|
||||
if (Mag != 0.0)
|
||||
operator*=( 1.0/Mag );
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V) const
|
||||
{
|
||||
return FGColumnVector3(Entry(1) * V(1), Entry(2) * V(2), Entry(3) * V(3));
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
ostream& operator<<(ostream& os, const FGColumnVector3& col)
|
||||
{
|
||||
os << col(1) << " , " << col(2) << " , " << col(3);
|
||||
return os;
|
||||
}
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
// 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 FGColumnVector3::Debug(int from)
|
||||
{
|
||||
if (debug_lvl <= 0) return;
|
||||
|
||||
if (debug_lvl & 1) { // Standard console startup message output
|
||||
}
|
||||
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
|
||||
if (from == 0) cout << "Instantiated: FGColumnVector3" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGColumnVector3" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace JSBSim
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGColumnVector3.cpp
|
||||
Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
|
||||
Date started: 1998
|
||||
Purpose: FGColumnVector3 class
|
||||
Called by: Various
|
||||
|
||||
FUNCTIONAL DESCRIPTION
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
HISTORY
|
||||
--------------------------------------------------------------------------------
|
||||
??/??/?? TP Created
|
||||
03/16/2000 JSB Added exception throwing
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#include "FGColumnVector3.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_COLUMNVECTOR3;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
FGColumnVector3::FGColumnVector3(void)
|
||||
{
|
||||
data[0] = data[1] = data[2] = 0.0;
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3 FGColumnVector3::operator/(const double scalar) const
|
||||
{
|
||||
if (scalar != 0.0)
|
||||
return operator*( 1.0/scalar );
|
||||
|
||||
cerr << "Attempt to divide by zero in method "
|
||||
"FGColumnVector3::operator/(const double scalar), "
|
||||
"object " << this << endl;
|
||||
return FGColumnVector3();
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3& FGColumnVector3::operator/=(const double scalar)
|
||||
{
|
||||
if (scalar != 0.0)
|
||||
operator*=( 1.0/scalar );
|
||||
else
|
||||
cerr << "Attempt to divide by zero in method "
|
||||
"FGColumnVector3::operator/=(const double scalar), "
|
||||
"object " << this << endl;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
double FGColumnVector3::Magnitude(void) const
|
||||
{
|
||||
if (data[1] == 0.0 && data[2] == 0.0 && data[3] == 0.0)
|
||||
return 0.0;
|
||||
else
|
||||
return sqrt( Entry(1)*Entry(1) + Entry(2)*Entry(2) + Entry(3)*Entry(3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3& FGColumnVector3::Normalize(void)
|
||||
{
|
||||
double Mag = Magnitude();
|
||||
|
||||
if (Mag != 0.0)
|
||||
operator*=( 1.0/Mag );
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V) const
|
||||
{
|
||||
return FGColumnVector3(Entry(1) * V(1), Entry(2) * V(2), Entry(3) * V(3));
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
ostream& operator<<(ostream& os, const FGColumnVector3& col)
|
||||
{
|
||||
os << col(1) << " , " << col(2) << " , " << col(3);
|
||||
return os;
|
||||
}
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
// 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 FGColumnVector3::Debug(int from)
|
||||
{
|
||||
if (debug_lvl <= 0) return;
|
||||
|
||||
if (debug_lvl & 1) { // Standard console startup message output
|
||||
}
|
||||
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
|
||||
if (from == 0) cout << "Instantiated: FGColumnVector3" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGColumnVector3" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace JSBSim
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@ INCLUDES
|
|||
SG_USING_STD(cerr);
|
||||
SG_USING_STD(cout);
|
||||
SG_USING_STD(endl);
|
||||
SG_USING_STD(sqrt);
|
||||
// SG_USING_STD(sqrt);
|
||||
#else
|
||||
# include <string>
|
||||
# if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ INCLUDES
|
|||
SG_USING_STD(cerr);
|
||||
SG_USING_STD(cout);
|
||||
SG_USING_STD(endl);
|
||||
SG_USING_STD(sqrt);
|
||||
// SG_USING_STD(sqrt);
|
||||
#else
|
||||
# include <string>
|
||||
# if defined (sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
|
||||
|
|
|
|||
|
|
@ -1,180 +1,180 @@
|
|||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Source: FGForce.cpp
|
||||
Author: Tony Peden
|
||||
Date started: 6/10/00
|
||||
|
||||
------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
|
||||
|
||||
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
|
||||
--------------------------------------------------------------------------------
|
||||
6/10/00 TP Created
|
||||
|
||||
|
||||
FUNCTIONAL DESCRIPTION
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
The purpose of this class is to provide storage for computed forces and
|
||||
encapsulate all the functionality associated with transforming those
|
||||
forces from their native coord system to the body system. This includes
|
||||
computing the moments due to the difference between the point of application
|
||||
and the cg.
|
||||
|
||||
*/
|
||||
|
||||
#include "FGFDMExec.h"
|
||||
#include "FGAircraft.h"
|
||||
#include "FGTranslation.h"
|
||||
#include "FGMassBalance.h"
|
||||
#include "FGState.h"
|
||||
#include "FGForce.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_FORCE;
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGForce::FGForce(FGFDMExec *FDMExec) :
|
||||
ttype(tNone),
|
||||
fdmex(FDMExec)
|
||||
{
|
||||
mT(1,1) = 1; //identity matrix
|
||||
mT(2,2) = 1;
|
||||
mT(3,3) = 1;
|
||||
vSense.InitMatrix(1);
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGForce::~FGForce()
|
||||
{
|
||||
Debug(1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3& FGForce::GetBodyForces(void)
|
||||
{
|
||||
vFb = Transform()*(vFn.multElementWise(vSense));
|
||||
|
||||
// Find the distance from this vector's acting location to the cg; this
|
||||
// needs to be done like this to convert from structural to body coords.
|
||||
// CG and RP values are in inches
|
||||
|
||||
vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);
|
||||
|
||||
vM = vMn + vDXYZ*vFb;
|
||||
|
||||
return vFb;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGForce::Transform(void)
|
||||
{
|
||||
switch(ttype) {
|
||||
case tWindBody:
|
||||
return fdmex->GetState()->GetTs2b();
|
||||
case tLocalBody:
|
||||
return fdmex->GetState()->GetTl2b();
|
||||
case tCustom:
|
||||
case tNone:
|
||||
return mT;
|
||||
default:
|
||||
cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
|
||||
{
|
||||
if (ttype == tCustom) {
|
||||
double cp,sp,cr,sr,cy,sy;
|
||||
|
||||
cp=cos(bpitch); sp=sin(bpitch);
|
||||
cr=cos(broll); sr=sin(broll);
|
||||
cy=cos(byaw); sy=sin(byaw);
|
||||
|
||||
mT(1,1)=cp*cy;
|
||||
mT(1,2)=cp*sy;
|
||||
mT(1,3)=-1*sp;
|
||||
|
||||
mT(2,1)=sr*sp*cy-cr*sy;
|
||||
mT(2,2)=sr*sp*sy+cr*cy;
|
||||
mT(2,3)=sr*cp;
|
||||
|
||||
mT(3,1)=cr*sp*cy+sr*sy;
|
||||
mT(3,2)=cr*sp*sy-sr*cy;
|
||||
mT(3,3)=cr*cp;
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// 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 FGForce::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: FGForce" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGForce" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Source: FGForce.cpp
|
||||
Author: Tony Peden
|
||||
Date started: 6/10/00
|
||||
|
||||
------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
|
||||
|
||||
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
|
||||
--------------------------------------------------------------------------------
|
||||
6/10/00 TP Created
|
||||
|
||||
|
||||
FUNCTIONAL DESCRIPTION
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
The purpose of this class is to provide storage for computed forces and
|
||||
encapsulate all the functionality associated with transforming those
|
||||
forces from their native coord system to the body system. This includes
|
||||
computing the moments due to the difference between the point of application
|
||||
and the cg.
|
||||
|
||||
*/
|
||||
|
||||
#include "FGFDMExec.h"
|
||||
#include "FGAircraft.h"
|
||||
#include "FGTranslation.h"
|
||||
#include "FGMassBalance.h"
|
||||
#include "FGState.h"
|
||||
#include "FGForce.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_FORCE;
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGForce::FGForce(FGFDMExec *FDMExec) :
|
||||
ttype(tNone),
|
||||
fdmex(FDMExec)
|
||||
{
|
||||
mT(1,1) = 1; //identity matrix
|
||||
mT(2,2) = 1;
|
||||
mT(3,3) = 1;
|
||||
vSense.InitMatrix(1);
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGForce::~FGForce()
|
||||
{
|
||||
Debug(1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3& FGForce::GetBodyForces(void)
|
||||
{
|
||||
vFb = Transform()*(vFn.multElementWise(vSense));
|
||||
|
||||
// Find the distance from this vector's acting location to the cg; this
|
||||
// needs to be done like this to convert from structural to body coords.
|
||||
// CG and RP values are in inches
|
||||
|
||||
vDXYZ = fdmex->GetMassBalance()->StructuralToBody(vActingXYZn);
|
||||
|
||||
vM = vMn + vDXYZ*vFb;
|
||||
|
||||
return vFb;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGForce::Transform(void)
|
||||
{
|
||||
switch(ttype) {
|
||||
case tWindBody:
|
||||
return fdmex->GetState()->GetTs2b();
|
||||
case tLocalBody:
|
||||
return fdmex->GetState()->GetTl2b();
|
||||
case tCustom:
|
||||
case tNone:
|
||||
return mT;
|
||||
default:
|
||||
cout << "Unrecognized tranform requested from FGForce::Transform()" << endl;
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGForce::SetAnglesToBody(double broll, double bpitch, double byaw)
|
||||
{
|
||||
if (ttype == tCustom) {
|
||||
double cp,sp,cr,sr,cy,sy;
|
||||
|
||||
cp=cos(bpitch); sp=sin(bpitch);
|
||||
cr=cos(broll); sr=sin(broll);
|
||||
cy=cos(byaw); sy=sin(byaw);
|
||||
|
||||
mT(1,1)=cp*cy;
|
||||
mT(1,2)=cp*sy;
|
||||
mT(1,3)=-1*sp;
|
||||
|
||||
mT(2,1)=sr*sp*cy-cr*sy;
|
||||
mT(2,2)=sr*sp*sy+cr*cy;
|
||||
mT(2,3)=sr*cp;
|
||||
|
||||
mT(3,1)=cr*sp*cy+sr*sy;
|
||||
mT(3,2)=cr*sp*sy-sr*cy;
|
||||
mT(3,3)=cr*cp;
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// 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 FGForce::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: FGForce" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGForce" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,199 +1,199 @@
|
|||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
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"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
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' };
|
||||
|
||||
const double FGJSBBase::radtodeg = 57.29578;
|
||||
const double FGJSBBase::degtorad = 1.745329E-2;
|
||||
const double FGJSBBase::hptoftlbssec = 550.0;
|
||||
const double FGJSBBase::psftoinhg = 0.014138;
|
||||
const double FGJSBBase::fpstokts = 0.592484;
|
||||
const double FGJSBBase::ktstofps = 1.68781;
|
||||
const double FGJSBBase::inchtoft = 0.08333333;
|
||||
const double FGJSBBase::in3tom3 = 1.638706E-5;
|
||||
double FGJSBBase::Reng = 1716.0;
|
||||
const double FGJSBBase::SHRatio = 1.40;
|
||||
|
||||
// Note that definition of lbtoslug by the inverse of slugtolb and not
|
||||
// to a different constant you can also get from some tables will make
|
||||
// lbtoslug*slugtolb == 1 up to the magnitude of roundoff. So converting from
|
||||
// slug to lb and back will yield to the original value you started with up
|
||||
// to the magnitude of roundoff.
|
||||
// Taken from units gnu commandline tool
|
||||
const double FGJSBBase::slugtolb = 32.174049;
|
||||
const double FGJSBBase::lbtoslug = 1.0/slugtolb;
|
||||
|
||||
const string FGJSBBase::needed_cfg_version = "1.61";
|
||||
const string FGJSBBase::JSBSim_version = "0.9.5";
|
||||
|
||||
std::queue <FGJSBBase::Message*> FGJSBBase::Messages;
|
||||
FGJSBBase::Message FGJSBBase::localMsg;
|
||||
unsigned int FGJSBBase::messageId = 0;
|
||||
unsigned int FGJSBBase::frame = 0;
|
||||
|
||||
short FGJSBBase::debug_lvl = 1;
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::FGJSBBase()
|
||||
{
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(Message* msg)
|
||||
{
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(string text)
|
||||
{
|
||||
Message *msg = new Message();
|
||||
msg->text = text;
|
||||
msg->messageId = messageId++;
|
||||
msg->subsystem = "FDM";
|
||||
msg->type = Message::eText;
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(string text, bool bVal)
|
||||
{
|
||||
Message *msg = new Message();
|
||||
msg->text = text;
|
||||
msg->messageId = messageId++;
|
||||
msg->subsystem = "FDM";
|
||||
msg->type = Message::eBool;
|
||||
msg->bVal = bVal;
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(string text, int iVal)
|
||||
{
|
||||
Message *msg = new Message();
|
||||
msg->text = text;
|
||||
msg->messageId = messageId++;
|
||||
msg->subsystem = "FDM";
|
||||
msg->type = Message::eInteger;
|
||||
msg->bVal = (iVal != 0);
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(string text, double dVal)
|
||||
{
|
||||
Message *msg = new Message();
|
||||
msg->text = text;
|
||||
msg->messageId = messageId++;
|
||||
msg->subsystem = "FDM";
|
||||
msg->type = Message::eDouble;
|
||||
msg->bVal = (dVal != 0.0);
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::ReadMessage(void)
|
||||
{
|
||||
if (!Messages.empty()) return Messages.front();
|
||||
else return NULL;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::ProcessMessage(void)
|
||||
{
|
||||
if (!Messages.empty())
|
||||
localMsg = *(Messages.front());
|
||||
else
|
||||
return NULL;
|
||||
Messages.pop();
|
||||
return &localMsg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGJSBBase::disableHighLighting(void) {
|
||||
highint[0]='\0';
|
||||
halfint[0]='\0';
|
||||
normint[0]='\0';
|
||||
reset[0]='\0';
|
||||
underon[0]='\0';
|
||||
underoff[0]='\0';
|
||||
fgblue[0]='\0';
|
||||
fgcyan[0]='\0';
|
||||
fgred[0]='\0';
|
||||
fggreen[0]='\0';
|
||||
fgdef[0]='\0';
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
} // namespace JSBSim
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
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"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
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' };
|
||||
|
||||
const double FGJSBBase::radtodeg = 57.29578;
|
||||
const double FGJSBBase::degtorad = 1.745329E-2;
|
||||
const double FGJSBBase::hptoftlbssec = 550.0;
|
||||
const double FGJSBBase::psftoinhg = 0.014138;
|
||||
const double FGJSBBase::fpstokts = 0.592484;
|
||||
const double FGJSBBase::ktstofps = 1.68781;
|
||||
const double FGJSBBase::inchtoft = 0.08333333;
|
||||
const double FGJSBBase::in3tom3 = 1.638706E-5;
|
||||
double FGJSBBase::Reng = 1716.0;
|
||||
const double FGJSBBase::SHRatio = 1.40;
|
||||
|
||||
// Note that definition of lbtoslug by the inverse of slugtolb and not
|
||||
// to a different constant you can also get from some tables will make
|
||||
// lbtoslug*slugtolb == 1 up to the magnitude of roundoff. So converting from
|
||||
// slug to lb and back will yield to the original value you started with up
|
||||
// to the magnitude of roundoff.
|
||||
// Taken from units gnu commandline tool
|
||||
const double FGJSBBase::slugtolb = 32.174049;
|
||||
const double FGJSBBase::lbtoslug = 1.0/slugtolb;
|
||||
|
||||
const string FGJSBBase::needed_cfg_version = "1.61";
|
||||
const string FGJSBBase::JSBSim_version = "0.9.5";
|
||||
|
||||
std::queue <FGJSBBase::Message*> FGJSBBase::Messages;
|
||||
FGJSBBase::Message FGJSBBase::localMsg;
|
||||
unsigned int FGJSBBase::messageId = 0;
|
||||
unsigned int FGJSBBase::frame = 0;
|
||||
|
||||
short FGJSBBase::debug_lvl = 1;
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::FGJSBBase()
|
||||
{
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(Message* msg)
|
||||
{
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(string text)
|
||||
{
|
||||
Message *msg = new Message();
|
||||
msg->text = text;
|
||||
msg->messageId = messageId++;
|
||||
msg->subsystem = "FDM";
|
||||
msg->type = Message::eText;
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(string text, bool bVal)
|
||||
{
|
||||
Message *msg = new Message();
|
||||
msg->text = text;
|
||||
msg->messageId = messageId++;
|
||||
msg->subsystem = "FDM";
|
||||
msg->type = Message::eBool;
|
||||
msg->bVal = bVal;
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(string text, int iVal)
|
||||
{
|
||||
Message *msg = new Message();
|
||||
msg->text = text;
|
||||
msg->messageId = messageId++;
|
||||
msg->subsystem = "FDM";
|
||||
msg->type = Message::eInteger;
|
||||
msg->bVal = (iVal != 0);
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::PutMessage(string text, double dVal)
|
||||
{
|
||||
Message *msg = new Message();
|
||||
msg->text = text;
|
||||
msg->messageId = messageId++;
|
||||
msg->subsystem = "FDM";
|
||||
msg->type = Message::eDouble;
|
||||
msg->bVal = (dVal != 0.0);
|
||||
Messages.push(msg);
|
||||
return msg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::ReadMessage(void)
|
||||
{
|
||||
if (!Messages.empty()) return Messages.front();
|
||||
else return NULL;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGJSBBase::Message* FGJSBBase::ProcessMessage(void)
|
||||
{
|
||||
if (!Messages.empty())
|
||||
localMsg = *(Messages.front());
|
||||
else
|
||||
return NULL;
|
||||
Messages.pop();
|
||||
return &localMsg;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGJSBBase::disableHighLighting(void) {
|
||||
highint[0]='\0';
|
||||
halfint[0]='\0';
|
||||
normint[0]='\0';
|
||||
reset[0]='\0';
|
||||
underon[0]='\0';
|
||||
underoff[0]='\0';
|
||||
fgblue[0]='\0';
|
||||
fgcyan[0]='\0';
|
||||
fgred[0]='\0';
|
||||
fggreen[0]='\0';
|
||||
fgdef[0]='\0';
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
} // namespace JSBSim
|
||||
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
|
|
@ -1,391 +1,391 @@
|
|||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGMatrix33.cpp
|
||||
Author: Tony Peden, Jon Berndt, Mathias Frolich
|
||||
Date started: 1998
|
||||
Purpose: FGMatrix33 class
|
||||
Called by: Various
|
||||
|
||||
FUNCTIONAL DESCRIPTION
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
HISTORY
|
||||
--------------------------------------------------------------------------------
|
||||
??/??/?? TP Created
|
||||
03/16/2000 JSB Added exception throwing
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#include "FGMatrix33.h"
|
||||
#include "FGColumnVector3.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_MATRIX33;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33::FGMatrix33(void)
|
||||
{
|
||||
data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
|
||||
data[6] = data[7] = data[8] = 0.0;
|
||||
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
ostream& operator<<(ostream& os, const FGMatrix33& M)
|
||||
{
|
||||
for (unsigned int i=1; i<=M.Rows(); i++) {
|
||||
for (unsigned int j=1; j<=M.Cols(); j++) {
|
||||
if (i == M.Rows() && j == M.Cols())
|
||||
os << M(i,j);
|
||||
else
|
||||
os << M(i,j) << ", ";
|
||||
}
|
||||
}
|
||||
return os;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
istream& operator>>(istream& is, FGMatrix33& M)
|
||||
{
|
||||
for (unsigned int i=1; i<=M.Rows(); i++) {
|
||||
for (unsigned int j=1; j<=M.Cols(); j++) {
|
||||
is >> M(i,j);
|
||||
}
|
||||
}
|
||||
return is;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
double FGMatrix33::Determinant(void) const {
|
||||
return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
|
||||
+ Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
|
||||
- Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::Inverse(void) const {
|
||||
// Compute the inverse of a general matrix using Cramers rule.
|
||||
// I guess googling for cramers rule gives tons of references
|
||||
// for this. :)
|
||||
double rdet = 1.0/Determinant();
|
||||
|
||||
double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
|
||||
double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
|
||||
double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
|
||||
double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
|
||||
double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
|
||||
double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
|
||||
double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
|
||||
double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
|
||||
double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
|
||||
|
||||
return FGMatrix33( i11, i12, i13,
|
||||
i21, i22, i23,
|
||||
i31, i32, i33 );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGMatrix33::InitMatrix(void)
|
||||
{
|
||||
data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
|
||||
data[6] = data[7] = data[8] = 0.0;
|
||||
}
|
||||
|
||||
// *****************************************************************************
|
||||
// binary operators ************************************************************
|
||||
// *****************************************************************************
|
||||
|
||||
FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
|
||||
{
|
||||
return FGMatrix33( Entry(1,1) - M(1,1),
|
||||
Entry(1,2) - M(1,2),
|
||||
Entry(1,3) - M(1,3),
|
||||
Entry(2,1) - M(2,1),
|
||||
Entry(2,2) - M(2,2),
|
||||
Entry(2,3) - M(2,3),
|
||||
Entry(3,1) - M(3,1),
|
||||
Entry(3,2) - M(3,2),
|
||||
Entry(3,3) - M(3,3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
|
||||
{
|
||||
data[0] -= M.data[0];
|
||||
data[1] -= M.data[1];
|
||||
data[2] -= M.data[2];
|
||||
data[3] -= M.data[3];
|
||||
data[4] -= M.data[4];
|
||||
data[5] -= M.data[5];
|
||||
data[6] -= M.data[6];
|
||||
data[7] -= M.data[7];
|
||||
data[8] -= M.data[8];
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
|
||||
{
|
||||
return FGMatrix33( Entry(1,1) + M(1,1),
|
||||
Entry(1,2) + M(1,2),
|
||||
Entry(1,3) + M(1,3),
|
||||
Entry(2,1) + M(2,1),
|
||||
Entry(2,2) + M(2,2),
|
||||
Entry(2,3) + M(2,3),
|
||||
Entry(3,1) + M(3,1),
|
||||
Entry(3,2) + M(3,2),
|
||||
Entry(3,3) + M(3,3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
|
||||
{
|
||||
Entry(1,1) += M(1,1);
|
||||
Entry(1,2) += M(1,2);
|
||||
Entry(1,3) += M(1,3);
|
||||
Entry(2,1) += M(2,1);
|
||||
Entry(2,2) += M(2,2);
|
||||
Entry(2,3) += M(2,3);
|
||||
Entry(3,1) += M(3,1);
|
||||
Entry(3,2) += M(3,2);
|
||||
Entry(3,3) += M(3,3);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::operator*(const double scalar) const
|
||||
{
|
||||
return FGMatrix33( scalar * Entry(1,1),
|
||||
scalar * Entry(1,2),
|
||||
scalar * Entry(1,3),
|
||||
scalar * Entry(2,1),
|
||||
scalar * Entry(2,2),
|
||||
scalar * Entry(2,3),
|
||||
scalar * Entry(3,1),
|
||||
scalar * Entry(3,2),
|
||||
scalar * Entry(3,3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 operator*(double scalar, FGMatrix33 &M)
|
||||
{
|
||||
return FGMatrix33( scalar * M(1,1),
|
||||
scalar * M(1,2),
|
||||
scalar * M(1,3),
|
||||
scalar * M(2,1),
|
||||
scalar * M(2,2),
|
||||
scalar * M(2,3),
|
||||
scalar * M(3,1),
|
||||
scalar * M(3,2),
|
||||
scalar * M(3,3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator*=(const double scalar)
|
||||
{
|
||||
Entry(1,1) *= scalar;
|
||||
Entry(1,2) *= scalar;
|
||||
Entry(1,3) *= scalar;
|
||||
Entry(2,1) *= scalar;
|
||||
Entry(2,2) *= scalar;
|
||||
Entry(2,3) *= scalar;
|
||||
Entry(3,1) *= scalar;
|
||||
Entry(3,2) *= scalar;
|
||||
Entry(3,3) *= scalar;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
|
||||
{
|
||||
// FIXME: Make compiler friendlier
|
||||
FGMatrix33 Product;
|
||||
|
||||
Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
|
||||
Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
|
||||
Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
|
||||
Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
|
||||
Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
|
||||
Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
|
||||
Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
|
||||
Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
|
||||
Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
|
||||
|
||||
return Product;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
|
||||
{
|
||||
// FIXME: Make compiler friendlier
|
||||
double a,b,c;
|
||||
|
||||
a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
|
||||
Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
|
||||
Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
|
||||
Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
|
||||
|
||||
a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
|
||||
Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
|
||||
Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
|
||||
Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
|
||||
|
||||
a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
|
||||
Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
|
||||
Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
|
||||
Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::operator/(const double scalar) const
|
||||
{
|
||||
FGMatrix33 Quot;
|
||||
|
||||
if ( scalar != 0 ) {
|
||||
double tmp = 1.0/scalar;
|
||||
Quot(1,1) = Entry(1,1) * tmp;
|
||||
Quot(1,2) = Entry(1,2) * tmp;
|
||||
Quot(1,3) = Entry(1,3) * tmp;
|
||||
Quot(2,1) = Entry(2,1) * tmp;
|
||||
Quot(2,2) = Entry(2,2) * tmp;
|
||||
Quot(2,3) = Entry(2,3) * tmp;
|
||||
Quot(3,1) = Entry(3,1) * tmp;
|
||||
Quot(3,2) = Entry(3,2) * tmp;
|
||||
Quot(3,3) = Entry(3,3) * tmp;
|
||||
} else {
|
||||
MatrixException mE;
|
||||
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
|
||||
throw mE;
|
||||
}
|
||||
return Quot;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator/=(const double scalar)
|
||||
{
|
||||
if ( scalar != 0 ) {
|
||||
double tmp = 1.0/scalar;
|
||||
Entry(1,1) *= tmp;
|
||||
Entry(1,2) *= tmp;
|
||||
Entry(1,3) *= tmp;
|
||||
Entry(2,1) *= tmp;
|
||||
Entry(2,2) *= tmp;
|
||||
Entry(2,3) *= tmp;
|
||||
Entry(3,1) *= tmp;
|
||||
Entry(3,2) *= tmp;
|
||||
Entry(3,3) *= tmp;
|
||||
} else {
|
||||
MatrixException mE;
|
||||
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
|
||||
throw mE;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGMatrix33::T(void)
|
||||
{
|
||||
for (unsigned int i=1; i<=3; i++) {
|
||||
for (unsigned int j=i+1; j<=3; j++) {
|
||||
double tmp = Entry(i,j);
|
||||
Entry(i,j) = Entry(j,i);
|
||||
Entry(j,i) = tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
|
||||
double tmp1 = v(1)*Entry(1,1);
|
||||
double tmp2 = v(1)*Entry(2,1);
|
||||
double tmp3 = v(1)*Entry(3,1);
|
||||
|
||||
tmp1 += v(2)*Entry(1,2);
|
||||
tmp2 += v(2)*Entry(2,2);
|
||||
tmp3 += v(2)*Entry(3,2);
|
||||
|
||||
tmp1 += v(3)*Entry(1,3);
|
||||
tmp2 += v(3)*Entry(2,3);
|
||||
tmp3 += v(3)*Entry(3,3);
|
||||
|
||||
return FGColumnVector3( tmp1, tmp2, tmp3 );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// The bitmasked value choices are as follows:
|
||||
// unset: In this case (the default) JSBSim would only print
|
||||
// out the normally expected messages, essentially echoing
|
||||
// the config files as they are read. If the environment
|
||||
// variable is not set, debug_lvl is set to 1 internally
|
||||
// 0: This requests JSBSim not to output any messages
|
||||
// whatsoever.
|
||||
// 1: This value explicity requests the normal JSBSim
|
||||
// startup messages
|
||||
// 2: This value asks for a message to be printed out when
|
||||
// a class is instantiated
|
||||
// 4: When this value is set, a message is displayed when a
|
||||
// FGModel object executes its Run() method
|
||||
// 8: When this value is set, various runtime state variables
|
||||
// are printed out periodically
|
||||
// 16: When set various parameters are sanity checked and
|
||||
// a message is printed out when they go out of bounds
|
||||
|
||||
void FGMatrix33::Debug(int from)
|
||||
{
|
||||
if (debug_lvl <= 0) return;
|
||||
|
||||
if (debug_lvl & 1) { // Standard console startup message output
|
||||
if (from == 0) { // Constructor
|
||||
|
||||
}
|
||||
}
|
||||
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
|
||||
if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
|
||||
}
|
||||
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
|
||||
}
|
||||
if (debug_lvl & 8 ) { // Runtime state variables
|
||||
}
|
||||
if (debug_lvl & 16) { // Sanity checking
|
||||
}
|
||||
if (debug_lvl & 64) {
|
||||
if (from == 0) { // Constructor
|
||||
cout << IdSrc << endl;
|
||||
cout << IdHdr << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGMatrix33.cpp
|
||||
Author: Tony Peden, Jon Berndt, Mathias Frolich
|
||||
Date started: 1998
|
||||
Purpose: FGMatrix33 class
|
||||
Called by: Various
|
||||
|
||||
FUNCTIONAL DESCRIPTION
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
HISTORY
|
||||
--------------------------------------------------------------------------------
|
||||
??/??/?? TP Created
|
||||
03/16/2000 JSB Added exception throwing
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#include "FGMatrix33.h"
|
||||
#include "FGColumnVector3.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_MATRIX33;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33::FGMatrix33(void)
|
||||
{
|
||||
data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
|
||||
data[6] = data[7] = data[8] = 0.0;
|
||||
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
ostream& operator<<(ostream& os, const FGMatrix33& M)
|
||||
{
|
||||
for (unsigned int i=1; i<=M.Rows(); i++) {
|
||||
for (unsigned int j=1; j<=M.Cols(); j++) {
|
||||
if (i == M.Rows() && j == M.Cols())
|
||||
os << M(i,j);
|
||||
else
|
||||
os << M(i,j) << ", ";
|
||||
}
|
||||
}
|
||||
return os;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
istream& operator>>(istream& is, FGMatrix33& M)
|
||||
{
|
||||
for (unsigned int i=1; i<=M.Rows(); i++) {
|
||||
for (unsigned int j=1; j<=M.Cols(); j++) {
|
||||
is >> M(i,j);
|
||||
}
|
||||
}
|
||||
return is;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
double FGMatrix33::Determinant(void) const {
|
||||
return Entry(1,1)*Entry(2,2)*Entry(3,3) + Entry(1,2)*Entry(2,3)*Entry(3,1)
|
||||
+ Entry(1,3)*Entry(2,1)*Entry(3,2) - Entry(1,3)*Entry(2,2)*Entry(3,1)
|
||||
- Entry(1,2)*Entry(2,1)*Entry(3,3) - Entry(2,3)*Entry(3,2)*Entry(1,1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::Inverse(void) const {
|
||||
// Compute the inverse of a general matrix using Cramers rule.
|
||||
// I guess googling for cramers rule gives tons of references
|
||||
// for this. :)
|
||||
double rdet = 1.0/Determinant();
|
||||
|
||||
double i11 = rdet*(Entry(2,2)*Entry(3,3)-Entry(2,3)*Entry(3,2));
|
||||
double i21 = rdet*(Entry(2,3)*Entry(3,1)-Entry(2,1)*Entry(3,3));
|
||||
double i31 = rdet*(Entry(2,1)*Entry(3,2)-Entry(2,2)*Entry(3,1));
|
||||
double i12 = rdet*(Entry(1,3)*Entry(3,2)-Entry(1,2)*Entry(3,3));
|
||||
double i22 = rdet*(Entry(1,1)*Entry(3,3)-Entry(1,3)*Entry(3,1));
|
||||
double i32 = rdet*(Entry(1,2)*Entry(3,1)-Entry(1,1)*Entry(3,2));
|
||||
double i13 = rdet*(Entry(1,2)*Entry(2,3)-Entry(1,3)*Entry(2,2));
|
||||
double i23 = rdet*(Entry(1,3)*Entry(2,1)-Entry(1,1)*Entry(2,3));
|
||||
double i33 = rdet*(Entry(1,1)*Entry(2,2)-Entry(1,2)*Entry(2,1));
|
||||
|
||||
return FGMatrix33( i11, i12, i13,
|
||||
i21, i22, i23,
|
||||
i31, i32, i33 );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGMatrix33::InitMatrix(void)
|
||||
{
|
||||
data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
|
||||
data[6] = data[7] = data[8] = 0.0;
|
||||
}
|
||||
|
||||
// *****************************************************************************
|
||||
// binary operators ************************************************************
|
||||
// *****************************************************************************
|
||||
|
||||
FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M) const
|
||||
{
|
||||
return FGMatrix33( Entry(1,1) - M(1,1),
|
||||
Entry(1,2) - M(1,2),
|
||||
Entry(1,3) - M(1,3),
|
||||
Entry(2,1) - M(2,1),
|
||||
Entry(2,2) - M(2,2),
|
||||
Entry(2,3) - M(2,3),
|
||||
Entry(3,1) - M(3,1),
|
||||
Entry(3,2) - M(3,2),
|
||||
Entry(3,3) - M(3,3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator-=(const FGMatrix33 &M)
|
||||
{
|
||||
data[0] -= M.data[0];
|
||||
data[1] -= M.data[1];
|
||||
data[2] -= M.data[2];
|
||||
data[3] -= M.data[3];
|
||||
data[4] -= M.data[4];
|
||||
data[5] -= M.data[5];
|
||||
data[6] -= M.data[6];
|
||||
data[7] -= M.data[7];
|
||||
data[8] -= M.data[8];
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M) const
|
||||
{
|
||||
return FGMatrix33( Entry(1,1) + M(1,1),
|
||||
Entry(1,2) + M(1,2),
|
||||
Entry(1,3) + M(1,3),
|
||||
Entry(2,1) + M(2,1),
|
||||
Entry(2,2) + M(2,2),
|
||||
Entry(2,3) + M(2,3),
|
||||
Entry(3,1) + M(3,1),
|
||||
Entry(3,2) + M(3,2),
|
||||
Entry(3,3) + M(3,3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator+=(const FGMatrix33 &M)
|
||||
{
|
||||
Entry(1,1) += M(1,1);
|
||||
Entry(1,2) += M(1,2);
|
||||
Entry(1,3) += M(1,3);
|
||||
Entry(2,1) += M(2,1);
|
||||
Entry(2,2) += M(2,2);
|
||||
Entry(2,3) += M(2,3);
|
||||
Entry(3,1) += M(3,1);
|
||||
Entry(3,2) += M(3,2);
|
||||
Entry(3,3) += M(3,3);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::operator*(const double scalar) const
|
||||
{
|
||||
return FGMatrix33( scalar * Entry(1,1),
|
||||
scalar * Entry(1,2),
|
||||
scalar * Entry(1,3),
|
||||
scalar * Entry(2,1),
|
||||
scalar * Entry(2,2),
|
||||
scalar * Entry(2,3),
|
||||
scalar * Entry(3,1),
|
||||
scalar * Entry(3,2),
|
||||
scalar * Entry(3,3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 operator*(double scalar, FGMatrix33 &M)
|
||||
{
|
||||
return FGMatrix33( scalar * M(1,1),
|
||||
scalar * M(1,2),
|
||||
scalar * M(1,3),
|
||||
scalar * M(2,1),
|
||||
scalar * M(2,2),
|
||||
scalar * M(2,3),
|
||||
scalar * M(3,1),
|
||||
scalar * M(3,2),
|
||||
scalar * M(3,3) );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator*=(const double scalar)
|
||||
{
|
||||
Entry(1,1) *= scalar;
|
||||
Entry(1,2) *= scalar;
|
||||
Entry(1,3) *= scalar;
|
||||
Entry(2,1) *= scalar;
|
||||
Entry(2,2) *= scalar;
|
||||
Entry(2,3) *= scalar;
|
||||
Entry(3,1) *= scalar;
|
||||
Entry(3,2) *= scalar;
|
||||
Entry(3,3) *= scalar;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M) const
|
||||
{
|
||||
// FIXME: Make compiler friendlier
|
||||
FGMatrix33 Product;
|
||||
|
||||
Product(1,1) = Entry(1,1)*M(1,1) + Entry(1,2)*M(2,1) + Entry(1,3)*M(3,1);
|
||||
Product(1,2) = Entry(1,1)*M(1,2) + Entry(1,2)*M(2,2) + Entry(1,3)*M(3,2);
|
||||
Product(1,3) = Entry(1,1)*M(1,3) + Entry(1,2)*M(2,3) + Entry(1,3)*M(3,3);
|
||||
Product(2,1) = Entry(2,1)*M(1,1) + Entry(2,2)*M(2,1) + Entry(2,3)*M(3,1);
|
||||
Product(2,2) = Entry(2,1)*M(1,2) + Entry(2,2)*M(2,2) + Entry(2,3)*M(3,2);
|
||||
Product(2,3) = Entry(2,1)*M(1,3) + Entry(2,2)*M(2,3) + Entry(2,3)*M(3,3);
|
||||
Product(3,1) = Entry(3,1)*M(1,1) + Entry(3,2)*M(2,1) + Entry(3,3)*M(3,1);
|
||||
Product(3,2) = Entry(3,1)*M(1,2) + Entry(3,2)*M(2,2) + Entry(3,3)*M(3,2);
|
||||
Product(3,3) = Entry(3,1)*M(1,3) + Entry(3,2)*M(2,3) + Entry(3,3)*M(3,3);
|
||||
|
||||
return Product;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator*=(const FGMatrix33& M)
|
||||
{
|
||||
// FIXME: Make compiler friendlier
|
||||
double a,b,c;
|
||||
|
||||
a = Entry(1,1); b=Entry(1,2); c=Entry(1,3);
|
||||
Entry(1,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
|
||||
Entry(1,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
|
||||
Entry(1,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
|
||||
|
||||
a = Entry(2,1); b=Entry(2,2); c=Entry(2,3);
|
||||
Entry(2,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
|
||||
Entry(2,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
|
||||
Entry(2,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
|
||||
|
||||
a = Entry(3,1); b=Entry(3,2); c=Entry(3,3);
|
||||
Entry(3,1) = a*M(1,1) + b*M(2,1) + c*M(3,1);
|
||||
Entry(3,2) = a*M(1,2) + b*M(2,2) + c*M(3,2);
|
||||
Entry(3,3) = a*M(1,3) + b*M(2,3) + c*M(3,3);
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33 FGMatrix33::operator/(const double scalar) const
|
||||
{
|
||||
FGMatrix33 Quot;
|
||||
|
||||
if ( scalar != 0 ) {
|
||||
double tmp = 1.0/scalar;
|
||||
Quot(1,1) = Entry(1,1) * tmp;
|
||||
Quot(1,2) = Entry(1,2) * tmp;
|
||||
Quot(1,3) = Entry(1,3) * tmp;
|
||||
Quot(2,1) = Entry(2,1) * tmp;
|
||||
Quot(2,2) = Entry(2,2) * tmp;
|
||||
Quot(2,3) = Entry(2,3) * tmp;
|
||||
Quot(3,1) = Entry(3,1) * tmp;
|
||||
Quot(3,2) = Entry(3,2) * tmp;
|
||||
Quot(3,3) = Entry(3,3) * tmp;
|
||||
} else {
|
||||
MatrixException mE;
|
||||
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
|
||||
throw mE;
|
||||
}
|
||||
return Quot;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGMatrix33& FGMatrix33::operator/=(const double scalar)
|
||||
{
|
||||
if ( scalar != 0 ) {
|
||||
double tmp = 1.0/scalar;
|
||||
Entry(1,1) *= tmp;
|
||||
Entry(1,2) *= tmp;
|
||||
Entry(1,3) *= tmp;
|
||||
Entry(2,1) *= tmp;
|
||||
Entry(2,2) *= tmp;
|
||||
Entry(2,3) *= tmp;
|
||||
Entry(3,1) *= tmp;
|
||||
Entry(3,2) *= tmp;
|
||||
Entry(3,3) *= tmp;
|
||||
} else {
|
||||
MatrixException mE;
|
||||
mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
|
||||
throw mE;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGMatrix33::T(void)
|
||||
{
|
||||
for (unsigned int i=1; i<=3; i++) {
|
||||
for (unsigned int j=i+1; j<=3; j++) {
|
||||
double tmp = Entry(i,j);
|
||||
Entry(i,j) = Entry(j,i);
|
||||
Entry(j,i) = tmp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const {
|
||||
double tmp1 = v(1)*Entry(1,1);
|
||||
double tmp2 = v(1)*Entry(2,1);
|
||||
double tmp3 = v(1)*Entry(3,1);
|
||||
|
||||
tmp1 += v(2)*Entry(1,2);
|
||||
tmp2 += v(2)*Entry(2,2);
|
||||
tmp3 += v(2)*Entry(3,2);
|
||||
|
||||
tmp1 += v(3)*Entry(1,3);
|
||||
tmp2 += v(3)*Entry(2,3);
|
||||
tmp3 += v(3)*Entry(3,3);
|
||||
|
||||
return FGColumnVector3( tmp1, tmp2, tmp3 );
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// The bitmasked value choices are as follows:
|
||||
// unset: In this case (the default) JSBSim would only print
|
||||
// out the normally expected messages, essentially echoing
|
||||
// the config files as they are read. If the environment
|
||||
// variable is not set, debug_lvl is set to 1 internally
|
||||
// 0: This requests JSBSim not to output any messages
|
||||
// whatsoever.
|
||||
// 1: This value explicity requests the normal JSBSim
|
||||
// startup messages
|
||||
// 2: This value asks for a message to be printed out when
|
||||
// a class is instantiated
|
||||
// 4: When this value is set, a message is displayed when a
|
||||
// FGModel object executes its Run() method
|
||||
// 8: When this value is set, various runtime state variables
|
||||
// are printed out periodically
|
||||
// 16: When set various parameters are sanity checked and
|
||||
// a message is printed out when they go out of bounds
|
||||
|
||||
void FGMatrix33::Debug(int from)
|
||||
{
|
||||
if (debug_lvl <= 0) return;
|
||||
|
||||
if (debug_lvl & 1) { // Standard console startup message output
|
||||
if (from == 0) { // Constructor
|
||||
|
||||
}
|
||||
}
|
||||
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
|
||||
if (from == 0) cout << "Instantiated: FGMatrix33" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGMatrix33" << endl;
|
||||
}
|
||||
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
|
||||
}
|
||||
if (debug_lvl & 8 ) { // Runtime state variables
|
||||
}
|
||||
if (debug_lvl & 16) { // Sanity checking
|
||||
}
|
||||
if (debug_lvl & 64) {
|
||||
if (from == 0) { // Constructor
|
||||
cout << IdSrc << endl;
|
||||
cout << IdHdr << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
|
|
@ -1,367 +1,367 @@
|
|||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGPosition.cpp
|
||||
Author: Jon S. Berndt
|
||||
Date started: 01/05/99
|
||||
Purpose: Integrate the EOM to determine instantaneous position
|
||||
Called by: FGFDMExec
|
||||
|
||||
------------- Copyright (C) 1999 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
|
||||
--------------------------------------------------------------------------------
|
||||
This class encapsulates the integration of rates and accelerations to get the
|
||||
current position of the aircraft.
|
||||
|
||||
HISTORY
|
||||
--------------------------------------------------------------------------------
|
||||
01/05/99 JSB Created
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
COMMENTS, REFERENCES, and NOTES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
|
||||
Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
|
||||
School, January 1994
|
||||
[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
|
||||
JSC 12960, July 1977
|
||||
[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
|
||||
NASA-Ames", NASA CR-2497, January 1975
|
||||
[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
|
||||
Wiley & Sons, 1979 ISBN 0-471-03032-5
|
||||
[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
|
||||
1982 ISBN 0-471-08936-2
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#ifdef FGFS
|
||||
# include <simgear/compiler.h>
|
||||
# ifdef SG_HAVE_STD_INCLUDES
|
||||
# include <cmath>
|
||||
# include <iomanip>
|
||||
# else
|
||||
# include <math.h>
|
||||
# include <iomanip.h>
|
||||
# endif
|
||||
#else
|
||||
# if defined(sgi) && !defined(__GNUC__)
|
||||
# include <math.h>
|
||||
# if (_COMPILER_VERSION < 740)
|
||||
# include <iomanip.h>
|
||||
# else
|
||||
# include <iomanip>
|
||||
# endif
|
||||
# else
|
||||
# include <cmath>
|
||||
# include <iomanip>
|
||||
# endif
|
||||
#endif
|
||||
|
||||
#include "FGPosition.h"
|
||||
#include "FGState.h"
|
||||
#include "FGFDMExec.h"
|
||||
#include "FGAircraft.h"
|
||||
#include "FGMassBalance.h"
|
||||
#include "FGTranslation.h"
|
||||
#include "FGRotation.h"
|
||||
#include "FGInertial.h"
|
||||
#include "FGPropertyManager.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_POSITION;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex)
|
||||
{
|
||||
Name = "FGPosition";
|
||||
LongitudeDot = LatitudeDot = RadiusDot = 0.0;
|
||||
|
||||
for (int i=0;i<4;i++) {
|
||||
LatitudeDot_prev[i] = 0.0;
|
||||
LongitudeDot_prev[i] = 0.0;
|
||||
RadiusDot_prev[i] = 0.0;
|
||||
}
|
||||
|
||||
vVRPoffset.InitMatrix();
|
||||
|
||||
Longitude = Latitude = 0.0;
|
||||
LongitudeVRP = LatitudeVRP = 0.0;
|
||||
gamma = Vt = Vground = 0.0;
|
||||
hoverbmac = hoverbcg = 0.0;
|
||||
psigt = 0.0;
|
||||
bind();
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGPosition::~FGPosition(void)
|
||||
{
|
||||
unbind();
|
||||
Debug(1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
bool FGPosition::InitModel(void)
|
||||
{
|
||||
FGModel::InitModel();
|
||||
|
||||
h = 3.0; // Est. height of aircraft cg off runway
|
||||
SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY
|
||||
Radius = SeaLevelRadius + h;
|
||||
RunwayRadius = SeaLevelRadius;
|
||||
DistanceAGL = Radius - RunwayRadius; // Geocentric
|
||||
vRunwayNormal(3) = -1.0; // Initialized for standalone mode
|
||||
b = 1;
|
||||
return true;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
/*
|
||||
Purpose: Called on a schedule to perform Positioning algorithms
|
||||
Notes: [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always
|
||||
be the case
|
||||
[JB] Run in standalone mode, SeaLevelRadius will be reference radius.
|
||||
In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
|
||||
*/
|
||||
|
||||
bool FGPosition::Run(void)
|
||||
{
|
||||
double cosLat;
|
||||
double hdot_Vt;
|
||||
|
||||
if (!FGModel::Run()) {
|
||||
GetState();
|
||||
|
||||
Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) );
|
||||
|
||||
if (vVel(eNorth) == 0) psigt = 0;
|
||||
else psigt = atan2(vVel(eEast), vVel(eNorth));
|
||||
|
||||
if (psigt < 0.0) psigt += 2*M_PI;
|
||||
|
||||
Radius = h + SeaLevelRadius;
|
||||
|
||||
cosLat = cos(Latitude);
|
||||
if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);
|
||||
LatitudeDot = vVel(eNorth) / Radius;
|
||||
RadiusDot = -vVel(eDown);
|
||||
|
||||
Longitude += State->Integrate(FGState::TRAPZ, dt*rate, LongitudeDot, LongitudeDot_prev);
|
||||
Latitude += State->Integrate(FGState::TRAPZ, dt*rate, LatitudeDot, LatitudeDot_prev);
|
||||
Radius += State->Integrate(FGState::TRAPZ, dt*rate, RadiusDot, RadiusDot_prev);
|
||||
|
||||
h = Radius - SeaLevelRadius; // Geocentric
|
||||
|
||||
vVRPoffset = State->GetTb2l() * MassBalance->StructuralToBody(Aircraft->GetXYZvrp());
|
||||
|
||||
// vVRP - the vector to the Visual Reference Point - now contains the
|
||||
// offset from the CG to the VRP, in units of feet, in the Local coordinate
|
||||
// frame, where X points north, Y points East, and Z points down. This needs
|
||||
// to be converted to Lat/Lon/Alt, now.
|
||||
|
||||
if (cosLat != 0)
|
||||
LongitudeVRP = vVRPoffset(eEast) / (Radius * cosLat) + Longitude;
|
||||
|
||||
LatitudeVRP = vVRPoffset(eNorth) / Radius + Latitude;
|
||||
hVRP = h - vVRPoffset(eDown);
|
||||
/*
|
||||
cout << "Lat/Lon/Alt : " << Latitude << " / " << Longitude << " / " << h << endl;
|
||||
cout << "Lat/Lon/Alt VRP: " << LatitudeVRP << " / " << LongitudeVRP << " / " << hVRP << endl << endl;
|
||||
*/
|
||||
DistanceAGL = Radius - RunwayRadius; // Geocentric
|
||||
|
||||
hoverbcg = DistanceAGL/b;
|
||||
|
||||
vMac = State->GetTb2l()*MassBalance->StructuralToBody(Aircraft->GetXYZrp());
|
||||
hoverbmac = (DistanceAGL + vMac(3)) / b;
|
||||
|
||||
if (Vt > 0) {
|
||||
hdot_Vt = RadiusDot/Vt;
|
||||
if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt);
|
||||
} else {
|
||||
gamma = 0.0;
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::GetState(void)
|
||||
{
|
||||
dt = State->Getdt();
|
||||
|
||||
Vt = Translation->GetVt();
|
||||
vVel = State->GetTb2l() * Translation->GetUVW();
|
||||
vVelDot = State->GetTb2l() * Translation->GetUVWdot();
|
||||
|
||||
b = Aircraft->GetWingSpan();
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::Seth(double tt)
|
||||
{
|
||||
h = tt;
|
||||
Radius = h + SeaLevelRadius;
|
||||
DistanceAGL = Radius - RunwayRadius; // Geocentric
|
||||
hoverbcg = DistanceAGL/b;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::SetDistanceAGL(double tt)
|
||||
{
|
||||
DistanceAGL=tt;
|
||||
Radius = RunwayRadius + DistanceAGL;
|
||||
h = Radius - SeaLevelRadius;
|
||||
hoverbcg = DistanceAGL/b;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::bind(void)
|
||||
{
|
||||
PropertyManager->Tie("velocities/v-north-fps", this,
|
||||
&FGPosition::GetVn);
|
||||
PropertyManager->Tie("velocities/v-east-fps", this,
|
||||
&FGPosition::GetVe);
|
||||
PropertyManager->Tie("velocities/v-down-fps", this,
|
||||
&FGPosition::GetVd);
|
||||
PropertyManager->Tie("velocities/vg-fps", this,
|
||||
&FGPosition::GetVground);
|
||||
PropertyManager->Tie("flight-path/psi-gt-rad", this,
|
||||
&FGPosition::GetGroundTrack);
|
||||
PropertyManager->Tie("position/h-sl-ft", this,
|
||||
&FGPosition::Geth,
|
||||
&FGPosition::Seth,
|
||||
true);
|
||||
PropertyManager->Tie("velocities/h-dot-fps", this,
|
||||
&FGPosition::Gethdot);
|
||||
PropertyManager->Tie("position/lat-gc-rad", this,
|
||||
&FGPosition::GetLatitude,
|
||||
&FGPosition::SetLatitude);
|
||||
PropertyManager->Tie("position/lat-dot-gc-rad", this,
|
||||
&FGPosition::GetLatitudeDot);
|
||||
PropertyManager->Tie("position/long-gc-rad", this,
|
||||
&FGPosition::GetLongitude,
|
||||
&FGPosition::SetLongitude,
|
||||
true);
|
||||
PropertyManager->Tie("position/long-dot-gc-rad", this,
|
||||
&FGPosition::GetLongitudeDot);
|
||||
PropertyManager->Tie("metrics/runway-radius", this,
|
||||
&FGPosition::GetRunwayRadius,
|
||||
&FGPosition::SetRunwayRadius);
|
||||
PropertyManager->Tie("position/h-agl-ft", this,
|
||||
&FGPosition::GetDistanceAGL,
|
||||
&FGPosition::SetDistanceAGL);
|
||||
PropertyManager->Tie("position/radius-to-vehicle-ft", this,
|
||||
&FGPosition::GetRadius);
|
||||
PropertyManager->Tie("flight-path/gamma-rad", this,
|
||||
&FGPosition::GetGamma,
|
||||
&FGPosition::SetGamma);
|
||||
PropertyManager->Tie("aero/h_b-cg-ft", this,
|
||||
&FGPosition::GetHOverBCG);
|
||||
PropertyManager->Tie("aero/h_b-mac-ft", this,
|
||||
&FGPosition::GetHOverBMAC);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::unbind(void)
|
||||
{
|
||||
PropertyManager->Untie("velocities/v-north-fps");
|
||||
PropertyManager->Untie("velocities/v-east-fps");
|
||||
PropertyManager->Untie("velocities/v-down-fps");
|
||||
PropertyManager->Untie("velocities/vg-fps");
|
||||
PropertyManager->Untie("flight-path/psi-gt-rad");
|
||||
PropertyManager->Untie("position/h-sl-ft");
|
||||
PropertyManager->Untie("velocities/h-dot-fps");
|
||||
PropertyManager->Untie("position/lat-gc-rad");
|
||||
PropertyManager->Untie("position/lat-dot-gc-rad");
|
||||
PropertyManager->Untie("position/long-gc-rad");
|
||||
PropertyManager->Untie("position/long-dot-gc-rad");
|
||||
PropertyManager->Untie("metrics/runway-radius");
|
||||
PropertyManager->Untie("position/h-agl-ft");
|
||||
PropertyManager->Untie("position/radius-to-vehicle-ft");
|
||||
PropertyManager->Untie("flight-path/gamma-rad");
|
||||
PropertyManager->Untie("aero/h_b-cg-ft");
|
||||
PropertyManager->Untie("aero/h_b-mac-ft");
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// 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 FGPosition::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: FGPosition" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGPosition" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGPosition.cpp
|
||||
Author: Jon S. Berndt
|
||||
Date started: 01/05/99
|
||||
Purpose: Integrate the EOM to determine instantaneous position
|
||||
Called by: FGFDMExec
|
||||
|
||||
------------- Copyright (C) 1999 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
|
||||
--------------------------------------------------------------------------------
|
||||
This class encapsulates the integration of rates and accelerations to get the
|
||||
current position of the aircraft.
|
||||
|
||||
HISTORY
|
||||
--------------------------------------------------------------------------------
|
||||
01/05/99 JSB Created
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
COMMENTS, REFERENCES, and NOTES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
|
||||
Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
|
||||
School, January 1994
|
||||
[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
|
||||
JSC 12960, July 1977
|
||||
[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
|
||||
NASA-Ames", NASA CR-2497, January 1975
|
||||
[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
|
||||
Wiley & Sons, 1979 ISBN 0-471-03032-5
|
||||
[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
|
||||
1982 ISBN 0-471-08936-2
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#ifdef FGFS
|
||||
# include <simgear/compiler.h>
|
||||
# ifdef SG_HAVE_STD_INCLUDES
|
||||
# include <cmath>
|
||||
# include <iomanip>
|
||||
# else
|
||||
# include <math.h>
|
||||
# include <iomanip.h>
|
||||
# endif
|
||||
#else
|
||||
# if defined(sgi) && !defined(__GNUC__)
|
||||
# include <math.h>
|
||||
# if (_COMPILER_VERSION < 740)
|
||||
# include <iomanip.h>
|
||||
# else
|
||||
# include <iomanip>
|
||||
# endif
|
||||
# else
|
||||
# include <cmath>
|
||||
# include <iomanip>
|
||||
# endif
|
||||
#endif
|
||||
|
||||
#include "FGPosition.h"
|
||||
#include "FGState.h"
|
||||
#include "FGFDMExec.h"
|
||||
#include "FGAircraft.h"
|
||||
#include "FGMassBalance.h"
|
||||
#include "FGTranslation.h"
|
||||
#include "FGRotation.h"
|
||||
#include "FGInertial.h"
|
||||
#include "FGPropertyManager.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_POSITION;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex)
|
||||
{
|
||||
Name = "FGPosition";
|
||||
LongitudeDot = LatitudeDot = RadiusDot = 0.0;
|
||||
|
||||
for (int i=0;i<4;i++) {
|
||||
LatitudeDot_prev[i] = 0.0;
|
||||
LongitudeDot_prev[i] = 0.0;
|
||||
RadiusDot_prev[i] = 0.0;
|
||||
}
|
||||
|
||||
vVRPoffset.InitMatrix();
|
||||
|
||||
Longitude = Latitude = 0.0;
|
||||
LongitudeVRP = LatitudeVRP = 0.0;
|
||||
gamma = Vt = Vground = 0.0;
|
||||
hoverbmac = hoverbcg = 0.0;
|
||||
psigt = 0.0;
|
||||
bind();
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGPosition::~FGPosition(void)
|
||||
{
|
||||
unbind();
|
||||
Debug(1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
bool FGPosition::InitModel(void)
|
||||
{
|
||||
FGModel::InitModel();
|
||||
|
||||
h = 3.0; // Est. height of aircraft cg off runway
|
||||
SeaLevelRadius = Inertial->RefRadius(); // For initialization ONLY
|
||||
Radius = SeaLevelRadius + h;
|
||||
RunwayRadius = SeaLevelRadius;
|
||||
DistanceAGL = Radius - RunwayRadius; // Geocentric
|
||||
vRunwayNormal(3) = -1.0; // Initialized for standalone mode
|
||||
b = 1;
|
||||
return true;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
/*
|
||||
Purpose: Called on a schedule to perform Positioning algorithms
|
||||
Notes: [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always
|
||||
be the case
|
||||
[JB] Run in standalone mode, SeaLevelRadius will be reference radius.
|
||||
In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
|
||||
*/
|
||||
|
||||
bool FGPosition::Run(void)
|
||||
{
|
||||
double cosLat;
|
||||
double hdot_Vt;
|
||||
|
||||
if (!FGModel::Run()) {
|
||||
GetState();
|
||||
|
||||
Vground = sqrt( vVel(eNorth)*vVel(eNorth) + vVel(eEast)*vVel(eEast) );
|
||||
|
||||
if (vVel(eNorth) == 0) psigt = 0;
|
||||
else psigt = atan2(vVel(eEast), vVel(eNorth));
|
||||
|
||||
if (psigt < 0.0) psigt += 2*M_PI;
|
||||
|
||||
Radius = h + SeaLevelRadius;
|
||||
|
||||
cosLat = cos(Latitude);
|
||||
if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);
|
||||
LatitudeDot = vVel(eNorth) / Radius;
|
||||
RadiusDot = -vVel(eDown);
|
||||
|
||||
Longitude += State->Integrate(FGState::TRAPZ, dt*rate, LongitudeDot, LongitudeDot_prev);
|
||||
Latitude += State->Integrate(FGState::TRAPZ, dt*rate, LatitudeDot, LatitudeDot_prev);
|
||||
Radius += State->Integrate(FGState::TRAPZ, dt*rate, RadiusDot, RadiusDot_prev);
|
||||
|
||||
h = Radius - SeaLevelRadius; // Geocentric
|
||||
|
||||
vVRPoffset = State->GetTb2l() * MassBalance->StructuralToBody(Aircraft->GetXYZvrp());
|
||||
|
||||
// vVRP - the vector to the Visual Reference Point - now contains the
|
||||
// offset from the CG to the VRP, in units of feet, in the Local coordinate
|
||||
// frame, where X points north, Y points East, and Z points down. This needs
|
||||
// to be converted to Lat/Lon/Alt, now.
|
||||
|
||||
if (cosLat != 0)
|
||||
LongitudeVRP = vVRPoffset(eEast) / (Radius * cosLat) + Longitude;
|
||||
|
||||
LatitudeVRP = vVRPoffset(eNorth) / Radius + Latitude;
|
||||
hVRP = h - vVRPoffset(eDown);
|
||||
/*
|
||||
cout << "Lat/Lon/Alt : " << Latitude << " / " << Longitude << " / " << h << endl;
|
||||
cout << "Lat/Lon/Alt VRP: " << LatitudeVRP << " / " << LongitudeVRP << " / " << hVRP << endl << endl;
|
||||
*/
|
||||
DistanceAGL = Radius - RunwayRadius; // Geocentric
|
||||
|
||||
hoverbcg = DistanceAGL/b;
|
||||
|
||||
vMac = State->GetTb2l()*MassBalance->StructuralToBody(Aircraft->GetXYZrp());
|
||||
hoverbmac = (DistanceAGL + vMac(3)) / b;
|
||||
|
||||
if (Vt > 0) {
|
||||
hdot_Vt = RadiusDot/Vt;
|
||||
if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt);
|
||||
} else {
|
||||
gamma = 0.0;
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::GetState(void)
|
||||
{
|
||||
dt = State->Getdt();
|
||||
|
||||
Vt = Translation->GetVt();
|
||||
vVel = State->GetTb2l() * Translation->GetUVW();
|
||||
vVelDot = State->GetTb2l() * Translation->GetUVWdot();
|
||||
|
||||
b = Aircraft->GetWingSpan();
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::Seth(double tt)
|
||||
{
|
||||
h = tt;
|
||||
Radius = h + SeaLevelRadius;
|
||||
DistanceAGL = Radius - RunwayRadius; // Geocentric
|
||||
hoverbcg = DistanceAGL/b;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::SetDistanceAGL(double tt)
|
||||
{
|
||||
DistanceAGL=tt;
|
||||
Radius = RunwayRadius + DistanceAGL;
|
||||
h = Radius - SeaLevelRadius;
|
||||
hoverbcg = DistanceAGL/b;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::bind(void)
|
||||
{
|
||||
PropertyManager->Tie("velocities/v-north-fps", this,
|
||||
&FGPosition::GetVn);
|
||||
PropertyManager->Tie("velocities/v-east-fps", this,
|
||||
&FGPosition::GetVe);
|
||||
PropertyManager->Tie("velocities/v-down-fps", this,
|
||||
&FGPosition::GetVd);
|
||||
PropertyManager->Tie("velocities/vg-fps", this,
|
||||
&FGPosition::GetVground);
|
||||
PropertyManager->Tie("flight-path/psi-gt-rad", this,
|
||||
&FGPosition::GetGroundTrack);
|
||||
PropertyManager->Tie("position/h-sl-ft", this,
|
||||
&FGPosition::Geth,
|
||||
&FGPosition::Seth,
|
||||
true);
|
||||
PropertyManager->Tie("velocities/h-dot-fps", this,
|
||||
&FGPosition::Gethdot);
|
||||
PropertyManager->Tie("position/lat-gc-rad", this,
|
||||
&FGPosition::GetLatitude,
|
||||
&FGPosition::SetLatitude);
|
||||
PropertyManager->Tie("position/lat-dot-gc-rad", this,
|
||||
&FGPosition::GetLatitudeDot);
|
||||
PropertyManager->Tie("position/long-gc-rad", this,
|
||||
&FGPosition::GetLongitude,
|
||||
&FGPosition::SetLongitude,
|
||||
true);
|
||||
PropertyManager->Tie("position/long-dot-gc-rad", this,
|
||||
&FGPosition::GetLongitudeDot);
|
||||
PropertyManager->Tie("metrics/runway-radius", this,
|
||||
&FGPosition::GetRunwayRadius,
|
||||
&FGPosition::SetRunwayRadius);
|
||||
PropertyManager->Tie("position/h-agl-ft", this,
|
||||
&FGPosition::GetDistanceAGL,
|
||||
&FGPosition::SetDistanceAGL);
|
||||
PropertyManager->Tie("position/radius-to-vehicle-ft", this,
|
||||
&FGPosition::GetRadius);
|
||||
PropertyManager->Tie("flight-path/gamma-rad", this,
|
||||
&FGPosition::GetGamma,
|
||||
&FGPosition::SetGamma);
|
||||
PropertyManager->Tie("aero/h_b-cg-ft", this,
|
||||
&FGPosition::GetHOverBCG);
|
||||
PropertyManager->Tie("aero/h_b-mac-ft", this,
|
||||
&FGPosition::GetHOverBMAC);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGPosition::unbind(void)
|
||||
{
|
||||
PropertyManager->Untie("velocities/v-north-fps");
|
||||
PropertyManager->Untie("velocities/v-east-fps");
|
||||
PropertyManager->Untie("velocities/v-down-fps");
|
||||
PropertyManager->Untie("velocities/vg-fps");
|
||||
PropertyManager->Untie("flight-path/psi-gt-rad");
|
||||
PropertyManager->Untie("position/h-sl-ft");
|
||||
PropertyManager->Untie("velocities/h-dot-fps");
|
||||
PropertyManager->Untie("position/lat-gc-rad");
|
||||
PropertyManager->Untie("position/lat-dot-gc-rad");
|
||||
PropertyManager->Untie("position/long-gc-rad");
|
||||
PropertyManager->Untie("position/long-dot-gc-rad");
|
||||
PropertyManager->Untie("metrics/runway-radius");
|
||||
PropertyManager->Untie("position/h-agl-ft");
|
||||
PropertyManager->Untie("position/radius-to-vehicle-ft");
|
||||
PropertyManager->Untie("flight-path/gamma-rad");
|
||||
PropertyManager->Untie("aero/h_b-cg-ft");
|
||||
PropertyManager->Untie("aero/h_b-mac-ft");
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// 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 FGPosition::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: FGPosition" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGPosition" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,257 +1,257 @@
|
|||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGCondition.cpp
|
||||
Author: Jon S. Berndt
|
||||
Date started: 1/2/2003
|
||||
|
||||
-------------- Copyright (C) 2003 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
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
COMMENTS, REFERENCES, and NOTES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#include "FGCondition.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_CONDITION;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
string FGCondition::indent = " ";
|
||||
|
||||
|
||||
FGCondition::FGCondition(FGConfigFile* AC_cfg, FGPropertyManager* PropertyManager) :
|
||||
PropertyManager(PropertyManager)
|
||||
{
|
||||
string property1, property2;
|
||||
|
||||
mComparison["EQ"] = eEQ;
|
||||
mComparison["NE"] = eNE;
|
||||
mComparison["GT"] = eGT;
|
||||
mComparison["GE"] = eGE;
|
||||
mComparison["LT"] = eLT;
|
||||
mComparison["LE"] = eLE;
|
||||
mComparison["=="] = eEQ;
|
||||
mComparison["!="] = eNE;
|
||||
mComparison[">"] = eGT;
|
||||
mComparison[">="] = eGE;
|
||||
mComparison["<"] = eLT;
|
||||
mComparison["<="] = eLE;
|
||||
|
||||
TestParam1 = TestParam2 = 0L;
|
||||
TestValue = 0.0;
|
||||
Comparison = ecUndef;
|
||||
Logic = elUndef;
|
||||
conditions.clear();
|
||||
|
||||
if (AC_cfg->GetValue("CONDITION_GROUP").empty()) { // define a condition
|
||||
|
||||
*AC_cfg >> property1 >> conditional >> property2;
|
||||
TestParam1 = PropertyManager->GetNode(property1, true);
|
||||
Comparison = mComparison[conditional];
|
||||
|
||||
if (property2.find_first_not_of("-.0123456789eE") == string::npos) {
|
||||
TestValue = atof(property2.c_str());
|
||||
} else {
|
||||
TestParam2 = PropertyManager->GetNode(property2, true);
|
||||
}
|
||||
|
||||
isGroup = false;
|
||||
|
||||
} else { // define a condition group
|
||||
|
||||
if (AC_cfg->GetValue("LOGIC") == "OR") Logic = eOR;
|
||||
else if (AC_cfg->GetValue("LOGIC") == "AND") Logic = eAND;
|
||||
|
||||
AC_cfg->GetNextConfigLine();
|
||||
while (AC_cfg->GetValue() != string("/CONDITION_GROUP")) {
|
||||
conditions.push_back(FGCondition(AC_cfg, PropertyManager));
|
||||
}
|
||||
isGroup = true;
|
||||
AC_cfg->GetNextConfigLine();
|
||||
}
|
||||
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGCondition::~FGCondition(void)
|
||||
{
|
||||
Debug(1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
bool FGCondition::Evaluate(void )
|
||||
{
|
||||
vector <FGCondition>::iterator iConditions;
|
||||
bool pass = false;
|
||||
double compareValue;
|
||||
|
||||
if (Logic == eAND) {
|
||||
|
||||
iConditions = conditions.begin();
|
||||
pass = true;
|
||||
while (iConditions < conditions.end()) {
|
||||
if (!iConditions->Evaluate()) pass = false;
|
||||
*iConditions++;
|
||||
}
|
||||
|
||||
} else if (Logic == eOR) {
|
||||
|
||||
pass = false;
|
||||
while (iConditions < conditions.end()) {
|
||||
if (iConditions->Evaluate()) pass = true;
|
||||
*iConditions++;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
if (TestParam2 != 0L) compareValue = TestParam2->getDoubleValue();
|
||||
else compareValue = TestValue;
|
||||
|
||||
switch (Comparison) {
|
||||
case ecUndef:
|
||||
cerr << "Undefined comparison operator." << endl;
|
||||
break;
|
||||
case eEQ:
|
||||
pass = TestParam1->getDoubleValue() == compareValue;
|
||||
break;
|
||||
case eNE:
|
||||
pass = TestParam1->getDoubleValue() != compareValue;
|
||||
break;
|
||||
case eGT:
|
||||
pass = TestParam1->getDoubleValue() > compareValue;
|
||||
break;
|
||||
case eGE:
|
||||
pass = TestParam1->getDoubleValue() >= compareValue;
|
||||
break;
|
||||
case eLT:
|
||||
pass = TestParam1->getDoubleValue() < compareValue;
|
||||
break;
|
||||
case eLE:
|
||||
pass = TestParam1->getDoubleValue() <= compareValue;
|
||||
break;
|
||||
default:
|
||||
cerr << "Unknown comparison operator." << endl;
|
||||
}
|
||||
}
|
||||
|
||||
return pass;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGCondition::PrintCondition(void )
|
||||
{
|
||||
vector <FGCondition>::iterator iConditions;
|
||||
string scratch;
|
||||
|
||||
if (isGroup) {
|
||||
switch(Logic) {
|
||||
case (elUndef):
|
||||
scratch = " UNSET";
|
||||
cerr << "unset logic for test condition" << endl;
|
||||
break;
|
||||
case (eAND):
|
||||
scratch = " if all of the following are true";
|
||||
break;
|
||||
case (eOR):
|
||||
scratch = " if any of the following are true:";
|
||||
break;
|
||||
default:
|
||||
scratch = " UNKNOWN";
|
||||
cerr << "Unknown logic for test condition" << endl;
|
||||
}
|
||||
|
||||
iConditions = conditions.begin();
|
||||
cout << scratch << endl;
|
||||
while (iConditions < conditions.end()) {
|
||||
iConditions->PrintCondition();
|
||||
*iConditions++;
|
||||
}
|
||||
} else {
|
||||
if (TestParam2 != 0L)
|
||||
cout << TestParam1->GetName() << " " << conditional << " " << TestParam2->GetName();
|
||||
else
|
||||
cout << TestParam1->GetName() << " " << conditional << " " << TestValue;
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// 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 FGCondition::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: FGCondition" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGCondition" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} //namespace JSBSim
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGCondition.cpp
|
||||
Author: Jon S. Berndt
|
||||
Date started: 1/2/2003
|
||||
|
||||
-------------- Copyright (C) 2003 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
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
COMMENTS, REFERENCES, and NOTES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#include "FGCondition.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_CONDITION;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
string FGCondition::indent = " ";
|
||||
|
||||
|
||||
FGCondition::FGCondition(FGConfigFile* AC_cfg, FGPropertyManager* PropertyManager) :
|
||||
PropertyManager(PropertyManager)
|
||||
{
|
||||
string property1, property2;
|
||||
|
||||
mComparison["EQ"] = eEQ;
|
||||
mComparison["NE"] = eNE;
|
||||
mComparison["GT"] = eGT;
|
||||
mComparison["GE"] = eGE;
|
||||
mComparison["LT"] = eLT;
|
||||
mComparison["LE"] = eLE;
|
||||
mComparison["=="] = eEQ;
|
||||
mComparison["!="] = eNE;
|
||||
mComparison[">"] = eGT;
|
||||
mComparison[">="] = eGE;
|
||||
mComparison["<"] = eLT;
|
||||
mComparison["<="] = eLE;
|
||||
|
||||
TestParam1 = TestParam2 = 0L;
|
||||
TestValue = 0.0;
|
||||
Comparison = ecUndef;
|
||||
Logic = elUndef;
|
||||
conditions.clear();
|
||||
|
||||
if (AC_cfg->GetValue("CONDITION_GROUP").empty()) { // define a condition
|
||||
|
||||
*AC_cfg >> property1 >> conditional >> property2;
|
||||
TestParam1 = PropertyManager->GetNode(property1, true);
|
||||
Comparison = mComparison[conditional];
|
||||
|
||||
if (property2.find_first_not_of("-.0123456789eE") == string::npos) {
|
||||
TestValue = atof(property2.c_str());
|
||||
} else {
|
||||
TestParam2 = PropertyManager->GetNode(property2, true);
|
||||
}
|
||||
|
||||
isGroup = false;
|
||||
|
||||
} else { // define a condition group
|
||||
|
||||
if (AC_cfg->GetValue("LOGIC") == "OR") Logic = eOR;
|
||||
else if (AC_cfg->GetValue("LOGIC") == "AND") Logic = eAND;
|
||||
|
||||
AC_cfg->GetNextConfigLine();
|
||||
while (AC_cfg->GetValue() != string("/CONDITION_GROUP")) {
|
||||
conditions.push_back(FGCondition(AC_cfg, PropertyManager));
|
||||
}
|
||||
isGroup = true;
|
||||
AC_cfg->GetNextConfigLine();
|
||||
}
|
||||
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGCondition::~FGCondition(void)
|
||||
{
|
||||
Debug(1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
bool FGCondition::Evaluate(void )
|
||||
{
|
||||
vector <FGCondition>::iterator iConditions;
|
||||
bool pass = false;
|
||||
double compareValue;
|
||||
|
||||
if (Logic == eAND) {
|
||||
|
||||
iConditions = conditions.begin();
|
||||
pass = true;
|
||||
while (iConditions < conditions.end()) {
|
||||
if (!iConditions->Evaluate()) pass = false;
|
||||
*iConditions++;
|
||||
}
|
||||
|
||||
} else if (Logic == eOR) {
|
||||
|
||||
pass = false;
|
||||
while (iConditions < conditions.end()) {
|
||||
if (iConditions->Evaluate()) pass = true;
|
||||
*iConditions++;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
if (TestParam2 != 0L) compareValue = TestParam2->getDoubleValue();
|
||||
else compareValue = TestValue;
|
||||
|
||||
switch (Comparison) {
|
||||
case ecUndef:
|
||||
cerr << "Undefined comparison operator." << endl;
|
||||
break;
|
||||
case eEQ:
|
||||
pass = TestParam1->getDoubleValue() == compareValue;
|
||||
break;
|
||||
case eNE:
|
||||
pass = TestParam1->getDoubleValue() != compareValue;
|
||||
break;
|
||||
case eGT:
|
||||
pass = TestParam1->getDoubleValue() > compareValue;
|
||||
break;
|
||||
case eGE:
|
||||
pass = TestParam1->getDoubleValue() >= compareValue;
|
||||
break;
|
||||
case eLT:
|
||||
pass = TestParam1->getDoubleValue() < compareValue;
|
||||
break;
|
||||
case eLE:
|
||||
pass = TestParam1->getDoubleValue() <= compareValue;
|
||||
break;
|
||||
default:
|
||||
cerr << "Unknown comparison operator." << endl;
|
||||
}
|
||||
}
|
||||
|
||||
return pass;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
void FGCondition::PrintCondition(void )
|
||||
{
|
||||
vector <FGCondition>::iterator iConditions;
|
||||
string scratch;
|
||||
|
||||
if (isGroup) {
|
||||
switch(Logic) {
|
||||
case (elUndef):
|
||||
scratch = " UNSET";
|
||||
cerr << "unset logic for test condition" << endl;
|
||||
break;
|
||||
case (eAND):
|
||||
scratch = " if all of the following are true";
|
||||
break;
|
||||
case (eOR):
|
||||
scratch = " if any of the following are true:";
|
||||
break;
|
||||
default:
|
||||
scratch = " UNKNOWN";
|
||||
cerr << "Unknown logic for test condition" << endl;
|
||||
}
|
||||
|
||||
iConditions = conditions.begin();
|
||||
cout << scratch << endl;
|
||||
while (iConditions < conditions.end()) {
|
||||
iConditions->PrintCondition();
|
||||
*iConditions++;
|
||||
}
|
||||
} else {
|
||||
if (TestParam2 != 0L)
|
||||
cout << TestParam1->GetName() << " " << conditional << " " << TestParam2->GetName();
|
||||
else
|
||||
cout << TestParam1->GetName() << " " << conditional << " " << TestValue;
|
||||
}
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// 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 FGCondition::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: FGCondition" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGCondition" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} //namespace JSBSim
|
||||
|
||||
|
|
|
|||
|
|
@ -1,280 +1,280 @@
|
|||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGSwitch.cpp
|
||||
Author: Jon S. Berndt
|
||||
Date started: 4/2000
|
||||
|
||||
------------- Copyright (C) 2000 -------------
|
||||
|
||||
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
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
COMMENTS, REFERENCES, and NOTES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
The SWITCH component is defined as follows (see the API documentation for more
|
||||
information):
|
||||
|
||||
<COMPONENT NAME="switch1" TYPE="SWITCH">
|
||||
<TEST LOGIC="{AND|OR|DEFAULT}" OUTPUT="{property|value}">
|
||||
{property} {conditional} {property|value}
|
||||
<CONDITION_GROUP LOGIC="{AND|OR}">
|
||||
{property} {conditional} {property|value}
|
||||
...
|
||||
</CONDITION_GROUP>
|
||||
...
|
||||
</TEST>
|
||||
<TEST LOGIC="{AND|OR}" OUTPUT="{property|value}">
|
||||
{property} {conditional} {property|value}
|
||||
...
|
||||
</TEST>
|
||||
...
|
||||
</COMPONENT>
|
||||
|
||||
Also, see the header file (FGSwitch.h) for further details.
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#include "FGSwitch.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_SWITCH;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
|
||||
FGSwitch::FGSwitch(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
|
||||
AC_cfg(AC_cfg)
|
||||
{
|
||||
string token, value;
|
||||
struct test *current_test;
|
||||
struct FGCondition *current_condition;
|
||||
|
||||
Type = AC_cfg->GetValue("TYPE");
|
||||
Name = AC_cfg->GetValue("NAME");
|
||||
|
||||
AC_cfg->GetNextConfigLine();
|
||||
while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) {
|
||||
|
||||
// See the above documentation, or the API docs, for information on what
|
||||
// the SWITCH component is supposed to look like in the configuration file.
|
||||
// Below, the switch component is read in.
|
||||
|
||||
if (token == "TEST") {
|
||||
tests.push_back(test());
|
||||
current_test = &tests.back();
|
||||
|
||||
if (AC_cfg->GetValue("LOGIC") == "OR") {
|
||||
current_test->Logic = eOR;
|
||||
} else if (AC_cfg->GetValue("LOGIC") == "AND") {
|
||||
current_test->Logic = eAND;
|
||||
} else if (AC_cfg->GetValue("LOGIC") == "DEFAULT") {
|
||||
current_test->Logic = eDefault;
|
||||
} else { // error
|
||||
cerr << "Unrecognized LOGIC token in switch component: " << Name << endl;
|
||||
}
|
||||
|
||||
value = AC_cfg->GetValue("VALUE");
|
||||
if (value.empty()) {
|
||||
cerr << "No VALUE supplied for switch component: " << Name << endl;
|
||||
} else {
|
||||
if (value.find_first_not_of("-.0123456789eE") == string::npos) {
|
||||
// if true (and execution falls into this block), "value" is a number.
|
||||
current_test->OutputVal = atof(value.c_str());
|
||||
} else {
|
||||
// "value" must be a property if execution passes to here.
|
||||
if (value[0] == '-') {
|
||||
current_test->sign = -1.0;
|
||||
value.erase(0,1);
|
||||
} else {
|
||||
current_test->sign = 1.0;
|
||||
}
|
||||
current_test->OutputProp = PropertyManager->GetNode(value);
|
||||
}
|
||||
}
|
||||
|
||||
AC_cfg->GetNextConfigLine();
|
||||
while (AC_cfg->GetValue() != string("/TEST")) {
|
||||
current_test->conditions.push_back(FGCondition(AC_cfg, PropertyManager));
|
||||
}
|
||||
}
|
||||
AC_cfg->GetNextConfigLine();
|
||||
}
|
||||
|
||||
FGFCSComponent::bind();
|
||||
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGSwitch::~FGSwitch()
|
||||
{
|
||||
Debug(1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
bool FGSwitch::Run(void )
|
||||
{
|
||||
vector <test>::iterator iTests = tests.begin();
|
||||
vector <FGCondition>::iterator iConditions;
|
||||
bool pass = false;
|
||||
|
||||
FGFCSComponent::Run(); // call the base class for initialization of Input
|
||||
|
||||
while (iTests < tests.end()) {
|
||||
iConditions = iTests->conditions.begin();
|
||||
|
||||
if (iTests->Logic == eDefault) {
|
||||
Output = iTests->GetValue();
|
||||
} else if (iTests->Logic == eAND) {
|
||||
pass = true;
|
||||
while (iConditions < iTests->conditions.end()) {
|
||||
if (!iConditions->Evaluate()) pass = false;
|
||||
*iConditions++;
|
||||
}
|
||||
} else if (iTests->Logic == eOR) {
|
||||
pass = false;
|
||||
while (iConditions < iTests->conditions.end()) {
|
||||
if (iConditions->Evaluate()) pass = true;
|
||||
*iConditions++;
|
||||
}
|
||||
} else {
|
||||
cerr << "Invalid logic test" << endl;
|
||||
}
|
||||
|
||||
if (pass) {
|
||||
Output = iTests->GetValue();
|
||||
break;
|
||||
}
|
||||
*iTests++;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// 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 FGSwitch::Debug(int from)
|
||||
{
|
||||
vector <test>::iterator iTests = tests.begin();
|
||||
vector <FGCondition>::iterator iConditions;
|
||||
string comp, scratch;
|
||||
string indent = " ";
|
||||
bool first = false;
|
||||
|
||||
if (debug_lvl <= 0) return;
|
||||
|
||||
if (debug_lvl & 1) { // Standard console startup message output
|
||||
if (from == 0) { // Constructor
|
||||
while (iTests < tests.end()) {
|
||||
|
||||
scratch = " if ";
|
||||
|
||||
switch(iTests->Logic) {
|
||||
case (elUndef):
|
||||
comp = " UNSET ";
|
||||
cerr << "Unset logic for test condition" << endl;
|
||||
break;
|
||||
case (eAND):
|
||||
comp = " AND ";
|
||||
break;
|
||||
case (eOR):
|
||||
comp=" OR ";
|
||||
break;
|
||||
case (eDefault):
|
||||
scratch = " by default.";
|
||||
break;
|
||||
default:
|
||||
comp = " UNKNOWN ";
|
||||
cerr << "Unknown logic for test condition" << endl;
|
||||
}
|
||||
|
||||
if (iTests->OutputProp != 0L)
|
||||
if (iTests->sign < 0)
|
||||
cout << indent << "Switch VALUE is - " << iTests->OutputProp->GetName() << scratch << endl;
|
||||
else
|
||||
cout << indent << "Switch VALUE is " << iTests->OutputProp->GetName() << scratch << endl;
|
||||
else
|
||||
cout << indent << "Switch VALUE is " << iTests->OutputVal << scratch << endl;
|
||||
|
||||
iConditions = iTests->conditions.begin();
|
||||
first = true;
|
||||
while (iConditions < iTests->conditions.end()) {
|
||||
if (!first) cout << indent << comp << " ";
|
||||
else cout << indent << " ";
|
||||
first = false;
|
||||
iConditions->PrintCondition();
|
||||
cout << endl;
|
||||
*iConditions++;
|
||||
}
|
||||
cout << endl;
|
||||
*iTests++;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
|
||||
if (from == 0) cout << "Instantiated: FGSwitch" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGSwitch" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} //namespace JSBSim
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
Module: FGSwitch.cpp
|
||||
Author: Jon S. Berndt
|
||||
Date started: 4/2000
|
||||
|
||||
------------- Copyright (C) 2000 -------------
|
||||
|
||||
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
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
COMMENTS, REFERENCES, and NOTES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
The SWITCH component is defined as follows (see the API documentation for more
|
||||
information):
|
||||
|
||||
<COMPONENT NAME="switch1" TYPE="SWITCH">
|
||||
<TEST LOGIC="{AND|OR|DEFAULT}" OUTPUT="{property|value}">
|
||||
{property} {conditional} {property|value}
|
||||
<CONDITION_GROUP LOGIC="{AND|OR}">
|
||||
{property} {conditional} {property|value}
|
||||
...
|
||||
</CONDITION_GROUP>
|
||||
...
|
||||
</TEST>
|
||||
<TEST LOGIC="{AND|OR}" OUTPUT="{property|value}">
|
||||
{property} {conditional} {property|value}
|
||||
...
|
||||
</TEST>
|
||||
...
|
||||
</COMPONENT>
|
||||
|
||||
Also, see the header file (FGSwitch.h) for further details.
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
INCLUDES
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
#include "FGSwitch.h"
|
||||
|
||||
namespace JSBSim {
|
||||
|
||||
static const char *IdSrc = "$Id$";
|
||||
static const char *IdHdr = ID_SWITCH;
|
||||
|
||||
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
CLASS IMPLEMENTATION
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
|
||||
|
||||
|
||||
FGSwitch::FGSwitch(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
|
||||
AC_cfg(AC_cfg)
|
||||
{
|
||||
string token, value;
|
||||
struct test *current_test;
|
||||
struct FGCondition *current_condition;
|
||||
|
||||
Type = AC_cfg->GetValue("TYPE");
|
||||
Name = AC_cfg->GetValue("NAME");
|
||||
|
||||
AC_cfg->GetNextConfigLine();
|
||||
while ((token = AC_cfg->GetValue()) != string("/COMPONENT")) {
|
||||
|
||||
// See the above documentation, or the API docs, for information on what
|
||||
// the SWITCH component is supposed to look like in the configuration file.
|
||||
// Below, the switch component is read in.
|
||||
|
||||
if (token == "TEST") {
|
||||
tests.push_back(test());
|
||||
current_test = &tests.back();
|
||||
|
||||
if (AC_cfg->GetValue("LOGIC") == "OR") {
|
||||
current_test->Logic = eOR;
|
||||
} else if (AC_cfg->GetValue("LOGIC") == "AND") {
|
||||
current_test->Logic = eAND;
|
||||
} else if (AC_cfg->GetValue("LOGIC") == "DEFAULT") {
|
||||
current_test->Logic = eDefault;
|
||||
} else { // error
|
||||
cerr << "Unrecognized LOGIC token in switch component: " << Name << endl;
|
||||
}
|
||||
|
||||
value = AC_cfg->GetValue("VALUE");
|
||||
if (value.empty()) {
|
||||
cerr << "No VALUE supplied for switch component: " << Name << endl;
|
||||
} else {
|
||||
if (value.find_first_not_of("-.0123456789eE") == string::npos) {
|
||||
// if true (and execution falls into this block), "value" is a number.
|
||||
current_test->OutputVal = atof(value.c_str());
|
||||
} else {
|
||||
// "value" must be a property if execution passes to here.
|
||||
if (value[0] == '-') {
|
||||
current_test->sign = -1.0;
|
||||
value.erase(0,1);
|
||||
} else {
|
||||
current_test->sign = 1.0;
|
||||
}
|
||||
current_test->OutputProp = PropertyManager->GetNode(value);
|
||||
}
|
||||
}
|
||||
|
||||
AC_cfg->GetNextConfigLine();
|
||||
while (AC_cfg->GetValue() != string("/TEST")) {
|
||||
current_test->conditions.push_back(FGCondition(AC_cfg, PropertyManager));
|
||||
}
|
||||
}
|
||||
AC_cfg->GetNextConfigLine();
|
||||
}
|
||||
|
||||
FGFCSComponent::bind();
|
||||
|
||||
Debug(0);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
FGSwitch::~FGSwitch()
|
||||
{
|
||||
Debug(1);
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
bool FGSwitch::Run(void )
|
||||
{
|
||||
vector <test>::iterator iTests = tests.begin();
|
||||
vector <FGCondition>::iterator iConditions;
|
||||
bool pass = false;
|
||||
|
||||
FGFCSComponent::Run(); // call the base class for initialization of Input
|
||||
|
||||
while (iTests < tests.end()) {
|
||||
iConditions = iTests->conditions.begin();
|
||||
|
||||
if (iTests->Logic == eDefault) {
|
||||
Output = iTests->GetValue();
|
||||
} else if (iTests->Logic == eAND) {
|
||||
pass = true;
|
||||
while (iConditions < iTests->conditions.end()) {
|
||||
if (!iConditions->Evaluate()) pass = false;
|
||||
*iConditions++;
|
||||
}
|
||||
} else if (iTests->Logic == eOR) {
|
||||
pass = false;
|
||||
while (iConditions < iTests->conditions.end()) {
|
||||
if (iConditions->Evaluate()) pass = true;
|
||||
*iConditions++;
|
||||
}
|
||||
} else {
|
||||
cerr << "Invalid logic test" << endl;
|
||||
}
|
||||
|
||||
if (pass) {
|
||||
Output = iTests->GetValue();
|
||||
break;
|
||||
}
|
||||
*iTests++;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
// 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 FGSwitch::Debug(int from)
|
||||
{
|
||||
vector <test>::iterator iTests = tests.begin();
|
||||
vector <FGCondition>::iterator iConditions;
|
||||
string comp, scratch;
|
||||
string indent = " ";
|
||||
bool first = false;
|
||||
|
||||
if (debug_lvl <= 0) return;
|
||||
|
||||
if (debug_lvl & 1) { // Standard console startup message output
|
||||
if (from == 0) { // Constructor
|
||||
while (iTests < tests.end()) {
|
||||
|
||||
scratch = " if ";
|
||||
|
||||
switch(iTests->Logic) {
|
||||
case (elUndef):
|
||||
comp = " UNSET ";
|
||||
cerr << "Unset logic for test condition" << endl;
|
||||
break;
|
||||
case (eAND):
|
||||
comp = " AND ";
|
||||
break;
|
||||
case (eOR):
|
||||
comp=" OR ";
|
||||
break;
|
||||
case (eDefault):
|
||||
scratch = " by default.";
|
||||
break;
|
||||
default:
|
||||
comp = " UNKNOWN ";
|
||||
cerr << "Unknown logic for test condition" << endl;
|
||||
}
|
||||
|
||||
if (iTests->OutputProp != 0L)
|
||||
if (iTests->sign < 0)
|
||||
cout << indent << "Switch VALUE is - " << iTests->OutputProp->GetName() << scratch << endl;
|
||||
else
|
||||
cout << indent << "Switch VALUE is " << iTests->OutputProp->GetName() << scratch << endl;
|
||||
else
|
||||
cout << indent << "Switch VALUE is " << iTests->OutputVal << scratch << endl;
|
||||
|
||||
iConditions = iTests->conditions.begin();
|
||||
first = true;
|
||||
while (iConditions < iTests->conditions.end()) {
|
||||
if (!first) cout << indent << comp << " ";
|
||||
else cout << indent << " ";
|
||||
first = false;
|
||||
iConditions->PrintCondition();
|
||||
cout << endl;
|
||||
*iConditions++;
|
||||
}
|
||||
cout << endl;
|
||||
*iTests++;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (debug_lvl & 2 ) { // Instantiation/Destruction notification
|
||||
if (from == 0) cout << "Instantiated: FGSwitch" << endl;
|
||||
if (from == 1) cout << "Destroyed: FGSwitch" << 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} //namespace JSBSim
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue