1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 Author: Tony Peden, Jon Berndt, Mathias Frolich
8 --------------------------------------------------------------------------------
10 03/16/2000 JSB Added exception throwing
11 03/06/2004 MF Rework of the code to make it a bit compiler friendlier
13 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
15 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
20 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
22 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
27 # include <simgear/compiler.h>
30 # include STL_IOSTREAM
32 SG_USING_STD(ostream);
33 SG_USING_STD(istream);
39 # if defined(sgi) && !defined(__GNUC__) && (_COMPILER_VERSION < 740)
46 # if defined(sgi) && !defined(__GNUC__)
60 #include "FGColumnVector3.h"
61 #include "FGJSBBase.h"
63 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
65 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
67 #define ID_MATRIX33 "$Id$"
69 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
71 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
75 class FGColumnVector3;
77 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
81 /** Exception convenience class.
84 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
85 DECLARATION: MatrixException
86 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
88 class MatrixException : public FGJSBBase
94 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
96 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
98 /** Handles matrix math operations.
99 @author Tony Peden, Jon Berndt, Mathias Froelich
102 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
103 DECLARATION: FGMatrix33
104 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
106 class FGMatrix33 : public FGJSBBase
115 /** Default initializer.
116 Create a zero matrix. */
119 /** Copy constructor.
120 @param M Matrix which is used for initialization.
121 Create copy of the matrix given in the argument. */
122 FGMatrix33(const FGMatrix33& M) {
123 Entry(1,1) = M.Entry(1,1);
124 Entry(2,1) = M.Entry(2,1);
125 Entry(3,1) = M.Entry(3,1);
126 Entry(1,2) = M.Entry(1,2);
127 Entry(2,2) = M.Entry(2,2);
128 Entry(3,2) = M.Entry(3,2);
129 Entry(1,3) = M.Entry(1,3);
130 Entry(2,3) = M.Entry(2,3);
131 Entry(3,3) = M.Entry(3,3);
136 /** Initialization by given values.
137 @param m11 value of the 1,1 Matrix element.
138 @param m12 value of the 1,2 Matrix element.
139 @param m13 value of the 1,3 Matrix element.
140 @param m21 value of the 2,1 Matrix element.
141 @param m22 value of the 2,2 Matrix element.
142 @param m23 value of the 2,3 Matrix element.
143 @param m31 value of the 3,1 Matrix element.
144 @param m32 value of the 3,2 Matrix element.
145 @param m33 value of the 3,3 Matrix element.
146 Create a matrix from the doubles given in the arguments. */
147 FGMatrix33(double m11, double m12, double m13,
148 double m21, double m22, double m23,
149 double m31, double m32, double m33) {
164 ~FGMatrix33(void) { Debug(1); }
166 /** Read access the entries of the matrix.
167 @param row Row index.
168 @param col Column index.
169 @return the value of the matrix entry at the given row and
170 column indices. Indices are counted starting with 1. */
171 double operator()(unsigned int row, unsigned int col) const {
172 return Entry(row, col);
175 /** Write access the entries of the matrix.
176 Note that the indices given in the arguments are unchecked.
177 @param row Row index.
178 @param col Column index.
179 @return a reference to the matrix entry at the given row and
180 column indices. Indices are counted starting with 1. */
181 double& operator()(unsigned int row, unsigned int col) {
182 return Entry(row, col);
185 /** Read access the entries of the matrix.
186 This function is just a shortcut for the @ref double&
187 operator()(unsigned int row, unsigned int col) function. It is
188 used internally to access the elements in a more convenient way.
189 Note that the indices given in the arguments are unchecked.
190 @param row Row index.
191 @param col Column index.
192 @return the value of the matrix entry at the given row and
193 column indices. Indices are counted starting with 1. */
194 double Entry(unsigned int row, unsigned int col) const {
195 return data[(col-1)*eRows+row-1];
198 /** Write access the entries of the matrix.
199 This function is just a shortcut for the @ref double&
200 operator()(unsigned int row, unsigned int col) function. It is
201 used internally to access the elements in a more convenient way.
202 Note that the indices given in the arguments are unchecked.
203 @param row Row index.
204 @param col Column index.
205 @return a reference to the matrix entry at the given row and
206 column indices. Indices are counted starting with 1. */
207 double& Entry(unsigned int row, unsigned int col) {
208 return data[(col-1)*eRows+row-1];
211 /** Number of rows in the matrix.
212 @return the number of rows in the matrix. */
213 unsigned int Rows(void) const { return eRows; }
215 /** Number of cloumns in the matrix.
216 @return the number of columns in the matrix. */
217 unsigned int Cols(void) const { return eColumns; }
219 /** Transposed matrix.
220 This function only returns the transpose of this matrix. This matrix itself
222 @return the transposed matrix. */
223 FGMatrix33 Transposed(void) const {
224 return FGMatrix33( Entry(1,1), Entry(2,1), Entry(3,1),
225 Entry(1,2), Entry(2,2), Entry(3,2),
226 Entry(1,3), Entry(2,3), Entry(3,3) );
229 /** Transposes this matrix.
230 This function only transposes this matrix. Nothing is returned. */
233 /** Initialize the matrix.
234 This function initializes a matrix to all 0.0. */
235 void InitMatrix(void);
237 /** Initialize the matrix.
238 This function initializes a matrix to user specified values. */
239 void InitMatrix(double m11, double m12, double m13,
240 double m21, double m22, double m23,
241 double m31, double m32, double m33) {
253 /** Determinant of the matrix.
254 @return the determinant of the matrix. */
255 double Determinant(void) const;
257 /** Return if the matrix is invertible.
258 Checks and returns if the matrix is nonsingular and thus
259 invertible. This is done by simply computing the determinant and
260 check if it is zero. Note that this test does not cover any
261 instabilities caused by nearly singular matirces using finite
262 arithmetics. It only checks exact singularity. */
263 bool Invertible(void) const { return 0.0 != Determinant(); }
265 /** Return the inverse of the matrix.
266 Computes and returns if the inverse of the matrix. It is computed
267 by Cramers Rule. Also there are no checks performed if the matrix
268 is invertible. If you are not sure that it really is check this
269 with the @ref Invertible() call before. */
270 FGMatrix33 Inverse(void) const;
272 /** Assignment operator.
273 @param A source matrix.
274 Copy the content of the matrix given in the argument into *this. */
275 FGMatrix33& operator=(const FGMatrix33& A) {
288 /** Matrix vector multiplication.
289 @param v vector to multiply with.
290 @return matric vector product.
291 Compute and return the product of the current matrix with the
292 vector given in the argument. */
293 FGColumnVector3 operator*(const FGColumnVector3& v) const;
295 /** Matrix subtraction.
296 @param B matrix to add to.
297 @return difference of the matrices.
298 Compute and return the sum of the current matrix and the matrix
299 B given in the argument. */
300 FGMatrix33 operator-(const FGMatrix33& B) const;
303 @param B matrix to add to.
304 @return sum of the matrices.
305 Compute and return the sum of the current matrix and the matrix
306 B given in the argument. */
307 FGMatrix33 operator+(const FGMatrix33& B) const;
310 @param B matrix to add to.
311 @return product of the matrices.
312 Compute and return the product of the current matrix and the matrix
313 B given in the argument. */
314 FGMatrix33 operator*(const FGMatrix33& B) const;
316 /** Multiply the matrix with a scalar.
317 @param scalar scalar factor to multiply with.
318 @return scaled matrix.
319 Compute and return the product of the current matrix with the
320 scalar value scalar given in the argument. */
321 FGMatrix33 operator*(const double scalar) const;
323 /** Multiply the matrix with 1.0/scalar.
324 @param scalar scalar factor to divide through.
325 @return scaled matrix.
326 Compute and return the product of the current matrix with the
327 scalar value 1.0/scalar, where scalar is given in the argument. */
328 FGMatrix33 operator/(const double scalar) const;
330 /** In place matrix subtraction.
331 @param B matrix to subtract.
332 @return reference to the current matrix.
333 Compute the diffence from the current matrix and the matrix B
334 given in the argument. */
335 FGMatrix33& operator-=(const FGMatrix33 &B);
337 /** In place matrix addition.
338 @param B matrix to add.
339 @return reference to the current matrix.
340 Compute the sum of the current matrix and the matrix B
341 given in the argument. */
342 FGMatrix33& operator+=(const FGMatrix33 &B);
344 /** In place matrix multiplication.
345 @param B matrix to multiply with.
346 @return reference to the current matrix.
347 Compute the product of the current matrix and the matrix B
348 given in the argument. */
349 FGMatrix33& operator*=(const FGMatrix33 &B);
351 /** In place matrix scale.
352 @param scalar scalar value to multiply with.
353 @return reference to the current matrix.
354 Compute the product of the current matrix and the scalar value scalar
355 given in the argument. */
356 FGMatrix33& operator*=(const double scalar);
358 /** In place matrix scale.
359 @param scalar scalar value to divide through.
360 @return reference to the current matrix.
361 Compute the product of the current matrix and the scalar value
362 1.0/scalar, where scalar is given in the argument. */
363 FGMatrix33& operator/=(const double scalar);
366 double data[eRows*eColumns];
368 void Debug(int from);
371 /** Scalar multiplication.
372 @param scalar scalar value to multiply with.
373 @param A Matrix to multiply.
374 Multiply the Matrix with a scalar value.*/
375 inline FGMatrix33 operator*(double scalar, const FGMatrix33& A) {
376 // use already defined operation.
380 /** Write matrix to a stream.
381 @param os Stream to write to.
382 @param M Matrix to write.
383 Write the matrix to a stream.*/
384 ostream& operator<<(ostream& os, const FGMatrix33& M);
386 /** Read matrix from a stream.
387 @param os Stream to read from.
388 @param M Matrix to initialize with the values from the stream.
389 Read matrix from a stream.*/
390 istream& operator>>(istream& is, FGMatrix33& M);
392 } // namespace JSBSim