1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
6 Purpose: FGMatrix33 class
10 --------------------------------------------------------------------------------
13 --------------------------------------------------------------------------------
15 03/16/2000 JSB Added exception throwing
17 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
19 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
21 #include "FGColumnVector4.h"
25 static const char *IdSrc = "$Id$";
26 static const char *IdHdr = ID_COLUMNVECTOR4;
28 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
32 FGColumnVector4::FGColumnVector4(void)
35 data[1]=0;data[2]=0;data[3]=0;data[4]=0;
40 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
42 FGColumnVector4::FGColumnVector4(double A, double B, double C, double D)
45 data[1]=0;data[2]=0;data[3]=0;data[4]=0;
50 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
52 FGColumnVector4::~FGColumnVector4(void)
58 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
60 FGColumnVector4::FGColumnVector4(const FGColumnVector4& b)
72 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
74 FGColumnVector4 FGColumnVector4::operator=(const FGColumnVector4& b)
84 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
86 FGColumnVector4 FGColumnVector4::operator+(const FGColumnVector4& C)
90 Sum(1) = C(1) + data[1];
91 Sum(2) = C(2) + data[2];
92 Sum(3) = C(3) + data[3];
93 Sum(4) = C(4) + data[4];
97 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
99 void FGColumnVector4::operator+=(const FGColumnVector4& C)
107 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
109 FGColumnVector4 FGColumnVector4::operator*(const double scalar)
111 FGColumnVector4 Product;
113 Product(1) = scalar * data[1];
114 Product(2) = scalar * data[2];
115 Product(3) = scalar * data[3];
116 Product(4) = scalar * data[4];
121 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
123 void FGColumnVector4::operator*=(const double scalar)
131 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
133 FGColumnVector4 FGColumnVector4::operator-(const FGColumnVector4& V)
136 FGColumnVector4 Diff;
138 Diff(1) = data[1] - V(1);
139 Diff(2) = data[2] - V(2);
140 Diff(3) = data[3] - V(3);
141 Diff(4) = data[4] - V(4);
146 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
148 void FGColumnVector4::operator-=(const FGColumnVector4& V)
156 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
158 FGColumnVector4 FGColumnVector4::operator/(const double scalar)
160 FGColumnVector4 Quotient;
163 double tmp = 1.0/scalar;
164 Quotient(1) = data[1] * tmp;
165 Quotient(2) = data[2] * tmp;
166 Quotient(3) = data[3] * tmp;
167 Quotient(4) = data[4] * tmp;
169 cerr << "Attempt to divide by zero in method FGColumnVector4::operator/(const double scalar), object " << this << endl;
174 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
176 void FGColumnVector4::operator/=(const double scalar)
178 FGColumnVector4 Quotient;
181 double tmp = 1.0/scalar;
187 cerr << "Attempt to divide by zero in method FGColumnVector4::operator/=(const double scalar), object " << this << endl;
191 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
193 FGColumnVector4 operator*(const double scalar, const FGColumnVector4& C)
195 FGColumnVector4 Product;
197 Product(1) = scalar * C(1);
198 Product(2) = scalar * C(2);
199 Product(3) = scalar * C(3);
200 Product(4) = scalar * C(4);
204 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
206 double FGColumnVector4::Magnitude(void)
210 if ((data[1] == 0.00) &&
217 num = data[1]*data[1];
218 num += data[2]*data[2];
219 num += data[3]*data[3];
220 num += data[4]*data[4];
225 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
227 FGColumnVector4 FGColumnVector4::Normalize(void)
229 double Mag = Magnitude();
242 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
244 FGColumnVector4 FGColumnVector4::multElementWise(const FGColumnVector4& V)
246 FGColumnVector4 Product;
248 Product(1) = data[1] * V(1);
249 Product(2) = data[2] * V(2);
250 Product(3) = data[3] * V(3);
251 Product(4) = data[4] * V(4);
256 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
258 ostream& operator<<(ostream& os, FGColumnVector4& col)
260 os << col(1) << " , " << col(2) << " , " << col(3) << " , " << col(4);
264 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
266 FGColumnVector4& FGColumnVector4::operator<<(const double ff)
269 if (++rowCtr > 4) rowCtr = 1;
272 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
273 // The bitmasked value choices are as follows:
274 // unset: In this case (the default) JSBSim would only print
275 // out the normally expected messages, essentially echoing
276 // the config files as they are read. If the environment
277 // variable is not set, debug_lvl is set to 1 internally
278 // 0: This requests JSBSim not to output any messages
280 // 1: This value explicity requests the normal JSBSim
282 // 2: This value asks for a message to be printed out when
283 // a class is instantiated
284 // 4: When this value is set, a message is displayed when a
285 // FGModel object executes its Run() method
286 // 8: When this value is set, various runtime state variables
287 // are printed out periodically
288 // 16: When set various parameters are sanity checked and
289 // a message is printed out when they go out of bounds
291 void FGColumnVector4::Debug(int from)
293 if (debug_lvl <= 0) return;
295 if (debug_lvl & 1) { // Standard console startup message output
297 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
298 if (from == 0) cout << "Instantiated: FGColumnVector4" << endl;
299 if (from == 1) cout << "Destroyed: FGColumnVector4" << endl;
301 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
303 if (debug_lvl & 8 ) { // Runtime state variables
305 if (debug_lvl & 16) { // Sanity checking
307 if (debug_lvl & 64) { // Sanity checking
308 if (from == 0) { // Constructor
309 cout << IdSrc << endl;
310 cout << IdHdr << endl;