00001 #include <vecmath/Export>
00002
00003 #include <vecmath/_Matrix4.h>
00004 #include <vecmath/GMatrix.h>
00005
00006 const double Math::PI = M_PI;
00007 const double Math::MAXDOUBLE = DBL_MAX;
00008 const double Math::MINDOUBLE = DBL_MIN;
00009 const double Math::EPSILON = 1.0e-12;
00010
00011
00012 template <class Type>
00013 void _Matrix4<Type>::add(Type d, const _Matrix4<Type>& m)
00014 {
00015 set(m.m00 + d, m.m01 + d, m.m02 + d, m.m03 + d,
00016 m.m10 + d, m.m11 + d, m.m12 + d, m.m13 + d,
00017 m.m20 + d, m.m21 + d, m.m22 + d, m.m23 + d,
00018 m.m30 + d, m.m31 + d, m.m32 + d, m.m33 + d);
00019 }
00020
00021 template <class Type>
00022 void _Matrix4<Type>::add(const _Matrix4<Type>& m1, const _Matrix4<Type>& m2)
00023 {
00024 set(m1.m00 + m2.m00, m1.m01 + m2.m01, m1.m02 + m2.m02, m1.m03 + m2.m03,
00025 m1.m10 + m2.m10, m1.m11 + m2.m11, m1.m12 + m2.m12, m1.m13 + m2.m13,
00026 m1.m20 + m2.m20, m1.m21 + m2.m21, m1.m22 + m2.m22, m1.m23 + m2.m23,
00027 m1.m30 + m2.m30, m1.m31 + m2.m31, m1.m32 + m2.m32, m1.m33 + m2.m33);
00028 }
00029
00030 template <class Type>
00031 void _Matrix4<Type>::adjoint(const _Matrix4<Type>& m)
00032 {
00033 Type d = m.determinant();
00034
00035
00036 Type t[4][4];
00037
00038 t[0][0]= _Matrix3<Type>::determinant3x3(m.m11, m.m12, m.m13,
00039 m.m21, m.m22, m.m23,
00040 m.m31, m.m32, m.m33) / d;
00041 t[1][0]= -_Matrix3<Type>::determinant3x3(m.m10, m.m12, m.m13,
00042 m.m20, m.m22, m.m23,
00043 m.m30, m.m32, m.m33) / d;
00044 t[2][0]= _Matrix3<Type>::determinant3x3(m.m10, m.m11, m.m13,
00045 m.m20, m.m21, m.m23,
00046 m.m30, m.m31, m.m33) / d;
00047 t[3][0]= -_Matrix3<Type>::determinant3x3(m.m10, m.m11, m.m12,
00048 m.m20, m.m21, m.m22,
00049 m.m30, m.m31, m.m32) / d;
00050
00051
00052 t[0][1]= -_Matrix3<Type>::determinant3x3(m.m01, m.m02, m.m03,
00053 m.m21, m.m22, m.m23,
00054 m.m31, m.m32, m.m33) / d;
00055 t[1][1]= _Matrix3<Type>::determinant3x3(m.m00, m.m02, m.m03,
00056 m.m20, m.m22, m.m23,
00057 m.m30, m.m32, m.m33) / d;
00058 t[2][1]= -_Matrix3<Type>::determinant3x3(m.m00, m.m01, m.m03,
00059 m.m20, m.m21, m.m23,
00060 m.m30, m.m31, m.m33) / d;
00061 t[3][1]= _Matrix3<Type>::determinant3x3(m.m00, m.m01, m.m02,
00062 m.m20, m.m21, m.m22,
00063 m.m30, m.m31, m.m32) / d;
00064
00065
00066 t[0][2]= _Matrix3<Type>::determinant3x3(m.m01, m.m02, m.m03,
00067 m.m11, m.m12, m.m13,
00068 m.m31, m.m32, m.m33) / d;
00069 t[1][2]= -_Matrix3<Type>::determinant3x3(m.m00, m.m02, m.m03,
00070 m.m10, m.m12, m.m13,
00071 m.m30, m.m32, m.m33) / d;
00072 t[2][2]= _Matrix3<Type>::determinant3x3(m.m00, m.m01, m.m03,
00073 m.m10, m.m11, m.m13,
00074 m.m30, m.m31, m.m33) / d;
00075 t[3][2]= -_Matrix3<Type>::determinant3x3(m.m00, m.m01, m.m02,
00076 m.m10, m.m11, m.m12,
00077 m.m30, m.m31, m.m32) / d;
00078
00079
00080 t[0][3]= -_Matrix3<Type>::determinant3x3(m.m01, m.m02, m.m03,
00081 m.m11, m.m12, m.m13,
00082 m.m21, m.m22, m.m23) / d;
00083 t[1][3]= _Matrix3<Type>::determinant3x3(m.m00, m.m02, m.m03,
00084 m.m10, m.m12, m.m13,
00085 m.m20, m.m22, m.m23) / d;
00086 t[2][3]= -_Matrix3<Type>::determinant3x3(m.m00, m.m01, m.m03,
00087 m.m10, m.m11, m.m13,
00088 m.m20, m.m21, m.m23) / d;
00089 t[3][3]= _Matrix3<Type>::determinant3x3(m.m00, m.m01, m.m02,
00090 m.m10, m.m11, m.m12,
00091 m.m20, m.m21, m.m22) / d;
00092
00093 set((Type*)t);
00094 }
00095
00096
00097 template <class Type>
00098 bool _Matrix4<Type>::epsilonEquals(const _Matrix4<Type>& m, Type epsilon) const
00099 {
00100 return (Math::epsilonEquals(m00, m.m00, epsilon) &&
00101 Math::epsilonEquals(m01, m.m01, epsilon) &&
00102 Math::epsilonEquals(m02, m.m02, epsilon) &&
00103 Math::epsilonEquals(m03, m.m03, epsilon) &&
00104 Math::epsilonEquals(m10, m.m10, epsilon) &&
00105 Math::epsilonEquals(m11, m.m11, epsilon) &&
00106 Math::epsilonEquals(m12, m.m12, epsilon) &&
00107 Math::epsilonEquals(m13, m.m13, epsilon) &&
00108 Math::epsilonEquals(m20, m.m20, epsilon) &&
00109 Math::epsilonEquals(m21, m.m21, epsilon) &&
00110 Math::epsilonEquals(m22, m.m22, epsilon) &&
00111 Math::epsilonEquals(m23, m.m23, epsilon) &&
00112 Math::epsilonEquals(m30, m.m30, epsilon) &&
00113 Math::epsilonEquals(m31, m.m31, epsilon) &&
00114 Math::epsilonEquals(m32, m.m32, epsilon) &&
00115 Math::epsilonEquals(m33, m.m33, epsilon));
00116 }
00117
00118
00119 template <class Type>
00120 void _Matrix4<Type>::get(_Matrix3<double>& m) const
00121 {
00122 double tmp_scale[3];
00123 double tmp_rot[9];
00124 getScaleRotate(tmp_scale, tmp_rot);
00125 m.set(tmp_rot);
00126 }
00127
00128 template <class Type>
00129 Type _Matrix4<Type>::get(_Matrix3<double>& m, _Vector3<double>& v) const
00130 {
00131 double tmp_scale[3];
00132 double tmp_rot[9];
00133 getScaleRotate(tmp_scale, tmp_rot);
00134 m.set(tmp_rot);
00135 get(v);
00136 return Math::max3(tmp_scale);
00137 }
00138
00139 template <class Type>
00140 void _Matrix4<Type>::get(_Matrix3<double>& m, _Vector3<double>& v, _Vector3<double>& s) const
00141 {
00142 double tmp_scale[3];
00143 double tmp_rot[9];
00144 getScaleRotate(tmp_scale, tmp_rot);
00145 m.set(tmp_rot);
00146 s.set(tmp_scale);
00147 get(v);
00148 }
00149
00150 template <class Type>
00151 void _Matrix4<Type>::get(_Matrix3<float>& m) const
00152 {
00153 double tmp_scale[3];
00154 double tmp_rot[9];
00155 getScaleRotate(tmp_scale, tmp_rot);
00156 m.set(tmp_rot);
00157 }
00158
00159 template <class Type>
00160 Type _Matrix4<Type>::get(_Matrix3<float>& m, _Vector3<float>& v) const
00161 {
00162 double tmp_scale[3];
00163 double tmp_rot[9];
00164 getScaleRotate(tmp_scale, tmp_rot);
00165 m.set(tmp_rot);
00166 get(v);
00167 return Math::max3(tmp_scale);
00168 }
00169
00170 template <class Type>
00171 void _Matrix4<Type>::get(_Matrix3<float>& m, _Vector3<float>& v, _Vector3<float>& s) const
00172 {
00173 double tmp_scale[3];
00174 double tmp_rot[9];
00175 getScaleRotate(tmp_scale, tmp_rot);
00176 m.set(tmp_rot);
00177 s.set(tmp_scale);
00178 get(v);
00179 }
00180
00181 template <class Type>
00182 void _Matrix4<Type>::get(_Quat4<double>& q) const
00183 {
00184 double tmp_scale[3];
00185 double tmp_rot[9];
00186 getScaleRotate(tmp_scale, tmp_rot);
00187 q.set(tmp_rot);
00188 }
00189
00190 template <class Type>
00191 void _Matrix4<Type>::get(_Quat4<float>& q) const
00192 {
00193 double tmp_scale[3];
00194 double tmp_rot[9];
00195 getScaleRotate(tmp_scale, tmp_rot);
00196 q.set(tmp_rot);
00197 }
00198
00199 template <class Type>
00200 void _Matrix4<Type>::getColumn(int column, _Vector4<Type>& v) const
00201 {
00202 switch (column) {
00203 case 0: v.set(m00, m10, m20, m30); break;
00204 case 1: v.set(m01, m11, m21, m31); break;
00205 case 2: v.set(m02, m12, m22, m32); break;
00206 case 3: v.set(m03, m13, m23, m33); break;
00207 }
00208 }
00209
00210
00211 template <class Type>
00212 void _Matrix4<Type>::getColumn(int column, Type c[]) const
00213 {
00214 switch (column) {
00215 case 0: c[0] = m00; c[1] = m10; c[2] = m20; c[3] = m30; break;
00216 case 1: c[0] = m01; c[1] = m11; c[2] = m21; c[3] = m31; break;
00217 case 2: c[0] = m02; c[1] = m12; c[2] = m22; c[3] = m32; break;
00218 case 3: c[0] = m02; c[1] = m12; c[2] = m22; c[3] = m32; break;
00219 }
00220 }
00221
00222 template <class Type>
00223 Type _Matrix4<Type>::getElement(int row, int column) const
00224 {
00225 if (row >= 0 && row < 4 || column >= 0 || column < 4)
00226 return operator()(row, column);
00227 else
00228 return 0;
00229 }
00230
00231
00232
00233
00234
00235
00236
00237 template <class Type>
00238 void _Matrix4<Type>::getRotationScale(_Matrix3<double>& m3d) const
00239 {
00240 m3d.set(m00, m01, m02,
00241 m10, m11, m12,
00242 m20, m21, m22);
00243 }
00244
00245 template <class Type>
00246 void _Matrix4<Type>::getRotationScale(_Matrix3<float>& m3f) const
00247 {
00248 m3f.set(m00, m01, m02,
00249 m10, m11, m12,
00250 m20, m21, m22);
00251 }
00252
00253 template <class Type>
00254 void _Matrix4<Type>::getRow(int row, _Vector4<Type>& v) const
00255 {
00256 switch (row) {
00257 case 0: v.set(m00, m01, m02, m03); break;
00258 case 1: v.set(m10, m11, m12, m13); break;
00259 case 2: v.set(m20, m21, m22, m23); break;
00260 case 3: v.set(m30, m31, m32, m33); break;
00261 default:
00262 break;
00263 }
00264 }
00265
00266 template <class Type>
00267 void _Matrix4<Type>::getRow(int row, Type c[4]) const
00268 {
00269 switch (row) {
00270 case 0: c[0] = m00; c[1] = m01; c[2] = m02; c[3] = m03; break;
00271 case 1: c[0] = m10; c[1] = m11; c[2] = m12; c[3] = m13; break;
00272 case 2: c[0] = m20; c[1] = m21; c[2] = m22; c[3] = m23; break;
00273 case 3: c[0] = m20; c[1] = m21; c[2] = m22; c[3] = m23; break;
00274 }
00275 }
00276
00277 template <class Type>
00278 double _Matrix4<Type>::getScale() const {
00279 double tmp_scale[3];
00280 double tmp_rot[9];
00281 getScaleRotate(tmp_scale, tmp_rot);
00282 return Math::max3(tmp_scale);
00283 }
00284
00285 template <class Type>
00286 void _Matrix4<Type>::getScaleRotate(double scale[3], double rot[9]) const
00287 {
00288 double tmp[9];
00289 get3x3(tmp);
00290 _Matrix3<Type>::compute_svd(tmp, scale, rot, false);
00291 }
00292
00293 template <class Type>
00294 void _Matrix4<Type>::invert(const _Matrix4<Type>& m)
00295 {
00296 int ai[4];
00297 Type tmp[16];
00298 m.get(tmp);
00299
00300 Type ad[16];
00301
00302 for (int i = 0; i < 16; i++)
00303 ad[i] = 0.0;
00304 ad[0] = ad[5] = ad[10] = ad[15] = 1.0;
00305 luBacksubstitution(tmp, ai, ad);
00306 set(ad);
00307 }
00308
00309 template <class Type>
00310 void _Matrix4<Type>::luBacksubstitution(Type ad[16], int ai[4], Type ad1[16])
00311 {
00312 int j1 = 0;
00313 for(int i1 = 0; i1 < 4; i1++) {
00314 int k1 = i1;
00315 int j = -1;
00316 for(int i = 0; i < 4; i++) {
00317 int k = ai[j1 + i];
00318 Type d = ad1[k1 + 4 * k];
00319 ad1[k1 + 4 * k] = ad1[k1 + 4 * i];
00320 if(j >= 0) {
00321 int l1 = i * 4;
00322 for(int l = j; l <= i - 1; l++)
00323 d -= ad[l1 + l] * ad1[k1 + 4 * l];
00324
00325 } else
00326 if(d != 0.0)
00327 j = i;
00328 ad1[k1 + 4 * i] = d;
00329 }
00330
00331 int i2 = 12;
00332 ad1[k1 + 12] /= ad[i2 + 3];
00333 i2 -= 4;
00334 ad1[k1 + 8] = (ad1[k1 + 8] - ad[i2 + 3] * ad1[k1 + 12]) / ad[i2 + 2];
00335 i2 -= 4;
00336 ad1[k1 + 4] = (ad1[k1 + 4] - ad[i2 + 2] * ad1[k1 + 8] - ad[i2 + 3] * ad1[k1 + 12]) / ad[i2 + 1];
00337 i2 -= 4;
00338 ad1[k1] = (ad1[k1] - ad[i2 + 1] * ad1[k1 + 4] - ad[i2 + 2] * ad1[k1 + 8] - ad[i2 + 3] * ad1[k1 + 12]) / ad[i2];
00339 }
00340 }
00341
00342 template <class Type>
00343 bool _Matrix4<Type>::luDecomposition(Type ad[], int ai[])
00344 {
00345
00346
00347 Type ad1[4];
00348 int k = 0;
00349 int l = 0;
00350
00351 for(int i = 4; i-- != 0;) {
00352 Type d = 0.0;
00353 for(int j = 4; j-- != 0;) {
00354 Type d1 = ad[k++];
00355 d1 = fabs(d1);
00356 if(d1 > d)
00357 d = d1;
00358 }
00359
00360 if(d == 0.0)
00361 return false;
00362 ad1[l++] = 1.0 / d;
00363 }
00364
00365 int j1 = 0;
00366 for (int i1 = 0; i1 < 4; i1++) {
00367 for (int k1 = 0; k1 < i1; k1++) {
00368 int j3 = j1 + 4 * k1 + i1;
00369 Type d2 = ad[j3];
00370 int k2 = k1;
00371 int i4 = j1 + 4 * k1;
00372 for (int l4 = j1 + i1; k2-- != 0; l4 += 4) {
00373 d2 -= ad[i4] * ad[l4];
00374 i4++;
00375 }
00376
00377 ad[j3] = d2;
00378 }
00379
00380 Type d4 = 0.0;
00381 int j2 = -1;
00382 for (int l1 = i1; l1 < 4; l1++) {
00383 int k3 = j1 + 4 * l1 + i1;
00384 Type d3 = ad[k3];
00385 int l2 = i1;
00386 int j4 = j1 + 4 * l1;
00387 for (int i5 = j1 + i1; l2-- != 0; i5 += 4) {
00388 d3 -= ad[j4] * ad[i5];
00389 j4++;
00390 }
00391
00392 ad[k3] = d3;
00393 Type d5;
00394 if ((d5 = ad1[l1] * fabs(d3)) >= d4) {
00395 d4 = d5;
00396 j2 = l1;
00397 }
00398 }
00399
00400 if (i1 != j2) {
00401 int i3 = 4;
00402 int k4 = j1 + 4 * j2;
00403 int j5 = j1 + 4 * i1;
00404 while (i3-- != 0) {
00405 double d6 = ad[k4];
00406 ad[k4++] = ad[j5];
00407 ad[j5++] = d6;
00408 }
00409 ad1[j2] = ad1[i1];
00410 }
00411 ai[i1] = j2;
00412 if (ad[j1 + 4 * i1 + i1] == 0.0)
00413 return false;
00414
00415 if(i1 != 3) {
00416 Type d7 = ad[j1 + 4 * i1 + i1];
00417 int l3 = j1 + 4 * (i1 + 1) + i1;
00418 for (int i2 = 3 - i1; i2-- != 0;) {
00419 ad[l3] /= d7;
00420 l3 += 4;
00421 }
00422
00423 }
00424 }
00425
00426 return true;
00427 }
00428
00429 template <class Type>
00430 void _Matrix4<Type>::mul(Type d, const _Matrix4<Type>& m)
00431 {
00432 set(m.m00 * d, m.m01 * d, m.m02 * d, m.m03 * d,
00433 m.m10 * d, m.m11 * d, m.m12 * d, m.m13 * d,
00434 m.m20 * d, m.m21 * d, m.m22 * d, m.m23 * d,
00435 m.m30 * d, m.m31 * d, m.m32 * d, m.m33 * d);
00436 }
00437
00438 template <class Type>
00439 void _Matrix4<Type>::mul(const _Matrix4<Type>& m)
00440 {
00441 mul(*this, m);
00442 }
00443
00444 template <class Type>
00445 void _Matrix4<Type>::mul(const _Matrix4<Type>& m1, const _Matrix4<Type>& m2)
00446 {
00447 if(this != &m1 && this != &m2) {
00448 m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20 + m1.m03 * m2.m30;
00449 m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21 + m1.m03 * m2.m31;
00450 m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22 + m1.m03 * m2.m32;
00451 m03 = m1.m00 * m2.m03 + m1.m01 * m2.m13 + m1.m02 * m2.m23 + m1.m03 * m2.m33;
00452 m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20 + m1.m13 * m2.m30;
00453 m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21 + m1.m13 * m2.m31;
00454 m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22 + m1.m13 * m2.m32;
00455 m13 = m1.m10 * m2.m03 + m1.m11 * m2.m13 + m1.m12 * m2.m23 + m1.m13 * m2.m33;
00456 m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20 + m1.m23 * m2.m30;
00457 m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21 + m1.m23 * m2.m31;
00458 m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22 + m1.m23 * m2.m32;
00459 m23 = m1.m20 * m2.m03 + m1.m21 * m2.m13 + m1.m22 * m2.m23 + m1.m23 * m2.m33;
00460 m30 = m1.m30 * m2.m00 + m1.m31 * m2.m10 + m1.m32 * m2.m20 + m1.m33 * m2.m30;
00461 m31 = m1.m30 * m2.m01 + m1.m31 * m2.m11 + m1.m32 * m2.m21 + m1.m33 * m2.m31;
00462 m32 = m1.m30 * m2.m02 + m1.m31 * m2.m12 + m1.m32 * m2.m22 + m1.m33 * m2.m32;
00463 m33 = m1.m30 * m2.m03 + m1.m31 * m2.m13 + m1.m32 * m2.m23 + m1.m33 * m2.m33;
00464 } else {
00465 Type d0 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20 + m1.m03 * m2.m30;
00466 Type d1 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21 + m1.m03 * m2.m31;
00467 Type d2 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22 + m1.m03 * m2.m32;
00468 Type d3 = m1.m00 * m2.m03 + m1.m01 * m2.m13 + m1.m02 * m2.m23 + m1.m03 * m2.m33;
00469 Type d4 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20 + m1.m13 * m2.m30;
00470 Type d5 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21 + m1.m13 * m2.m31;
00471 Type d6 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22 + m1.m13 * m2.m32;
00472 Type d7 = m1.m10 * m2.m03 + m1.m11 * m2.m13 + m1.m12 * m2.m23 + m1.m13 * m2.m33;
00473 Type d8 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20 + m1.m23 * m2.m30;
00474 Type d9 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21 + m1.m23 * m2.m31;
00475 Type d10 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22 + m1.m23 * m2.m32;
00476 Type d11 = m1.m20 * m2.m03 + m1.m21 * m2.m13 + m1.m22 * m2.m23 + m1.m23 * m2.m33;
00477 Type d12 = m1.m30 * m2.m00 + m1.m31 * m2.m10 + m1.m32 * m2.m20 + m1.m33 * m2.m30;
00478 Type d13 = m1.m30 * m2.m01 + m1.m31 * m2.m11 + m1.m32 * m2.m21 + m1.m33 * m2.m31;
00479 Type d14 = m1.m30 * m2.m02 + m1.m31 * m2.m12 + m1.m32 * m2.m22 + m1.m33 * m2.m32;
00480 Type d15 = m1.m30 * m2.m03 + m1.m31 * m2.m13 + m1.m32 * m2.m23 + m1.m33 * m2.m33;
00481 set(d0, d1, d2, d3,
00482 d4, d5, d6, d7,
00483 d8, d9, d10, d11,
00484 d12, d13, d14, d15);
00485 }
00486 }
00487
00488 template <class Type>
00489 void _Matrix4<Type>::mulTransposeBoth(const _Matrix4<Type>& m1, const _Matrix4<Type>& m2)
00490 {
00491 if (this != &m1 && this != &m2) {
00492 m00 = m1.m00 * m2.m00 + m1.m10 * m2.m01 + m1.m20 * m2.m02 + m1.m30 * m2.m03;
00493 m01 = m1.m00 * m2.m10 + m1.m10 * m2.m11 + m1.m20 * m2.m12 + m1.m30 * m2.m13;
00494 m02 = m1.m00 * m2.m20 + m1.m10 * m2.m21 + m1.m20 * m2.m22 + m1.m30 * m2.m23;
00495 m03 = m1.m00 * m2.m30 + m1.m10 * m2.m31 + m1.m20 * m2.m32 + m1.m30 * m2.m33;
00496 m10 = m1.m01 * m2.m00 + m1.m11 * m2.m01 + m1.m21 * m2.m02 + m1.m31 * m2.m03;
00497 m11 = m1.m01 * m2.m10 + m1.m11 * m2.m11 + m1.m21 * m2.m12 + m1.m31 * m2.m13;
00498 m12 = m1.m01 * m2.m20 + m1.m11 * m2.m21 + m1.m21 * m2.m22 + m1.m31 * m2.m23;
00499 m13 = m1.m01 * m2.m30 + m1.m11 * m2.m31 + m1.m21 * m2.m32 + m1.m31 * m2.m33;
00500 m20 = m1.m02 * m2.m00 + m1.m12 * m2.m01 + m1.m22 * m2.m02 + m1.m32 * m2.m03;
00501 m21 = m1.m02 * m2.m10 + m1.m12 * m2.m11 + m1.m22 * m2.m12 + m1.m32 * m2.m13;
00502 m22 = m1.m02 * m2.m20 + m1.m12 * m2.m21 + m1.m22 * m2.m22 + m1.m32 * m2.m23;
00503 m23 = m1.m02 * m2.m30 + m1.m12 * m2.m31 + m1.m22 * m2.m32 + m1.m32 * m2.m33;
00504 m30 = m1.m03 * m2.m00 + m1.m13 * m2.m01 + m1.m23 * m2.m02 + m1.m33 * m2.m03;
00505 m31 = m1.m03 * m2.m10 + m1.m13 * m2.m11 + m1.m23 * m2.m12 + m1.m33 * m2.m13;
00506 m32 = m1.m03 * m2.m20 + m1.m13 * m2.m21 + m1.m23 * m2.m22 + m1.m33 * m2.m23;
00507 m33 = m1.m03 * m2.m30 + m1.m13 * m2.m31 + m1.m23 * m2.m32 + m1.m33 * m2.m33;
00508 } else {
00509 Type d0 = m1.m00 * m2.m00 + m1.m10 * m2.m01 + m1.m20 * m2.m02 + m1.m30 * m2.m03;
00510 Type d1 = m1.m00 * m2.m10 + m1.m10 * m2.m11 + m1.m20 * m2.m12 + m1.m30 * m2.m13;
00511 Type d2 = m1.m00 * m2.m20 + m1.m10 * m2.m21 + m1.m20 * m2.m22 + m1.m30 * m2.m23;
00512 Type d3 = m1.m00 * m2.m30 + m1.m10 * m2.m31 + m1.m20 * m2.m32 + m1.m30 * m2.m33;
00513 Type d4 = m1.m01 * m2.m00 + m1.m11 * m2.m01 + m1.m21 * m2.m02 + m1.m31 * m2.m03;
00514 Type d5 = m1.m01 * m2.m10 + m1.m11 * m2.m11 + m1.m21 * m2.m12 + m1.m31 * m2.m13;
00515 Type d6 = m1.m01 * m2.m20 + m1.m11 * m2.m21 + m1.m21 * m2.m22 + m1.m31 * m2.m23;
00516 Type d7 = m1.m01 * m2.m30 + m1.m11 * m2.m31 + m1.m21 * m2.m32 + m1.m31 * m2.m33;
00517 Type d8 = m1.m02 * m2.m00 + m1.m12 * m2.m01 + m1.m22 * m2.m02 + m1.m32 * m2.m03;
00518 Type d9 = m1.m02 * m2.m10 + m1.m12 * m2.m11 + m1.m22 * m2.m12 + m1.m32 * m2.m13;
00519 Type d10 = m1.m02 * m2.m20 + m1.m12 * m2.m21 + m1.m22 * m2.m22 + m1.m32 * m2.m23;
00520 Type d11 = m1.m02 * m2.m30 + m1.m12 * m2.m31 + m1.m22 * m2.m32 + m1.m32 * m2.m33;
00521 Type d12 = m1.m03 * m2.m00 + m1.m13 * m2.m01 + m1.m23 * m2.m02 + m1.m33 * m2.m03;
00522 Type d13 = m1.m03 * m2.m10 + m1.m13 * m2.m11 + m1.m23 * m2.m12 + m1.m33 * m2.m13;
00523 Type d14 = m1.m03 * m2.m20 + m1.m13 * m2.m21 + m1.m23 * m2.m22 + m1.m33 * m2.m23;
00524 Type d15 = m1.m03 * m2.m30 + m1.m13 * m2.m31 + m1.m23 * m2.m32 + m1.m33 * m2.m33;
00525 set(d0, d1, d2, d3,
00526 d4, d5, d6, d7,
00527 d8, d9, d10, d11,
00528 d12, d13, d14, d15);
00529 }
00530 }
00531
00532 template <class Type>
00533 void _Matrix4<Type>::mulTransposeLeft(const _Matrix4<Type>& m1, const _Matrix4<Type>& m2)
00534 {
00535 if (this != &m1 && this != &m2) {
00536 m00 = m1.m00 * m2.m00 + m1.m10 * m2.m10 + m1.m20 * m2.m20 + m1.m30 * m2.m30;
00537 m01 = m1.m00 * m2.m01 + m1.m10 * m2.m11 + m1.m20 * m2.m21 + m1.m30 * m2.m31;
00538 m02 = m1.m00 * m2.m02 + m1.m10 * m2.m12 + m1.m20 * m2.m22 + m1.m30 * m2.m32;
00539 m03 = m1.m00 * m2.m03 + m1.m10 * m2.m13 + m1.m20 * m2.m23 + m1.m30 * m2.m33;
00540 m10 = m1.m01 * m2.m00 + m1.m11 * m2.m10 + m1.m21 * m2.m20 + m1.m31 * m2.m30;
00541 m11 = m1.m01 * m2.m01 + m1.m11 * m2.m11 + m1.m21 * m2.m21 + m1.m31 * m2.m31;
00542 m12 = m1.m01 * m2.m02 + m1.m11 * m2.m12 + m1.m21 * m2.m22 + m1.m31 * m2.m32;
00543 m13 = m1.m01 * m2.m03 + m1.m11 * m2.m13 + m1.m21 * m2.m23 + m1.m31 * m2.m33;
00544 m20 = m1.m02 * m2.m00 + m1.m12 * m2.m10 + m1.m22 * m2.m20 + m1.m32 * m2.m30;
00545 m21 = m1.m02 * m2.m01 + m1.m12 * m2.m11 + m1.m22 * m2.m21 + m1.m32 * m2.m31;
00546 m22 = m1.m02 * m2.m02 + m1.m12 * m2.m12 + m1.m22 * m2.m22 + m1.m32 * m2.m32;
00547 m23 = m1.m02 * m2.m03 + m1.m12 * m2.m13 + m1.m22 * m2.m23 + m1.m32 * m2.m33;
00548 m30 = m1.m03 * m2.m00 + m1.m13 * m2.m10 + m1.m23 * m2.m20 + m1.m33 * m2.m30;
00549 m31 = m1.m03 * m2.m01 + m1.m13 * m2.m11 + m1.m23 * m2.m21 + m1.m33 * m2.m31;
00550 m32 = m1.m03 * m2.m02 + m1.m13 * m2.m12 + m1.m23 * m2.m22 + m1.m33 * m2.m32;
00551 m33 = m1.m03 * m2.m03 + m1.m13 * m2.m13 + m1.m23 * m2.m23 + m1.m33 * m2.m33;
00552 } else {
00553 Type d0 = m1.m00 * m2.m00 + m1.m10 * m2.m10 + m1.m20 * m2.m20 + m1.m30 * m2.m30;
00554 Type d1 = m1.m00 * m2.m01 + m1.m10 * m2.m11 + m1.m20 * m2.m21 + m1.m30 * m2.m31;
00555 Type d2 = m1.m00 * m2.m02 + m1.m10 * m2.m12 + m1.m20 * m2.m22 + m1.m30 * m2.m32;
00556 Type d3 = m1.m00 * m2.m03 + m1.m10 * m2.m13 + m1.m20 * m2.m23 + m1.m30 * m2.m33;
00557 Type d4 = m1.m01 * m2.m00 + m1.m11 * m2.m10 + m1.m21 * m2.m20 + m1.m31 * m2.m30;
00558 Type d5 = m1.m01 * m2.m01 + m1.m11 * m2.m11 + m1.m21 * m2.m21 + m1.m31 * m2.m31;
00559 Type d6 = m1.m01 * m2.m02 + m1.m11 * m2.m12 + m1.m21 * m2.m22 + m1.m31 * m2.m32;
00560 Type d7 = m1.m01 * m2.m03 + m1.m11 * m2.m13 + m1.m21 * m2.m23 + m1.m31 * m2.m33;
00561 Type d8 = m1.m02 * m2.m00 + m1.m12 * m2.m10 + m1.m22 * m2.m20 + m1.m32 * m2.m30;
00562 Type d9 = m1.m02 * m2.m01 + m1.m12 * m2.m11 + m1.m22 * m2.m21 + m1.m32 * m2.m31;
00563 Type d10 = m1.m02 * m2.m02 + m1.m12 * m2.m12 + m1.m22 * m2.m22 + m1.m32 * m2.m32;
00564 Type d11 = m1.m02 * m2.m03 + m1.m12 * m2.m13 + m1.m22 * m2.m23 + m1.m32 * m2.m33;
00565 Type d12 = m1.m03 * m2.m00 + m1.m13 * m2.m10 + m1.m23 * m2.m20 + m1.m33 * m2.m30;
00566 Type d13 = m1.m03 * m2.m01 + m1.m13 * m2.m11 + m1.m23 * m2.m21 + m1.m33 * m2.m31;
00567 Type d14 = m1.m03 * m2.m02 + m1.m13 * m2.m12 + m1.m23 * m2.m22 + m1.m33 * m2.m32;
00568 Type d15 = m1.m03 * m2.m03 + m1.m13 * m2.m13 + m1.m23 * m2.m23 + m1.m33 * m2.m33;
00569 set(d0, d1, d2, d3,
00570 d4, d5, d6, d7,
00571 d8, d9, d10, d11,
00572 d12, d13, d14, d15);
00573 }
00574 }
00575
00576 template <class Type>
00577 void _Matrix4<Type>::mulTransposeRight(const _Matrix4<Type>& m1, const _Matrix4<Type>& m2)
00578 {
00579 if (this != &m1 && this != &m2) {
00580 m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02 + m1.m03 * m2.m03;
00581 m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12 + m1.m03 * m2.m13;
00582 m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22 + m1.m03 * m2.m23;
00583 m03 = m1.m00 * m2.m30 + m1.m01 * m2.m31 + m1.m02 * m2.m32 + m1.m03 * m2.m33;
00584 m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02 + m1.m13 * m2.m03;
00585 m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12 + m1.m13 * m2.m13;
00586 m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22 + m1.m13 * m2.m23;
00587 m13 = m1.m10 * m2.m30 + m1.m11 * m2.m31 + m1.m12 * m2.m32 + m1.m13 * m2.m33;
00588 m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02 + m1.m23 * m2.m03;
00589 m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12 + m1.m23 * m2.m13;
00590 m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22 + m1.m23 * m2.m23;
00591 m23 = m1.m20 * m2.m30 + m1.m21 * m2.m31 + m1.m22 * m2.m32 + m1.m23 * m2.m33;
00592 m30 = m1.m30 * m2.m00 + m1.m31 * m2.m01 + m1.m32 * m2.m02 + m1.m33 * m2.m03;
00593 m31 = m1.m30 * m2.m10 + m1.m31 * m2.m11 + m1.m32 * m2.m12 + m1.m33 * m2.m13;
00594 m32 = m1.m30 * m2.m20 + m1.m31 * m2.m21 + m1.m32 * m2.m22 + m1.m33 * m2.m23;
00595 m33 = m1.m30 * m2.m30 + m1.m31 * m2.m31 + m1.m32 * m2.m32 + m1.m33 * m2.m33;
00596 } else {
00597 Type d0 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02 + m1.m03 * m2.m03;
00598 Type d1 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12 + m1.m03 * m2.m13;
00599 Type d2 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22 + m1.m03 * m2.m23;
00600 Type d3 = m1.m00 * m2.m30 + m1.m01 * m2.m31 + m1.m02 * m2.m32 + m1.m03 * m2.m33;
00601 Type d4 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02 + m1.m13 * m2.m03;
00602 Type d5 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12 + m1.m13 * m2.m13;
00603 Type d6 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22 + m1.m13 * m2.m23;
00604 Type d7 = m1.m10 * m2.m30 + m1.m11 * m2.m31 + m1.m12 * m2.m32 + m1.m13 * m2.m33;
00605 Type d8 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02 + m1.m23 * m2.m03;
00606 Type d9 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12 + m1.m23 * m2.m13;
00607 Type d10 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22 + m1.m23 * m2.m23;
00608 Type d11 = m1.m20 * m2.m30 + m1.m21 * m2.m31 + m1.m22 * m2.m32 + m1.m23 * m2.m33;
00609 Type d12 = m1.m30 * m2.m00 + m1.m31 * m2.m01 + m1.m32 * m2.m02 + m1.m33 * m2.m03;
00610 Type d13 = m1.m30 * m2.m10 + m1.m31 * m2.m11 + m1.m32 * m2.m12 + m1.m33 * m2.m13;
00611 Type d14 = m1.m30 * m2.m20 + m1.m31 * m2.m21 + m1.m32 * m2.m22 + m1.m33 * m2.m23;
00612 Type d15 = m1.m30 * m2.m30 + m1.m31 * m2.m31 + m1.m32 * m2.m32 + m1.m33 * m2.m33;
00613 set(d0, d1, d2, d3,
00614 d4, d5, d6, d7,
00615 d8, d9, d10, d11,
00616 d12, d13, d14, d15);
00617 }
00618 }
00619
00620 template <class Type>
00621 void _Matrix4<Type>::rotX(double angle)
00622 {
00623 double s = sin(angle);
00624 double c = cos(angle);
00625
00626 set(1.0, 0.0, 0.0, 0.0,
00627 0.0, c, -s, 0.0,
00628 0.0, s, c, 0.0,
00629 0.0, 0.0, 0.0, 1.0);
00630 }
00631
00632 template <class Type>
00633 void _Matrix4<Type>::rotY(double angle)
00634 {
00635 double s = sin(angle);
00636 double c = cos(angle);
00637
00638 set(c , 0.0, s, 0.0,
00639 0.0, 1.0, 0.0, 0.0,
00640 -s , 0.0, c, 0.0,
00641 0.0, 0.0, 0.0, 1.0);
00642 }
00643
00644 template <class Type>
00645 void _Matrix4<Type>::rotZ(double angle)
00646 {
00647 double s = sin(angle);
00648 double c = cos(angle);
00649
00650 set(c , -s, 0.0, 0.0,
00651 s , c, 0.0, 0.0,
00652 0.0, 0.0, 1.0, 0.0,
00653 0.0, 0.0, 0.0, 1.0);
00654 }
00655
00656
00657 template <class Type>
00658 void _Matrix4<Type>::rotationGL(double angle, const _Vector3<Type>& axisRot)
00659 {
00660 double c = cos(angle), s = sin(angle), t = 1.0 - c;
00661 _Vector3<Type> axis;
00662 axis.normalize(axisRot);
00663 set(t * axis.x * axis.x + c,
00664 t * axis.x * axis.y - s * axis.z,
00665 t * axis.x * axis.z + s * axis.y,
00666 0.0,
00667 t * axis.x * axis.y + s * axis.z,
00668 t * axis.y * axis.y + c,
00669 t * axis.y * axis.z - s * axis.x,
00670 0.0,
00671 t * axis.x * axis.z - s * axis.y,
00672 t * axis.y * axis.z + s * axis.x,
00673 t * axis.z * axis.z + c,
00674 0.0,
00675 0.0, 0.0, 0.0, 1.0);
00676 }
00677
00678 template <class Type>
00679 void _Matrix4<Type>::setColumn(int column, Type x, Type y, Type z, Type w)
00680 {
00681 switch (column) {
00682 case 0: m00 = x; m10 = y; m20 = z; m30 =w; break;
00683 case 1: m01 = x; m11 = y; m21 = z; m31 =w; break;
00684 case 2: m02 = x; m12 = y; m22 = z; m32 =w; break;
00685 case 3: m03 = x; m13 = y; m23 = z; m33 =w; break;
00686 }
00687 }
00688
00689 template <class Type>
00690 void _Matrix4<Type>::setElement(int row, int column, Type value)
00691 {
00692 if (row >= 0 && row < 4 || column >= 0 || column < 4) {
00693 operator()(row, column) = value;
00694 return;
00695 }
00696 }
00697
00698 template <class Type>
00699 void _Matrix4<Type>::setRow(int row, Type x, Type y, Type z, Type w)
00700 {
00701 switch (row) {
00702 case 0: m00 = x, m01 = y, m02 = z, m03 =w; break;
00703 case 1: m10 = x, m11 = y, m12 = z, m13 =w; break;
00704 case 2: m20 = x, m21 = y, m22 = z, m23 =w; break;
00705 case 3: m30 = x, m31 = y, m32 = z, m33 =w; break;
00706 }
00707 }
00708
00709 template <class Type>
00710 void _Matrix4<Type>::sub(const _Matrix4<Type>& m1, const _Matrix4<Type>& m2)
00711 {
00712 set(m1.m00 - m2.m00, m1.m01 - m2.m01, m1.m02 - m2.m02, m1.m03 - m2.m03,
00713 m1.m10 - m2.m10, m1.m11 - m2.m11, m1.m12 - m2.m12, m1.m13 - m2.m13,
00714 m1.m20 - m2.m20, m1.m21 - m2.m21, m1.m22 - m2.m22, m1.m23 - m2.m23,
00715 m1.m30 - m2.m30, m1.m31 - m2.m31, m1.m32 - m2.m32, m1.m33 - m2.m33);
00716 }
00717
00718 #if 0
00719 template <class Type>
00720 void _Matrix4<Type>::setRotation(const _AxisAngle4<double>& a)
00721 {
00722 Matrix3d m3(a);
00723 setRotation(m3);
00724 }
00725
00726 template <class Type>
00727 void _Matrix4<Type>::setRotation(const _AxisAngle4<float>& a)
00728 {
00729 Matrix3f m3(a);
00730 setRotation(m3);
00731 }
00732 #endif
00733
00734 template <class Type>
00735 void _Matrix4<Type>::setRotation(const _Matrix3<double>& m)
00736 {
00737 setRotationScale3x3(m.m00, m.m01, m.m02,
00738 m.m10, m.m11, m.m12,
00739 m.m20, m.m21, m.m22);
00740 }
00741
00742 template <class Type>
00743 void _Matrix4<Type>::setRotation(const _Matrix3<float>& m)
00744 {
00745 setRotationScale3x3(m.m00, m.m01, m.m02,
00746 m.m10, m.m11, m.m12,
00747 m.m20, m.m21, m.m22);
00748 }
00749
00750 template <class Type>
00751 void _Matrix4<Type>::setRotation(const _Quat4<double>& q)
00752 {
00753 Matrix3d m3;
00754 m3.set(q);
00755 setRotation(m3);
00756 }
00757
00758 template <class Type>
00759 void _Matrix4<Type>::setRotation(const _Quat4<float>& q)
00760 {
00761 Matrix3f m3;
00762 m3.set(q);
00763 setRotation(m3);
00764 }
00765
00766
00767 template <class Type>
00768 void _Matrix4<Type>::transform(const _Tuple3<double>& t, _Tuple3<double>& t1) const
00769 {
00770 double x = t.x, y = t.y;
00771
00772 t1.x = m00 * x + m01 * y + m02 * t.z + m03;
00773 t1.y = m10 * x + m11 * y + m12 * t.z + m13;
00774 t1.z = m20 * x + m21 * y + m22 * t.z + m23;
00775 }
00776
00777 template <class Type>
00778 void _Matrix4<Type>::transform(const _Tuple3<float>& t, _Tuple3<float>& t1) const
00779 {
00780 float x = t.x, y = t.y;
00781
00782 t1.x = m00 * x + m01 * y + m02 * t.z + m03;
00783 t1.y = m10 * x + m11 * y + m12 * t.z + m13;
00784 t1.z = m20 * x + m21 * y + m22 * t.z + m23;
00785 }
00786
00787
00788 template <class Type>
00789 void _Matrix4<Type>::transform(const _Tuple4<double>& t, _Tuple4<double>& t1) const
00790 {
00791 double x = t.x, y = t.y, z = t.z;
00792
00793 t1.x = m00 * x + m01 * y + m02 * z + m03 * t.w;
00794 t1.y = m10 * x + m11 * y + m12 * z + m13 * t.w;
00795 t1.z = m20 * x + m21 * y + m22 * z + m23 * t.w;
00796 t1.w = m30 * x + m31 * y + m32 * z + m33 * t.w;
00797 }
00798
00799 template <class Type>
00800 void _Matrix4<Type>::transform(const _Tuple4<float>& t, _Tuple4<float>& t1) const
00801 {
00802 float x = t.x, y = t.y, z = t.z;
00803
00804 t1.x = m00 * x + m01 * y + m02 * z + m03 * t.w;
00805 t1.y = m10 * x + m11 * y + m12 * z + m13 * t.w;
00806 t1.z = m20 * x + m21 * y + m22 * z + m23 * t.w;
00807 t1.w = m30 * x + m31 * y + m32 * z + m33 * t.w;
00808 }
00809
00810 template <class Type>
00811 void _Matrix4<Type>::transpose()
00812 {
00813 Math::swap(m10, m01);
00814 Math::swap(m20, m02);
00815 Math::swap(m30, m03);
00816 Math::swap(m12, m21);
00817 Math::swap(m13, m31);
00818 Math::swap(m23, m32);
00819 }
00820
00821 template <class Type>
00822 void _Matrix4<Type>::transpose(const _Matrix4<Type>& m)
00823 {
00824 if (this != &m) {
00825 set(m.m00, m.m10, m.m20, m.m30,
00826 m.m01, m.m11, m.m21, m.m31,
00827 m.m02, m.m12, m.m22, m.m32,
00828 m.m03, m.m13, m.m23, m.m33);
00829 } else
00830 transpose();
00831 }
00832
00833 template <class Type>
00834 std::ostream& operator <<(std::ostream& os, const _Matrix4<Type>& m)
00835 {
00836 os << "{ ";
00837 for (int i = 0; i < 4; i++) {
00838 for (int j = 0; j < 4; j++) {
00839
00840 double d = m(i, j);
00841 if (Math::abs(d) < 1e-15)
00842 d = 0.0;
00843 os << d;
00844 if (i < 3 || j < 3)
00845 os << ", ";
00846 }
00847
00848 if (i < 3)
00849 os << std::endl << " ";
00850 else
00851 os << " }" << std::endl;
00852 }
00853
00854 return os;
00855 }
00856
00857 template <class Type>
00858 std::ostream& operator <<(std::ostream& os, const _Matrix4<Type>* m)
00859 {
00860 return os << *m;
00861 }
00862
00863 template class _Matrix4<float>;
00864 template class _Matrix4<double>;
00865 template std::ostream& operator <<(std::ostream&, const _Matrix4<double>&);
00866 template std::ostream& operator <<(std::ostream&, const _Matrix4<double>*);
00867 template std::ostream& operator <<(std::ostream&, const _Matrix4<float>&);
00868 template std::ostream& operator <<(std::ostream&, const _Matrix4<float>*);
00869
