Mat4.cpp

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