Branch data Line data Source code
1 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 : :
3 : : Header: FGMatrix33.h
4 : : Author: Tony Peden, Jon Berndt, Mathias Frolich
5 : : Date started: Unknown
6 : :
7 : : ------------- Copyright (C) 2001 Jon S. Berndt (jon@jsbsim.org) -------------
8 : :
9 : : This program is free software; you can redistribute it and/or modify it under
10 : : the terms of the GNU Lesser General Public License as published by the Free Software
11 : : Foundation; either version 2 of the License, or (at your option) any later
12 : : version.
13 : :
14 : : This program is distributed in the hope that it will be useful, but WITHOUT
15 : : ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 : : FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
17 : : details.
18 : :
19 : : You should have received a copy of the GNU Lesser General Public License along with
20 : : this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21 : : Place - Suite 330, Boston, MA 02111-1307, USA.
22 : :
23 : : Further information about the GNU Lesser General Public License can also be found on
24 : : the world wide web at http://www.gnu.org.
25 : :
26 : : HISTORY
27 : : --------------------------------------------------------------------------------
28 : : ??/??/?? TP Created
29 : : 03/16/2000 JSB Added exception throwing
30 : : 03/06/2004 MF Rework of the code to make it a bit compiler friendlier
31 : :
32 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
33 : : SENTRY
34 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
35 : :
36 : : #ifndef FGMATRIX33_H
37 : : #define FGMATRIX33_H
38 : :
39 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
40 : : INCLUDES
41 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
42 : :
43 : : #include <string>
44 : : #include <iosfwd>
45 : :
46 : : #include "FGColumnVector3.h"
47 : : #include "FGJSBBase.h"
48 : :
49 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
50 : : DEFINITIONS
51 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
52 : :
53 : : #define ID_MATRIX33 "$Id: FGMatrix33.h,v 1.12 2010/08/21 17:13:47 jberndt Exp $"
54 : :
55 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
56 : : FORWARD DECLARATIONS
57 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
58 : :
59 : : namespace JSBSim {
60 : :
61 : : class FGColumnVector3;
62 : : class FGQuaternion;
63 : :
64 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
65 : : CLASS DOCUMENTATION
66 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
67 : :
68 : : /** Exception convenience class.
69 : : */
70 : :
71 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
72 : : DECLARATION: MatrixException
73 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
74 : :
75 : : class MatrixException : public FGJSBBase
76 : 0 : {
77 : : public:
78 : : std::string Message;
79 : : };
80 : :
81 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
82 : : CLASS DOCUMENTATION
83 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
84 : :
85 : : /** Handles matrix math operations.
86 : : @author Tony Peden, Jon Berndt, Mathias Froelich
87 : : */
88 : :
89 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
90 : : DECLARATION: FGMatrix33
91 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
92 : :
93 : : class FGMatrix33 : public FGJSBBase
94 : : {
95 : : public:
96 : :
97 : : enum {
98 : : eRows = 3,
99 : : eColumns = 3
100 : : };
101 : :
102 : : /** Default initializer.
103 : :
104 : : Create a zero matrix.
105 : : */
106 : : FGMatrix33(void);
107 : :
108 : : /** Copy constructor.
109 : :
110 : : @param M Matrix which is used for initialization.
111 : :
112 : : Create copy of the matrix given in the argument.
113 : : */
114 : 0 : FGMatrix33(const FGMatrix33& M)
115 : 1674175 : {
116 : 1674175 : data[0] = M.data[0];
117 : 1674175 : data[1] = M.data[1];
118 : 1674175 : data[2] = M.data[2];
119 : 1674175 : data[3] = M.data[3];
120 : 1674175 : data[4] = M.data[4];
121 : 1674175 : data[5] = M.data[5];
122 : 1674175 : data[6] = M.data[6];
123 : 1674175 : data[7] = M.data[7];
124 : 1674175 : data[8] = M.data[8];
125 : :
126 : : // Debug(0);
127 : 0 : }
128 : :
129 : : /** Initialization by given values.
130 : :
131 : : @param m11 value of the 1,1 Matrix element.
132 : : @param m12 value of the 1,2 Matrix element.
133 : : @param m13 value of the 1,3 Matrix element.
134 : : @param m21 value of the 2,1 Matrix element.
135 : : @param m22 value of the 2,2 Matrix element.
136 : : @param m23 value of the 2,3 Matrix element.
137 : : @param m31 value of the 3,1 Matrix element.
138 : : @param m32 value of the 3,2 Matrix element.
139 : : @param m33 value of the 3,3 Matrix element.
140 : :
141 : : Create a matrix from the doubles given in the arguments.
142 : : */
143 : : FGMatrix33(double m11, double m12, double m13,
144 : : double m21, double m22, double m23,
145 : 2808331 : double m31, double m32, double m33) {
146 : 2808331 : data[0] = m11;
147 : 2808331 : data[1] = m21;
148 : 2808331 : data[2] = m31;
149 : 2808331 : data[3] = m12;
150 : 2808331 : data[4] = m22;
151 : 2808331 : data[5] = m32;
152 : 2808331 : data[6] = m13;
153 : 2808331 : data[7] = m23;
154 : 2808331 : data[8] = m33;
155 : :
156 : : // Debug(0);
157 : : }
158 : :
159 : : /** Destructor.
160 : : */
161 : 0 : ~FGMatrix33(void) { /* Debug(1); */ }
162 : :
163 : : /** Prints the contents of the matrix.
164 : : @param delimeter the item separator (tab or comma)
165 : : @return a string with the delimeter-separated contents of the matrix */
166 : : std::string Dump(const std::string& delimeter) const;
167 : :
168 : : /** Prints the contents of the matrix.
169 : : @param delimeter the item separator (tab or comma, etc.)
170 : : @param prefix an additional prefix that is used to indent the 3X3 matrix printout
171 : : @return a string with the delimeter-separated contents of the matrix */
172 : : std::string Dump(const std::string& delimiter, const std::string& prefix) const;
173 : :
174 : : /** Read access the entries of the matrix.
175 : : @param row Row index.
176 : : @param col Column index.
177 : :
178 : : @return the value of the matrix entry at the given row and
179 : : column indices. Indices are counted starting with 1.
180 : : */
181 : : double operator()(unsigned int row, unsigned int col) const {
182 : 6 : return data[(col-1)*eRows+row-1];
183 : : }
184 : :
185 : : /** Write access the entries of the matrix.
186 : : Note that the indices given in the arguments are unchecked.
187 : :
188 : : @param row Row index.
189 : : @param col Column index.
190 : :
191 : : @return a reference to the matrix entry at the given row and
192 : : column indices. Indices are counted starting with 1.
193 : : */
194 : : double& operator()(unsigned int row, unsigned int col) {
195 : 11176583 : return data[(col-1)*eRows+row-1];
196 : : }
197 : :
198 : : /** Read access the entries of the matrix.
199 : : This function is just a shortcut for the <tt>double&
200 : : operator()(unsigned int row, unsigned int col)</tt> function. It is
201 : : used internally to access the elements in a more convenient way.
202 : :
203 : : Note that the indices given in the arguments are unchecked.
204 : :
205 : : @param row Row index.
206 : : @param col Column index.
207 : :
208 : : @return the value of the matrix entry at the given row and
209 : : column indices. Indices are counted starting with 1.
210 : : */
211 : : double Entry(unsigned int row, unsigned int col) const {
212 : : return data[(col-1)*eRows+row-1];
213 : : }
214 : :
215 : : /** Write access the entries of the matrix.
216 : : This function is just a shortcut for the <tt>double&
217 : : operator()(unsigned int row, unsigned int col)</tt> function. It is
218 : : used internally to access the elements in a more convenient way.
219 : :
220 : : Note that the indices given in the arguments are unchecked.
221 : :
222 : : @param row Row index.
223 : : @param col Column index.
224 : :
225 : : @return a reference to the matrix entry at the given row and
226 : : column indices. Indices are counted starting with 1.
227 : : */
228 : : double& Entry(unsigned int row, unsigned int col) {
229 : : return data[(col-1)*eRows+row-1];
230 : : }
231 : :
232 : : /** Number of rows in the matrix.
233 : : @return the number of rows in the matrix.
234 : : */
235 : 0 : unsigned int Rows(void) const { return eRows; }
236 : :
237 : : /** Number of cloumns in the matrix.
238 : : @return the number of columns in the matrix.
239 : : */
240 : 0 : unsigned int Cols(void) const { return eColumns; }
241 : :
242 : : /** Transposed matrix.
243 : : This function only returns the transpose of this matrix. This matrix itself
244 : : remains unchanged.
245 : : @return the transposed matrix.
246 : : */
247 : 324036 : FGMatrix33 Transposed(void) const {
248 : : return FGMatrix33( data[0], data[1], data[2],
249 : : data[3], data[4], data[5],
250 : 972114 : data[6], data[7], data[8] );
251 : : }
252 : :
253 : : /** Transposes this matrix.
254 : : This function only transposes this matrix. Nothing is returned.
255 : : */
256 : : void T(void);
257 : :
258 : : /** Initialize the matrix.
259 : : This function initializes a matrix to all 0.0.
260 : : */
261 : : void InitMatrix(void);
262 : :
263 : : /** Initialize the matrix.
264 : : This function initializes a matrix to user specified values.
265 : : */
266 : : void InitMatrix(double m11, double m12, double m13,
267 : : double m21, double m22, double m23,
268 : : double m31, double m32, double m33) {
269 : 54006 : data[0] = m11;
270 : 54006 : data[1] = m21;
271 : 54006 : data[2] = m31;
272 : 54006 : data[3] = m12;
273 : 54006 : data[4] = m22;
274 : 54006 : data[5] = m32;
275 : 54006 : data[6] = m13;
276 : 54006 : data[7] = m23;
277 : 54006 : data[8] = m33;
278 : : }
279 : :
280 : : /** Returns the quaternion associated with this direction cosine (rotation) matrix.
281 : : */
282 : : FGQuaternion GetQuaternion(void);
283 : :
284 : : /** Determinant of the matrix.
285 : : @return the determinant of the matrix.
286 : : */
287 : : double Determinant(void) const;
288 : :
289 : : /** Return if the matrix is invertible.
290 : : Checks and returns if the matrix is nonsingular and thus
291 : : invertible. This is done by simply computing the determinant and
292 : : check if it is zero. Note that this test does not cover any
293 : : instabilities caused by nearly singular matirces using finite
294 : : arithmetics. It only checks exact singularity.
295 : : */
296 : : bool Invertible(void) const { return 0.0 != Determinant(); }
297 : :
298 : : /** Return the inverse of the matrix.
299 : : Computes and returns if the inverse of the matrix. It is computed
300 : : by Cramers Rule. Also there are no checks performed if the matrix
301 : : is invertible. If you are not sure that it really is check this
302 : : with the @ref Invertible() call before.
303 : : */
304 : : FGMatrix33 Inverse(void) const;
305 : :
306 : : /** Assignment operator.
307 : :
308 : : @param A source matrix.
309 : :
310 : : Copy the content of the matrix given in the argument into *this.
311 : : */
312 : 2106250 : FGMatrix33& operator=(const FGMatrix33& A) {
313 : 2376280 : data[0] = A.data[0];
314 : 2376280 : data[1] = A.data[1];
315 : 2376280 : data[2] = A.data[2];
316 : 2376280 : data[3] = A.data[3];
317 : 2376280 : data[4] = A.data[4];
318 : 2376280 : data[5] = A.data[5];
319 : 2376280 : data[6] = A.data[6];
320 : 2376280 : data[7] = A.data[7];
321 : 2376280 : data[8] = A.data[8];
322 : 2376280 : return *this;
323 : : }
324 : :
325 : : /** Matrix vector multiplication.
326 : :
327 : : @param v vector to multiply with.
328 : : @return matric vector product.
329 : :
330 : : Compute and return the product of the current matrix with the
331 : : vector given in the argument.
332 : : */
333 : : FGColumnVector3 operator*(const FGColumnVector3& v) const;
334 : :
335 : : /** Matrix subtraction.
336 : :
337 : : @param B matrix to add to.
338 : : @return difference of the matrices.
339 : :
340 : : Compute and return the sum of the current matrix and the matrix
341 : : B given in the argument.
342 : : */
343 : : FGMatrix33 operator-(const FGMatrix33& B) const;
344 : :
345 : : /** Matrix addition.
346 : :
347 : : @param B matrix to add to.
348 : : @return sum of the matrices.
349 : :
350 : : Compute and return the sum of the current matrix and the matrix
351 : : B given in the argument.
352 : : */
353 : : FGMatrix33 operator+(const FGMatrix33& B) const;
354 : :
355 : : /** Matrix product.
356 : :
357 : : @param B matrix to add to.
358 : : @return product of the matrices.
359 : :
360 : : Compute and return the product of the current matrix and the matrix
361 : : B given in the argument.
362 : : */
363 : : FGMatrix33 operator*(const FGMatrix33& B) const;
364 : :
365 : : /** Multiply the matrix with a scalar.
366 : :
367 : : @param scalar scalar factor to multiply with.
368 : : @return scaled matrix.
369 : :
370 : : Compute and return the product of the current matrix with the
371 : : scalar value scalar given in the argument.
372 : : */
373 : : FGMatrix33 operator*(const double scalar) const;
374 : :
375 : : /** Multiply the matrix with 1.0/scalar.
376 : :
377 : : @param scalar scalar factor to divide through.
378 : : @return scaled matrix.
379 : :
380 : : Compute and return the product of the current matrix with the
381 : : scalar value 1.0/scalar, where scalar is given in the argument.
382 : : */
383 : : FGMatrix33 operator/(const double scalar) const;
384 : :
385 : : /** In place matrix subtraction.
386 : :
387 : : @param B matrix to subtract.
388 : : @return reference to the current matrix.
389 : :
390 : : Compute the diffence from the current matrix and the matrix B
391 : : given in the argument.
392 : : */
393 : : FGMatrix33& operator-=(const FGMatrix33 &B);
394 : :
395 : : /** In place matrix addition.
396 : :
397 : : @param B matrix to add.
398 : : @return reference to the current matrix.
399 : :
400 : : Compute the sum of the current matrix and the matrix B
401 : : given in the argument.
402 : : */
403 : : FGMatrix33& operator+=(const FGMatrix33 &B);
404 : :
405 : : /** In place matrix multiplication.
406 : :
407 : : @param B matrix to multiply with.
408 : : @return reference to the current matrix.
409 : :
410 : : Compute the product of the current matrix and the matrix B
411 : : given in the argument.
412 : : */
413 : : FGMatrix33& operator*=(const FGMatrix33 &B);
414 : :
415 : : /** In place matrix scale.
416 : :
417 : : @param scalar scalar value to multiply with.
418 : : @return reference to the current matrix.
419 : :
420 : : Compute the product of the current matrix and the scalar value scalar
421 : : given in the argument.
422 : : */
423 : : FGMatrix33& operator*=(const double scalar);
424 : :
425 : : /** In place matrix scale.
426 : :
427 : : @param scalar scalar value to divide through.
428 : : @return reference to the current matrix.
429 : :
430 : : Compute the product of the current matrix and the scalar value
431 : : 1.0/scalar, where scalar is given in the argument.
432 : : */
433 : : FGMatrix33& operator/=(const double scalar);
434 : :
435 : : private:
436 : : double data[eRows*eColumns];
437 : :
438 : : void Debug(int from);
439 : : };
440 : :
441 : : /** Scalar multiplication.
442 : :
443 : : @param scalar scalar value to multiply with.
444 : : @param A Matrix to multiply.
445 : :
446 : : Multiply the Matrix with a scalar value.
447 : : */
448 : : inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
449 : : // use already defined operation.
450 : : return A*scalar;
451 : : }
452 : :
453 : : /** Write matrix to a stream.
454 : :
455 : : @param os Stream to write to.
456 : : @param M Matrix to write.
457 : :
458 : : Write the matrix to a stream.
459 : : */
460 : : std::ostream& operator<<(std::ostream& os, const FGMatrix33& M);
461 : :
462 : : /** Read matrix from a stream.
463 : :
464 : : @param os Stream to read from.
465 : : @param M Matrix to initialize with the values from the stream.
466 : :
467 : : Read matrix from a stream.
468 : : */
469 : : std::istream& operator>>(std::istream& is, FGMatrix33& M);
470 : :
471 : : } // namespace JSBSim
472 : :
473 : : #include "FGQuaternion.h"
474 : :
475 : : #endif
|
