]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/FGColumnVector3.cpp
#include <config.h> where needed for cygwin/gcc-3.2.
[flightgear.git] / src / FDM / JSBSim / FGColumnVector3.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2
3 Module: FGColumnVector3.cpp
4 Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
5 Date started: 1998
6 Purpose: FGColumnVector3 class
7 Called by: Various
8
9 FUNCTIONAL DESCRIPTION
10 --------------------------------------------------------------------------------
11
12 HISTORY
13 --------------------------------------------------------------------------------
14 ??/??/??   TP   Created
15 03/16/2000 JSB  Added exception throwing
16
17 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
18 INCLUDES
19 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
20
21 #include "FGColumnVector3.h"
22
23 static const char *IdSrc = "$Id$";
24 static const char *IdHdr = ID_COLUMNVECTOR3;
25
26 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
27 CLASS IMPLEMENTATION
28 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
29
30 FGColumnVector3::FGColumnVector3(void)
31 {
32   rowCtr = 1;
33   data[0]=0; data[1]=0; data[2]=0; data[3]=0;
34
35   Debug(0);
36 }
37
38 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39
40 FGColumnVector3::FGColumnVector3(double X, double Y, double Z)
41 {
42   rowCtr = 1;
43   data[0] = 0; data[eX] = X; data[eY] = Y; data[eZ] = Z;
44
45   Debug(0);
46 }
47
48 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
49
50 FGColumnVector3::~FGColumnVector3(void)
51 {
52   Debug(1);
53 }
54
55
56 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
57
58 FGColumnVector3::FGColumnVector3(const FGColumnVector3& b) 
59 {
60   data[1] = b.data[1];
61   data[2] = b.data[2];
62   data[3] = b.data[3];
63   rowCtr = 1;
64
65   Debug(0);
66 }
67
68 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
69
70 FGColumnVector3 FGColumnVector3::operator=(const FGColumnVector3& b) 
71 {
72   data[1] = b.data[1];
73   data[2] = b.data[2];
74   data[3] = b.data[3];
75   rowCtr = 1;
76
77   return *this;
78 }
79
80 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
81
82 FGColumnVector3 FGColumnVector3::operator+(const FGColumnVector3& C)
83 {
84   FGColumnVector3 Sum; 
85   Sum(1) = C(1) + data[1];
86   Sum(2) = C(2) + data[2];
87   Sum(3) = C(3) + data[3];
88
89   return Sum;
90 }
91
92 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
93
94 void FGColumnVector3::operator+=(const FGColumnVector3& C)
95 {
96    data[1] += C(1);
97    data[2] += C(2);
98    data[3] += C(3);
99 }
100
101 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
102
103 FGColumnVector3 FGColumnVector3::operator*(const double scalar)
104 {
105   FGColumnVector3 Product;
106
107   Product(1) = scalar * data[1];
108   Product(2) = scalar * data[2];
109   Product(3) = scalar * data[3];
110
111   return Product;
112 }
113
114 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
115
116 void FGColumnVector3::operator*=(const double scalar)
117 {
118   data[1] *= scalar;
119   data[2] *= scalar;
120   data[3] *= scalar;
121 }
122
123 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
124
125 FGColumnVector3 FGColumnVector3::operator-(const FGColumnVector3& V)
126 {
127   
128   FGColumnVector3 Diff; 
129   
130   Diff(1) = data[1] - V(1);
131   Diff(2) = data[2] - V(2);
132   Diff(3) = data[3] - V(3);
133
134   return Diff;
135 }
136
137 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
138
139 void FGColumnVector3::operator-=(const FGColumnVector3& V)
140 {
141   data[1] -= V(1);
142   data[2] -= V(2);
143   data[3] -= V(3);
144 }
145
146 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
147
148 FGColumnVector3 FGColumnVector3::operator/(const double scalar)
149 {
150   FGColumnVector3 Quotient;
151
152   if (scalar != 0) {
153           double tmp = 1.0/scalar;
154     Quotient(1) = data[1] * tmp;
155     Quotient(2) = data[2] * tmp;
156     Quotient(3) = data[3] * tmp;
157   } else {
158     cerr << "Attempt to divide by zero in method FGColumnVector3::operator/(const double scalar), object " << this << endl; 
159   }
160   return Quotient;
161 }
162
163 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
164
165 void FGColumnVector3::operator/=(const double scalar)
166 {
167   FGColumnVector3 Quotient;
168
169   if (scalar != 0) {
170           double tmp = 1.0/scalar;
171     data[1] *= tmp;
172     data[2] *= tmp;
173     data[3] *= tmp;
174   } else {
175     cerr << "Attempt to divide by zero in method FGColumnVector3::operator/=(const double scalar), object " << this << endl; 
176   }
177 }
178
179 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
180
181 FGColumnVector3 operator*(const double scalar, const FGColumnVector3& C)
182 {
183   FGColumnVector3 Product;
184
185   Product(1) = scalar * C(1);
186   Product(2) = scalar * C(2);
187   Product(3) = scalar * C(3);
188   
189   return Product;
190 }
191
192 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
193
194 double FGColumnVector3::Magnitude(void)
195 {
196   double num;
197
198   if ((data[1] == 0.00) &&
199       (data[2] == 0.00) &&
200       (data[3] == 0.00))
201   {
202     return 0.00;
203   } else {
204     num  = data[1]*data[1];
205     num += data[2]*data[2];
206     num += data[3]*data[3];
207     return sqrt(num);
208   }
209 }
210
211 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
212
213 FGColumnVector3 FGColumnVector3::Normalize(void)
214 {
215   double Mag = Magnitude();
216
217   if (Mag != 0) {
218           Mag = 1.0/Mag;
219      data[1] *= Mag;
220      data[2] *= Mag;
221      data[3] *= Mag;
222   }    
223
224   return *this;
225 }
226
227 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
228
229 FGColumnVector3 FGColumnVector3::operator*(const FGColumnVector3& V)
230 {
231   FGColumnVector3 Product;
232   
233   Product(1) = data[2] * V(3) - data[3] * V(2);
234   Product(2) = data[3] * V(1) - data[1] * V(3);
235   Product(3) = data[1] * V(2) - data[2] * V(1);
236
237   return Product;
238 }
239
240 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
241
242 void FGColumnVector3::operator*=(const FGColumnVector3& V)
243 {
244   double a,b,c;
245   a = data[1]; b=data[2]; c=data[3];
246   
247   data[1] = b * V(3) - c * V(2);
248   data[2] = c * V(1) - a * V(3);
249   data[3] = a * V(2) - b * V(1);
250
251 }
252
253 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
254
255 FGColumnVector3 FGColumnVector3::multElementWise(const FGColumnVector3& V)
256 {
257   FGColumnVector3 Product;
258
259   Product(1) = data[1] * V(1);
260   Product(2) = data[2] * V(2);
261   Product(3) = data[3] * V(3);
262
263   return Product;
264 }
265
266 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
267
268 ostream& operator<<(ostream& os, const FGColumnVector3& col)
269 {
270   os << col(1) << " , " << col(2) << " , " << col(3);
271   return os;
272 }  
273
274 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
275
276 FGColumnVector3& FGColumnVector3::operator<<(const double ff)
277 {
278   data[rowCtr] = ff;
279   if (++rowCtr > 3 )
280       rowCtr = 1;
281   return *this;
282 }
283
284 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
285 //    The bitmasked value choices are as follows:
286 //    unset: In this case (the default) JSBSim would only print
287 //       out the normally expected messages, essentially echoing
288 //       the config files as they are read. If the environment
289 //       variable is not set, debug_lvl is set to 1 internally
290 //    0: This requests JSBSim not to output any messages
291 //       whatsoever.
292 //    1: This value explicity requests the normal JSBSim
293 //       startup messages
294 //    2: This value asks for a message to be printed out when
295 //       a class is instantiated
296 //    4: When this value is set, a message is displayed when a
297 //       FGModel object executes its Run() method
298 //    8: When this value is set, various runtime state variables
299 //       are printed out periodically
300 //    16: When set various parameters are sanity checked and
301 //       a message is printed out when they go out of bounds
302
303 void FGColumnVector3::Debug(int from)
304 {
305   if (debug_lvl <= 0) return;
306
307   if (debug_lvl & 1) { // Standard console startup message output
308   }
309   if (debug_lvl & 2 ) { // Instantiation/Destruction notification
310     if (from == 0) cout << "Instantiated: FGColumnVector3" << endl;
311     if (from == 1) cout << "Destroyed:    FGColumnVector3" << endl;
312   }
313   if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
314   }
315   if (debug_lvl & 8 ) { // Runtime state variables
316   }
317   if (debug_lvl & 16) { // Sanity checking
318   }
319   if (debug_lvl & 64) {
320     if (from == 0) { // Constructor
321       cout << IdSrc << endl;
322       cout << IdHdr << endl;
323     }
324   }
325 }
326