]> git.mxchange.org Git - flightgear.git/blob - src/Time/mymath.cxx
Allow aircraft model file to be specified from the command line.
[flightgear.git] / src / Time / mymath.cxx
1 /* -*- Mode: C++ -*- *****************************************************
2  * mymath.cc
3  * Written by Durk Talsma, around 1995/1996.
4  * 
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License as
7  * published by the Free Software Foundation; either version 2 of the
8  * License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13  * General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
18  *
19  **************************************************************************/
20
21 /********************************************************************
22  * This file defines a simple Vector and Matrix library. These were 
23  * originally written for my (yet) unpublished planetarium / solar 
24  * system simulator program. The are included here, because I have 
25  * experience the code to calculate angles between vectors. I'm sure 
26  * similar functions exist already somewhere else so I don't mind 
27  * whether this gets eventually replaced by something more suitable
28  * The functions are based on a description in Tyler. A. (1994). C++
29  * Real-time 3D graphics. Sigma press, Wilmslow, England.
30  * 
31  * The original versions were written under windows, hence the occasional 
32  *::MessageBox() statements between conditional compile statements
33  *
34  ********************************************************************/
35
36
37
38 #include "mymath.h"
39
40 const double PiOver180 = 1.74532925199433E-002;
41 const double Pix4dif3  = 4.1887902;
42
43 Vector::Vector(int size)
44 {
45         build(size);
46     for (int i = 0; i < nrData; i++)
47         data[i] = 0.00;
48 }
49
50 double Vector::VecLen()
51 {
52
53    double length = 0;
54    for (int i= 0; i < nrData; i++)
55                 length += pow(data[i],2);
56    return sqrt(length);
57 }
58
59
60 double VecDot(Vector first, Vector second)
61 {
62         /*double
63         result = ((first.xVal*second.xVal) +
64                                   (first.yVal*second.yVal) +
65                           (first.zVal*second.zVal));
66     return result; */
67
68     double result = 0;
69     for (int i = 0; i < first.nrData; i++)
70         result += first.data[i] * second.data[i];
71
72     return result;
73 }
74
75 Vector VecCross(Vector first, Vector second)
76 {
77
78         /*return Vec3 ( (first.yVal*second.zVal - first.zVal*second.yVal),
79                                   (first.zVal*second.xVal - first.zVal*second.xVal),
80                       (first.xVal*second.yVal - first.yVal*second.xVal) );
81                 */
82         #ifdef DEBUG
83         if ( (first.nrData != 4) || (first.nrData != second.nrData) )
84         {
85                 ::MessageBox(0, "Attempting to calculate Cross product with 2\n"
86                          "unequally sized vectors in\n"
87                          "Vector VecCross(Vector, Vector", "Error",
88                          MB_OK);
89             exit(1);
90         }
91         #endif
92         
93         double x = first.data[1] * second.data[2] - first.data[2]*second.data[1];
94         double y = first.data[2] * second.data[0] - first.data[0]*second.data[2];
95         double z = first.data[0] * second.data[1] - first.data[1]*second.data[0];
96         Vector result(x,y,z);
97         return result;
98 }
99
100
101
102
103 Vector operator- (Vector first, Vector second)
104 {
105         /*return Vec3( first.xVal - second.xVal,
106                          first.yVal - second.yVal,
107                  first.zVal - second.zVal );
108     */
109             #ifdef DEBUG
110         if ( first.nrData != second.nrData )
111         {
112                 ::MessageBox(0, "Attempting to subtract 2 \n"
113                          "unequally sized vectors in\n"
114                          "Vector operator-(Vector, Vector", "Error",
115                          MB_OK);
116             //exit(1);
117             return Vector(0);
118         }
119         #endif
120         double *temp = new double[first.nrData];
121         for (int i = 0; i < first.nrData; i++)
122                 temp[i] = first.data[i] - second.data[i];
123         Vector result(first.nrData, temp);
124         delete [] temp;
125         return result;
126 }
127
128
129 Vector operator+ (Vector first, Vector second)
130 {
131         /*return Vec3( first.xVal + second.xVal,
132                          first.yVal + second.yVal,
133                  first.zVal + second.zVal );
134     */
135     #ifdef DEBUG
136     if ( first.nrData != second.nrData )
137         {
138         ::MessageBox(0, "Attempting to add 2\n"
139                          "unequally sized vectors in\n"
140                          "Vector operator+(Vector, Vector", "Error",
141                          MB_OK);
142         exit(1);
143     }
144     #endif
145     double *temp = new double[first.nrData];
146     for (int i = 0; i < first.nrData; i++)
147         temp[i] = first.data[i] + second.data[i];
148     Vector result(first.nrData, temp);
149     delete [] temp;
150     return result;
151 }
152
153 Vector Vector::operator +=(Vector other)
154 {
155         #ifdef DEBUG
156     if ( first.nrData != second.nrData )
157     {
158         ::MessageBox(0, "Attempting to add 2\n"
159                          "unequally sized vectors in\n"
160                          "Vector operator+(Vector, Vector", "Error",
161                          MB_OK);
162         exit(1);
163     }
164     #endif
165     for (int i = 0; i < nrData; i++)
166         data[i] += other.data[i];
167     return *this;
168 }
169
170 Vector Vector::operator -=(Vector other)
171 {
172         #ifdef DEBUG
173     if ( first.nrData != second.nrData )
174     {
175         ::MessageBox(0, "Attempting to add 2\n"
176                          "unequally sized vectors in\n"
177                          "Vector operator+(Vector, Vector", "Error",
178                          MB_OK);
179         exit(1);
180     }
181     #endif
182     for (int i = 0; i < nrData; i++)
183         data[i] -= other.data[i];
184     return *this;
185 }
186
187
188
189
190
191 Vector operator* (Matrix mx, Vector vc)
192 {
193         int sizes[3];
194     sizes[0] = vc.nrData;
195     sizes[1] = mx.rows;
196     sizes[2] = mx.columns;
197
198     #ifdef DEBUG
199     if ( (sizes[0] != sizes[1]) || (sizes[0] != sizes[2]) )
200     {
201         char buffer[50];
202         sprintf(buffer, "Sizes don't match in function\n"
203                                         "Vector operator*(Matrix, Vector)\n"
204                         "sizes are: %d, %d, %d", sizes[0], sizes[1],sizes[2]);
205         MessageBox(0, buffer, "Error", MB_OK | MB_ICONEXCLAMATION);
206         exit(1);
207     }
208     #endif
209     double* result = new double[sizes[0]];
210     int col, row;
211
212     for (col = 0; col < sizes[0]; col++)
213     {
214         result[col] = 0;
215         for (row = 0; row < sizes[0]; row++)
216                 result[col] += vc[row] * mx[row][col];
217     }
218     Vector res(4, result);
219
220    /*
221     #ifdef DEBUG
222     char buffer[200];
223     sprintf(buffer, "return value of vector * matrix multiplication is\n"
224                                 "(%f, %f, %f, %f) ", result[0], result[1], result[2], result[3]);
225     ::MessageBox(0, buffer, "Information", MB_OK);
226     #endif
227    */
228         delete [] result;
229     return res;
230 }
231
232 Vector operator*(Vector v1, Vector v2)
233 {
234         int size1 = v1.nrData;
235
236     #ifdef DEBUG
237
238     int size2 = v2.nrData;
239     if(size1 != size2)
240     {
241         ::MessageBox(0, "Vector sizes don't match in Vector operator*(Vector, Vector)",
242                                 "Error", MB_OK);
243         exit(1);
244     }
245     #endif
246     double *tempRes = new double[size1];
247     for (int i= 0; i < size1; i++)
248         tempRes[i] = v1[i] * v2[i];
249     Vector result(size1, tempRes);
250     delete tempRes;
251     return result;
252 }
253
254
255 Vector operator*(Vector vec, double d)
256 {
257         double* tempRes = new double[vec.nrData];
258     for (int i = 0; i < vec.nrData; i++)
259         tempRes[i] = vec[i] * d;
260     Vector result(vec.nrData, tempRes);
261     delete tempRes;
262     return result;
263 }
264
265 Vector operator/(Vector vec, double d)
266 {
267         double* tempRes = new double[vec.nrData];
268     for (int i = 0; i < vec.nrData; i++)
269         tempRes[i] = vec[i] / d;
270     Vector result(vec.nrData, tempRes);
271     delete tempRes;
272     return result;
273 }
274
275 ostream& operator << (ostream& os, Vector& vec)
276 {
277   os << /*setw(4) << */vec.nrData << '\t';
278     for (int i = 0; i < vec.nrData; i++)
279       os /*<< setw(25) */<< vec[i] << '\t';
280     return os;
281 }
282
283 istream& operator >> (istream& is, Vector& vec)
284 {
285         is >> vec.nrData;
286     if (vec.data)
287         delete [] vec.data;
288     vec.data = new double[vec.nrData];
289     for (int i = 0; i < vec.nrData; i++)
290         is >> vec.data[i];
291     return is;
292 }
293
294 ostream& Vector::binSave(ostream& os)
295 {
296         os.write((char*) &nrData, sizeof(int));
297     os.write((char*) data, nrData* sizeof(double));
298         return os;
299 }
300
301
302
303
304
305
306 /******************************************************************************
307                 Matrix manipulation routines
308 ******************************************************************************/
309
310 Matrix::Matrix(int r, int c, double*dta)
311 {
312         build(r,c);
313     for (int i = 0; i < rows; i++)
314         for (int j = 0; j < columns; j++)
315                 data[i][j] = (*dta++);
316 }
317
318 Matrix::Matrix(int r, int c, double** dta)
319 {
320         build(r,c);
321     for (int i = 0; i < rows; i++)
322         for (int j = 0; j < columns; j++)
323                         data[i][j] = dta[i][j];
324 }
325
326 Matrix::Matrix(int r, int c, Vector* dta)
327 {
328         build(r,c);
329     for (int i = 0; i < rows; i++)
330         data[i] = dta[i];
331 }
332
333 Matrix::Matrix(Matrix& other)
334 {
335         build (other.rows, other.columns);
336     for (int i = 0; i< rows; i++)
337         (*this)[i] = other[i];
338 }
339
340 void Matrix::build(int row, int col)
341 {
342     rows = row;
343     columns = col;
344
345         data = new Vector [rows];
346     for (int i = 0; i < rows; i++)
347         data[i].build(col);
348 }
349
350 Matrix& Matrix::operator =(Matrix& other)
351 {
352         rows = other.rows;
353     columns = other.columns;
354     for (int i = 0; i < rows; i++)
355         (*this)[i] = other[i];
356     return *this;
357 }
358
359
360 Vector Matrix::operator ()(int col)
361 {
362         Vector Col(rows);
363     for (int i = 0; i < rows; i++)
364         Col[i] = data[i][col];
365     return Col;
366 }
367
368 void Matrix::norm(int scale)
369 {
370         for (int i = 0; i < rows; i++)
371         for (int j = 0; j < columns; j++)
372                 data[i][j] /= scale;
373 }