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