tmatrix.h

00001 #ifndef TMATRIX_H
00002 #define TMATRIX_H
00003 
00004 /** 
00005  * @file tmatrix.h
00006  *
00007  * @brief A dimension-templatized class for matrices of values.
00008  */
00009 #include <cmath>
00010 #include <mbed.h>
00011 #include <Stream.h>
00012 
00013 // Structures for static assert.  http://www.boost.org
00014 template <bool x> struct STATIC_ASSERTION_FAILURE;
00015 template <> struct STATIC_ASSERTION_FAILURE<true> { enum { value = 1 }; };
00016 template<int x> struct static_assert_test{};
00017 #define STATIC_ASSERT( B ) \
00018   typedef __attribute__((unused)) static_assert_test<sizeof(STATIC_ASSERTION_FAILURE<(bool)(B)>) > \
00019   static_assert_typedef##__LINE__
00020 
00021 #if DEBUG
00022 #define ERROR_CHECK(X) (X) 
00023 #else
00024 #define ERROR_CHECK(X)
00025 #endif
00026 
00027 // Forward Decl.
00028 template <uint16_t, uint16_t, typename> class BasicMatrix;
00029 template <uint16_t, uint16_t, typename> class TMatrix;
00030 class TMatrixDummy { };
00031 
00032 /**
00033  * @brief Class that layers on operator[] functionality for
00034  * typical matrices.
00035  */
00036 template <uint16_t Rows, uint16_t Cols, typename value_type>
00037 class BasicIndexMatrix : public BasicMatrix<Rows,Cols,value_type> {
00038     typedef BasicMatrix<Rows,Cols,value_type> BaseType;
00039 protected:
00040     BasicIndexMatrix(TMatrixDummy d) : BaseType(d) { }
00041 public:
00042     BasicIndexMatrix() { }
00043     BasicIndexMatrix(const value_type* data) : BaseType(data) {}
00044 
00045     const value_type* operator[](uint16_t r) const {
00046         ERROR_CHECK(if (r >= Rows) {
00047             std::clog << "Invalid row index " << r << std::endl;
00048             return &BaseType::mData[0];
00049         }
00050         )
00051         return &BaseType::mData[r*Cols];
00052     }
00053 
00054     value_type* operator[](uint16_t r) {
00055         ERROR_CHECK(if (r >= Rows) {
00056             std::clog << "Invalid row index " << r << std::endl;
00057             return &BaseType::mData[0];
00058         }
00059         )
00060         return &BaseType::mData[r*Cols];
00061     }
00062 };
00063 
00064 /**
00065  * @brief Specialization of BasicIndexMatrix that provides
00066  * single-indexing operator for column vectors.
00067  */
00068 template <uint16_t Rows, typename value_type>
00069 class BasicIndexMatrix<Rows,1,value_type> :
00070         public BasicMatrix<Rows,1,value_type> {
00071     typedef BasicMatrix<Rows,1,value_type>  BaseType ;
00072 protected:
00073     BasicIndexMatrix(TMatrixDummy dummy) : BaseType (dummy) {}
00074 public:
00075     BasicIndexMatrix() { }
00076     BasicIndexMatrix(const value_type* data) : BaseType (data) {}
00077 
00078     value_type operator[](uint16_t r) const {
00079         ERROR_CHECK(if (r >= Rows) {
00080             std::clog << "Invalid vector index " << r << std::endl;
00081             return BaseType::mData[0];
00082         })
00083         return BaseType::mData[r];
00084     }
00085     value_type& operator[](uint16_t r) {
00086         ERROR_CHECK(if (r >= Rows) {
00087             std::clog << "Invalid vector index " << r << std::endl;
00088             return BaseType::mData[0];
00089         })
00090         return BaseType::mData[r];
00091     }
00092 
00093     value_type norm() const {
00094         return sqrt(norm2());
00095     }
00096 
00097     value_type norm2() const {
00098         double normSum = 0;
00099         for (uint32_t i = 0; i < Rows; i++) {
00100             normSum += BaseType::mData[i]*BaseType::mData[i];
00101         }
00102         return normSum;
00103     }
00104 
00105     /** @brief Returns matrix with vector elements on diagonal. */
00106     TMatrix<Rows, Rows, value_type> diag(void) const {
00107         TMatrix<Rows, Rows, value_type> d;
00108         for (uint32_t i = 0; i < Rows; i++) d.element(i,i, BaseType::mData[i]);
00109         return d;
00110     }
00111 
00112 };
00113 
00114 /**
00115  * @brief A dimension-templatized class for matrices of values.
00116  *
00117  * This template class generically supports any constant-sized
00118  * matrix of values.  The @p Rows and @p Cols template parameters
00119  * define the size of the matrix at @e compile-time.  Hence, the
00120  * size of the matrix cannot be chosen at runtime.  However, the
00121  * dimensions are appropriately type-checked at compile-time where
00122  * possible.
00123  *
00124  * By default, the matrix contains values of type @p double.  The @p
00125  * value_type template parameter may be selected to allow matrices
00126  * of integers or floats.
00127  *
00128  * @note At present, type cohersion between matrices with different
00129  * @p value_type parameters is not implemented.  It is recommended
00130  * that matrices with value type @p double be used for all numerical
00131  * computation.
00132  *
00133  * Note that the special cases of row and column vectors are
00134  * subsumed by this class.
00135  */
00136 template <uint16_t Rows, uint16_t Cols, typename value_type = double>
00137 class TMatrix : public BasicIndexMatrix<Rows, Cols, value_type> {
00138     typedef BasicIndexMatrix<Rows, Cols, value_type>  BaseType;
00139     template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix;
00140     TMatrix(TMatrixDummy d) : BaseType(d) {}
00141 
00142 public:
00143     TMatrix() : BaseType() { }
00144     TMatrix(const value_type* data) : BaseType(data) {}
00145 };
00146 
00147 /**
00148  * @brief Template specialization of TMatrix for Vector4.
00149  */
00150 template <typename value_type>
00151 class TMatrix<4,1, value_type> : public BasicIndexMatrix<4,1, value_type> {
00152     typedef BasicIndexMatrix<4, 1, value_type>   BaseType ;
00153     template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix;
00154     TMatrix(TMatrixDummy d) : BaseType (d) {}
00155 
00156 public:
00157     TMatrix() { }
00158     TMatrix(const value_type* data) : BaseType (data) {}
00159     TMatrix(value_type a0, value_type a1, value_type a2, value_type a3) : BaseType (TMatrixDummy()) {
00160         BaseType::mData[0] = a0;
00161         BaseType::mData[1] = a1;
00162         BaseType::mData[2] = a2;
00163         BaseType::mData[3] = a3;
00164     }
00165 };
00166 
00167 /**
00168  * @brief Template specialization of TMatrix for Vector3.
00169  */
00170 template <typename value_type>
00171 class TMatrix<3,1, value_type> : public BasicIndexMatrix<3,1, value_type> {
00172     typedef BasicIndexMatrix<3, 1, value_type>   BaseType ;
00173     template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix;
00174     TMatrix(TMatrixDummy d) : BaseType (d) {}
00175 
00176 public:
00177     TMatrix() { }
00178     TMatrix(const value_type* data) : BaseType (data) {}
00179     TMatrix(value_type a0, value_type a1, value_type a2) : BaseType (TMatrixDummy()) {
00180         BaseType::mData[0] = a0;
00181         BaseType::mData[1] = a1;
00182         BaseType::mData[2] = a2;
00183     }
00184 
00185     TMatrix<3,1,value_type> cross(const TMatrix<3,1, value_type>& v) const {
00186         const TMatrix<3,1,value_type>& u = *this;
00187         return TMatrix<3,1,value_type>(u[1]*v[2]-u[2]*v[1],
00188                                        u[2]*v[0]-u[0]*v[2],
00189                                        u[0]*v[1]-u[1]*v[0]);
00190     }
00191 };
00192 
00193 /**
00194  * @brief Template specialization of TMatrix for Vector2.
00195  */
00196 template <typename value_type>
00197 class TMatrix<2,1, value_type> : public BasicIndexMatrix<2,1, value_type> {
00198     typedef BasicIndexMatrix<2, 1, value_type>   BaseType ;
00199     template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix;
00200     TMatrix(TMatrixDummy d) : BaseType (d) {}
00201 
00202 public:
00203     TMatrix() { }
00204     TMatrix(const value_type* data) : BaseType (data) {}
00205     TMatrix(value_type a0, value_type a1) : BaseType (TMatrixDummy()) {
00206         BaseType::mData[0] = a0;
00207         BaseType::mData[1] = a1;
00208     }
00209 };
00210 
00211 /**
00212  * @brief Template specialization of TMatrix for a 1x1 vector.
00213  */
00214 template <typename value_type>
00215 class TMatrix<1,1, value_type> : public BasicIndexMatrix<1,1, value_type> {
00216     typedef BasicIndexMatrix<1, 1, value_type>   BaseType ;
00217     template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix;
00218     TMatrix(TMatrixDummy dummy) : BaseType (dummy) {}
00219 
00220 public:
00221     TMatrix() { }
00222     TMatrix(const value_type* data) : BaseType (data) {}
00223 
00224     // explicit conversion from value_type
00225     explicit TMatrix(value_type a0) : BaseType (TMatrixDummy()) { // don't initialize
00226         BaseType::mData[0] = a0;
00227     }
00228 
00229     // implicit conversion to value_type
00230     operator value_type() const {
00231         return BaseType::mData[0];
00232     }
00233 
00234     const TMatrix<1,1, value_type>&
00235     operator=(const TMatrix<1,1, value_type>& m) {
00236         BaseType::operator=(m);
00237         return *this;
00238     }
00239 
00240     double operator=(double a0) {
00241         BaseType::mData[0] = a0;
00242         return BaseType::mData[0];
00243     }
00244 };
00245 
00246 
00247 /**
00248  * @brief Base class implementing standard matrix functionality.
00249  */
00250 template <uint16_t Rows, uint16_t Cols, typename value_type = double>
00251 class BasicMatrix {
00252 protected:
00253     value_type mData[Rows*Cols];
00254 
00255     // Constructs uninitialized matrix.
00256     BasicMatrix(TMatrixDummy dummy) {}
00257 public:
00258     BasicMatrix() { // constructs zero matrix
00259         for (uint16_t i = 0; i < Rows*Cols; ++i) {
00260             mData[i] = 0;
00261         }
00262     }
00263     BasicMatrix(const value_type* data) { // constructs from array
00264         MBED_ASSERT(data);
00265 
00266         for (uint16_t i = 0; i < Rows*Cols; ++i) {
00267             mData[i] = data[i];
00268         }
00269     }
00270 
00271     uint32_t rows() const { return Rows; }
00272     uint32_t columns() const { return Cols; }
00273     uint32_t elementCount() const { return Cols*Rows; }
00274 
00275     value_type element(uint16_t row, uint16_t col) const {
00276         ERROR_CHECK(if (row >= rows() || col >= columns()) {
00277             std::cerr << "Illegal read access: " << row << ", " << col
00278                       << " in " << Rows << "x" << Cols << " matrix." << std::endl;
00279             return mData[0];
00280         })
00281         return mData[row*Cols+col];
00282     }
00283 
00284     value_type& element(uint16_t row, uint16_t col) {
00285         ERROR_CHECK(if (row >= rows() || col >= columns()) {
00286             std::cerr << "Illegal read access: " << row << ", " << col
00287                       << " in " << Rows << "x" << Cols << " matrix." << std::endl;
00288             return mData[0];
00289         })
00290         return mData[row*Cols+col];
00291     }
00292 
00293     void element(uint16_t row, uint16_t col, value_type value) {
00294         ERROR_CHECK(if (row >= rows() || col >= columns()) {
00295             std::cerr << "Illegal write access: " << row << ", " << col
00296                       << " in " << Rows << "x" << Cols << " matrix." << std::endl;
00297             return ;
00298         })
00299         mData[row*Cols+col] = value;
00300     }
00301 
00302     TMatrix<Rows*Cols,1, value_type> vec() const {
00303         return TMatrix<Rows*Cols,1, value_type>(mData);
00304     }
00305 
00306     void vec(const TMatrix<Rows*Cols, 1, value_type>& vector) {
00307         for (uint32_t i = 0; i < Rows*Cols; i++) {
00308             mData[i] = vector.mData[i];
00309         }
00310     }
00311 
00312     template <uint16_t R, uint16_t C, uint16_t RowRangeSize, uint16_t ColRangeSize>
00313     TMatrix<RowRangeSize, ColRangeSize, value_type> subMatrix(void) const {
00314         STATIC_ASSERT((R+RowRangeSize <= Rows) &&
00315                       (C+ColRangeSize <= Cols));
00316         TMatrix<RowRangeSize, ColRangeSize, value_type> result;
00317         for (uint32_t i = 0; i < RowRangeSize; i++) {
00318             for (uint32_t j = 0; j < ColRangeSize; j++) {
00319                 result.element(i,j, element(i+R, j+C));
00320             }
00321         }
00322         return result;
00323     }
00324 
00325 
00326     template <uint16_t R, uint16_t C, uint16_t RowRangeSize, uint16_t ColRangeSize>
00327     void subMatrix(const TMatrix<RowRangeSize, ColRangeSize, value_type>& m) {
00328         STATIC_ASSERT((R+RowRangeSize <= Rows) &&
00329                       (C+ColRangeSize <= Cols));
00330         for (uint32_t i = 0; i < RowRangeSize; i++) {
00331             for (uint32_t j = 0; j < ColRangeSize; j++) {
00332                 element(i+R,j+C, m.element(i, j));
00333             }
00334         }
00335     }
00336 
00337     /**
00338      * @brief Matrix multiplication operator.
00339      * @return A matrix where result is the matrix product
00340      * (*this) * rhs.
00341      */
00342     template <uint16_t RhsCols>
00343     TMatrix<Rows, RhsCols, value_type> operator*(const TMatrix<Cols, RhsCols, value_type>& rhs) const {
00344 
00345         TMatrix<Rows, RhsCols, value_type> result;
00346         const value_type* rPtr = rhs.row(0);
00347         for (uint32_t i = 0; i < Rows; i++)
00348         {
00349             const value_type* rL = row(i);
00350             const value_type* cR = rPtr;
00351             value_type* resultRow = result.row(i);
00352             for (uint32_t j = 0; j < RhsCols; j++)
00353             {
00354                 const value_type* rR = cR; // start at first element of right col
00355                 const value_type* cL = rL; // start at first element of left row
00356                 double r = 0;
00357                 for (uint32_t k = 0; k < Cols; k++)
00358                 {
00359                     r += (*cL)*(*rR);
00360                     cL++; // step to next col of left matrix
00361                     rR += Cols; // step to next row of right matrix
00362                 }
00363                 resultRow[j] = r;
00364                 cR++; // step to next column of right matrix
00365             }
00366         }
00367         return result;
00368     }
00369 
00370     /**
00371      * @brief Element-wise addition operator.
00372      * @return A matrix where result(i,j) = (*this)(i,j) + rhs(i,j).
00373      */
00374     TMatrix<Rows, Cols, value_type> operator+(const TMatrix<Rows, Cols, value_type>& rhs) const {
00375         TMatrixDummy dummy;
00376         TMatrix<Rows, Cols, value_type> result(dummy);
00377         for (uint32_t i = 0;  i < Rows*Cols;  i++) {
00378             result.mData[i] = mData[i] + rhs.mData[i];
00379         }
00380         return result;
00381     }
00382 
00383     /**
00384      * @brief Element-wise subtraction operator.
00385      * @return A matrix where result(i,j) = (*this)(i,j) - rhs(i,j).
00386      */
00387     TMatrix<Rows, Cols, value_type> operator-(const TMatrix<Rows, Cols, value_type>& rhs) const {
00388         TMatrixDummy dummy;
00389         TMatrix<Rows, Cols, value_type> result(dummy);
00390         for (uint32_t i = 0;  i < Rows*Cols;  i++) {
00391             result.mData[i] = mData[i] - rhs.mData[i];
00392         }
00393         return result;
00394     }
00395 
00396     /**
00397      * @brief Scalar multiplication operator.
00398      * @return A matrix where result(i,j) = (*this)(i,j) * s.
00399      */
00400     TMatrix<Rows, Cols, value_type> operator*(value_type s) const {
00401         TMatrixDummy dummy;
00402         TMatrix<Rows, Cols, value_type> result(dummy);
00403         for (uint32_t i = 0;  i < Rows*Cols;  i++) {
00404             result.mData[i] = mData[i] * s;
00405         }
00406         return result;
00407     }
00408 
00409     /**
00410      * @brief Scalar division operator.
00411      * @return A matrix where result(i,j) = (*this)(i,j) / s.
00412      */
00413     TMatrix<Rows, Cols, value_type> operator/(value_type s) const {
00414         TMatrixDummy dummy;
00415         TMatrix<Rows, Cols, value_type> result(dummy);
00416         for (uint32_t i = 0;  i < Rows*Cols;  i++) {
00417             result.mData[i] = mData[i] / s;
00418         }
00419         return result;
00420     }
00421 
00422     /**
00423      * @brief Unary negation operator.
00424      * @return A matrix where result(i,j) = -(*this)(i,j).
00425      */
00426     TMatrix<Rows, Cols, value_type> operator-(void) const {
00427         TMatrixDummy dummy;
00428         TMatrix<Rows, Cols, value_type> result(dummy);
00429         for (uint32_t i = 0;  i < Rows*Cols;  i++) {
00430             result.mData[i] = -mData[i];
00431         }
00432         return result;
00433     }
00434 
00435     /**
00436      * @brief Returns the matrix transpose of this matrix.
00437      *
00438      * @return A TMatrix of dimension @p Cols by @p Rows where
00439      * result(i,j) = (*this)(j,i) for each element.
00440      */
00441     TMatrix<Cols, Rows, value_type> transpose(void) const {
00442         TMatrixDummy dummy;
00443         TMatrix<Cols, Rows, value_type> result(dummy);
00444         for (uint16_t i = 0;  i < Rows;  i++) {
00445             for (uint16_t j = 0;  j < Cols;  j++) {
00446                 result.element(j,i, element(i,j));
00447             }
00448         }
00449         return result;
00450     }
00451 
00452     /**
00453      * @brief Returns the diagonal elements of the matrix.
00454      *
00455      * @return A column vector @p v with dimension MIN(Rows,Cols) where
00456      * @p v[i] = (*this)[i][i].
00457      */
00458     TMatrix<(Rows>Cols)?Cols:Rows, 1, value_type> diag() const {
00459         TMatrixDummy dummy;
00460         TMatrix<(Rows>Cols)?Cols:Rows, 1> d(dummy);
00461         for (uint32_t i = 0; i < d.rows(); i++) {
00462             d[i] = mData[i*(Cols + 1)];
00463         }
00464         return d;
00465     }
00466 
00467     /** @brief Returns the sum of the matrix entries. */
00468     value_type sum(void) const {
00469         value_type s = 0;
00470         for (uint32_t i = 0; i < Rows*Cols; i++) { s += mData[i]; }
00471         return s;
00472     }
00473     /** @brief Returns the sum of the log of the matrix entries.
00474      */
00475     value_type sumLog(void) const {
00476         value_type s = 0;
00477         for (uint32_t i = 0; i < Rows*Cols; i++) { s += log(mData[i]); }
00478         return s;
00479     }
00480 
00481     /** @brief Returns this vector with its elements replaced by their reciprocals. */
00482     TMatrix<Rows,Cols, value_type> recip(void) const {
00483         TMatrixDummy dummy;
00484         TMatrix<Rows,Cols, value_type> result(dummy);
00485         for (uint32_t i = 0; i < Rows*Cols; i++) {
00486             result.mData[i] = 1.0/mData[i];
00487         }
00488         return result;
00489     }
00490 
00491     /** @brief Returns this vector with its elements replaced by their reciprocals,
00492      * unless a value is less than epsilon, in which case it is left as zero.
00493      *
00494      * This is used mostly for pseudo-inverse computations.
00495      */
00496     TMatrix<Rows,Cols, value_type> pseudoRecip(double epsilon = 1e-50) const {
00497         TMatrixDummy dummy;
00498         TMatrix<Rows,Cols, value_type> result(dummy);
00499         for (uint32_t i = 0; i < Rows*Cols; i++) {
00500             if (fabs(mData[i]) >= epsilon) {
00501                 result.mData[i] = 1.0/mData[i];
00502             } else {
00503                 result.mData[i] = 0;
00504             }
00505         }
00506         return result;
00507     }
00508 
00509     /**
00510      * @brief Returns an "identity" matrix with dimensions given by the
00511      * class's template parameters.
00512      *
00513      * In the case that @p Rows != @p Cols, this matrix is simply the
00514      * one where the Aii elements for i < min(Rows, Cols) are 1, and all
00515      * other elements are 0.
00516      *
00517      * @return A TMatrix<Rows, Cols, value_type> with off-diagonal
00518      * elements set to 0, and diagonal elements set to 1.
00519      */
00520     static TMatrix<Rows, Cols, value_type> identity() {
00521         TMatrix<Rows, Cols, value_type> id;
00522         for (uint16_t i = 0; i < Rows && i < Cols; i++) {
00523             id.element(i,i) = 1;
00524         }
00525         return id;
00526     }
00527 
00528     /**
00529      * @brief Returns a ones matrix with dimensions given by the
00530      * class's template parameters.
00531      *
00532      * @return A TMatrix<Rows, Cols, value_type> with all elements set
00533      * to 1.
00534      */
00535     static TMatrix<Rows, Cols, value_type> one() {
00536         TMatrix<Rows, Cols, value_type> ones;
00537         for (uint16_t i = 0; i < Rows; i++) {
00538             for (uint16_t j = 0; j < Cols; j++) {
00539                 ones.element(i,j, 1);
00540             }
00541         }
00542         return ones;
00543     }
00544 
00545     /**
00546      * @brief Returns a zero matrix with dimensions given by the
00547      * class's template parameters.
00548      *
00549      * @return A TMatrix<Rows, Cols, value_type> containing all 0.
00550      */
00551     static TMatrix<Rows, Cols, value_type> zero() {
00552         return TMatrix<Rows, Cols, value_type>();
00553     }
00554 
00555     value_type* row(uint32_t i) { return &mData[i*Cols]; }
00556     const value_type* row(uint32_t i) const { return &mData[i*Cols]; }
00557 
00558     /**
00559      * @brief Checks to see if any of this matrix's elements are NaN.
00560      */
00561     bool hasNaN(void) const {
00562         for (uint32_t i = 0; i < Rows*Cols; i++) {
00563             if (isnan(mData[i])) {
00564                 return true;
00565             }
00566         }
00567         return false;
00568     }
00569 
00570     void print(Stream & os, bool oneLine = false) const {
00571         for (uint16_t i = 0; i < Rows; i++) {
00572             for (uint16_t j = 0; j < Cols; j++) {
00573                 os.printf("%.06f ", element(i, j));
00574             }
00575 
00576             if(!oneLine)
00577             {
00578                 os.printf("\n");
00579             }
00580         }
00581     }
00582 
00583 private:
00584 
00585     template <uint16_t Rows2, uint16_t Cols2, typename value_type2>
00586     friend TMatrix<Rows2,Cols2,value_type2> operator*(double s, const TMatrix<Rows2, Cols2, value_type2>& m);
00587 };
00588 
00589 typedef TMatrix<2,2, float>  TMatrix2;
00590 typedef TMatrix<3,3, float>  TMatrix3;
00591 typedef TMatrix<4,4, float>  TMatrix4;
00592 typedef TMatrix<2,1, float>  TVector2;
00593 typedef TMatrix<3,1, float>   TVector3 ;
00594 typedef TMatrix<4,1, float>  TVector4;
00595 
00596 // left-side scalar multiply
00597 template <uint16_t Rows, uint16_t Cols, typename value_type>
00598 TMatrix<Rows,Cols,value_type> operator*(double s, const TMatrix<Rows, Cols, value_type>& m) {
00599     return m * s;
00600 }
00601 
00602 
00603 #endif /* TMATRIX_H */