00001 /*
00002 File: Mat4.cc
00003
00004 Function: Implements Mat4.h
00005
00006 Author(s): Andrew Willmott
00007
00008 Copyright: (c) 1995-2000, Andrew Willmott
00009
00010 Notes:
00011
00012 */
00013
00014
00015 #include "vl/Mat4.h"
00016 #include <ctype.h>
00017 #include <iomanip.h>
00018
00019
00020 TMat4::TMat4(TMReal a, TMReal b, TMReal c, TMReal d,
00021 TMReal e, TMReal f, TMReal g, TMReal h,
00022 TMReal i, TMReal j, TMReal k, TMReal l,
00023 TMReal m, TMReal n, TMReal o, TMReal p)
00024 {
00025 row[0][0] = a; row[0][1] = b; row[0][2] = c; row[0][3] = d;
00026 row[1][0] = e; row[1][1] = f; row[1][2] = g; row[1][3] = h;
00027 row[2][0] = i; row[2][1] = j; row[2][2] = k; row[2][3] = l;
00028 row[3][0] = m; row[3][1] = n; row[3][2] = o; row[3][3] = p;
00029 }
00030
00031 TMat4::TMat4(const TMat4 &m)
00032 {
00033 row[0] = m[0];
00034 row[1] = m[1];
00035 row[2] = m[2];
00036 row[3] = m[3];
00037 }
00038
00039
00040 TMat4 &TMat4::operator = (const TMat4 &m)
00041 {
00042 row[0] = m[0];
00043 row[1] = m[1];
00044 row[2] = m[2];
00045 row[3] = m[3];
00046
00047 return(SELF);
00048 }
00049
00050 TMat4 &TMat4::operator += (const TMat4 &m)
00051 {
00052 row[0] += m[0];
00053 row[1] += m[1];
00054 row[2] += m[2];
00055 row[3] += m[3];
00056
00057 return(SELF);
00058 }
00059
00060 TMat4 &TMat4::operator -= (const TMat4 &m)
00061 {
00062 row[0] -= m[0];
00063 row[1] -= m[1];
00064 row[2] -= m[2];
00065 row[3] -= m[3];
00066
00067 return(SELF);
00068 }
00069
00070 TMat4 &TMat4::operator *= (const TMat4 &m)
00071 {
00072 SELF = SELF * m;
00073
00074 return(SELF);
00075 }
00076
00077 TMat4 &TMat4::operator *= (TMReal s)
00078 {
00079 row[0] *= s;
00080 row[1] *= s;
00081 row[2] *= s;
00082 row[3] *= s;
00083
00084 return(SELF);
00085 }
00086
00087 TMat4 &TMat4::operator /= (TMReal s)
00088 {
00089 row[0] /= s;
00090 row[1] /= s;
00091 row[2] /= s;
00092 row[3] /= s;
00093
00094 return(SELF);
00095 }
00096
00097
00098 Bool TMat4::operator == (const TMat4 &m) const
00099 {
00100 return(row[0] == m[0] && row[1] == m[1] && row[2] == m[2] && row[3] == m[3]);
00101 }
00102
00103 Bool TMat4::operator != (const TMat4 &m) const
00104 {
00105 return(row[0] != m[0] || row[1] != m[1] || row[2] != m[2] || row[3] != m[3]);
00106 }
00107
00108
00109 TMat4 TMat4::operator + (const TMat4 &m) const
00110 {
00111 TMat4 result;
00112
00113 result[0] = row[0] + m[0];
00114 result[1] = row[1] + m[1];
00115 result[2] = row[2] + m[2];
00116 result[3] = row[3] + m[3];
00117
00118 return(result);
00119 }
00120
00121 TMat4 TMat4::operator - (const TMat4 &m) const
00122 {
00123 TMat4 result;
00124
00125 result[0] = row[0] - m[0];
00126 result[1] = row[1] - m[1];
00127 result[2] = row[2] - m[2];
00128 result[3] = row[3] - m[3];
00129
00130 return(result);
00131 }
00132
00133 TMat4 TMat4::operator - () const
00134 {
00135 TMat4 result;
00136
00137 result[0] = -row[0];
00138 result[1] = -row[1];
00139 result[2] = -row[2];
00140 result[3] = -row[3];
00141
00142 return(result);
00143 }
00144
00145 TMat4 TMat4::operator * (const TMat4 &m) const
00146 {
00147 #define N(x,y) row[x][y]
00148 #define M(x,y) m[x][y]
00149 #define R(x,y) result[x][y]
00150
00151 TMat4 result;
00152
00153 R(0,0) = N(0,0) * M(0,0) + N(0,1) * M(1,0) + N(0,2) * M(2,0) + N(0,3) * M(3,0);
00154 R(0,1) = N(0,0) * M(0,1) + N(0,1) * M(1,1) + N(0,2) * M(2,1) + N(0,3) * M(3,1);
00155 R(0,2) = N(0,0) * M(0,2) + N(0,1) * M(1,2) + N(0,2) * M(2,2) + N(0,3) * M(3,2);
00156 R(0,3) = N(0,0) * M(0,3) + N(0,1) * M(1,3) + N(0,2) * M(2,3) + N(0,3) * M(3,3);
00157
00158 R(1,0) = N(1,0) * M(0,0) + N(1,1) * M(1,0) + N(1,2) * M(2,0) + N(1,3) * M(3,0);
00159 R(1,1) = N(1,0) * M(0,1) + N(1,1) * M(1,1) + N(1,2) * M(2,1) + N(1,3) * M(3,1);
00160 R(1,2) = N(1,0) * M(0,2) + N(1,1) * M(1,2) + N(1,2) * M(2,2) + N(1,3) * M(3,2);
00161 R(1,3) = N(1,0) * M(0,3) + N(1,1) * M(1,3) + N(1,2) * M(2,3) + N(1,3) * M(3,3);
00162
00163 R(2,0) = N(2,0) * M(0,0) + N(2,1) * M(1,0) + N(2,2) * M(2,0) + N(2,3) * M(3,0);
00164 R(2,1) = N(2,0) * M(0,1) + N(2,1) * M(1,1) + N(2,2) * M(2,1) + N(2,3) * M(3,1);
00165 R(2,2) = N(2,0) * M(0,2) + N(2,1) * M(1,2) + N(2,2) * M(2,2) + N(2,3) * M(3,2);
00166 R(2,3) = N(2,0) * M(0,3) + N(2,1) * M(1,3) + N(2,2) * M(2,3) + N(2,3) * M(3,3);
00167
00168 R(3,0) = N(3,0) * M(0,0) + N(3,1) * M(1,0) + N(3,2) * M(2,0) + N(3,3) * M(3,0);
00169 R(3,1) = N(3,0) * M(0,1) + N(3,1) * M(1,1) + N(3,2) * M(2,1) + N(3,3) * M(3,1);
00170 R(3,2) = N(3,0) * M(0,2) + N(3,1) * M(1,2) + N(3,2) * M(2,2) + N(3,3) * M(3,2);
00171 R(3,3) = N(3,0) * M(0,3) + N(3,1) * M(1,3) + N(3,2) * M(2,3) + N(3,3) * M(3,3);
00172
00173 return(result);
00174
00175 #undef N
00176 #undef M
00177 #undef R
00178 }
00179
00180 TMat4 TMat4::operator * (TMReal s) const
00181 {
00182 TMat4 result;
00183
00184 result[0] = row[0] * s;
00185 result[1] = row[1] * s;
00186 result[2] = row[2] * s;
00187 result[3] = row[3] * s;
00188
00189 return(result);
00190 }
00191
00192 TMat4 TMat4::operator / (TMReal s) const
00193 {
00194 TMat4 result;
00195
00196 result[0] = row[0] / s;
00197 result[1] = row[1] / s;
00198 result[2] = row[2] / s;
00199 result[3] = row[3] / s;
00200
00201 return(result);
00202 }
00203
00204 TMVec4 operator * (const TMat4 &m, const TMVec4 &v) // m * v
00205 {
00206 TMVec4 result;
00207
00208 result[0] = v[0] * m[0][0] + v[1] * m[0][1] + v[2] * m[0][2] + v[3] * m[0][3];
00209 result[1] = v[0] * m[1][0] + v[1] * m[1][1] + v[2] * m[1][2] + v[3] * m[1][3];
00210 result[2] = v[0] * m[2][0] + v[1] * m[2][1] + v[2] * m[2][2] + v[3] * m[2][3];
00211 result[3] = v[0] * m[3][0] + v[1] * m[3][1] + v[2] * m[3][2] + v[3] * m[3][3];
00212
00213 return(result);
00214 }
00215
00216 TMVec4 operator * (const TMVec4 &v, const TMat4 &m) // v * m
00217 {
00218 TMVec4 result;
00219
00220 result[0] = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0] + v[3] * m[3][0];
00221 result[1] = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1] + v[3] * m[3][1];
00222 result[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2] + v[3] * m[3][2];
00223 result[3] = v[0] * m[0][3] + v[1] * m[1][3] + v[2] * m[2][3] + v[3] * m[3][3];
00224
00225 return(result);
00226 }
00227
00228 TMVec4 &operator *= (TMVec4 &v, const TMat4 &m) // v *= m
00229 {
00230 TMReal t0, t1, t2;
00231
00232 t0 = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0] + v[3] * m[3][0];
00233 t1 = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1] + v[3] * m[3][1];
00234 t2 = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2] + v[3] * m[3][2];
00235 v[3] = v[0] * m[0][3] + v[1] * m[1][3] + v[2] * m[2][3] + v[3] * m[3][3];
00236 v[0] = t0;
00237 v[1] = t1;
00238 v[2] = t2;
00239
00240 return(v);
00241 }
00242
00243 TMat4 trans(const TMat4 &m)
00244 {
00245 #define M(x,y) m[x][y]
00246 #define R(x,y) result[x][y]
00247
00248 TMat4 result;
00249
00250 R(0,0) = M(0,0); R(0,1) = M(1,0); R(0,2) = M(2,0); R(0,3) = M(3,0);
00251 R(1,0) = M(0,1); R(1,1) = M(1,1); R(1,2) = M(2,1); R(1,3) = M(3,1);
00252 R(2,0) = M(0,2); R(2,1) = M(1,2); R(2,2) = M(2,2); R(2,3) = M(3,2);
00253 R(3,0) = M(0,3); R(3,1) = M(1,3); R(3,2) = M(2,3); R(3,3) = M(3,3);
00254
00255 return(result);
00256
00257 #undef M
00258 #undef R
00259 }
00260
00261 TMat4 adj(const TMat4 &m)
00262 {
00263 TMat4 result;
00264
00265 result[0] = cross(m[1], m[2], m[3]);
00266 result[1] = -cross(m[0], m[2], m[3]);
00267 result[2] = cross(m[0], m[1], m[3]);
00268 result[3] = -cross(m[0], m[1], m[2]);
00269
00270 return(result);
00271 }
00272
00273 TMReal trace(const TMat4 &m)
00274 {
00275 return(m[0][0] + m[1][1] + m[2][2] + m[3][3]);
00276 }
00277
00278 TMReal det(const TMat4 &m)
00279 {
00280 return(dot(m[0], cross(m[1], m[2], m[3])));
00281 }
00282
00283 TMat4 inv(const TMat4 &m)
00284 {
00285 TMReal mDet;
00286 TMat4 adjoint;
00287 TMat4 result;
00288
00289 adjoint = adj(m); // Find the adjoint
00290 mDet = dot(adjoint[0], m[0]);
00291
00292 Assert(mDet != 0, "(Mat4::inv) matrix is non-singular");
00293
00294 result = trans(adjoint);
00295 result /= mDet;
00296
00297 return(result);
00298 }
00299
00300 TMat4 oprod(const TMVec4 &a, const TMVec4 &b)
00301 // returns outerproduct of a and b: a * trans(b)
00302 {
00303 TMat4 result;
00304
00305 result[0] = a[0] * b;
00306 result[1] = a[1] * b;
00307 result[2] = a[2] * b;
00308 result[3] = a[3] * b;
00309
00310 return(result);
00311 }
00312
00313 Void TMat4::MakeZero()
00314 {
00315 Int i;
00316
00317 for (i = 0; i < 16; i++)
00318 ((TMReal *) row)[i] = vl_zero;
00319 }
00320
00321 Void TMat4::MakeDiag(TMReal k)
00322 {
00323 Int i, j;
00324
00325 for (i = 0; i < 4; i++)
00326 for (j = 0; j < 4; j++)
00327 if (i == j)
00328 row[i][j] = k;
00329 else
00330 row[i][j] = vl_zero;
00331 }
00332
00333 Void TMat4::MakeBlock(TMReal k)
00334 {
00335 Int i;
00336
00337 for (i = 0; i < 16; i++)
00338 ((TMReal *) row)[i] = k;
00339 }
00340
00341 ostream &operator << (ostream &s, const TMat4 &m)
00342 {
00343 Int w = s.width();
00344
00345 return(s << '[' << m[0] << endl << setw(w) << m[1] << endl
00346 << setw(w) << m[2] << endl << setw(w) << m[3] << ']' << endl);
00347 }
00348
00349 istream &operator >> (istream &s, TMat4 &m)
00350 {
00351 TMat4 result;
00352 Char c;
00353
00354 // Expected format: [[1 2 3] [4 5 6] [7 8 9]]
00355 // Each vector is a column of the matrix.
00356
00357 while (s >> c && isspace(c)) // ignore leading white space
00358 ;
00359
00360 if (c == '[')
00361 {
00362 s >> result[0] >> result[1] >> result[2] >> result[3];
00363
00364 if (!s)
00365 {
00366 cerr << "Expected number while reading matrix\n";
00367 return(s);
00368 }
00369
00370 while (s >> c && isspace(c))
00371 ;
00372
00373 if (c != ']')
00374 {
00375 s.clear(ios::failbit);
00376 cerr << "Expected ']' while reading matrix\n";
00377 return(s);
00378 }
00379 }
00380 else
00381 {
00382 s.clear(ios::failbit);
00383 cerr << "Expected '[' while reading matrix\n";
00384 return(s);
00385 }
00386
00387 m = result;
00388 return(s);
00389 }
00390
00391
00392 TMat4& TMat4::MakeHRot(const TQuaternion &q)
00393 {
00394 TMReal i2 = 2 * q[0],
00395 j2 = 2 * q[1],
00396 k2 = 2 * q[2],
00397 ij = i2 * q[1],
00398 ik = i2 * q[2],
00399 jk = j2 * q[2],
00400 ri = i2 * q[3],
00401 rj = j2 * q[3],
00402 rk = k2 * q[3];
00403
00404 MakeDiag();
00405
00406 i2 *= q[0];
00407 j2 *= q[1];
00408 k2 *= q[2];
00409
00410 #if VL_ROW_ORIENT
00411 row[0][0] = 1 - j2 - k2; row[0][1] = ij + rk ; row[0][2] = ik - rj;
00412 row[1][0] = ij - rk ; row[1][1] = 1 - i2- k2; row[1][2] = jk + ri;
00413 row[2][0] = ik + rj ; row[2][1] = jk - ri ; row[2][2] = 1 - i2 - j2;
00414 #else
00415 row[0][0] = 1 - j2 - k2; row[0][1] = ij - rk ; row[0][2] = ik + rj;
00416 row[1][0] = ij + rk ; row[1][1] = 1 - i2- k2; row[1][2] = jk - ri;
00417 row[2][0] = ik - rj ; row[2][1] = jk + ri ; row[2][2] = 1 - i2 - j2;
00418 #endif
00419
00420 return(SELF);
00421 }
00422
00423 TMat4& TMat4::MakeHRot(const TMVec3 &axis, Real theta)
00424 {
00425 TMReal s;
00426 TMVec4 q;
00427
00428 theta /= 2.0;
00429 s = sin(theta);
00430
00431 q[0] = s * axis[0];
00432 q[1] = s * axis[1];
00433 q[2] = s * axis[2];
00434 q[3] = cos(theta);
00435
00436 MakeHRot(q);
00437
00438 return(SELF);
00439 }
00440
00441 TMat4& TMat4::MakeHScale(const TMVec3 &s)
00442 {
00443 MakeDiag();
00444
00445 row[0][0] = s[0];
00446 row[1][1] = s[1];
00447 row[2][2] = s[2];
00448
00449 return(SELF);
00450 }
00451
00452 TMat4& TMat4::MakeHTrans(const TMVec3 &t)
00453 {
00454 MakeDiag();
00455
00456 #ifdef VL_ROW_ORIENT
00457 row[3][0] = t[0];
00458 row[3][1] = t[1];
00459 row[3][2] = t[2];
00460 #else
00461 row[0][3] = t[0];
00462 row[1][3] = t[1];
00463 row[2][3] = t[2];
00464 #endif
00465
00466 return(SELF);
00467 }
00468
00469 TMat4& TMat4::Transpose()
00470 {
00471 row[0][1] = row[1][0]; row[0][2] = row[2][0]; row[0][3] = row[3][0];
00472 row[1][0] = row[0][1]; row[1][2] = row[2][1]; row[1][3] = row[3][1];
00473 row[2][0] = row[0][2]; row[2][1] = row[1][2]; row[2][3] = row[3][2];
00474 row[3][0] = row[0][3]; row[3][1] = row[1][3]; row[3][2] = row[2][3];
00475
00476 return(SELF);
00477 }
00478
00479 TMat4& TMat4::AddShift(const TMVec3 &t)
00480 {
00481 #ifdef VL_ROW_ORIENT
00482 row[3][0] += t[0];
00483 row[3][1] += t[1];
00484 row[3][2] += t[2];
00485 #else
00486 row[0][3] += t[0];
00487 row[1][3] += t[1];
00488 row[2][3] += t[2];
00489 #endif
00490
00491 return(SELF);
00492 }
