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