--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+Module: FGMatrix33.cpp
+Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
+Date started: 1998
+Purpose: FGMatrix33 class
+Called by: Various
+
+FUNCTIONAL DESCRIPTION
+--------------------------------------------------------------------------------
+
+HISTORY
+--------------------------------------------------------------------------------
+??/??/?? TP Created
+03/16/2000 JSB Added exception throwing
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include "FGColumnVector3.h"
+#include "FGMatrix33.h"
+
+
+static const char *IdSrc = "$Id$";
+static const char *IdHdr = ID_COLUMNVECTOR3;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+FGColumnVector3::FGColumnVector3(void)
+{
+ data = new double[4];
+ rowCtr = 1;
+ //cout << "Allocated: " << data << endl;
+ //if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3::FGColumnVector3(int m)
+{
+ data = new double[4];
+ rowCtr = 1;
+ data[1]=0;data[2]=0;data[3]=0;
+ //cout << "Allocated: " << data << endl;
+ //if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3::~FGColumnVector3(void)
+{
+ //cout << "Freed: " << data << endl;
+ delete[] data;
+ data = NULL;
+ if (debug_lvl & 2) cout << "Destroyed: FGColumnVector3" << endl;
+}
+
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3::FGColumnVector3(const FGColumnVector3& b)
+{
+ data = new double[4];
+ data[1] = b.data[1];
+ data[2] = b.data[2];
+ data[3] = b.data[3];
+ rowCtr = 1;
+
+ if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGColumnVector3::operator=(const FGColumnVector3& b)
+{
+ data = new double[4];
+ data[1] = b.data[1];
+ data[2] = b.data[2];
+ data[3] = b.data[3];
+ rowCtr = 1;
+
+ if (debug_lvl & 2) cout << "Instantiated: FGColumnVector3" << endl;
+
+ return *this;
+}
+
+
+/* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+double& FGColumnVector3::operator()(int m) const
+{
+ return data[m];
+}
+ */
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+/* FGColumnVector3 operator*(const FGMatrix33& Mat, FGColumnVector3& Col)
+{
+ FGColumnVector3 Product;
+
+ Product(1) = Col(1)*Mat(1,1) + Col(2)*Mat(1,2) + Col(3)*Mat(1,3);
+ Product(2) = Col(1)*Mat(2,1) + Col(2)*Mat(2,2) + Col(3)*Mat(2,3);
+ Product(3) = Col(1)*Mat(3,1) + Col(2)*Mat(3,2) + Col(3)*Mat(3,3);
+
+ return Product;
+}
+ */
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGColumnVector3::operator+(const FGColumnVector3& C)
+{
+ FGColumnVector3 Sum;
+ Sum(1) = C(1) + data[1];
+ Sum(2) = C(2) + data[2];
+ Sum(3) = C(3) + data[3];
+
+ return Sum;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector3::operator+=(const FGColumnVector3& C)
+{
+ data[1] += C(1);
+ data[2] += C(2);
+ data[3] += C(3);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGColumnVector3::operator*(const double scalar)
+{
+ FGColumnVector3 Product;
+
+ Product(1) = scalar * data[1];
+ Product(2) = scalar * data[2];
+ Product(3) = scalar * data[3];
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector3::operator*=(const double scalar)
+{
+ data[1] *= scalar;
+ data[2] *= scalar;
+ data[3] *= scalar;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGColumnVector3::operator-(const FGColumnVector3& V)
+{
+
+ FGColumnVector3 Diff;
+
+ Diff(1) = data[1] - V(1);
+ Diff(2) = data[2] - V(2);
+ Diff(3) = data[3] - V(3);
+
+ return Diff;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector3::operator-=(const FGColumnVector3& V)
+{
+ data[1] -= V(1);
+ data[2] -= V(2);
+ data[3] -= V(3);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGColumnVector3::operator/(const double scalar)
+{
+ FGColumnVector3 Quotient;
+
+ if (scalar != 0) {
+ double tmp = 1.0/scalar;
+ Quotient(1) = data[1] * tmp;
+ Quotient(2) = data[2] * tmp;
+ Quotient(3) = data[3] * tmp;
+ } else {
+ cerr << "Attempt to divide by zero in method FGColumnVector3::operator/(const double scalar), object " << this << endl;
+ }
+ return Quotient;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector3::operator/=(const double scalar)
+{
+ FGColumnVector3 Quotient;
+
+ if (scalar != 0) {
+ double tmp = 1.0/scalar;
+ data[1] *= tmp;
+ data[2] *= tmp;
+ data[3] *= tmp;
+ } else {
+ cerr << "Attempt to divide by zero in method FGColumnVector3::operator/=(const double scalar), object " << this << endl;
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 operator*(const double scalar, const FGColumnVector3& C)
+{
+ FGColumnVector3 Product;
+
+ Product(1) = scalar * C(1);
+ Product(2) = scalar * C(2);
+ Product(3) = scalar * C(3);
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+float FGColumnVector3::Magnitude(void)
+{
+ double num;
+
+ if ((data[1] == 0.00) &&
+ (data[2] == 0.00) &&
+ (data[3] == 0.00))
+ {
+ return 0.00;
+ } else {
+ num = data[1]*data[1];
+ num += data[2]*data[2];
+ num += data[3]*data[3];
+ return sqrt(num);
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGColumnVector3::Normalize(void)
+{
+ double Mag = Magnitude();
+
+ if (Mag != 0) {
+ Mag = 1.0/Mag;
+ data[1] *= Mag;
+ data[2] *= Mag;
+ data[3] *= Mag;
+ }
+
+ return *this;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGColumnVector3::operator*(const FGColumnVector3& V)
+{
+ FGColumnVector3 Product;
+
+ Product(1) = data[2] * V(3) - data[3] * V(2);
+ Product(2) = data[3] * V(1) - data[1] * V(3);
+ Product(3) = data[1] * V(2) - data[2] * V(1);
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector3::operator*=(const FGColumnVector3& V)
+{
+ double a,b,c;
+ a = data[1]; b=data[2]; c=data[3];
+
+ data[1] = b * V(3) - c * V(2);
+ data[2] = c * V(1) - a * V(3);
+ data[3] = a * V(2) - b * V(1);
+
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V)
+{
+ FGColumnVector3 Product;
+
+ Product(1) = data[1] * V(1);
+ Product(2) = data[2] * V(2);
+ Product(3) = data[3] * V(3);
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector3::Debug(void)
+{
+ //TODO: Add your source code here
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ostream& operator<<(ostream& os, const FGColumnVector3& col)
+{
+ os << col(1) << " , " << col(2) << " , " << col(3);
+ return os;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3& FGColumnVector3::operator<<(const float ff)
+{
+ data[rowCtr] = ff;
+ if (++rowCtr > 3 )
+ rowCtr = 1;
+ return *this;
+}
--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+Header: FGMatrix33.h
+Author: Originally by Tony Peden [formatted and adapted here by Jon Berndt]
+Date started: Unknown
+
+HISTORY
+--------------------------------------------------------------------------------
+??/??/?? TP Created
+03/16/2000 JSB Added exception throwing
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+SENTRY
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#ifndef FGCOLUMNVECTOR3_H
+#define FGCOLUMNVECTOR3_H
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include <stdlib.h>
+#ifdef FGFS
+# include <math.h>
+# include <simgear/compiler.h>
+# include STL_STRING
+# include STL_FSTREAM
+# include STL_IOSTREAM
+ SG_USING_STD(string);
+# if !defined(SG_HAVE_NATIVE_SGI_COMPILERS)
+ SG_USING_STD(ostream);
+ SG_USING_STD(istream);
+ SG_USING_STD(cerr);
+ SG_USING_STD(cout);
+ SG_USING_STD(endl);
+# endif
+#else
+# include <string>
+# if defined(sgi) && !defined(__GNUC__)
+# include <fstream.h>
+# include <math.h>
+# include <iostream.h>
+# else
+# include <fstream>
+# include <cmath>
+# include <iostream>
+ using std::ostream;
+ using std::istream;
+ using std::cerr;
+ using std::cout;
+ using std::endl;
+# endif
+ using std::string;
+#endif
+
+#include "FGMatrix33.h"
+#include "FGJSBBase.h"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DEFINITIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#define ID_COLUMNVECTOR3 "$Id$"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+FORWARD DECLARATIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class FGMatrix33;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DECLARATION: FGColumnVector3
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class FGColumnVector3 : public FGJSBBase
+{
+public:
+ FGColumnVector3(void);
+ FGColumnVector3(int m);
+ FGColumnVector3(const FGColumnVector3& b);
+ ~FGColumnVector3(void);
+
+ FGColumnVector3 operator=(const FGColumnVector3& b);
+
+ FGColumnVector3 operator*(const double scalar);
+ FGColumnVector3 operator*(const FGColumnVector3& V); // Cross product operator
+ FGColumnVector3 operator/(const double scalar);
+ FGColumnVector3 operator+(const FGColumnVector3& B); // must not return reference
+ FGColumnVector3 operator-(const FGColumnVector3& B);
+
+ void operator-=(const FGColumnVector3 &B);
+ void operator+=(const FGColumnVector3 &B);
+ void operator*=(const FGColumnVector3 &B);
+ void operator*=(const double scalar);
+ void operator/=(const double scalar);
+
+ FGColumnVector3& operator<<(const float ff);
+
+ inline void InitMatrix(void) { data[1]=0; data[2]=0; data[3]=0; }
+ inline void InitMatrix(float ff) { data[1]=ff; data[2]=ff; data[3]=ff; }
+
+ float Magnitude(void);
+ FGColumnVector3 Normalize(void);
+
+ friend FGColumnVector3 operator*(const double scalar, const FGColumnVector3& A);
+ //friend FGColumnVector3 operator*(const FGMatrix33& M, FGColumnVector3& V);
+
+ friend ostream& operator<<(ostream& os, const FGColumnVector3& col);
+
+ inline double& operator()(int m) const { return data[m]; }
+
+ FGColumnVector3 multElementWise(const FGColumnVector3& V);
+
+private:
+ double *data;
+ int rowCtr;
+ void Debug(void);
+};
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+#endif
--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+Module: FGMatrix33.cpp
+Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
+Date started: 1998
+Purpose: FGMatrix33 class
+Called by: Various
+
+FUNCTIONAL DESCRIPTION
+--------------------------------------------------------------------------------
+
+HISTORY
+--------------------------------------------------------------------------------
+??/??/?? TP Created
+03/16/2000 JSB Added exception throwing
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include "FGColumnVector4.h"
+
+static const char *IdSrc = "$Id$";
+static const char *IdHdr = ID_COLUMNVECTOR4;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+FGColumnVector4::FGColumnVector4(void)
+{
+ data = new double[5];
+ rowCtr = 1;
+ //cout << "Allocated: " << data << endl;
+ if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4::FGColumnVector4(int m)
+{
+ data = new double[5];
+ rowCtr = 1;
+ data[1]=0;data[2]=0;data[3]=0;data[4]=0;
+ //cout << "Allocated: " << data << endl;
+ if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4::~FGColumnVector4(void)
+{
+ //cout << "Freed: " << data << endl;
+ delete[] data;
+ data = NULL;
+ if (debug_lvl & 2) cout << "Destroyed: FGColumnVector4" << endl;
+}
+
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4::FGColumnVector4(const FGColumnVector4& b)
+{
+ data = new double[5];
+ data[1] = b.data[1];
+ data[2] = b.data[2];
+ data[3] = b.data[3];
+ data[4] = b.data[4];
+
+ rowCtr = 1;
+
+ if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4 FGColumnVector4::operator=(const FGColumnVector4& b)
+{
+ data = new double[5];
+ data[1] = b.data[1];
+ data[2] = b.data[2];
+ data[3] = b.data[3];
+ data[4] = b.data[4];
+ rowCtr = 1;
+
+ if (debug_lvl & 2) cout << "Instantiated: FGColumnVector4" << endl;
+
+ return *this;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4 FGColumnVector4::operator+(const FGColumnVector4& C)
+{
+ FGColumnVector4 Sum;
+
+ Sum(1) = C(1) + data[1];
+ Sum(2) = C(2) + data[2];
+ Sum(3) = C(3) + data[3];
+ Sum(4) = C(4) + data[4];
+ return Sum;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector4::operator+=(const FGColumnVector4& C)
+{
+ data[1] += C(1);
+ data[2] += C(2);
+ data[3] += C(3);
+ data[4] += C(4);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4 FGColumnVector4::operator*(const double scalar)
+{
+ FGColumnVector4 Product;
+
+ Product(1) = scalar * data[1];
+ Product(2) = scalar * data[2];
+ Product(3) = scalar * data[3];
+ Product(4) = scalar * data[4];
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector4::operator*=(const double scalar)
+{
+ data[1] *= scalar;
+ data[2] *= scalar;
+ data[3] *= scalar;
+ data[4] *= scalar;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4 FGColumnVector4::operator-(const FGColumnVector4& V)
+{
+
+ FGColumnVector4 Diff;
+
+ Diff(1) = data[1] - V(1);
+ Diff(2) = data[2] - V(2);
+ Diff(3) = data[3] - V(3);
+ Diff(4) = data[4] - V(4);
+
+ return Diff;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector4::operator-=(const FGColumnVector4& V)
+{
+ data[1] -= V(1);
+ data[2] -= V(2);
+ data[3] -= V(3);
+ data[4] -= V(4);
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4 FGColumnVector4::operator/(const double scalar)
+{
+ FGColumnVector4 Quotient;
+
+ if (scalar != 0) {
+ double tmp = 1.0/scalar;
+ Quotient(1) = data[1] * tmp;
+ Quotient(2) = data[2] * tmp;
+ Quotient(3) = data[3] * tmp;
+ Quotient(4) = data[4] * tmp;
+ } else {
+ cerr << "Attempt to divide by zero in method FGColumnVector4::operator/(const double scalar), object " << this << endl;
+ }
+ return Quotient;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector4::operator/=(const double scalar)
+{
+ FGColumnVector4 Quotient;
+
+ if (scalar != 0) {
+ double tmp = 1.0/scalar;
+ data[1] *= tmp;
+ data[2] *= tmp;
+ data[3] *= tmp;
+ data[4] *= tmp;
+ } else {
+ cerr << "Attempt to divide by zero in method FGColumnVector4::operator/=(const double scalar), object " << this << endl;
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4 operator*(const double scalar, const FGColumnVector4& C)
+{
+ FGColumnVector4 Product;
+
+ Product(1) = scalar * C(1);
+ Product(2) = scalar * C(2);
+ Product(3) = scalar * C(3);
+ Product(4) = scalar * C(4);
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+float FGColumnVector4::Magnitude(void)
+{
+ double num;
+
+ if ((data[1] == 0.00) &&
+ (data[2] == 0.00) &&
+ (data[3] == 0.00) &&
+ (data[4] == 0.00))
+ {
+ return 0.00;
+ } else {
+ num = data[1]*data[1];
+ num += data[2]*data[2];
+ num += data[3]*data[3];
+ num += data[4]*data[4];
+ return sqrt(num);
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4 FGColumnVector4::Normalize(void)
+{
+ double Mag = Magnitude();
+
+ if (Mag != 0) {
+ Mag = 1.0/Mag;
+ data[1] *= Mag;
+ data[2] *= Mag;
+ data[3] *= Mag;
+ data[4] *= Mag;
+ }
+
+ return *this;
+}
+
+/* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4 FGColumnVector4::operator*(const FGColumnVector4& V)
+{
+ FGColumnVector4 Product;
+
+ Product(1) = data[2] * V(3) - data[3] * V(2);
+ Product(2) = data[3] * V(1) - data[1] * V(3);
+ Product(3) = data[1] * V(2) - data[2] * V(1);
+
+ return Product;
+} */
+
+
+/* //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector4::operator*=(const FGColumnVector4& V)
+{
+ double a,b,c;
+ a = data[1]; b=data[2]; c=data[3];
+
+ data[1] = b * V(3) - c * V(2);
+ data[2] = c * V(1) - a * V(3);
+ data[3] = a * V(2) - b * V(1);
+
+}
+ */
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4 FGColumnVector4::multElementWise(const FGColumnVector4& V)
+{
+ FGColumnVector4 Product;
+
+ Product(1) = data[1] * V(1);
+ Product(2) = data[2] * V(2);
+ Product(3) = data[3] * V(3);
+ Product(4) = data[4] * V(4);
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGColumnVector4::Debug(void)
+{
+ //TODO: Add your source code here
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ostream& operator<<(ostream& os, FGColumnVector4& col)
+{
+ os << col(1) << " , " << col(2) << " , " << col(3) << " , " << col(4);
+ return os;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector4& FGColumnVector4::operator<<(const float ff)
+{
+ data[rowCtr] = ff;
+ if (++rowCtr > 4 )
+ rowCtr = 1;
+ return *this;
+}
+
--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+Header: FGMatrix33.h
+Author: Originally by Tony Peden [formatted and adapted here by Jon Berndt]
+Date started: Unknown
+
+HISTORY
+--------------------------------------------------------------------------------
+??/??/?? TP Created
+03/16/2000 JSB Added exception throwing
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+SENTRY
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#ifndef FGCOLUMNVECTOR4_H
+#define FGCOLUMNVECTOR4_H
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include <stdlib.h>
+#ifdef FGFS
+# include <math.h>
+# include <simgear/compiler.h>
+# include STL_STRING
+# include STL_FSTREAM
+# include STL_IOSTREAM
+ SG_USING_STD(string);
+# if !defined(SG_HAVE_NATIVE_SGI_COMPILERS)
+ SG_USING_STD(ostream);
+ SG_USING_STD(istream);
+ SG_USING_STD(cerr);
+ SG_USING_STD(cout);
+ SG_USING_STD(endl);
+# endif
+#else
+# include <string>
+# if defined (sgi) && !defined(__GNUC__)
+# include <fstream.h>
+# include <math.h>
+# include <iostream.h>
+# else
+# include <fstream>
+# include <cmath>
+# include <iostream>
+ using std::ostream;
+ using std::istream;
+ using std::cerr;
+ using std::cout;
+ using std::endl;
+# endif
+ using std::string;
+#endif
+
+#include "FGJSBBase.h"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DEFINITIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#define ID_COLUMNVECTOR4 "$Id$"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+FORWARD DECLARATIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DECLARATION: FGColumnVector4
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class FGColumnVector4 : public FGJSBBase
+{
+public:
+ FGColumnVector4(void);
+ FGColumnVector4(int m);
+ FGColumnVector4(const FGColumnVector4& b);
+ ~FGColumnVector4(void);
+
+ FGColumnVector4 operator=(const FGColumnVector4& b);
+
+ FGColumnVector4 operator*(const double scalar);
+ //FGColumnVector4 operator*(const FGColumnVector4& V); // Cross product operator
+ FGColumnVector4 operator/(const double scalar);
+ FGColumnVector4 operator+(const FGColumnVector4& B); // must not return reference
+ FGColumnVector4 operator-(const FGColumnVector4& B);
+
+ void operator-=(const FGColumnVector4 &B);
+ void operator+=(const FGColumnVector4 &B);
+ //void operator*=(const FGColumnVector4 &B);
+ void operator*=(const double scalar);
+ void operator/=(const double scalar);
+
+ inline double& operator()(int m) const { return data[m]; }
+
+ FGColumnVector4& operator<<(const float ff);
+
+ inline void InitMatrix(void) { data[1]=0; data[2]=0; data[3]=0; }
+ inline void InitMatrix(float ff) { data[1]=ff; data[2]=ff; data[3]=ff; }
+
+ float Magnitude(void);
+ FGColumnVector4 Normalize(void);
+
+ friend FGColumnVector4 operator*(const double scalar, const FGColumnVector4& A);
+
+ friend ostream& operator<<(ostream& os, FGColumnVector4& col);
+
+
+ FGColumnVector4 multElementWise(const FGColumnVector4& V);
+
+private:
+ double *data;
+ int rowCtr;
+ void Debug(void);
+};
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+#endif
--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ Module: FGJSBBase.cpp
+ Author: Jon S. Berndt
+ Date started: 07/01/01
+ Purpose: Encapsulates the JSBBase object
+
+ ------------- Copyright (C) 2001 Jon S. Berndt (jsb@hal-pc.org) -------------
+
+ This program is free software; you can redistribute it and/or modify it under
+ the terms of the GNU General Public License as published by the Free Software
+ Foundation; either version 2 of the License, or (at your option) any later
+ version.
+
+ This program is distributed in the hope that it will be useful, but WITHOUT
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
+ details.
+
+ You should have received a copy of the GNU General Public License along with
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple
+ Place - Suite 330, Boston, MA 02111-1307, USA.
+
+ Further information about the GNU General Public License can also be found on
+ the world wide web at http://www.gnu.org.
+
+FUNCTIONAL DESCRIPTION
+--------------------------------------------------------------------------------
+
+HISTORY
+--------------------------------------------------------------------------------
+07/01/01 JSB Created
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include "FGJSBBase.h"
+
+static const char *IdSrc = "$Id$";
+static const char *IdHdr = ID_JSBBASE;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+char FGJSBBase::highint[5] = {27, '[', '1', 'm', '\0' };
+char FGJSBBase::halfint[5] = {27, '[', '2', 'm', '\0' };
+char FGJSBBase::normint[6] = {27, '[', '2', '2', 'm', '\0' };
+char FGJSBBase::reset[5] = {27, '[', '0', 'm', '\0' };
+char FGJSBBase::underon[5] = {27, '[', '4', 'm', '\0' };
+char FGJSBBase::underoff[6] = {27, '[', '2', '4', 'm', '\0' };
+char FGJSBBase::fgblue[6] = {27, '[', '3', '4', 'm', '\0' };
+char FGJSBBase::fgcyan[6] = {27, '[', '3', '6', 'm', '\0' };
+char FGJSBBase::fgred[6] = {27, '[', '3', '1', 'm', '\0' };
+char FGJSBBase::fggreen[6] = {27, '[', '3', '2', 'm', '\0' };
+char FGJSBBase::fgdef[6] = {27, '[', '3', '9', 'm', '\0' };
+
+short FGJSBBase::debug_lvl = 0;
+
--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ Header: FGJSBBase.h
+ Author: Jon S. Berndt
+ Date started: 07/01/01
+
+ ------------- Copyright (C) 2001 Jon S. Berndt (jsb@hal-pc.org) -------------
+
+ This program is free software; you can redistribute it and/or modify it under
+ the terms of the GNU General Public License as published by the Free Software
+ Foundation; either version 2 of the License, or (at your option) any later
+ version.
+
+ This program is distributed in the hope that it will be useful, but WITHOUT
+ ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
+ details.
+
+ You should have received a copy of the GNU General Public License along with
+ this program; if not, write to the Free Software Foundation, Inc., 59 Temple
+ Place - Suite 330, Boston, MA 02111-1307, USA.
+
+ Further information about the GNU General Public License can also be found on
+ the world wide web at http://www.gnu.org.
+
+HISTORY
+--------------------------------------------------------------------------------
+07/01/01 JSB Created
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+SENTRY
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#ifndef FGJSBBASE_H
+#define FGJSBBASE_H
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#ifdef FGFS
+# include <simgear/compiler.h>
+# include <math.h>
+#else
+# if defined(sgi) && !defined(__GNUC__)
+# include <math.h>
+# else
+# include <cmath>
+# endif
+#endif
+
+#ifndef M_PI
+# include <simgear/constants.h>
+# define M_PI SG_PI
+#endif
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DEFINITIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#define ID_JSBBASE "$Id$"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+FORWARD DECLARATIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs]
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS DOCUMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+/** JSBSim Base class.
+ @author Jon S. Berndt
+ @version $Id$
+*/
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS DECLARATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class FGJSBBase {
+public:
+ /// Constructor for FGJSBBase.
+ FGJSBBase() {};
+
+ /// Destructor for FGJSBBase
+ virtual ~FGJSBBase() {};
+
+ enum {eL = 1, eM, eN };
+ enum {eP = 1, eQ, eR };
+ enum {eU = 1, eV, eW };
+ enum {eX = 1, eY, eZ };
+ enum {ePhi = 1, eTht, ePsi };
+ enum {eDrag = 1, eSide, eLift };
+ enum {eRoll = 1, ePitch, eYaw };
+ enum {eNorth = 1, eEast, eDown };
+
+ static char highint[5];
+ static char halfint[5];
+ static char normint[6];
+ static char reset[5];
+ static char underon[5];
+ static char underoff[6];
+ static char fgblue[6];
+ static char fgcyan[6];
+ static char fgred[6];
+ static char fggreen[6];
+ static char fgdef[6];
+
+protected:
+ virtual void Debug(void) {};
+
+ static short debug_lvl;
+ static int frame;
+};
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+#endif
+
--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+Module: FGMatrix33.cpp
+Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
+Date started: 1998
+Purpose: FGMatrix33 class
+Called by: Various
+
+FUNCTIONAL DESCRIPTION
+--------------------------------------------------------------------------------
+
+HISTORY
+--------------------------------------------------------------------------------
+??/??/?? TP Created
+03/16/2000 JSB Added exception throwing
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include "FGMatrix33.h"
+#include "FGColumnVector3.h"
+
+static const char *IdSrc = "$Id$";
+static const char *IdHdr = ID_MATRIX33;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+CLASS IMPLEMENTATION
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+double** FGalloc(void)
+{
+ double **A;
+
+ A = new double *[4];
+ if (!A) return NULL;
+
+ double *tmp;
+ tmp = new double [16];
+
+ if (!tmp) {
+ delete A;
+ return NULL;
+ }
+ A[0] = tmp;
+ A[1] = tmp + 4;
+ A[2] = tmp + 8;
+ A[3] = tmp + 12;
+#if 0
+ A[0] = new double [4];
+ if (!A[0]) return NULL;
+ A[1] = new double [4];
+ if (!A[1]) return NULL;
+ A[2] = new double [4];
+ if (!A[2]) return NULL;
+ A[3] = new double [4];
+ if (!A[3]) return NULL;
+#endif
+
+ return A;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void dealloc(double **A)
+{
+ delete[] A[0];
+ delete[] A;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33::FGMatrix33(void)
+{
+ data=FGalloc();
+ InitMatrix();
+ rowCtr = colCtr = 1;
+
+ if (debug_lvl & 2) cout << "Instantiated: FGMatrix33" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33::FGMatrix33(int r, int c)
+{
+ data=FGalloc();
+ InitMatrix();
+ rowCtr = colCtr = 1;
+
+ if (debug_lvl & 2) cout << "Instantiated: FGMatrix33" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33::FGMatrix33(const FGMatrix33& M)
+{
+ rowCtr = colCtr = 1;
+ width = M.width;
+ prec = M.prec;
+ delim = M.delim;
+ origin = M.origin;
+
+ data=FGalloc();
+
+ data[1][1] = M.data[1][1];
+ data[1][2] = M.data[1][2];
+ data[1][3] = M.data[1][3];
+ data[2][1] = M.data[2][1];
+ data[2][2] = M.data[2][2];
+ data[2][3] = M.data[2][3];
+ data[3][1] = M.data[3][1];
+ data[3][2] = M.data[3][2];
+ data[3][3] = M.data[3][3];
+
+ if (debug_lvl & 2) cout << "Instantiated: FGMatrix33" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33::~FGMatrix33(void)
+{
+ dealloc(data);
+ rowCtr = colCtr = 1;
+
+ if (debug_lvl & 2) cout << "Destroyed: FGMatrix33" << endl;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+ostream& operator<<(ostream& os, const FGMatrix33& M)
+{
+ for (unsigned int i=1; i<=M.Rows(); i++) {
+ for (unsigned int j=1; j<=M.Cols(); j++) {
+ if (i == M.Rows() && j == M.Cols())
+ os << M.data[i][j];
+ else
+ os << M.data[i][j] << ", ";
+ }
+ }
+ return os;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33& FGMatrix33::operator<<(const float ff)
+{
+ data[rowCtr][colCtr] = ff;
+ if (++colCtr > Cols()) {
+ colCtr = 1;
+ if (++rowCtr > Rows())
+ rowCtr = 1;
+ }
+ return *this;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+istream& operator>>(istream& is, FGMatrix33& M)
+{
+ for (unsigned int i=1; i<=M.Rows(); i++) {
+ for (unsigned int j=1; j<=M.Cols(); j++) {
+ is >> M.data[i][j];
+ }
+ }
+ return is;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33& FGMatrix33::operator=(const FGMatrix33& M)
+{
+ if (&M != this) {
+ if (data != NULL) dealloc(data);
+
+ width = M.width;
+ prec = M.prec;
+ delim = M.delim;
+ origin = M.origin;
+
+ data=FGalloc();
+
+ data[1][1] = M.data[1][1];
+ data[1][2] = M.data[1][2];
+ data[1][3] = M.data[1][3];
+ data[2][1] = M.data[2][1];
+ data[2][2] = M.data[2][2];
+ data[2][3] = M.data[2][3];
+ data[3][1] = M.data[3][1];
+ data[3][2] = M.data[3][2];
+ data[3][3] = M.data[3][3];
+
+ }
+ return *this;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::SetOParams(char delim,int width,int prec,int origin)
+{
+ FGMatrix33::delim = delim;
+ FGMatrix33::width = width;
+ FGMatrix33::prec = prec;
+ FGMatrix33::origin = origin;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::InitMatrix(double value)
+{
+ if (data) {
+ data[1][1] = value;
+ data[1][2] = value;
+ data[1][3] = value;
+ data[2][1] = value;
+ data[2][2] = value;
+ data[2][3] = value;
+ data[3][1] = value;
+ data[3][2] = value;
+ data[3][3] = value;
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::InitMatrix(void)
+{
+ this->InitMatrix(0);
+}
+
+// *****************************************************************************
+// binary operators ************************************************************
+// *****************************************************************************
+
+FGMatrix33 FGMatrix33::operator-(const FGMatrix33& M)
+{
+ FGMatrix33 Diff;
+
+ Diff(1,1) = data[1][1] - M(1,1);
+ Diff(1,2) = data[1][2] - M(1,2);
+ Diff(1,3) = data[1][3] - M(1,3);
+ Diff(2,1) = data[2][1] - M(2,1);
+ Diff(2,2) = data[2][2] - M(2,2);
+ Diff(2,3) = data[2][3] - M(2,3);
+ Diff(3,1) = data[3][1] - M(3,1);
+ Diff(3,2) = data[3][2] - M(3,2);
+ Diff(3,3) = data[3][3] - M(3,3);
+
+
+ return Diff;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::operator-=(const FGMatrix33 &M)
+{
+ data[1][1] -= M(1,1);
+ data[1][2] -= M(1,2);
+ data[1][3] -= M(1,3);
+ data[2][1] -= M(2,1);
+ data[2][2] -= M(2,2);
+ data[2][3] -= M(2,3);
+ data[3][1] -= M(3,1);
+ data[3][2] -= M(3,2);
+ data[3][3] -= M(3,3);
+
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 FGMatrix33::operator+(const FGMatrix33& M)
+{
+ FGMatrix33 Sum;
+
+ Sum(1,1) = data[1][1] + M(1,1);
+ Sum(1,2) = data[1][2] + M(1,2);
+ Sum(1,3) = data[1][3] + M(1,3);
+ Sum(2,1) = data[2][1] + M(2,1);
+ Sum(2,2) = data[2][2] + M(2,2);
+ Sum(2,3) = data[2][3] + M(2,3);
+ Sum(3,1) = data[3][1] + M(3,1);
+ Sum(3,2) = data[3][2] + M(3,2);
+ Sum(3,3) = data[3][3] + M(3,3);
+
+ return Sum;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::operator+=(const FGMatrix33 &M)
+{
+ data[1][1] += M(1,1);
+ data[1][2] += M(1,2);
+ data[1][3] += M(1,3);
+ data[2][1] += M(2,1);
+ data[2][2] += M(2,2);
+ data[2][3] += M(2,3);
+ data[3][1] += M(3,1);
+ data[3][2] += M(3,2);
+ data[3][3] += M(3,3);
+
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 FGMatrix33::operator*(const double scalar)
+{
+ FGMatrix33 Product;
+
+ Product(1,1) = data[1][1] * scalar;
+ Product(1,2) = data[1][2] * scalar;
+ Product(1,3) = data[1][3] * scalar;
+ Product(2,1) = data[2][1] * scalar;
+ Product(2,2) = data[2][2] * scalar;
+ Product(2,3) = data[2][3] * scalar;
+ Product(3,1) = data[3][1] * scalar;
+ Product(3,2) = data[3][2] * scalar;
+ Product(3,3) = data[3][3] * scalar;
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 operator*(double scalar, FGMatrix33 &M)
+{
+ FGMatrix33 Product;
+
+ Product(1,1) = M(1,1) * scalar;
+ Product(1,2) = M(1,2) * scalar;
+ Product(1,3) = M(1,3) * scalar;
+ Product(2,1) = M(2,1) * scalar;
+ Product(2,2) = M(2,2) * scalar;
+ Product(2,3) = M(2,3) * scalar;
+ Product(3,1) = M(3,1) * scalar;
+ Product(3,2) = M(3,2) * scalar;
+ Product(3,3) = M(3,3) * scalar;
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::operator*=(const double scalar)
+{
+
+ data[1][1] *= scalar;
+ data[1][2] *= scalar;
+ data[1][3] *= scalar;
+ data[2][1] *= scalar;
+ data[2][2] *= scalar;
+ data[2][3] *= scalar;
+ data[3][1] *= scalar;
+ data[3][2] *= scalar;
+ data[3][3] *= scalar;
+
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 FGMatrix33::operator*(const FGMatrix33& M)
+{
+ FGMatrix33 Product;
+
+ Product(1,1) = data[1][1]*M(1,1) + data[1][2]*M(2,1) + data[1][3]*M(3,1);
+ Product(1,2) = data[1][1]*M(1,2) + data[1][2]*M(2,2) + data[1][3]*M(3,2);
+ Product(1,3) = data[1][1]*M(1,3) + data[1][2]*M(2,3) + data[1][3]*M(3,3);
+ Product(2,1) = data[2][1]*M(1,1) + data[2][2]*M(2,1) + data[2][3]*M(3,1);
+ Product(2,2) = data[2][1]*M(1,2) + data[2][2]*M(2,2) + data[2][3]*M(3,2);
+ Product(2,3) = data[2][1]*M(1,3) + data[2][2]*M(2,3) + data[2][3]*M(3,3);
+ Product(3,1) = data[3][1]*M(1,1) + data[3][2]*M(2,1) + data[3][3]*M(3,1);
+ Product(3,2) = data[3][1]*M(1,2) + data[3][2]*M(2,2) + data[3][3]*M(3,2);
+ Product(3,3) = data[3][1]*M(1,3) + data[3][2]*M(2,3) + data[3][3]*M(3,3);
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::operator*=(const FGMatrix33& M)
+{
+ float a,b,c;
+
+ a = data[1][1]; b=data[1][2]; c=data[1][3];
+ data[1][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
+ data[1][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
+ data[1][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
+
+ a = data[2][1]; b=data[2][2]; c=data[2][3];
+ data[2][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
+ data[2][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
+ data[2][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
+
+ a = data[3][1]; b=data[3][2]; c=data[3][3];
+ data[3][1] = a*M(1,1) + b*M(2,1) + c*M(3,1);
+ data[3][2] = a*M(1,2) + b*M(2,2) + c*M(3,2);
+ data[3][3] = a*M(1,3) + b*M(2,3) + c*M(3,3);
+
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGMatrix33 FGMatrix33::operator/(const double scalar)
+{
+ FGMatrix33 Quot;
+
+ if( scalar != 0 ) {
+ double tmp = 1.0/scalar;
+ Quot(1,1) = data[1][1] * tmp;
+ Quot(1,2) = data[1][2] * tmp;
+ Quot(1,3) = data[1][3] * tmp;
+ Quot(2,1) = data[2][1] * tmp;
+ Quot(2,2) = data[2][2] * tmp;
+ Quot(2,3) = data[2][3] * tmp;
+ Quot(3,1) = data[3][1] * tmp;
+ Quot(3,2) = data[3][2] * tmp;
+ Quot(3,3) = data[3][3] * tmp;
+ } else {
+ MatrixException mE;
+ mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/(const double scalar)";
+ throw mE;
+ }
+ return Quot;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::operator/=(const double scalar)
+{
+ if( scalar != 0 ) {
+ double tmp = 1.0/scalar;
+ data[1][1] *= tmp;
+ data[1][2] *= tmp;
+ data[1][3] *= tmp;
+ data[2][1] *= tmp;
+ data[2][2] *= tmp;
+ data[2][3] *= tmp;
+ data[3][1] *= tmp;
+ data[3][2] *= tmp;
+ data[3][3] *= tmp;
+ } else {
+ MatrixException mE;
+ mE.Message = "Attempt to divide by zero in method FGMatrix33::operator/=(const double scalar)";
+ throw mE;
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::T(void)
+{
+ for (unsigned int i=1; i<=3; i++) {
+ for (unsigned int j=i+1; j<=3; j++) {
+ double tmp = data[i][j];
+ data[i][j] = data[j][i];
+ data[j][i] = tmp;
+ }
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& Col)
+{
+ FGColumnVector3 Product;
+
+ Product(1) = data[1][1]*Col(1) + data[1][2]*Col(2) + data[1][3]*Col(3);
+ Product(2) = data[2][1]*Col(1) + data[2][2]*Col(2) + data[2][3]*Col(3);
+ Product(3) = data[3][1]*Col(1) + data[3][2]*Col(2) + data[3][3]*Col(3);
+
+ return Product;
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGMatrix33::Debug(void)
+{
+ //TODO: Add your source code here
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
--- /dev/null
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+Header: FGMatrix33.h
+Author: Originally by Tony Peden [formatted and adapted here by Jon Berndt]
+Date started: Unknown
+
+HISTORY
+--------------------------------------------------------------------------------
+??/??/?? TP Created
+03/16/2000 JSB Added exception throwing
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+SENTRY
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#ifndef FGMATRIX33_H
+#define FGMATRIX33_H
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+INCLUDES
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#include <stdlib.h>
+#ifdef FGFS
+# include <math.h>
+# include <simgear/compiler.h>
+# include STL_STRING
+# include STL_FSTREAM
+# include STL_IOSTREAM
+ SG_USING_STD(string);
+# if !defined(SG_HAVE_NATIVE_SGI_COMPILERS)
+ SG_USING_STD(ostream);
+ SG_USING_STD(istream);
+ SG_USING_STD(cerr);
+ SG_USING_STD(cout);
+ SG_USING_STD(endl);
+# endif
+#else
+# include <string>
+# if defined(sgi) && !defined(__GNUC__)
+# include <fstream.h>
+# include <math.h>
+# include <iostream.h>
+# else
+# include <fstream>
+# include <cmath>
+# include <iostream>
+ using std::ostream;
+ using std::istream;
+ using std::cerr;
+ using std::cout;
+ using std::endl;
+# endif
+ using std::string;
+#endif
+
+#include "FGColumnVector3.h"
+#include "FGJSBBase.h"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DEFINITIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+#define ID_MATRIX33 "$Id$"
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+FORWARD DECLARATIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class FGColumnVector3;
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DECLARATION: MatrixException
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class MatrixException : public FGJSBBase
+{
+public:
+ string Message;
+};
+
+/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+DECLARATION: FGMatrix33
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
+
+class FGMatrix33 : public FGJSBBase
+{
+public:
+ FGMatrix33(void);
+ FGMatrix33(int r, int c);
+ FGMatrix33(const FGMatrix33& A);
+ ~FGMatrix33(void);
+
+ FGMatrix33& operator=(const FGMatrix33& A);
+ inline double& operator()(unsigned int row, unsigned int col) const {return data[row][col];}
+
+ FGColumnVector3 operator*(const FGColumnVector3& Col);
+
+ inline unsigned int Rows(void) const { return 3; }
+ inline unsigned int Cols(void) const { return 3; }
+
+ void T(void);
+ void InitMatrix(void);
+ void InitMatrix(double value);
+
+ //friend FGMatrix33 operator*(double scalar,FGMatrix33& A);
+
+ FGMatrix33 operator-(const FGMatrix33& B);
+ FGMatrix33 operator+(const FGMatrix33& B);
+ FGMatrix33 operator*(const FGMatrix33& B);
+ FGMatrix33 operator*(const double scalar);
+ FGMatrix33 operator/(const double scalar);
+ FGMatrix33& operator<<(const float ff);
+
+ friend ostream& operator<<(ostream& os, const FGMatrix33& M);
+ friend istream& operator>>(istream& is, FGMatrix33& M);
+
+ void operator-=(const FGMatrix33 &B);
+ void operator+=(const FGMatrix33 &B);
+ void operator*=(const FGMatrix33 &B);
+ void operator*=(const double scalar);
+ void operator/=(const double scalar);
+
+
+ void SetOParams(char delim,int width,int prec, int origin=0);
+
+protected:
+ double **data;
+
+private:
+ char delim;
+ int width,prec,origin;
+ void TransposeSquare(void);
+ unsigned int rowCtr, colCtr;
+ void Debug(void);
+};
+
+#endif
projects / flightgear.git / commitdiff