00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
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
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