Mat.cpp

00001 /*
00002     File:           Mat.cpp
00003 
00004     Function:       Implements Mat.h
00005 
00006     Author(s):      Andrew Willmott
00007 
00008     Copyright:      (c) 1995-2001, Andrew Willmott
00009 
00010 */
00011 
00012 #include "Mat.h"
00013 
00014 //#include <cctype>
00015 //#include <cstring>
00016 #include <cstdarg>
00017 //#include <iomanip>
00018 
00019 
00020 
00021 // --- Mat Constructors & Destructors -----------------------------------------
00022 
00023 
00024 Mat::Mat(Int rows, Int cols, ZeroOrOne k) : rows(rows), cols(cols)
00025 {
00026     Assert(rows > 0 && cols > 0, "(Mat) illegal matrix size");
00027 
00028     data = new Real[rows * cols];
00029 
00030     MakeDiag(k);
00031 }
00032 
00033 Mat::Mat(Int rows, Int cols, Block k) : rows(rows), cols(cols)
00034 {
00035     Assert(rows > 0 && cols > 0, "(Mat) illegal matrix size");
00036 
00037     data = new Real[rows * cols];
00038 
00039     MakeBlock(k);
00040 }
00041 
00042 Mat::Mat(Int rows, Int cols, double elt0, ...) : rows(rows), cols(cols)
00043 // The double is hardwired here because it is the only type that will work
00044 // with var args and C++ real numbers.
00045 {
00046     Assert(rows > 0 && cols > 0, "(Mat) illegal matrix size");
00047 
00048     va_list ap;
00049     Int     i, j;
00050 
00051     data = new Real[rows * cols];
00052     va_start(ap, elt0);
00053 
00054     SetReal(data[0], elt0);
00055 
00056     for (i = 1; i < cols; i++)
00057         SetReal(Elt(0, i), va_arg(ap, double));
00058 
00059     for (i = 1; i < rows; i++)
00060         for (j = 0; j < cols; j++)
00061             SetReal(Elt(i, j), va_arg(ap, double));
00062 
00063     va_end(ap);
00064 }
00065 
00066 Mat::Mat(const Mat &m) : cols(m.cols)
00067 {
00068     Assert(m.data != 0, "(Mat) Can't construct from null matrix");
00069     rows = m.Rows();
00070 
00071     UInt    elts = rows * cols;
00072 
00073     data = new Real[elts];
00074 #ifdef VL_USE_MEMCPY
00075     memcpy(data, m.data, elts * sizeof(Real));
00076 #else
00077     for (UInt i = 0; i < elts; i++)
00078         data[i] = m.data[i];
00079 #endif
00080 }
00081 
00082 Mat::Mat(const Mat2 &m) : data(m.Ref()), rows(2 | VL_REF_FLAG), cols(2)
00083 {
00084 }
00085 
00086 Mat::Mat(const Mat3 &m) : data(m.Ref()), rows(3 | VL_REF_FLAG), cols(3)
00087 {
00088 }
00089 
00090 Mat::Mat(const Mat4 &m) : data(m.Ref()), rows(4 | VL_REF_FLAG), cols(4)
00091 {
00092 }
00093 
00094 
00095 // --- Mat Assignment Operators -----------------------------------------------
00096 
00097 
00098 Mat &Mat::operator = (const Mat &m)
00099 {
00100     if (!IsRef())
00101         SetSize(m.Rows(), m.Cols());
00102     else
00103         Assert(Rows() == m.Rows(), "(Mat::=) Matrix rows don't match");
00104     for (Int i = 0; i < Rows(); i++)
00105         SELF[i] = m[i];
00106 
00107     return(SELF);
00108 }
00109 
00110 Mat &Mat::operator = (const Mat2 &m)
00111 {
00112     if (!IsRef())
00113         SetSize(m.Rows(), m.Cols());
00114     else
00115         Assert(Rows() == m.Rows(), "(Mat::=) Matrix rows don't match");
00116     for (Int i = 0; i < Rows(); i++)
00117         SELF[i] = m[i];
00118 
00119     return(SELF);
00120 }
00121 
00122 Mat &Mat::operator = (const Mat3 &m)
00123 {
00124     if (!IsRef())
00125         SetSize(m.Rows(), m.Cols());
00126     else
00127         Assert(Rows() == m.Rows(), "(Mat::=) Matrix rows don't match");
00128     for (Int i = 0; i < Rows(); i++)
00129         SELF[i] = m[i];
00130 
00131     return(SELF);
00132 }
00133 
00134 Mat &Mat::operator = (const Mat4 &m)
00135 {
00136     if (!IsRef())
00137         SetSize(m.Rows(), m.Cols());
00138     else
00139         Assert(Rows() == m.Rows(), "(Mat::=) Matrix rows don't match");
00140 
00141     for (Int i = 0; i < Rows(); i++)
00142         SELF[i] = m[i];
00143 
00144     return(SELF);
00145 }
00146 
00147 Void Mat::SetSize(Int nrows, Int ncols)
00148 {
00149     UInt    elts = nrows * ncols;
00150     Assert(nrows > 0 && ncols > 0, "(Mat::SetSize) Illegal matrix size.");
00151     UInt    oldElts = Rows() * Cols();
00152 
00153     if (IsRef())
00154     {
00155         // Abort! We don't allow this operation on references.
00156         _Error("(Mat::SetSize) Trying to resize a matrix reference");
00157     }
00158 
00159     rows = nrows;
00160     cols = ncols;
00161 
00162     // Don't reallocate if we already have enough storage
00163     if (elts <= oldElts)
00164         return;
00165 
00166     // Otherwise, delete old storage and reallocate
00167     delete[] data;
00168     data = 0;
00169     data = new Real[elts]; // may throw an exception
00170 }
00171 
00172 Void Mat::SetSize(const Mat &m)
00173 {
00174     SetSize(m.Rows(), m.Cols());
00175 }
00176 
00177 Void Mat::MakeZero()
00178 {
00179 #ifdef VL_USE_MEMCPY
00180     memset(data, 0, sizeof(Real) * Rows() * Cols());
00181 #else
00182     Int     i;
00183 
00184     for (i = 0; i < Rows(); i++)
00185         SELF[i] = vl_zero;
00186 #endif
00187 }
00188 
00189 Void Mat::MakeDiag(Real k)
00190 {
00191     Int     i, j;
00192 
00193     for (i = 0; i < Rows(); i++)
00194         for (j = 0; j < Cols(); j++)
00195             if (i == j)
00196                 Elt(i,j) = k;
00197             else
00198                 Elt(i,j) = vl_zero;
00199 }
00200 
00201 Void Mat::MakeDiag()
00202 {
00203     Int     i, j;
00204 
00205     for (i = 0; i < Rows(); i++)
00206         for (j = 0; j < Cols(); j++)
00207             Elt(i,j) = (i == j) ? vl_one : vl_zero;
00208 }
00209 
00210 Void Mat::MakeBlock(Real k)
00211 {
00212     Int     i;
00213 
00214     for (i = 0; i < Rows(); i++)
00215         SELF[i].MakeBlock(k);
00216 }
00217 
00218 Void Mat::MakeBlock()
00219 {
00220     Int     i, j;
00221 
00222     for (i = 0; i < Rows(); i++)
00223         for (j = 0; j < Cols(); j++)
00224             Elt(i,j) = vl_one;
00225 }
00226 
00227 
00228 // --- Mat Assignment Operators -----------------------------------------------
00229 
00230 
00231 Mat &Mat::operator += (const Mat &m)
00232 {
00233     Assert(Rows() == m.Rows(), "(Mat::+=) matrix rows don't match");
00234 
00235     Int     i;
00236 
00237     for (i = 0; i < Rows(); i++)
00238         SELF[i] += m[i];
00239 
00240     return(SELF);
00241 }
00242 
00243 Mat &Mat::operator -= (const Mat &m)
00244 {
00245     Assert(Rows() == m.Rows(), "(Mat::-=) matrix rows don't match");
00246 
00247     Int     i;
00248 
00249     for (i = 0; i < Rows(); i++)
00250         SELF[i] -= m[i];
00251 
00252     return(SELF);
00253 }
00254 
00255 Mat &Mat::operator *= (const Mat &m)
00256 {
00257     Assert(Cols() == m.Cols(), "(Mat::*=) matrix columns don't match");
00258 
00259     Int     i;
00260 
00261     for (i = 0; i < Rows(); i++)
00262         SELF[i] = SELF[i] * m;
00263 
00264     return(SELF);
00265 }
00266 
00267 Mat &Mat::operator *= (Real s)
00268 {
00269     Int     i;
00270 
00271     for (i = 0; i < Rows(); i++)
00272         SELF[i] *= s;
00273 
00274     return(SELF);
00275 }
00276 
00277 Mat &Mat::operator /= (Real s)
00278 {
00279     Int     i;
00280 
00281     for (i = 0; i < Rows(); i++)
00282         SELF[i] /= s;
00283 
00284     return(SELF);
00285 }
00286 
00287 
00288 // --- Mat Comparison Operators -----------------------------------------------
00289 
00290 
00291 Bool operator == (const Mat &m, const Mat &n)
00292 {
00293     Assert(n.Rows() == m.Rows(), "(Mat::==) matrix rows don't match");
00294 
00295     Int     i;
00296 
00297     for (i = 0; i < m.Rows(); i++)
00298         if (m[i] != n[i])
00299             return(0);
00300 
00301     return(1);
00302 }
00303 
00304 Bool operator != (const Mat &m, const Mat &n)
00305 {
00306     Assert(n.Rows() == m.Rows(), "(Mat::!=) matrix rows don't match");
00307 
00308     Int     i;
00309 
00310     for (i = 0; i < m.Rows(); i++)
00311         if (m[i] != n[i])
00312             return(1);
00313 
00314     return(0);
00315 }
00316 
00317 
00318 // --- Mat Arithmetic Operators -----------------------------------------------
00319 
00320 
00321 Mat operator + (const Mat &m, const Mat &n)
00322 {
00323     Assert(n.Rows() == m.Rows(), "(Mat::+) matrix rows don't match");
00324 
00325     Mat result(m.Rows(), m.Cols());
00326     Int     i;
00327 
00328     for (i = 0; i < m.Rows(); i++)
00329         result[i] = m[i] + n[i];
00330 
00331     return(result);
00332 }
00333 
00334 Mat operator - (const Mat &m, const Mat &n)
00335 {
00336     Assert(n.Rows() == m.Rows(), "(Mat::-) matrix rows don't match");
00337 
00338     Mat result(m.Rows(), m.Cols());
00339     Int     i;
00340 
00341     for (i = 0; i < m.Rows(); i++)
00342         result[i] = m[i] - n[i];
00343 
00344     return(result);
00345 }
00346 
00347 Mat operator - (const Mat &m)
00348 {
00349     Mat result(m.Rows(), m.Cols());
00350     Int     i;
00351 
00352     for (i = 0; i < m.Rows(); i++)
00353         result[i] = -m[i];
00354 
00355     return(result);
00356 }
00357 
00358 Mat operator * (const Mat &m, const Mat &n)
00359 {
00360     Assert(m.Cols() == n.Rows(), "(Mat::*m) matrix cols don't match");
00361 
00362     Mat result(m.Rows(), n.Cols());
00363     Int     i;
00364 
00365     for (i = 0; i < m.Rows(); i++)
00366         result[i] = m[i] * n;
00367 
00368     return(result);
00369 }
00370 
00371 Vec operator * (const Mat &m, const Vec &v)
00372 {
00373     Assert(m.Cols() == v.Elts(), "(Mat::*v) matrix and vector sizes don't match");
00374 
00375     Int     i;
00376     Vec result(m.Rows());
00377 
00378     for (i = 0; i < m.Rows(); i++)
00379         result[i] = dot(v, m[i]);
00380 
00381     return(result);
00382 }
00383 
00384 Mat operator * (const Mat &m, Real s)
00385 {
00386     Int     i;
00387     Mat result(m.Rows(), m.Cols());
00388 
00389     for (i = 0; i < m.Rows(); i++)
00390         result[i] = m[i] * s;
00391 
00392     return(result);
00393 }
00394 
00395 Mat operator / (const Mat &m, Real s)
00396 {
00397     Int     i;
00398     Mat result(m.Rows(), m.Cols());
00399 
00400     for (i = 0; i < m.Rows(); i++)
00401         result[i] = m[i] / s;
00402 
00403     return(result);
00404 }
00405 
00406 
00407 // --- Mat Mat-Vec Functions --------------------------------------------------
00408 
00409 
00410 Vec operator * (const Vec &v, const Mat &m)         // v * m
00411 {
00412     Assert(v.Elts() == m.Rows(), "(Mat::v*m) vector/matrix sizes don't match");
00413 
00414     Vec     result(m.Cols(), vl_zero);
00415     Int     i;
00416 
00417     for (i = 0; i < m.Rows(); i++)
00418         result += m[i] * v[i];
00419 
00420     return(result);
00421 }
00422 
00423 
00424 // --- Mat Special Functions --------------------------------------------------
00425 
00426 
00427 Mat trans(const Mat &m)
00428 {
00429     Int     i,j;
00430     Mat result(m.Cols(), m.Rows());
00431 
00432     for (i = 0; i < m.Rows(); i++)
00433         for (j = 0; j < m.Cols(); j++)
00434             result.Elt(j,i) = m.Elt(i,j);
00435 
00436     return(result);
00437 }
00438 
00439 Real trace(const Mat &m)
00440 {
00441     Int     i;
00442     Real    result = vl_0;
00443 
00444     for (i = 0; i < m.Rows(); i++)
00445         result += m.Elt(i,i);
00446 
00447     return(result);
00448 }
00449 
00450 Mat &Mat::Clamp(Real fuzz)
00451 //  clamps all values of the matrix with a magnitude
00452 //  smaller than fuzz to zero.
00453 {
00454     Int i;
00455 
00456     for (i = 0; i < Rows(); i++)
00457         SELF[i].Clamp(fuzz);
00458 
00459     return(SELF);
00460 }
00461 
00462 Mat &Mat::Clamp()
00463 {
00464     return(Clamp(1e-7));
00465 }
00466 
00467 Mat clamped(const Mat &m, Real fuzz)
00468 //  clamps all values of the matrix with a magnitude
00469 //  smaller than fuzz to zero.
00470 {
00471     Mat result(m);
00472 
00473     return(result.Clamp(fuzz));
00474 }
00475 
00476 Mat clamped(const Mat &m)
00477 {
00478     return(clamped(m, 1e-7));
00479 }
00480 
00481 Mat oprod(const Vec &a, const Vec &b)
00482 // returns outerproduct of a and b:  a * trans(b)
00483 {
00484     Mat result;
00485     Int     i;
00486 
00487     result.SetSize(a.Elts(), b.Elts());
00488     for (i = 0; i < a.Elts(); i++)
00489         result[i] = a[i] * b;
00490 
00491     return(result);
00492 }
00493 
00494 
00495 // --- Mat Input & Output -----------------------------------------------------
00496 
00497 void printMat(const Mat &m)
00498 {
00499     Int i;
00500     
00501     printf("[");
00502     for (i = 0; i < m.Rows() - 1; i++){
00503         printVec(m[i]);
00504         printf("\r\n");
00505     }
00506     printVec(m[i]);
00507     printf("]\r\n");
00508 }
00509     
00510 /*
00511 ostream &operator << (ostream &s, const Mat &m)
00512 {
00513     Int i, w = s.width();
00514 
00515     s << '[';
00516     for (i = 0; i < m.Rows() - 1; i++)
00517         s << setw(w) << m[i] << "\r\n";
00518     s << setw(w) << m[i] << ']' << "\r\n";
00519     return(s);
00520 }
00521 */
00522 
00523 inline Void CopyPartialMat(const Mat &m, Mat &n, Int numRows)
00524 {
00525     for (Int i = 0; i < numRows; i++)
00526         n[i] = m[i];
00527 }
00528 
00529 /*
00530 istream &operator >> (istream &s, Mat &m)
00531 {
00532     Int     size = 1;
00533     Char    c;
00534     Mat     inMat;
00535 
00536     //  Expected format: [row0 row1 row2 ...]
00537 
00538     while (isspace(s.peek()))           //  chomp white space
00539         s.get(c);
00540 
00541     if (s.peek() == '[')
00542     {
00543         Vec     row;
00544 
00545         s.get(c);
00546 
00547         s >> row;
00548         inMat.SetSize(2 * row.Elts(), row.Elts());
00549         inMat[0] = row;
00550 
00551         while (isspace(s.peek()))       //  chomp white space
00552             s.get(c);
00553 
00554         while (s.peek() != ']')         //  resize if needed
00555         {
00556             if (size == inMat.Rows())
00557             {
00558                 Mat     holdMat(inMat);
00559 
00560                 inMat.SetSize(size * 2, inMat.Cols());
00561                 CopyPartialMat(holdMat, inMat, size);
00562             }
00563 
00564             s >> row;           //  read a row
00565             inMat[size++] = row;
00566 
00567             if (!s)
00568             {
00569                 _Warning("Couldn't read matrix row");
00570                 return(s);
00571             }
00572 
00573             while (isspace(s.peek()))   //  chomp white space
00574                 s.get(c);
00575         }
00576         s.get(c);
00577     }
00578     else
00579     {
00580         s.clear(ios::failbit);
00581         _Warning("Error: Expected '[' while reading matrix");
00582         return(s);
00583     }
00584 
00585     m.SetSize(size, inMat.Cols());
00586     CopyPartialMat(inMat, m, size);
00587 
00588     return(s);
00589 }
00590 */
00591 
00592 // --- Matrix Inversion -------------------------------------------------------
00593 
00594 #if !defined(CL_CHECKING) && !defined(VL_CHECKING)
00595 // we #define away pAssertEps if it is not used, to avoid
00596 // compiler warnings.
00597 #define pAssertEps
00598 #endif
00599 
00600 Mat inv(const Mat &m, Real *determinant, Real pAssertEps)
00601 // matrix inversion using Gaussian pivoting
00602 {
00603     Assert(m.IsSquare(), "(inv) Matrix not square");
00604 
00605     Int             i, j, k;
00606     Int             n = m.Rows();
00607     Real            t, pivot, det;
00608     Real            max;
00609     Mat         A(m);
00610     Mat         B(n, n, vl_I);
00611 
00612     // ---------- Forward elimination ---------- ------------------------------
00613 
00614     det = vl_1;
00615 
00616     for (i = 0; i < n; i++)             // Eliminate in column i, below diag
00617     {
00618         max = -1.0;
00619 
00620         for (k = i; k < n; k++)         // Find a pivot for column i
00621             if (len(A[k][i]) > max)
00622             {
00623                 max = len(A[k][i]);
00624                 j = k;
00625             }
00626 
00627         Assert(max > pAssertEps, "(inv) Matrix not invertible");
00628 
00629         if (j != i)                     // Swap rows i and j
00630         {
00631             for (k = i; k < n; k++)
00632                 Swap(A.Elt(i, k), A.Elt(j, k));
00633             for (k = 0; k < n; k++)
00634                 Swap(B.Elt(i, k), B.Elt(j, k));
00635 
00636             det = -det;
00637         }
00638 
00639         pivot = A.Elt(i, i);
00640         Assert(abs(pivot) > pAssertEps, "(inv) Matrix not invertible");
00641         det *= pivot;
00642 
00643         for (k = i + 1; k < n; k++)     // Only do elements to the right of the pivot
00644             A.Elt(i, k) /= pivot;
00645 
00646         for (k = 0; k < n; k++)
00647             B.Elt(i, k) /= pivot;
00648 
00649         // We know that A(i, i) will be set to 1, so don't bother to do it
00650 
00651         for (j = i + 1; j < n; j++)
00652         {                               // Eliminate in rows below i
00653             t = A.Elt(j, i);            // We're gonna zero this guy
00654             for (k = i + 1; k < n; k++) // Subtract scaled row i from row j
00655                 A.Elt(j, k) -= A.Elt(i, k) * t; // (Ignore k <= i, we know they're 0)
00656             for (k = 0; k < n; k++)
00657                 B.Elt(j, k) -= B.Elt(i, k) * t;
00658         }
00659     }
00660 
00661     // ---------- Backward elimination ---------- -----------------------------
00662 
00663     for (i = n - 1; i > 0; i--)         // Eliminate in column i, above diag
00664     {
00665         for (j = 0; j < i; j++)         // Eliminate in rows above i
00666         {
00667             t = A.Elt(j, i);            // We're gonna zero this guy
00668             for (k = 0; k < n; k++)     // Subtract scaled row i from row j
00669                 B.Elt(j, k) -= B.Elt(i, k) * t;
00670         }
00671     }
00672     if (determinant)
00673         *determinant = det;
00674     return(B);
00675 }
00676