1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Tony Peden, Jon Berndt, Mathias Frolich
7 ------------- Copyright (C) 2001 Jon S. Berndt (jsb@hal-pc.org) -------------
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
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
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.
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.
27 --------------------------------------------------------------------------------
29 03/16/2000 JSB Added exception throwing
30 03/06/2004 MF Rework of the code to make it a bit compiler friendlier
32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
34 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
39 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
46 # include <simgear/compiler.h>
49 # include STL_IOSTREAM
51 SG_USING_STD(ostream);
52 SG_USING_STD(istream);
58 # if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
65 # if defined(sgi) && !defined(__GNUC__)
79 #include "FGColumnVector3.h"
80 #include "FGJSBBase.h"
82 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
84 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
86 #define ID_MATRIX33 "$Id$"
88 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
90 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
94 class FGColumnVector3;
96 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
98 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
100 /** Exception convenience class.
103 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
104 DECLARATION: MatrixException
105 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
107 class MatrixException : public FGJSBBase
113 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
115 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
117 /** Handles matrix math operations.
118 @author Tony Peden, Jon Berndt, Mathias Froelich
121 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
122 DECLARATION: FGMatrix33
123 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
125 class FGMatrix33 : public FGJSBBase
134 /** Default initializer.
136 Create a zero matrix.
140 /** Copy constructor.
142 @param M Matrix which is used for initialization.
144 Create copy of the matrix given in the argument.
146 FGMatrix33(const FGMatrix33& M) {
147 Entry(1,1) = M.Entry(1,1);
148 Entry(2,1) = M.Entry(2,1);
149 Entry(3,1) = M.Entry(3,1);
150 Entry(1,2) = M.Entry(1,2);
151 Entry(2,2) = M.Entry(2,2);
152 Entry(3,2) = M.Entry(3,2);
153 Entry(1,3) = M.Entry(1,3);
154 Entry(2,3) = M.Entry(2,3);
155 Entry(3,3) = M.Entry(3,3);
160 /** Initialization by given values.
162 @param m11 value of the 1,1 Matrix element.
163 @param m12 value of the 1,2 Matrix element.
164 @param m13 value of the 1,3 Matrix element.
165 @param m21 value of the 2,1 Matrix element.
166 @param m22 value of the 2,2 Matrix element.
167 @param m23 value of the 2,3 Matrix element.
168 @param m31 value of the 3,1 Matrix element.
169 @param m32 value of the 3,2 Matrix element.
170 @param m33 value of the 3,3 Matrix element.
172 Create a matrix from the doubles given in the arguments.
174 FGMatrix33(double m11, double m12, double m13,
175 double m21, double m22, double m23,
176 double m31, double m32, double m33) {
192 ~FGMatrix33(void) { Debug(1); }
194 /** Read access the entries of the matrix.
195 @param row Row index.
196 @param col Column index.
198 @return the value of the matrix entry at the given row and
199 column indices. Indices are counted starting with 1.
201 double operator()(unsigned int row, unsigned int col) const {
202 return Entry(row, col);
205 /** Write access the entries of the matrix.
206 Note that the indices given in the arguments are unchecked.
208 @param row Row index.
209 @param col Column index.
211 @return a reference to the matrix entry at the given row and
212 column indices. Indices are counted starting with 1.
214 double& operator()(unsigned int row, unsigned int col) {
215 return Entry(row, col);
218 /** Read access the entries of the matrix.
219 This function is just a shortcut for the @ref double&
220 operator()(unsigned int row, unsigned int col) function. It is
221 used internally to access the elements in a more convenient way.
223 Note that the indices given in the arguments are unchecked.
225 @param row Row index.
226 @param col Column index.
228 @return the value of the matrix entry at the given row and
229 column indices. Indices are counted starting with 1.
231 double Entry(unsigned int row, unsigned int col) const {
232 return data[(col-1)*eRows+row-1];
235 /** Write access the entries of the matrix.
236 This function is just a shortcut for the @ref double&
237 operator()(unsigned int row, unsigned int col) function. It is
238 used internally to access the elements in a more convenient way.
240 Note that the indices given in the arguments are unchecked.
242 @param row Row index.
243 @param col Column index.
245 @return a reference to the matrix entry at the given row and
246 column indices. Indices are counted starting with 1.
248 double& Entry(unsigned int row, unsigned int col) {
249 return data[(col-1)*eRows+row-1];
252 /** Number of rows in the matrix.
253 @return the number of rows in the matrix.
255 unsigned int Rows(void) const { return eRows; }
257 /** Number of cloumns in the matrix.
258 @return the number of columns in the matrix.
260 unsigned int Cols(void) const { return eColumns; }
262 /** Transposed matrix.
263 This function only returns the transpose of this matrix. This matrix itself
265 @return the transposed matrix.
267 FGMatrix33 Transposed(void) const {
268 return FGMatrix33( Entry(1,1), Entry(2,1), Entry(3,1),
269 Entry(1,2), Entry(2,2), Entry(3,2),
270 Entry(1,3), Entry(2,3), Entry(3,3) );
273 /** Transposes this matrix.
274 This function only transposes this matrix. Nothing is returned.
278 /** Initialize the matrix.
279 This function initializes a matrix to all 0.0.
281 void InitMatrix(void);
283 /** Initialize the matrix.
284 This function initializes a matrix to user specified values.
286 void InitMatrix(double m11, double m12, double m13,
287 double m21, double m22, double m23,
288 double m31, double m32, double m33) {
300 /** Determinant of the matrix.
301 @return the determinant of the matrix.
303 double Determinant(void) const;
305 /** Return if the matrix is invertible.
306 Checks and returns if the matrix is nonsingular and thus
307 invertible. This is done by simply computing the determinant and
308 check if it is zero. Note that this test does not cover any
309 instabilities caused by nearly singular matirces using finite
310 arithmetics. It only checks exact singularity.
312 bool Invertible(void) const { return 0.0 != Determinant(); }
314 /** Return the inverse of the matrix.
315 Computes and returns if the inverse of the matrix. It is computed
316 by Cramers Rule. Also there are no checks performed if the matrix
317 is invertible. If you are not sure that it really is check this
318 with the @ref Invertible() call before.
320 FGMatrix33 Inverse(void) const;
322 /** Assignment operator.
324 @param A source matrix.
326 Copy the content of the matrix given in the argument into *this.
328 FGMatrix33& operator=(const FGMatrix33& A) {
341 /** Matrix vector multiplication.
343 @param v vector to multiply with.
344 @return matric vector product.
346 Compute and return the product of the current matrix with the
347 vector given in the argument.
349 FGColumnVector3 operator*(const FGColumnVector3& v) const;
351 /** Matrix subtraction.
353 @param B matrix to add to.
354 @return difference of the matrices.
356 Compute and return the sum of the current matrix and the matrix
357 B given in the argument.
359 FGMatrix33 operator-(const FGMatrix33& B) const;
363 @param B matrix to add to.
364 @return sum of the matrices.
366 Compute and return the sum of the current matrix and the matrix
367 B given in the argument.
369 FGMatrix33 operator+(const FGMatrix33& B) const;
373 @param B matrix to add to.
374 @return product of the matrices.
376 Compute and return the product of the current matrix and the matrix
377 B given in the argument.
379 FGMatrix33 operator*(const FGMatrix33& B) const;
381 /** Multiply the matrix with a scalar.
383 @param scalar scalar factor to multiply with.
384 @return scaled matrix.
386 Compute and return the product of the current matrix with the
387 scalar value scalar given in the argument.
389 FGMatrix33 operator*(const double scalar) const;
391 /** Multiply the matrix with 1.0/scalar.
393 @param scalar scalar factor to divide through.
394 @return scaled matrix.
396 Compute and return the product of the current matrix with the
397 scalar value 1.0/scalar, where scalar is given in the argument.
399 FGMatrix33 operator/(const double scalar) const;
401 /** In place matrix subtraction.
403 @param B matrix to subtract.
404 @return reference to the current matrix.
406 Compute the diffence from the current matrix and the matrix B
407 given in the argument.
409 FGMatrix33& operator-=(const FGMatrix33 &B);
411 /** In place matrix addition.
413 @param B matrix to add.
414 @return reference to the current matrix.
416 Compute the sum of the current matrix and the matrix B
417 given in the argument.
419 FGMatrix33& operator+=(const FGMatrix33 &B);
421 /** In place matrix multiplication.
423 @param B matrix to multiply with.
424 @return reference to the current matrix.
426 Compute the product of the current matrix and the matrix B
427 given in the argument.
429 FGMatrix33& operator*=(const FGMatrix33 &B);
431 /** In place matrix scale.
433 @param scalar scalar value to multiply with.
434 @return reference to the current matrix.
436 Compute the product of the current matrix and the scalar value scalar
437 given in the argument.
439 FGMatrix33& operator*=(const double scalar);
441 /** In place matrix scale.
443 @param scalar scalar value to divide through.
444 @return reference to the current matrix.
446 Compute the product of the current matrix and the scalar value
447 1.0/scalar, where scalar is given in the argument.
449 FGMatrix33& operator/=(const double scalar);
452 double data[eRows*eColumns];
454 void Debug(int from);
457 /** Scalar multiplication.
459 @param scalar scalar value to multiply with.
460 @param A Matrix to multiply.
462 Multiply the Matrix with a scalar value.
464 inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
465 // use already defined operation.
469 /** Write matrix to a stream.
471 @param os Stream to write to.
472 @param M Matrix to write.
474 Write the matrix to a stream.
476 ostream& operator<<(ostream& os, const FGMatrix33& M);
478 /** Read matrix from a stream.
480 @param os Stream to read from.
481 @param M Matrix to initialize with the values from the stream.
483 Read matrix from a stream.
485 istream& operator>>(istream& is, FGMatrix33& M);
487 } // namespace JSBSim