torcs - 1.2.2

src/modules/simu/simuv2.sav/SOLID-2.0/include/3D/Matrix.h

Go to the documentation of this file.
00001 /*
00002   3D - C++ Class Library for 3D Transformations
00003   Copyright (C) 1996-1998  Gino van den Bergen
00004 
00005   This library is free software; you can redistribute it and/or
00006   modify it under the terms of the GNU Library General Public
00007   License as published by the Free Software Foundation; either
00008   version 2 of the License, or (at your option) any later version.
00009 
00010   This library is distributed in the hope that it will be useful,
00011   but WITHOUT ANY WARRANTY; without even the implied warranty of
00012   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013   Library General Public License for more details.
00014 
00015   You should have received a copy of the GNU Library General Public
00016   License along with this library; if not, write to the Free
00017   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00018 
00019   Please send remarks, questions and bug reports to gino@win.tue.nl,
00020   or write to:
00021                   Gino van den Bergen
00022                   Department of Mathematics and Computing Science
00023                   Eindhoven University of Technology
00024                   P.O. Box 513, 5600 MB Eindhoven, The Netherlands
00025 */
00026 
00027 #ifndef _MATRIX_H_
00028 #define _MATRIX_H_
00029 
00030 #include "Vector.h"
00031 #include "Quaternion.h"
00032 
00033 typedef Scalar Mat3[3][3];
00034 
00035 // Row-major 3x3 matrix
00036 
00037 class Matrix {
00038 public:
00039   Matrix() {}
00040   Matrix(const float *m) { setValue(m); }
00041   Matrix(const double *m) { setValue(m); }
00042   Matrix(const Quaternion& q) { setRotation(q); }
00043   Matrix(Scalar x, Scalar y, Scalar z) { setScaling(x, y, z); }
00044   Matrix(Scalar xx, Scalar xy, Scalar xz,
00045          Scalar yx, Scalar yy, Scalar yz,
00046          Scalar zx, Scalar zy, Scalar zz) { 
00047     setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz);
00048   }
00049   
00050   Vector&       operator[](int i)       { return *(Vector *)elem[i]; }
00051   const Vector& operator[](int i) const { return *(Vector *)elem[i]; }
00052   
00053   Mat3&       getValue()       { return elem; }
00054   const Mat3& getValue() const { return elem; }
00055 
00056   void setValue(const float *m) {
00057     elem[X][X] = *m++; elem[Y][X] = *m++; elem[Z][X] = *m++; m++;
00058     elem[X][Y] = *m++; elem[Y][Y] = *m++; elem[Z][Y] = *m++; m++;
00059     elem[X][Z] = *m++; elem[Y][Z] = *m++; elem[Z][Z] = *m;
00060   }
00061 
00062   void setValue(const double *m) {
00063     elem[X][X] = *m++; elem[Y][X] = *m++; elem[Z][X] = *m++; m++;
00064     elem[X][Y] = *m++; elem[Y][Y] = *m++; elem[Z][Y] = *m++; m++;
00065     elem[X][Z] = *m++; elem[Y][Z] = *m++; elem[Z][Z] = *m;
00066   }
00067 
00068   void setValue(Scalar xx, Scalar xy, Scalar xz, 
00069                 Scalar yx, Scalar yy, Scalar yz, 
00070                 Scalar zx, Scalar zy, Scalar zz) {
00071     elem[X][X] = xx; elem[X][Y] = xy; elem[X][Z] = xz;
00072     elem[Y][X] = yx; elem[Y][Y] = yy; elem[Y][Z] = yz;
00073     elem[Z][X] = zx; elem[Z][Y] = zy; elem[Z][Z] = zz;
00074   }
00075   
00076   void setRotation(const Quaternion& q) {
00077     Scalar d = q.length2();
00078     assert(!eqz(d));
00079     Scalar s = 2 / d;
00080     Scalar xs = q[X] * s,   ys = q[Y] * s,   zs = q[Z] * s;
00081     Scalar wx = q[W] * xs,  wy = q[W] * ys,  wz = q[W] * zs;
00082     Scalar xx = q[X] * xs,  xy = q[X] * ys,  xz = q[X] * zs;
00083     Scalar yy = q[Y] * ys,  yz = q[Y] * zs,  zz = q[Z] * zs;
00084     setValue(1 - (yy + zz),       xy - wz,       xz + wy,
00085              xy + wz      , 1 - (xx + zz),       yz - wx,
00086              xz - wy      , yz + wx      , 1 - (xx + yy));
00087   }
00088   
00089   void setScaling(Scalar x, Scalar y, Scalar z) {
00090     setValue(x, 0, 0, 0, y, 0, 0, 0, z); 
00091   }
00092   
00093   void setIdentity() { setValue(1, 0, 0, 0, 1, 0, 0, 0, 1); }
00094   
00095   Matrix& operator*=(const Matrix& m); 
00096 
00097   Scalar tdot(int i, const Vector& v) const {
00098     return elem[X][i] * v[X] + elem[Y][i] * v[Y] + elem[Z][i] * v[Z];
00099   }
00100   
00101   Scalar determinant() const;
00102   Matrix absolute() const;
00103   Matrix transpose() const;
00104   Matrix adjoint() const;
00105   Matrix inverse() const; 
00106   
00107 protected:
00108   Mat3 elem;
00109 };
00110 
00111 Vector operator*(const Matrix& m, const Vector& v);
00112 Vector operator*(const Vector& v, const Matrix& m);
00113 Matrix operator*(const Matrix& m1, const Matrix& m2);
00114 
00115 Matrix multTransposeLeft(const Matrix& m1, const Matrix& m2);
00116 
00117 Matrix transpose(const Matrix& m);
00118 Matrix adjoint(const Matrix& m);
00119 Matrix inverse(const Matrix& m);
00120 Matrix absolute(const Matrix& m);
00121 
00122 ostream& operator<<(ostream& os, const Matrix& m);
00123 
00124 
00125 
00126 inline Matrix& Matrix::operator*=(const Matrix& m) {
00127   setValue(elem[X][X] * m[X][X] + elem[X][Y] * m[Y][X] + elem[X][Z] * m[Z][X],
00128            elem[X][X] * m[X][Y] + elem[X][Y] * m[Y][Y] + elem[X][Z] * m[Z][Y],
00129            elem[X][X] * m[X][Z] + elem[X][Y] * m[Y][Z] + elem[X][Z] * m[Z][Z],
00130            elem[Y][X] * m[X][X] + elem[Y][Y] * m[Y][X] + elem[Y][Z] * m[Z][X],
00131            elem[Y][X] * m[X][Y] + elem[Y][Y] * m[Y][Y] + elem[Y][Z] * m[Z][Y],
00132            elem[Y][X] * m[X][Z] + elem[Y][Y] * m[Y][Z] + elem[Y][Z] * m[Z][Z],
00133            elem[Z][X] * m[X][X] + elem[Z][Y] * m[Y][X] + elem[Z][Z] * m[Z][X],
00134            elem[Z][X] * m[X][Y] + elem[Z][Y] * m[Y][Y] + elem[Z][Z] * m[Z][Y],
00135            elem[Z][X] * m[X][Z] + elem[Z][Y] * m[Y][Z] + elem[Z][Z] * m[Z][Z]);
00136   return *this;
00137 }
00138 
00139 inline Scalar Matrix::determinant() const { 
00140   return triple((*this)[X], (*this)[Y], (*this)[Z]);
00141 }
00142 
00143 inline Matrix Matrix::absolute() const {
00144   return Matrix(fabs(elem[X][X]), fabs(elem[X][Y]), fabs(elem[X][Z]),
00145                 fabs(elem[Y][X]), fabs(elem[Y][Y]), fabs(elem[Y][Z]),
00146                 fabs(elem[Z][X]), fabs(elem[Z][Y]), fabs(elem[Z][Z]));
00147 }
00148 
00149 inline Matrix Matrix::transpose() const {
00150   return Matrix(elem[X][X], elem[Y][X], elem[Z][X],
00151                 elem[X][Y], elem[Y][Y], elem[Z][Y],
00152                 elem[X][Z], elem[Y][Z], elem[Z][Z]);
00153 }
00154 
00155 inline Matrix Matrix::adjoint() const {
00156   return Matrix(elem[Y][Y] * elem[Z][Z] - elem[Y][Z] * elem[Z][Y],
00157                 elem[X][Z] * elem[Z][Y] - elem[X][Y] * elem[Z][Z],
00158                 elem[X][Y] * elem[Y][Z] - elem[X][Z] * elem[Y][Y],
00159                 elem[Y][Z] * elem[Z][X] - elem[Y][X] * elem[Z][Z],
00160                 elem[X][X] * elem[Z][Z] - elem[X][Z] * elem[Z][X],
00161                 elem[X][Z] * elem[Y][X] - elem[X][X] * elem[Y][Z],
00162                 elem[Y][X] * elem[Z][Y] - elem[Y][Y] * elem[Z][X],
00163                 elem[X][Y] * elem[Z][X] - elem[X][X] * elem[Z][Y],
00164                 elem[X][X] * elem[Y][Y] - elem[X][Y] * elem[Y][X]);
00165 }
00166 
00167 inline Matrix Matrix::inverse() const {
00168   Vector co(elem[Y][Y] * elem[Z][Z] - elem[Y][Z] * elem[Z][Y],
00169             elem[Y][Z] * elem[Z][X] - elem[Y][X] * elem[Z][Z], 
00170             elem[Y][X] * elem[Z][Y] - elem[Y][Y] * elem[Z][X]);
00171   Scalar d = dot((*this)[X], co);
00172   assert(!eqz(d));
00173   Scalar s = 1 / d;
00174   return Matrix(co[X] * s,
00175                 (elem[X][Z] * elem[Z][Y] - elem[X][Y] * elem[Z][Z]) * s,
00176                 (elem[X][Y] * elem[Y][Z] - elem[X][Z] * elem[Y][Y]) * s,
00177                 co[Y] * s,
00178                 (elem[X][X] * elem[Z][Z] - elem[X][Z] * elem[Z][X]) * s,
00179                 (elem[X][Z] * elem[Y][X] - elem[X][X] * elem[Y][Z]) * s,
00180                 co[Z] * s,
00181                 (elem[X][Y] * elem[Z][X] - elem[X][X] * elem[Z][Y]) * s,
00182                 (elem[X][X] * elem[Y][Y] - elem[X][Y] * elem[Y][X]) * s);
00183 }
00184 
00185 inline Vector operator*(const Matrix& m, const Vector& v) {
00186   return Vector(dot(m[X], v), dot(m[Y], v), dot(m[Z], v));
00187 }
00188 
00189 inline Vector operator*(const Vector& v, const Matrix& m) {
00190   return Vector(m.tdot(X, v), m.tdot(Y, v), m.tdot(Z, v));
00191 }
00192 
00193 inline Matrix operator*(const Matrix& m1, const Matrix& m2) {
00194   return Matrix(
00195     m1[X][X] * m2[X][X] + m1[X][Y] * m2[Y][X] + m1[X][Z] * m2[Z][X],
00196     m1[X][X] * m2[X][Y] + m1[X][Y] * m2[Y][Y] + m1[X][Z] * m2[Z][Y],
00197     m1[X][X] * m2[X][Z] + m1[X][Y] * m2[Y][Z] + m1[X][Z] * m2[Z][Z],
00198     m1[Y][X] * m2[X][X] + m1[Y][Y] * m2[Y][X] + m1[Y][Z] * m2[Z][X],
00199     m1[Y][X] * m2[X][Y] + m1[Y][Y] * m2[Y][Y] + m1[Y][Z] * m2[Z][Y],
00200     m1[Y][X] * m2[X][Z] + m1[Y][Y] * m2[Y][Z] + m1[Y][Z] * m2[Z][Z],
00201     m1[Z][X] * m2[X][X] + m1[Z][Y] * m2[Y][X] + m1[Z][Z] * m2[Z][X],
00202     m1[Z][X] * m2[X][Y] + m1[Z][Y] * m2[Y][Y] + m1[Z][Z] * m2[Z][Y],
00203     m1[Z][X] * m2[X][Z] + m1[Z][Y] * m2[Y][Z] + m1[Z][Z] * m2[Z][Z]);
00204 }
00205 
00206 inline Matrix multTransposeLeft(const Matrix& m1, const Matrix& m2) {
00207   return Matrix(
00208     m1[X][X] * m2[X][X] + m1[Y][X] * m2[Y][X] + m1[Z][X] * m2[Z][X],
00209     m1[X][X] * m2[X][Y] + m1[Y][X] * m2[Y][Y] + m1[Z][X] * m2[Z][Y],
00210     m1[X][X] * m2[X][Z] + m1[Y][X] * m2[Y][Z] + m1[Z][X] * m2[Z][Z],
00211     m1[X][Y] * m2[X][X] + m1[Y][Y] * m2[Y][X] + m1[Z][Y] * m2[Z][X],
00212     m1[X][Y] * m2[X][Y] + m1[Y][Y] * m2[Y][Y] + m1[Z][Y] * m2[Z][Y],
00213     m1[X][Y] * m2[X][Z] + m1[Y][Y] * m2[Y][Z] + m1[Z][Y] * m2[Z][Z],
00214     m1[X][Z] * m2[X][X] + m1[Y][Z] * m2[Y][X] + m1[Z][Z] * m2[Z][X],
00215     m1[X][Z] * m2[X][Y] + m1[Y][Z] * m2[Y][Y] + m1[Z][Z] * m2[Z][Y],
00216     m1[X][Z] * m2[X][Z] + m1[Y][Z] * m2[Y][Z] + m1[Z][Z] * m2[Z][Z]);
00217 }
00218 
00219 inline Scalar determinant(const Matrix& m) { return m.determinant(); }
00220 inline Matrix absolute(const Matrix& m) { return m.absolute(); }
00221 inline Matrix transpose(const Matrix& m) { return m.transpose(); }
00222 inline Matrix adjoint(const Matrix& m) { return m.adjoint(); }
00223 inline Matrix inverse(const Matrix& m) { return m.inverse(); }
00224 
00225 inline ostream& operator<<(ostream& os, const Matrix& m) {
00226   return os << m[X] << endl << m[Y] << endl << m[Z] << endl;
00227 }
00228 
00229 #endif

Generated at Thu Feb 26 21:52:30 2004 for torcs by doxygen 1.3.3 written by Dimitri van Heesch, © 1997-1999
TORCS © Eric Espié 1999, 2002.