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 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
56 #include "FGColumnVector3.h"
57 #include "FGJSBBase.h"
59 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
61 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
63 #define ID_MATRIX33 "$Id$"
65 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
67 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
71 class FGColumnVector3;
73 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
75 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
77 /** Exception convenience class.
80 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
81 DECLARATION: MatrixException
82 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
84 class MatrixException : public FGJSBBase
90 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
92 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
94 /** Handles matrix math operations.
95 @author Tony Peden, Jon Berndt, Mathias Froelich
98 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
99 DECLARATION: FGMatrix33
100 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
102 class FGMatrix33 : public FGJSBBase
111 /** Default initializer.
113 Create a zero matrix.
117 /** Copy constructor.
119 @param M Matrix which is used for initialization.
121 Create copy of the matrix given in the argument.
123 FGMatrix33(const FGMatrix33& M) {
124 Entry(1,1) = M.Entry(1,1);
125 Entry(2,1) = M.Entry(2,1);
126 Entry(3,1) = M.Entry(3,1);
127 Entry(1,2) = M.Entry(1,2);
128 Entry(2,2) = M.Entry(2,2);
129 Entry(3,2) = M.Entry(3,2);
130 Entry(1,3) = M.Entry(1,3);
131 Entry(2,3) = M.Entry(2,3);
132 Entry(3,3) = M.Entry(3,3);
137 /** Initialization by given values.
139 @param m11 value of the 1,1 Matrix element.
140 @param m12 value of the 1,2 Matrix element.
141 @param m13 value of the 1,3 Matrix element.
142 @param m21 value of the 2,1 Matrix element.
143 @param m22 value of the 2,2 Matrix element.
144 @param m23 value of the 2,3 Matrix element.
145 @param m31 value of the 3,1 Matrix element.
146 @param m32 value of the 3,2 Matrix element.
147 @param m33 value of the 3,3 Matrix element.
149 Create a matrix from the doubles given in the arguments.
151 FGMatrix33(double m11, double m12, double m13,
152 double m21, double m22, double m23,
153 double m31, double m32, double m33) {
169 ~FGMatrix33(void) { Debug(1); }
171 /** Read access the entries of the matrix.
172 @param row Row index.
173 @param col Column index.
175 @return the value of the matrix entry at the given row and
176 column indices. Indices are counted starting with 1.
178 double operator()(unsigned int row, unsigned int col) const {
179 return Entry(row, col);
182 /** Write access the entries of the matrix.
183 Note that the indices given in the arguments are unchecked.
185 @param row Row index.
186 @param col Column index.
188 @return a reference to the matrix entry at the given row and
189 column indices. Indices are counted starting with 1.
191 double& operator()(unsigned int row, unsigned int col) {
192 return Entry(row, col);
195 /** Read access the entries of the matrix.
196 This function is just a shortcut for the <tt>double&
197 operator()(unsigned int row, unsigned int col)</tt> function. It is
198 used internally to access the elements in a more convenient way.
200 Note that the indices given in the arguments are unchecked.
202 @param row Row index.
203 @param col Column index.
205 @return the value of the matrix entry at the given row and
206 column indices. Indices are counted starting with 1.
208 double Entry(unsigned int row, unsigned int col) const {
209 return data[(col-1)*eRows+row-1];
212 /** Write access the entries of the matrix.
213 This function is just a shortcut for the <tt>double&
214 operator()(unsigned int row, unsigned int col)</tt> function. It is
215 used internally to access the elements in a more convenient way.
217 Note that the indices given in the arguments are unchecked.
219 @param row Row index.
220 @param col Column index.
222 @return a reference to the matrix entry at the given row and
223 column indices. Indices are counted starting with 1.
225 double& Entry(unsigned int row, unsigned int col) {
226 return data[(col-1)*eRows+row-1];
229 /** Number of rows in the matrix.
230 @return the number of rows in the matrix.
232 unsigned int Rows(void) const { return eRows; }
234 /** Number of cloumns in the matrix.
235 @return the number of columns in the matrix.
237 unsigned int Cols(void) const { return eColumns; }
239 /** Transposed matrix.
240 This function only returns the transpose of this matrix. This matrix itself
242 @return the transposed matrix.
244 FGMatrix33 Transposed(void) const {
245 return FGMatrix33( Entry(1,1), Entry(2,1), Entry(3,1),
246 Entry(1,2), Entry(2,2), Entry(3,2),
247 Entry(1,3), Entry(2,3), Entry(3,3) );
250 /** Transposes this matrix.
251 This function only transposes this matrix. Nothing is returned.
255 /** Initialize the matrix.
256 This function initializes a matrix to all 0.0.
258 void InitMatrix(void);
260 /** Initialize the matrix.
261 This function initializes a matrix to user specified values.
263 void InitMatrix(double m11, double m12, double m13,
264 double m21, double m22, double m23,
265 double m31, double m32, double m33) {
277 /** Determinant of the matrix.
278 @return the determinant of the matrix.
280 double Determinant(void) const;
282 /** Return if the matrix is invertible.
283 Checks and returns if the matrix is nonsingular and thus
284 invertible. This is done by simply computing the determinant and
285 check if it is zero. Note that this test does not cover any
286 instabilities caused by nearly singular matirces using finite
287 arithmetics. It only checks exact singularity.
289 bool Invertible(void) const { return 0.0 != Determinant(); }
291 /** Return the inverse of the matrix.
292 Computes and returns if the inverse of the matrix. It is computed
293 by Cramers Rule. Also there are no checks performed if the matrix
294 is invertible. If you are not sure that it really is check this
295 with the @ref Invertible() call before.
297 FGMatrix33 Inverse(void) const;
299 /** Assignment operator.
301 @param A source matrix.
303 Copy the content of the matrix given in the argument into *this.
305 FGMatrix33& operator=(const FGMatrix33& A) {
318 /** Matrix vector multiplication.
320 @param v vector to multiply with.
321 @return matric vector product.
323 Compute and return the product of the current matrix with the
324 vector given in the argument.
326 FGColumnVector3 operator*(const FGColumnVector3& v) const;
328 /** Matrix subtraction.
330 @param B matrix to add to.
331 @return difference of the matrices.
333 Compute and return the sum of the current matrix and the matrix
334 B given in the argument.
336 FGMatrix33 operator-(const FGMatrix33& B) const;
340 @param B matrix to add to.
341 @return sum of the matrices.
343 Compute and return the sum of the current matrix and the matrix
344 B given in the argument.
346 FGMatrix33 operator+(const FGMatrix33& B) const;
350 @param B matrix to add to.
351 @return product of the matrices.
353 Compute and return the product of the current matrix and the matrix
354 B given in the argument.
356 FGMatrix33 operator*(const FGMatrix33& B) const;
358 /** Multiply the matrix with a scalar.
360 @param scalar scalar factor to multiply with.
361 @return scaled matrix.
363 Compute and return the product of the current matrix with the
364 scalar value scalar given in the argument.
366 FGMatrix33 operator*(const double scalar) const;
368 /** Multiply the matrix with 1.0/scalar.
370 @param scalar scalar factor to divide through.
371 @return scaled matrix.
373 Compute and return the product of the current matrix with the
374 scalar value 1.0/scalar, where scalar is given in the argument.
376 FGMatrix33 operator/(const double scalar) const;
378 /** In place matrix subtraction.
380 @param B matrix to subtract.
381 @return reference to the current matrix.
383 Compute the diffence from the current matrix and the matrix B
384 given in the argument.
386 FGMatrix33& operator-=(const FGMatrix33 &B);
388 /** In place matrix addition.
390 @param B matrix to add.
391 @return reference to the current matrix.
393 Compute the sum of the current matrix and the matrix B
394 given in the argument.
396 FGMatrix33& operator+=(const FGMatrix33 &B);
398 /** In place matrix multiplication.
400 @param B matrix to multiply with.
401 @return reference to the current matrix.
403 Compute the product of the current matrix and the matrix B
404 given in the argument.
406 FGMatrix33& operator*=(const FGMatrix33 &B);
408 /** In place matrix scale.
410 @param scalar scalar value to multiply with.
411 @return reference to the current matrix.
413 Compute the product of the current matrix and the scalar value scalar
414 given in the argument.
416 FGMatrix33& operator*=(const double scalar);
418 /** In place matrix scale.
420 @param scalar scalar value to divide through.
421 @return reference to the current matrix.
423 Compute the product of the current matrix and the scalar value
424 1.0/scalar, where scalar is given in the argument.
426 FGMatrix33& operator/=(const double scalar);
429 double data[eRows*eColumns];
431 void Debug(int from);
434 /** Scalar multiplication.
436 @param scalar scalar value to multiply with.
437 @param A Matrix to multiply.
439 Multiply the Matrix with a scalar value.
441 inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
442 // use already defined operation.
446 /** Write matrix to a stream.
448 @param os Stream to write to.
449 @param M Matrix to write.
451 Write the matrix to a stream.
453 ostream& operator<<(ostream& os, const FGMatrix33& M);
455 /** Read matrix from a stream.
457 @param os Stream to read from.
458 @param M Matrix to initialize with the values from the stream.
460 Read matrix from a stream.
462 istream& operator>>(istream& is, FGMatrix33& M);
464 } // namespace JSBSim