Mat3.cpp

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